public override void Update(GameSettings settings, GameTime gameTime) { base.Update(settings, gameTime); DistanceInput input = new DistanceInput(); input.ProxyA.Set(_polygonA, 0); input.ProxyB.Set(_polygonB, 0); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; SimplexCache cache; cache.Count = 0; DistanceOutput output; Distance.ComputeDistance(out output, out cache, input); DebugView.DrawString(50, TextLine, "Distance = {0:n7}", output.Distance); TextLine += 15; DebugView.DrawString(50, TextLine, "Iterations = {0:n0}", output.Iterations); TextLine += 15; DebugView.BeginCustomDraw(); { Color color = new Color(0.9f, 0.9f, 0.9f); CCVector2[] v = new CCVector2[Settings.MaxPolygonVertices]; for (int i = 0; i < _polygonA.Vertices.Count; ++i) { v[i] = (CCVector2)MathUtils.Multiply(ref _transformA, _polygonA.Vertices[i]); } DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color); for (int i = 0; i < _polygonB.Vertices.Count; ++i) { v[i] = (CCVector2)MathUtils.Multiply(ref _transformB, _polygonB.Vertices[i]); } DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color); } Vector2 x1 = output.PointA; Vector2 x2 = output.PointB; DebugView.DrawPoint(x1, 0.5f, new Color(1.0f, 0.0f, 0.0f)); DebugView.DrawPoint(x2, 0.5f, new Color(1.0f, 0.0f, 0.0f)); DebugView.DrawSegment(x1, x2, new Color(1.0f, 1.0f, 0.0f)); DebugView.EndCustomDraw(); }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = new SimplexCache(); ++GJKCalls; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); // Get simplex vertices as an array. const int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3<int> saveA = new FixedArray3<int>(); FixedArray3<int> saveB = new FixedArray3<int>(); Vector2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.LengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop. int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = simplex.V[i].IndexA; saveB[i] = simplex.V[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. Vector2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = simplex.V[simplex.Count]; vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MultiplyT(ref input.TransformA.R, -d)); vertex.WA = MathUtils.Multiply(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MultiplyT(ref input.TransformB.R, d)); vertex.WB = MathUtils.Multiply(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); vertex.W = vertex.WB - vertex.WA; simplex.V[simplex.Count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; ++GJKIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } GJKMaxIters = Math.Max(GJKMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).Length(); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { float rA = input.ProxyA.Radius; float rB = input.ProxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vector2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. Vector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = new SimplexCache(); if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++GJKCalls; } // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3 <int> saveA = new FixedArray3 <int>(); FixedArray3 <int> saveB = new FixedArray3 <int>(); //float distanceSqr1 = Settings.MaxFloat; // Main iteration loop. int iter = 0; while (iter < Settings.MaxGJKIterations) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = simplex.V[i].IndexA; saveB[i] = simplex.V[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } //FPE: This code was not used anyway. // Compute closest point. //Vector2 p = simplex.GetClosestPoint(); //float distanceSqr2 = p.LengthSquared(); // Ensure progress //if (distanceSqr2 >= distanceSqr1) //{ //break; //} //distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = simplex.V[simplex.Count]; vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d)); vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d)); vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); vertex.W = vertex.WB - vertex.WA; simplex.V[simplex.Count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++GJKIters; } // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { GJKMaxIters = Math.Max(GJKMaxIters, iter); } // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).Length(); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { float rA = input.ProxyA.Radius; float rB = input.ProxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vector2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. Vector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function /// again. /// Note: use Distance() to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output">The output.</param> /// <param name="input">The input.</param> public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input) { if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOICalls; } output = new TOIOutput(); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float tMax = input.TMax; float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. DistanceInput distanceInput = new DistanceInput(); distanceInput.ProxyA = input.ProxyA; distanceInput.ProxyB = input.ProxyB; distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ;) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; DistanceOutput distanceOutput; SimplexCache cache; Distance.ComputeDistance(out distanceOutput, out cache, distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0.0f) { // Failure! output.State = TOIOutputState.Overlapped; output.T = 0.0f; break; } if (distanceOutput.Distance < target + tolerance) { // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } SeparationFunction.Set(ref cache, ref input.ProxyA, ref sweepA, ref input.ProxyB, ref sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of vertices. bool done = false; float t2 = tMax; int pushBackIter = 0; for (; ;) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output.State = TOIOutputState.Seperated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = SeparationFunction.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; float a1 = t1, a2 = t2; for (; ;) { // Use a mix of the secant rule and bisection. float t; if ((rootIterCount & 1) != 0) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } ++rootIterCount; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOIRootIters; } float s = SeparationFunction.Evaluate(indexA, indexB, t); if (Math.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } if (rootIterCount == 50) { break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); } ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOIIters; } if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { TOIMaxIters = Math.Max(TOIMaxIters, iter); } }
/// <summary> /// Test overlap between the two shapes. /// </summary> /// <param name="shapeA">The first shape.</param> /// <param name="indexA">The index for the first shape.</param> /// <param name="shapeB">The second shape.</param> /// <param name="indexB">The index for the second shape.</param> /// <param name="xfA">The transform for the first shape.</param> /// <param name="xfB">The transform for the seconds shape.</param> /// <returns></returns> public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA, ref Transform xfB) { _input = _input ?? new DistanceInput(); _input.ProxyA.Set(shapeA, indexA); _input.ProxyB.Set(shapeB, indexB); _input.TransformA = xfA; _input.TransformB = xfB; _input.UseRadii = true; SimplexCache cache; DistanceOutput output; Distance.ComputeDistance(out output, out cache, _input); return output.Distance < 10.0f * Settings.Epsilon; }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function /// again. /// Note: use Distance() to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output">The output.</param> /// <param name="input">The input.</param> public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) { if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled ++TOICalls; output = new TOIOutput(); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float tMax = input.TMax; float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. _distanceInput = _distanceInput ?? new DistanceInput(); _distanceInput.ProxyA = input.ProxyA; _distanceInput.ProxyB = input.ProxyB; _distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ; ) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. _distanceInput.TransformA = xfA; _distanceInput.TransformB = xfB; DistanceOutput distanceOutput; SimplexCache cache; Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0.0f) { // Failure! output.State = TOIOutputState.Overlapped; output.T = 0.0f; break; } if (distanceOutput.Distance < target + tolerance) { // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of vertices. bool done = false; float t2 = tMax; int pushBackIter = 0; for (; ; ) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output.State = TOIOutputState.Seperated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = SeparationFunction.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; float a1 = t1, a2 = t2; for (; ; ) { // Use a mix of the secant rule and bisection. float t; if ((rootIterCount & 1) != 0) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } ++rootIterCount; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled ++TOIRootIters; float s = SeparationFunction.Evaluate(indexA, indexB, t); if (Math.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } if (rootIterCount == 50) { break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled ++TOIIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled TOIMaxIters = Math.Max(TOIMaxIters, iter); }