void SimplexDemo::displayCallback()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0; i<numObjects; i++)
{
btTransform transA;
transA.setIdentity();
btVector3 dpos(0.f,5.f,0.f);
transA.setOrigin( dpos );
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
transA.setRotation(orn);
transA.getOpenGLMatrix( m );
/// draw the simplex
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
/// calculate closest point from simplex to the origin, and draw this vector
simplex.calcClosest(m);
}
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void btContinuousConvexCollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
/*for (i=0;i<numObjects;i++)
{
fromTrans[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
}
*/
if (getDebugMode()==btIDebugDraw::DBG_DrawAabb)
{
i=0;//for (i=1;i<numObjects;i++)
{
//for each object, subdivide the from/to transform in 10 equal steps
int numSubSteps = 10;
for (int s=0;s<10;s++)
{
btScalar subStep = s * 1.f/(float)numSubSteps;
btTransform interpolatedTrans;
btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans);
//fromTrans[i].getOpenGLMatrix(m);
//m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
//toTrans[i].getOpenGLMatrix(m);
//m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
interpolatedTrans.getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,0,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
}
}
btMatrix3x3 mat;
mat.setEulerZYX(yaw,pitch,roll);
btQuaternion orn;
mat.getRotation(orn);
orn.setEuler(yaw,pitch,roll);
fromTrans[1].setRotation(orn);
toTrans[1].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
// yaw += 0.01f;
}
// btVector3 fromA(-25,11,0);
// btVector3 toA(-15,11,0);
// btQuaternion ornFromA(0.f,0.f,0.f,1.f);
// btQuaternion ornToA(0.f,0.f,0.f,1.f);
// btTransform rayFromWorld(ornFromA,fromA);
// btTransform rayToWorld(ornToA,toA);
btTransform rayFromWorld = fromTrans[0];
btTransform rayToWorld = toTrans[0];
if (drawLine)
{
glBegin(GL_LINES);
glColor3f(0, 0, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
glEnd();
}
//now perform a raycast on the shapes, in local (shape) space
gGjkSimplexSolver.reset();
//choose one of the following lines
for (i=0;i<numObjects;i++)
{
fromTrans[i].getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
btDebugCastResult rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0],&m_shapeDrawer);
for (i=1;i<numObjects;i++)
{
btConvexCast::CastResult rayResult2;
btConvexCast::CastResult* rayResultPtr;
if (btIDebugDraw::DBG_DrawAabb)
{
rayResultPtr = &rayResult1;
} else
{
rayResultPtr = &rayResult2;
}
//GjkConvexCast convexCaster(&gGjkSimplexSolver);
//SubsimplexConvexCast convexCaster(&gGjkSimplexSolver);
//optional
btConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
gGjkSimplexSolver.reset();
if (convexCaster.calcTimeOfImpact(fromTrans[0],toTrans[0],fromTrans[i] ,toTrans[i] ,*rayResultPtr))
{
glDisable(GL_DEPTH_TEST);
btTransform hitTrans;
btTransformUtil::integrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[0],btVector3(0,1,0),getDebugMode(),worldBoundsMin,worldBoundsMax);
btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(0,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
}
glFlush();
glutSwapBuffers();
}
void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
GL_ShapeDrawer::drawCoordSystem();
static btScalar angle = 0.f;
angle+=getDeltaTimeMicroseconds()/1000000.0;
tr[1].setRotation(btQuaternion(btVector3(1,0,0),angle));
btTransform toA, toB;
toA = tr[0];
toA.setOrigin( btVector3( 0.0f, 0.f, 0.0f ) );
toB = tr[1];
toB.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );
gGjkSimplexSolver.reset();
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
//btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
btConvexCast::CastResult result;
result.m_hitPoint.setValue(0,0,0);
convexCaster.calcTimeOfImpact( tr[ 0 ], toA, tr[ 1 ], toB, result );
btScalar m1[16], m2[16],m3[16];
tr[ 0 ].getOpenGLMatrix( m1 );
tr[ 1 ].getOpenGLMatrix( m2 );
btSphereShape sphere(0.2);
btTransform tmp = tr[0];
tmp.setOrigin(result.m_hitPoint);
tmp.getOpenGLMatrix(m3);
m_shapeDrawer.drawOpenGL( m3, &sphere, btVector3( 1, 0, 1 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
btVector3 originA, originB;
originA.setInterpolate3( tr[ 0 ].getOrigin(), toA.getOrigin(), result.m_fraction );
originB.setInterpolate3( tr[ 1 ].getOrigin(), toB.getOrigin(), result.m_fraction );
btTransform A = tr[ 0 ];
A.setOrigin( originA );
btTransform B = tr[ 1 ];
B.setOrigin( originB );
A.getOpenGLMatrix( m1 );
B.getOpenGLMatrix( m2 );
m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
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();
}