1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
17///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
18///with reproduction case
19//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
20//#define ZERO_MARGIN
21
22#include "btConvexConvexAlgorithm.h"
23
24//#include <stdio.h>
25#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
26#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
27#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
28#include "BulletCollision/CollisionShapes/btConvexShape.h"
29#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
30#include "BulletCollision/CollisionShapes/btTriangleShape.h"
31
32
33
34#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
35#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
36#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
37#include "BulletCollision/CollisionShapes/btBoxShape.h"
38#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
39
40#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
41#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
42#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
43#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
44
45
46
47#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
48#include "BulletCollision/CollisionShapes/btSphereShape.h"
49
50#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
51
52#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
53#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
54#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
55#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
56
57///////////
58
59
60
61static SIMD_FORCE_INLINE void segmentsClosestPoints(
62	btVector3& ptsVector,
63	btVector3& offsetA,
64	btVector3& offsetB,
65	btScalar& tA, btScalar& tB,
66	const btVector3& translation,
67	const btVector3& dirA, btScalar hlenA,
68	const btVector3& dirB, btScalar hlenB )
69{
70	// compute the parameters of the closest points on each line segment
71
72	btScalar dirA_dot_dirB = btDot(dirA,dirB);
73	btScalar dirA_dot_trans = btDot(dirA,translation);
74	btScalar dirB_dot_trans = btDot(dirB,translation);
75
76	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
77
78	if ( denom == 0.0f ) {
79		tA = 0.0f;
80	} else {
81		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
82		if ( tA < -hlenA )
83			tA = -hlenA;
84		else if ( tA > hlenA )
85			tA = hlenA;
86	}
87
88	tB = tA * dirA_dot_dirB - dirB_dot_trans;
89
90	if ( tB < -hlenB ) {
91		tB = -hlenB;
92		tA = tB * dirA_dot_dirB + dirA_dot_trans;
93
94		if ( tA < -hlenA )
95			tA = -hlenA;
96		else if ( tA > hlenA )
97			tA = hlenA;
98	} else if ( tB > hlenB ) {
99		tB = hlenB;
100		tA = tB * dirA_dot_dirB + dirA_dot_trans;
101
102		if ( tA < -hlenA )
103			tA = -hlenA;
104		else if ( tA > hlenA )
105			tA = hlenA;
106	}
107
108	// compute the closest points relative to segment centers.
109
110	offsetA = dirA * tA;
111	offsetB = dirB * tB;
112
113	ptsVector = translation - offsetA + offsetB;
114}
115
116
117static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
118	btVector3& normalOnB,
119	btVector3& pointOnB,
120	btScalar capsuleLengthA,
121	btScalar	capsuleRadiusA,
122	btScalar capsuleLengthB,
123	btScalar	capsuleRadiusB,
124	int capsuleAxisA,
125	int capsuleAxisB,
126	const btTransform& transformA,
127	const btTransform& transformB,
128	btScalar distanceThreshold )
129{
130	btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
131	btVector3 translationA = transformA.getOrigin();
132	btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
133	btVector3 translationB = transformB.getOrigin();
134
135	// translation between centers
136
137	btVector3 translation = translationB - translationA;
138
139	// compute the closest points of the capsule line segments
140
141	btVector3 ptsVector;           // the vector between the closest points
142
143	btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
144	btScalar tA, tB;              // parameters on line segment
145
146	segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
147						   directionA, capsuleLengthA, directionB, capsuleLengthB );
148
149	btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
150
151	if ( distance > distanceThreshold )
152		return distance;
153
154	btScalar lenSqr = ptsVector.length2();
155	if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
156	{
157		//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
158		btVector3 q;
159		btPlaneSpace1(directionA,normalOnB,q);
160	} else
161	{
162		// compute the contact normal
163		normalOnB = ptsVector*-btRecipSqrt(lenSqr);
164	}
165	pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
166
167	return distance;
168}
169
170
171
172
173
174
175
176//////////
177
178
179
180
181
182btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
183{
184	m_numPerturbationIterations = 0;
185	m_minimumPointsPerturbationThreshold = 3;
186	m_simplexSolver = simplexSolver;
187	m_pdSolver = pdSolver;
188}
189
190btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
191{
192}
193
194btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
195: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
196m_simplexSolver(simplexSolver),
197m_pdSolver(pdSolver),
198m_ownManifold (false),
199m_manifoldPtr(mf),
200m_lowLevelOfDetail(false),
201#ifdef USE_SEPDISTANCE_UTIL2
202m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
203			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
204#endif
205m_numPerturbationIterations(numPerturbationIterations),
206m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
207{
208	(void)body0Wrap;
209	(void)body1Wrap;
210}
211
212
213
214
215btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
216{
217	if (m_ownManifold)
218	{
219		if (m_manifoldPtr)
220			m_dispatcher->releaseManifold(m_manifoldPtr);
221	}
222}
223
224void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
225{
226	m_lowLevelOfDetail = useLowLevel;
227}
228
229
230struct btPerturbedContactResult : public btManifoldResult
231{
232	btManifoldResult* m_originalManifoldResult;
233	btTransform m_transformA;
234	btTransform m_transformB;
235	btTransform	m_unPerturbedTransform;
236	bool	m_perturbA;
237	btIDebugDraw*	m_debugDrawer;
238
239
240	btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
241		:m_originalManifoldResult(originalResult),
242		m_transformA(transformA),
243		m_transformB(transformB),
244		m_unPerturbedTransform(unPerturbedTransform),
245		m_perturbA(perturbA),
246		m_debugDrawer(debugDrawer)
247	{
248	}
249	virtual ~ btPerturbedContactResult()
250	{
251	}
252
253	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
254	{
255		btVector3 endPt,startPt;
256		btScalar newDepth;
257		btVector3 newNormal;
258
259		if (m_perturbA)
260		{
261			btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
262			endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
263			newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
264			startPt = endPt+normalOnBInWorld*newDepth;
265		} else
266		{
267			endPt = pointInWorld + normalOnBInWorld*orgDepth;
268			startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
269			newDepth = (endPt -  startPt).dot(normalOnBInWorld);
270
271		}
272
273//#define DEBUG_CONTACTS 1
274#ifdef DEBUG_CONTACTS
275		m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
276		m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
277		m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
278#endif //DEBUG_CONTACTS
279
280
281		m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
282	}
283
284};
285
286extern btScalar gContactBreakingThreshold;
287
288
289//
290// Convex-Convex collision algorithm
291//
292void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
293{
294
295	if (!m_manifoldPtr)
296	{
297		//swapped?
298		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
299		m_ownManifold = true;
300	}
301	resultOut->setPersistentManifold(m_manifoldPtr);
302
303	//comment-out next line to test multi-contact generation
304	//resultOut->getPersistentManifold()->clearManifold();
305
306
307	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
308	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
309
310	btVector3  normalOnB;
311		btVector3  pointOnBWorld;
312#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
313	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
314	{
315		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
316		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
317	//	btVector3 localScalingA = capsuleA->getLocalScaling();
318	//	btVector3 localScalingB = capsuleB->getLocalScaling();
319
320		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
321
322		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
323			capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
324			body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
325
326		if (dist<threshold)
327		{
328			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
329			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
330		}
331		resultOut->refreshContactPoints();
332		return;
333	}
334#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
335
336
337
338
339#ifdef USE_SEPDISTANCE_UTIL2
340	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
341	{
342		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
343	}
344
345	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
346#endif //USE_SEPDISTANCE_UTIL2
347
348	{
349
350
351	btGjkPairDetector::ClosestPointInput input;
352
353	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
354	//TODO: if (dispatchInfo.m_useContinuous)
355	gjkPairDetector.setMinkowskiA(min0);
356	gjkPairDetector.setMinkowskiB(min1);
357
358#ifdef USE_SEPDISTANCE_UTIL2
359	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
360	{
361		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
362	} else
363#endif //USE_SEPDISTANCE_UTIL2
364	{
365		//if (dispatchInfo.m_convexMaxDistanceUseCPT)
366		//{
367		//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
368		//} else
369		//{
370		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
371//		}
372
373		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
374	}
375
376	input.m_transformA = body0Wrap->getWorldTransform();
377	input.m_transformB = body1Wrap->getWorldTransform();
378
379
380
381
382
383#ifdef USE_SEPDISTANCE_UTIL2
384	btScalar sepDist = 0.f;
385	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
386	{
387		sepDist = gjkPairDetector.getCachedSeparatingDistance();
388		if (sepDist>SIMD_EPSILON)
389		{
390			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
391			//now perturbe directions to get multiple contact points
392
393		}
394	}
395#endif //USE_SEPDISTANCE_UTIL2
396
397	if (min0->isPolyhedral() && min1->isPolyhedral())
398	{
399
400
401		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
402		{
403			virtual void setShapeIdentifiersA(int partId0,int index0){}
404			virtual void setShapeIdentifiersB(int partId1,int index1){}
405			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
406			{
407			}
408		};
409
410
411		struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
412		{
413			btDiscreteCollisionDetectorInterface::Result* m_originalResult;
414			btVector3	m_reportedNormalOnWorld;
415			btScalar m_marginOnA;
416			btScalar m_marginOnB;
417			btScalar	m_reportedDistance;
418
419			bool		m_foundResult;
420			btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
421			:m_originalResult(result),
422			m_marginOnA(marginOnA),
423			m_marginOnB(marginOnB),
424			m_foundResult(false)
425			{
426			}
427
428			virtual void setShapeIdentifiersA(int partId0,int index0){}
429			virtual void setShapeIdentifiersB(int partId1,int index1){}
430			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
431			{
432				m_reportedDistance = depthOrg;
433				m_reportedNormalOnWorld = normalOnBInWorld;
434
435				btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
436				m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
437				if (m_reportedDistance<0.f)
438				{
439					m_foundResult = true;
440				}
441				m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
442			}
443		};
444
445
446		btDummyResult dummy;
447
448///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
449
450		btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
451		btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
452
453		btWithoutMarginResult	withoutMargin(resultOut, min0Margin,min1Margin);
454
455		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
456		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
457		if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
458		{
459
460
461
462
463			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
464
465			btScalar minDist = -1e30f;
466			btVector3 sepNormalWorldSpace;
467			bool foundSepAxis  = true;
468
469			if (dispatchInfo.m_enableSatConvex)
470			{
471				foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
472					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
473					body0Wrap->getWorldTransform(),
474					body1Wrap->getWorldTransform(),
475					sepNormalWorldSpace,*resultOut);
476			} else
477			{
478#ifdef ZERO_MARGIN
479				gjkPairDetector.setIgnoreMargin(true);
480				gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
481#else
482
483
484				gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
485				//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
486#endif //ZERO_MARGIN
487				//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
488				//if (l2>SIMD_EPSILON)
489				{
490					sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
491					//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
492					minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
493
494#ifdef ZERO_MARGIN
495					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
496#else
497					foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
498#endif
499				}
500			}
501			if (foundSepAxis)
502			{
503
504//				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
505
506				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
507					body0Wrap->getWorldTransform(),
508					body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
509
510			}
511			if (m_ownManifold)
512			{
513				resultOut->refreshContactPoints();
514			}
515			return;
516
517		} else
518		{
519			//we can also deal with convex versus triangle (without connectivity data)
520			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
521			{
522
523				btVertexArray vertices;
524				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
525				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
526				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
527				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
528
529				//tri->initializePolyhedralFeatures();
530
531				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
532
533				btVector3 sepNormalWorldSpace;
534				btScalar minDist =-1e30f;
535				btScalar maxDist = threshold;
536
537				bool foundSepAxis = false;
538				if (0)
539				{
540					polyhedronB->initializePolyhedralFeatures();
541					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
542					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
543					body0Wrap->getWorldTransform(),
544					body1Wrap->getWorldTransform(),
545					sepNormalWorldSpace,*resultOut);
546				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
547
548				} else
549				{
550#ifdef ZERO_MARGIN
551					gjkPairDetector.setIgnoreMargin(true);
552					gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
553#else
554					gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
555#endif//ZERO_MARGIN
556
557					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
558					if (l2>SIMD_EPSILON)
559					{
560						sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
561						//minDist = gjkPairDetector.getCachedSeparatingDistance();
562						//maxDist = threshold;
563						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
564						foundSepAxis = true;
565					}
566				}
567
568
569			if (foundSepAxis)
570			{
571				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
572					body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
573			}
574
575
576				if (m_ownManifold)
577				{
578					resultOut->refreshContactPoints();
579				}
580
581				return;
582			}
583
584		}
585
586
587	}
588
589	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
590
591	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
592
593	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
594	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
595	{
596
597		int i;
598		btVector3 v0,v1;
599		btVector3 sepNormalWorldSpace;
600		btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
601
602		if (l2>SIMD_EPSILON)
603		{
604			sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
605
606			btPlaneSpace1(sepNormalWorldSpace,v0,v1);
607
608
609			bool perturbeA = true;
610			const btScalar angleLimit = 0.125f * SIMD_PI;
611			btScalar perturbeAngle;
612			btScalar radiusA = min0->getAngularMotionDisc();
613			btScalar radiusB = min1->getAngularMotionDisc();
614			if (radiusA < radiusB)
615			{
616				perturbeAngle = gContactBreakingThreshold /radiusA;
617				perturbeA = true;
618			} else
619			{
620				perturbeAngle = gContactBreakingThreshold / radiusB;
621				perturbeA = false;
622			}
623			if ( perturbeAngle > angleLimit )
624					perturbeAngle = angleLimit;
625
626			btTransform unPerturbedTransform;
627			if (perturbeA)
628			{
629				unPerturbedTransform = input.m_transformA;
630			} else
631			{
632				unPerturbedTransform = input.m_transformB;
633			}
634
635			for ( i=0;i<m_numPerturbationIterations;i++)
636			{
637				if (v0.length2()>SIMD_EPSILON)
638				{
639				btQuaternion perturbeRot(v0,perturbeAngle);
640				btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
641				btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
642
643
644				if (perturbeA)
645				{
646					input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
647					input.m_transformB = body1Wrap->getWorldTransform();
648	#ifdef DEBUG_CONTACTS
649					dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
650	#endif //DEBUG_CONTACTS
651				} else
652				{
653					input.m_transformA = body0Wrap->getWorldTransform();
654					input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
655	#ifdef DEBUG_CONTACTS
656					dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
657	#endif
658				}
659
660				btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
661				gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
662				}
663			}
664		}
665	}
666
667
668
669#ifdef USE_SEPDISTANCE_UTIL2
670	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
671	{
672		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
673	}
674#endif //USE_SEPDISTANCE_UTIL2
675
676
677	}
678
679	if (m_ownManifold)
680	{
681		resultOut->refreshContactPoints();
682	}
683
684}
685
686
687
688bool disableCcd = false;
689btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
690{
691	(void)resultOut;
692	(void)dispatchInfo;
693	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
694
695	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
696	///col0->m_worldTransform,
697	btScalar resultFraction = btScalar(1.);
698
699
700	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
701	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
702
703	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
704		squareMot1 < col1->getCcdSquareMotionThreshold())
705		return resultFraction;
706
707	if (disableCcd)
708		return btScalar(1.);
709
710
711	//An adhoc way of testing the Continuous Collision Detection algorithms
712	//One object is approximated as a sphere, to simplify things
713	//Starting in penetration should report no time of impact
714	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
715	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
716
717
718	/// Convex0 against sphere for Convex1
719	{
720		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
721
722		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
723		btConvexCast::CastResult result;
724		btVoronoiSimplexSolver voronoiSimplex;
725		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
726		///Simplification, one object is simplified as a sphere
727		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
728		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
729		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
730			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
731		{
732
733			//store result.m_fraction in both bodies
734
735			if (col0->getHitFraction()> result.m_fraction)
736				col0->setHitFraction( result.m_fraction );
737
738			if (col1->getHitFraction() > result.m_fraction)
739				col1->setHitFraction( result.m_fraction);
740
741			if (resultFraction > result.m_fraction)
742				resultFraction = result.m_fraction;
743
744		}
745
746
747
748
749	}
750
751	/// Sphere (for convex0) against Convex1
752	{
753		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
754
755		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
756		btConvexCast::CastResult result;
757		btVoronoiSimplexSolver voronoiSimplex;
758		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
759		///Simplification, one object is simplified as a sphere
760		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
761		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
762		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
763			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
764		{
765
766			//store result.m_fraction in both bodies
767
768			if (col0->getHitFraction()	> result.m_fraction)
769				col0->setHitFraction( result.m_fraction);
770
771			if (col1->getHitFraction() > result.m_fraction)
772				col1->setHitFraction( result.m_fraction);
773
774			if (resultFraction > result.m_fraction)
775				resultFraction = result.m_fraction;
776
777		}
778	}
779
780	return resultFraction;
781
782}
783
784