public void Set(b2Sweep other)
{
localCenter.SetV(other.localCenter);
c0.SetV(other.c0);
c.SetV(other.c);
a0 = other.a0;
a = other.a;
t0 = other.t0;
}
public float ComputeTOI(b2Sweep sweepA, b2Sweep sweepB)
{
s_input.proxyA.Set(m_fixtureA.GetShape());
s_input.proxyB.Set(m_fixtureB.GetShape());
s_input.sweepA = sweepA;
s_input.sweepB = sweepB;
s_input.tolerance = b2Settings.b2_linearSlop;
return(b2TimeOfImpact.TimeOfImpact(s_input));
}
public b2Sweep Copy()
{
b2Sweep copy = new b2Sweep();
copy.localCenter.SetV(localCenter);
copy.c0.SetV(c0);
copy.c.SetV(c);
copy.a0 = a0;
copy.a = a;
copy.t0 = t0;
return(copy);
}
// Find TOI contacts and solve them.
public void SolveTOI(b2TimeStep step)
{
b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);
if (m_stepComplete)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b.BodyFlags &= ~b2Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
{
// Invalidate TOI
c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
b2Contact minContact = null;
float minAlpha = 1.0f;
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > b2Settings.b2_maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2BodyType typeA = bA.BodyType;
b2BodyType typeB = bB.BodyType;
bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.alpha0;
if (bA.Sweep.alpha0 < bB.Sweep.alpha0)
{
alpha0 = bB.Sweep.alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.alpha0 < bA.Sweep.alpha0)
{
alpha0 = bA.Sweep.alpha0;
bB.Sweep.Advance(alpha0);
}
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input = new b2TOIInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.sweepA = bA.Sweep;
input.sweepB = bB.Sweep;
input.tMax = 1.0f;
b2TOIOutput output = b2TimeOfImpact(input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == b2TOIOutputType.e_touching)
{
alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.ContactFlags |= b2ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
{
// Advance the bodies to the TOI.
b2Fixture fA = minContact.GetFixtureA();
b2Fixture fB = minContact.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2Sweep backup1 = bA.Sweep;
b2Sweep backup2 = bB.Sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.ContactListener);
minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.Sweep = backup1;
bB.Sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.BodyFlags |= b2BodyFlags.e_islandFlag;
bB.BodyFlags |= b2BodyFlags.e_islandFlag;
minContact.ContentType |= b2ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body[] bodies = new b2Body[] { bA, bB };
for (int i = 0; i < 2; ++i)
{
b2Body body = bodies[i];
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
b2Contact contact = ce.contact;
// Has this contact already been added to the island?
if (contact.ContactType & b2ContactType.e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body other = ce.other;
if (other.BodyType == b2BodyType.b2_dynamicBody &&
body.IsBullet() == false && other.IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other.Sweep;
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(m_contactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.IsEnabled() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.ContactFlags |= b2ContactFlags.e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
continue;
}
// Add the other body to the island.
other.BodyFlags |= b2BodyFlags.e_islandFlag;
if (other.BodyType != b2BodyType.b2_staticBody)
{
other.SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i)
{
b2Body body = island.m_bodies[i];
body.BodyFlags &= ~b2BodyFlags.e_islandFlag;
if (body.BodyType != b2BodyType.b2_dynamicBody)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void Compute(out b2TOIOutput output, ref b2TOIInput input)
{
++b2_toiCalls;
output.state = b2ImpactState.e_unknown;
output.t = input.tMax;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Sweep sweepA = input.sweepA;
b2Sweep sweepB = input.sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.Radius + proxyB.Radius;
float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop);
float tolerance = 0.25f * b2Settings.b2_linearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
// Prepare input for distance query.
b2SimplexCache cache = _cache;
b2DistanceInput distanceInput;
distanceInput.proxyA = input.proxyA;
distanceInput.proxyB = input.proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
while (true)
{
// Get the distance between shapes. We can also use the results
// to get a separating axis.
sweepA.GetTransform(out distanceInput.transformA, t1);
sweepB.GetTransform(out distanceInput.transformB, t1);
b2DistanceOutput distanceOutput;
b2Simplex.b2Distance(out distanceOutput, ref cache, ref distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = b2ImpactState.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = b2ImpactState.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn = new b2SeparationFunction();
fcn.Initialize(ref cache, proxyA, ref sweepA, proxyB, ref sweepB, t1,
ref distanceInput.transformA, ref distanceInput.transformB);
#if false
// Dump the curve seen by the root finder
{
int N = 100;
float dx = 1.0f / N;
float xs[N + 1];
// 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);
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Sweep obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
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(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 = new b2Transform();
sweepA.GetTransform(ref 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 = new b2Transform();
sweepB.GetTransform(ref 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(ref 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(ref 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(b2SimplexCache cache, b2DistanceProxy proxyA, b2Sweep sweepA, b2DistanceProxy proxyB, 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 = new b2Transform();
b2Transform xfB = new b2Transform();
m_sweepA.GetTransform(ref xfA, t1);
m_sweepB.GetTransform(ref xfB, t1);
if (count == 1)
{
m_type = Type.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
b2Vec2 pointB = Utils.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 = Type.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utils.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = Utils.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
float s = Utils.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 = Type.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utils.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = Utils.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = Utils.b2Mul(xfB, localPointB);
float s = Utils.b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
public static float TimeOfImpact(b2TOIInput input)
{
++b2_toiCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Sweep sweepA = input.sweepA;
b2Sweep sweepB = input.sweepB;
b2Settings.b2Assert(sweepA.t0 == sweepB.t0);
b2Settings.b2Assert(1.0f - sweepA.t0 > float.MinValue);
float radius = proxyA.m_radius + proxyB.m_radius;
float tolerance = input.tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; //TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
s_cache.count = 0;
s_distanceInput.useRadii = false;
for (;;)
{
sweepA.GetTransform(s_xfA, alpha);
sweepB.GetTransform(s_xfB, alpha);
// Get the distance between shapes
s_distanceInput.proxyA = proxyA;
s_distanceInput.proxyB = proxyB;
s_distanceInput.transformA = s_xfA;
s_distanceInput.transformB = s_xfB;
b2Distance.Distance(s_distanceOutput, s_cache, s_distanceInput);
if (s_distanceOutput.distance <= 0.0f)
{
alpha = 1.0f;
break;
}
s_fcn.Initialize(s_cache, proxyA, s_xfA, proxyB, s_xfB);
float separation = s_fcn.Evaluate(s_xfA, s_xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance
if (separation > radius)
{
target = b2Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = b2Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
//#if 0
// Dump the curve seen by the root finder
//{
//const N:int = 100;
//var dx:Number = 1.0 / N;
//var xs:Vector.<Number> = new Array(N + 1);
//var fs:Vector.<Number> = new Array(N + 1);
//
//var x:Number = 0.0;
//for (var i:int = 0; i <= N; i++)
//{
//sweepA.GetTransform(xfA, x);
//sweepB.GetTransform(xfB, x);
//var f:Number = fcn.Evaluate(xfA, xfB) - target;
//
//trace(x, f);
//xs[i] = x;
//fx[i] = f'
//
//x += dx;
//}
//}
//#endif
// Compute 1D root of f(x) - target = 0
float newAlpha = alpha;
{
float x1 = alpha;
float x2 = 1.0f;
float f1 = separation;
sweepA.GetTransform(s_xfA, x2);
sweepB.GetTransform(s_xfB, x2);
float f2 = s_fcn.Evaluate(s_xfA, s_xfB);
// If intervals don't overlap at t2, then we are done
if (f2 >= target)
{
alpha = 1.0f;
break;
}
// Determine when intervals intersect
int rootIterCount = 0;
for (;;)
{
// Use a mis of the secand rule and bisection
float x;
if ((rootIterCount & 1) > 0)
{
// Secant rule to improve convergence
x = x1 + (target - f1) * (x2 - x1) / (f2 - f1);
}
else
{
// Bisection to guarantee progress
x = 0.5f * (x1 + x2);
}
sweepA.GetTransform(s_xfA, x);
sweepB.GetTransform(s_xfB, x);
float f = s_fcn.Evaluate(s_xfA, s_xfB);
if (b2Math.Abs(f - target) < 0.025f * tolerance)
{
newAlpha = x;
break;
}
// Ensure we continue to bracket the root
if (f > target)
{
x1 = x;
f1 = f;
}
else
{
x2 = x;
f2 = f;
}
++rootIterCount;
++b2_toiRootIters;
if (rootIterCount == 50)
{
break;
}
}
b2_toiMaxRootIters = (int)b2Math.Max((float)b2_toiMaxRootIters, (float)rootIterCount);
}
// Ensure significant advancement
if (newAlpha < (1.0f + 100.0f * float.MinValue) * alpha)
{
break;
}
alpha = newAlpha;
iter++;
++b2_toiIters;
if (iter == k_maxIterations)
{
break;
}
}
b2_toiMaxIters = (int)b2Math.Max((float)b2_toiMaxIters, (float)iter);
return(alpha);
}
// 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);
}
}
