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 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
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,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver,int numPerturbationIterations,int minimumPointsPerturbationThreshold)60 btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
61 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
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)body0Wrap;
71 (void)body1Wrap;
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(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)99 void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
100 {
101
102 if (!m_manifoldPtr)
103 {
104 //swapped?
105 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
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 const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
115 const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->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_transformA = body0Wrap->getWorldTransform();
136 input.m_transformB = body1Wrap->getWorldTransform();
137
138 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
139
140 btVector3 v0,v1;
141 btVector3 sepNormalWorldSpace;
142
143 }
144
145 if (m_ownManifold)
146 {
147 resultOut->refreshContactPoints();
148 }
149
150 }
151
152
153
154
calculateTimeOfImpact(btCollisionObject * col0,btCollisionObject * col1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)155 btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
156 {
157 (void)resultOut;
158 (void)dispatchInfo;
159 ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
160
161 ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
162 ///col0->m_worldTransform,
163 btScalar resultFraction = btScalar(1.);
164
165
166 btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
167 btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
168
169 if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
170 squareMot1 < col1->getCcdSquareMotionThreshold())
171 return resultFraction;
172
173
174 //An adhoc way of testing the Continuous Collision Detection algorithms
175 //One object is approximated as a sphere, to simplify things
176 //Starting in penetration should report no time of impact
177 //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
178 //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
179
180
181 /// Convex0 against sphere for Convex1
182 {
183 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
184
185 btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
186 btConvexCast::CastResult result;
187 btVoronoiSimplexSolver voronoiSimplex;
188 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
189 ///Simplification, one object is simplified as a sphere
190 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
191 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
192 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
193 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
194 {
195
196 //store result.m_fraction in both bodies
197
198 if (col0->getHitFraction()> result.m_fraction)
199 col0->setHitFraction( result.m_fraction );
200
201 if (col1->getHitFraction() > result.m_fraction)
202 col1->setHitFraction( result.m_fraction);
203
204 if (resultFraction > result.m_fraction)
205 resultFraction = result.m_fraction;
206
207 }
208
209
210
211
212 }
213
214 /// Sphere (for convex0) against Convex1
215 {
216 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
217
218 btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
219 btConvexCast::CastResult result;
220 btVoronoiSimplexSolver voronoiSimplex;
221 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
222 ///Simplification, one object is simplified as a sphere
223 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
224 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
225 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
226 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
227 {
228
229 //store result.m_fraction in both bodies
230
231 if (col0->getHitFraction() > result.m_fraction)
232 col0->setHitFraction( result.m_fraction);
233
234 if (col1->getHitFraction() > result.m_fraction)
235 col1->setHitFraction( result.m_fraction);
236
237 if (resultFraction > result.m_fraction)
238 resultFraction = result.m_fraction;
239
240 }
241 }
242
243 return resultFraction;
244
245 }
246
247