bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btConvex2dConvex2dAlgorithm.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 "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
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
56 btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
57
58 }
59
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
77 btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
78 {
79         if (m_ownManifold)
80         {
81                 if (m_manifoldPtr)
82                         m_dispatcher->releaseManifold(m_manifoldPtr);
83         }
84 }
85
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 //
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_stackAlloc = dispatchInfo.m_stackAllocator;
136                 input.m_transformA = body0Wrap->getWorldTransform();
137                 input.m_transformB = body1Wrap->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
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