Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added Pierre's PenetrationTestBullet.cpp

Browse Source 
See http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=638
This commit is contained in:
ejcoumans
2006-10-27 21:22:29 +00:00
parent b8376a1673
commit 7987be45c5
2 changed files with 625 additions and 464 deletions
Download Patch File Download Diff File Expand all files Collapse all files

464
Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp
View File

@@ -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 <GL/glut.h>
#include "GlutStuff.h"
#include <iostream>
#include <list>
#include <time.h>
#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)
{
}

625
Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp Normal file
View File

@@ -0,0 +1,625 @@
#include <stdio.h>
#include <GL/glut.h>
#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<mNbVerts;i++)
{
float vals[3];
fread(vals, sizeof(float)*3, 1, fp);
mVerts[i].setX(vals[0]);
mVerts[i].setY(vals[1]);
mVerts[i].setZ(vals[2]);
}
fread(&mNbPolys, sizeof(int), 1, fp);
mPolys = new MyPoly[mNbPolys];
for(int i=0;i<mNbPolys;i++)
{
fread(&mPolys[i].mNbVerts, sizeof(short), 1, fp);
mPolys[i].mIndices = new char[mPolys[i].mNbVerts];
fread(mPolys[i].mIndices, mPolys[i].mNbVerts, 1, fp);
fread(mPolys[i].mPlane, sizeof(float)*4, 1, fp);
}
fclose(fp);
return true;
}
void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const
{
const float Scale = 1.0f;
glPushMatrix();
float glmat[16]; //4x4 column major matrix for OpenGL.
mTransform.getOpenGLMatrix(glmat);
glMultMatrixf(&(glmat[0]));
if(!only_wireframe)
{
btVector3 color(0.0f, 0.5f, 1.0f);
for(int i=0;i<mNbPolys;i++)
{
glNormal3f(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);
int NbTris = mPolys[i].mNbVerts-2;
const btVector3& p0 = mVerts[mPolys[i].mIndices[0]]*Scale;
for(int j=1;j<=NbTris;j++)
{
int k = (j+1)%mPolys[i].mNbVerts;
const btVector3& p1 = mVerts[mPolys[i].mIndices[j]]*Scale;
const btVector3& p2 = mVerts[mPolys[i].mIndices[k]]*Scale;
DrawTriangle(p0, p1, p2, color);
}
}
}
{
btVector3 color;
if(only_wireframe)
color = wire_color;
else
color = btVector3(0.0f, 0.0f, 0.0f);
for(int i=0;i<mNbPolys;i++)
{
for(int j=0;j<mPolys[i].mNbVerts;j++)
{
int k = (j+1)%mPolys[i].mNbVerts;
DrawLine(mVerts[mPolys[i].mIndices[j]]*Scale, mVerts[mPolys[i].mIndices[k]]*Scale, color, 1.0f);
}
}
}
glPopMatrix();
}
void MyConvex::Project(const btVector3& dir, float& min, float& max) const
{
min = FLT_MAX;
max = -FLT_MAX;
for(int i=0;i<mNbVerts;i++)
{
btVector3 pt = mTransform * mVerts[i];
float dp = pt.dot(dir);
if(dp < min) min = dp;
if(dp > 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<Min1 || Max1<Min0)
return false;
float d0 = Max0 - Min1;
assert(d0>=0.0f);
float d1 = Max1 - Min0;
assert(d1>=0.0f);
depth = d0<d1 ? d0:d1;
return true;
}
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-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;i<hull0.mNbPolys;i++)
{
btVector3 Normal(hull0.mPolys[i].mPlane[0], hull0.mPolys[i].mPlane[1], hull0.mPolys[i].mPlane[2]);
// btVector3 WorldNormal = hull0.mTransform * Normal;
btVector3 WorldNormal = hull0.mTransform.getBasis() * Normal;
float d;
if(!TestSepAxis(WorldNormal, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
// Test normals from hull1
for(int i=0;i<hull1.mNbPolys;i++)
{
btVector3 Normal(hull1.mPolys[i].mPlane[0], hull1.mPolys[i].mPlane[1], hull1.mPolys[i].mPlane[2]);
// btVector3 WorldNormal = hull1.mTransform * Normal;
btVector3 WorldNormal = hull1.mTransform.getBasis() * Normal;
float d;
if(!TestSepAxis(WorldNormal, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
// Test edges
for(int j=0;j<hull0.mNbPolys;j++)
{
const MyPoly& poly0 = hull0.mPolys[j];
for(int i=0;i<hull1.mNbPolys;i++)
{
const MyPoly& poly1 = hull1.mPolys[i];
for(int e0=0;e0<poly0.mNbVerts;e0++)
{
const btVector3& a = hull0.mVerts[poly0.mIndices[e0]];
const btVector3& b = hull0.mVerts[poly0.mIndices[(e0+1)%poly0.mNbVerts]];
btVector3 edge0 = a - b;
btVector3 WorldEdge0 = hull0.mTransform.getBasis() * edge0;
for(int e1=0;e1<poly1.mNbVerts;e1++)
{
const btVector3& c = hull1.mVerts[poly1.mIndices[e1]];
const btVector3& d = hull1.mVerts[poly1.mIndices[(e1+1)%poly1.mNbVerts]];
btVector3 edge1 = c - d;
btVector3 WorldEdge1 = hull1.mTransform.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
float d;
if(!TestSepAxis(Cross, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = Cross;
}
}
}
}
}
}
btVector3 DeltaC = (hull1.mTransform * hull1.mTransform.getOrigin()) - (hull0.mTransform * hull0.mTransform.getOrigin());
if((DeltaC.dot(sep))>0.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;
}