internal void WriteCache(ref SimplexCache cache) { cache.Metric = GetMetric(); cache.Count = (ushort)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, ref DistanceProxy proxyA, ref Transform transformA, ref 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]; Vector2 wALocal = proxyA._vertices[v.IndexA]; Vector2 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) { float metric1 = cache.Metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < MathConstants.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; Vector2 wALocal = proxyA._vertices[0]; Vector2 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; } }