Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/presentation/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @ 2459

Last change on this file since 2459 was 2459, checked in by rgrieder, 16 years ago

Merged physics_merge back to presentation branch.

  • Property svn:eol-style set to native
File size: 8.9 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_simplexSolver = simplexSolver;
55        m_pdSolver = pdSolver;
56}
57
58btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
59{ 
60}
61
62btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
63: btActivatingCollisionAlgorithm(ci,body0,body1),
64m_simplexSolver(simplexSolver),
65m_pdSolver(pdSolver),
66m_ownManifold (false),
67m_manifoldPtr(mf),
68m_lowLevelOfDetail(false)
69#ifdef USE_SEPDISTANCE_UTIL2
70,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
71                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
72#endif
73{
74        (void)body0;
75        (void)body1;
76}
77
78
79
80
81btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
82{
83        if (m_ownManifold)
84        {
85                if (m_manifoldPtr)
86                        m_dispatcher->releaseManifold(m_manifoldPtr);
87        }
88}
89
90void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
91{
92        m_lowLevelOfDetail = useLowLevel;
93}
94
95
96
97
98
99//
100// Convex-Convex collision algorithm
101//
102void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
103{
104
105        if (!m_manifoldPtr)
106        {
107                //swapped?
108                m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
109                m_ownManifold = true;
110        }
111        resultOut->setPersistentManifold(m_manifoldPtr);
112
113       
114
115        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
116        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
117
118#ifdef USE_SEPDISTANCE_UTIL2
119        m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
120        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
121#endif //USE_SEPDISTANCE_UTIL2
122
123        {
124
125       
126        btGjkPairDetector::ClosestPointInput input;
127
128        btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
129        //TODO: if (dispatchInfo.m_useContinuous)
130        gjkPairDetector.setMinkowskiA(min0);
131        gjkPairDetector.setMinkowskiB(min1);
132
133#ifdef USE_SEPDISTANCE_UTIL2
134        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
135        {
136                input.m_maximumDistanceSquared = 1e30f;
137        } else
138#endif //USE_SEPDISTANCE_UTIL2
139        {
140                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
141                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
142        }
143
144        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
145        input.m_transformA = body0->getWorldTransform();
146        input.m_transformB = body1->getWorldTransform();
147
148        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
149
150        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
151
152#ifdef USE_SEPDISTANCE_UTIL2
153        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
154        {
155                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
156        }
157#endif //USE_SEPDISTANCE_UTIL2
158
159
160        }
161
162        if (m_ownManifold)
163        {
164                resultOut->refreshContactPoints();
165        }
166
167}
168
169
170
171bool disableCcd = false;
172btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
173{
174        (void)resultOut;
175        (void)dispatchInfo;
176        ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
177   
178        ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
179        ///col0->m_worldTransform,
180        btScalar resultFraction = btScalar(1.);
181
182
183        btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
184        btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
185   
186        if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
187                squareMot1 < col1->getCcdSquareMotionThreshold())
188                return resultFraction;
189
190        if (disableCcd)
191                return btScalar(1.);
192
193
194        //An adhoc way of testing the Continuous Collision Detection algorithms
195        //One object is approximated as a sphere, to simplify things
196        //Starting in penetration should report no time of impact
197        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
198        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
199
200               
201        /// Convex0 against sphere for Convex1
202        {
203                btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
204
205                btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
206                btConvexCast::CastResult result;
207                btVoronoiSimplexSolver voronoiSimplex;
208                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
209                ///Simplification, one object is simplified as a sphere
210                btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
211                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
212                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
213                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
214                {
215               
216                        //store result.m_fraction in both bodies
217               
218                        if (col0->getHitFraction()> result.m_fraction)
219                                col0->setHitFraction( result.m_fraction );
220
221                        if (col1->getHitFraction() > result.m_fraction)
222                                col1->setHitFraction( result.m_fraction);
223
224                        if (resultFraction > result.m_fraction)
225                                resultFraction = result.m_fraction;
226
227                }
228               
229               
230
231
232        }
233
234        /// Sphere (for convex0) against Convex1
235        {
236                btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
237
238                btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
239                btConvexCast::CastResult result;
240                btVoronoiSimplexSolver voronoiSimplex;
241                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
242                ///Simplification, one object is simplified as a sphere
243                btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
244                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
245                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
246                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
247                {
248               
249                        //store result.m_fraction in both bodies
250               
251                        if (col0->getHitFraction()      > result.m_fraction)
252                                col0->setHitFraction( result.m_fraction);
253
254                        if (col1->getHitFraction() > result.m_fraction)
255                                col1->setHitFraction( result.m_fraction);
256
257                        if (resultFraction > result.m_fraction)
258                                resultFraction = result.m_fraction;
259
260                }
261        }
262       
263        return resultFraction;
264
265}
266
Note: See TracBrowser for help on using the repository browser.