Demos/ContinuousConvexCollision/ContinuousConvexCollisionDemo.cpp

   1 /*
   2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
   3  *
   4  * Permission to use, copy, modify, distribute and sell this software
   5  * and its documentation for any purpose is hereby granted without fee,
   6  * provided that the above copyright notice appear in all copies.
   7  * Erwin Coumans makes no representations about the suitability
   8  * of this software for any purpose.
   9  * It is provided "as is" without express or implied warranty.
  10  */
  11
  12
  13 /*
  14         Continuous Convex Collision Demo demonstrates an efficient continuous collision detection algorithm.
  15         Both linear and angular velocities are supported. Convex Objects are sampled using Supporting Vertex.
  16         Motion using Exponential Map.
  17         Future ideas: Comparison with Screwing Motion.
  18         Also comparision with Algebraic CCD and Interval Arithmetic methods (Stephane Redon)
  19 */
  20
  21
  22 ///This low level demo need internal access, and intentionally doesn't include the btBulletCollisionCommon.h headerfile
  23 #include "LinearMath/btQuaternion.h"
  24 #include "LinearMath/btTransform.h"
  25 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  26 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  27 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
  28
  29 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  30 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
  31 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  32 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
  33
  34 #include "LinearMath/btTransformUtil.h"
  35 #include "DebugCastResult.h"
  36
  37 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  38
  39 #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
  40
  41 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  42 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
  43
  44 #include "GL_ShapeDrawer.h"
  45 #include "ContinuousConvexCollision.h"
  46 #include "GlutStuff.h"
  47
  48
  49 float yaw=0.f,pitch=0.f,roll=0.f;
  50 const int maxNumObjects = 4;
  51 const int numObjects = 2;
  52
  53 btVector3 angVels[numObjects];
  54 btVector3 linVels[numObjects];
  55
  56 btPolyhedralConvexShape*        shapePtr[maxNumObjects];
  57
  58
  59 btTransform     fromTrans[maxNumObjects];
  60 btTransform     toTrans[maxNumObjects];
  61
  62
  63 int screenWidth = 640;
  64 int screenHeight = 480;
  65
  66
  67 int main(int argc,char** argv)
  68 {
  69         btContinuousConvexCollisionDemo* ccdDemo = new btContinuousConvexCollisionDemo();
  70
  71         ccdDemo->setCameraDistance(40.f);
  72
  73         ccdDemo->initPhysics();
  74
  75         return glutmain(argc, argv,screenWidth,screenHeight,"Continuous Convex Collision Demo",ccdDemo);
  76 }
  77
  78
  79 void    btContinuousConvexCollisionDemo::initPhysics()
  80 {
  81         fromTrans[0].setOrigin(btVector3(0,10,20));
  82           toTrans[0].setOrigin(btVector3(0,10,-20));
  83         fromTrans[1].setOrigin(btVector3(-2,7,0));
  84           toTrans[1].setOrigin(btVector3(-2,10,0));
  85
  86           btMatrix3x3 identBasis;
  87         identBasis.setIdentity();
  88
  89         btMatrix3x3 basisA;
  90         basisA.setIdentity();
  91         basisA.setEulerZYX(0.f,-SIMD_HALF_PI,0.f);
  92
  93         fromTrans[0].setBasis(identBasis);
  94           toTrans[0].setBasis(basisA);
  95
  96         fromTrans[1].setBasis(identBasis);
  97           toTrans[1].setBasis(identBasis);
  98
  99         toTrans[1].setBasis(identBasis);
 100         btVector3 boxHalfExtentsA(10,1,1);
 101         btVector3 boxHalfExtentsB(1.1f,1.1f,1.1f);
 102         btBoxShape*     boxA = new btBoxShape(boxHalfExtentsA);
 103 //      btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btVector3(-2,0,-2),btVector3(2,0,-2),btVector3(0,0,2),btVector3(0,2,0));
 104 //      btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btVector3(-12,0,0),btVector3(12,0,0));
 105
 106
 107         btBoxShape*     boxB = new btBoxShape(boxHalfExtentsB);
 108
 109         shapePtr[0] = boxA;
 110         shapePtr[1] = boxB;
 111
 112         shapePtr[0]->setMargin(0.01f);
 113         shapePtr[1]->setMargin(0.01f);
 114
 115         for (int i=0;i<numObjects;i++)
 116         {
 117                 btTransformUtil::calculateVelocity(fromTrans[i],toTrans[i],1.f,linVels[i],angVels[i]);
 118         }
 119
 120 }
 121
 122 //to be implemented by the demo
 123
 124 void btContinuousConvexCollisionDemo::clientMoveAndDisplay()
 125 {
 126         displayCallback();
 127 }
 128
 129
 130 static btVoronoiSimplexSolver sVoronoiSimplexSolver;
 131
 132 btSimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
 133
 134 bool drawLine= false;
 135
 136 int minlines = 0;
 137
 138 int maxlines = 512;
 139
 140
 141 void btContinuousConvexCollisionDemo::displayCallback(void) {
 142
 143     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 144         glDisable(GL_LIGHTING);
 145
 146         //GL_ShapeDrawer::drawCoordSystem();
 147
 148         btScalar m[16];
 149         int i;
 150
 151         btVector3 worldBoundsMin(-1000,-1000,-1000);
 152         btVector3 worldBoundsMax(1000,1000,1000);
 153
 154
 155         /*for (i=0;i<numObjects;i++)
 156         {
 157                 fromTrans[i].getOpenGLMatrix( m );
 158                 m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
 159         }
 160 */
 161
 162         if (getDebugMode()==btIDebugDraw::DBG_DrawAabb)
 163         {
 164                 i=0;//for (i=1;i<numObjects;i++)
 165                 {
 166                         //for each object, subdivide the from/to transform in 10 equal steps
 167
 168                         int numSubSteps = 10;
 169                         for (int s=0;s<10;s++)
 170                         {
 171                                 btScalar subStep = s * 1.f/(float)numSubSteps;
 172                                 btTransform interpolatedTrans;
 173
 174                                 btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans);
 175
 176                                 //fromTrans[i].getOpenGLMatrix(m);
 177                                 //m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
 178
 179                                 //toTrans[i].getOpenGLMatrix(m);
 180                                 //m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
 181
 182                                 interpolatedTrans.getOpenGLMatrix( m );
 183                                 m_shapeDrawer->drawOpenGL(m,shapePtr[i],btVector3(1,0,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
 184                         }
 185                 }
 186         }
 187
 188
 189         btMatrix3x3 mat;
 190         mat.setEulerZYX(yaw,pitch,roll);
 191         btQuaternion orn;
 192         mat.getRotation(orn);
 193         orn.setEuler(yaw,pitch,roll);
 194         fromTrans[1].setRotation(orn);
 195         toTrans[1].setRotation(orn);
 196
 197
 198         if (m_stepping || m_singleStep)
 199         {
 200                 m_singleStep = false;
 201                 pitch += 0.005f;
 202 //              yaw += 0.01f;
 203         }
 204 //      btVector3 fromA(-25,11,0);
 205 //      btVector3 toA(-15,11,0);
 206
 207 //      btQuaternion ornFromA(0.f,0.f,0.f,1.f);
 208 //      btQuaternion ornToA(0.f,0.f,0.f,1.f);
 209
 210 //      btTransform     rayFromWorld(ornFromA,fromA);
 211 //      btTransform     rayToWorld(ornToA,toA);
 212
 213         btTransform     rayFromWorld = fromTrans[0];
 214         btTransform     rayToWorld = toTrans[0];
 215
 216
 217         if (drawLine)
 218         {
 219                 glBegin(GL_LINES);
 220                 glColor3f(0, 0, 1);
 221                 glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
 222                 glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
 223                 glEnd();
 224         }
 225
 226         //now perform a raycast on the shapes, in local (shape) space
 227         gGjkSimplexSolver.reset();
 228
 229         //choose one of the following lines
 230
 231
 232         for (i=0;i<numObjects;i++)
 233         {
 234                 fromTrans[i].getOpenGLMatrix(m);
 235                 m_shapeDrawer->drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
 236         }
 237
 238         btDebugCastResult       rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0],m_shapeDrawer);
 239
 240
 241         for (i=1;i<numObjects;i++)
 242         {
 243                 btConvexCast::CastResult        rayResult2;
 244                 btConvexCast::CastResult*       rayResultPtr;
 245                 if (btIDebugDraw::DBG_DrawAabb)
 246                 {
 247                         rayResultPtr = &rayResult1;
 248                 } else
 249                 {
 250                         rayResultPtr = &rayResult2;
 251                 }
 252
 253                 //GjkConvexCast convexCaster(&gGjkSimplexSolver);
 254                 //SubsimplexConvexCast convexCaster(&gGjkSimplexSolver);
 255
 256                 //optional
 257                 btConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
 258                 btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
 259
 260                 gGjkSimplexSolver.reset();
 261
 262
 263
 264                 if (convexCaster.calcTimeOfImpact(fromTrans[0],toTrans[0],fromTrans[i] ,toTrans[i] ,*rayResultPtr))
 265                 {
 266
 267                         glDisable(GL_DEPTH_TEST);
 268
 269                         btTransform hitTrans;
 270                         btTransformUtil::integrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
 271
 272                         hitTrans.getOpenGLMatrix(m);
 273                         m_shapeDrawer->drawOpenGL(m,shapePtr[0],btVector3(0,1,0),getDebugMode(),worldBoundsMin,worldBoundsMax);
 274
 275                         btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans);
 276
 277                         hitTrans.getOpenGLMatrix(m);
 278                         m_shapeDrawer->drawOpenGL(m,shapePtr[i],btVector3(0,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
 279
 280
 281                 }
 282         }
 283
 284         swapBuffers();
 285 }
 286
 287