virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = -1;
shapeInfo.m_triangleIndex = m_i;
if (r.m_localShapeInfo == NULL)
r.m_localShapeInfo = &shapeInfo;
const btScalar result = m_userCallback->addSingleResult(r, b);
m_closestHitFraction = m_userCallback->m_closestHitFraction;
return result;
}
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);
}
}
}
}
}