86c27a7c9d
Improved this penetration test with more verbose output retrieve worldtransform from motionstate when rigidbody gets motionstate assigned
771 lines
19 KiB
C++
771 lines
19 KiB
C++
|
|
///contribution by Pierre Terdiman to check penetration depth solvers
|
|
///see http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=638
|
|
|
|
#ifdef WIN32//for glut.h
|
|
#include <windows.h>
|
|
#endif
|
|
|
|
|
|
//think different
|
|
#if defined(__APPLE__) && !defined (VMDMESA)
|
|
#include <OpenGL/gl.h>
|
|
#include <OpenGL/glu.h>
|
|
#include <GLUT/glut.h>
|
|
#else
|
|
#include <GL/glut.h>
|
|
#endif
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#define VERBOSE_TEXT_ONSCREEN 1
|
|
#ifdef VERBOSE_TEXT_ONSCREEN
|
|
#include "BMF_Api.h"
|
|
#endif
|
|
|
|
#include "btBulletCollisionCommon.h"
|
|
|
|
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
|
|
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
|
|
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
|
|
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
|
|
#include "../Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h"
|
|
#include "../Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h"
|
|
#include "../Extras/EPA/EpaPenetrationDepthSolver.h"
|
|
|
|
|
|
|
|
static bool gRefMode = false;
|
|
static int gMethod = 0;
|
|
static int gLastUsedMethod = -1;
|
|
static int gNumGjkIterations = -1;
|
|
static int gLastDegenerateSimplex = -1;
|
|
|
|
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 int glutScreenHeight = 512;
|
|
static int glutScreenWidth = 512;
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
//See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho
|
|
static void setOrthographicProjection()
|
|
{
|
|
|
|
// switch to projection mode
|
|
glMatrixMode(GL_PROJECTION);
|
|
// save previous matrix which contains the
|
|
//settings for the perspective projection
|
|
glPushMatrix();
|
|
// reset matrix
|
|
glLoadIdentity();
|
|
// set a 2D orthographic projection
|
|
gluOrtho2D(0, glutScreenWidth, 0, glutScreenHeight);
|
|
// invert the y axis, down is positive
|
|
glScalef(1, -1, 1);
|
|
// mover the origin from the bottom left corner
|
|
// to the upper left corner
|
|
glTranslatef(0, -glutScreenHeight, 0);
|
|
glMatrixMode(GL_MODELVIEW);
|
|
}
|
|
|
|
static void resetPerspectiveProjection()
|
|
{
|
|
glMatrixMode(GL_PROJECTION);
|
|
glPopMatrix();
|
|
glMatrixMode(GL_MODELVIEW);
|
|
}
|
|
|
|
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 btMinkowskiPenetrationDepthSolver Solver0;
|
|
static Solid3EpaPenetrationDepth Solver1;
|
|
static EpaPenetrationDepthSolver Solver2;
|
|
|
|
|
|
|
|
btConvexPenetrationDepthSolver* Solver;
|
|
if(gMethod==0)
|
|
Solver = &Solver0;
|
|
else if(gMethod==1)
|
|
Solver = &Solver1;
|
|
else
|
|
Solver = &Solver2;
|
|
|
|
btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver);
|
|
GJK.m_catchDegeneracies = 1;
|
|
convexA.setMargin(0.01f);
|
|
convexB.setMargin(0.01f);
|
|
|
|
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
|
|
input.m_transformA = hull0.mTransform;
|
|
input.m_transformB = hull1.mTransform;
|
|
|
|
MyResult output;
|
|
GJK.getClosestPoints(input, output, 0);
|
|
gLastUsedMethod = GJK.m_lastUsedMethod;
|
|
gNumGjkIterations = GJK.m_curIter;
|
|
gLastDegenerateSimplex= GJK.m_degenerateSimplex;
|
|
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);
|
|
|
|
//clear previous frames result
|
|
gNormal.setValue(10,0,0);
|
|
gPoint.setValue(0,0,0);
|
|
gDepth = 999.999;
|
|
gLastUsedMethod = -1;
|
|
gNumGjkIterations = -1;
|
|
|
|
|
|
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());
|
|
|
|
#ifdef VERBOSE_TEXT_ONSCREEN
|
|
glColor3f(255.f, 255.f, 255.f);
|
|
|
|
setOrthographicProjection();
|
|
float xOffset = 10.f;
|
|
float yStart = 20.f;
|
|
float yIncr = 20.f;
|
|
char buf[124];
|
|
|
|
glRasterPos3f(xOffset,yStart,0);
|
|
sprintf(buf,"gDepth=%f: gNormal=(%f %f %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
|
|
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
|
yStart += yIncr;
|
|
|
|
glRasterPos3f(xOffset,yStart,0);
|
|
sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
|
|
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
|
yStart += yIncr;
|
|
|
|
|
|
|
|
|
|
|
|
if (gLastUsedMethod >= 3)
|
|
{
|
|
switch ( gMethod)
|
|
{
|
|
case 0:
|
|
sprintf(buf,"Minkowski sampling Penetration depth solver\n" );
|
|
break;
|
|
case 1:
|
|
sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
|
|
break;
|
|
case 2:
|
|
sprintf(buf,"EPA Penetration depth solver (WorkInProgress, zlib free\n" );
|
|
break;
|
|
default:
|
|
sprintf(buf,"Unknown Penetration Depth\n" );
|
|
}
|
|
glRasterPos3f(xOffset,yStart,0);
|
|
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
|
yStart += yIncr;
|
|
|
|
} else
|
|
{
|
|
glRasterPos3f(xOffset,yStart,0);
|
|
sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
|
|
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
|
yStart += yIncr;
|
|
}
|
|
|
|
if (gLastDegenerateSimplex)
|
|
{
|
|
glRasterPos3f(xOffset,yStart,0);
|
|
sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
|
|
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
|
yStart += yIncr;
|
|
}
|
|
|
|
|
|
|
|
|
|
resetPerspectiveProjection();
|
|
#endif //VERBOSE_TEXT_ONSCREEN
|
|
|
|
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(glutScreenWidth, glutScreenHeight);
|
|
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("convex0.bin");
|
|
if(!Status)
|
|
{
|
|
Status = gConvex0.LoadFromFile("../../convex0.bin");
|
|
if(!Status)
|
|
{
|
|
printf("Failed to load object!\n");
|
|
exit(0);
|
|
}
|
|
}
|
|
Status = gConvex1.LoadFromFile("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;
|
|
}
|