Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp

   1
   2 ///contribution by Pierre Terdiman to check penetration depth solvers
   3 ///see http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=638
   4
   5 #ifdef WIN32//for glut.h
   6 #include <windows.h>
   7 #endif
   8
   9
  10 //think different
  11 #if defined(__APPLE__) && !defined (VMDMESA)
  12 #include <OpenGL/gl.h>
  13 #include <OpenGL/glu.h>
  14 #include <GLUT/glut.h>
  15 #else
  16 #include <GL/glut.h>
  17 #endif
  18
  19
  20 #include <stdlib.h>
  21 #include <stdio.h>
  22 #include <math.h>
  23
  24 #define VERBOSE_TEXT_ONSCREEN 1
  25 #ifdef VERBOSE_TEXT_ONSCREEN
  26 #include "GLDebugFont.h"
  27 #endif
  28
  29 #include "btBulletCollisionCommon.h"
  30
  31 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
  32 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
  33 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
  34 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  35 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
  36 #include "LinearMath/btStackAlloc.h"
  37
  38 //We can use the Bullet EPA or sampling penetration depth solver, but comparison might be useful
  39 //#define COMPARE_WITH_SOLID35_AND_OTHER_EPA 1
  40 #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
  41 #include "../Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h"
  42 #include "../Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h"
  43 #include "../Extras/EPA/EpaPenetrationDepthSolver.h"
  44 #endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
  45
  46 #define USE_ORIGINAL 1
  47 #ifndef USE_ORIGINAL
  48 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  49 #endif //USE_ORIGINAL
  50
  51 static bool gRefMode = false;
  52 static int gMethod = 0;
  53 static int gLastUsedMethod = -1;
  54 static int gNumGjkIterations = -1;
  55 static int gLastDegenerateSimplex = -1;
  56
  57 static const float gDisp = 0.01f;
  58 static const float gCamSpeed = 0.1f;
  59 static btVector3 Eye(3.0616338f, 1.1985892f, 2.5769043f);
  60 static btVector3 Dir(-0.66853905,-0.14004262,-0.73037237);
  61 static btVector3 N;
  62 static int mx = 0;
  63 static int my = 0;
  64 static int glutScreenHeight = 512;
  65 static int glutScreenWidth = 512;
  66
  67 static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width)
  68 {
  69         glDisable(GL_LIGHTING);
  70         glLineWidth(line_width);
  71         glColor4f(color.x(), color.y(), color.z(), 1.0f);
  72         btVector3 tmp[] = {p0, p1};
  73         glEnableClientState(GL_VERTEX_ARRAY);
  74 #ifndef BT_USE_DOUBLE_PRECISION
  75         glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
  76 #else
  77         glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
  78 #endif
  79         glDrawArrays(GL_LINES, 0, 2);
  80         glDisableClientState(GL_VERTEX_ARRAY);
  81         glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
  82         glEnable(GL_LIGHTING);
  83 }
  84
  85 void DrawTriangle(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& color)
  86 {
  87 //      glDisable(GL_LIGHTING);
  88         glColor4f(color.x(), color.y(), color.z(), 1.0f);
  89         btVector3 tmp[] = {p0, p1, p2};
  90         glEnableClientState(GL_VERTEX_ARRAY);
  91 #ifndef BT_USE_DOUBLE_PRECISION
  92         glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
  93 #else
  94         glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
  95 #endif
  96         glDrawArrays(GL_TRIANGLES, 0, 3);
  97         glDisableClientState(GL_VERTEX_ARRAY);
  98 //      glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
  99 //      glEnable(GL_LIGHTING);
 100 }
 101
 102 class MyPoly
 103 {
 104         public:
 105                         MyPoly() : mNbVerts(0), mIndices(NULL)  {}
 106                         ~MyPoly()                                                               { delete[]mIndices;     }
 107
 108         short   mNbVerts;
 109         char*   mIndices;
 110         float   mPlane[4];
 111 };
 112
 113 class MyConvex
 114 {
 115         public:
 116                                 MyConvex();
 117                                 ~MyConvex();
 118
 119         bool            LoadFromFile(const char* filename);
 120         void            Render(bool only_wireframe, const btVector3& wire_color) const;
 121         void            Project(const btVector3& dir, float& min, float& max) const;
 122
 123         int                     mNbVerts;
 124         btVector3*      mVerts;
 125         int                     mNbPolys;
 126         MyPoly*         mPolys;
 127         btTransform     mTransform;
 128 };
 129
 130 MyConvex::MyConvex() :
 131         mNbVerts        (0),
 132         mVerts          (NULL),
 133         mNbPolys        (0),
 134         mPolys          (NULL)
 135 {
 136         mTransform.setIdentity();
 137 }
 138
 139 MyConvex::~MyConvex()
 140 {
 141         delete[]mPolys;
 142         delete[]mVerts;
 143 }
 144
 145 bool MyConvex::LoadFromFile(const char* filename)
 146 {
 147         FILE* fp = fopen(filename, "rb");
 148         if(!fp) return false;
 149
 150         fread(&mNbVerts, sizeof(int), 1, fp);
 151
 152         int i;
 153
 154         mVerts = new btVector3[mNbVerts];
 155         for( i=0;i<mNbVerts;i++)
 156         {
 157                 float vals[3];
 158                 fread(vals, sizeof(float)*3, 1, fp);
 159                 mVerts[i].setX(vals[0]);
 160                 mVerts[i].setY(vals[1]);
 161                 mVerts[i].setZ(vals[2]);
 162         }
 163
 164         fread(&mNbPolys, sizeof(int), 1, fp);
 165         mPolys = new MyPoly[mNbPolys];
 166
 167         for(i=0;i<mNbPolys;i++)
 168         {
 169                 fread(&mPolys[i].mNbVerts, sizeof(short), 1, fp);
 170                 mPolys[i].mIndices = new char[mPolys[i].mNbVerts];
 171                 fread(mPolys[i].mIndices, mPolys[i].mNbVerts, 1, fp);
 172                 fread(mPolys[i].mPlane, sizeof(float)*4, 1, fp);
 173         }
 174         fclose(fp);
 175         return true;
 176 }
 177
 178
 179
 180 //See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho
 181 static void setOrthographicProjection()
 182 {
 183
 184         // switch to projection mode
 185         glMatrixMode(GL_PROJECTION);
 186         // save previous matrix which contains the
 187         //settings for the perspective projection
 188         glPushMatrix();
 189         // reset matrix
 190         glLoadIdentity();
 191         // set a 2D orthographic projection
 192         gluOrtho2D(0, glutScreenWidth, 0, glutScreenHeight);
 193         // invert the y axis, down is positive
 194         glScalef(1, -1, 1);
 195         // mover the origin from the bottom left corner
 196         // to the upper left corner
 197         glTranslatef(0, -glutScreenHeight, 0);
 198         glMatrixMode(GL_MODELVIEW);
 199 }
 200
 201 static void resetPerspectiveProjection()
 202 {
 203         glMatrixMode(GL_PROJECTION);
 204         glPopMatrix();
 205         glMatrixMode(GL_MODELVIEW);
 206 }
 207
 208 void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const
 209 {
 210         const float Scale = 1.0f;
 211         glPushMatrix();
 212
 213         ATTRIBUTE_ALIGNED16(btScalar) glmat[16];        //4x4 column major matrix for OpenGL.
 214
 215         mTransform.getOpenGLMatrix(glmat);
 216 #ifndef BT_USE_DOUBLE_PRECISION
 217         glMultMatrixf(&(glmat[0]));
 218 #else
 219         glMultMatrixd(&(glmat[0]));
 220 #endif
 221         if(!only_wireframe)
 222         {
 223                 btVector3 color(0.0f, 0.5f, 1.0f);
 224                 for(int i=0;i<mNbPolys;i++)
 225                 {
 226                         glNormal3f(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);
 227
 228                         int NbTris = mPolys[i].mNbVerts-2;
 229                         const btVector3& p0 = mVerts[mPolys[i].mIndices[0]]*Scale;
 230                         for(int j=1;j<=NbTris;j++)
 231                         {
 232                                 int k = (j+1)%mPolys[i].mNbVerts;
 233
 234                                 const btVector3& p1 = mVerts[mPolys[i].mIndices[j]]*Scale;
 235                                 const btVector3& p2 = mVerts[mPolys[i].mIndices[k]]*Scale;
 236
 237                                 DrawTriangle(p0, p1, p2, color);
 238                         }
 239                 }
 240         }
 241
 242         {
 243                 btVector3 color;
 244                 if(only_wireframe)
 245                         color = wire_color;
 246                 else
 247                         color = btVector3(0.0f, 0.0f, 0.0f);
 248
 249                 for(int i=0;i<mNbPolys;i++)
 250                 {
 251                         for(int j=0;j<mPolys[i].mNbVerts;j++)
 252                         {
 253                                 int k = (j+1)%mPolys[i].mNbVerts;
 254                                 DrawLine(mVerts[mPolys[i].mIndices[j]]*Scale, mVerts[mPolys[i].mIndices[k]]*Scale, color, 1.0f);
 255                         }
 256                 }
 257         }
 258
 259         glPopMatrix();
 260 }
 261
 262 void MyConvex::Project(const btVector3& dir, float& min, float& max) const
 263 {
 264         min = FLT_MAX;
 265         max = -FLT_MAX;
 266         for(int i=0;i<mNbVerts;i++)
 267         {
 268                 btVector3 pt = mTransform * mVerts[i];
 269                 float dp = pt.dot(dir);
 270                 if(dp < min)    min = dp;
 271                 if(dp > max)    max = dp;
 272         }
 273         if(min>max)
 274         {
 275                 float tmp = min;
 276                 min = max;
 277                 max = tmp;
 278         }
 279 }
 280
 281 static btVector3 gNormal;
 282 static btVector3 gPoint;
 283 static float gDepth;
 284
 285         struct MyResult : public btDiscreteCollisionDetectorInterface::Result
 286         {
 287                 virtual void setShapeIdentifiersA(int partId0, int index0)
 288                 {
 289                 }
 290                 virtual void setShapeIdentifiersB(int partId1, int index1)
 291                 {
 292                 }
 293
 294                 virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 295                 {
 296                         gNormal = normalOnBInWorld;
 297                         gPoint  = pointInWorld;
 298                         gDepth  = depth;
 299                 }
 300         };
 301
 302 //2 Mb by default, could be made smaller
 303 btStackAlloc gStackAlloc(1024*1024*2);
 304
 305 static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 306 {
 307         static btSimplexSolverInterface simplexSolver;
 308 #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
 309 //      static Solid3JohnsonSimplexSolver simplexSolver2;
 310 #endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
 311
 312         simplexSolver.reset();
 313
 314         btConvexHullShape convexA((btScalar*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3));
 315         btConvexHullShape convexB((btScalar*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3));
 316
 317         static btGjkEpaPenetrationDepthSolver Solver0;
 318         static btMinkowskiPenetrationDepthSolver Solver1;
 319
 320 #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
 321         static Solid3EpaPenetrationDepth Solver2;
 322         static EpaPenetrationDepthSolver Solver3;
 323 #endif
 324
 325
 326         btConvexPenetrationDepthSolver* Solver = NULL ;
 327                         if(gMethod==0)
 328                                 Solver = &Solver0;
 329         else    if(gMethod==1)
 330                                 Solver = &Solver1;
 331 #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
 332         else    if(gMethod==2)
 333                                 Solver = &Solver2;
 334         else
 335                                 Solver = &Solver3;
 336 #endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
 337
 338
 339 #ifdef USE_ORIGINAL
 340
 341         btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver);
 342         GJK.m_catchDegeneracies = 1;
 343         convexA.setMargin(0.01f);
 344         convexB.setMargin(0.01f);
 345
 346         btDiscreteCollisionDetectorInterface::ClosestPointInput input;
 347         input.m_transformA = hull0.mTransform;
 348         input.m_transformB = hull1.mTransform;
 349         input.m_stackAlloc = &gStackAlloc;
 350
 351         MyResult output;
 352         GJK.getClosestPoints(input, output, 0);
 353         gLastUsedMethod = GJK.m_lastUsedMethod;
 354         gNumGjkIterations = GJK.m_curIter;
 355         gLastDegenerateSimplex= GJK.m_degenerateSimplex;
 356 #else
 357         MyResult output;
 358         btVector3       witnesses[2];
 359         btVector3       normal;
 360         btScalar        depth;
 361
 362         btGjkEpaSolver::sResults results;
 363         btScalar radialMargin = 0.01f;
 364
 365         btGjkEpaSolver::Collide(&convexA,hull0.mTransform,
 366                 &convexB,hull1.mTransform,
 367                 radialMargin,
 368                 results);
 369         if (results.depth>0)
 370         {
 371                 output.addContactPoint(results.normal,results.witnesses[1],-results.depth);
 372         }
 373 #endif
 374         return true;
 375 }
 376
 377 static bool TestSepAxis(const btVector3& sep_axis, const MyConvex& hull0, const MyConvex& hull1, float& depth)
 378 {
 379         float Min0,Max0;
 380         float Min1,Max1;
 381         hull0.Project(sep_axis, Min0, Max0);
 382         hull1.Project(sep_axis, Min1, Max1);
 383
 384         if(Max0<Min1 || Max1<Min0)
 385                 return false;
 386
 387         float d0 = Max0 - Min1;
 388         btAssert(d0>=0.0f);
 389         float d1 = Max1 - Min0;
 390         btAssert(d1>=0.0f);
 391         depth = d0<d1 ? d0:d1;
 392         return true;
 393 }
 394
 395 inline bool IsAlmostZero(const btVector3& v)
 396 {
 397         if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
 398         return true;
 399 }
 400
 401 static bool ReferenceCode(const MyConvex& hull0, const MyConvex& hull1, float& dmin, btVector3& sep)
 402 {
 403         dmin = FLT_MAX;
 404
 405         int i;
 406
 407         // Test normals from hull0
 408         for( i=0;i<hull0.mNbPolys;i++)
 409         {
 410                 btVector3 Normal(hull0.mPolys[i].mPlane[0], hull0.mPolys[i].mPlane[1], hull0.mPolys[i].mPlane[2]);
 411
 412 //              btVector3 WorldNormal = hull0.mTransform * Normal;
 413                 btVector3 WorldNormal = hull0.mTransform.getBasis() * Normal;
 414
 415                 float d;
 416                 if(!TestSepAxis(WorldNormal, hull0, hull1, d))
 417                         return false;
 418
 419                 if(d<dmin)
 420                 {
 421                         dmin = d;
 422                         sep = WorldNormal;
 423                 }
 424         }
 425
 426         // Test normals from hull1
 427         for( i=0;i<hull1.mNbPolys;i++)
 428         {
 429                 btVector3 Normal(hull1.mPolys[i].mPlane[0], hull1.mPolys[i].mPlane[1], hull1.mPolys[i].mPlane[2]);
 430
 431 //              btVector3 WorldNormal = hull1.mTransform * Normal;
 432                 btVector3 WorldNormal = hull1.mTransform.getBasis() * Normal;
 433
 434                 float d;
 435                 if(!TestSepAxis(WorldNormal, hull0, hull1, d))
 436                         return false;
 437
 438                 if(d<dmin)
 439                 {
 440                         dmin = d;
 441                         sep = WorldNormal;
 442                 }
 443         }
 444
 445         // Test edges
 446         for(int j=0;j<hull0.mNbPolys;j++)
 447         {
 448                 const MyPoly& poly0 = hull0.mPolys[j];
 449
 450                 for(int i=0;i<hull1.mNbPolys;i++)
 451                 {
 452                         const MyPoly& poly1 = hull1.mPolys[i];
 453
 454                         for(int e0=0;e0<poly0.mNbVerts;e0++)
 455                         {
 456                                 const btVector3& a = hull0.mVerts[poly0.mIndices[e0]];
 457                                 const btVector3& b = hull0.mVerts[poly0.mIndices[(e0+1)%poly0.mNbVerts]];
 458
 459                                 btVector3 edge0 = a - b;
 460                                 btVector3 WorldEdge0 = hull0.mTransform.getBasis() * edge0;
 461
 462                                 for(int e1=0;e1<poly1.mNbVerts;e1++)
 463                                 {
 464                                         const btVector3& c = hull1.mVerts[poly1.mIndices[e1]];
 465                                         const btVector3& d = hull1.mVerts[poly1.mIndices[(e1+1)%poly1.mNbVerts]];
 466
 467                                         btVector3 edge1 = c - d;
 468                                         btVector3 WorldEdge1 = hull1.mTransform.getBasis() * edge1;
 469
 470                                         btVector3 Cross = WorldEdge0.cross(WorldEdge1);
 471                                         if(!IsAlmostZero(Cross))
 472                                         {
 473                                                 Cross = Cross.normalize();
 474
 475                                                 float d;
 476                                                 if(!TestSepAxis(Cross, hull0, hull1, d))
 477                                                         return false;
 478
 479                                                 if(d<dmin)
 480                                                 {
 481                                                         dmin = d;
 482                                                         sep = Cross;
 483                                                 }
 484
 485                                         }
 486                                 }
 487                         }
 488                 }
 489         }
 490
 491
 492
 493         btVector3 DeltaC = (hull1.mTransform * hull1.mTransform.getOrigin()) - (hull0.mTransform * hull0.mTransform.getOrigin());
 494         if((DeltaC.dot(sep))>0.0f)      sep = -sep;
 495
 496         return true;
 497 }
 498
 499
 500
 501 static MyConvex gConvex0;
 502 static MyConvex gConvex1;
 503
 504 static void KeyboardCallback(unsigned char key, int x, int y)
 505 {
 506         switch (key)
 507         {
 508         case 27:        exit(0); break;
 509
 510         case 'R':
 511         case 'r':
 512                 gRefMode = !gRefMode;
 513                 break;
 514
 515         case ' ':
 516                 gMethod++;
 517 #ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
 518                 if(gMethod==4)  gMethod=0;
 519 #else
 520                 if(gMethod==2)  gMethod=0;
 521 #endif
 522                 break;
 523
 524         case '4':
 525                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(-gDisp,0,0));
 526                 break;
 527         case '7':
 528                 gConvex0.mTransform.setRotation(gConvex0.mTransform.getRotation()*btQuaternion(btVector3(1,0,0),0.01));
 529                 break;
 530         case '9':
 531                 gConvex0.mTransform.setRotation(gConvex0.mTransform.getRotation()*btQuaternion(btVector3(1,0,0),-0.01));
 532                 break;
 533         case '1':
 534                 gConvex0.mTransform.setRotation(gConvex0.mTransform.getRotation()*btQuaternion(btVector3(0,1,0),0.01));
 535                 break;
 536         case '3':
 537                 gConvex0.mTransform.setRotation(gConvex0.mTransform.getRotation()*btQuaternion(btVector3(0,1,0),-0.01));
 538                 break;
 539         case '5':
 540                 gConvex0.mTransform.setRotation(gConvex0.mTransform.getRotation()*btQuaternion(btVector3(0,0,1),0.01));
 541                 break;
 542
 543         case '6':
 544                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(gDisp,0,0));
 545                 break;
 546         case '8':
 547                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(0,gDisp,0));
 548                 break;
 549         case '2':
 550                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(0,-gDisp,0));
 551                 break;
 552
 553         case 101:       Eye += Dir * gCamSpeed; break;
 554         case 103:       Eye -= Dir * gCamSpeed; break;
 555         case 100:       Eye -= N * gCamSpeed; break;
 556         case 102:       Eye += N * gCamSpeed; break;
 557         }
 558 }
 559
 560 static void ArrowKeyCallback(int key, int x, int y)
 561 {
 562         KeyboardCallback(key,x,y);
 563 }
 564
 565 static void MouseCallback(int button, int state, int x, int y)
 566 {
 567         mx = x;
 568         my = y;
 569 }
 570
 571 static const float NxPiF32              = 3.141592653589793f;
 572
 573 float degToRad(float a)
 574         {
 575         return (float)0.01745329251994329547 * a;
 576         }
 577
 578 class NxQuat
 579         {
 580         public:
 581         NxQuat(){}
 582
 583         NxQuat(const float angle, const btVector3 & axis)
 584         {
 585         x = axis.x();
 586         y = axis.y();
 587         z = axis.z();
 588
 589         const float i_length =  1.0f / sqrtf( x*x + y*y + z*z );
 590         x = x * i_length;
 591         y = y * i_length;
 592         z = z * i_length;
 593
 594         float Half = degToRad(angle * 0.5f);
 595
 596         w = cosf(Half);
 597         const float sin_theta_over_two = sinf(Half );
 598         x = x * sin_theta_over_two;
 599         y = y * sin_theta_over_two;
 600         z = z * sin_theta_over_two;
 601         }
 602
 603         void multiply(const NxQuat& left, const btVector3& right)
 604         {
 605         float a,b,c,d;
 606
 607         a = - left.x*right.x() - left.y*right.y() - left.z *right.z();
 608         b =   left.w*right.x() + left.y*right.z() - right.y()*left.z;
 609         c =   left.w*right.y() + left.z*right.x() - right.z()*left.x;
 610         d =   left.w*right.z() + left.x*right.y() - right.x()*left.y;
 611
 612         w = a;
 613         x = b;
 614         y = c;
 615         z = d;
 616         }
 617
 618         void rotate(btVector3 & v) const
 619         {
 620         NxQuat myInverse;
 621         myInverse.x = -x;
 622         myInverse.y = -y;
 623         myInverse.z = -z;
 624         myInverse.w =  w;
 625
 626         NxQuat left;
 627         left.multiply(*this,v);
 628         float vx = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
 629         float vy = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
 630         float vz = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
 631         v.setValue(vx, vy, vz);
 632         }
 633
 634     float x,y,z,w;
 635 };
 636
 637
 638 static void MotionCallback(int x, int y)
 639 {
 640         int dx = mx - x;
 641         int dy = my - y;
 642
 643         Dir = Dir.normalize();
 644         N = Dir.cross(btVector3(0,1,0));
 645
 646         NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0));
 647         qx.rotate(Dir);
 648         NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N);
 649         qy.rotate(Dir);
 650
 651         mx = x;
 652         my = y;
 653 }
 654
 655 static void RenderCallback()
 656 {
 657         // Clear buffers
 658         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 659
 660         // Setup camera
 661         glMatrixMode(GL_PROJECTION);
 662         glLoadIdentity();
 663         gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f);
 664         gluLookAt(Eye.x(), Eye.y(), Eye.z(), Eye.x() + Dir.x(), Eye.y() + Dir.y(), Eye.z() + Dir.z(), 0.0f, 1.0f, 0.0f);
 665
 666         glMatrixMode(GL_MODELVIEW);
 667         glLoadIdentity();
 668
 669         glEnable(GL_LIGHTING);
 670
 671         //clear previous frames result
 672         gNormal.setValue(10,0,0);
 673         gPoint.setValue(0,0,0);
 674         gDepth = 999.999;
 675         gLastUsedMethod = -1;
 676         gNumGjkIterations = -1;
 677
 678
 679         TestEPA(gConvex0, gConvex1);
 680         glMatrixMode(GL_MODELVIEW);
 681         glLoadIdentity();
 682
 683         btVector3 RefSep(btScalar(0.), btScalar(0.), btScalar(0.));
 684         float RefDMin=0.f;
 685         bool RefResult = false;
 686         if(gRefMode)
 687                 RefResult = ReferenceCode(gConvex0, gConvex1, RefDMin, RefSep);
 688
 689 //      DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f);
 690 //      printf("%f:  %f  %f  %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
 691
 692 #ifdef VERBOSE_TEXT_ONSCREEN
 693         glColor3f(255.f, 255.f, 255.f);
 694
 695         setOrthographicProjection();
 696         float xOffset = 10.f;
 697         float yStart = 20.f;
 698         float yIncr = 20.f;
 699         char buf[124];
 700
 701         sprintf(buf,"gDepth=%f:  gNormal=(%f  %f  %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
 702         GLDebugDrawString(xOffset,yStart,buf);
 703         yStart += yIncr;
 704
 705         sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
 706         GLDebugDrawString(xOffset,yStart,buf);
 707         yStart += yIncr;
 708
 709         sprintf(buf,"gLastUsedMethod=%d\n", gLastUsedMethod);
 710         GLDebugDrawString(xOffset,yStart,buf);
 711         yStart += yIncr;
 712
 713
 714
 715
 716
 717         if (gLastUsedMethod >= 3)
 718         {
 719                 switch (        gMethod)
 720                 {
 721                 case 0:
 722                         sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" );
 723                         break;
 724                 case 1:
 725                         sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" );
 726                         break;
 727                 case 2:
 728                                 sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
 729                                 break;
 730                 case 3:
 731                         sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" );
 732                         break;
 733                 default:
 734                                 sprintf(buf,"Unknown Penetration Depth\n" );
 735                 }
 736                 GLDebugDrawString(xOffset,yStart,buf);
 737                 yStart += yIncr;
 738
 739         } else
 740         {
 741                 sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
 742                 GLDebugDrawString(xOffset,yStart,buf);
 743                 yStart += yIncr;
 744         }
 745
 746         if (gLastDegenerateSimplex)
 747         {
 748                 sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
 749                 GLDebugDrawString(xOffset,yStart,buf);
 750                 yStart += yIncr;
 751         }
 752
 753
 754
 755
 756         resetPerspectiveProjection();
 757 #endif //VERBOSE_TEXT_ONSCREEN
 758
 759         btVector3 color(0,0,0);
 760         gConvex0.Render(false, color);
 761         gConvex1.Render(false, color);
 762
 763         if(gDepth<0.0f)
 764         {
 765                 btTransform Saved = gConvex0.mTransform;
 766                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() - btVector3(gNormal*gDepth));
 767                 gConvex0.Render(true, btVector3(1.0f, 0.5f, 0.0f));
 768                 gConvex0.mTransform = Saved;
 769         }
 770         else
 771         {
 772                 DrawLine(gPoint, gPoint + gNormal, btVector3(0,1,0), 2.0f);
 773         }
 774
 775         if(RefResult & gRefMode)
 776         {
 777                 btTransform Saved = gConvex0.mTransform;
 778                 gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(RefSep*RefDMin));
 779                 gConvex0.Render(true, btVector3(0.0f, 0.5f, 1.0f));
 780                 gConvex0.mTransform = Saved;
 781         }
 782
 783         glutSwapBuffers();
 784 }
 785
 786 static void ReshapeCallback(int width, int height)
 787 {
 788         glViewport(0, 0, width, height);
 789 }
 790
 791 static void IdleCallback()
 792 {
 793         glutPostRedisplay();
 794 }
 795
 796 int main(int argc, char** argv)
 797 {
 798         // Initialize Glut
 799         glutInit(&argc, argv);
 800         glutInitWindowSize(glutScreenWidth, glutScreenHeight);
 801         glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
 802         int mainHandle = glutCreateWindow("TestBullet");
 803         glutSetWindow(mainHandle);
 804         glutDisplayFunc(RenderCallback);
 805         glutReshapeFunc(ReshapeCallback);
 806         glutIdleFunc(IdleCallback);
 807         glutKeyboardFunc(KeyboardCallback);
 808         glutSpecialFunc(ArrowKeyCallback);
 809         glutMouseFunc(MouseCallback);
 810         glutMotionFunc(MotionCallback);
 811         MotionCallback(0,0);
 812
 813         // Setup default render states
 814         glClearColor(0.3f, 0.4f, 0.5f, 1.0);
 815         glEnable(GL_DEPTH_TEST);
 816         glEnable(GL_COLOR_MATERIAL);
 817         glEnable(GL_CULL_FACE);
 818
 819         // Setup lighting
 820         glEnable(GL_LIGHTING);
 821         float AmbientColor[] = { 0.0f, 0.1f, 0.2f, 0.0f };              glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientColor);
 822         float DiffuseColor[] = { 1.0f, 1.0f, 1.0f, 0.0f };              glLightfv(GL_LIGHT0, GL_DIFFUSE, DiffuseColor);
 823         float SpecularColor[] = { 0.0f, 0.0f, 0.0f, 0.0f };             glLightfv(GL_LIGHT0, GL_SPECULAR, SpecularColor);
 824         float Position[] = { -10.0f, 1000.0f, -4.0f, 1.0f };    glLightfv(GL_LIGHT0, GL_POSITION, Position);
 825         glEnable(GL_LIGHT0);
 826
 827         //
 828         bool Status = gConvex0.LoadFromFile("convex0.bin");
 829         if(!Status)
 830         {
 831                 Status = gConvex0.LoadFromFile("../../convex0.bin");
 832                 if(!Status)
 833                 {
 834                         printf("Failed to load object!\n");
 835                         exit(0);
 836                 }
 837         }
 838         Status = gConvex1.LoadFromFile("convex0.bin");
 839         if(!Status)
 840         {
 841                 Status = gConvex1.LoadFromFile("../../convex0.bin");
 842                 if(!Status)
 843                 {
 844                         printf("Failed to load object!\n");
 845                         exit(0);
 846                 }
 847         }
 848
 849 //      gConvex0.mTransform.setOrigin(btVector3(1.0f, 1.0f, 0.0f));
 850         gConvex0.mTransform.setOrigin(btVector3(0.20000069f, 0.95000005f, 0.0f));
 851
 852         // Run
 853         glutMainLoop();
 854
 855         return 0;
 856 }