Bullet Collision Detection & Physics Library: btGjkPairDetector.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btGjkPairDetector.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "btGjkPairDetector.h"
17#include "BulletCollision/CollisionShapes/btConvexShape.h"
18#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
19#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
20
21#if defined(DEBUG) || defined(_DEBUG)
22//#define TEST_NON_VIRTUAL 1
23#include <stdio.h> //for debug printf
24#ifdef __SPU__
25#include <spu_printf.h>
26#define printf spu_printf
27#endif //__SPU__
28#endif
29
30//must be above the machine epsilon
31#ifdef BT_USE_DOUBLE_PRECISION
32#define REL_ERROR2 btScalar(1.0e-12)
33 btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
34#else
35#define REL_ERROR2 btScalar(1.0e-6)
36 btScalar gGjkEpaPenetrationTolerance = 0.001;
37#endif
38
39
40btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
41 : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
42 m_penetrationDepthSolver(penetrationDepthSolver),
43 m_simplexSolver(simplexSolver),
44 m_minkowskiA(objectA),
45 m_minkowskiB(objectB),
46 m_shapeTypeA(objectA->getShapeType()),
47 m_shapeTypeB(objectB->getShapeType()),
48 m_marginA(objectA->getMargin()),
49 m_marginB(objectB->getMargin()),
50 m_ignoreMargin(false),
51 m_lastUsedMethod(-1),
52 m_catchDegeneracies(1),
53 m_fixContactNormalDirection(1)
54{
55}
56 btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
57 : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
58 m_penetrationDepthSolver(penetrationDepthSolver),
59 m_simplexSolver(simplexSolver),
60 m_minkowskiA(objectA),
61 m_minkowskiB(objectB),
62 m_shapeTypeA(shapeTypeA),
63 m_shapeTypeB(shapeTypeB),
64 m_marginA(marginA),
65 m_marginB(marginB),
66 m_ignoreMargin(false),
67 m_lastUsedMethod(-1),
68 m_catchDegeneracies(1),
69 m_fixContactNormalDirection(1)
70{
71}
72
73 void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults)
74{
75 (void)swapResults;
76
77 getClosestPointsNonVirtual(input, output, debugDraw);
78}
79
80 static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
81{
82 btVector3 separatingAxisInA = (dir)*localTransA.getBasis();
83 btVector3 separatingAxisInB = (-dir) * localTransB.getBasis();
84
85 btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
86 btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
87
88 btVector3 pInA = pInANoMargin;
89 btVector3 qInB = qInBNoMargin;
90
91 supAworld = localTransA(pInA);
92 supBworld = localTransB(qInB);
93
94 if (check2d)
95 {
96 supAworld[2] = 0.f;
97 supBworld[2] = 0.f;
98 }
99
100 aMinb = supAworld - supBworld;
101}
102
103 struct btSupportVector
104{
105 btVector3 v;
106 btVector3 v1;
107 btVector3 v2;
108};
109
110 struct btSimplex
111{
112 btSupportVector ps[4];
113 int last;
114};
115
116 static btVector3 ccd_vec3_origin(0, 0, 0);
117
118 inline void btSimplexInit(btSimplex *s)
119{
120 s->last = -1;
121}
122
123 inline int btSimplexSize(const btSimplex *s)
124{
125 return s->last + 1;
126}
127
128 inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx)
129{
130 // here is no check on boundaries
131 return &s->ps[idx];
132}
133 inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
134{
135 *d = *s;
136}
137
138 inline void btVec3Copy(btVector3 *v, const btVector3 *w)
139{
140 *v = *w;
141}
142
143 inline void ccdVec3Add(btVector3 *v, const btVector3 *w)
144{
145 v->m_floats[0] += w->m_floats[0];
146 v->m_floats[1] += w->m_floats[1];
147 v->m_floats[2] += w->m_floats[2];
148}
149
150 inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
151{
152 *v -= *w;
153}
154 inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
155{
156 *d = (*v) - (*w);
157}
158 inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
159{
160 btScalar dot;
161 dot = a->dot(*b);
162
163 return dot;
164}
165
166 inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
167{
168 btVector3 ab;
169 btVec3Sub2(&ab, a, b);
170 return btVec3Dot(&ab, &ab);
171}
172
173 inline void btVec3Scale(btVector3 *d, btScalar k)
174{
175 d->m_floats[0] *= k;
176 d->m_floats[1] *= k;
177 d->m_floats[2] *= k;
178}
179
180 inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
181{
182 d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
183 d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
184 d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
185}
186
187 inline void btTripleCross(const btVector3 *a, const btVector3 *b,
188 const btVector3 *c, btVector3 *d)
189{
190 btVector3 e;
191 btVec3Cross(&e, a, b);
192 btVec3Cross(d, &e, c);
193}
194
195 inline int ccdEq(btScalar _a, btScalar _b)
196{
197 btScalar ab;
198 btScalar a, b;
199
200 ab = btFabs(_a - _b);
201 if (btFabs(ab) < SIMD_EPSILON)
202 return 1;
203
204 a = btFabs(_a);
205 b = btFabs(_b);
206 if (b > a)
207 {
208 return ab < SIMD_EPSILON * b;
209 }
210 else
211 {
212 return ab < SIMD_EPSILON * a;
213 }
214}
215
216 btScalar ccdVec3X(const btVector3 *v)
217{
218 return v->x();
219}
220
221 btScalar ccdVec3Y(const btVector3 *v)
222{
223 return v->y();
224}
225
226 btScalar ccdVec3Z(const btVector3 *v)
227{
228 return v->z();
229}
230 inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
231{
232 return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
233}
234
235 inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
236{
237 // here is no check on boundaries in sake of speed
238 ++s->last;
239 btSupportCopy(s->ps + s->last, v);
240}
241
242 inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
243{
244 btSupportCopy(s->ps + pos, a);
245}
246
247 inline void btSimplexSetSize(btSimplex *s, int size)
248{
249 s->last = size - 1;
250}
251
252 inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
253{
254 return btSimplexPoint(s, s->last);
255}
256
257 inline int ccdSign(btScalar val)
258{
259 if (btFuzzyZero(val))
260 {
261 return 0;
262 }
263 else if (val < btScalar(0))
264 {
265 return -1;
266 }
267 return 1;
268}
269
270 inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
271 const btVector3 *x0,
272 const btVector3 *b,
273 btVector3 *witness)
274{
275 // The computation comes from solving equation of segment:
276 // S(t) = x0 + t.d
277 // where - x0 is initial point of segment
278 // - d is direction of segment from x0 (|d| > 0)
279 // - t belongs to <0, 1> interval
280 //
281 // Than, distance from a segment to some point P can be expressed:
282 // D(t) = |x0 + t.d - P|^2
283 // which is distance from any point on segment. Minimization
284 // of this function brings distance from P to segment.
285 // Minimization of D(t) leads to simple quadratic equation that's
286 // solving is straightforward.
287 //
288 // Bonus of this method is witness point for free.
289
290 btScalar dist, t;
291 btVector3 d, a;
292
293 // direction of segment
294 btVec3Sub2(&d, b, x0);
295
296 // precompute vector from P to x0
297 btVec3Sub2(&a, x0, P);
298
299 t = -btScalar(1.) * btVec3Dot(&a, &d);
300 t /= btVec3Dot(&d, &d);
301
302 if (t < btScalar(0) || btFuzzyZero(t))
303 {
304 dist = ccdVec3Dist2(x0, P);
305 if (witness)
306 btVec3Copy(witness, x0);
307 }
308 else if (t > btScalar(1) || ccdEq(t, btScalar(1)))
309 {
310 dist = ccdVec3Dist2(b, P);
311 if (witness)
312 btVec3Copy(witness, b);
313 }
314 else
315 {
316 if (witness)
317 {
318 btVec3Copy(witness, &d);
319 btVec3Scale(witness, t);
320 ccdVec3Add(witness, x0);
321 dist = ccdVec3Dist2(witness, P);
322 }
323 else
324 {
325 // recycling variables
326 btVec3Scale(&d, t);
327 ccdVec3Add(&d, &a);
328 dist = btVec3Dot(&d, &d);
329 }
330 }
331
332 return dist;
333}
334
335 btScalar btVec3PointTriDist2(const btVector3 *P,
336 const btVector3 *x0, const btVector3 *B,
337 const btVector3 *C,
338 btVector3 *witness)
339{
340 // Computation comes from analytic expression for triangle (x0, B, C)
341 // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
342 // Then equation for distance is:
343 // D(s, t) = | T(s, t) - P |^2
344 // This leads to minimization of quadratic function of two variables.
345 // The solution from is taken only if s is between 0 and 1, t is
346 // between 0 and 1 and t + s < 1, otherwise distance from segment is
347 // computed.
348
349 btVector3 d1, d2, a;
350 double u, v, w, p, q, r;
351 double s, t, dist, dist2;
352 btVector3 witness2;
353
354 btVec3Sub2(&d1, B, x0);
355 btVec3Sub2(&d2, C, x0);
356 btVec3Sub2(&a, x0, P);
357
358 u = btVec3Dot(&a, &a);
359 v = btVec3Dot(&d1, &d1);
360 w = btVec3Dot(&d2, &d2);
361 p = btVec3Dot(&a, &d1);
362 q = btVec3Dot(&a, &d2);
363 r = btVec3Dot(&d1, &d2);
364
365 s = (q * r - w * p) / (w * v - r * r);
366 t = (-s * r - q) / w;
367
368 if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1)))
369 {
370 if (witness)
371 {
372 btVec3Scale(&d1, s);
373 btVec3Scale(&d2, t);
374 btVec3Copy(witness, x0);
375 ccdVec3Add(witness, &d1);
376 ccdVec3Add(witness, &d2);
377
378 dist = ccdVec3Dist2(witness, P);
379 }
380 else
381 {
382 dist = s * s * v;
383 dist += t * t * w;
384 dist += btScalar(2.) * s * t * r;
385 dist += btScalar(2.) * s * p;
386 dist += btScalar(2.) * t * q;
387 dist += u;
388 }
389 }
390 else
391 {
392 dist = btVec3PointSegmentDist2(P, x0, B, witness);
393
394 dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
395 if (dist2 < dist)
396 {
397 dist = dist2;
398 if (witness)
399 btVec3Copy(witness, &witness2);
400 }
401
402 dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
403 if (dist2 < dist)
404 {
405 dist = dist2;
406 if (witness)
407 btVec3Copy(witness, &witness2);
408 }
409 }
410
411 return dist;
412}
413
414 static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
415{
416 const btSupportVector *A, *B;
417 btVector3 AB, AO, tmp;
418 btScalar dot;
419
420 // get last added as A
421 A = ccdSimplexLast(simplex);
422 // get the other point
423 B = btSimplexPoint(simplex, 0);
424 // compute AB oriented segment
425 btVec3Sub2(&AB, &B->v, &A->v);
426 // compute AO vector
427 btVec3Copy(&AO, &A->v);
428 btVec3Scale(&AO, -btScalar(1));
429
430 // dot product AB . AO
431 dot = btVec3Dot(&AB, &AO);
432
433 // check if origin doesn't lie on AB segment
434 btVec3Cross(&tmp, &AB, &AO);
435 if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
436 {
437 return 1;
438 }
439
440 // check if origin is in area where AB segment is
441 if (btFuzzyZero(dot) || dot < btScalar(0))
442 {
443 // origin is in outside are of A
444 btSimplexSet(simplex, 0, A);
445 btSimplexSetSize(simplex, 1);
446 btVec3Copy(dir, &AO);
447 }
448 else
449 {
450 // origin is in area where AB segment is
451
452 // keep simplex untouched and set direction to
453 // AB x AO x AB
454 btTripleCross(&AB, &AO, &AB, dir);
455 }
456
457 return 0;
458}
459
460 static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
461{
462 const btSupportVector *A, *B, *C;
463 btVector3 AO, AB, AC, ABC, tmp;
464 btScalar dot, dist;
465
466 // get last added as A
467 A = ccdSimplexLast(simplex);
468 // get the other points
469 B = btSimplexPoint(simplex, 1);
470 C = btSimplexPoint(simplex, 0);
471
472 // check touching contact
473 dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
474 if (btFuzzyZero(dist))
475 {
476 return 1;
477 }
478
479 // check if triangle is really triangle (has area > 0)
480 // if not simplex can't be expanded and thus no itersection is found
481 if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))
482 {
483 return -1;
484 }
485
486 // compute AO vector
487 btVec3Copy(&AO, &A->v);
488 btVec3Scale(&AO, -btScalar(1));
489
490 // compute AB and AC segments and ABC vector (perpendircular to triangle)
491 btVec3Sub2(&AB, &B->v, &A->v);
492 btVec3Sub2(&AC, &C->v, &A->v);
493 btVec3Cross(&ABC, &AB, &AC);
494
495 btVec3Cross(&tmp, &ABC, &AC);
496 dot = btVec3Dot(&tmp, &AO);
497 if (btFuzzyZero(dot) || dot > btScalar(0))
498 {
499 dot = btVec3Dot(&AC, &AO);
500 if (btFuzzyZero(dot) || dot > btScalar(0))
501 {
502 // C is already in place
503 btSimplexSet(simplex, 1, A);
504 btSimplexSetSize(simplex, 2);
505 btTripleCross(&AC, &AO, &AC, dir);
506 }
507 else
508 {
509 dot = btVec3Dot(&AB, &AO);
510 if (btFuzzyZero(dot) || dot > btScalar(0))
511 {
512 btSimplexSet(simplex, 0, B);
513 btSimplexSet(simplex, 1, A);
514 btSimplexSetSize(simplex, 2);
515 btTripleCross(&AB, &AO, &AB, dir);
516 }
517 else
518 {
519 btSimplexSet(simplex, 0, A);
520 btSimplexSetSize(simplex, 1);
521 btVec3Copy(dir, &AO);
522 }
523 }
524 }
525 else
526 {
527 btVec3Cross(&tmp, &AB, &ABC);
528 dot = btVec3Dot(&tmp, &AO);
529 if (btFuzzyZero(dot) || dot > btScalar(0))
530 {
531 dot = btVec3Dot(&AB, &AO);
532 if (btFuzzyZero(dot) || dot > btScalar(0))
533 {
534 btSimplexSet(simplex, 0, B);
535 btSimplexSet(simplex, 1, A);
536 btSimplexSetSize(simplex, 2);
537 btTripleCross(&AB, &AO, &AB, dir);
538 }
539 else
540 {
541 btSimplexSet(simplex, 0, A);
542 btSimplexSetSize(simplex, 1);
543 btVec3Copy(dir, &AO);
544 }
545 }
546 else
547 {
548 dot = btVec3Dot(&ABC, &AO);
549 if (btFuzzyZero(dot) || dot > btScalar(0))
550 {
551 btVec3Copy(dir, &ABC);
552 }
553 else
554 {
555 btSupportVector tmp;
556 btSupportCopy(&tmp, C);
557 btSimplexSet(simplex, 0, B);
558 btSimplexSet(simplex, 1, &tmp);
559
560 btVec3Copy(dir, &ABC);
561 btVec3Scale(dir, -btScalar(1));
562 }
563 }
564 }
565
566 return 0;
567}
568
569 static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
570{
571 const btSupportVector *A, *B, *C, *D;
572 btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
573 int B_on_ACD, C_on_ADB, D_on_ABC;
574 int AB_O, AC_O, AD_O;
575 btScalar dist;
576
577 // get last added as A
578 A = ccdSimplexLast(simplex);
579 // get the other points
580 B = btSimplexPoint(simplex, 2);
581 C = btSimplexPoint(simplex, 1);
582 D = btSimplexPoint(simplex, 0);
583
584 // check if tetrahedron is really tetrahedron (has volume > 0)
585 // if it is not simplex can't be expanded and thus no intersection is
586 // found
587 dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
588 if (btFuzzyZero(dist))
589 {
590 return -1;
591 }
592
593 // check if origin lies on some of tetrahedron's face - if so objects
594 // intersect
595 dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
596 if (btFuzzyZero(dist))
597 return 1;
598 dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
599 if (btFuzzyZero(dist))
600 return 1;
601 dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
602 if (btFuzzyZero(dist))
603 return 1;
604 dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
605 if (btFuzzyZero(dist))
606 return 1;
607
608 // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
609 btVec3Copy(&AO, &A->v);
610 btVec3Scale(&AO, -btScalar(1));
611 btVec3Sub2(&AB, &B->v, &A->v);
612 btVec3Sub2(&AC, &C->v, &A->v);
613 btVec3Sub2(&AD, &D->v, &A->v);
614 btVec3Cross(&ABC, &AB, &AC);
615 btVec3Cross(&ACD, &AC, &AD);
616 btVec3Cross(&ADB, &AD, &AB);
617
618 // side (positive or negative) of B, C, D relative to planes ACD, ADB
619 // and ABC respectively
620 B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
621 C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
622 D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
623
624 // whether origin is on same side of ACD, ADB, ABC as B, C, D
625 // respectively
626 AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
627 AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
628 AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
629
630 if (AB_O && AC_O && AD_O)
631 {
632 // origin is in tetrahedron
633 return 1;
634 // rearrange simplex to triangle and call btDoSimplex3()
635 }
636 else if (!AB_O)
637 {
638 // B is farthest from the origin among all of the tetrahedron's
639 // points, so remove it from the list and go on with the triangle
640 // case
641
642 // D and C are in place
643 btSimplexSet(simplex, 2, A);
644 btSimplexSetSize(simplex, 3);
645 }
646 else if (!AC_O)
647 {
648 // C is farthest
649 btSimplexSet(simplex, 1, D);
650 btSimplexSet(simplex, 0, B);
651 btSimplexSet(simplex, 2, A);
652 btSimplexSetSize(simplex, 3);
653 }
654 else
655 { // (!AD_O)
656 btSimplexSet(simplex, 0, C);
657 btSimplexSet(simplex, 1, B);
658 btSimplexSet(simplex, 2, A);
659 btSimplexSetSize(simplex, 3);
660 }
661
662 return btDoSimplex3(simplex, dir);
663}
664
665 static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
666{
667 if (btSimplexSize(simplex) == 2)
668 {
669 // simplex contains segment only one segment
670 return btDoSimplex2(simplex, dir);
671 }
672 else if (btSimplexSize(simplex) == 3)
673 {
674 // simplex contains triangle
675 return btDoSimplex3(simplex, dir);
676 }
677 else
678 { // btSimplexSize(simplex) == 4
679 // tetrahedron - this is the only shape which can encapsule origin
680 // so btDoSimplex4() also contains test on it
681 return btDoSimplex4(simplex, dir);
682 }
683}
684
685#ifdef __SPU__
686void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
687#else
688 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
689#endif
690{
691 m_cachedSeparatingDistance = 0.f;
692
693 btScalar distance = btScalar(0.);
694 btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
695
696 btVector3 pointOnA, pointOnB;
697 btTransform localTransA = input.m_transformA;
698 btTransform localTransB = input.m_transformB;
699 btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
700 localTransA.getOrigin() -= positionOffset;
701 localTransB.getOrigin() -= positionOffset;
702
703 bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
704
705 btScalar marginA = m_marginA;
706 btScalar marginB = m_marginB;
707
708
709 //for CCD we don't use margins
710 if (m_ignoreMargin)
711 {
712 marginA = btScalar(0.);
713 marginB = btScalar(0.);
714 }
715
716 m_curIter = 0;
717 int gGjkMaxIter = 1000; //this is to catch invalid input, perhaps check for #NaN?
718 m_cachedSeparatingAxis.setValue(0, 1, 0);
719
720 bool isValid = false;
721 bool checkSimplex = false;
722 bool checkPenetration = true;
723 m_degenerateSimplex = 0;
724
725 m_lastUsedMethod = -1;
726 int status = -2;
727 btVector3 orgNormalInB(0, 0, 0);
728 btScalar margin = marginA + marginB;
729
730 //we add a separate implementation to check if the convex shapes intersect
731 //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
732 //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
733 //and remove this temporary code from libCCD
734 //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
735 //note, for large differences in shapes, use double precision build!
736 {
737 btScalar squaredDistance = BT_LARGE_FLOAT;
738 btScalar delta = btScalar(0.);
739
740 btSimplex simplex1;
741 btSimplex *simplex = &simplex1;
742 btSimplexInit(simplex);
743
744 btVector3 dir(1, 0, 0);
745
746 {
747 btVector3 lastSupV;
748 btVector3 supAworld;
749 btVector3 supBworld;
750 btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
751
752 btSupportVector last;
753 last.v = lastSupV;
754 last.v1 = supAworld;
755 last.v2 = supBworld;
756
757 btSimplexAdd(simplex, &last);
758
759 dir = -lastSupV;
760
761 // start iterations
762 for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
763 {
764 // obtain support point
765 btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
766
767 // check if farthest point in Minkowski difference in direction dir
768 // isn't somewhere before origin (the test on negative dot product)
769 // - because if it is, objects are not intersecting at all.
770 btScalar delta = lastSupV.dot(dir);
771 if (delta < 0)
772 {
773 //no intersection, besides margin
774 status = -1;
775 break;
776 }
777
778 // add last support vector to simplex
779 last.v = lastSupV;
780 last.v1 = supAworld;
781 last.v2 = supBworld;
782
783 btSimplexAdd(simplex, &last);
784
785 // if btDoSimplex returns 1 if objects intersect, -1 if objects don't
786 // intersect and 0 if algorithm should continue
787
788 btVector3 newDir;
789 int do_simplex_res = btDoSimplex(simplex, &dir);
790
791 if (do_simplex_res == 1)
792 {
793 status = 0; // intersection found
794 break;
795 }
796 else if (do_simplex_res == -1)
797 {
798 // intersection not found
799 status = -1;
800 break;
801 }
802
803 if (btFuzzyZero(btVec3Dot(&dir, &dir)))
804 {
805 // intersection not found
806 status = -1;
807 }
808
809 if (dir.length2() < SIMD_EPSILON)
810 {
811 //no intersection, besides margin
812 status = -1;
813 break;
814 }
815
816 if (dir.fuzzyZero())
817 {
818 // intersection not found
819 status = -1;
820 break;
821 }
822 }
823 }
824
825 m_simplexSolver->reset();
826 if (status == 0)
827 {
828 //status = 0;
829 //printf("Intersect!\n");
830 }
831
832 if (status == -1)
833 {
834 //printf("not intersect\n");
835 }
836 //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
837 if (1)
838 {
839 for (;;)
840 //while (true)
841 {
842 btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
843 btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
844
845 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
846 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
847
848 btVector3 pWorld = localTransA(pInA);
849 btVector3 qWorld = localTransB(qInB);
850
851 if (check2d)
852 {
853 pWorld[2] = 0.f;
854 qWorld[2] = 0.f;
855 }
856
857 btVector3 w = pWorld - qWorld;
858 delta = m_cachedSeparatingAxis.dot(w);
859
860 // potential exit, they don't overlap
861 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
862 {
863 m_degenerateSimplex = 10;
864 checkSimplex = true;
865 //checkPenetration = false;
866 break;
867 }
868
869 //exit 0: the new point is already in the simplex, or we didn't come any closer
870 if (m_simplexSolver->inSimplex(w))
871 {
872 m_degenerateSimplex = 1;
873 checkSimplex = true;
874 break;
875 }
876 // are we getting any closer ?
877 btScalar f0 = squaredDistance - delta;
878 btScalar f1 = squaredDistance * REL_ERROR2;
879
880 if (f0 <= f1)
881 {
882 if (f0 <= btScalar(0.))
883 {
884 m_degenerateSimplex = 2;
885 }
886 else
887 {
888 m_degenerateSimplex = 11;
889 }
890 checkSimplex = true;
891 break;
892 }
893
894 //add current vertex to simplex
895 m_simplexSolver->addVertex(w, pWorld, qWorld);
896 btVector3 newCachedSeparatingAxis;
897
898 //calculate the closest point to the origin (update vector v)
899 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
900 {
901 m_degenerateSimplex = 3;
902 checkSimplex = true;
903 break;
904 }
905
906 if (newCachedSeparatingAxis.length2() < REL_ERROR2)
907 {
908 m_cachedSeparatingAxis = newCachedSeparatingAxis;
909 m_degenerateSimplex = 6;
910 checkSimplex = true;
911 break;
912 }
913
914 btScalar previousSquaredDistance = squaredDistance;
915 squaredDistance = newCachedSeparatingAxis.length2();
916#if 0
918 if (squaredDistance > previousSquaredDistance)
919 {
920 m_degenerateSimplex = 7;
921 squaredDistance = previousSquaredDistance;
922 checkSimplex = false;
923 break;
924 }
925#endif //
926
927 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
928
929 //are we getting any closer ?
930 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
931 {
932 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
933 checkSimplex = true;
934 m_degenerateSimplex = 12;
935
936 break;
937 }
938
939 m_cachedSeparatingAxis = newCachedSeparatingAxis;
940
941 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
942 if (m_curIter++ > gGjkMaxIter)
943 {
944#if defined(DEBUG) || defined(_DEBUG)
945
946 printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
947 printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
948 m_cachedSeparatingAxis.getX(),
949 m_cachedSeparatingAxis.getY(),
950 m_cachedSeparatingAxis.getZ(),
951 squaredDistance,
952 m_minkowskiA->getShapeType(),
953 m_minkowskiB->getShapeType());
954
955#endif
956 break;
957 }
958
959 bool check = (!m_simplexSolver->fullSimplex());
960 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
961
962 if (!check)
963 {
964 //do we need this backup_closest here ?
965 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
966 m_degenerateSimplex = 13;
967 break;
968 }
969 }
970
971 if (checkSimplex)
972 {
973 m_simplexSolver->compute_points(pointOnA, pointOnB);
974 normalInB = m_cachedSeparatingAxis;
975
976 btScalar lenSqr = m_cachedSeparatingAxis.length2();
977
978 //valid normal
979 if (lenSqr < REL_ERROR2)
980 {
981 m_degenerateSimplex = 5;
982 }
983 if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
984 {
985 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
986 normalInB *= rlen; //normalize
987
988 btScalar s = btSqrt(squaredDistance);
989
990 btAssert(s > btScalar(0.0));
991 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
992 pointOnB += m_cachedSeparatingAxis * (marginB / s);
993 distance = ((btScalar(1.) / rlen) - margin);
994 isValid = true;
995 orgNormalInB = normalInB;
996
997 m_lastUsedMethod = 1;
998 }
999 else
1000 {
1001 m_lastUsedMethod = 2;
1002 }
1003 }
1004 }
1005
1006 bool catchDegeneratePenetrationCase =
1007 (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance));
1008
1009 //if (checkPenetration && !isValid)
1010 if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
1011 {
1012 //penetration case
1013
1014 //if there is no way to handle penetrations, bail out
1015 if (m_penetrationDepthSolver)
1016 {
1017 // Penetration depth case.
1018 btVector3 tmpPointOnA, tmpPointOnB;
1019
1020 m_cachedSeparatingAxis.setZero();
1021
1022 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
1023 *m_simplexSolver,
1024 m_minkowskiA, m_minkowskiB,
1025 localTransA, localTransB,
1026 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
1027 debugDraw);
1028
1029 if (m_cachedSeparatingAxis.length2())
1030 {
1031 if (isValid2)
1032 {
1033 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
1034 btScalar lenSqr = tmpNormalInB.length2();
1035 if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
1036 {
1037 tmpNormalInB = m_cachedSeparatingAxis;
1038 lenSqr = m_cachedSeparatingAxis.length2();
1039 }
1040
1041 if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
1042 {
1043 tmpNormalInB /= btSqrt(lenSqr);
1044 btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
1045 m_lastUsedMethod = 3;
1046 //only replace valid penetrations when the result is deeper (check)
1047 if (!isValid || (distance2 < distance))
1048 {
1049 distance = distance2;
1050 pointOnA = tmpPointOnA;
1051 pointOnB = tmpPointOnB;
1052 normalInB = tmpNormalInB;
1053 isValid = true;
1054 }
1055 else
1056 {
1057 m_lastUsedMethod = 8;
1058 }
1059 }
1060 else
1061 {
1062 m_lastUsedMethod = 9;
1063 }
1064 }
1065 else
1066
1067 {
1073
1074 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
1075 {
1076 btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
1077 //only replace valid distances when the distance is less
1078 if (!isValid || (distance2 < distance))
1079 {
1080 distance = distance2;
1081 pointOnA = tmpPointOnA;
1082 pointOnB = tmpPointOnB;
1083 pointOnA -= m_cachedSeparatingAxis * marginA;
1084 pointOnB += m_cachedSeparatingAxis * marginB;
1085 normalInB = m_cachedSeparatingAxis;
1086 normalInB.normalize();
1087
1088 isValid = true;
1089 m_lastUsedMethod = 6;
1090 }
1091 else
1092 {
1093 m_lastUsedMethod = 5;
1094 }
1095 }
1096 }
1097 }
1098 else
1099 {
1100 //printf("EPA didn't return a valid value\n");
1101 }
1102 }
1103 }
1104 }
1105
1106 if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
1107 {
1108 m_cachedSeparatingAxis = normalInB;
1109 m_cachedSeparatingDistance = distance;
1110 if (1)
1111 {
1116
1117 btScalar d2 = 0.f;
1118 {
1119 btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
1120 btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis();
1121
1122 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1123 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1124
1125 btVector3 pWorld = localTransA(pInA);
1126 btVector3 qWorld = localTransB(qInB);
1127 btVector3 w = pWorld - qWorld;
1128 d2 = orgNormalInB.dot(w) - margin;
1129 }
1130
1131 btScalar d1 = 0;
1132 {
1133 btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis();
1134 btVector3 separatingAxisInB = -normalInB * localTransB.getBasis();
1135
1136 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1137 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1138
1139 btVector3 pWorld = localTransA(pInA);
1140 btVector3 qWorld = localTransB(qInB);
1141 btVector3 w = pWorld - qWorld;
1142 d1 = (-normalInB).dot(w) - margin;
1143 }
1144 btScalar d0 = 0.f;
1145 {
1146 btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
1147 btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis();
1148
1149 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
1150 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
1151
1152 btVector3 pWorld = localTransA(pInA);
1153 btVector3 qWorld = localTransB(qInB);
1154 btVector3 w = pWorld - qWorld;
1155 d0 = normalInB.dot(w) - margin;
1156 }
1157
1158 if (d1 > d0)
1159 {
1160 m_lastUsedMethod = 10;
1161 normalInB *= -1;
1162 }
1163
1164 if (orgNormalInB.length2())
1165 {
1166 if (d2 > d0 && d2 > d1 && d2 > distance)
1167 {
1168 normalInB = orgNormalInB;
1169 distance = d2;
1170 }
1171 }
1172 }
1173
1174 output.addContactPoint(
1175 normalInB,
1176 pointOnB + positionOffset,
1177 distance);
1178 }
1179 else
1180 {
1181 //printf("invalid gjk query\n");
1182 }
1183}
btConvexPenetrationDepthSolver.h
btConvexShape.h
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition btDbvt.cpp:52
btSimplexPoint
const btSupportVector * btSimplexPoint(const btSimplex *s, int idx)
Definition btGjkPairDetector.cpp:128
btVec3PointTriDist2
btScalar btVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
Definition btGjkPairDetector.cpp:335
btVec3Eq
int btVec3Eq(const btVector3 *a, const btVector3 *b)
Definition btGjkPairDetector.cpp:230
btSimplexSize
int btSimplexSize(const btSimplex *s)
Definition btGjkPairDetector.cpp:123
btComputeSupport
static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
Definition btGjkPairDetector.cpp:80
ccdVec3Sub
void ccdVec3Sub(btVector3 *v, const btVector3 *w)
Definition btGjkPairDetector.cpp:150
ccdVec3X
btScalar ccdVec3X(const btVector3 *v)
Definition btGjkPairDetector.cpp:216
btSupportCopy
void btSupportCopy(btSupportVector *d, const btSupportVector *s)
Definition btGjkPairDetector.cpp:133
btSimplexSet
void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
Definition btGjkPairDetector.cpp:242
REL_ERROR2
#define REL_ERROR2
Definition btGjkPairDetector.cpp:35
btVec3PointSegmentDist2
btScalar btVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
Definition btGjkPairDetector.cpp:270
gGjkEpaPenetrationTolerance
btScalar gGjkEpaPenetrationTolerance
Definition btGjkPairDetector.cpp:36
btVec3Copy
void btVec3Copy(btVector3 *v, const btVector3 *w)
Definition btGjkPairDetector.cpp:138
btVec3Sub2
void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
Definition btGjkPairDetector.cpp:154
btVec3Cross
void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
Definition btGjkPairDetector.cpp:180
btVec3Dot
btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
Definition btGjkPairDetector.cpp:158
btSimplexInit
void btSimplexInit(btSimplex *s)
Definition btGjkPairDetector.cpp:118
ccd_vec3_origin
static btVector3 ccd_vec3_origin(0, 0, 0)
btTripleCross
void btTripleCross(const btVector3 *a, const btVector3 *b, const btVector3 *c, btVector3 *d)
Definition btGjkPairDetector.cpp:187
ccdVec3Z
btScalar ccdVec3Z(const btVector3 *v)
Definition btGjkPairDetector.cpp:226
btVec3Scale
void btVec3Scale(btVector3 *d, btScalar k)
Definition btGjkPairDetector.cpp:173
btSimplexAdd
void btSimplexAdd(btSimplex *s, const btSupportVector *v)
Definition btGjkPairDetector.cpp:235
ccdSimplexLast
const btSupportVector * ccdSimplexLast(const btSimplex *s)
Definition btGjkPairDetector.cpp:252
btDoSimplex4
static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
Definition btGjkPairDetector.cpp:569
ccdVec3Add
void ccdVec3Add(btVector3 *v, const btVector3 *w)
Definition btGjkPairDetector.cpp:143
btDoSimplex3
static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
Definition btGjkPairDetector.cpp:460
ccdVec3Y
btScalar ccdVec3Y(const btVector3 *v)
Definition btGjkPairDetector.cpp:221
ccdSign
int ccdSign(btScalar val)
Definition btGjkPairDetector.cpp:257
ccdVec3Dist2
btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
Definition btGjkPairDetector.cpp:166
btSimplexSetSize
void btSimplexSetSize(btSimplex *s, int size)
Definition btGjkPairDetector.cpp:247
btDoSimplex
static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
Definition btGjkPairDetector.cpp:665
btDoSimplex2
static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
Definition btGjkPairDetector.cpp:414
ccdEq
int ccdEq(btScalar _a, btScalar _b)
Definition btGjkPairDetector.cpp:195
btGjkPairDetector.h
output
#define output
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:888
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition btQuaternion.h:895
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition btScalar.h:316
btSqrt
btScalar btSqrt(btScalar y)
Definition btScalar.h:466
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:497
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition btScalar.h:572
btAssert
#define btAssert(x)
Definition btScalar.h:153
btSimplexSolverInterface.h
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition btSimplexSolverInterface.h:24
btConvexPenetrationDepthSolver
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
Definition btConvexPenetrationDepthSolver.h:26
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition btConvexShape.h:33
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition btConvexShape.cpp:131
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition btGjkPairDetector.cpp:73
btGjkPairDetector::btGjkPairDetector
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
Definition btGjkPairDetector.cpp:40
btGjkPairDetector::getClosestPointsNonVirtual
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
Definition btGjkPairDetector.cpp:688
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition btIDebugDraw.h:27
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition btTransform.h:109
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition btTransform.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::z
const btScalar & z() const
Return the z value.
Definition btVector3.h:579
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
btVector3::m_floats
btScalar m_floats[4]
Definition btVector3.h:111
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:251
btVector3::x
const btScalar & x() const
Return the x value.
Definition btVector3.h:575
btVector3::setZero
void setZero()
Definition btVector3.h:671
btVector3::fuzzyZero
bool fuzzyZero() const
Definition btVector3.h:688
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition btVector3.h:303
btVector3::y
const btScalar & y() const
Return the y value.
Definition btVector3.h:577
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition btDiscreteCollisionDetectorInterface.h:40
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition btDiscreteCollisionDetectorInterface.h:47
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition btDiscreteCollisionDetectorInterface.h:46
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition btDiscreteCollisionDetectorInterface.h:48
btDiscreteCollisionDetectorInterface::Result
Definition btDiscreteCollisionDetectorInterface.h:30
btSimplex
Definition btGjkPairDetector.cpp:111
btSimplex::last
int last
index of last added point
Definition btGjkPairDetector.cpp:113
btSimplex::ps
btSupportVector ps[4]
Definition btGjkPairDetector.cpp:112
btSupportVector
Definition btGjkPairDetector.cpp:104
btSupportVector::v2
btVector3 v2
Support point in obj2.
Definition btGjkPairDetector.cpp:107
btSupportVector::v1
btVector3 v1
Support point in obj1.
Definition btGjkPairDetector.cpp:106
btSupportVector::v
btVector3 v
Support point in minkowski sum.
Definition btGjkPairDetector.cpp:105