1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
26 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
27 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
28 #include "BulletCollision/CollisionShapes/btBoxShape.h"
29 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
30
31 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
32 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
33 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
34 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
35
36 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
37 #include "BulletCollision/CollisionShapes/btSphereShape.h"
38
39 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
40
41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
42 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
43 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
44
CreateFunc(btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver)45 btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
46 {
47 m_simplexSolver = simplexSolver;
48 m_pdSolver = pdSolver;
49 }
50
~CreateFunc()51 btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
52 {
53 }
54
btConvex2dConvex2dAlgorithm(btPersistentManifold * mf,const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver,int,int)55 btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
56 : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
57 m_simplexSolver(simplexSolver),
58 m_pdSolver(pdSolver),
59 m_ownManifold(false),
60 m_manifoldPtr(mf),
61 m_lowLevelOfDetail(false)
62 {
63 (void)body0Wrap;
64 (void)body1Wrap;
65 }
66
~btConvex2dConvex2dAlgorithm()67 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
68 {
69 if (m_ownManifold)
70 {
71 if (m_manifoldPtr)
72 m_dispatcher->releaseManifold(m_manifoldPtr);
73 }
74 }
75
setLowLevelOfDetail(bool useLowLevel)76 void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
77 {
78 m_lowLevelOfDetail = useLowLevel;
79 }
80
81 extern btScalar gContactBreakingThreshold;
82
83 //
84 // Convex-Convex collision algorithm
85 //
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)86 void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
87 {
88 if (!m_manifoldPtr)
89 {
90 //swapped?
91 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
92 m_ownManifold = true;
93 }
94 resultOut->setPersistentManifold(m_manifoldPtr);
95
96 //comment-out next line to test multi-contact generation
97 //resultOut->getPersistentManifold()->clearManifold();
98
99 const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
100 const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
101
102 btVector3 normalOnB;
103 btVector3 pointOnBWorld;
104
105 {
106 btGjkPairDetector::ClosestPointInput input;
107
108 btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
109 //TODO: if (dispatchInfo.m_useContinuous)
110 gjkPairDetector.setMinkowskiA(min0);
111 gjkPairDetector.setMinkowskiB(min1);
112
113 {
114 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
115 input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
116 }
117
118 input.m_transformA = body0Wrap->getWorldTransform();
119 input.m_transformB = body1Wrap->getWorldTransform();
120
121 gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw);
122
123 btVector3 v0, v1;
124 btVector3 sepNormalWorldSpace;
125 }
126
127 if (m_ownManifold)
128 {
129 resultOut->refreshContactPoints();
130 }
131 }
132
calculateTimeOfImpact(btCollisionObject * col0,btCollisionObject * col1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)133 btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
134 {
135 (void)resultOut;
136 (void)dispatchInfo;
137 ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
138
139 ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
140 ///col0->m_worldTransform,
141 btScalar resultFraction = btScalar(1.);
142
143 btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
144 btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
145
146 if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
147 squareMot1 < col1->getCcdSquareMotionThreshold())
148 return resultFraction;
149
150 //An adhoc way of testing the Continuous Collision Detection algorithms
151 //One object is approximated as a sphere, to simplify things
152 //Starting in penetration should report no time of impact
153 //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
154 //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
155
156 /// Convex0 against sphere for Convex1
157 {
158 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
159
160 btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
161 btConvexCast::CastResult result;
162 btVoronoiSimplexSolver voronoiSimplex;
163 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
164 ///Simplification, one object is simplified as a sphere
165 btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex);
166 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
167 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
168 col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
169 {
170 //store result.m_fraction in both bodies
171
172 if (col0->getHitFraction() > result.m_fraction)
173 col0->setHitFraction(result.m_fraction);
174
175 if (col1->getHitFraction() > result.m_fraction)
176 col1->setHitFraction(result.m_fraction);
177
178 if (resultFraction > result.m_fraction)
179 resultFraction = result.m_fraction;
180 }
181 }
182
183 /// Sphere (for convex0) against Convex1
184 {
185 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
186
187 btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
188 btConvexCast::CastResult result;
189 btVoronoiSimplexSolver voronoiSimplex;
190 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
191 ///Simplification, one object is simplified as a sphere
192 btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex);
193 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
194 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(),
195 col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result))
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 return resultFraction;
211 }
212