// TODO_ERIN might not need to return the separation public float initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep 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; m_sweepA.getTransform(xfa, t1); m_sweepB.getTransform(xfb, t1); // log.debug("initializing separation.\n" + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" // "distance: "+proxyA. if (count == 1) { m_type = TOIType.POINTS; /* * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2 * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize(); */ localPointA.set(m_proxyA.getVertex(cache.indexA[0])); localPointB.set(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfa, localPointA, ref pointA); Transform.mulToOutUnsafe(xfb, localPointB, ref pointB); m_axis.set(pointB); m_axis.subLocal(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 = TOIType.FACE_B; localPointB1.set(m_proxyB.getVertex(cache.indexB[0])); localPointB2.set(m_proxyB.getVertex(cache.indexB[1])); temp.set(localPointB2); temp.subLocal(localPointB1); Vec2.crossToOutUnsafe(temp, 1f, ref m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfb.q, m_axis, ref normal); m_localPoint.set(localPointB1); m_localPoint.addLocal(localPointB2); m_localPoint.mulLocal(.5f); Transform.mulToOutUnsafe(xfb, m_localPoint, ref pointB); localPointA.set(proxyA.getVertex(cache.indexA[0])); Transform.mulToOutUnsafe(xfa, localPointA, ref pointA); temp.set(pointA); temp.subLocal(pointB); float s = Vec2.dot(temp, normal); if (s < 0.0f) { m_axis.negateLocal(); s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = TOIType.FACE_A; localPointA1.set(m_proxyA.getVertex(cache.indexA[0])); localPointA2.set(m_proxyA.getVertex(cache.indexA[1])); temp.set(localPointA2); temp.subLocal(localPointA1); Vec2.crossToOutUnsafe(temp, 1.0f, ref m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfa.q, m_axis, ref normal); m_localPoint.set(localPointA1); m_localPoint.addLocal(localPointA2); m_localPoint.mulLocal(.5f); Transform.mulToOutUnsafe(xfa, m_localPoint, ref pointA); localPointB.set(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfb, localPointB, ref pointB); temp.set(pointB); temp.subLocal(pointA); float s = Vec2.dot(temp, normal); if (s < 0.0f) { m_axis.negateLocal(); s = -s; } return s; } }
public 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, ref v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB); v.w.set(v.wB); v.w.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 = getMetric(); 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, ref v.wA); Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB); v.w.set(v.wB); v.w.subLocal(v.wA); m_count = 1; } }