Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @ 1963

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

Added Bullet physics engine.

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