public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
{
if (m_convexB1 != null)
{
m_simplexSolver.Reset();
GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.GetClosestPoints(ref input, pointCollector, null);
}
else
{
//convex versus plane
ConvexShape convexShape = m_convexA;
StaticPlaneShape planeShape = m_planeShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = transA;
IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform;
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal;
pointCollector.AddContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance);
}
}
public override IndexedVector3 LocalGetSupportingVertex(ref IndexedVector3 vec)
{
return(m_childConvexShape.LocalGetSupportingVertex(ref vec));
}
