public static b2Sweep Create()
{
b2Sweep w = new b2Sweep();
w.Defaults();
return(w);
}
public static b2Sweep Create()
{
b2Sweep w = new b2Sweep();
w.Defaults();
return (w);
}
// 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, ref b2Transform xfA, ref b2Transform xfB)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.m_vertices[(int)cache.indexA[0]];
b2Vec2 localPointB = m_proxyB.m_vertices[(int)cache.indexB[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
m_axis.x = pointBx - pointAx;
m_axis.y = pointBy - pointAy;
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.m_vertices[(int)cache.indexB[0]];
b2Vec2 localPointB2 = proxyB.m_vertices[(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();
float normalx = xfB.q.c * m_axis.x - xfB.q.s * m_axis.y;
float normaly = xfB.q.s * m_axis.x + xfB.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointB1.x + localPointB2.x);
m_localPoint.y = 0.5f * (localPointB1.y + localPointB2.y);
float pointBx = (xfB.q.c * m_localPoint.x - xfB.q.s * m_localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * m_localPoint.x + xfB.q.c * m_localPoint.y) + xfB.p.y;
b2Vec2 localPointA = proxyA.m_vertices[(int)cache.indexA[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float aminusbx = pointAx - pointBx;
float aminusby = pointAy - pointBy;
float s = aminusbx * normalx + aminusby * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
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.m_vertices[cache.indexA[0]];
b2Vec2 localPointA2 = m_proxyA.m_vertices[cache.indexA[1]];
float a2minusa1x = localPointA2.x - localPointA1.x;
float a2minusa1y = localPointA2.y - localPointA1.y;
//m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.x = a2minusa1y;
m_axis.y = -a2minusa1x;
m_axis.Normalize();
float normalx = xfA.q.c * m_axis.x - xfA.q.s * m_axis.y;
float normaly = xfA.q.s * m_axis.x + xfA.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointA1.x + localPointA2.x);
m_localPoint.y = 0.5f * (localPointA1.y + localPointA2.y);
float pointAx = (xfA.q.c * m_localPoint.x - xfA.q.s * m_localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * m_localPoint.x + xfA.q.c * m_localPoint.y) + xfA.p.y;
b2Vec2 localPointB = m_proxyB.m_vertices[cache.indexB[0]];
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
float bminusax = pointBx - pointAx;
float bminusay = pointBy - pointAy;
float s = bminusax * normalx + bminusay * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return s;
}
}
// 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;
}
}
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,
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;
}
}
