protected override void Draw(Settings settings) { base.Draw(settings); b2Sweep sweepA = new b2Sweep(); sweepA.alpha0 = 0; sweepA.c0.Set(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter.SetZero(); b2Sweep sweepB = new b2Sweep(); sweepB.alpha0 = 0; sweepB.c0.Set(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; // - 162.0f * b2_pi; sweepB.c.Set(54.595478f, -51.083473f); sweepB.a = 513.62781f; // - 162.0f * b2_pi; sweepB.localCenter.SetZero(); //sweepB.a0 -= 300.0f * b2_pi; //sweepB.a -= 300.0f * b2_pi; b2TOIInput input = b2TOIInput.Create(); input.proxyA.Set(m_shapeA, 0); input.proxyB.Set(m_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; b2TOIOutput output; b2TimeOfImpact.Compute(out output, ref input); m_debugDraw.DrawString(5, m_textLine, "toi = {0}", output.t); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "max toi iters = {0}, max root iters = {1}", b2TimeOfImpact.b2_toiMaxIters, b2TimeOfImpact.b2_toiMaxRootIters); m_textLine += 15; b2Vec2[] vertices = new b2Vec2[b2Settings.b2_maxPolygonVertices]; b2Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < m_shapeA.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformA, m_shapeA.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeA.VertexCount, new b2Color(0.9f, 0.9f, 0.9f)); b2Transform transformB; sweepB.GetTransform(out transformB, 0.0f); b2Vec2 localPoint = new b2Vec2(2.0f, -0.1f); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.9f, 0.5f, 0.5f)); #if false for (float t = 0.0f; t < 1.0f; t += 0.1f) { sweepB.GetTransform(ref transformB, t); for (int i = 0; i < m_shapeB.VertexCount; ++i) { vertices[i] = b2Math.b2Mul(transformB, m_shapeB.Vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.VertexCount, new b2Color(0.9f, 0.5f, 0.5f)); } #endif }
// TODO_ERIN might not need to return the separation public float Initialize(ref b2SimplexCache cache, b2DistanceProxy proxyA, ref b2Sweep sweepA, b2DistanceProxy proxyB, ref b2Sweep 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; b2Transform xfA, xfB; m_sweepA.GetTransform(out xfA, t1); m_sweepB.GetTransform(out xfB, t1); if (count == 1) { m_type = SeparationType.e_points; b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]); b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]); b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA); b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB); m_axis = pointB - 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 = SeparationType.e_faceB; b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]); b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]); float b21x = localPointB2.x - localPointB1.x; float b21y = localPointB2.y - localPointB1.y; m_axis.x = -b21y; m_axis.y = b21x; // m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint); b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]); b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA); b2Vec2 aminusb = pointA - pointB; float s = b2Math.b2Dot(ref aminusb, ref normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); b2Vec2 a2minusa1 = localPointA2 - localPointA1; m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB); b2Vec2 bminusa = pointB - pointA; float s = b2Math.b2Dot(ref bminusa, ref normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } }
// TODO_ERIN might not need to return the separation public float Initialize(ref b2SimplexCache cache, ref b2DistanceProxy proxyA, ref b2Sweep sweepA, ref b2DistanceProxy proxyB, ref b2Sweep 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; b2Transform xfA = b2Transform.Default, xfB = b2Transform.Default; m_sweepA.GetTransform(ref xfA, t1); m_sweepB.GetTransform(ref xfB, t1); if (count == 1) { m_type = SeparationType.e_points; b2Vec2 localPointA = m_proxyA.GetVertex((int)cache.indexA[0]); b2Vec2 localPointB = m_proxyB.GetVertex((int)cache.indexB[0]); b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA); b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB); m_axis = pointB - 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 = SeparationType.e_faceB; b2Vec2 localPointB1 = proxyB.GetVertex((int)cache.indexB[0]); b2Vec2 localPointB2 = proxyB.GetVertex((int)cache.indexB[1]); m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Math.b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = b2Math.b2Mul(xfB, m_localPoint); b2Vec2 localPointA = proxyA.GetVertex((int)cache.indexA[0]); b2Vec2 pointA = b2Math.b2Mul(xfA, localPointA); float s = b2Math.b2Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = b2Math.b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Math.b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = b2Math.b2Mul(xfA, m_localPoint); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointB = b2Math.b2Mul(xfB, localPointB); float s = b2Math.b2Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } }