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