diff --git a/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp b/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp deleted file mode 100644 index f0cbb09bf..000000000 --- a/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp +++ /dev/null @@ -1,464 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -EPA Copyright (c) Ricardo Padrela 2006 - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -/** -* Sample that shows the Expanding Polytope Algorithm ( EPA ) -* bterates two convex shapes and calculates the penetration depth -* between them in case they are penetrating -*/ - -#include "GL_Simplex1to4.h" -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btTransform.h" -#include "GL_ShapeDrawer.h" -#include -#include "GlutStuff.h" - -#include -#include -#include - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" - -#include "NarrowPhaseCollision/EpaCommon.h" -#include "NarrowPhaseCollision/EpaVertex.h" -#include "NarrowPhaseCollision/EpaHalfEdge.h" -#include "NarrowPhaseCollision/EpaFace.h" -#include "NarrowPhaseCollision/EpaPolyhedron.h" -#include "NarrowPhaseCollision/Epa.h" -#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" -#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" -EpaPenetrationDepthSolver epaPenDepthSolver; - - -btSimplexSolverInterface simplexSolver; - - -int screenWidth = 640.f; -int screenHeight = 480.f; - -// Scene stuff -btPoint3 g_sceneVolumeMin( -1, -1, -1 ); -btPoint3 g_sceneVolumeMax( 1, 1, 1 ); - -bool g_shapesPenetrate = false; - -btVector3 g_wWitnesses[ 2 ]; - -// Shapes stuff -btConvexShape* g_pConvexShapes[ 2 ] = { 0 }; -btTransform g_convexShapesTransform[ 2 ]; - -btScalar g_animAngle = SIMD_RADS_PER_DEG; -bool g_pauseAnim = true; - -// 0 - Box ; 1 - Sphere -int g_shapesType[ 2 ] = { 0 }; - -// Box config -btVector3 g_boxExtents( 1, 1, 1 ); -// Sphere config -btScalar g_sphereRadius = 1; - -float randomFloat( float rangeMin, float rangeMax ) -{ - return ( ( ( float ) ( rand() ) / ( float ) ( RAND_MAX ) ) * ( rangeMax - rangeMin ) + rangeMin ); -} - -int randomShapeType( int minShapeType, int maxShapeType ) -{ - return ( ( ( ( maxShapeType - minShapeType ) + 1 ) * rand() ) / ( ( RAND_MAX + 1 ) + minShapeType ) ); -} - -btVector3 randomPosition( const btPoint3& minPoint, const btPoint3& maxPoint ) -{ - return btVector3( randomFloat( minPoint.getX(), maxPoint.getX() ), - randomFloat( minPoint.getY(), maxPoint.getY() ), - randomFloat( minPoint.getZ(), maxPoint.getZ() ) ); -} - -bool createBoxShape( int shapeIndex ) -{ - //#ifdef _DEBUG - //static bool b = true; - // - //if ( b ) - //{ - // g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 1, 1, 1 ) ); - - // g_pConvexShapes[ shapeIndex ]->setMargin( 0.05 ); - - // g_convexShapesTransform[ shapeIndex ].setIdentity(); - - // btMatrix3x3 basis( 0.99365157, 0.024418538, -0.10981932, - // -0.025452739, 0.99964380, -0.0080251107, - // 0.10958424, 0.010769366, 0.99391919 ); - - // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 4.4916530, -19.059078, -0.22695254 ) ); - // g_convexShapesTransform[ shapeIndex ].setBasis( basis ); - - // b = false; - //} - //else - //{ - // g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 25, 10, 25 ) ); - - // g_pConvexShapes[ shapeIndex ]->setMargin( 0.05 ); - - // //btMatrix3x3 basis( 0.658257, 0.675022, -0.333709, - // // -0.333120, 0.658556, 0.675023, - // // 0.675314, -0.333120, 0.658256 ); - - // g_convexShapesTransform[ shapeIndex ].setIdentity(); - - // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0, -30, 0/*0.326090, -0.667531, 0.214331*/ ) ); - // //g_convexShapesTransform[ shapeIndex ].setBasis( basis ); - //} - //#endif - - g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 1, 1, 1 ) ); - - g_pConvexShapes[ shapeIndex ]->setMargin( 1e-1 ); - - g_convexShapesTransform[ shapeIndex ].setIdentity(); - - g_convexShapesTransform[ shapeIndex ].setOrigin( randomPosition( g_sceneVolumeMin, g_sceneVolumeMax ) ); - - return true; -} - -bool createSphereShape( int shapeIndex ) -{ - g_pConvexShapes[ shapeIndex ] = new btSphereShape( g_sphereRadius ); - - g_pConvexShapes[ shapeIndex ]->setMargin( 1e-1 ); - - g_convexShapesTransform[ shapeIndex ].setIdentity(); - g_convexShapesTransform[ shapeIndex ].setOrigin( randomPosition( g_sceneVolumeMin, g_sceneVolumeMax ) ); - - //#ifdef _DEBUG - //static bool b = true; - //if ( b ) - //{ - // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0.001, 0, 0 ) ); - // b = false; - //} - //else - //{ - // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0, 0, 0 ) ); - //} - //#endif - - return true; -} - -void destroyShapes() -{ - if ( g_pConvexShapes[ 0 ] ) - { - delete g_pConvexShapes[ 0 ]; - g_pConvexShapes[ 0 ] = 0; - } - - if ( g_pConvexShapes[ 1 ] ) - { - delete g_pConvexShapes[ 1 ]; - g_pConvexShapes[ 1 ] = 0; - } -} - -bool calcPenDepth() -{ - // Ryn Hybrid Pen Depth and EPA if necessary - - btVector3 v( 1, 0, 0 ); - - btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = 0.f; - - const btScalar margin = g_pConvexShapes[ 0 ]->getMargin() + g_pConvexShapes[ 1 ]->getMargin(); - const btScalar marginSqrd = margin * margin; - - btScalar maxRelErrorSqrd = 1e-3 * 1e-3; - - simplexSolver.reset(); - - while ( true ) - { - assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); - - btVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis(); - btVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis(); - - btVector3 pInA = g_pConvexShapes[ 0 ]->localGetSupportingVertexWithoutMargin( seperatingAxisInA ); - btVector3 qInB = g_pConvexShapes[ 1 ]->localGetSupportingVertexWithoutMargin( seperatingAxisInB ); - - btPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA ); - btPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB ); - - btVector3 w = pWorld - qWorld; - delta = v.dot( w ); - - // potential exit, they don't overlap - if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) ) - { - // Convex shapes do not overlap - return false; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if ( ( squaredDistance - delta <= squaredDistance * maxRelErrorSqrd ) || simplexSolver.inSimplex( w ) ) - { - simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); - - assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); - btScalar vLength = sqrt( squaredDistance ); - - g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->getMargin() / vLength ); - g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->getMargin() / vLength ); - - return true; - } - - //add current vertex to simplex - simplexSolver.addVertex( w, pWorld, qWorld ); - - //calculate the closest point to the origin (update vector v) - if ( !simplexSolver.closest( v ) ) - { - simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); - - assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); - btScalar vLength = sqrt( squaredDistance ); - - g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->getMargin() / vLength ); - g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->getMargin() / vLength ); - - return true; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = v.length2(); - - //are we getting any closer ? - if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) - { - simplexSolver.backup_closest( v ); - squaredDistance = v.length2(); - - simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); - - assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); - btScalar vLength = sqrt( squaredDistance ); - - g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->getMargin() / vLength ); - g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->getMargin() / vLength ); - - return true; - } - - if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) - { - // Run EPA - break; - } - } - - return epaPenDepthSolver.calcPenDepth( simplexSolver, g_pConvexShapes[ 0 ], g_pConvexShapes[ 1 ], - g_convexShapesTransform[ 0 ], g_convexShapesTransform[ 1 ], v, - g_wWitnesses[ 0 ], g_wWitnesses[ 1 ], 0 ); -} - -int main(int argc,char** argv) -{ - srand( time( 0 ) ); - - g_shapesType[ 0 ] = randomShapeType( 0, 1 ); - g_shapesType[ 1 ] = randomShapeType( 0, 1 ); - - ( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 ); - ( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 ); - - g_shapesPenetrate = calcPenDepth(); - - return glutmain( argc, argv, screenWidth, screenHeight, "EPAPenDepthDemo" ); -} - -void drawShape( int shapeIndex ) -{ - float m[ 16 ]; - g_convexShapesTransform[ shapeIndex ].getOpenGLMatrix( m ); - - glMultMatrixf( m ); - - if ( g_pConvexShapes[ shapeIndex ]->getShapeType() == BOX_SHAPE_PROXYTYPE ) - { - glutWireCube( ( ( btBoxShape* ) g_pConvexShapes[ shapeIndex ] )->getHalfExtents().x() * 2 ); - } - else if ( g_pConvexShapes[ shapeIndex ]->getShapeType() == SPHERE_SHAPE_PROXYTYPE ) - { - glutWireSphere( 1, 16, 16 ); - } -} - -void drawPenDepthVector() -{ - glLoadIdentity(); - - glColor3f( 1, 1, 0 ); - - glPointSize( 5 ); - glBegin( GL_POINTS ); - - glVertex3f( g_wWitnesses[ 0 ].getX(), g_wWitnesses[ 0 ].getY(), g_wWitnesses[ 0 ].getZ() ); - glVertex3f( g_wWitnesses[ 1 ].getX(), g_wWitnesses[ 1 ].getY(), g_wWitnesses[ 1 ].getZ() ); - - glEnd(); - - glColor3f( 1, 0, 0 ); - - glBegin( GL_LINES ); - - glVertex3f( g_wWitnesses[ 0 ].getX(), g_wWitnesses[ 0 ].getY(), g_wWitnesses[ 0 ].getZ() ); - glVertex3f( g_wWitnesses[ 1 ].getX(), g_wWitnesses[ 1 ].getY(), g_wWitnesses[ 1 ].getZ() ); - - glEnd(); -} - -void clientMoveAndDisplay() -{ - if ( !g_pauseAnim ) - { - btMatrix3x3 rot; - rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 ); - - btTransform t; - t.setIdentity(); - t.setBasis( rot ); - - //g_convexShapesTransform[ 0 ].mult( g_convexShapesTransform[ 0 ], t ); - g_convexShapesTransform[ 1 ].mult( g_convexShapesTransform[ 1 ], t ); - - g_shapesPenetrate = calcPenDepth(); - } - - clientDisplay(); -} - -void clientDisplay(void) { - - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - glDisable( GL_LIGHTING ); - - GL_ShapeDrawer::drawCoordSystem(); - - glMatrixMode( GL_MODELVIEW ); - - for ( int i = 0; i < 2; ++i ) - { - glPushMatrix(); - - drawShape( i ); - - glPopMatrix(); - } - - if ( g_shapesPenetrate ) - { - glPushMatrix(); - drawPenDepthVector(); - glPopMatrix(); - } - - glFlush(); - glutSwapBuffers(); -} - -void clientResetScene() -{ -} - -void clientKeyboard(unsigned char key, int x, int y) -{ - if ( key == 'R' || key == 'r' ) - { - destroyShapes(); - - g_shapesType[ 0 ] = randomShapeType( 0, 1 ); - g_shapesType[ 1 ] = randomShapeType( 0, 1 ); - - ( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 ); - ( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 ); - - g_shapesPenetrate = calcPenDepth(); - } - else if ( key == 'Q' || key == 'q' ) - { - destroyShapes(); - } - else if ( key == 'T' || key == 't' ) - { -#ifdef DEBUG_ME - btVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin(); - btVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin(); - - btMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis(); - btMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis(); - - FILE* fp = 0; - - fopen_s( &fp, "shapes.txt", "w" ); - - char str[ 256 ]; - sprintf_s( str, 256, "PosA: %f, %f, %f\nPosB: %f, %f, %f\n", shapeAPos.x(), shapeAPos.y(), shapeAPos.z(), - shapeBPos.x(), shapeBPos.y(), shapeBPos.z() ); - fputs( str, fp ); - - sprintf_s( str, 256, "RotA: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\nRotB: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\n\n", - shapeARot.getRow( 0 ).x(), shapeARot.getRow( 0 ).y(), shapeARot.getRow( 0 ).z(), - shapeARot.getRow( 1 ).x(), shapeARot.getRow( 1 ).y(), shapeARot.getRow( 1 ).z(), - shapeARot.getRow( 2 ).x(), shapeARot.getRow( 2 ).y(), shapeARot.getRow( 2 ).z(), - shapeBRot.getRow( 0 ).x(), shapeBRot.getRow( 0 ).y(), shapeBRot.getRow( 0 ).z(), - shapeBRot.getRow( 1 ).x(), shapeBRot.getRow( 1 ).y(), shapeBRot.getRow( 1 ).z(), - shapeBRot.getRow( 2 ).x(), shapeBRot.getRow( 2 ).y(), shapeBRot.getRow( 2 ).z()); - fputs( str, fp ); - - fclose( fp ); -#endif //DEBUG_ME - } - else if ( key == 'P' || key =='p' ) - { - g_pauseAnim = !g_pauseAnim; - } - - defaultKeyboard(key, x, y); -} - - -void clientMouseFunc(int button, int state, int x, int y) -{ -} - -void clientMotionFunc(int x,int y) -{ -} diff --git a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp new file mode 100644 index 000000000..3085ff5d3 --- /dev/null +++ b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp @@ -0,0 +1,625 @@ +#include +#include + +#include "btDiscreteCollisionDetectorInterface.h" +#include "btSimplexSolverInterface.h" +#include "btConvexHullShape.h" +#include "Solid3EpaPenetrationDepth.h" +#include "Solid3JohnsonSimplexSolver.h" +#include "btGjkPairDetector.h" +#include "btMinkowskiPenetrationDepthSolver.h" +#include "EpaPenetrationDepthSolver.h" + +static bool gRefMode = false; +static int gMethod = 0; +static const float gDisp = 0.01f; +static const float gCamSpeed = 0.1f; +static btVector3 Eye(3.0616338f, 1.1985892f, 2.5769043f); +static btVector3 Dir(-0.66853905,-0.14004262,-0.73037237); +static btVector3 N; +static int mx = 0; +static int my = 0; + +static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width) +{ + glDisable(GL_LIGHTING); + glLineWidth(line_width); + glColor4f(color.x(), color.y(), color.z(), 1.0f); + btVector3 tmp[] = {p0, p1}; + glEnableClientState(GL_VERTEX_ARRAY); + glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x()); + glDrawArrays(GL_LINES, 0, 2); + glDisableClientState(GL_VERTEX_ARRAY); + glColor4f(1.0f, 1.0f, 1.0f, 1.0f); + glEnable(GL_LIGHTING); +} + +void DrawTriangle(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& color) +{ +// glDisable(GL_LIGHTING); + glColor4f(color.x(), color.y(), color.z(), 1.0f); + btVector3 tmp[] = {p0, p1, p2}; + glEnableClientState(GL_VERTEX_ARRAY); + glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x()); + glDrawArrays(GL_TRIANGLES, 0, 3); + glDisableClientState(GL_VERTEX_ARRAY); +// glColor4f(1.0f, 1.0f, 1.0f, 1.0f); +// glEnable(GL_LIGHTING); +} + +class MyPoly +{ + public: + MyPoly() : mNbVerts(0), mIndices(NULL) {} + ~MyPoly() { delete[]mIndices; } + + short mNbVerts; + char* mIndices; + float mPlane[4]; +}; + +class MyConvex +{ + public: + MyConvex(); + ~MyConvex(); + + bool LoadFromFile(const char* filename); + void Render(bool only_wireframe, const btVector3& wire_color) const; + void Project(const btVector3& dir, float& min, float& max) const; + + int mNbVerts; + btVector3* mVerts; + int mNbPolys; + MyPoly* mPolys; + btTransform mTransform; +}; + +MyConvex::MyConvex() : + mNbVerts (0), + mVerts (NULL), + mNbPolys (0), + mPolys (NULL) +{ + mTransform.setIdentity(); +} + +MyConvex::~MyConvex() +{ + delete[]mPolys; + delete[]mVerts; +} + +bool MyConvex::LoadFromFile(const char* filename) +{ + FILE* fp = fopen(filename, "rb"); + if(!fp) return false; + + fread(&mNbVerts, sizeof(int), 1, fp); + + mVerts = new btVector3[mNbVerts]; + for(int i=0;i max) max = dp; + } + if(min>max) + { + float tmp = min; + min = max; + max = tmp; + } +} + +static btVector3 gNormal; +static btVector3 gPoint; +static float gDepth; + + struct MyResult : public btDiscreteCollisionDetectorInterface::Result + { + virtual void setShapeIdentifiers(int partId0, int index0, int partId1, int index1) + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, float depth) + { + gNormal = normalOnBInWorld; + gPoint = pointInWorld; + gDepth = depth; + } + }; + +static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1) +{ + static btSimplexSolverInterface simplexSolver; +// static Solid3JohnsonSimplexSolver simplexSolver; + simplexSolver.reset(); + + btConvexHullShape convexA((float*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3)); + btConvexHullShape convexB((float*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3)); + + static Solid3EpaPenetrationDepth Solver0; + static EpaPenetrationDepthSolver Solver1; + static btMinkowskiPenetrationDepthSolver Solver2; + + btConvexPenetrationDepthSolver* Solver; + if(gMethod==0) Solver = &Solver0; + else if(gMethod==1) Solver = &Solver1; + else Solver = &Solver2; + + btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver); + GJK.setIgnoreMargin(true); + convexA.setMargin(0.0f); + convexB.setMargin(0.0f); + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_transformA = hull0.mTransform; + input.m_transformB = hull1.mTransform; + + MyResult output; + GJK.getClosestPoints(input, output, 0); + return true; +} + +static bool TestSepAxis(const btVector3& sep_axis, const MyConvex& hull0, const MyConvex& hull1, float& depth) +{ + float Min0,Max0; + float Min1,Max1; + hull0.Project(sep_axis, Min0, Max0); + hull1.Project(sep_axis, Min1, Max1); + + if(Max0=0.0f); + float d1 = Max1 - Min0; + assert(d1>=0.0f); + depth = d01e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false; + return true; +} + +static bool ReferenceCode(const MyConvex& hull0, const MyConvex& hull1, float& dmin, btVector3& sep) +{ + dmin = FLT_MAX; + + // Test normals from hull0 + for(int i=0;i0.0f) sep = -sep; + + return true; +} + + + +static MyConvex gConvex0; +static MyConvex gConvex1; + +static void KeyboardCallback(unsigned char key, int x, int y) +{ + switch (key) + { + case 27: exit(0); break; + + case 'R': + case 'r': + gRefMode = !gRefMode; + break; + + case ' ': + gMethod++; + if(gMethod==3) gMethod=0; + break; + + case '4': + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(-gDisp,0,0)); + break; + case '6': + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(gDisp,0,0)); + break; + case '8': + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(0,gDisp,0)); + break; + case '2': + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(0,-gDisp,0)); + break; + + case 101: Eye += Dir * gCamSpeed; break; + case 103: Eye -= Dir * gCamSpeed; break; + case 100: Eye -= N * gCamSpeed; break; + case 102: Eye += N * gCamSpeed; break; + } +} + +static void ArrowKeyCallback(int key, int x, int y) +{ + KeyboardCallback(key,x,y); +} + +static void MouseCallback(int button, int state, int x, int y) +{ + mx = x; + my = y; +} + +static const float NxPiF32 = 3.141592653589793f; + +float degToRad(float a) + { + return (float)0.01745329251994329547 * a; + } + +class NxQuat + { + public: + NxQuat(){} + + NxQuat(const float angle, const btVector3 & axis) + { + x = axis.x(); + y = axis.y(); + z = axis.z(); + + const float i_length = 1.0f / sqrtf( x*x + y*y + z*z ); + x = x * i_length; + y = y * i_length; + z = z * i_length; + + float Half = degToRad(angle * 0.5f); + + w = cosf(Half); + const float sin_theta_over_two = sinf(Half ); + x = x * sin_theta_over_two; + y = y * sin_theta_over_two; + z = z * sin_theta_over_two; + } + +void NxQuat::multiply(const NxQuat& left, const btVector3& right) + { + float a,b,c,d; + + a = - left.x*right.x() - left.y*right.y() - left.z *right.z(); + b = left.w*right.x() + left.y*right.z() - right.y()*left.z; + c = left.w*right.y() + left.z*right.x() - right.z()*left.x; + d = left.w*right.z() + left.x*right.y() - right.x()*left.y; + + w = a; + x = b; + y = c; + z = d; + } + +void NxQuat::rotate(btVector3 & v) const + { + NxQuat myInverse; + myInverse.x = -x; + myInverse.y = -y; + myInverse.z = -z; + myInverse.w = w; + + NxQuat left; + left.multiply(*this,v); + float vx = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z; + float vy = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x; + float vz = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y; + v.setValue(vx, vy, vz); + } + + float x,y,z,w; + }; + + +static void MotionCallback(int x, int y) +{ + int dx = mx - x; + int dy = my - y; + + Dir = Dir.normalize(); + N = Dir.cross(btVector3(0,1,0)); + + NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0)); + qx.rotate(Dir); + NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N); + qy.rotate(Dir); + + mx = x; + my = y; +} + +static void RenderCallback() +{ + // Clear buffers + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + // Setup camera + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f); + 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); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + + glEnable(GL_LIGHTING); + + TestEPA(gConvex0, gConvex1); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + + float RefDMin; + btVector3 RefSep; + bool RefResult = false; + if(gRefMode) + RefResult = ReferenceCode(gConvex0, gConvex1, RefDMin, RefSep); + +// DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f); +// printf("%f: %f %f %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z()); + + btVector3 color(0,0,0); + gConvex0.Render(false, color); + gConvex1.Render(false, color); + + if(gDepth<0.0f) + { + btTransform Saved = gConvex0.mTransform; + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() - btVector3(gNormal*gDepth)); + gConvex0.Render(true, btVector3(1.0f, 0.5f, 0.0f)); + gConvex0.mTransform = Saved; + } + else + { + DrawLine(gPoint, gPoint + gNormal, btVector3(0,1,0), 2.0f); + } + + if(RefResult & gRefMode) + { + btTransform Saved = gConvex0.mTransform; + gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(RefSep*RefDMin)); + gConvex0.Render(true, btVector3(0.0f, 0.5f, 1.0f)); + gConvex0.mTransform = Saved; + } + + glutSwapBuffers(); +} + +static void ReshapeCallback(int width, int height) +{ + glViewport(0, 0, width, height); +} + +static void IdleCallback() +{ + glutPostRedisplay(); +} + +int main(int argc, char** argv) +{ + // Initialize Glut + glutInit(&argc, argv); + glutInitWindowSize(512, 512); + glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); + int mainHandle = glutCreateWindow("TestBullet"); + glutSetWindow(mainHandle); + glutDisplayFunc(RenderCallback); + glutReshapeFunc(ReshapeCallback); + glutIdleFunc(IdleCallback); + glutKeyboardFunc(KeyboardCallback); + glutSpecialFunc(ArrowKeyCallback); + glutMouseFunc(MouseCallback); + glutMotionFunc(MotionCallback); + MotionCallback(0,0); + + // Setup default render states + glClearColor(0.3f, 0.4f, 0.5f, 1.0); + glEnable(GL_DEPTH_TEST); + glEnable(GL_COLOR_MATERIAL); + glEnable(GL_CULL_FACE); + + // Setup lighting + glEnable(GL_LIGHTING); + float AmbientColor[] = { 0.0f, 0.1f, 0.2f, 0.0f }; glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientColor); + float DiffuseColor[] = { 1.0f, 1.0f, 1.0f, 0.0f }; glLightfv(GL_LIGHT0, GL_DIFFUSE, DiffuseColor); + float SpecularColor[] = { 0.0f, 0.0f, 0.0f, 0.0f }; glLightfv(GL_LIGHT0, GL_SPECULAR, SpecularColor); + float Position[] = { -10.0f, 1000.0f, -4.0f, 1.0f }; glLightfv(GL_LIGHT0, GL_POSITION, Position); + glEnable(GL_LIGHT0); + + // + bool Status = gConvex0.LoadFromFile("c:\\convex0.bin"); + if(!Status) + { + Status = gConvex0.LoadFromFile("convex0.bin"); + if(!Status) + { + printf("Failed to load object!\n"); + exit(0); + } + } + Status = gConvex1.LoadFromFile("c:\\convex0.bin"); + if(!Status) + { + Status = gConvex1.LoadFromFile("convex0.bin"); + if(!Status) + { + printf("Failed to load object!\n"); + exit(0); + } + } + +// gConvex0.mTransform.setOrigin(btVector3(1.0f, 1.0f, 0.0f)); + gConvex0.mTransform.setOrigin(btVector3(0.20000069f, 0.95000005f, 0.0f)); + + // Run + glutMainLoop(); + + return 0; +}