update Bullet 2.x with latest changes, notice that the integration is not finished...
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btConvexConvexAlgorithm.cpp
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 "btConvexConvexAlgorithm.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/NarrowPhaseCollision/btGjkPairDetector.h"
24 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
25 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
26 #include "BulletCollision/CollisionShapes/btBoxShape.h"
27 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
28
29 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
30 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
31 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
32 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
33
34
35
36 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
37 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
38 #include "BulletCollision/CollisionShapes/btSphereShape.h"
39
40 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
41
42 //#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
43
44 #ifdef WIN32
45 #if _MSC_VER >= 1310
46 //only use SIMD Hull code under Win32
47 #ifdef TEST_HULL
48 #define USE_HULL 1
49 #endif //TEST_HULL
50 #endif //_MSC_VER 
51 #endif //WIN32
52
53
54 #ifdef USE_HULL
55
56 #include "NarrowPhaseCollision/Hull.h"
57 #include "NarrowPhaseCollision/HullContactCollector.h"
58
59
60 #endif //USE_HULL
61
62 bool gUseEpa = false;
63
64
65 #ifdef WIN32
66 void DrawRasterizerLine(const float* from,const float* to,int color);
67 #endif
68
69
70
71
72 //#define PROCESS_SINGLE_CONTACT
73 #ifdef WIN32
74 bool gForceBoxBox = false;//false;//true;
75
76 #else
77 bool gForceBoxBox = false;//false;//true;
78 #endif
79 bool gBoxBoxUseGjk = true;//true;//false;
80 bool gDisableConvexCollision = false;
81
82
83
84 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
85 : btCollisionAlgorithm(ci),
86 m_gjkPairDetector(0,0,&m_simplexSolver,0),
87 m_useEpa(!gUseEpa),
88 m_ownManifold (false),
89 m_manifoldPtr(mf),
90 m_lowLevelOfDetail(false)
91 {
92         checkPenetrationDepthSolver();
93
94 }
95
96
97
98
99 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
100 {
101         if (m_ownManifold)
102         {
103                 if (m_manifoldPtr)
104                         m_dispatcher->releaseManifold(m_manifoldPtr);
105         }
106 }
107
108 void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
109 {
110         m_lowLevelOfDetail = useLowLevel;
111 }
112
113
114
115
116 static btMinkowskiPenetrationDepthSolver        gPenetrationDepthSolver;
117
118 //static EpaPenetrationDepthSolver      gEpaPenetrationDepthSolver;
119
120 #ifdef USE_EPA
121 Solid3EpaPenetrationDepth       gSolidEpaPenetrationSolver;
122 #endif //USE_EPA
123
124 void    btConvexConvexAlgorithm::checkPenetrationDepthSolver()
125 {
126         if (m_useEpa != gUseEpa)
127         {
128                 m_useEpa  = gUseEpa;
129                 if (m_useEpa)
130                 {
131                         
132                 //      m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver);
133                                                 
134                         
135                 } else
136                 {
137                         m_gjkPairDetector.setPenetrationDepthSolver(&gPenetrationDepthSolver);
138                 }
139         }
140         
141 }
142
143
144 //
145 // Convex-Convex collision algorithm
146 //
147 void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
148 {
149
150         if (!m_manifoldPtr)
151         {
152                 //swapped?
153                 m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
154                 m_ownManifold = true;
155         }
156
157
158         checkPenetrationDepthSolver();
159
160         btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape);
161         btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape);
162         
163         btGjkPairDetector::ClosestPointInput input;
164
165         //TODO: if (dispatchInfo.m_useContinuous)
166         m_gjkPairDetector.setMinkowskiA(min0);
167         m_gjkPairDetector.setMinkowskiB(min1);
168         input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
169         input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
170         
171 //      input.m_maximumDistanceSquared = 1e30f;
172         
173         input.m_transformA = body0->m_worldTransform;
174         input.m_transformB = body1->m_worldTransform;
175         
176         resultOut->setPersistentManifold(m_manifoldPtr);
177         m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
178
179 }
180
181
182
183 bool disableCcd = false;
184 float   btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
185 {
186         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
187     
188         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
189         ///col0->m_worldTransform,
190         float resultFraction = 1.f;
191
192
193         float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
194     
195         if (squareMot0 < col0->m_ccdSquareMotionThreshold &&
196                 squareMot0 < col0->m_ccdSquareMotionThreshold)
197                 return resultFraction;
198
199
200
201         if (disableCcd)
202                 return 1.f;
203
204         checkPenetrationDepthSolver();
205
206         //An adhoc way of testing the Continuous Collision Detection algorithms
207         //One object is approximated as a sphere, to simplify things
208         //Starting in penetration should report no time of impact
209         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
210         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
211
212                 
213         /// Convex0 against sphere for Convex1
214         {
215                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape);
216
217                 btSphereShape   sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
218                 btConvexCast::CastResult result;
219                 btVoronoiSimplexSolver voronoiSimplex;
220                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
221                 ///Simplification, one object is simplified as a sphere
222                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
223                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
224                 if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
225                         col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
226                 {
227                 
228                         //store result.m_fraction in both bodies
229                 
230                         if (col0->m_hitFraction > result.m_fraction)
231                                 col0->m_hitFraction  = result.m_fraction;
232
233                         if (col1->m_hitFraction > result.m_fraction)
234                                 col1->m_hitFraction  = result.m_fraction;
235
236                         if (resultFraction > result.m_fraction)
237                                 resultFraction = result.m_fraction;
238
239                 }
240                 
241                 
242
243
244         }
245
246         /// Sphere (for convex0) against Convex1
247         {
248                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape);
249
250                 btSphereShape   sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
251                 btConvexCast::CastResult result;
252                 btVoronoiSimplexSolver voronoiSimplex;
253                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
254                 ///Simplification, one object is simplified as a sphere
255                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
256                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
257                 if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
258                         col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
259                 {
260                 
261                         //store result.m_fraction in both bodies
262                 
263                         if (col0->m_hitFraction > result.m_fraction)
264                                 col0->m_hitFraction  = result.m_fraction;
265
266                         if (col1->m_hitFraction > result.m_fraction)
267                                 col1->m_hitFraction  = result.m_fraction;
268
269                         if (resultFraction > result.m_fraction)
270                                 resultFraction = result.m_fraction;
271
272                 }
273         }
274         
275         return resultFraction;
276
277 }