2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
18 /// Collision Demo shows a degenerate case, where the Simplex solver has to deal with near-affine dependent cases
19 /// See the define CATCH_DEGENERATE_TETRAHEDRON in Bullet's btVoronoiSimplexSolver.cpp
23 //#define CHECK_GENSHER_TRIANGLE_CASE 1
26 ///This low-level internal demo does intentionally NOT use the btBulletCollisionCommon.h header
27 ///It needs internal access
28 #include "GL_Simplex1to4.h"
29 #include "LinearMath/btQuaternion.h"
30 #include "LinearMath/btTransform.h"
31 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
32 #include "BulletCollision/CollisionShapes/btBoxShape.h"
33 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
34 #include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
35 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
36 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
37 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
38 #include "LinearMath/btTransformUtil.h"
40 #include "CollisionDemo.h"
41 #include "GL_ShapeDrawer.h"
42 #include "GlutStuff.h"
43 #include "LinearMath/btIDebugDraw.h"
44 #include "../OpenGL/GLDebugDrawer.h"
45 GLDebugDrawer debugDrawer;
48 float yaw=0.f,pitch=0.f,roll=0.f;
49 const int maxNumObjects = 4;
50 const int numObjects = 2;
52 GL_Simplex1to4 simplex;
54 btPolyhedralConvexShape* shapePtr[maxNumObjects];
56 btTransform tr[numObjects];
57 int screenWidth = 640;
58 int screenHeight = 480;
60 void DrawRasterizerLine(float const* , float const*, int)
65 int main(int argc,char** argv)
67 CollisionDemo* colDemo = new CollisionDemo();
69 #ifdef CHECK_GENSHER_TRIANGLE_CASE
70 colDemo->setCameraDistance(8.f);
72 colDemo->setCameraDistance(4.f);
75 colDemo->initPhysics();
79 return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo",colDemo);
82 void CollisionDemo::initPhysics()
87 //m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
88 #ifdef CHECK_GENSHER_TRIANGLE_CASE
97 m_cameraTargetPosition.setValue(8.12,0.39,0);
100 tr[0].setOrigin(btVector3(10,0,0));
102 tr[1].setOrigin(btVector3(0,0,0));
105 #ifdef CHECK_GENSHER_TRIANGLE_CASE
108 #endif //CHECK_GENSHER_TRIANGLE_CASE
110 btVector3 boxHalfExtentsA(1,1,1);//1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
111 btVector3 boxHalfExtentsB(4,4,4);//3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
113 #ifndef CHECK_GENSHER_TRIANGLE_CASE
114 btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
115 btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
123 #ifdef CHECK_GENSHER_TRIANGLE_CASE
124 shapePtr[0] = trishapeA;
125 shapePtr[1] = trishapeB;
133 void CollisionDemo::clientMoveAndDisplay()
140 static btVoronoiSimplexSolver sGjkSimplexSolver;
141 btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
143 static btScalar gContactBreakingThreshold=.02f;
147 int checkPerturbation = 1;
148 int numPerturbationIterations = 20;
149 void CollisionDemo::displayCallback(void) {
151 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
152 glDisable(GL_LIGHTING);
154 btVoronoiSimplexSolver sGjkSimplexSolver;
155 btGjkEpaPenetrationDepthSolver epaSolver;
156 btPointCollector gjkOutput;
157 btVector3 worldBoundsMin(-1000,-1000,-1000);
158 btVector3 worldBoundsMax(1000,1000,1000);
160 btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epaSolver);
162 btGjkPairDetector::ClosestPointInput input;
163 input.m_transformA = tr[0];
164 input.m_transformB = tr[1];
167 convexConvex.getClosestPoints(input, gjkOutput, 0);
179 for (i=0;i<numObjects;i++)
181 tr[i].getOpenGLMatrix( m );
182 //m_shapeDrawer->drawOpenGL(m,shapePtr[i],btVector3(119./255.,147./255.,60./255.),btIDebugDraw::DBG_FastWireframe,worldBoundsMin,worldBoundsMax);
183 m_shapeDrawer->drawOpenGL(m,shapePtr[i],btVector3(0.6,0.6,0.6),btIDebugDraw::DBG_FastWireframe,worldBoundsMin,worldBoundsMax);
186 if (gjkOutput.m_hasResult)
188 printf("original distance: %10.4f\n", gjkOutput.m_distance);
189 btVector3 endPt = gjkOutput.m_pointInWorld +
190 gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
192 debugDrawer.drawLine(gjkOutput.m_pointInWorld,endPt,btVector3(0,0,0));
193 debugDrawer.drawSphere(gjkOutput.m_pointInWorld,0.05,btVector3(0,0,0));
194 debugDrawer.drawSphere(endPt,0.05,btVector3(0,0,0));
196 bool perturbeA = false;//true;
197 const btScalar angleLimit = 0.125f * SIMD_PI;
198 btScalar perturbeAngle;
199 btScalar radiusA = shapePtr[0]->getAngularMotionDisc();
200 btScalar radiusB = shapePtr[1]->getAngularMotionDisc();
202 if (radiusA < radiusB)
204 perturbeAngle = gContactBreakingThreshold /radiusA;
208 perturbeAngle = gContactBreakingThreshold / radiusB;
211 if ( perturbeAngle > angleLimit )
212 perturbeAngle = angleLimit;
217 btPlaneSpace1(gjkOutput.m_normalOnBInWorld,v0,v1);
222 if (myiter>=numPerturbationIterations)
229 for ( ;i<numPerturbationIterations;i++)
234 btGjkPairDetector::ClosestPointInput input;
235 input.m_transformA = tr[0];
236 input.m_transformB = tr[1];
237 sGjkSimplexSolver.reset();
239 btQuaternion perturbeRot(v0,perturbeAngle);
240 btScalar iterationAngle = i*(SIMD_2_PI/btScalar(numPerturbationIterations));
241 btQuaternion rotq(gjkOutput.m_normalOnBInWorld,iterationAngle);
244 input.m_transformA.setBasis( btMatrix3x3(rotq*perturbeRot*rotq.inverse())*tr[0].getBasis());
247 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*tr[1].getBasis());
249 debugDrawer.drawTransform(input.m_transformA,1.0);
250 btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epaSolver);
251 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
252 gjkOutput.m_distance = BT_LARGE_FLOAT;
253 convexConvex.getClosestPoints(input, gjkOutput, 0);
257 if (mystate!=2 || i==myiter)
262 input.m_transformA.getOpenGLMatrix( m );
263 //m_shapeDrawer->drawOpenGL(m,shapePtr[0],btVector3(108./255.,131./255.,158./255),btIDebugDraw::DBG_FastWireframe,worldBoundsMin,worldBoundsMax);
264 m_shapeDrawer->drawOpenGL(m,shapePtr[0],btVector3(0.3,0.3,1),btIDebugDraw::DBG_FastWireframe,worldBoundsMin,worldBoundsMax);
268 if (1)//gjkOutput.m_hasResult)
271 printf("perturbed distance: %10.4f\n", gjkOutput.m_distance);
272 btVector3 startPt,endPt;
276 btVector3 endPtOrg = gjkOutput.m_pointInWorld + gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
277 endPt = (tr[0]*input.m_transformA.inverse())(endPtOrg);
278 depth = (endPt - gjkOutput.m_pointInWorld).dot(gjkOutput.m_normalOnBInWorld);
279 startPt = endPt-gjkOutput.m_normalOnBInWorld*depth;
282 endPt = gjkOutput.m_pointInWorld + gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
283 startPt = (tr[1]*input.m_transformB.inverse())(gjkOutput.m_pointInWorld);
284 depth = (endPt - startPt).dot(gjkOutput.m_normalOnBInWorld);
287 printf("corrected distance: %10.4f\n", depth);
291 debugDrawer.drawLine(startPt,endPt,btVector3(1,0,0));
292 debugDrawer.drawSphere(startPt,0.05,btVector3(0,1,0));
293 debugDrawer.drawSphere(endPt,0.05,btVector3(0,0,1));
297 if (mystate==2 && i>myiter)
304 static int looper = 0;
309 if (checkPerturbation>numPerturbationIterations)
313 GL_ShapeDrawer::drawCoordSystem();
317 if (mystate==1 || mystate==2)
319 static int count = 10;
329 orn.setEuler(yaw,pitch,roll);
331 //tr[0].setRotation(orn);
341 void CollisionDemo::specialKeyboard(int key, int x, int y)
351 DemoApplication::specialKeyboard(key,x,y);
357 void CollisionDemo::specialKeyboardUp(int key, int x, int y)
377 DemoApplication::specialKeyboardUp(key,x,y);