public void GetWitnessPoints(Vec2 pA, Vec2 pB) { switch (Count) { case 0: Debug.Assert(false); break; case 1: pA.Set(m_v1.WA); pB.Set(m_v1.WB); break; case 2: case2.Set(m_v1.WA).MulLocal(m_v1.A); pA.Set(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(m_v1.WB).MulLocal(m_v1.A); pB.Set(m_v2.WB).MulLocal(m_v2.A).AddLocal(case2); break; case 3: pA.Set(m_v1.WA).MulLocal(m_v1.A); case3.Set(m_v2.WA).MulLocal(m_v2.A); case33.Set(m_v3.WA).MulLocal(m_v3.A); pA.AddLocal(case3).AddLocal(case33); pB.Set(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 ComputeCentroidToOut(Vec2[] vs, int count, Vec2 result) { Debug.Assert(count >= 3); result.Set(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; const 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(p2).SubLocal(p1); e2.Set(p3).SubLocal(p1); float D = Vec2.Cross(e1, e2); float triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid e1.Set(p1).AddLocal(p2).AddLocal(p3).MulLocal(triangleArea * inv3); result.AddLocal(e1); } // Centroid Debug.Assert(area > Settings.EPSILON); result.MulLocal(1.0f / area); }
public void GetLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 result) { result.Set(worldPoint).SubLocal(Sweep.C); Vec2.CrossToOut(m_angularVelocity, result, result); result.AddLocal(m_linearVelocity); }