void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
{
BT_PROFILE("convexSweepTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
btTransform convexFromTrans,convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btVector3 zeroLinVel;
zeroLinVel.setValue(0,0,0);
btTransform R;
R.setIdentity ();
R.setRotation (convexFromTrans.getRotation());
castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
}
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
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);
}
}
}
}
}
virtual bool needsCollision(btBroadphaseProxy* p) const
{
return m_userCallback->needsCollision(p);
}