public void Set(SimplexCache sc) { Array.Copy(sc.IndexA, 0, IndexA, 0, IndexA.Length); Array.Copy(sc.IndexB, 0, IndexB, 0, IndexB.Length); Metric = sc.Metric; Count = sc.Count; }
public static void ReadCache( out Simplex simplex, SimplexCache cache, WorldTransform xfA, DistanceProxy proxyA, WorldTransform xfB, DistanceProxy proxyB) { System.Diagnostics.Debug.Assert(cache.Count <= 3); // Copy data from cache. simplex = new Simplex { Count = cache.Count }; for (var i = 0; i < simplex.Count; ++i) { SimplexVertex v; v.IndexA = cache.IndexA[i]; v.IndexB = cache.IndexB[i]; v.VertexA = xfA.ToGlobal(proxyA.Vertices[v.IndexA]); v.VertexB = xfB.ToGlobal(proxyB.Vertices[v.IndexB]); // ReSharper disable RedundantCast Necessary for FarPhysics. v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA); // ReSharper restore RedundantCast v.Alpha = 0.0f; simplex.Vertices[i] = v; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (simplex.Count > 1) { var metric1 = cache.Metric; var metric2 = simplex.GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon) { // Reset the simplex, triggers computation below. simplex.Count = 0; } } // If the cache is empty or invalid ... if (simplex.Count == 0) { SimplexVertex v; v.IndexA = 0; v.IndexB = 0; v.VertexA = xfA.ToGlobal(proxyA.Vertices[0]); v.VertexB = xfB.ToGlobal(proxyB.Vertices[0]); // ReSharper disable RedundantCast Necessary for FarPhysics. v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA); // ReSharper restore RedundantCast v.Alpha = 1.0f; simplex.Vertices.Item1 = v; simplex.Count = 1; } }
public void WriteCache(ref SimplexCache cache) { cache.Metric = GetMetric(); cache.Count = (ushort)Count; for (var i = 0; i < Count; ++i) { cache.IndexA[i] = (byte)Vertices[i].IndexA; cache.IndexB[i] = (byte)Vertices[i].IndexB; } }
public void WriteCache(SimplexCache cache) { cache.Metric = Metric; cache.Count = Count; for (int i = 0; i < Count; ++i) { cache.IndexA[i] = (Vertices[i].IndexA); cache.IndexB[i] = (Vertices[i].IndexB); } }
/// <summary>Tests whether two shapes overlap, using their distance proxies.</summary> /// <param name="proxyA">The proxy for the first shape.</param> /// <param name="proxyB">The proxy for the second shape.</param> /// <param name="transformA">The transform of the first shape.</param> /// <param name="transformB">The transform of the second shape.</param> /// <returns></returns> internal static bool TestOverlap( DistanceProxy proxyA, DistanceProxy proxyB, WorldTransform transformA, WorldTransform transformB) { var cache = new SimplexCache { Count = 0 }; return(Distance(ref cache, proxyA, transformA, proxyB, transformB, true) < 10 * Settings.Epsilon); }
public override void Update(GameSettings settings, GameTime gameTime) { base.Update(settings, gameTime); DistanceInput input = new DistanceInput(); input.ProxyA = new DistanceProxy(_polygonA, 0); input.ProxyB = new DistanceProxy(_polygonB, 0); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; SimplexCache cache = new SimplexCache(); cache.Count = 0; DistanceOutput output; DistanceGJK.ComputeDistance(ref input, out output, out cache); DrawString($"distance = {output.Distance}"); DrawString($"iterations = {output.Iterations}"); DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View); { Color color = new Color(0.9f, 0.9f, 0.9f); Vector2[] v = new Vector2[Settings.MaxPolygonVertices]; for (int i = 0; i < _polygonA.Vertices.Count; ++i) { v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]); } DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color); for (int i = 0; i < _polygonB.Vertices.Count; ++i) { v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]); } DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color); } Vector2 x1 = output.PointA; Vector2 x2 = output.PointB; Color c1 = new Color(1.0f, 0.0f, 0.0f); DebugView.DrawPoint(x1, 4.0f, c1); Color c2 = new Color(1.0f, 1.0f, 0.0f); DebugView.DrawPoint(x2, 4.0f, c2); DebugView.EndCustomDraw(); }
public void Step(TestSettings settings) { base.Step(settings); DistanceInput input = new DistanceInput(); input.proxyA.Set(m_polygonA, 0); input.proxyB.Set(m_polygonB, 0); input.transformA = m_transformA; input.transformB = m_transformB; input.useRadii = true; SimplexCache cache = new SimplexCache(); cache.count = 0; DistanceOutput output; Utilities.Distance(out output, cache, input); m_debugDraw.DrawString("distance = %g", output.distance); m_debugDraw.DrawString("iterations = %d", output.iterations); { Color color = Color.FromArgb(225, 225, 225); Vec2[] v = new Vec2[Settings._maxPolygonVertices]; for (int i = 0; i < m_polygonA.m_count; ++i) { v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]); } m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color); for (int i = 0; i < m_polygonB.m_count; ++i) { v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]); } m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color); } Vec2 x1 = output.pointA; Vec2 x2 = output.pointB; Color c1 = Color.FromArgb(255, 0, 0); m_debugDraw.DrawPoint(x1, 4.0f, c1); Color c2 = Color.FromArgb(255, 255, 0); m_debugDraw.DrawPoint(x2, 4.0f, c2); }
public override void Step(Settings settings) { base.Step(settings); DistanceInput input = new DistanceInput(); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; SimplexCache cache = new SimplexCache(); cache.Count = 0; DistanceOutput output; Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB); StringBuilder strBld = new StringBuilder(); strBld.AppendFormat("distance = {0}", new object[] { output.Distance }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; strBld = new StringBuilder(); strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); int i; for (i = 0; i < _polygonA.VertexCount; ++i) { _dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color); for (i = 0; i < _polygonB.VertexCount; ++i) { _dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color); } Vec2 x1 = output.PointA; Vec2 x2 = output.PointB; OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0)); OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0)); OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0)); }
public override void Step(Framework.Settings settings) { base.Step(settings); 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 = new SimplexCache(); cache.count = 0; DistanceOutput output = new DistanceOutput(); Distance.ComputeDistance(out output, out cache, ref input); _debugDraw.DrawString(50, _textLine, "distance = {0:n}", output.distance); _textLine += 15; _debugDraw.DrawString(50, _textLine, "iterations = {0:n}", output.iterations); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); FixedArray8 <Vector2> v = new FixedArray8 <Vector2>(); for (int i = 0; i < _polygonA._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color); for (int i = 0; i < _polygonB._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color); } Vector2 x1 = output.pointA; Vector2 x2 = output.pointB; _debugDraw.DrawPoint(x1, 0.5f, new Color(1.0f, 0.0f, 0.0f)); _debugDraw.DrawPoint(x2, 0.5f, new Color(1.0f, 0.0f, 0.0f)); _debugDraw.DrawSegment(x1, x2, new Color(1.0f, 1.0f, 0.0f)); }
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { Debug.Assert(cache.Count <= 3); // Copy data from cache. Count = cache.Count; for (int i = 0; i < Count; ++i) { SimplexVertex v = Vertices[i]; v.IndexA = cache.IndexA[i]; v.IndexB = cache.IndexB[i]; Vec2 wALocal = proxyA.GetVertex(v.IndexA); Vec2 wBLocal = proxyB.GetVertex(v.IndexB); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); v.A = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (Count > 1) { float metric1 = cache.Metric; float metric2 = Metric; if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { // Reset the simplex. Count = 0; } } // If the cache is empty or invalid ... if (Count == 0) { SimplexVertex v = Vertices[0]; v.IndexA = 0; v.IndexB = 0; Vec2 wALocal = proxyA.GetVertex(0); Vec2 wBLocal = proxyB.GetVertex(0); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); Count = 1; } }
public override void OnRender() { var input = new DistanceInput(); input.ProxyA.Set(_polygonA, 0); input.ProxyB.Set(_polygonB, 0); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; var cache = new SimplexCache(); DistanceAlgorithm.Distance(out var output, ref cache, input); DrawString($"distance = {output.Distance}"); DrawString($"iterations = {output.Iterations}"); { var color = Color.FromArgb(230, 230, 230); var v = new Vector2[Settings.MaxPolygonVertices]; for (var i = 0; i < _polygonA.Count; ++i) { v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]); } Drawer.DrawPolygon(v, _polygonA.Count, color); for (var i = 0; i < _polygonB.Count; ++i) { v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]); } Drawer.DrawPolygon(v, _polygonB.Count, color); } var x1 = output.PointA; var x2 = output.PointB; var c1 = Color.FromArgb(255, 0, 0); Drawer.DrawPoint(x1, 4.0f, c1); var c2 = Color.FromArgb(255, 255, 0); Drawer.DrawPoint(x2, 4.0f, c2); }
private float distanceWithPhysicObj(PhysicObj obj) { if (obj == null) { return(-1); } Fixture proxyAfix = owner.getBoundsFixture(); Fixture proxyBfix = obj.getBoundsFixture(); if (proxyAfix == null || proxyBfix == null) { return(-1); } DistanceProxy proxyA = new DistanceProxy(); proxyA.Set(proxyAfix.Shape, 0); DistanceProxy proxyB = new DistanceProxy(); proxyB.Set(proxyBfix.Shape, 1); DistanceInput distInput = new DistanceInput(); distInput.ProxyA = proxyA; distInput.ProxyB = proxyB; Transform transformA; owner.body.GetTransform(out transformA); Transform transformB; obj.body.GetTransform(out transformB); distInput.TransformA = transformA; distInput.TransformB = transformB; DistanceOutput distout = new DistanceOutput(); SimplexCache simplexCache = new SimplexCache(); Distance.ComputeDistance(out distout, out simplexCache, distInput); return(distout.Distance); }
public static void Initialize( out SeparationFunction f, SimplexCache cache, DistanceProxy proxyA, DistanceProxy proxyB, Sweep sweepA, Sweep sweepB, float t1) { System.Diagnostics.Debug.Assert(0 < cache.Count && cache.Count < 3); f._proxyA = proxyA; f._proxyB = proxyB; f._sweepA = sweepA; f._sweepB = sweepB; WorldTransform xfA, xfB; f._sweepA.GetTransform(out xfA, t1); f._sweepB.GetTransform(out xfB, t1); if (cache.Count == 1) { f._type = Type.Points; var localPointA = f._proxyA.Vertices[cache.IndexA.Item1]; var localPointB = f._proxyB.Vertices[cache.IndexB.Item1]; var pointA = xfA.ToGlobal(localPointA); var pointB = xfB.ToGlobal(localPointB); // ReSharper disable RedundantCast Necessary for FarPhysics. f._axis = (Vector2)(pointB - pointA); // ReSharper restore RedundantCast f._axis.Normalize(); f._localPoint = LocalPoint.Zero; } else if (cache.IndexA.Item1 == cache.IndexA.Item2) { // Two points on B and one on A. f._type = Type.FaceB; var localPointB1 = proxyB.Vertices[cache.IndexB.Item1]; var localPointB2 = proxyB.Vertices[cache.IndexB.Item2]; f._axis = Vector2Util.Cross(localPointB2 - localPointB1, 1.0f); f._axis.Normalize(); var normal = xfB.Rotation * f._axis; f._localPoint = 0.5f * (localPointB1 + localPointB2); var pointB = xfB.ToGlobal(f._localPoint); var localPointA = proxyA.Vertices[cache.IndexA[0]]; var pointA = xfA.ToGlobal(localPointA); // ReSharper disable RedundantCast Necessary for FarPhysics. var s = Vector2Util.Dot((Vector2)(pointA - pointB), normal); // ReSharper restore RedundantCast if (s < 0.0f) { f._axis = -f._axis; } } else { // Two points on A and one or two points on B. f._type = Type.FaceA; var localPointA1 = f._proxyA.Vertices[cache.IndexA.Item1]; var localPointA2 = f._proxyA.Vertices[cache.IndexA.Item2]; f._axis = Vector2Util.Cross(localPointA2 - localPointA1, 1.0f); f._axis.Normalize(); var normal = xfA.Rotation * f._axis; f._localPoint = 0.5f * (localPointA1 + localPointA2); var pointA = xfA.ToGlobal(f._localPoint); var localPointB = f._proxyB.Vertices[cache.IndexB[0]]; var pointB = xfB.ToGlobal(localPointB); // ReSharper disable RedundantCast Necessary for FarPhysics. var s = Vector2Util.Dot((Vector2)(pointB - pointA), normal); // ReSharper restore RedundantCast if (s < 0.0f) { f._axis = -f._axis; } } }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static bool TimeOfImpact( DistanceProxy proxyA, DistanceProxy proxyB, Sweep sweepA, Sweep sweepB, float tMax, out float t) { // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); var totalRadius = proxyA.Radius + proxyB.Radius; var target = System.Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; System.Diagnostics.Debug.Assert(target > tolerance); // Prepare input for distance query. var cache = new SimplexCache { Count = 0 }; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). var t1 = 0.0f; for (var iter = 0; iter < 20; ++iter) { WorldTransform 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. var distance = Distance(ref cache, proxyA, xfA, proxyB, xfB); // If the shapes are overlapped, we give up on continuous collision. if (distance <= 0.0f) { // Failure! t = 0.0f; return(false); } if (distance < target + tolerance) { // Victory! t = t1; return(true); } // Initialize the separating axis. SeparationFunction fcn; SeparationFunction.Initialize(out fcn, cache, proxyA, proxyB, sweepA, 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. var t2 = tMax; for (var pushBackIter = 0; pushBackIter < Settings.MaxPolygonVertices; ++pushBackIter) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; var s2 = fcn.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! t = tMax; return(false); } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. var s1 = fcn.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { t = t1; return(false); } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). t = t1; return(true); } // Compute 1D root of: f(x) - target = 0 float a1 = t1, a2 = t2; for (var rootIterCount = 0; rootIterCount < 50; ++rootIterCount) { // Use a mix of the secant rule and bisection. float u; if ((rootIterCount & 1) != 0) { // Secant rule to improve convergence. u = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. u = 0.5f * (a1 + a2); } var s = fcn.Evaluate(indexA, indexB, u); if (System.Math.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = u; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = u; s1 = s; } else { a2 = u; s2 = s; } } } } // Root finder got stuck. Semi-victory. t = t1; return(false); }
/// <summary> /// Compute the closest points between two shapes. Supports any combination of: CircleShape and /// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to /// zero. /// </summary> /// <param name="output"></param> /// <param name="cache"></param> /// <param name="input"></param> public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input) { GJK_CALLS++; DistanceProxy proxyA = input.ProxyA; DistanceProxy proxyB = input.ProxyB; Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.Vertices; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. // (pooled above) simplex.GetClosestPoint(closestPoint); float distanceSqr1 = closestPoint.LengthSquared(); // Main iteration loop int iter = 0; while (iter < GJK_MAX_ITERS) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; i++) { saveA[i] = vertices[i].IndexA; saveB[i] = vertices[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. simplex.GetClosestPoint(closestPoint); float distanceSqr2 = closestPoint.LengthSquared(); // ensure progress if (distanceSqr2 >= distanceSqr1) { // break; } distanceSqr1 = distanceSqr2; // get search direction; simplex.GetSearchDirection(d); // 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; } /* * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; */ // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = vertices[simplex.Count]; Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp); vertex.IndexA = proxyA.GetSupport(temp); Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA); // Vec2 wBLocal; Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp); vertex.IndexB = proxyB.GetSupport(temp); Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB); vertex.W.Set(vertex.WB).SubLocal(vertex.WA); // Iteration count is equated to the number of support point calls. ++iter; ++GJK_ITERS; // 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; } GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter); // Prepare output. simplex.GetWitnessPoints(output.PointA, output.PointB); output.Distance = MathUtils.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.UseRadii) { float rA = proxyA.Radius; float rB = 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); normal.Set(output.PointB).SubLocal(output.PointA); normal.Normalize(); temp.Set(normal).MulLocal(rA); output.PointA.AddLocal(temp); temp.Set(normal).MulLocal(rB); output.PointB.SubLocal(temp); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. // Vec2 p = 0.5f * (output.pointA + output.pointB); output.PointA.AddLocal(output.PointB).MulLocal(.5f); output.PointB.Set(output.PointA); output.Distance = 0.0f; } } }
public override void Step(Framework.Settings settings) { base.Step(settings); 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 = new SimplexCache(); cache.count = 0; DistanceOutput output = new DistanceOutput(); Distance.ComputeDistance( out output, out cache, ref input); _debugDraw.DrawString(50, _textLine, "distance = {0:n}", output.distance); _textLine += 15; _debugDraw.DrawString(50, _textLine, "iterations = {0:n}", output.iterations); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); FixedArray8<Vector2> v = new FixedArray8<Vector2>(); for (int i = 0; i < _polygonA._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformA, _polygonA._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonA._vertexCount, color); for (int i = 0; i < _polygonB._vertexCount; ++i) { v[i] = MathUtils.Multiply(ref _transformB, _polygonB._vertices[i]); } _debugDraw.DrawPolygon(ref v, _polygonB._vertexCount, color); } Vector2 x1 = output.pointA; Vector2 x2 = output.pointB; _debugDraw.DrawPoint(x1, 0.5f, new Color(1.0f, 0.0f, 0.0f)); _debugDraw.DrawPoint(x2, 0.5f, new Color(1.0f, 0.0f, 0.0f)); _debugDraw.DrawSegment(x1, x2, new Color(1.0f, 1.0f, 0.0f)); }
// TODO_ERIN might not need to return the separation public float Initialize( ref SimplexCache cache, DistanceProxy proxyA, in Sweep sweepA,
public override void OnRender() { var transformA = new Transform { Position = new Vector2(0.0f, 0.25f) }; transformA.Rotation.SetIdentity(); var transformB = new Transform(); transformB.SetIdentity(); var input = new ShapeCastInput(); input.ProxyA.Set(_vAs, _countA, _radiusA); input.ProxyB.Set(_vBs, _countB, _radiusB); input.TransformA = transformA; input.TransformB = transformB; input.TranslationB.Set(8.0f, 0.0f); var hit = DistanceAlgorithm.ShapeCast(out var output, input); var transformB2 = new Transform { Rotation = transformB.Rotation, Position = transformB.Position + output.Lambda * input.TranslationB }; var distanceInput = new DistanceInput { TransformA = transformA, TransformB = transformB2, UseRadii = false }; distanceInput.ProxyA.Set(_vAs, _countA, _radiusA); distanceInput.ProxyB.Set(_vBs, _countB, _radiusB); var simplexCache = new SimplexCache(); DistanceAlgorithm.Distance(out var distanceOutput, ref simplexCache, distanceInput); DrawString( $"hit = {hit}, iters = {output.Iterations}, lambda = {output.Lambda}, distance = {distanceOutput.Distance}"); var vertices = new Vector2[Settings.MaxPolygonVertices]; for (var i = 0; i < _countA; ++i) { vertices[i] = MathUtils.Mul(transformA, _vAs[i]); } //g_debugDraw.DrawCircle(vertices[0], _radiusA, b2Color(0.9f, 0.9f, 0.9f)); Drawer.DrawPolygon(vertices, _countA, Color.FromArgb(230, 230, 230)); for (var i = 0; i < _countB; ++i) { vertices[i] = MathUtils.Mul(transformB, _vBs[i]); } //g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.9f, 0.5f)); Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 230, 127)); for (var i = 0; i < _countB; ++i) { vertices[i] = MathUtils.Mul(transformB2, _vBs[i]); } //g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.7f, 0.9f)); Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 129, 230)); if (hit) { var p1 = output.Point; Drawer.DrawPoint(p1, 10.0f, Color.FromArgb(230, 77, 77)); var p2 = p1 + output.Normal; Drawer.DrawSegment(p1, p2, Color.FromArgb(230, 77, 77)); } }
public virtual void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { Debug.Assert(cache.count <= 3); // Copy data from cache. m_count = cache.count; for (int i = 0; i < m_count; ++i) { SimplexVertex v = vertices[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; Vec2 wALocal = proxyA.getVertex(v.indexA); Vec2 wBLocal = proxyB.getVertex(v.indexB); Transform.mulToOutUnsafe(transformA, wALocal, v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); v.w.set_Renamed(v.wB).subLocal(v.wA); v.a = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { float metric1 = cache.metric; float metric2 = Metric; if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { SimplexVertex v = vertices[0]; v.indexA = 0; v.indexB = 0; Vec2 wALocal = proxyA.getVertex(0); Vec2 wBLocal = proxyB.getVertex(0); Transform.mulToOutUnsafe(transformA, wALocal, v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); v.w.set_Renamed(v.wB).subLocal(v.wA); m_count = 1; } }
/// <inheritdoc /> protected override void PostStep() { if (Input.GetKeyDown(KeyCode.A)) { _positionB.X -= 0.1f; } if (Input.GetKeyDown(KeyCode.D)) { _positionB.X += 0.1f; } if (Input.GetKeyDown(KeyCode.S)) { _positionB.Y -= 0.1f; } if (Input.GetKeyDown(KeyCode.W)) { _positionB.Y += 0.1f; } if (Input.GetKeyDown(KeyCode.Q)) { _angleB += 0.1f * Settings.Pi; } if (Input.GetKeyDown(KeyCode.E)) { _angleB -= 0.1f * Settings.Pi; } _transformB.Set(_positionB, _angleB); var input = new DistanceInput(); input.ProxyA.Set(_polygonA, 0); input.ProxyB.Set(_polygonB, 0); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; var cache = SimplexCache.Create(); cache.Count = 0; DistanceAlgorithm.Distance(out var output, ref cache, input); DrawString($"distance = {output.Distance}"); DrawString($"iterations = {output.Iterations}"); { var color = Color.FromArgb(230, 230, 230); var v = new Vector2[Settings.MaxPolygonVertices]; for (var i = 0; i < _polygonA.Count; ++i) { v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]); } Drawer.DrawPolygon(v, _polygonA.Count, color); for (var i = 0; i < _polygonB.Count; ++i) { v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]); } Drawer.DrawPolygon(v, _polygonB.Count, color); } var x1 = output.PointA; var x2 = output.PointB; var c1 = Color.FromArgb(255, 0, 0); Drawer.DrawPoint(x1, 4.0f, c1); var c2 = Color.FromArgb(255, 255, 0); Drawer.DrawPoint(x2, 4.0f, c2); }
public void Set(SimplexCache sc) { Array.Copy(sc.IndexA, 0, IndexA, 0, IndexA.Length); Array.Copy(sc.IndexB, 0, IndexB, 0, IndexB.Length); Metric = sc.Metric; Count = sc.Count; }
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { Debug.Assert(cache.Count <= 3); // Copy data from cache. Count = cache.Count; for (int i = 0; i < Count; ++i) { SimplexVertex v = Vertices[i]; v.IndexA = cache.IndexA[i]; v.IndexB = cache.IndexB[i]; Vec2 wALocal = proxyA.GetVertex(v.IndexA); Vec2 wBLocal = proxyB.GetVertex(v.IndexB); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); v.A = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (Count > 1) { float metric1 = cache.Metric; float metric2 = Metric; if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { // Reset the simplex. Count = 0; } } // If the cache is empty or invalid ... if (Count == 0) { SimplexVertex v = Vertices[0]; v.IndexA = 0; v.IndexB = 0; Vec2 wALocal = proxyA.GetVertex(0); Vec2 wBLocal = proxyB.GetVertex(0); Transform.MulToOutUnsafe(transformA, wALocal, v.WA); Transform.MulToOutUnsafe(transformB, wBLocal, v.WB); v.W.Set(v.WB).SubLocal(v.WA); Count = 1; } }
/// <summary> /// Computes the distance between two shapes represented by the specified proxies, positioned as defined by the /// specified transforms. The cache is read-write and will be updated for future calls. /// </summary> private static float Distance( ref SimplexCache cache, DistanceProxy proxyA, WorldTransform xfA, DistanceProxy proxyB, WorldTransform xfB, bool useRadii = false) { // Initialize the simplex. Simplex simplex; Simplex.ReadCache(out simplex, cache, xfA, proxyA, xfB, proxyB); // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. var saveA = new FixedArray3 <int>(); var saveB = new FixedArray3 <int>(); // Main iteration loop. for (var i = 0; i < 20; ++i) { // Copy simplex so we can identify duplicates. var saveCount = simplex.Count; for (var j = 0; j < saveCount; ++j) { saveA[j] = simplex.Vertices[j].IndexA; saveB[j] = simplex.Vertices[j].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: throw new ArgumentOutOfRangeException(); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Get search direction. var 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 v; v.IndexA = proxyA.GetSupport(-xfA.Rotation * -d); v.IndexB = proxyB.GetSupport(-xfB.Rotation * d); v.VertexA = xfA.ToGlobal(proxyA.Vertices[v.IndexA]); v.VertexB = xfB.ToGlobal(proxyB.Vertices[v.IndexB]); // ReSharper disable RedundantCast Necessary for FarPhysics. v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA); // ReSharper restore RedundantCast v.Alpha = 0; simplex.Vertices[simplex.Count] = v; // Check for duplicate support points. This is the main termination criteria. var duplicate = false; for (var j = 0; j < saveCount; ++j) { if (v.IndexA == saveA[j] && v.IndexB == saveB[j]) { 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; } // Cache the simplex. simplex.WriteCache(ref cache); // Prepare output. var distance = simplex.GetWitnessPointDistance(); // Apply radii if requested. if (useRadii) { var rA = proxyA.Radius; var rB = proxyB.Radius; if (distance > rA + rB && distance > Settings.Epsilon) { // Shapes are still not overlapped. return(distance - (rA + rB)); } // Shapes are overlapped when radii are considered. return(0.0f); } return(distance); }
public static void ComputeDistance(ref DistanceInput input, out DistanceOutput output, out SimplexCache cache) { cache = new SimplexCache(); if (Settings.EnableDiagnostics) //Velcro: We only gather diagnostics when enabled { ++GJKCalls; } // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref 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>(); // 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; } // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < MathConstants.Epsilon * MathConstants.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) //Velcro: 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) //Velcro: 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 > MathConstants.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 closest points between two shapes. Supports any combination of: CircleShape and /// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to /// zero. /// </summary> /// <param name="output"></param> /// <param name="cache"></param> /// <param name="input"></param> public void distance(DistanceOutput output, SimplexCache cache, DistanceInput input) { GJK_CALLS++; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; Transform transformA = input.transformA; Transform transformB = input.transformB; // Initialize the simplex. simplex.readCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.vertices; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. // (pooled above) int saveCount = 0; simplex.getClosestPoint(closestPoint); float distanceSqr1 = closestPoint.lengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop int iter = 0; while (iter < GJK_MAX_ITERS) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int i = 0; i < saveCount; i++) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_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.m_count == 3) { break; } // Compute closest point. simplex.getClosestPoint(closestPoint); distanceSqr2 = closestPoint.lengthSquared(); // ensure progress if (distanceSqr2 >= distanceSqr1) { // break; } distanceSqr1 = distanceSqr2; // get search direction; simplex.getSearchDirection(d); // 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; } /* * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; */ // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = vertices[simplex.m_count]; Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp); vertex.indexA = proxyA.getSupport(temp); Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA); // Vec2 wBLocal; Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp); vertex.indexB = proxyB.getSupport(temp); Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB); vertex.w.set_Renamed(vertex.wB).subLocal(vertex.wA); // Iteration count is equated to the number of support point calls. ++iter; ++GJK_ITERS; // 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.m_count; } GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter); // Prepare output. simplex.getWitnessPoints(output.pointA, output.pointB); output.distance = MathUtils.distance(output.pointA, output.pointB); output.iterations = iter; // Cache the simplex. simplex.writeCache(cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.m_radius; float rB = proxyB.m_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); normal.set_Renamed(output.pointB).subLocal(output.pointA); normal.normalize(); temp.set_Renamed(normal).mulLocal(rA); output.pointA.addLocal(temp); temp.set_Renamed(normal).mulLocal(rB); output.pointB.subLocal(temp); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. // Vec2 p = 0.5f * (output.pointA + output.pointB); output.pointA.addLocal(output.pointB).mulLocal(.5f); output.pointB.set_Renamed(output.pointA); output.distance = 0.0f; } } }
public virtual void writeCache(SimplexCache cache) { cache.metric = Metric; cache.count = m_count; for (int i = 0; i < m_count; ++i) { cache.indexA[i] = (vertices[i].indexA); cache.indexB[i] = (vertices[i].indexB); } }
public void Initialize(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB) { _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Box2DXDebug.Assert(0 < count && count < 3); if (count == 1) { _type = Type.Points; Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.IndexB[0] == cache.IndexB[1]) { // Two points on A and one on B _type = Type.FaceA; Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]); _localPoint = 0.5f*(localPointA1 + localPointA2); _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, _localPoint); Vec2 pointB = Math.Mul(transformB, localPointB); float s = Vec2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = Type.FaceB; Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]); _localPoint = 0.5f*(localPointB1 + localPointB2); _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointB = Math.Mul(transformB, _localPoint); Vec2 pointA = Math.Mul(transformA, localPointA); float s = Vec2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on B and two points on A. // The faces are parallel. Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]); Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]); Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]); Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]); Vec2 pA = Math.Mul(transformA, localPointA1); Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1); Vec2 pB = Math.Mul(transformB, localPointB1); Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1); float a = Vec2.Dot(dA, dA); float e = Vec2.Dot(dB, dB); Vec2 r = pA - pB; float c = Vec2.Dot(dA, r); float f = Vec2.Dot(dB, r); float b = Vec2.Dot(dA, dB); float denom = a*e - b*b; float s = 0.0f; if (denom != 0.0f) { s = Math.Clamp((b*f - c*e)/denom, 0.0f, 1.0f); } float t = (b*s + f)/e; if (t < 0.0f) { t = 0.0f; s = Math.Clamp(-c/a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = Math.Clamp((b - c)/a, 0.0f, 1.0f); } Vec2 localPointA = localPointA1 + s*(localPointA2 - localPointA1); Vec2 localPointB = localPointB1 + t*(localPointB2 - localPointB1); if (s == 0.0f || s == 1.0f) { _type = Type.FaceB; _axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); _localPoint = localPointB; Vec2 normal = Math.Mul(transformB.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointA - pointB, normal); if (sgn < 0.0f) { _axis = -_axis; } } else { _type = Type.FaceA; _axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); _localPoint = localPointA; Vec2 normal = Math.Mul(transformA.R, _axis); Vec2 pointA = Math.Mul(transformA, localPointA); Vec2 pointB = Math.Mul(transformB, localPointB); float sgn = Vec2.Dot(pointB - pointA, normal); if (sgn < 0.0f) { _axis = -_axis; } } } }
public void WriteCache(SimplexCache cache) { cache.Metric = Metric; cache.Count = Count; for (int i = 0; i < Count; ++i) { cache.IndexA[i] = (Vertices[i].IndexA); cache.IndexB[i] = (Vertices[i].IndexB); } }
public static void Initialize(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1, out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type) { int count = cache.Count; Debug.Assert(0 < count && count < 3); sweepA.GetTransform(out Transform xfA, t1); sweepB.GetTransform(out Transform xfB, t1); if (count == 1) { localPoint = Vector2.Zero; type = SeparationFunctionType.Points; Vector2 localPointA = proxyA._vertices[cache.IndexA[0]]; Vector2 localPointB = proxyB._vertices[cache.IndexB[0]]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); axis = pointB - pointA; axis.Normalize(); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB._vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB._vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; axis = new Vector2(a.Y, -a.X); axis.Normalize(); Vector2 normal = MathUtils.Mul(ref xfB.q, axis); localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Mul(ref xfB, localPoint); Vector2 localPointA = proxyA._vertices[cache.IndexA[0]]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { axis = -axis; } } else { // Two points on A and one or two points on B. type = SeparationFunctionType.FaceA; Vector2 localPointA1 = proxyA._vertices[cache.IndexA[0]]; Vector2 localPointA2 = proxyA._vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; axis = new Vector2(a.Y, -a.X); axis.Normalize(); Vector2 normal = MathUtils.Mul(ref xfA.q, axis); localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Mul(ref xfA, localPoint); Vector2 localPointB = proxyB._vertices[cache.IndexB[0]]; Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { axis = -axis; } } //Velcro note: the returned value that used to be here has been removed, as it was not used. }
public virtual void set_Renamed(SimplexCache sc) { Array.Copy(sc.indexA, 0, indexA, 0, indexA.Length); Array.Copy(sc.indexB, 0, indexB, 0, indexB.Length); metric = sc.metric; count = sc.count; }