Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/environment/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @ 8206

Last change on this file since 8206 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 13.2 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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/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
44
45
46
47
48
49
50
51
52btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
53{
54        m_numPerturbationIterations = 0;
55        m_minimumPointsPerturbationThreshold = 3;
56        m_simplexSolver = simplexSolver;
57        m_pdSolver = pdSolver;
58}
59
60btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
61{ 
62}
63
64btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
65: btActivatingCollisionAlgorithm(ci,body0,body1),
66m_simplexSolver(simplexSolver),
67m_pdSolver(pdSolver),
68m_ownManifold (false),
69m_manifoldPtr(mf),
70m_lowLevelOfDetail(false),
71#ifdef USE_SEPDISTANCE_UTIL2
72,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
73                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
74#endif
75m_numPerturbationIterations(numPerturbationIterations),
76m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
77{
78        (void)body0;
79        (void)body1;
80}
81
82
83
84
85btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
86{
87        if (m_ownManifold)
88        {
89                if (m_manifoldPtr)
90                        m_dispatcher->releaseManifold(m_manifoldPtr);
91        }
92}
93
94void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
95{
96        m_lowLevelOfDetail = useLowLevel;
97}
98
99
100struct btPerturbedContactResult : public btManifoldResult
101{
102        btManifoldResult* m_originalManifoldResult;
103        btTransform m_transformA;
104        btTransform m_transformB;
105        btTransform     m_unPerturbedTransform;
106        bool    m_perturbA;
107        btIDebugDraw*   m_debugDrawer;
108
109
110        btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
111                :m_originalManifoldResult(originalResult),
112                m_transformA(transformA),
113                m_transformB(transformB),
114                m_perturbA(perturbA),
115                m_unPerturbedTransform(unPerturbedTransform),
116                m_debugDrawer(debugDrawer)
117        {
118        }
119        virtual ~ btPerturbedContactResult()
120        {
121        }
122
123        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
124        {
125                btVector3 endPt,startPt;
126                btScalar newDepth;
127                btVector3 newNormal;
128
129                if (m_perturbA)
130                {
131                        btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
132                        endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
133                        newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
134                        startPt = endPt+normalOnBInWorld*newDepth;
135                } else
136                {
137                        endPt = pointInWorld + normalOnBInWorld*orgDepth;
138                        startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
139                        newDepth = (endPt -  startPt).dot(normalOnBInWorld);
140                       
141                }
142
143//#define DEBUG_CONTACTS 1
144#ifdef DEBUG_CONTACTS
145                m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
146                m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
147                m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
148#endif //DEBUG_CONTACTS
149
150               
151                m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
152        }
153
154};
155
156extern btScalar gContactBreakingThreshold;
157
158//
159// Convex-Convex collision algorithm
160//
161void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
162{
163
164        if (!m_manifoldPtr)
165        {
166                //swapped?
167                m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
168                m_ownManifold = true;
169        }
170        resultOut->setPersistentManifold(m_manifoldPtr);
171
172        //comment-out next line to test multi-contact generation
173        //resultOut->getPersistentManifold()->clearManifold();
174       
175
176        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
177        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
178
179#ifdef USE_SEPDISTANCE_UTIL2
180        m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
181        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
182#endif //USE_SEPDISTANCE_UTIL2
183
184        {
185
186       
187        btGjkPairDetector::ClosestPointInput input;
188
189        btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
190        //TODO: if (dispatchInfo.m_useContinuous)
191        gjkPairDetector.setMinkowskiA(min0);
192        gjkPairDetector.setMinkowskiB(min1);
193
194#ifdef USE_SEPDISTANCE_UTIL2
195        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
196        {
197                input.m_maximumDistanceSquared = 1e30f;
198        } else
199#endif //USE_SEPDISTANCE_UTIL2
200        {
201                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
202                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
203        }
204
205        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
206        input.m_transformA = body0->getWorldTransform();
207        input.m_transformB = body1->getWorldTransform();
208
209        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
210        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
211
212        //now perturbe directions to get multiple contact points
213        btVector3 v0,v1;
214        btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
215        btPlaneSpace1(sepNormalWorldSpace,v0,v1);
216        //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
217       
218        //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
219        if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
220        {
221               
222                int i;
223
224                bool perturbeA = true;
225                const btScalar angleLimit = 0.125f * SIMD_PI;
226                btScalar perturbeAngle;
227                btScalar radiusA = min0->getAngularMotionDisc();
228                btScalar radiusB = min1->getAngularMotionDisc();
229                if (radiusA < radiusB)
230                {
231                        perturbeAngle = gContactBreakingThreshold /radiusA;
232                        perturbeA = true;
233                } else
234                {
235                        perturbeAngle = gContactBreakingThreshold / radiusB;
236                        perturbeA = false;
237                }
238                if ( perturbeAngle > angleLimit ) 
239                                perturbeAngle = angleLimit;
240
241                btTransform unPerturbedTransform;
242                if (perturbeA)
243                {
244                        unPerturbedTransform = input.m_transformA;
245                } else
246                {
247                        unPerturbedTransform = input.m_transformB;
248                }
249               
250                for ( i=0;i<m_numPerturbationIterations;i++)
251                {
252                        btQuaternion perturbeRot(v0,perturbeAngle);
253                        btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
254                        btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
255                       
256                       
257                        if (perturbeA)
258                        {
259                                input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
260                                input.m_transformB = body1->getWorldTransform();
261#ifdef DEBUG_CONTACTS
262                                dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
263#endif //DEBUG_CONTACTS
264                        } else
265                        {
266                                input.m_transformA = body0->getWorldTransform();
267                                input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
268#ifdef DEBUG_CONTACTS
269                                dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
270#endif
271                        }
272                       
273                        btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
274                        gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
275                       
276                       
277                }
278        }
279
280       
281
282#ifdef USE_SEPDISTANCE_UTIL2
283        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
284        {
285                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
286        }
287#endif //USE_SEPDISTANCE_UTIL2
288
289
290        }
291
292        if (m_ownManifold)
293        {
294                resultOut->refreshContactPoints();
295        }
296
297}
298
299
300
301bool disableCcd = false;
302btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
303{
304        (void)resultOut;
305        (void)dispatchInfo;
306        ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
307   
308        ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
309        ///col0->m_worldTransform,
310        btScalar resultFraction = btScalar(1.);
311
312
313        btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
314        btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
315   
316        if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
317                squareMot1 < col1->getCcdSquareMotionThreshold())
318                return resultFraction;
319
320        if (disableCcd)
321                return btScalar(1.);
322
323
324        //An adhoc way of testing the Continuous Collision Detection algorithms
325        //One object is approximated as a sphere, to simplify things
326        //Starting in penetration should report no time of impact
327        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
328        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
329
330               
331        /// Convex0 against sphere for Convex1
332        {
333                btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
334
335                btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
336                btConvexCast::CastResult result;
337                btVoronoiSimplexSolver voronoiSimplex;
338                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
339                ///Simplification, one object is simplified as a sphere
340                btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
341                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
342                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
343                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
344                {
345               
346                        //store result.m_fraction in both bodies
347               
348                        if (col0->getHitFraction()> result.m_fraction)
349                                col0->setHitFraction( result.m_fraction );
350
351                        if (col1->getHitFraction() > result.m_fraction)
352                                col1->setHitFraction( result.m_fraction);
353
354                        if (resultFraction > result.m_fraction)
355                                resultFraction = result.m_fraction;
356
357                }
358               
359               
360
361
362        }
363
364        /// Sphere (for convex0) against Convex1
365        {
366                btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
367
368                btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
369                btConvexCast::CastResult result;
370                btVoronoiSimplexSolver voronoiSimplex;
371                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
372                ///Simplification, one object is simplified as a sphere
373                btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
374                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
375                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
376                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
377                {
378               
379                        //store result.m_fraction in both bodies
380               
381                        if (col0->getHitFraction()      > result.m_fraction)
382                                col0->setHitFraction( result.m_fraction);
383
384                        if (col1->getHitFraction() > result.m_fraction)
385                                col1->setHitFraction( result.m_fraction);
386
387                        if (resultFraction > result.m_fraction)
388                                resultFraction = result.m_fraction;
389
390                }
391        }
392       
393        return resultFraction;
394
395}
396
Note: See TracBrowser for help on using the repository browser.