aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/convex.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/convex.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/convex.cpp1294
1 files changed, 1294 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/convex.cpp b/libraries/ode-0.9/ode/src/convex.cpp
new file mode 100644
index 0000000..db93beb
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/convex.cpp
@@ -0,0 +1,1294 @@
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/*
24Code for Convex Collision Detection
25By Rodrigo Hernandez
26*/
27//#include <algorithm>
28#include <ode/common.h>
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33#include "collision_kernel.h"
34#include "collision_std.h"
35#include "collision_util.h"
36
37#ifdef _MSC_VER
38#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
39#endif
40
41#if _MSC_VER <= 1200
42#define dMIN(A,B) ((A)>(B) ? B : A)
43#define dMAX(A,B) ((A)>(B) ? A : B)
44#else
45#define dMIN(A,B) std::min(A,B)
46#define dMAX(A,B) std::max(A,B)
47#endif
48
49//****************************************************************************
50// Convex public API
51dxConvex::dxConvex (dSpaceID space,
52 dReal *_planes,
53 unsigned int _planecount,
54 dReal *_points,
55 unsigned int _pointcount,
56 unsigned int *_polygons) :
57 dxGeom (space,1)
58{
59 dAASSERT (_planes != NULL);
60 dAASSERT (_points != NULL);
61 dAASSERT (_polygons != NULL);
62 //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);
63 type = dConvexClass;
64 planes = _planes;
65 planecount = _planecount;
66 // we need points as well
67 points = _points;
68 pointcount = _pointcount;
69 polygons=_polygons;
70 FillEdges();
71}
72
73
74void dxConvex::computeAABB()
75{
76 // this can, and should be optimized
77 dVector3 point;
78 dMULTIPLY0_331 (point,final_posr->R,points);
79 aabb[0] = point[0]+final_posr->pos[0];
80 aabb[1] = point[0]+final_posr->pos[0];
81 aabb[2] = point[1]+final_posr->pos[1];
82 aabb[3] = point[1]+final_posr->pos[1];
83 aabb[4] = point[2]+final_posr->pos[2];
84 aabb[5] = point[2]+final_posr->pos[2];
85 for(unsigned int i=3;i<(pointcount*3);i+=3)
86 {
87 dMULTIPLY0_331 (point,final_posr->R,&points[i]);
88 aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]);
89 aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]);
90 aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]);
91 aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]);
92 aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]);
93 aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]);
94 }
95}
96
97/*! \brief Populates the edges set, should be called only once whenever
98 the polygon array gets updated */
99void dxConvex::FillEdges()
100{
101 unsigned int *points_in_poly=polygons;
102 unsigned int *index=polygons+1;
103 for(unsigned int i=0;i<planecount;++i)
104 {
105 //fprintf(stdout,"Points in Poly: %d\n",*points_in_poly);
106 for(unsigned int j=0;j<*points_in_poly;++j)
107 {
108 edges.insert(edge(dMIN(index[j],index[(j+1)%*points_in_poly]),
109 dMAX(index[j],index[(j+1)%*points_in_poly])));
110 //fprintf(stdout,"Insert %d-%d\n",index[j],index[(j+1)%*points_in_poly]);
111 }
112 points_in_poly+=(*points_in_poly+1);
113 index=points_in_poly+1;
114 }
115 /*
116 fprintf(stdout,"Edge Count: %d\n",edges.size());
117 for(std::set<edge>::iterator it=edges.begin();
118 it!=edges.end();
119 ++it)
120 {
121 fprintf(stdout,"Edge: %d-%d\n",it->first,it->second);
122 }
123 */
124}
125
126dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,
127 dReal *_points,
128 unsigned int _pointcount,
129 unsigned int *_polygons)
130{
131 //fprintf(stdout,"dxConvex dCreateConvex\n");
132 return new dxConvex(space,_planes, _planecount,
133 _points,
134 _pointcount,
135 _polygons);
136}
137
138void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,
139 dReal *_points,
140 unsigned int _pointcount,
141 unsigned int *_polygons)
142{
143 //fprintf(stdout,"dxConvex dGeomSetConvex\n");
144 dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
145 dxConvex *s = (dxConvex*) g;
146 s->planes = _planes;
147 s->planecount = _planecount;
148 s->points = _points;
149 s->pointcount = _pointcount;
150 s->polygons=_polygons;
151}
152
153//****************************************************************************
154// Helper Inlines
155//
156
157/*! \brief Returns Whether or not the segment ab intersects plane p
158 \param a origin of the segment
159 \param b segment destination
160 \param p plane to test for intersection
161 \param t returns the time "t" in the segment ray that gives us the intersecting
162 point
163 \param q returns the intersection point
164 \return true if there is an intersection, otherwise false.
165*/
166bool IntersectSegmentPlane(dVector3 a,
167 dVector3 b,
168 dVector4 p,
169 dReal &t,
170 dVector3 q)
171{
172 // Compute the t value for the directed line ab intersecting the plane
173 dVector3 ab;
174 ab[0]= b[0] - a[0];
175 ab[1]= b[1] - a[1];
176 ab[2]= b[2] - a[2];
177
178 t = (p[3] - dDOT(p,a)) / dDOT(p,ab);
179
180 // If t in [0..1] compute and return intersection point
181 if (t >= 0.0 && t <= 1.0)
182 {
183 q[0] = a[0] + t * ab[0];
184 q[1] = a[1] + t * ab[1];
185 q[2] = a[2] + t * ab[2];
186 return true;
187 }
188 // Else no intersection
189 return false;
190}
191
192/*! \brief Returns the Closest Point in Ray 1 to Ray 2
193 \param Origin1 The origin of Ray 1
194 \param Direction1 The direction of Ray 1
195 \param Origin1 The origin of Ray 2
196 \param Direction1 The direction of Ray 3
197 \param t the time "t" in Ray 1 that gives us the closest point
198 (closest_point=Origin1+(Direction*t).
199 \return true if there is a closest point, false if the rays are paralell.
200*/
201inline bool ClosestPointInRay(const dVector3 Origin1,
202 const dVector3 Direction1,
203 const dVector3 Origin2,
204 const dVector3 Direction2,
205 dReal& t)
206{
207 dVector3 w = {Origin1[0]-Origin2[0],
208 Origin1[1]-Origin2[1],
209 Origin1[2]-Origin2[2]};
210 dReal a = dDOT(Direction1 , Direction1);
211 dReal b = dDOT(Direction1 , Direction2);
212 dReal c = dDOT(Direction2 , Direction2);
213 dReal d = dDOT(Direction1 , w);
214 dReal e = dDOT(Direction2 , w);
215 dReal denominator = (a*c)-(b*b);
216 if(denominator==0.0f)
217 {
218 return false;
219 }
220 t = ((a*e)-(b*d))/denominator;
221 return true;
222}
223
224/*! \brief Returns the Ray on which 2 planes intersect if they do.
225 \param p1 Plane 1
226 \param p2 Plane 2
227 \param p Contains the origin of the ray upon returning if planes intersect
228 \param d Contains the direction of the ray upon returning if planes intersect
229 \return true if the planes intersect, false if paralell.
230*/
231inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d)
232{
233 // Compute direction of intersection line
234 //Cross(p1, p2,d);
235 dCROSS(d,=,p1,p2);
236
237 // If d is (near) zero, the planes are parallel (and separated)
238 // or coincident, so they're not considered intersecting
239 dReal denom = dDOT(d, d);
240 if (denom < dEpsilon) return false;
241
242 dVector3 n;
243 n[0]=p1[3]*p2[0] - p2[3]*p1[0];
244 n[1]=p1[3]*p2[1] - p2[3]*p1[1];
245 n[2]=p1[3]*p2[2] - p2[3]*p1[2];
246 // Compute point on intersection line
247 //Cross(n, d,p);
248 dCROSS(p,=,n,d);
249 p[0]/=denom;
250 p[1]/=denom;
251 p[2]/=denom;
252 return true;
253}
254
255/*! \brief Finds out if a point lies inside a 2D polygon
256 \param p Point to test
257 \param polygon a pointer to the start of the convex polygon index buffer
258 \param out the closest point in the polygon if the point is not inside
259 \return true if the point lies inside of the polygon, false if not.
260*/
261inline bool IsPointInPolygon(dVector3 p,
262 unsigned int *polygon,
263 dxConvex *convex,
264 dVector3 out)
265{
266 // p is the point we want to check,
267 // polygon is a pointer to the polygon we
268 // are checking against, remember it goes
269 // number of vertices then that many indexes
270 // out returns the closest point on the border of the
271 // polygon if the point is not inside it.
272 size_t pointcount=polygon[0];
273 dVector3 a;
274 dVector3 b;
275 dVector3 c;
276 dVector3 ab;
277 dVector3 ac;
278 dVector3 ap;
279 dVector3 bp;
280 dReal d1;
281 dReal d2;
282 dReal d3;
283 dReal d4;
284 dReal vc;
285 polygon++; // skip past pointcount
286 for(size_t i=0;i<pointcount;++i)
287 {
288 dMULTIPLY0_331 (a,convex->final_posr->R,&convex->points[(polygon[i]*3)]);
289 a[0]=convex->final_posr->pos[0]+a[0];
290 a[1]=convex->final_posr->pos[1]+a[1];
291 a[2]=convex->final_posr->pos[2]+a[2];
292
293 dMULTIPLY0_331 (b,convex->final_posr->R,
294 &convex->points[(polygon[(i+1)%pointcount]*3)]);
295 b[0]=convex->final_posr->pos[0]+b[0];
296 b[1]=convex->final_posr->pos[1]+b[1];
297 b[2]=convex->final_posr->pos[2]+b[2];
298
299 dMULTIPLY0_331 (c,convex->final_posr->R,
300 &convex->points[(polygon[(i+2)%pointcount]*3)]);
301 c[0]=convex->final_posr->pos[0]+c[0];
302 c[1]=convex->final_posr->pos[1]+c[1];
303 c[2]=convex->final_posr->pos[2]+c[2];
304
305 ab[0] = b[0] - a[0];
306 ab[1] = b[1] - a[1];
307 ab[2] = b[2] - a[2];
308 ac[0] = c[0] - a[0];
309 ac[1] = c[1] - a[1];
310 ac[2] = c[2] - a[2];
311 ap[0] = p[0] - a[0];
312 ap[1] = p[1] - a[1];
313 ap[2] = p[2] - a[2];
314 d1 = dDOT(ab,ap);
315 d2 = dDOT(ac,ap);
316 if (d1 <= 0.0f && d2 <= 0.0f)
317 {
318 out[0]=a[0];
319 out[1]=a[1];
320 out[2]=a[2];
321 return false;
322 }
323 bp[0] = p[0] - b[0];
324 bp[1] = p[1] - b[1];
325 bp[2] = p[2] - b[2];
326 d3 = dDOT(ab,bp);
327 d4 = dDOT(ac,bp);
328 if (d3 >= 0.0f && d4 <= d3)
329 {
330 out[0]=b[0];
331 out[1]=b[1];
332 out[2]=b[2];
333 return false;
334 }
335 vc = d1*d4 - d3*d2;
336 if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
337 {
338 dReal v = d1 / (d1 - d3);
339 out[0] = a[0] + (ab[0]*v);
340 out[1] = a[1] + (ab[1]*v);
341 out[2] = a[2] + (ab[2]*v);
342 return false;
343 }
344 }
345 return true;
346}
347
348int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
349 dContactGeom *contact, int skip)
350{
351 dIASSERT (skip >= (int)sizeof(dContactGeom));
352 dIASSERT (o1->type == dConvexClass);
353 dIASSERT (o2->type == dPlaneClass);
354 dIASSERT ((flags & NUMC_MASK) >= 1);
355
356 dxConvex *Convex = (dxConvex*) o1;
357 dxPlane *Plane = (dxPlane*) o2;
358 unsigned int contacts=0;
359 unsigned int maxc = flags & NUMC_MASK;
360 dVector3 v2;
361
362#define LTEQ_ZERO 0x10000000
363#define GTEQ_ZERO 0x20000000
364#define BOTH_SIGNS (LTEQ_ZERO | GTEQ_ZERO)
365 dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later
366
367 unsigned int totalsign = 0;
368 for(unsigned int i=0;i<Convex->pointcount;++i)
369 {
370 dMULTIPLY0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);
371 dVector3Add(Convex->final_posr->pos, v2, v2);
372
373 unsigned int distance2sign = GTEQ_ZERO;
374 dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D
375 if((distance2 <= REAL(0.0)))
376 {
377 distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS;
378
379 if (contacts != maxc)
380 {
381 dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
382 dVector3Copy(Plane->p, target->normal);
383 dVector3Copy(v2, target->pos);
384 target->depth = -distance2;
385 target->g1 = Convex;
386 target->g2 = Plane;
387 contacts++;
388 }
389 }
390
391 // Take new sign into account
392 totalsign |= distance2sign;
393 // Check if contacts are full and both signs have been already found
394 if ((contacts ^ maxc | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less
395 {
396 break; // Nothing can be changed any more
397 }
398 }
399 if (totalsign == BOTH_SIGNS) return contacts;
400 return 0;
401#undef BOTH_SIGNS
402#undef GTEQ_ZERO
403#undef LTEQ_ZERO
404}
405
406int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
407 dContactGeom *contact, int skip)
408{
409 dIASSERT (skip >= (int)sizeof(dContactGeom));
410 dIASSERT (o1->type == dSphereClass);
411 dIASSERT (o2->type == dConvexClass);
412 dIASSERT ((flags & NUMC_MASK) >= 1);
413
414 dxSphere *Sphere = (dxSphere*) o1;
415 dxConvex *Convex = (dxConvex*) o2;
416 dReal dist,closestdist=dInfinity;
417 dVector4 plane;
418 // dVector3 contactpoint;
419 dVector3 offsetpos,out,temp;
420 unsigned int *pPoly=Convex->polygons;
421 int closestplane;
422 bool sphereinside=true;
423 /*
424 Do a good old sphere vs plane check first,
425 if a collision is found then check if the contact point
426 is within the polygon
427 */
428 // offset the sphere final_posr->position into the convex space
429 offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0];
430 offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1];
431 offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2];
432 //fprintf(stdout,"Begin Check\n");
433 for(unsigned int i=0;i<Convex->planecount;++i)
434 {
435 // apply rotation to the plane
436 dMULTIPLY0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]);
437 plane[3]=(&Convex->planes[(i*4)])[3];
438 // Get the distance from the sphere origin to the plane
439 dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D
440 if(dist>0)
441 {
442 // if we get here, we know the center of the sphere is
443 // outside of the convex hull.
444 if(dist<Sphere->radius)
445 {
446 // if we get here we know the sphere surface penetrates
447 // the plane
448 if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,Convex,out))
449 {
450 // finally if we get here we know that the
451 // sphere is directly touching the inside of the polyhedron
452 //fprintf(stdout,"Contact! distance=%f\n",dist);
453 contact->normal[0] = plane[0];
454 contact->normal[1] = plane[1];
455 contact->normal[2] = plane[2];
456 contact->pos[0] = Sphere->final_posr->pos[0]+
457 (-contact->normal[0]*Sphere->radius);
458 contact->pos[1] = Sphere->final_posr->pos[1]+
459 (-contact->normal[1]*Sphere->radius);
460 contact->pos[2] = Sphere->final_posr->pos[2]+
461 (-contact->normal[2]*Sphere->radius);
462 contact->depth = Sphere->radius-dist;
463 contact->g1 = Sphere;
464 contact->g2 = Convex;
465 return 1;
466 }
467 else
468 {
469 // the sphere may not be directly touching
470 // the polyhedron, but it may be touching
471 // a point or an edge, if the distance between
472 // the closest point on the poly (out) and the
473 // center of the sphere is less than the sphere
474 // radius we have a hit.
475 temp[0] = (Sphere->final_posr->pos[0]-out[0]);
476 temp[1] = (Sphere->final_posr->pos[1]-out[1]);
477 temp[2] = (Sphere->final_posr->pos[2]-out[2]);
478 dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]);
479 // avoid the sqrt unless really necesary
480 if(dist<(Sphere->radius*Sphere->radius))
481 {
482 // We got an indirect hit
483 dist=dSqrt(dist);
484 contact->normal[0] = temp[0]/dist;
485 contact->normal[1] = temp[1]/dist;
486 contact->normal[2] = temp[2]/dist;
487 contact->pos[0] = Sphere->final_posr->pos[0]+
488 (-contact->normal[0]*Sphere->radius);
489 contact->pos[1] = Sphere->final_posr->pos[1]+
490 (-contact->normal[1]*Sphere->radius);
491 contact->pos[2] = Sphere->final_posr->pos[2]+
492 (-contact->normal[2]*Sphere->radius);
493 contact->depth = Sphere->radius-dist;
494 contact->g1 = Sphere;
495 contact->g2 = Convex;
496 return 1;
497 }
498 }
499 }
500 sphereinside=false;
501 }
502 if(sphereinside)
503 {
504 if(closestdist>dFabs(dist))
505 {
506 closestdist=dFabs(dist);
507 closestplane=i;
508 }
509 }
510 pPoly+=pPoly[0]+1;
511 }
512 if(sphereinside)
513 {
514 // if the center of the sphere is inside
515 // the Convex, we need to pop it out
516 dMULTIPLY0_331(contact->normal,
517 Convex->final_posr->R,
518 &Convex->planes[(closestplane*4)]);
519 contact->pos[0] = Sphere->final_posr->pos[0];
520 contact->pos[1] = Sphere->final_posr->pos[1];
521 contact->pos[2] = Sphere->final_posr->pos[2];
522 contact->depth = closestdist+Sphere->radius;
523 contact->g1 = Sphere;
524 contact->g2 = Convex;
525 return 1;
526 }
527 return 0;
528}
529
530int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
531 dContactGeom *contact, int skip)
532{
533 dIASSERT (skip >= (int)sizeof(dContactGeom));
534 dIASSERT (o1->type == dConvexClass);
535 dIASSERT (o2->type == dBoxClass);
536 dIASSERT ((flags & NUMC_MASK) >= 1);
537
538 dxConvex *Convex = (dxConvex*) o1;
539 dxBox *Box = (dxBox*) o2;
540
541 return 0;
542}
543
544int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
545 int flags, dContactGeom *contact, int skip)
546{
547 dIASSERT (skip >= (int)sizeof(dContactGeom));
548 dIASSERT (o1->type == dConvexClass);
549 dIASSERT (o2->type == dCapsuleClass);
550 dIASSERT ((flags & NUMC_MASK) >= 1);
551
552 dxConvex *Convex = (dxConvex*) o1;
553 dxCapsule *Capsule = (dxCapsule*) o2;
554
555 return 0;
556}
557
558/*!
559 \brief Retrieves the proper convex and plane index between 2 convex objects.
560
561 Seidel's Algorithm does not discriminate between 2 different Convex Hulls,
562 it only cares about planes, so we feed it the planes of Convex 1 followed
563 by the planes of Convex 2 as a single collection of planes,
564 given an index into the single collection,
565 this function determines the correct convex object and index to retrieve
566 the current plane.
567
568 \param c1 Convex 1
569 \param c2 Convex 2
570 \param i Plane Index to retrieve
571 \param index contains the translated index uppon return
572 \return a pointer to the convex object containing plane index "i"
573*/
574inline dxConvex* GetPlaneIndex(dxConvex& c1,
575 dxConvex& c2,
576 unsigned int i,unsigned int& index)
577{
578 if(i>=c1.planecount)
579 {
580 index = i-c1.planecount;
581 return &c2;
582 }
583 index=i;
584 return &c1;
585}
586
587inline void dumpplanes(dxConvex& cvx)
588{
589 // This is just a dummy debug function
590 dVector4 plane;
591 fprintf(stdout,"DUMP PLANES\n");
592 for (unsigned int i=0;i<cvx.planecount;++i)
593 {
594 dMULTIPLY0_331(plane,cvx.final_posr->R,&cvx.planes[(i*4)]);
595 // Translate
596 plane[3]=
597 (cvx.planes[(i*4)+3])+
598 ((plane[0] * cvx.final_posr->pos[0]) +
599 (plane[1] * cvx.final_posr->pos[1]) +
600 (plane[2] * cvx.final_posr->pos[2]));
601 fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]);
602 }
603}
604
605// this variable is for debuggin purpuses only, should go once everything works
606static bool hit=false;
607
608/*
609 \brief Tests whether 2 convex objects collide
610
611 Seidel's algorithm is a method to solve linear programs,
612 it finds the optimum vertex "v" of a set of functions,
613 in our case, the set of functions are the plane functions
614 for the 2 convex objects being tested.
615 We don't really care about the value optimum vertex though,
616 but the 2 convex objects only collide if this vertex exists,
617 otherwise, the set of functions is said to be "empty" or "void".
618
619 Seidel's Original algorithm is recursive and not bound to any number
620 of dimensions, the one I present here is Iterative rather than recursive
621 and bound to 3 dimensions, which is what we care about.
622
623 If you're interested (and you should be!) on the algorithm, the paper
624 by Raimund Seidel himself should explain a lot better than I did, you can
625 find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf
626
627 If posible, read the section about Linear Programming in
628 Christer Ericson's RealTime Collision Detection book.
629
630 \note currently there seem to be some issues with this function since
631 it doesn't detect collisions except for the most simple tests :(.
632*/
633bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2)
634{
635 dVector3 c={1,0,0}; // The Objective vector can be anything
636 dVector3 solution; // We dont really need the solution so its local
637 dxConvex *cvx;
638 unsigned int index;
639 unsigned int planecount=cvx1.planecount+cvx2.planecount;
640 dReal sum,min,max,med;
641 dVector3 c1; // ,c2;
642 dVector4 aoveral,aoveram; // these will contain cached computations
643 unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension
644 unsigned int i,j,k;
645 dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly
646 // Get the support mapping for a HUGE bounding box in direction c
647 solution[0]= (c[0]>0) ? dInfinity : -dInfinity;
648 solution[1]= (c[1]>0) ? dInfinity : -dInfinity;
649 solution[2]= (c[2]>0) ? dInfinity : -dInfinity;
650 for( i=0;i<planecount;++i)
651 {
652 // Isolate plane equation
653 cvx=GetPlaneIndex(cvx1,cvx2,i,index);
654 // Rotate
655 dMULTIPLY0_331(eq1,cvx->final_posr->R,&cvx->planes[(index*4)]);
656
657 // Translate
658 eq1[3]=(cvx->planes[(index*4)+3])+
659 ((eq1[0] * cvx->final_posr->pos[0]) +
660 (eq1[1] * cvx->final_posr->pos[1]) +
661 (eq1[2] * cvx->final_posr->pos[2]));
662 // if(!hit)
663 // {
664 // fprintf(stdout,"Plane I %d: %f %f %f %f\n",i,
665 // cvx->planes[(index*4)+0],
666 // cvx->planes[(index*4)+1],
667 // cvx->planes[(index*4)+2],
668 // cvx->planes[(index*4)+3]);
669 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i,
670 // eq1[0],
671 // eq1[1],eq1[2],eq1[3]);
672 // fprintf(stdout,"POS %f,%f,%f\n",
673 // cvx->final_posr->pos[0],
674 // cvx->final_posr->pos[1],
675 // cvx->final_posr->pos[2]);
676 // }
677 // find if the solution is behind the current plane
678 sum=
679 ((eq1[0]*solution[0])+
680 (eq1[1]*solution[1])+
681 (eq1[2]*solution[2]))-eq1[3];
682 // if not we need to discard the current solution
683 if(sum>0)
684 {
685 // go down a dimension by eliminating a variable
686 // first find l
687 l=0;
688 for( j=0;j<3;++j)
689 {
690 if(fabs(eq1[j])>fabs(eq1[l]))
691 {
692 l=j;
693 }
694 }
695 if(eq1[l]==0)
696 {
697 if(!hit)
698 {
699 fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i,
700 eq1[0],eq1[1],eq1[2],eq1[3]);
701 }
702 return false;
703 }
704 // then compute a/a[l] c1 and solution
705 aoveral[0]=eq1[0]/eq1[l];
706 aoveral[1]=eq1[1]/eq1[l];
707 aoveral[2]=eq1[2]/eq1[l];
708 aoveral[3]=eq1[3]/eq1[l];
709 c1[0]=c[0]-((c[l]/eq1[l])*eq1[0]);
710 c1[1]=c[1]-((c[l]/eq1[l])*eq1[1]);
711 c1[2]=c[2]-((c[l]/eq1[l])*eq1[2]);
712 solution[0]=solution[0]-((solution[l]/eq1[l])*eq1[0]);
713 solution[1]=solution[1]-((solution[l]/eq1[l])*eq1[1]);
714 solution[2]=solution[2]-((solution[l]/eq1[l])*eq1[2]);
715 // iterate a to get the new equations with the help of a/a[l]
716 for( j=0;j<planecount;++j)
717 {
718 if(i!=j)
719 {
720 cvx=GetPlaneIndex(cvx1,cvx2,j,index);
721 // Rotate
722 dMULTIPLY0_331(eq2,cvx->final_posr->R,&cvx->planes[(index*4)]);
723 // Translate
724 eq2[3]=(cvx->planes[(index*4)+3])+
725 ((eq2[0] * cvx->final_posr->pos[0]) +
726 (eq2[1] * cvx->final_posr->pos[1]) +
727 (eq2[2] * cvx->final_posr->pos[2]));
728
729 // if(!hit)
730 // {
731 // fprintf(stdout,"Plane J %d: %f %f %f %f\n",j,
732 // cvx->planes[(index*4)+0],
733 // cvx->planes[(index*4)+1],
734 // cvx->planes[(index*4)+2],
735 // cvx->planes[(index*4)+3]);
736 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j,
737 // eq2[0],
738 // eq2[1],
739 // eq2[2],
740 // eq2[3]);
741 // fprintf(stdout,"POS %f,%f,%f\n",
742 // cvx->final_posr->pos[0],
743 // cvx->final_posr->pos[1],
744 // cvx->final_posr->pos[2]);
745 // }
746
747 // Take The equation down a dimension
748 eq2[0]-=(cvx->planes[(index*4)+l]*aoveral[0]);
749 eq2[1]-=(cvx->planes[(index*4)+l]*aoveral[1]);
750 eq2[2]-=(cvx->planes[(index*4)+l]*aoveral[2]);
751 eq2[3]-=(cvx->planes[(index*4)+l]*aoveral[3]);
752 sum=
753 ((eq2[0]*solution[0])+
754 (eq2[1]*solution[1])+
755 (eq2[2]*solution[2]))-eq2[3];
756 if(sum>0)
757 {
758 m=0;
759 for( k=0;k<3;++k)
760 {
761 if(fabs(eq2[k])>fabs(eq2[m]))
762 {
763 m=k;
764 }
765 }
766 if(eq2[m]==0)
767 {
768 /*
769 if(!hit) fprintf(stdout,
770 "Plane %d: %f %f %f %f is invalid\n",j,
771 eq2[0],eq2[1],eq2[2],eq2[3]);
772 */
773 return false;
774 }
775 // then compute a/a[m] c1 and solution
776 aoveram[0]=eq2[0]/eq2[m];
777 aoveram[1]=eq2[1]/eq2[m];
778 aoveram[2]=eq2[2]/eq2[m];
779 aoveram[3]=eq2[3]/eq2[m];
780 c1[0]=c[0]-((c[m]/eq2[m])*eq2[0]);
781 c1[1]=c[1]-((c[m]/eq2[m])*eq2[1]);
782 c1[2]=c[2]-((c[m]/eq2[m])*eq2[2]);
783 solution[0]=solution[0]-((solution[m]/eq2[m])*eq2[0]);
784 solution[1]=solution[1]-((solution[m]/eq2[m])*eq2[1]);
785 solution[2]=solution[2]-((solution[m]/eq2[m])*eq2[2]);
786 // figure out the value for n by elimination
787 n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0);
788 // iterate a to get the new equations with the help of a/a[l]
789 min=-dInfinity;
790 max=med=dInfinity;
791 for(k=0;k<planecount;++k)
792 {
793 if((i!=k)&&(j!=k))
794 {
795 cvx=GetPlaneIndex(cvx1,cvx2,k,index);
796 // Rotate
797 dMULTIPLY0_331(eq3,cvx->final_posr->R,&cvx->planes[(index*4)]);
798 // Translate
799 eq3[3]=(cvx->planes[(index*4)+3])+
800 ((eq3[0] * cvx->final_posr->pos[0]) +
801 (eq3[1] * cvx->final_posr->pos[1]) +
802 (eq3[2] * cvx->final_posr->pos[2]));
803 // if(!hit)
804 // {
805 // fprintf(stdout,"Plane K %d: %f %f %f %f\n",k,
806 // cvx->planes[(index*4)+0],
807 // cvx->planes[(index*4)+1],
808 // cvx->planes[(index*4)+2],
809 // cvx->planes[(index*4)+3]);
810 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k,
811 // eq3[0],
812 // eq3[1],
813 // eq3[2],
814 // eq3[3]);
815 // fprintf(stdout,"POS %f,%f,%f\n",
816 // cvx->final_posr->pos[0],
817 // cvx->final_posr->pos[1],
818 // cvx->final_posr->pos[2]);
819 // }
820
821 // Take the equation Down to 1D
822 eq3[0]-=(cvx->planes[(index*4)+m]*aoveram[0]);
823 eq3[1]-=(cvx->planes[(index*4)+m]*aoveram[1]);
824 eq3[2]-=(cvx->planes[(index*4)+m]*aoveram[2]);
825 eq3[3]-=(cvx->planes[(index*4)+m]*aoveram[3]);
826 if(eq3[n]>0)
827 {
828 max=dMIN(max,eq3[3]/eq3[n]);
829 }
830 else if(eq3[n]<0)
831 {
832 min=dMAX(min,eq3[3]/eq3[n]);
833 }
834 else
835 {
836 med=dMIN(med,eq3[3]);
837 }
838 }
839 }
840 if ((max<min)||(med<0))
841 {
842 // if(!hit) fprintf(stdout,"((max<min)||(med<0)) ((%f<%f)||(%f<0))",max,min,med);
843 if(!hit)
844 {
845 dumpplanes(cvx1);
846 dumpplanes(cvx2);
847 }
848 //fprintf(stdout,"Planes %d %d\n",i,j);
849 return false;
850
851 }
852 // find the value for the solution in one dimension
853 solution[n] = (c1[n]>=0) ? max:min;
854 // lift to 2D
855 solution[m] = (eq2[3]-(eq2[n]*solution[n]))/eq2[m];
856 // lift to 3D
857 solution[l] = (eq1[3]-(eq1[m]*solution[m]+
858 eq1[n]*solution[n]))/eq1[l];
859 }
860 }
861 }
862 }
863 }
864 return true;
865}
866
867/*! \brief A Support mapping function for convex shapes
868 \param dir direction to find the Support Point for
869 \param cvx convex object to find the support point for
870 \param out the support mapping in dir.
871 */
872inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out)
873{
874 unsigned int index = 0;
875 dVector3 point;
876 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
877 point[0]+=cvx.final_posr->pos[0];
878 point[1]+=cvx.final_posr->pos[1];
879 point[2]+=cvx.final_posr->pos[2];
880
881 dReal max = dDOT(point,dir);
882 dReal tmp;
883 for (unsigned int i = 1; i < cvx.pointcount; ++i)
884 {
885 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3));
886 point[0]+=cvx.final_posr->pos[0];
887 point[1]+=cvx.final_posr->pos[1];
888 point[2]+=cvx.final_posr->pos[2];
889 tmp = dDOT(point, dir);
890 if (tmp > max)
891 {
892 out[0]=point[0];
893 out[1]=point[1];
894 out[2]=point[2];
895 max = tmp;
896 }
897 }
898}
899
900inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
901{
902 dVector3 point;
903 dReal value;
904 //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);
905 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
906 //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);
907 point[0]+=cvx.final_posr->pos[0];
908 point[1]+=cvx.final_posr->pos[1];
909 point[2]+=cvx.final_posr->pos[2];
910 max = min = dDOT(axis,cvx.points);
911 for (unsigned int i = 1; i < cvx.pointcount; ++i)
912 {
913 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3));
914 point[0]+=cvx.final_posr->pos[0];
915 point[1]+=cvx.final_posr->pos[1];
916 point[2]+=cvx.final_posr->pos[2];
917 value=dDOT(axis,point);
918 if(value<min)
919 {
920 min=value;
921 }
922 else if(value>max)
923 {
924 max=value;
925 }
926 }
927 //fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max);
928}
929
930/*! \brief Does an axis separation test between the 2 convex shapes
931using faces and edges */
932int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
933 dContactGeom *contact, int skip)
934{
935 dVector4 plane,savedplane;
936 dReal min1,max1,min2,max2,min_depth=-dInfinity;
937 dVector3 e1,e2,t;
938 int maxc = flags & NUMC_MASK; // this is causing a segfault
939 dIASSERT(maxc != 0);
940 dxConvex *g1,*g2;
941 unsigned int *pPoly;
942 dVector3 v;
943 // Test faces of cvx1 for separation
944 pPoly=cvx1.polygons;
945 for(unsigned int i=0;i<cvx1.planecount;++i)
946 {
947 // -- Apply Transforms --
948 // Rotate
949 dMULTIPLY0_331(plane,cvx1.final_posr->R,cvx1.planes+(i*4));
950 dNormalize3(plane);
951 // Translate
952 plane[3]=
953 (cvx1.planes[(i*4)+3])+
954 ((plane[0] * cvx1.final_posr->pos[0]) +
955 (plane[1] * cvx1.final_posr->pos[1]) +
956 (plane[2] * cvx1.final_posr->pos[2]));
957 ComputeInterval(cvx1,plane,min1,max1);
958 ComputeInterval(cvx2,plane,min2,max2);
959 //fprintf(stdout,"width %f\n",max1-min1);
960 if(max2<min1 || max1<min2) return 0;
961#if 1
962 // this one ON works
963 else if ((max1>min2)&&(max1<max2))
964 {
965 min_depth=max1-min2;
966 savedplane[0]=plane[0];
967 savedplane[1]=plane[1];
968 savedplane[2]=plane[2];
969 savedplane[3]=plane[3];
970 g1=&cvx2; // cvx2 moves
971 g2=&cvx1;
972 }
973#endif
974 pPoly+=pPoly[0]+1;
975 }
976 // Test faces of cvx2 for separation
977 pPoly=cvx2.polygons;
978 for(unsigned int i=0;i<cvx2.planecount;++i)
979 {
980 // fprintf(stdout,"Poly verts %d\n",pPoly[0]);
981 // -- Apply Transforms --
982 // Rotate
983 dMULTIPLY0_331(plane,
984 cvx2.final_posr->R,
985 cvx2.planes+(i*4));
986 dNormalize3(plane);
987 // Translate
988 plane[3]=
989 (cvx2.planes[(i*4)+3])+
990 ((plane[0] * cvx2.final_posr->pos[0]) +
991 (plane[1] * cvx2.final_posr->pos[1]) +
992 (plane[2] * cvx2.final_posr->pos[2]));
993 ComputeInterval(cvx2,plane,min1,max1);
994 ComputeInterval(cvx1,plane,min2,max2);
995 //fprintf(stdout,"width %f\n",max1-min1);
996 if(max2<min1 || max1<min2) return 0;
997#if 1
998 // this one ON does not work
999 else if ((max1>min2)&&(max1<max2))
1000 {
1001 min_depth=max1-min2;
1002 savedplane[0]=plane[0];
1003 savedplane[1]=plane[1];
1004 savedplane[2]=plane[2];
1005 savedplane[3]=plane[3];
1006 g1=&cvx1; // cvx1 moves
1007 g2=&cvx2;
1008 }
1009#endif
1010 pPoly+=pPoly[0]+1;
1011 }
1012 // Test cross products of pairs of edges
1013 for(std::set<edge>::iterator i = cvx1.edges.begin();
1014 i!= cvx1.edges.end();
1015 ++i)
1016 {
1017 // we only need to apply rotation here
1018 dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+(i->first*3));
1019 dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+(i->second*3));
1020 e1[0]-=t[0];
1021 e1[1]-=t[1];
1022 e1[2]-=t[2];
1023 for(std::set<edge>::iterator j = cvx2.edges.begin();
1024 j!= cvx2.edges.end();
1025 ++j)
1026 {
1027 // we only need to apply rotation here
1028 dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+(j->first*3));
1029 dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+(j->second*3));
1030 e2[0]-=t[0];
1031 e2[1]-=t[1];
1032 e2[2]-=t[2];
1033 dCROSS(plane,=,e1,e2);
1034 plane[3]=0;
1035 ComputeInterval(cvx1,plane,min1,max1);
1036 ComputeInterval(cvx2,plane,min2,max2);
1037 if(max2<min1 || max1 < min2) return 0;
1038 }
1039 }
1040/* -- uncomment if you are debugging
1041 static int cvxhit=0;
1042 if(cvxhit<2)
1043 fprintf(stdout,"Plane: %f,%f,%f,%f\n",
1044 (double)savedplane[0],
1045 (double)savedplane[1],
1046 (double)savedplane[2],
1047 (double)savedplane[3]);
1048*/
1049 // If we get here, there was a collision
1050 int contacts=0;
1051 for(unsigned int i=0;i<g1->pointcount;++i)
1052 {
1053 dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);
1054 dVector3Add(g1->final_posr->pos, v, v);
1055
1056 dReal distance = dVector3Dot(savedplane, v) - savedplane[3]; // Ax + By + Cz - D
1057 if(distance<0)
1058 {
1059 dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
1060 dVector3Copy(savedplane, target->normal);
1061 dVector3Copy(v, target->pos);
1062 target->depth = -distance;
1063 target->g1 = g1;
1064 target->g2 = g2;
1065/* -- uncomment if you are debugging
1066 if(cvxhit<2)
1067 fprintf(stdout,"Contact: %f,%f,%f depth %f\n",
1068 (double)target->pos[0],
1069 (double)target->pos[1],
1070 (double)target->pos[2],
1071 (double)target->depth);
1072*/
1073 contacts++;
1074 if (contacts==maxc) break;
1075 }
1076 }
1077/* -- uncomment if you are debugging
1078 cvxhit++;
1079*/
1080
1081 return contacts;
1082}
1083
1084int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
1085 dContactGeom *contact, int skip)
1086{
1087 dIASSERT (skip >= (int)sizeof(dContactGeom));
1088 dIASSERT (o1->type == dConvexClass);
1089 dIASSERT (o2->type == dConvexClass);
1090 dIASSERT ((flags & NUMC_MASK) >= 1);
1091
1092// if(!hit) fprintf(stdout,"dCollideConvexConvex\n");
1093 dxConvex *Convex1 = (dxConvex*) o1;
1094 dxConvex *Convex2 = (dxConvex*) o2;
1095 int contacts;
1096 if(contacts=TestConvexIntersection(*Convex1,*Convex2,flags,
1097 contact,skip))
1098 {
1099 //fprintf(stdout,"We have a Hit!\n");
1100 }
1101 return contacts;
1102}
1103
1104#if 0
1105
1106int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
1107 dContactGeom *contact, int skip)
1108{
1109 dIASSERT (skip >= (int)sizeof(dContactGeom));
1110 dIASSERT( o1->type == dRayClass );
1111 dIASSERT( o2->type == dConvexClass );
1112 dIASSERT ((flags & NUMC_MASK) >= 1);
1113
1114 dxRay* ray = (dxRay*) o1;
1115 dxConvex* convex = (dxConvex*) o2;
1116 dVector3 origin,destination,contactpoint,out;
1117 dReal depth;
1118 dVector4 plane;
1119 unsigned int *pPoly=convex->polygons;
1120 // Calculate ray origin and destination
1121 destination[0]=0;
1122 destination[1]=0;
1123 destination[2]= ray->length;
1124 // -- Rotate --
1125 dMULTIPLY0_331(destination,ray->final_posr->R,destination);
1126 origin[0]=ray->final_posr->pos[0];
1127 origin[1]=ray->final_posr->pos[1];
1128 origin[2]=ray->final_posr->pos[2];
1129 destination[0]+=origin[0];
1130 destination[1]+=origin[1];
1131 destination[2]+=origin[2];
1132 for(int i=0;i<convex->planecount;++i)
1133 {
1134 // Rotate
1135 dMULTIPLY0_331(plane,convex->final_posr->R,convex->planes+(i*4));
1136 // Translate
1137 plane[3]=
1138 (convex->planes[(i*4)+3])+
1139 ((plane[0] * convex->final_posr->pos[0]) +
1140 (plane[1] * convex->final_posr->pos[1]) +
1141 (plane[2] * convex->final_posr->pos[2]));
1142 if(IntersectSegmentPlane(origin,
1143 destination,
1144 plane,
1145 depth,
1146 contactpoint))
1147 {
1148 if(IsPointInPolygon(contactpoint,pPoly,convex,out))
1149 {
1150 contact->pos[0]=contactpoint[0];
1151 contact->pos[1]=contactpoint[1];
1152 contact->pos[2]=contactpoint[2];
1153 contact->normal[0]=plane[0];
1154 contact->normal[1]=plane[1];
1155 contact->normal[2]=plane[2];
1156 contact->depth=depth;
1157 contact->g1 = ray;
1158 contact->g2 = convex;
1159 return 1;
1160 }
1161 }
1162 pPoly+=pPoly[0]+1;
1163 }
1164 return 0;
1165}
1166
1167#else
1168
1169// Ray - Convex collider by David Walters, June 2006
1170int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
1171 int flags, dContactGeom *contact, int skip )
1172{
1173 dIASSERT( skip >= (int)sizeof(dContactGeom) );
1174 dIASSERT( o1->type == dRayClass );
1175 dIASSERT( o2->type == dConvexClass );
1176 dIASSERT ((flags & NUMC_MASK) >= 1);
1177
1178 dxRay* ray = (dxRay*) o1;
1179 dxConvex* convex = (dxConvex*) o2;
1180
1181 contact->g1 = ray;
1182 contact->g2 = convex;
1183
1184 dReal alpha, beta, nsign;
1185 int flag;
1186
1187 //
1188 // Compute some useful info
1189 //
1190
1191 flag = 0; // Assume start point is behind all planes.
1192
1193 for ( unsigned int i = 0; i < convex->planecount; ++i )
1194 {
1195 // Alias this plane.
1196 dReal* plane = convex->planes + ( i * 4 );
1197
1198 // If alpha >= 0 then start point is outside of plane.
1199 alpha = dDOT( plane, ray->final_posr->pos ) - plane[3];
1200
1201 // If any alpha is positive, then
1202 // the ray start is _outside_ of the hull
1203 if ( alpha >= 0 )
1204 {
1205 flag = 1;
1206 break;
1207 }
1208 }
1209
1210 // If the ray starts inside the convex hull, then everything is flipped.
1211 nsign = ( flag ) ? REAL( 1.0 ) : REAL( -1.0 );
1212
1213
1214 //
1215 // Find closest contact point
1216 //
1217
1218 // Assume no contacts.
1219 contact->depth = dInfinity;
1220
1221 for ( unsigned int i = 0; i < convex->planecount; ++i )
1222 {
1223 // Alias this plane.
1224 dReal* plane = convex->planes + ( i * 4 );
1225
1226 // If alpha >= 0 then point is outside of plane.
1227 alpha = nsign * ( dDOT( plane, ray->final_posr->pos ) - plane[3] );
1228
1229 // Compute [ plane-normal DOT ray-normal ], (/flip)
1230 beta = dDOT13( plane, ray->final_posr->R+2 ) * nsign;
1231
1232 // Ray is pointing at the plane? ( beta < 0 )
1233 // Ray start to plane is within maximum ray length?
1234 // Ray start to plane is closer than the current best distance?
1235 if ( beta < -dEpsilon &&
1236 alpha >= 0 && alpha <= ray->length &&
1237 alpha < contact->depth )
1238 {
1239 // Compute contact point on convex hull surface.
1240 contact->pos[0] = ray->final_posr->pos[0] + alpha * ray->final_posr->R[0*4+2];
1241 contact->pos[1] = ray->final_posr->pos[1] + alpha * ray->final_posr->R[1*4+2];
1242 contact->pos[2] = ray->final_posr->pos[2] + alpha * ray->final_posr->R[2*4+2];
1243
1244 flag = 0;
1245
1246 // For all _other_ planes.
1247 for ( unsigned int j = 0; j < convex->planecount; ++j )
1248 {
1249 if ( i == j )
1250 continue; // Skip self.
1251
1252 // Alias this plane.
1253 dReal* planej = convex->planes + ( j * 4 );
1254
1255 // If beta >= 0 then start is outside of plane.
1256 beta = dDOT( planej, contact->pos ) - plane[3];
1257
1258 // If any beta is positive, then the contact point
1259 // is not on the surface of the convex hull - it's just
1260 // intersecting some part of its infinite extent.
1261 if ( beta > dEpsilon )
1262 {
1263 flag = 1;
1264 break;
1265 }
1266 }
1267
1268 // Contact point isn't outside hull's surface? then it's a good contact!
1269 if ( flag == 0 )
1270 {
1271 // Store the contact normal, possibly flipped.
1272 contact->normal[0] = nsign * plane[0];
1273 contact->normal[1] = nsign * plane[1];
1274 contact->normal[2] = nsign * plane[2];
1275
1276 // Store depth
1277 contact->depth = alpha;
1278
1279 if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length )
1280 {
1281 // Break on any contact if contacts are not important
1282 break;
1283 }
1284 }
1285 }
1286 }
1287
1288 // Contact?
1289 return ( contact->depth <= ray->length );
1290}
1291
1292#endif
1293
1294//<-- Convex Collision
generated by cgit v1.1 at 2024-11-11 00:40:41 +0000