|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 /* |
|
24 |
|
25 standard ODE geometry primitives: public API and pairwise collision functions. |
|
26 |
|
27 the rule is that only the low level primitive collision functions should set |
|
28 dContactGeom::g1 and dContactGeom::g2. |
|
29 |
|
30 */ |
|
31 |
|
32 #include <ode/common.h> |
|
33 #include <ode/collision.h> |
|
34 #include <ode/matrix.h> |
|
35 #include <ode/rotation.h> |
|
36 #include <ode/odemath.h> |
|
37 #include "collision_kernel.h" |
|
38 #include "collision_std.h" |
|
39 #include "collision_util.h" |
|
40 |
|
41 |
|
42 //**************************************************************************** |
|
43 // ray public API |
|
44 |
|
45 dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) |
|
46 { |
|
47 type = dRayClass; |
|
48 length = _length; |
|
49 } |
|
50 |
|
51 |
|
52 void dxRay::computeAABB() |
|
53 { |
|
54 dVector3 e; |
|
55 e[0] = final_posr->pos[0] + dMUL(final_posr->R[0*4+2],length); |
|
56 e[1] = final_posr->pos[1] + dMUL(final_posr->R[1*4+2],length); |
|
57 e[2] = final_posr->pos[2] + dMUL(final_posr->R[2*4+2],length); |
|
58 |
|
59 if (final_posr->pos[0] < e[0]){ |
|
60 aabb[0] = final_posr->pos[0]; |
|
61 aabb[1] = e[0]; |
|
62 } |
|
63 else{ |
|
64 aabb[0] = e[0]; |
|
65 aabb[1] = final_posr->pos[0]; |
|
66 } |
|
67 |
|
68 if (final_posr->pos[1] < e[1]){ |
|
69 aabb[2] = final_posr->pos[1]; |
|
70 aabb[3] = e[1]; |
|
71 } |
|
72 else{ |
|
73 aabb[2] = e[1]; |
|
74 aabb[3] = final_posr->pos[1]; |
|
75 } |
|
76 |
|
77 if (final_posr->pos[2] < e[2]){ |
|
78 aabb[4] = final_posr->pos[2]; |
|
79 aabb[5] = e[2]; |
|
80 } |
|
81 else{ |
|
82 aabb[4] = e[2]; |
|
83 aabb[5] = final_posr->pos[2]; |
|
84 } |
|
85 } |
|
86 |
|
87 |
|
88 EXPORT_C dGeomID dCreateRay (dSpaceID space, dReal length) |
|
89 { |
|
90 return new dxRay (space,length); |
|
91 } |
|
92 |
|
93 |
|
94 EXPORT_C void dGeomRaySetLength (dGeomID g, dReal length) |
|
95 { |
|
96 dxRay *r = (dxRay*) g; |
|
97 r->length = length; |
|
98 dGeomMoved (g); |
|
99 } |
|
100 |
|
101 |
|
102 EXPORT_C dReal dGeomRayGetLength (dGeomID g) |
|
103 { |
|
104 dxRay *r = (dxRay*) g; |
|
105 return r->length; |
|
106 } |
|
107 |
|
108 |
|
109 EXPORT_C void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, |
|
110 dReal dx, dReal dy, dReal dz) |
|
111 { |
|
112 g->recomputePosr(); |
|
113 dReal* rot = g->final_posr->R; |
|
114 dReal* pos = g->final_posr->pos; |
|
115 dVector3 n; |
|
116 pos[0] = px; |
|
117 pos[1] = py; |
|
118 pos[2] = pz; |
|
119 |
|
120 n[0] = dx; |
|
121 n[1] = dy; |
|
122 n[2] = dz; |
|
123 dNormalize3(n); |
|
124 rot[0*4+2] = n[0]; |
|
125 rot[1*4+2] = n[1]; |
|
126 rot[2*4+2] = n[2]; |
|
127 dGeomMoved (g); |
|
128 } |
|
129 |
|
130 |
|
131 EXPORT_C void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) |
|
132 { |
|
133 g->recomputePosr(); |
|
134 start[0] = g->final_posr->pos[0]; |
|
135 start[1] = g->final_posr->pos[1]; |
|
136 start[2] = g->final_posr->pos[2]; |
|
137 dir[0] = g->final_posr->R[0*4+2]; |
|
138 dir[1] = g->final_posr->R[1*4+2]; |
|
139 dir[2] = g->final_posr->R[2*4+2]; |
|
140 } |
|
141 |
|
142 |
|
143 EXPORT_C void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) |
|
144 { |
|
145 if (FirstContact){ |
|
146 g->gflags |= RAY_FIRSTCONTACT; |
|
147 } |
|
148 else g->gflags &= ~RAY_FIRSTCONTACT; |
|
149 |
|
150 if (BackfaceCull){ |
|
151 g->gflags |= RAY_BACKFACECULL; |
|
152 } |
|
153 else g->gflags &= ~RAY_BACKFACECULL; |
|
154 } |
|
155 |
|
156 |
|
157 EXPORT_C void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull) |
|
158 { |
|
159 (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0); |
|
160 (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0); |
|
161 } |
|
162 |
|
163 |
|
164 EXPORT_C void dGeomRaySetClosestHit (dxGeom *g, int closestHit) |
|
165 { |
|
166 if (closestHit){ |
|
167 g->gflags |= RAY_CLOSEST_HIT; |
|
168 } |
|
169 else g->gflags &= ~RAY_CLOSEST_HIT; |
|
170 } |
|
171 |
|
172 |
|
173 EXPORT_C int dGeomRayGetClosestHit (dxGeom *g) |
|
174 { |
|
175 return ((g->gflags & RAY_CLOSEST_HIT) != 0); |
|
176 } |
|
177 |
|
178 |
|
179 |
|
180 // if mode==1 then use the sphere exit contact, not the entry contact |
|
181 |
|
182 static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, |
|
183 dContactGeom *contact, int mode) |
|
184 { |
|
185 dVector3 q; |
|
186 q[0] = ray->final_posr->pos[0] - sphere_pos[0]; |
|
187 q[1] = ray->final_posr->pos[1] - sphere_pos[1]; |
|
188 q[2] = ray->final_posr->pos[2] - sphere_pos[2]; |
|
189 dReal B = dDOT14(q,ray->final_posr->R+2); |
|
190 dReal C = dDOT(q,q) - dMUL(radius,radius); |
|
191 // note: if C <= 0 then the start of the ray is inside the sphere |
|
192 dReal k = dMUL(B,B) - C; |
|
193 if (k < 0) return 0; |
|
194 k = dSqrt(k); |
|
195 dReal alpha; |
|
196 if (mode && C >= 0) { |
|
197 alpha = -B + k; |
|
198 if (alpha < 0) return 0; |
|
199 } |
|
200 else { |
|
201 alpha = -B - k; |
|
202 if (alpha < 0) { |
|
203 alpha = -B + k; |
|
204 if (alpha < 0) return 0; |
|
205 } |
|
206 } |
|
207 if (alpha > ray->length) return 0; |
|
208 contact->pos[0] = ray->final_posr->pos[0] + dMUL(alpha,ray->final_posr->R[0*4+2]); |
|
209 contact->pos[1] = ray->final_posr->pos[1] + dMUL(alpha,ray->final_posr->R[1*4+2]); |
|
210 contact->pos[2] = ray->final_posr->pos[2] + dMUL(alpha,ray->final_posr->R[2*4+2]); |
|
211 dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); |
|
212 contact->normal[0] = dMUL(nsign,(contact->pos[0] - sphere_pos[0])); |
|
213 contact->normal[1] = dMUL(nsign,(contact->pos[1] - sphere_pos[1])); |
|
214 contact->normal[2] = dMUL(nsign,(contact->pos[2] - sphere_pos[2])); |
|
215 dNormalize3 (contact->normal); |
|
216 contact->depth = alpha; |
|
217 return 1; |
|
218 } |
|
219 |
|
220 |
|
221 int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int /*flags*/, |
|
222 dContactGeom *contact, int /*skip*/) |
|
223 { |
|
224 dxRay *ray = (dxRay*) o1; |
|
225 dxSphere *sphere = (dxSphere*) o2; |
|
226 contact->g1 = ray; |
|
227 contact->g2 = sphere; |
|
228 return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0); |
|
229 } |
|
230 |
|
231 |
|
232 int dCollideRayBox (dxGeom *o1, dxGeom *o2, int /*flags*/, |
|
233 dContactGeom *contact, int /*skip*/) |
|
234 { |
|
235 dxRay *ray = (dxRay*) o1; |
|
236 dxBox *box = (dxBox*) o2; |
|
237 |
|
238 contact->g1 = ray; |
|
239 contact->g2 = box; |
|
240 |
|
241 int i; |
|
242 |
|
243 // compute the start and delta of the ray relative to the box. |
|
244 // we will do all subsequent computations in this box-relative coordinate |
|
245 // system. we have to do a translation and rotation for each point. |
|
246 dVector3 tmp,s,v; |
|
247 tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0]; |
|
248 tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1]; |
|
249 tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2]; |
|
250 dMULTIPLY1_331 (s,box->final_posr->R,tmp); |
|
251 tmp[0] = ray->final_posr->R[0*4+2]; |
|
252 tmp[1] = ray->final_posr->R[1*4+2]; |
|
253 tmp[2] = ray->final_posr->R[2*4+2]; |
|
254 dMULTIPLY1_331 (v,box->final_posr->R,tmp); |
|
255 |
|
256 // mirror the line so that v has all components >= 0 |
|
257 dVector3 sign; |
|
258 for (i=0; i<3; i++) { |
|
259 if (v[i] < 0) { |
|
260 s[i] = -s[i]; |
|
261 v[i] = -v[i]; |
|
262 sign[i] = REAL(1.0); |
|
263 } |
|
264 else sign[i] = REAL(-1.0); |
|
265 } |
|
266 |
|
267 // compute the half-sides of the box |
|
268 dReal h[3]; |
|
269 h[0] = dMUL(REAL(0.5),box->side[0]); |
|
270 h[1] = dMUL(REAL(0.5),box->side[1]); |
|
271 h[2] = dMUL(REAL(0.5),box->side[2]); |
|
272 |
|
273 // do a few early exit tests |
|
274 if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || |
|
275 (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || |
|
276 (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || |
|
277 (v[0] == 0 && v[1] == 0 && v[2] == 0)) { |
|
278 return 0; |
|
279 } |
|
280 |
|
281 // compute the t=[lo..hi] range for where s+v*t intersects the box |
|
282 dReal lo = -dInfinity; |
|
283 dReal hi = dInfinity; |
|
284 int nlo = 0, nhi = 0; |
|
285 for (i=0; i<3; i++) { |
|
286 if (v[i] != 0) { |
|
287 dReal k = dDIV((-h[i] - s[i]),v[i]); |
|
288 if (k > lo) { |
|
289 lo = k; |
|
290 nlo = i; |
|
291 } |
|
292 k = dDIV((h[i] - s[i]),v[i]); |
|
293 if (k < hi) { |
|
294 hi = k; |
|
295 nhi = i; |
|
296 } |
|
297 } |
|
298 } |
|
299 |
|
300 // check if the ray intersects |
|
301 if (lo > hi) return 0; |
|
302 dReal alpha; |
|
303 int n; |
|
304 if (lo >= 0) { |
|
305 alpha = lo; |
|
306 n = nlo; |
|
307 } |
|
308 else { |
|
309 alpha = hi; |
|
310 n = nhi; |
|
311 } |
|
312 if (alpha < 0 || alpha > ray->length) return 0; |
|
313 contact->pos[0] = ray->final_posr->pos[0] + dMUL(alpha,ray->final_posr->R[0*4+2]); |
|
314 contact->pos[1] = ray->final_posr->pos[1] + dMUL(alpha,ray->final_posr->R[1*4+2]); |
|
315 contact->pos[2] = ray->final_posr->pos[2] + dMUL(alpha,ray->final_posr->R[2*4+2]); |
|
316 contact->normal[0] = dMUL(box->final_posr->R[0*4+n],sign[n]); |
|
317 contact->normal[1] = dMUL(box->final_posr->R[1*4+n],sign[n]); |
|
318 contact->normal[2] = dMUL(box->final_posr->R[2*4+n],sign[n]); |
|
319 contact->depth = alpha; |
|
320 return 1; |
|
321 } |
|
322 |
|
323 |
|
324 int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, |
|
325 int /*flags*/, dContactGeom *contact, int /*skip*/) |
|
326 { |
|
327 dxRay *ray = (dxRay*) o1; |
|
328 dxCapsule *ccyl = (dxCapsule*) o2; |
|
329 |
|
330 contact->g1 = ray; |
|
331 contact->g2 = ccyl; |
|
332 dReal lz2 = dMUL(ccyl->lz,REAL(0.5)); |
|
333 |
|
334 // compute some useful info |
|
335 dVector3 cs,q,r; |
|
336 dReal C,k; |
|
337 cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0]; |
|
338 cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1]; |
|
339 cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2]; |
|
340 k = dDOT41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis |
|
341 q[0] = dMUL(k,ccyl->final_posr->R[0*4+2]) - cs[0]; |
|
342 q[1] = dMUL(k,ccyl->final_posr->R[1*4+2]) - cs[1]; |
|
343 q[2] = dMUL(k,ccyl->final_posr->R[2*4+2]) - cs[2]; |
|
344 C = dDOT(q,q) - dMUL(ccyl->radius,ccyl->radius); |
|
345 // if C < 0 then ray start position within infinite extension of cylinder |
|
346 |
|
347 // see if ray start position is inside the capped cylinder |
|
348 int inside_ccyl = 0; |
|
349 if (C < 0) { |
|
350 if (k < -lz2) k = -lz2; |
|
351 else if (k > lz2) k = lz2; |
|
352 r[0] = ccyl->final_posr->pos[0] + dMUL(k,ccyl->final_posr->R[0*4+2]); |
|
353 r[1] = ccyl->final_posr->pos[1] + dMUL(k,ccyl->final_posr->R[1*4+2]); |
|
354 r[2] = ccyl->final_posr->pos[2] + dMUL(k,ccyl->final_posr->R[2*4+2]); |
|
355 if (dMUL((ray->final_posr->pos[0]-r[0]),(ray->final_posr->pos[0]-r[0])) + |
|
356 dMUL((ray->final_posr->pos[1]-r[1]),(ray->final_posr->pos[1]-r[1])) + |
|
357 dMUL((ray->final_posr->pos[2]-r[2]),(ray->final_posr->pos[2]-r[2])) < dMUL(ccyl->radius,ccyl->radius)) { |
|
358 inside_ccyl = 1; |
|
359 } |
|
360 } |
|
361 |
|
362 // compute ray collision with infinite cylinder, except for the case where |
|
363 // the ray is outside the capped cylinder but within the infinite cylinder |
|
364 // (it that case the ray can only hit endcaps) |
|
365 if (!inside_ccyl && C < 0) { |
|
366 // set k to cap position to check |
|
367 if (k < 0) k = -lz2; else k = lz2; |
|
368 } |
|
369 else { |
|
370 dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2); |
|
371 r[0] = dMUL(uv,ccyl->final_posr->R[0*4+2]) - ray->final_posr->R[0*4+2]; |
|
372 r[1] = dMUL(uv,ccyl->final_posr->R[1*4+2]) - ray->final_posr->R[1*4+2]; |
|
373 r[2] = dMUL(uv,ccyl->final_posr->R[2*4+2]) - ray->final_posr->R[2*4+2]; |
|
374 dReal A = dDOT(r,r); |
|
375 dReal B = 2*dDOT(q,r); |
|
376 k = dMUL(B,B)-4*dMUL(A,C); |
|
377 if (k < 0) { |
|
378 // the ray does not intersect the infinite cylinder, but if the ray is |
|
379 // inside and parallel to the cylinder axis it may intersect the end |
|
380 // caps. set k to cap position to check. |
|
381 if (!inside_ccyl) return 0; |
|
382 if (uv < 0) k = -lz2; else k = lz2; |
|
383 } |
|
384 else { |
|
385 k = dSqrt(k); |
|
386 A = dRecip (2*A); |
|
387 dReal alpha = dMUL((-B-k),A); |
|
388 if (alpha < 0) { |
|
389 alpha = dMUL((-B+k),A); |
|
390 if (alpha < 0) return 0; |
|
391 } |
|
392 if (alpha > ray->length) return 0; |
|
393 |
|
394 // the ray intersects the infinite cylinder. check to see if the |
|
395 // intersection point is between the caps |
|
396 contact->pos[0] = ray->final_posr->pos[0] + dMUL(alpha,ray->final_posr->R[0*4+2]); |
|
397 contact->pos[1] = ray->final_posr->pos[1] + dMUL(alpha,ray->final_posr->R[1*4+2]); |
|
398 contact->pos[2] = ray->final_posr->pos[2] + dMUL(alpha,ray->final_posr->R[2*4+2]); |
|
399 q[0] = contact->pos[0] - ccyl->final_posr->pos[0]; |
|
400 q[1] = contact->pos[1] - ccyl->final_posr->pos[1]; |
|
401 q[2] = contact->pos[2] - ccyl->final_posr->pos[2]; |
|
402 k = dDOT14(q,ccyl->final_posr->R+2); |
|
403 dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0); |
|
404 if (k >= -lz2 && k <= lz2) { |
|
405 contact->normal[0] = dMUL(nsign,(contact->pos[0] - |
|
406 (ccyl->final_posr->pos[0] + dMUL(k,ccyl->final_posr->R[0*4+2])))); |
|
407 contact->normal[1] = dMUL(nsign,(contact->pos[1] - |
|
408 (ccyl->final_posr->pos[1] + dMUL(k,ccyl->final_posr->R[1*4+2])))); |
|
409 contact->normal[2] = dMUL(nsign,(contact->pos[2] - |
|
410 (ccyl->final_posr->pos[2] + dMUL(k,ccyl->final_posr->R[2*4+2])))); |
|
411 dNormalize3 (contact->normal); |
|
412 contact->depth = alpha; |
|
413 return 1; |
|
414 } |
|
415 |
|
416 // the infinite cylinder intersection point is not between the caps. |
|
417 // set k to cap position to check. |
|
418 if (k < 0) k = -lz2; else k = lz2; |
|
419 } |
|
420 } |
|
421 |
|
422 // check for ray intersection with the caps. k must indicate the cap |
|
423 // position to check |
|
424 q[0] = ccyl->final_posr->pos[0] + dMUL(k,ccyl->final_posr->R[0*4+2]); |
|
425 q[1] = ccyl->final_posr->pos[1] + dMUL(k,ccyl->final_posr->R[1*4+2]); |
|
426 q[2] = ccyl->final_posr->pos[2] + dMUL(k,ccyl->final_posr->R[2*4+2]); |
|
427 return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl); |
|
428 } |
|
429 |
|
430 |
|
431 int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int /*flags*/, |
|
432 dContactGeom *contact, int /*skip*/) |
|
433 { |
|
434 dxRay *ray = (dxRay*) o1; |
|
435 dxPlane *plane = (dxPlane*) o2; |
|
436 |
|
437 dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos); |
|
438 // note: if alpha > 0 the starting point is below the plane |
|
439 dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); |
|
440 dReal k = dDOT14(plane->p,ray->final_posr->R+2); |
|
441 if (k==0) return 0; // ray parallel to plane |
|
442 alpha = dDIV(alpha,k); |
|
443 if (alpha < 0 || alpha > ray->length) return 0; |
|
444 contact->pos[0] = ray->final_posr->pos[0] + dMUL(alpha,ray->final_posr->R[0*4+2]); |
|
445 contact->pos[1] = ray->final_posr->pos[1] + dMUL(alpha,ray->final_posr->R[1*4+2]); |
|
446 contact->pos[2] = ray->final_posr->pos[2] + dMUL(alpha,ray->final_posr->R[2*4+2]); |
|
447 contact->normal[0] = dMUL(nsign,plane->p[0]); |
|
448 contact->normal[1] = dMUL(nsign,plane->p[1]); |
|
449 contact->normal[2] = dMUL(nsign,plane->p[2]); |
|
450 contact->depth = alpha; |
|
451 contact->g1 = ray; |
|
452 contact->g2 = plane; |
|
453 return 1; |
|
454 } |
|
455 |
|
456 // Ray - Cylinder collider by David Walters (June 2006) |
|
457 int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int /*flags*/, dContactGeom *contact, int /*skip*/ ) |
|
458 { |
|
459 dxRay* ray = (dxRay*)( o1 ); |
|
460 dxCylinder* cyl = (dxCylinder*)( o2 ); |
|
461 |
|
462 // Fill in contact information. |
|
463 contact->g1 = ray; |
|
464 contact->g2 = cyl; |
|
465 |
|
466 const dReal half_length = dMUL(cyl->lz,REAL( 0.5 )); |
|
467 |
|
468 // |
|
469 // Compute some useful info |
|
470 // |
|
471 |
|
472 dVector3 q, r; |
|
473 dReal d, C, k; |
|
474 |
|
475 // Vector 'r', line segment from C to R (ray start) ( r = R - C ) |
|
476 r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0]; |
|
477 r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1]; |
|
478 r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2]; |
|
479 |
|
480 // Distance that ray start is along cyl axis ( Z-axis direction ) |
|
481 d = dDOT41( cyl->final_posr->R + 2, r ); |
|
482 |
|
483 // |
|
484 // Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz). |
|
485 // |
|
486 // Point on axis ( in world space ): cp = ( d * Cz ) + C |
|
487 // |
|
488 // Line 'q' from R to cp: q = cp - R |
|
489 // q = ( d * Cz ) + C - R |
|
490 // q = ( d * Cz ) - ( R - C ) |
|
491 |
|
492 q[ 0 ] = dMUL( d,cyl->final_posr->R[0*4+2] ) - r[ 0 ]; |
|
493 q[ 1 ] = dMUL( d,cyl->final_posr->R[1*4+2] ) - r[ 1 ]; |
|
494 q[ 2 ] = dMUL( d,cyl->final_posr->R[2*4+2] ) - r[ 2 ]; |
|
495 |
|
496 |
|
497 // Compute square length of 'q'. Subtract from radius squared to |
|
498 // get square distance 'C' between the line q and the radius. |
|
499 |
|
500 // if C < 0 then ray start position is within infinite extension of cylinder |
|
501 |
|
502 C = dDOT( q, q ) - dMUL( cyl->radius,cyl->radius ); |
|
503 |
|
504 // Compute the projection of ray direction normal onto cylinder direction normal. |
|
505 dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 ); |
|
506 |
|
507 // |
|
508 // Find ray collision with infinite cylinder |
|
509 // |
|
510 |
|
511 // Compute vector from end of ray direction normal to projection on cylinder direction normal. |
|
512 r[ 0 ] = dMUL( uv,cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2]; |
|
513 r[ 1 ] = dMUL( uv,cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2]; |
|
514 r[ 2 ] = dMUL( uv,cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2]; |
|
515 |
|
516 |
|
517 // Quadratic Formula Magic |
|
518 // Compute discriminant 'k': |
|
519 |
|
520 // k < 0 : No intersection |
|
521 // k = 0 : Tangent |
|
522 // k > 0 : Intersection |
|
523 |
|
524 dReal A = dDOT( r, r ); |
|
525 dReal B = 2 * dDOT( q, r ); |
|
526 |
|
527 k = dMUL(B,B) - 4*dMUL(A,C); |
|
528 |
|
529 // |
|
530 // Collision with Flat Caps ? |
|
531 // |
|
532 |
|
533 // No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases ) |
|
534 if ( k < dEpsilon && C <= 0 ) |
|
535 { |
|
536 // The ray does not intersect the edge of the infinite cylinder, |
|
537 // but the ray start is inside and so must run parallel to the axis. |
|
538 // It may yet intersect an end cap. The following cases are valid: |
|
539 |
|
540 // -ve-cap , -half centre +half , +ve-cap |
|
541 // <<================|-------------------|------------->>>---|================>> |
|
542 // | | |
|
543 // | d-------------------> 1. |
|
544 // 2. d------------------> | |
|
545 // 3. <------------------d | |
|
546 // | <-------------------d 4. |
|
547 // | | |
|
548 // <<================|-------------------|------------->>>---|===============>> |
|
549 |
|
550 // Negative if the ray and cylinder axes point in opposite directions. |
|
551 const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 ); |
|
552 |
|
553 // Negative if the ray start is inside the cylinder |
|
554 const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 ); |
|
555 |
|
556 // Ray and Cylinder axes run in the same direction ( cases 1, 2 ) |
|
557 // Ray and Cylinder axes run in opposite directions ( cases 3, 4 ) |
|
558 if ( ( ( uv > 0 ) && ( d + dMUL( uvsign,ray->length ) < dMUL(half_length,internal) ) ) || |
|
559 ( ( uv < 0 ) && ( d + dMUL( uvsign,ray->length ) > dMUL(half_length,internal) ) ) ) |
|
560 { |
|
561 return 0; // No intersection with caps or curved surface. |
|
562 } |
|
563 |
|
564 // Compute depth (distance from ray to cylinder) |
|
565 contact->depth = ( dMUL( -uvsign,d ) - dMUL( internal,half_length ) ); |
|
566 |
|
567 // Compute contact point. |
|
568 contact->pos[0] = ray->final_posr->pos[0] + dMUL( contact->depth,ray->final_posr->R[0*4+2] ); |
|
569 contact->pos[1] = ray->final_posr->pos[1] + dMUL( contact->depth,ray->final_posr->R[1*4+2] ); |
|
570 contact->pos[2] = ray->final_posr->pos[2] + dMUL( contact->depth,ray->final_posr->R[2*4+2] ); |
|
571 |
|
572 // Compute reflected contact normal. |
|
573 contact->normal[0] = dMUL(uvsign,( cyl->final_posr->R[0*4+2] )); |
|
574 contact->normal[1] = dMUL(uvsign,( cyl->final_posr->R[1*4+2] )); |
|
575 contact->normal[2] = dMUL(uvsign,( cyl->final_posr->R[2*4+2] )); |
|
576 |
|
577 // Contact! |
|
578 return 1; |
|
579 } |
|
580 |
|
581 |
|
582 // |
|
583 // Collision with Curved Edge ? |
|
584 // |
|
585 |
|
586 if ( k > 0 ) |
|
587 { |
|
588 // Finish off quadratic formula to get intersection co-efficient |
|
589 k = dSqrt( k ); |
|
590 A = dRecip( 2 * A ); |
|
591 |
|
592 // Compute distance along line to contact point. |
|
593 dReal alpha = dMUL(( -B - k ),A); |
|
594 if ( alpha < 0 ) |
|
595 { |
|
596 // Flip in the other direction. |
|
597 alpha = dMUL(( -B + k ),A); |
|
598 } |
|
599 |
|
600 // Intersection point is within ray length? |
|
601 if ( alpha >= 0 && alpha <= ray->length ) |
|
602 { |
|
603 // The ray intersects the infinite cylinder! |
|
604 |
|
605 // Compute contact point. |
|
606 contact->pos[0] = ray->final_posr->pos[0] + dMUL( alpha,ray->final_posr->R[0*4+2] ); |
|
607 contact->pos[1] = ray->final_posr->pos[1] + dMUL( alpha,ray->final_posr->R[1*4+2] ); |
|
608 contact->pos[2] = ray->final_posr->pos[2] + dMUL( alpha,ray->final_posr->R[2*4+2] ); |
|
609 |
|
610 // q is the vector from the cylinder centre to the contact point. |
|
611 q[0] = contact->pos[0] - cyl->final_posr->pos[0]; |
|
612 q[1] = contact->pos[1] - cyl->final_posr->pos[1]; |
|
613 q[2] = contact->pos[2] - cyl->final_posr->pos[2]; |
|
614 |
|
615 // Compute the distance along the cylinder axis of this contact point. |
|
616 d = dDOT14( q, cyl->final_posr->R+2 ); |
|
617 |
|
618 // Check to see if the intersection point is between the flat end caps |
|
619 if ( d >= -half_length && d <= +half_length ) |
|
620 { |
|
621 // Flip the normal if the start point is inside the cylinder. |
|
622 const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 ); |
|
623 |
|
624 // Compute contact normal. |
|
625 contact->normal[0] = dMUL(nsign,(contact->pos[0] - (cyl->final_posr->pos[0] + dMUL(d,cyl->final_posr->R[0*4+2])))); |
|
626 contact->normal[1] = dMUL(nsign,(contact->pos[1] - (cyl->final_posr->pos[1] + dMUL(d,cyl->final_posr->R[1*4+2])))); |
|
627 contact->normal[2] = dMUL(nsign,(contact->pos[2] - (cyl->final_posr->pos[2] + dMUL(d,cyl->final_posr->R[2*4+2])))); |
|
628 dNormalize3( contact->normal ); |
|
629 |
|
630 // Store depth. |
|
631 contact->depth = alpha; |
|
632 |
|
633 // Contact! |
|
634 return 1; |
|
635 } |
|
636 } |
|
637 } |
|
638 |
|
639 // No contact with anything. |
|
640 return 0; |
|
641 } |
|
642 |