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;
}
}
