void SimplexDemo::initPhysics()
{
simplex.setSimplexSolver(&simplexSolver);
simplex.addVertex(btVector3(-2,0,-2));
simplex.addVertex(btVector3(2,0,-2));
simplex.addVertex(btVector3(0,0,2));
simplex.addVertex(btVector3(0,2,0));
shapePtr[0] = &simplex;
btTransform tr;
tr.setIdentity();
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
float m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
ATTRIBUTE_ALIGNED16(btScalar) m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
gContactBreakingThreshold=1e10f;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
if (debugMode)
{
/// for polyhedral shapes
if (shapePtr[i]->isPolyhedral())
{
if (!shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = new btConvexHullComputer();
shapePtr[i]->setUserPointer(convexUtil);
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shapePtr[i];
btAlignedObjectArray<btVector3> vertices;
vertices.resize(polyshape->getNumVertices());
for (int i=0;i<polyshape->getNumVertices();i++)
{
polyshape->getVertex(i,vertices[i]);
}
bool useDoublePrecision = false;
convexUtil->compute(&vertices[0].getX(),sizeof(btVector3), polyshape->getNumVertices(),0,0);
}
if (shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = (btConvexHullComputer*)shapePtr[i]->getUserPointer();
//printf("num faces = %d\n",convexUtil->faces.size());
for (int j=0;j<convexUtil->faces.size();j++)
{
int face = convexUtil->faces[j];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
do
{
int src = edge->getSourceVertex();
int targ = edge->getTargetVertex();
//printf("src=%d target = %d\n", src,targ);
btVector3 wa = tr[i] * convexUtil->vertices[src];
btVector3 wb = tr[i] * convexUtil->vertices[targ];
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3f(wa.getX(),wa.getY(),wa.getZ());
glVertex3f(wb.getX(),wb.getY(),wb.getZ());
glEnd();
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
}
}
}
} else
{
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void CollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
btVoronoiSimplexSolver sGjkSimplexSolver;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
//VECCOPY(pa, gjkOutput.m_pointInWorld);
//VECCOPY(pb, gjkOutput.m_pointInWorld);
//VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance);
printf("bullet: %10.10f\n", gjkOutput.m_distance); // = 0.24 => that's absolutely wrong!
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
// btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
//let it rotate
//tr[0].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}