float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
{
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the ConvexCast)
CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject;
CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
//todo: only do if the motion exceeds the 'radius'
struct LocalTriangleRaycastCallback : public TriangleRaycastCallback
{
LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
:TriangleRaycastCallback(from,to)
{
}
virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
{
//todo: handle ccd here
return 0.f;
}
};
LocalTriangleRaycastCallback raycastCallback(from,to);
raycastCallback.m_hitFraction = convexbody->m_hitFraction;
SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f);
SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
if (triBody->m_collisionShape->IsConcave())
{
CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject;
ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
if (triangleMesh)
{
triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
}
}
if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
{
convexbody->m_hitFraction = raycastCallback.m_hitFraction;
return raycastCallback.m_hitFraction;
}
return 1.f;
}
btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
btCollisionObject* triBody = m_isSwapped ? body0 : body1;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
{
return btScalar(1.);
}
//const btVector3& from = convexbody->m_worldTransform.getOrigin();
//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
//todo: only do if the motion exceeds the 'radius'
btTransform triInv = triBody->getWorldTransform().inverse();
btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
struct LocalTriangleSphereCastCallback : public btTriangleCallback
{
btTransform m_ccdSphereFromTrans;
btTransform m_ccdSphereToTrans;
btTransform m_meshTransform;
btScalar m_ccdSphereRadius;
btScalar m_hitFraction;
LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
:m_ccdSphereFromTrans(from),
m_ccdSphereToTrans(to),
m_ccdSphereRadius(ccdSphereRadius),
m_hitFraction(hitFraction)
{
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
(void)partId;
(void)triangleIndex;
//do a swept sphere for now
btTransform ident;
ident.setIdentity();
btConvexCast::CastResult castResult;
castResult.m_fraction = m_hitFraction;
btSphereShape pointShape(m_ccdSphereRadius);
btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
btVoronoiSimplexSolver simplexSolver;
btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
//local space?
if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
ident,ident,castResult))
{
if (m_hitFraction > castResult.m_fraction)
m_hitFraction = castResult.m_fraction;
}
}
};
if (triBody->getCollisionShape()->isConcave())
{
btVector3 rayAabbMin = convexFromLocal.getOrigin();
rayAabbMin.setMin(convexToLocal.getOrigin());
btVector3 rayAabbMax = convexFromLocal.getOrigin();
rayAabbMax.setMax(convexToLocal.getOrigin());
btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
btScalar curHitFraction = btScalar(1.); //is this available?
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
convexbody->getCcdSweptSphereRadius(),curHitFraction);
raycastCallback.m_hitFraction = convexbody->getHitFraction();
btCollisionObject* concavebody = triBody;
btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
if (triangleMesh)
{
triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
{
convexbody->setHitFraction( raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction;
}
}
return btScalar(1.);
}
