internal void WriteCache(ref SimplexCache cache) { cache.Metric = this.GetMetric(); cache.Count = (ushort)this.Count; for (int i = 0; i < this.Count; i++) { cache.IndexA[i] = (byte)this.V[i].IndexA; cache.IndexB[i] = (byte)this.V[i].IndexB; } }
internal void WriteCache(ref SimplexCache cache) { cache.Metric = GetMetric(); cache.Count = (UInt16)Count; for (int i = 0; i < Count; ++i) { cache.IndexA[i] = (byte)(V[i].IndexA); cache.IndexB[i] = (byte)(V[i].IndexB); } }
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB) { Debug.Assert(cache.Count <= 3); // Copy data from cache. Count = cache.Count; for (int i = 0; i < Count; ++i) { SimplexVertex v = V[i]; v.IndexA = cache.IndexA[i]; v.IndexB = cache.IndexB[i]; TSVector2 wALocal = proxyA.Vertices[v.IndexA]; TSVector2 wBLocal = proxyB.Vertices[v.IndexB]; v.WA = MathUtils.Mul(ref transformA, wALocal); v.WB = MathUtils.Mul(ref transformB, wBLocal); v.W = v.WB - v.WA; v.A = 0.0f; V[i] = v; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (Count > 1) { FP metric1 = cache.Metric; FP metric2 = GetMetric(); 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 = V[0]; v.IndexA = 0; v.IndexB = 0; TSVector2 wALocal = proxyA.Vertices[0]; TSVector2 wBLocal = proxyB.Vertices[0]; v.WA = MathUtils.Mul(ref transformA, wALocal); v.WB = MathUtils.Mul(ref transformB, wBLocal); v.W = v.WB - v.WA; v.A = 1.0f; V[0] = v; Count = 1; } }
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB) { Debug.Assert(cache.Count <= 3); this.Count = (int)cache.Count; for (int i = 0; i < this.Count; i++) { SimplexVertex simplexVertex = this.V[i]; simplexVertex.IndexA = (int)cache.IndexA[i]; simplexVertex.IndexB = (int)cache.IndexB[i]; TSVector2 v = proxyA.Vertices[simplexVertex.IndexA]; TSVector2 v2 = proxyB.Vertices[simplexVertex.IndexB]; simplexVertex.WA = MathUtils.Mul(ref transformA, v); simplexVertex.WB = MathUtils.Mul(ref transformB, v2); simplexVertex.W = simplexVertex.WB - simplexVertex.WA; simplexVertex.A = 0f; this.V[i] = simplexVertex; } bool flag = this.Count > 1; if (flag) { FP metric = cache.Metric; FP metric2 = this.GetMetric(); bool flag2 = metric2 < 0.5f * metric || 2f * metric < metric2 || metric2 < Settings.Epsilon; if (flag2) { this.Count = 0; } } bool flag3 = this.Count == 0; if (flag3) { SimplexVertex simplexVertex2 = this.V[0]; simplexVertex2.IndexA = 0; simplexVertex2.IndexB = 0; TSVector2 v3 = proxyA.Vertices[0]; TSVector2 v4 = proxyB.Vertices[0]; simplexVertex2.WA = MathUtils.Mul(ref transformA, v3); simplexVertex2.WB = MathUtils.Mul(ref transformB, v4); simplexVertex2.W = simplexVertex2.WB - simplexVertex2.WA; simplexVertex2.A = 1f; this.V[0] = simplexVertex2; this.Count = 1; } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = default(SimplexCache); Simplex simplex = default(Simplex); simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); FixedArray3 <int> fixedArray = default(FixedArray3 <int>); FixedArray3 <int> fixedArray2 = default(FixedArray3 <int>); int i = 0; while (i < 20) { int count = simplex.Count; for (int j = 0; j < count; j++) { fixedArray[j] = simplex.V[j].IndexA; fixedArray2[j] = simplex.V[j].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } bool flag = simplex.Count == 3; if (flag) { break; } TSVector2 searchDirection = simplex.GetSearchDirection(); bool flag2 = searchDirection.LengthSquared() < Settings.EpsilonSqr; if (flag2) { break; } SimplexVertex simplexVertex = simplex.V[simplex.Count]; simplexVertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -searchDirection)); simplexVertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[simplexVertex.IndexA]); simplexVertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, searchDirection)); simplexVertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[simplexVertex.IndexB]); simplexVertex.W = simplexVertex.WB - simplexVertex.WA; simplex.V[simplex.Count] = simplexVertex; i++; bool flag3 = false; for (int k = 0; k < count; k++) { bool flag4 = simplexVertex.IndexA == fixedArray[k] && simplexVertex.IndexB == fixedArray2[k]; if (flag4) { flag3 = true; break; } } bool flag5 = flag3; if (flag5) { break; } simplex.Count++; } simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).magnitude; output.Iterations = i; simplex.WriteCache(ref cache); bool useRadii = input.UseRadii; if (useRadii) { FP radius = input.ProxyA.Radius; FP radius2 = input.ProxyB.Radius; bool flag6 = output.Distance > radius + radius2 && output.Distance > Settings.Epsilon; if (flag6) { output.Distance -= radius + radius2; TSVector2 value = output.PointB - output.PointA; value.Normalize(); output.PointA += radius * value; output.PointB -= radius2 * value; } else { TSVector2 tSVector = 0.5f * (output.PointA + output.PointB); output.PointA = tSVector; output.PointB = tSVector; output.Distance = 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>(); //FP distanceSqr1 = Settings.MaxFP; // 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(); //FP distanceSqr2 = p.LengthSquared(); // Ensure progress //if (distanceSqr2 >= distanceSqr1) //{ //break; //} //distanceSqr1 = distanceSqr2; // Get search direction. TSVector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.EpsilonSqr) { // 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).magnitude; output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { FP rA = input.ProxyA.Radius; FP 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; TSVector2 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. TSVector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1) { _localPoint = TSVector2.zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; TSVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA); TSVector2 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; TSVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; TSVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; TSVector2 a = localPointB2 - localPointB1; _axis = new TSVector2(a.y, -a.x); _axis.Normalize(); TSVector2 normal = MathUtils.Mul(ref xfB.q, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); TSVector2 pointB = MathUtils.Mul(ref xfB, _localPoint); TSVector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FP s = TSVector2.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; TSVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; TSVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; TSVector2 a = localPointA2 - localPointA1; _axis = new TSVector2(a.y, -a.x); _axis.Normalize(); TSVector2 normal = MathUtils.Mul(ref xfA.q, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); TSVector2 pointA = MathUtils.Mul(ref xfA, _localPoint); TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB); FP s = TSVector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } //FPE note: the returned value that used to be here has been removed, as it was not used. }