Demos/CollisionDemo/CollisionDemo.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
  17 ///
  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
  20 ///
  21
  22
  23 //#define CHECK_GENSHER_TRIANGLE_CASE 1
  24
  25
  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"
  39
  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;
  46
  47
  48 float yaw=0.f,pitch=0.f,roll=0.f;
  49 const int maxNumObjects = 4;
  50 const int numObjects = 2;
  51
  52 GL_Simplex1to4 simplex;
  53
  54 btPolyhedralConvexShape*        shapePtr[maxNumObjects];
  55
  56 btTransform tr[numObjects];
  57 int screenWidth = 640;
  58 int screenHeight = 480;
  59
  60 void DrawRasterizerLine(float const* , float const*, int)
  61 {
  62
  63 }
  64
  65 int main(int argc,char** argv)
  66 {
  67         CollisionDemo* colDemo = new CollisionDemo();
  68
  69 #ifdef CHECK_GENSHER_TRIANGLE_CASE
  70         colDemo->setCameraDistance(8.f);
  71 #else
  72         colDemo->setCameraDistance(4.f);
  73
  74 #endif //
  75         colDemo->initPhysics();
  76
  77
  78
  79         return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo",colDemo);
  80 }
  81
  82 void CollisionDemo::initPhysics()
  83 {
  84         setTexturing(false);
  85         setShadows(false);
  86
  87         //m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
  88 #ifdef CHECK_GENSHER_TRIANGLE_CASE
  89         m_azi = 140.f;
  90 #else
  91         m_azi = 250.f;
  92 #endif
  93         m_ele = 25.f;
  94
  95         m_azi = 0;
  96         m_ele = 0;
  97         m_cameraTargetPosition.setValue(8.12,0.39,0);
  98
  99         tr[0].setIdentity();
 100         tr[0].setOrigin(btVector3(10,0,0));
 101         tr[1].setIdentity();
 102         tr[1].setOrigin(btVector3(0,0,0));
 103
 104
 105 #ifdef CHECK_GENSHER_TRIANGLE_CASE
 106         tr[0].setIdentity();
 107         tr[1].setIdentity();
 108 #endif //CHECK_GENSHER_TRIANGLE_CASE
 109
 110         btVector3 boxHalfExtentsA(1,1,1);//1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
 111         btVector3 boxHalfExtentsB(4,4,4);//3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
 112
 113 #ifndef CHECK_GENSHER_TRIANGLE_CASE
 114         btBoxShape*     boxA = new btBoxShape(boxHalfExtentsA);
 115         btBoxShape*     boxB = new btBoxShape(boxHalfExtentsB);
 116 #endif
 117
 118
 119
 120
 121
 122
 123 #ifdef CHECK_GENSHER_TRIANGLE_CASE
 124         shapePtr[0] = trishapeA;
 125         shapePtr[1] = trishapeB;
 126 #else
 127         shapePtr[0] = boxA;
 128         shapePtr[1] = boxB;
 129 #endif
 130
 131 }
 132
 133 void CollisionDemo::clientMoveAndDisplay()
 134 {
 135
 136         displayCallback();
 137 }
 138
 139
 140 static btVoronoiSimplexSolver sGjkSimplexSolver;
 141 btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
 142
 143 static btScalar gContactBreakingThreshold=.02f;
 144 int myiter = 1;
 145 int mystate = 2;
 146
 147 int checkPerturbation = 1;
 148 int numPerturbationIterations = 20;
 149 void CollisionDemo::displayCallback(void) {
 150
 151     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 152         glDisable(GL_LIGHTING);
 153
 154    btVoronoiSimplexSolver sGjkSimplexSolver;
 155    btGjkEpaPenetrationDepthSolver epaSolver;
 156    btPointCollector gjkOutput;
 157    btVector3 worldBoundsMin(-1000,-1000,-1000);
 158                 btVector3 worldBoundsMax(1000,1000,1000);
 159    {
 160            btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epaSolver);
 161
 162            btGjkPairDetector::ClosestPointInput input;
 163            input.m_transformA = tr[0];
 164            input.m_transformB = tr[1];
 165
 166
 167            convexConvex.getClosestPoints(input, gjkOutput, 0);
 168    }
 169
 170         ATTRIBUTE_ALIGNED16(btScalar) m[16];
 171         int i;
 172
 173         //m_ele = 21.2;
 174         //m_azi = -56.6;
 175
 176
 177
 178
 179         for (i=0;i<numObjects;i++)
 180         {
 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);
 184         }
 185
 186    if (gjkOutput.m_hasResult)
 187    {
 188         printf("original  distance: %10.4f\n", gjkOutput.m_distance);
 189                 btVector3 endPt = gjkOutput.m_pointInWorld +
 190                 gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
 191
 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));
 195
 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();
 201
 202                 if (radiusA < radiusB)
 203                 {
 204                         perturbeAngle = gContactBreakingThreshold /radiusA;
 205                         perturbeA = true;
 206                 } else
 207                 {
 208                         perturbeAngle = gContactBreakingThreshold / radiusB;
 209                         perturbeA = false;
 210                 }
 211                 if ( perturbeAngle > angleLimit )
 212                                 perturbeAngle = angleLimit;
 213
 214                 perturbeAngle*=5;
 215
 216                 btVector3 v0,v1;
 217                 btPlaneSpace1(gjkOutput.m_normalOnBInWorld,v0,v1);
 218
 219                 glLineWidth(5);
 220                 int i;
 221                 i=0;
 222                 if (myiter>=numPerturbationIterations)
 223                         myiter=0;
 224                 if (mystate<2)
 225                 {
 226                         i= myiter;
 227                 }
 228
 229                 for ( ;i<numPerturbationIterations;i++)
 230                 {
 231
 232
 233
 234                         btGjkPairDetector::ClosestPointInput input;
 235                         input.m_transformA = tr[0];
 236                         input.m_transformB = tr[1];
 237                         sGjkSimplexSolver.reset();
 238
 239                         btQuaternion perturbeRot(v0,perturbeAngle);
 240                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(numPerturbationIterations));
 241                         btQuaternion rotq(gjkOutput.m_normalOnBInWorld,iterationAngle);
 242                         if (perturbeA)
 243                         {
 244                                 input.m_transformA.setBasis( btMatrix3x3(rotq*perturbeRot*rotq.inverse())*tr[0].getBasis());
 245                         } else
 246                         {
 247                                 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*tr[1].getBasis());
 248                         }
 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);
 254
 255
 256
 257                         if (mystate!=2 || i==myiter)
 258
 259                         {
 260                                 ATTRIBUTE_ALIGNED16(btScalar) m[16];
 261
 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);
 265
 266                         }
 267
 268                         if (1)//gjkOutput.m_hasResult)
 269                         {
 270
 271                                 printf("perturbed distance: %10.4f\n", gjkOutput.m_distance);
 272                                 btVector3 startPt,endPt;
 273                                 btScalar depth = 0;
 274                                 if (perturbeA)
 275                                 {
 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;
 280                                 } else
 281                                 {
 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);
 285                                 }
 286
 287                                 printf("corrected distance: %10.4f\n", depth);
 288
 289
 290
 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));
 294                   }
 295                         if (mystate<2)
 296                                 break;
 297                         if (mystate==2 && i>myiter)
 298                                 break;
 299                 }
 300
 301
 302    }
 303
 304    static int looper = 0;
 305    if (looper++>10)
 306    {
 307         looper =0;
 308         checkPerturbation++;
 309         if (checkPerturbation>numPerturbationIterations)
 310                 checkPerturbation=0;
 311    }
 312
 313    GL_ShapeDrawer::drawCoordSystem();
 314
 315
 316
 317         if (mystate==1 || mystate==2)
 318         {
 319                 static int count = 10;
 320                 count--;
 321                 if (count<0)
 322                 {
 323                         count=10;
 324                         myiter++;
 325                 }
 326         }
 327
 328         btQuaternion orn;
 329         orn.setEuler(yaw,pitch,roll);
 330         //let it rotate
 331         //tr[0].setRotation(orn);
 332
 333         pitch += 0.005f;
 334         yaw += 0.01f;
 335
 336         glFlush();
 337     glutSwapBuffers();
 338 }
 339
 340
 341 void CollisionDemo::specialKeyboard(int key, int x, int y)
 342 {
 343    switch (key)
 344         {
 345    case GLUT_KEY_DOWN:
 346         case GLUT_KEY_UP:
 347                 {
 348                 break;
 349                 }
 350         default:
 351                 DemoApplication::specialKeyboard(key,x,y);
 352                 break;
 353         }
 354
 355 }
 356
 357 void CollisionDemo::specialKeyboardUp(int key, int x, int y)
 358 {
 359    switch (key)
 360         {
 361         case GLUT_KEY_UP :
 362                 {
 363                         myiter++;
 364                 break;
 365                 }
 366
 367                 case GLUT_KEY_DOWN:
 368                 {
 369                         mystate++;
 370                         if (mystate>1)
 371                                 myiter=0;
 372                         if (mystate>=4)
 373                                 mystate = 0;
 374                 break;
 375                 }
 376         default:
 377                 DemoApplication::specialKeyboardUp(key,x,y);
 378                 break;
 379         }
 380 }