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 Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1) { SeparationFunction._localPoint = TSVector2.zero; SeparationFunction._proxyA = proxyA; SeparationFunction._proxyB = proxyB; int count = (int)cache.Count; Debug.Assert(0 < count && count < 3); SeparationFunction._sweepA = sweepA; SeparationFunction._sweepB = sweepB; Transform transform; SeparationFunction._sweepA.GetTransform(out transform, t1); Transform transform2; SeparationFunction._sweepB.GetTransform(out transform2, t1); bool flag = count == 1; if (flag) { SeparationFunction._type = SeparationFunctionType.Points; TSVector2 v = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 v2 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 value = MathUtils.Mul(ref transform, v); TSVector2 value2 = MathUtils.Mul(ref transform2, v2); SeparationFunction._axis = value2 - value; SeparationFunction._axis.Normalize(); } else { bool flag2 = cache.IndexA[0] == cache.IndexA[1]; if (flag2) { SeparationFunction._type = SeparationFunctionType.FaceB; TSVector2 tSVector = proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 tSVector2 = proxyB.Vertices[(int)cache.IndexB[1]]; TSVector2 tSVector3 = tSVector2 - tSVector; SeparationFunction._axis = new TSVector2(tSVector3.y, -tSVector3.x); SeparationFunction._axis.Normalize(); TSVector2 value3 = MathUtils.Mul(ref transform2.q, SeparationFunction._axis); SeparationFunction._localPoint = 0.5f * (tSVector + tSVector2); TSVector2 value4 = MathUtils.Mul(ref transform2, SeparationFunction._localPoint); TSVector2 v3 = proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 value5 = MathUtils.Mul(ref transform, v3); FP x = TSVector2.Dot(value5 - value4, value3); bool flag3 = x < 0f; if (flag3) { SeparationFunction._axis = -SeparationFunction._axis; } } else { SeparationFunction._type = SeparationFunctionType.FaceA; TSVector2 tSVector4 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 tSVector5 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[1]]; TSVector2 tSVector6 = tSVector5 - tSVector4; SeparationFunction._axis = new TSVector2(tSVector6.y, -tSVector6.x); SeparationFunction._axis.Normalize(); TSVector2 value6 = MathUtils.Mul(ref transform.q, SeparationFunction._axis); SeparationFunction._localPoint = 0.5f * (tSVector4 + tSVector5); TSVector2 value7 = MathUtils.Mul(ref transform, SeparationFunction._localPoint); TSVector2 v4 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 value8 = MathUtils.Mul(ref transform2, v4); FP x2 = TSVector2.Dot(value8 - value7, value6); bool flag4 = x2 < 0f; if (flag4) { SeparationFunction._axis = -SeparationFunction._axis; } } } }
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. }