public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) { pa = new Vector3(); pb = new Vector3(); //just take fixed number of orientation, and sample the penetration depth in that direction float minProj = 1e30f; Vector3 minNorm = new Vector3(); Vector3 minA = new Vector3(), minB = new Vector3(); Vector3 seperatingAxisInA, seperatingAxisInB; Vector3 pInA, qInB, pWorld, qWorld, w; Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; int numSampleDirections = UnitSpherePointsCount; for (int i = 0; i < numSampleDirections; i++) { Vector3 norm = penetrationDirections[i]; seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); } { int numPDA = convexA.PreferredPenetrationDirectionsCount; if (numPDA != 0) { for (int i = 0; i < numPDA; i++) { Vector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); norm = Vector3.TransformNormal(norm, transformA); penetrationDirections[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); numSampleDirections++; } } } { int numPDB = convexB.PreferredPenetrationDirectionsCount; if (numPDB != 0) { for (int i = 0; i < numPDB; i++) { Vector3 norm; convexB.GetPreferredPenetrationDirection(i, out norm); norm = Vector3.TransformNormal(norm, transformB); penetrationDirections[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); numSampleDirections++; } } } convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); for (int i = 0; i < numSampleDirections; i++) { Vector3 norm = penetrationDirections[i]; seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; pWorld = MathHelper.MatrixToVector(transformA, pInA); qWorld = MathHelper.MatrixToVector(transformB, qInB); w = qWorld - pWorld; float delta = Vector3.Dot(norm, w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } //add the margins minA += minNorm * convexA.Margin; minB -= minNorm * convexB.Margin; //no penetration if (minProj < 0) return false; minProj += (convexA.Margin + convexB.Margin); GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); float offsetDist = minProj; Vector3 offset = minNorm * offsetDist; GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); Vector3 newOrg = transformA.Translation + offset; Matrix displacedTrans = transformA; displacedTrans.Translation = newOrg; input.TransformA = displacedTrans; input.TransformB = transformB; input.MaximumDistanceSquared = 1e30f;//minProj; IntermediateResult res = new IntermediateResult(); gjkdet.GetClosestPoints(input, res, debugDraw); float correctedMinNorm = minProj - res.Depth; //the penetration depth is over-estimated, relax it float penetration_relaxation = 1; minNorm *= penetration_relaxation; if (res.HasResult) { pa = res.PointInWorld - minNorm * correctedMinNorm; pb = res.PointInWorld; } return res.HasResult; }
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) { pa = new Vector3(); pb = new Vector3(); //just take fixed number of orientation, and sample the penetration depth in that direction float minProj = 1e30f; Vector3 minNorm = new Vector3(); Vector3 minA = new Vector3(), minB = new Vector3(); Vector3 seperatingAxisInA, seperatingAxisInB; Vector3 pInA, qInB, pWorld, qWorld, w; Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; int numSampleDirections = UnitSpherePointsCount; for (int i = 0; i < numSampleDirections; i++) { Vector3 norm = penetrationDirections[i]; seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); } { int numPDA = convexA.PreferredPenetrationDirectionsCount; if (numPDA != 0) { for (int i = 0; i < numPDA; i++) { Vector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); norm = Vector3.TransformNormal(norm, transformA); penetrationDirections[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); numSampleDirections++; } } } { int numPDB = convexB.PreferredPenetrationDirectionsCount; if (numPDB != 0) { for (int i = 0; i < numPDB; i++) { Vector3 norm; convexB.GetPreferredPenetrationDirection(i, out norm); norm = Vector3.TransformNormal(norm, transformB); penetrationDirections[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); numSampleDirections++; } } } convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); for (int i = 0; i < numSampleDirections; i++) { Vector3 norm = penetrationDirections[i]; seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; pWorld = MathHelper.MatrixToVector(transformA, pInA); qWorld = MathHelper.MatrixToVector(transformB, qInB); w = qWorld - pWorld; float delta = Vector3.Dot(norm, w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } //add the margins minA += minNorm * convexA.Margin; minB -= minNorm * convexB.Margin; //no penetration if (minProj < 0) { return(false); } minProj += (convexA.Margin + convexB.Margin); GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); float offsetDist = minProj; Vector3 offset = minNorm * offsetDist; GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); Vector3 newOrg = transformA.Translation + offset; Matrix displacedTrans = transformA; displacedTrans.Translation = newOrg; input.TransformA = displacedTrans; input.TransformB = transformB; input.MaximumDistanceSquared = 1e30f; //minProj; IntermediateResult res = new IntermediateResult(); gjkdet.GetClosestPoints(input, res, debugDraw); float correctedMinNorm = minProj - res.Depth; //the penetration depth is over-estimated, relax it float penetration_relaxation = 1; minNorm *= penetration_relaxation; if (res.HasResult) { pa = res.PointInWorld - minNorm * correctedMinNorm; pb = res.PointInWorld; } return(res.HasResult); }