public void ReadCache(b2SimplexCache cache, b2DistanceProxy proxyA, b2Transform transformA, b2DistanceProxy proxyB, b2Transform transformB) { Debug.Assert(cache.count <= 3); // Copy data from cache. m_count = cache.count; b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 }; for (int i = 0; i < m_count; ++i) { b2SimplexVertex v = vertices[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; b2Vec2 wALocal = proxyA.GetVertex(v.indexA); b2Vec2 wBLocal = proxyB.GetVertex(v.indexB); v.wA = Utils.b2Mul(transformA, wALocal); v.wB = Utils.b2Mul(transformB, wBLocal); v.w = v.wB - 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 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { b2SimplexVertex v = vertices[0]; v.indexA = 0; v.indexB = 0; b2Vec2 wALocal = proxyA.GetVertex(0); b2Vec2 wBLocal = proxyB.GetVertex(0); v.wA = Utils.b2Mul(transformA, wALocal); v.wB = Utils.b2Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 1.0f; m_count = 1; } }
// TODO_ERIN might not need to return the separation public float Initialize(b2SimplexCache cache, b2DistanceProxy proxyA, b2Sweep sweepA, b2DistanceProxy proxyB, b2Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Debug.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; b2Transform xfA = new b2Transform(); b2Transform xfB = new b2Transform(); m_sweepA.GetTransform(ref xfA, t1); m_sweepB.GetTransform(ref xfB, t1); if (count == 1) { m_type = Type.e_points; b2Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointA = Utils.b2Mul(xfA, localPointA); b2Vec2 pointB = Utils.b2Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return(s); } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = Type.e_faceB; b2Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); b2Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utils.b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); b2Vec2 normal = Utils.b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = Utils.b2Mul(xfB, m_localPoint); b2Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); b2Vec2 pointA = Utils.b2Mul(xfA, localPointA); float s = Utils.b2Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } else { // Two points on A and one or two points on B. m_type = Type.e_faceA; b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utils.b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); b2Vec2 normal = Utils.b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = Utils.b2Mul(xfA, m_localPoint); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointB = Utils.b2Mul(xfB, localPointB); float s = Utils.b2Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } }