public Sweep Set(Sweep argCloneFrom) { LocalCenter.Set(argCloneFrom.LocalCenter); C0.Set(argCloneFrom.C0); C.Set(argCloneFrom.C); A0 = argCloneFrom.A0; A = argCloneFrom.A; return(this); }
// 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 virtual float initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.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 = 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_Renamed(m_proxyA.getVertex(cache.indexA[0])); localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfa, localPointA, pointA); Transform.mulToOutUnsafe(xfb, localPointB, pointB); m_axis.set_Renamed(pointB).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 = Type.FACE_B; localPointB1.set_Renamed(m_proxyB.getVertex(cache.indexB[0])); localPointB2.set_Renamed(m_proxyB.getVertex(cache.indexB[1])); temp.set_Renamed(localPointB2).subLocal(localPointB1); Vec2.crossToOutUnsafe(temp, 1f, m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); m_localPoint.set_Renamed(localPointB1).addLocal(localPointB2).mulLocal(.5f); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); localPointA.set_Renamed(proxyA.getVertex(cache.indexA[0])); Transform.mulToOutUnsafe(xfa, localPointA, pointA); temp.set_Renamed(pointA).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 = Type.FACE_A; localPointA1.set_Renamed(m_proxyA.getVertex(cache.indexA[0])); localPointA2.set_Renamed(m_proxyA.getVertex(cache.indexA[1])); temp.set_Renamed(localPointA2).subLocal(localPointA1); Vec2.crossToOutUnsafe(temp, 1.0f, m_axis); m_axis.normalize(); Rot.mulToOutUnsafe(xfa.q, m_axis, normal); m_localPoint.set_Renamed(localPointA1).addLocal(localPointA2).mulLocal(.5f); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); localPointB.set_Renamed(m_proxyB.getVertex(cache.indexB[0])); Transform.mulToOutUnsafe(xfb, localPointB, pointB); temp.set_Renamed(pointB).subLocal(pointA); float s = Vec2.dot(temp, normal); if (s < 0.0f) { m_axis.negateLocal(); s = -s; } return s; } }
public Sweep Set(Sweep argCloneFrom) { LocalCenter.Set(argCloneFrom.LocalCenter); C0.Set(argCloneFrom.C0); C.Set(argCloneFrom.C); A0 = argCloneFrom.A0; A = argCloneFrom.A; return this; }
public Sweep set_Renamed(Sweep argCloneFrom) { localCenter.set_Renamed(argCloneFrom.localCenter); c0.set_Renamed(argCloneFrom.c0); c.set_Renamed(argCloneFrom.c); a0 = argCloneFrom.a0; a = argCloneFrom.a; return this; }