/// <summary> /// cast a convex against another convex object /// </summary> /// <param name="fromA"></param> /// <param name="toA"></param> /// <param name="fromB"></param> /// <param name="toB"></param> /// <param name="result"></param> /// <returns></returns> public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) { MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; Matrix transformA = fromA; Matrix transformB = fromB; transformA.Translation = new Vector3(0, 0, 0); transformB.Translation = new Vector3(0, 0, 0); combined.TransformA = transformA; combined.TransformB = transformB; float radius = 0.01f; float lambda = 0; Vector3 s = rayFromLocalA.Translation; Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; Vector3 x = s; Vector3 n = new Vector3(); Vector3 c = new Vector3(); bool hasResult = false; float lastLambda = lambda; IConvexPenetrationDepthSolver penSolver = null; Matrix identityTransform = Matrix.Identity; SphereShape raySphere = new SphereShape(0.0f); raySphere.Margin = 0.0f; Matrix sphereTransform = Matrix.Identity; sphereTransform.Translation = rayFromLocalA.Translation; result.DrawCoordSystem(sphereTransform); { PointCollector pointCollector = new PointCollector(); GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.TransformA = sphereTransform; input.TransformB = identityTransform; gjk.GetClosestPoints(input, pointCollector, null); hasResult = pointCollector.HasResult; c = pointCollector.PointInWorld; n = pointCollector.NormalOnBInWorld; } if (hasResult) { float dist = (c - x).Length(); if (dist < radius) { lastLambda = 1.0f; } while (dist > radius) { n = x - c; float dot = Vector3.Dot(n, r); if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) { return(false); } lambda = lambda - Vector3.Distance(n, n) / dot; if (lambda <= lastLambda) { break; } lastLambda = lambda; x = s + lambda * r; sphereTransform.Translation = x; result.DrawCoordSystem(sphereTransform); PointCollector pointCollector = new PointCollector(); GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.TransformA = sphereTransform; input.TransformB = identityTransform; gjk.GetClosestPoints(input, pointCollector, null); if (pointCollector.HasResult) { if (pointCollector.Distance < 0.0f) { result.Fraction = lastLambda; result.Normal = n; return(true); } c = pointCollector.PointInWorld; dist = (c - x).Length(); } else { return(false); } } if (lastLambda < 1.0f) { result.Fraction = lastLambda; result.Normal = n; return(true); } } return(false); }
/// <summary> /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. /// </summary> /// <param name="fromA"></param> /// <param name="toA"></param> /// <param name="fromB"></param> /// <param name="toB"></param> /// <param name="result"></param> /// <returns></returns> public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) { MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); Matrix rayFromLocalA; Matrix rayToLocalA; rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; rayToLocalA = MathHelper.InvertMatrix(toA) * toB; _simplexSolver.Reset(); convex.TransformB = rayFromLocalA; float lambda = 0; //todo: need to verify this: //because of minkowski difference, we need the inverse direction Vector3 s = -rayFromLocalA.Translation; Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); Vector3 x = s; Vector3 v; Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); v = x - arbitraryPoint; int maxIter = MaxIterations; Vector3 n = new Vector3(); float lastLambda = lambda; float dist2 = v.LengthSquared(); float epsilon = 0.0001f; Vector3 w, p; float VdotR; while ((dist2 > epsilon) && (maxIter-- != 0)) { p = convex.LocalGetSupportingVertex(v); w = x - p; float VdotW = Vector3.Dot(v, w); if (VdotW > 0) { VdotR = Vector3.Dot(v, r); if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; else { lambda = lambda - VdotW / VdotR; x = s + lambda * r; _simplexSolver.Reset(); //check next line w = x - p; lastLambda = lambda; n = v; } } _simplexSolver.AddVertex(w, x, p); if (_simplexSolver.Closest(out v)) { dist2 = v.LengthSquared(); } else { dist2 = 0f; } } result.Fraction = lambda; result.Normal = n; return true; }
/// <summary> /// cast a convex against another convex object /// </summary> /// <param name="fromA"></param> /// <param name="toA"></param> /// <param name="fromB"></param> /// <param name="toB"></param> /// <param name="result"></param> /// <returns></returns> public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) { MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; Matrix transformA = fromA; Matrix transformB = fromB; transformA.Translation = new Vector3(0, 0, 0); transformB.Translation = new Vector3(0, 0, 0); combined.TransformA = transformA; combined.TransformB = transformB; float radius = 0.01f; float lambda = 0; Vector3 s = rayFromLocalA.Translation; Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; Vector3 x = s; Vector3 n = new Vector3(); Vector3 c = new Vector3(); bool hasResult = false; float lastLambda = lambda; IConvexPenetrationDepthSolver penSolver = null; Matrix identityTransform = Matrix.Identity; SphereShape raySphere = new SphereShape(0.0f); raySphere.Margin=0.0f; Matrix sphereTransform = Matrix.Identity; sphereTransform.Translation = rayFromLocalA.Translation; result.DrawCoordSystem(sphereTransform); { PointCollector pointCollector = new PointCollector(); GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.TransformA = sphereTransform; input.TransformB = identityTransform; gjk.GetClosestPoints(input, pointCollector, null); hasResult = pointCollector.HasResult; c = pointCollector.PointInWorld; n = pointCollector.NormalOnBInWorld; } if (hasResult) { float dist = (c - x).Length(); if (dist < radius) { lastLambda = 1.0f; } while (dist > radius) { n = x - c; float dot = Vector3.Dot(n, r); if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; lambda = lambda - Vector3.Distance(n, n) / dot; if (lambda <= lastLambda) break; lastLambda = lambda; x = s + lambda * r; sphereTransform.Translation = x; result.DrawCoordSystem(sphereTransform); PointCollector pointCollector = new PointCollector(); GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.TransformA = sphereTransform; input.TransformB = identityTransform; gjk.GetClosestPoints(input, pointCollector, null); if (pointCollector.HasResult) { if (pointCollector.Distance < 0.0f) { result.Fraction = lastLambda; result.Normal = n; return true; } c = pointCollector.PointInWorld; dist = (c - x).Length(); } else { return false; } } if (lastLambda < 1.0f) { result.Fraction = lastLambda; result.Normal = n; return true; } } return false; }
/// <summary> /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. /// </summary> /// <param name="fromA"></param> /// <param name="toA"></param> /// <param name="fromB"></param> /// <param name="toB"></param> /// <param name="result"></param> /// <returns></returns> public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) { MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); Matrix rayFromLocalA; Matrix rayToLocalA; rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; rayToLocalA = MathHelper.InvertMatrix(toA) * toB; _simplexSolver.Reset(); convex.TransformB = rayFromLocalA; float lambda = 0; //todo: need to verify this: //because of minkowski difference, we need the inverse direction Vector3 s = -rayFromLocalA.Translation; Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); Vector3 x = s; Vector3 v; Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); v = x - arbitraryPoint; int maxIter = MaxIterations; Vector3 n = new Vector3(); float lastLambda = lambda; float dist2 = v.LengthSquared(); float epsilon = 0.0001f; Vector3 w, p; float VdotR; while ((dist2 > epsilon) && (maxIter-- != 0)) { p = convex.LocalGetSupportingVertex(v); w = x - p; float VdotW = Vector3.Dot(v, w); if (VdotW > 0) { VdotR = Vector3.Dot(v, r); if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) { return(false); } else { lambda = lambda - VdotW / VdotR; x = s + lambda * r; _simplexSolver.Reset(); //check next line w = x - p; lastLambda = lambda; n = v; } } _simplexSolver.AddVertex(w, x, p); if (_simplexSolver.Closest(out v)) { dist2 = v.LengthSquared(); } else { dist2 = 0f; } } result.Fraction = lambda; result.Normal = n; return(true); }