void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
{
int i;
#ifdef DEBUG_PERSISTENCY
printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
trA.getOrigin().getX(),
trA.getOrigin().getY(),
trA.getOrigin().getZ(),
trB.getOrigin().getX(),
trB.getOrigin().getY(),
trB.getOrigin().getZ());
#endif //DEBUG_PERSISTENCY
/// first refresh worldspace positions and distance
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
manifoldPoint.m_lifeTime++;
}
/// then
btScalar distance2d;
btVector3 projectedDifference,projectedPoint;
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
} else
{
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
{
removeContactPoint(i);
} else
{
//contact point processed callback
if (gContactProcessedCallback)
(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
}
}
}
#ifdef DEBUG_PERSISTENCY
DebugPersistency();
#endif //
}
void SphereSphereCollisionAlgorithm::doProcessCollisionAlgorithm(const CollisionObjectWrapper &object1, const CollisionObjectWrapper &object2)
{
const CollisionSphereShape &sphere1 = static_cast<const CollisionSphereShape &>(object1.getShape());
const CollisionSphereShape &sphere2 = static_cast<const CollisionSphereShape &>(object2.getShape());
Vector3<float> diff = object2.getShapeWorldTransform().getPosition().vector(object1.getShapeWorldTransform().getPosition());
float length = diff.length();
float radius1 = sphere1.getRadius();
float radius2 = sphere2.getRadius();
if(length - getContactBreakingThreshold() < (radius1 + radius2))
{ //collision detected
//compute depth penetration
float depth = length - (radius1+radius2);
//compute normal (if both spheres have same origin: default normal is (1,0,0))
Vector3<float> normalFromObject2(1.0, 0.0, 0.0);
if(length > std::numeric_limits<float>::epsilon())
{ //normalize normal (from object2 to object1)
normalFromObject2 = diff / length;
}
//compute intersection point
Point3<float> pointOnObject2 = object2.getShapeWorldTransform().getPosition().translate(radius2 * normalFromObject2);
addNewContactPoint(normalFromObject2, pointOnObject2, depth);
}
}
int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
{
btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
int size = getNumContacts();
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
{
const btManifoldPoint &mp = m_pointCache[i];
btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
const btScalar distToManiPoint = diffA.dot(diffA);
if( distToManiPoint < shortestDist )
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return nearestPoint;
}
void ConvexHullConvexHullCollisionAlgorithm::doProcessCollisionAlgorithm(const CollisionObjectWrapper &object1, const CollisionObjectWrapper &object2)
{
//transform convex hull shapes
std::shared_ptr<CollisionConvexObject3D> convexObject1 = object1.getShape().toConvexObject(object1.getShapeWorldTransform());
std::shared_ptr<CollisionConvexObject3D> convexObject2 = object2.getShape().toConvexObject(object2.getShapeWorldTransform());
//process GJK and EPA hybrid algorithms
std::unique_ptr<GJKResult<float>> gjkResultWithoutMargin = gjkAlgorithm.processGJK(*convexObject1.get(), *convexObject2.get(), false);
if(gjkResultWithoutMargin->isValidResult())
{
if(gjkResultWithoutMargin->isCollide())
{ //collision detected on reduced objects (without margins)
std::unique_ptr<GJKResult<float>> gjkResultWithMargin = gjkAlgorithm.processGJK(*convexObject1.get(), *convexObject2.get(), true);
if(gjkResultWithMargin->isValidResult() && gjkResultWithMargin->isCollide())
{
std::unique_ptr<EPAResult<float>> epaResult = epaAlgorithm.processEPA(*convexObject1.get(), *convexObject2.get(), *gjkResultWithMargin.get());
if(epaResult->isValidResult() && epaResult->isCollide())
{ //should be always true except for problems due to float imprecision
const Vector3<float> &normalFromObject2 = -epaResult->getNormal();
const Point3<float> &pointOnObject2 = epaResult->getContactPointB();
const float penetrationDepth = -epaResult->getPenetrationDepth();
addNewContactPoint(normalFromObject2, pointOnObject2, penetrationDepth);
}
}
}else
{ //collision detected on enlarged objects (with margins) OR no collision detected
const Vector3<float> &vectorBA = gjkResultWithoutMargin->getClosestPointB().vector(gjkResultWithoutMargin->getClosestPointA());
float vectorBALength = vectorBA.length();
float sumMargins = convexObject1->getOuterMargin() + convexObject2->getOuterMargin();
if(sumMargins > vectorBALength - getContactBreakingThreshold())
{ //collision detected on enlarged objects
const Vector3<float> &normalFromObject2 = vectorBA.normalize();
const Point3<float> &pointOnObject2 = gjkResultWithoutMargin->getClosestPointB().translate(normalFromObject2 * convexObject2->getOuterMargin());
const float penetrationDepth = vectorBALength - sumMargins;
addNewContactPoint(normalFromObject2, pointOnObject2, penetrationDepth);
}
}
}
}