void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
int shapeIndex = 1;
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
yaw += 0.01f;
}
SimdVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld;
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
//choose one of the following lines
for (i=1;i<numObjects;i++)
{
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
//BU_CollisionPair (algebraic version) is currently broken, will look into this
//BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]);
SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult;
if (convexCaster3.calcTimeOfImpact(rayFromWorld,rayToWorld,tr[i],tr[i],rayResult))
{
glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(hitPoint.x(),hitPoint.y(),hitPoint.z());
glEnd();
glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld;
toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint);
toTransWorld.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode());
}
}
glFlush();
glutSwapBuffers();
}
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
ConvexResultCallback& resultCallback, btScalar allowedPenetration)
{
if (collisionShape->isConvex())
{
//BT_PROFILE("convexSweepConvex");
btConvexCast::CastResult castResult;
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
btConvexShape* convexShape = (btConvexShape*) collisionShape;
btVoronoiSimplexSolver simplexSolver;
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast* castPtr = &convexCaster1;
if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.length2() > btScalar(0.0001))
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.normalize();
btCollisionWorld::LocalConvexResult localConvexResult
(
collisionObject,
0,
castResult.m_normal,
castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
} else {
if (collisionShape->isConcave())
{
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
//ConvexCast::CastResult
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
{
btCollisionWorld::ConvexResultCallback* m_resultCallback;
btCollisionObject* m_collisionObject;
btTriangleMeshShape* m_triangleMesh;
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if (hitFraction <= m_resultCallback->m_closestHitFraction)
{
btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitPointLocal,
hitFraction);
bool normalInWorldSpace = true;
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
}
return hitFraction;
}
};
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 boxMinLocal, boxMaxLocal;
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
} else
{
//BT_PROFILE("convexSweepConcave");
btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
//ConvexCast::CastResult
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
{
btCollisionWorld::ConvexResultCallback* m_resultCallback;
btCollisionObject* m_collisionObject;
btConcaveShape* m_triangleMesh;
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if (hitFraction <= m_resultCallback->m_closestHitFraction)
{
btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitPointLocal,
hitFraction);
bool normalInWorldSpace = false;
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
}
return hitFraction;
}
};
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 boxMinLocal, boxMaxLocal;
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin(convexToLocal);
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax(convexToLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
}
} else {
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape->isCompound())
{
BT_PROFILE("convexSweepCompound");
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
int i=0;
for (i=0;i<compoundShape->getNumChildShapes();i++)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
btTransform childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback, allowedPenetration);
// restore
collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
}
