public virtual void getWitnessPoints(Vec2 pA, Vec2 pB) { switch (m_count) { case 0: Debug.Assert(false); break; case 1: pA.set_Renamed(m_v1.wA); pB.set_Renamed(m_v1.wB); break; case 2: case2.set_Renamed(m_v1.wA).mulLocal(m_v1.a); pA.set_Renamed(m_v2.wA).mulLocal(m_v2.a).addLocal(case2); // m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; // *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; case2.set_Renamed(m_v1.wB).mulLocal(m_v1.a); pB.set_Renamed(m_v2.wB).mulLocal(m_v2.a).addLocal(case2); break; case 3: pA.set_Renamed(m_v1.wA).mulLocal(m_v1.a); case3.set_Renamed(m_v2.wA).mulLocal(m_v2.a); case33.set_Renamed(m_v3.wA).mulLocal(m_v3.a); pA.addLocal(case3).addLocal(case33); pB.set_Renamed(pA); // *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; // *pB = *pA; break; default: Debug.Assert(false); break; } }
public void getLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 out_Renamed) { out_Renamed.set_Renamed(worldPoint).subLocal(m_sweep.c); Vec2.crossToOut(m_angularVelocity, out_Renamed, out_Renamed); out_Renamed.addLocal(m_linearVelocity); }
public void computeCentroidToOut(Vec2[] vs, int count, Vec2 out_Renamed) { Debug.Assert(count >= 3); out_Renamed.set_Renamed(0.0f, 0.0f); float area = 0.0f; // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). Vec2 pRef = pool1; pRef.setZero(); Vec2 e1 = pool2; Vec2 e2 = pool3; float inv3 = 1.0f / 3.0f; for (int i = 0; i < count; ++i) { // Triangle vertices. Vec2 p1 = pRef; Vec2 p2 = vs[i]; Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; e1.set_Renamed(p2).subLocal(p1); e2.set_Renamed(p3).subLocal(p1); float D = Vec2.cross(e1, e2); float triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid e1.set_Renamed(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3); out_Renamed.addLocal(e1); } // Centroid Debug.Assert(area > Settings.EPSILON); out_Renamed.mulLocal(1.0f / area); }