void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)dispatchInfo;
(void)resultOut;
if (!m_manifoldPtr)
return;
btCollisionObject* convexObj = m_isSwapped? body1 : body0;
btCollisionObject* planeObj = m_isSwapped? body0: body1;
btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform planeInConvex;
planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
}
if (m_ownManifold)
{
if (m_manifoldPtr->getNumContacts())
{
resultOut->refreshContactPoints();
}
}
}
void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
{
if (m_convexB1)
{
m_simplexSolver->reset();
btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints(input,pointCollector,0);
} else
{
//convex versus plane
const btConvexShape* convexShape = m_convexA;
const btStaticPlaneShape* planeShape = m_planeShape;
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
convexInPlaneTrans= transB.inverse() * convexWorldTransform;
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * transB;
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
pointCollector.addContactPoint(
normalOnSurfaceB,
vtxInPlaneWorld,
distance);
}
}
void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
}
}
void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)dispatchInfo;
if (!m_manifoldPtr)
return;
const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform planeInConvex;
planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
}
//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
//they keep on rolling forever because of the additional off-center contact points
//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
{
btVector3 v0,v1;
btPlaneSpace1(planeNormal,v0,v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
const btScalar angleLimit = 0.125f * SIMD_PI;
btScalar perturbeAngle;
btScalar radius = convexShape->getAngularMotionDisc();
perturbeAngle = gContactBreakingThreshold / radius;
if ( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btQuaternion perturbeRot(v0,perturbeAngle);
for (int i=0;i<m_numPerturbationIterations;i++)
{
btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
btQuaternion rotq(planeNormal,iterationAngle);
collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr->getNumContacts())
{
resultOut->refreshContactPoints();
}
}
}
