1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btConvex2dConvex2dAlgorithm.h"
17
18 //#include <stdio.h>
19 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
20 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22 #include "BulletCollision/CollisionShapes/btConvexShape.h"
23 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
24
25
26 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
27 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
28 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
29 #include "BulletCollision/CollisionShapes/btBoxShape.h"
30 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
31
32 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
33 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
34 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
35 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
36
37
38
39 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
40 #include "BulletCollision/CollisionShapes/btSphereShape.h"
41
42 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
43
44 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
45 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
46
47
CreateFunc(btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver)48 btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
49 {
50 m_numPerturbationIterations = 0;
51 m_minimumPointsPerturbationThreshold = 3;
52 m_simplexSolver = simplexSolver;
53 m_pdSolver = pdSolver;
54 }
55
~CreateFunc()56 btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
57 {
58 }
59
btConvex2dConvex2dAlgorithm(btPersistentManifold * mf,const btCollisionAlgorithmConstructionInfo & ci,btCollisionObject * body0,btCollisionObject * body1,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver,int numPerturbationIterations,int minimumPointsPerturbationThreshold)60 btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
61 : btActivatingCollisionAlgorithm(ci,body0,body1),
62 m_simplexSolver(simplexSolver),
63 m_pdSolver(pdSolver),
64 m_ownManifold (false),
65 m_manifoldPtr(mf),
66 m_lowLevelOfDetail(false),
67 m_numPerturbationIterations(numPerturbationIterations),
68 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
69 {
70 (void)body0;
71 (void)body1;
72 }
73
74
75
76
~btConvex2dConvex2dAlgorithm()77 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
78 {
79 if (m_ownManifold)
80 {
81 if (m_manifoldPtr)
82 m_dispatcher->releaseManifold(m_manifoldPtr);
83 }
84 }
85
setLowLevelOfDetail(bool useLowLevel)86 void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
87 {
88 m_lowLevelOfDetail = useLowLevel;
89 }
90
91
92
93 extern btScalar gContactBreakingThreshold;
94
95
96 //
97 // Convex-Convex collision algorithm
98 //
processCollision(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)99 void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
100 {
101
102 if (!m_manifoldPtr)
103 {
104 //swapped?
105 m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
106 m_ownManifold = true;
107 }
108 resultOut->setPersistentManifold(m_manifoldPtr);
109
110 //comment-out next line to test multi-contact generation
111 //resultOut->getPersistentManifold()->clearManifold();
112
113
114 btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
115 btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
116
117 btVector3 normalOnB;
118 btVector3 pointOnBWorld;
119
120 {
121
122
123 btGjkPairDetector::ClosestPointInput input;
124
125 btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
126 //TODO: if (dispatchInfo.m_useContinuous)
127 gjkPairDetector.setMinkowskiA(min0);
128 gjkPairDetector.setMinkowskiB(min1);
129
130 {
131 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
132 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
133 }
134
135 input.m_stackAlloc = dispatchInfo.m_stackAllocator;
136 input.m_transformA = body0->getWorldTransform();
137 input.m_transformB = body1->getWorldTransform();
138
139 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
140
141 btVector3 v0,v1;
142 btVector3 sepNormalWorldSpace;
143
144 }
145
146 if (m_ownManifold)
147 {
148 resultOut->refreshContactPoints();
149 }
150
151 }
152
153
154
155
calculateTimeOfImpact(btCollisionObject * col0,btCollisionObject * col1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)156 btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
157 {
158 (void)resultOut;
159 (void)dispatchInfo;
160 ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
161
162 ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
163 ///col0->m_worldTransform,
164 btScalar resultFraction = btScalar(1.);
165
166
167 btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
168 btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
169
170 if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
171 squareMot1 < col1->getCcdSquareMotionThreshold())
172 return resultFraction;
173
174
175 //An adhoc way of testing the Continuous Collision Detection algorithms
176 //One object is approximated as a sphere, to simplify things
177 //Starting in penetration should report no time of impact
178 //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
179 //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
180
181
182 /// Convex0 against sphere for Convex1
183 {
184 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
185
186 btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
187 btConvexCast::CastResult result;
188 btVoronoiSimplexSolver voronoiSimplex;
189 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
190 ///Simplification, one object is simplified as a sphere
191 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
192 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
193 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
194 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
195 {
196
197 //store result.m_fraction in both bodies
198
199 if (col0->getHitFraction()> result.m_fraction)
200 col0->setHitFraction( result.m_fraction );
201
202 if (col1->getHitFraction() > result.m_fraction)
203 col1->setHitFraction( result.m_fraction);
204
205 if (resultFraction > result.m_fraction)
206 resultFraction = result.m_fraction;
207
208 }
209
210
211
212
213 }
214
215 /// Sphere (for convex0) against Convex1
216 {
217 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
218
219 btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
220 btConvexCast::CastResult result;
221 btVoronoiSimplexSolver voronoiSimplex;
222 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
223 ///Simplification, one object is simplified as a sphere
224 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
225 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
226 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
227 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
228 {
229
230 //store result.m_fraction in both bodies
231
232 if (col0->getHitFraction() > result.m_fraction)
233 col0->setHitFraction( result.m_fraction);
234
235 if (col1->getHitFraction() > result.m_fraction)
236 col1->setHitFraction( result.m_fraction);
237
238 if (resultFraction > result.m_fraction)
239 resultFraction = result.m_fraction;
240
241 }
242 }
243
244 return resultFraction;
245
246 }
247
248