// float FindMinSeparation(int* indexA, int* indexB, float t) const public float FindMinSeparation(int[] indexes, float t) { SweepA.GetTransform(xfa, t); SweepB.GetTransform(xfb, t); switch (Type) { case Type.Points: { Rot.MulTransUnsafe(xfa.Q, Axis, axisA); Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB); Axis.NegateLocal(); indexes[0] = ProxyA.GetSupport(axisA); indexes[1] = ProxyB.GetSupport(axisB); localPointA.Set(ProxyA.GetVertex(indexes[0])); localPointB.Set(ProxyB.GetVertex(indexes[1])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis); return(separation); } case Type.FaceA: { Rot.MulToOutUnsafe(xfa.Q, Axis, normal); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB); normal.NegateLocal(); indexes[0] = -1; indexes[1] = ProxyB.GetSupport(axisB); localPointB.Set(ProxyB.GetVertex(indexes[1])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), normal); return(separation); } case Type.FaceB: { Rot.MulToOutUnsafe(xfb.Q, Axis, normal); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA); normal.NegateLocal(); indexes[1] = -1; indexes[0] = ProxyA.GetSupport(axisA); localPointA.Set(ProxyA.GetVertex(indexes[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); float separation = Vec2.Dot(pointA.SubLocal(pointB), normal); return(separation); } default: Debug.Assert(false); indexes[0] = -1; indexes[1] = -1; return(0f); } }
// TODO_ERIN might not need to return the separation public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1) { ProxyA = proxyA; ProxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); SweepA = sweepA; SweepB = sweepB; SweepA.GetTransform(xfa, t1); SweepB.GetTransform(xfb, t1); // log.debug("initializing separation.\n" + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" // "distance: "+proxyA. if (count == 1) { Type = Type.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(ProxyA.GetVertex(cache.IndexA[0])); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); Axis.Set(pointB).SubLocal(pointA); float s = Axis.Normalize(); return s; } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. Type = Type.FaceB; localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0])); localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1])); temp.Set(localPointB2).SubLocal(localPointB1); Vec2.CrossToOutUnsafe(temp, 1f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfb.Q, Axis, normal); LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); localPointA.Set(proxyA.GetVertex(cache.IndexA[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); temp.Set(pointA).SubLocal(pointB); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return s; } else { // Two points on A and one or two points on B. Type = Type.FaceA; localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1])); temp.Set(localPointA2).SubLocal(localPointA1); Vec2.CrossToOutUnsafe(temp, 1.0f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfa.Q, Axis, normal); LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); temp.Set(pointB).SubLocal(pointA); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return s; } }
// TODO_ERIN might not need to return the separation public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1) { ProxyA = proxyA; ProxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); SweepA = sweepA; SweepB = sweepB; SweepA.GetTransform(xfa, t1); SweepB.GetTransform(xfb, t1); // log.debug("initializing separation.\n" + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" // "distance: "+proxyA. if (count == 1) { Type = Type.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(ProxyA.GetVertex(cache.IndexA[0])); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); Axis.Set(pointB).SubLocal(pointA); float s = Axis.Normalize(); return(s); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. Type = Type.FaceB; localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0])); localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1])); temp.Set(localPointB2).SubLocal(localPointB1); Vec2.CrossToOutUnsafe(temp, 1f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfb.Q, Axis, normal); LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); localPointA.Set(proxyA.GetVertex(cache.IndexA[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); temp.Set(pointA).SubLocal(pointB); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } else { // Two points on A and one or two points on B. Type = Type.FaceA; localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1])); temp.Set(localPointA2).SubLocal(localPointA1); Vec2.CrossToOutUnsafe(temp, 1.0f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfa.Q, Axis, normal); LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); temp.Set(pointB).SubLocal(pointA); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } }