// TODO_ERIN might not need to return the separation
float Initialize(SimplexCache cache,
DistanceProxy proxyA, Sweep sweepA,
DistanceProxy proxyB, Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Utilities.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
Vec2 pointB = Utilities.Mul(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;
Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
Vec2 pointB = Utilities.Mul(xfB, m_localPoint);
Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
float s = Utilities.Dot(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;
Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
Vec2 pointA = Utilities.Mul(xfA, m_localPoint);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointB = Utilities.Mul(xfB, localPointB);
float s = Utilities.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use b2Distance to compute the contact point and normal at the time of impact.
public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input)
{
++b2_toiCalls;
output = new TOIOutput();
output.State = TOIOutputState.Unknown;
output.t = input.tMax;
Sweep sweepA = input.sweepA;
Sweep 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 = input.proxyA._radius + input.proxyB._radius;
float target = Math.Max(Settings.b2_linearSlop, totalRadius - 3.0f * Settings.b2_linearSlop);
float tolerance = 0.25f * Settings.b2_linearSlop;
//Debug.Assert(target > tolerance);
float t1 = 0.0f;
int k_maxIterations = 20;
int iter = 0;
// Prepare input for distance query.
SimplexCache cache;
DistanceInput 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).
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Distance.ComputeDistance(out distanceOutput, out cache, ref distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.State = TOIOutputState.Overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.State = TOIOutputState.Touching;
output.t = t1;
break;
}
SeparationFunction fcn = new SeparationFunction(ref cache, ref input.proxyA, ref sweepA, ref input.proxyB, ref sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
float t2 = tMax;
int pushBackIter = 0;
for (;;)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output.State = TOIOutputState.Seperated;
output.t = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output.State = TOIOutputState.Failed;
output.t = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output.State = TOIOutputState.Touching;
output.t = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rootIterCount = 0;
float a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
float t;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
float s = fcn.Evaluate(indexA, indexB, t);
if (Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
++rootIterCount;
++b2_toiRootIters;
if (rootIterCount == 50)
{
break;
}
}
b2_toiMaxRootIters = Math.Max(b2_toiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == Settings.b2_maxPolygonVertices)
{
break;
}
}
++iter;
++b2_toiIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.t = t1;
break;
}
}
b2_toiMaxIters = Math.Max(b2_toiMaxIters, iter);
}
public SeparationFunction(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Sweep sweepA,
ref DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.count;
//Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.GetType();
// Only bullets perform TOI with dynamic bodies.
if (bullet == true)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other._flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact._flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact._toiCount > 10)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
int indexA = contact._indexA;
int indexB = contact._indexB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
Body bodyA = fixtureA._body;
Body bodyB = fixtureB._body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fixtureA.GetShape(), indexA);
input.proxyB.Set(fixtureB.GetShape(), indexB);
input.sweepA = bodyA._sweep;
input.sweepB = bodyB._sweep;
input.tMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.t < toi)
{
toiContact = contact;
toi = output.t;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body._sweep;
body.Advance(toi);
toiContact.Update(_contactManager.ContactListener);
if (toiContact.IsEnabled() == false)
{
// Contact disabled. Backup and recurse.
body._sweep = backup;
SolveTOI(body);
}
++toiContact._toiCount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact.Update(_contactManager.ContactListener);
}
// Did the user disable the contact?
if (contact.IsEnabled() == false)
{
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
float k_toiBaumgarte = 0.75f;
//bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
//solved = true;
break;
}
}
if (toiOther.GetType() != BodyType.Static)
{
toiContact._flags |= ContactFlags.BulletHit;
}
}
private void SolveTOI(TimeStep step)
{
Island island = new Island(m_contactManager.m_contactListener);
if (m_stepComplete)
{
foreach (Body b in m_bodyList)
{
b.m_flags &= ~Body.BodyFlags.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
foreach (Contact c in m_contactManager.m_contactList)
{
// Invalidate TOI
c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
Fixture fA = null;
Fixture fB = null;
Body bA = null;
Body bB = null;
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
foreach (Contact c in m_contactManager.m_contactList)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > Settings._maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.m_flags.HasFlag(ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
fA = c.FixtureA;
fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
bA = fA.GetBody();
bB = fB.GetBody();
BodyType typeA = bA.m_type;
BodyType typeB = bB.m_type;
Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody);
bool activeA = bA.IsAwake() && typeA != BodyType._staticBody;
bool activeB = bB.IsAwake() && typeB != BodyType._staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody;
bool collideB = bB.IsBullet() || typeB != BodyType._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.m_sweep.alpha0;
if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0)
{
alpha0 = bB.m_sweep.alpha0;
bA.m_sweep.Advance(alpha0);
}
else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0)
{
alpha0 = bA.m_sweep.alpha0;
bB.m_sweep.Advance(alpha0);
}
Utilities.Assert(alpha0 < 1.0f);
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fA.GetShape(), indexA);
input.proxyB.Set(fB.GetShape(), indexB);
input.sweepA = bA.m_sweep;
input.sweepB = bB.m_sweep;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == TOIOutput.State.e_touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.m_flags |= ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
fA = minContact.FixtureA;
fB = minContact.FixtureB;
bA = fA.GetBody();
bB = fB.GetBody();
Sweep backup1 = bA.m_sweep;
Sweep backup2 = bB.m_sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.m_contactListener);
minContact.m_flags &= ~ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.m_sweep = backup1;
bB.m_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.m_flags |= Body.BodyFlags.e_islandFlag;
bB.m_flags |= Body.BodyFlags.e_islandFlag;
minContact.m_flags |= ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA, bB };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.m_type == BodyType._dynamicBody)
{
foreach (ContactEdge ce in body.m_contactList)
{
throw new NotImplementedException();
//if (island.m_bodies.Count() == island.m_bodyCapacity)
//{
// break;
//}
//if (island.m_bodies.Count() == island.m_contactCapacity)
//{
// break;
//}
//Contact* contact = ce.contact;
//// Has this contact already been added to the island?
//if (contact.m_flags & ContactFlags.e_islandFlag)
//{
// continue;
//}
//// Only add static, kinematic, or bullet bodies.
//Body* other = ce.other;
//if (other.m_type == _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.
//Sweep backup = other.m_sweep;
//if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0)
//{
// other.Advance(minAlpha);
//}
//// Update the contact points
//contact.Update(m_contactManager.m_contactListener);
//// Was the contact disabled by the user?
//if (contact.IsEnabled() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Are there contact points?
//if (contact.IsTouching() == false)
//{
// other.m_sweep = backup;
// other.SynchronizeTransform();
// continue;
//}
//// Add the contact to the island
//contact.m_flags |= ContactFlags.e_islandFlag;
//island.Add(contact);
//// Has the other body already been added to the island?
//if (other.m_flags & Body.BodyFlags.e_islandFlag)
//{
// continue;
//}
//// Add the other body to the island.
//other.m_flags |= Body.BodyFlags.e_islandFlag;
//if (other.m_type != _staticBody)
//{
// other.SetAwake(true);
//}
//island.Add(other);
}
}
}
TimeStep 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_bodies.Count(); ++i)
{
throw new NotImplementedException();
//Body* body = island.m_bodies[i];
//body.m_flags &= ~Body.BodyFlags.e_islandFlag;
//if (body.m_type != _dynamicBody)
//{
// continue;
//}
//body.SynchronizeFixtures();
//// Invalidate all contact TOIs on this displaced body.
//for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next)
//{
// ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.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;
}
}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0.Set(24.0f, -60.0f);
sweepA.a0 = 2.95f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter.SetZero();
Sweep sweepB = new Sweep();
sweepB.c0.Set(53.474274f, -50.252514f);
sweepB.a0 = 513.36676f; // - 162.0f * (float)Math.PI;
sweepB.c.Set(54.595478f, -51.083473f);
sweepB.a = 513.62781f; // - 162.0f * (float)Math.PI;
sweepB.localCenter.SetZero();
//sweepB.a0 -= 300.0f * (float)Math.PI;
//sweepB.a -= 300.0f * (float)Math.PI;
TOIInput input = new TOIInput();
input.proxyA.Set(m_shapeA, 0);
input.proxyB.Set(m_shapeB, 0);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
m_debugDraw.DrawString("toi = {0}", output.t);
m_debugDraw.DrawString("max toi iters = {0}, max root iters = {1}", Utilities._toiMaxIters, Utilities._toiMaxRootIters);
Vec2[] vertices = new Vec2[Settings._maxPolygonVertices];
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < m_shapeA.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformA, m_shapeA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, Color.FromArgb(225, 225, 225));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
//Vec2 localPoint(2.0f, -0.1f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 225, 128));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 175, 225));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 128, 128));
#if ZERO
for (float t = 0.0f; t < 1.0f; t += 0.1f)
{
sweepB.GetTransform(out transformB, t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 0.5f, 0.5f));
}
#endif
}
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use Distance to compute the contact point and normal at the time of impact.
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static void TimeOfImpact(out TOIOutput output, TOIInput input)
{
Timer timer = new Timer();
++_toiCalls;
output.state = TOIOutput.State.e_unknown;
output.t = input.tMax;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Sweep sweepA = input.sweepA;
Sweep 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.m_radius + proxyB.m_radius;
float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
float tolerance = 0.25f * Settings._linearSlop;
Utilities.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20; // TODO_ERIN Settings
int iter = 0;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceInput 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).
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Utilities.Distance(out distanceOutput, cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = TOIOutput.State.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = TOIOutput.State.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
throw new NotImplementedException();
// SeparationFunction fcn;
// fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
//#if ZERO
// // Dump the curve seen by the root finder
// {
// const int N = 100;
// float dx = 1.0f / N;
// float xs[N+1];
// float fs[N+1];
// float x = 0.0f;
// for (int i = 0; i <= N; ++i)
// {
// sweepA.GetTransform(out xfA, x);
// sweepB.GetTransform(out xfB, x);
// float f = fcn.Evaluate(xfA, xfB) - target;
// printf("%g %g\n", x, f);
// xs[i] = x;
// fs[i] = f;
// x += dx;
// }
// }
//#endif
// // Compute the TOI on the separating axis. We do this by successively
// // resolving the deepest point. This loop is bounded by the number of vertices.
// bool done = false;
// float t2 = tMax;
// int pushBackIter = 0;
// for (;;)
// {
// // Find the deepest point at t2. Store the witness point indices.
// int indexA, indexB;
// float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// // Is the final configuration separated?
// if (s2 > target + tolerance)
// {
// // Victory!
// output.state = TOIOutput.State.e_separated;
// output.t = tMax;
// done = true;
// break;
// }
// // Has the separation reached tolerance?
// if (s2 > target - tolerance)
// {
// // Advance the sweeps
// t1 = t2;
// break;
// }
// // Compute the initial separation of the witness points.
// float s1 = fcn.Evaluate(indexA, indexB, t1);
// // Check for initial overlap. This might happen if the root finder
// // runs out of iterations.
// if (s1 < target - tolerance)
// {
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// done = true;
// break;
// }
// // Check for touching
// if (s1 <= target + tolerance)
// {
// // Victory! t1 should hold the TOI (could be 0.0).
// output.state = TOIOutput.State.e_touching;
// output.t = t1;
// done = true;
// break;
// }
// // Compute 1D root of: f(x) - target = 0
// int rootIterCount = 0;
// float a1 = t1, a2 = t2;
// for (;;)
// {
// // Use a mix of the secant rule and bisection.
// float t;
// if (rootIterCount & 1)
// {
// // Secant rule to improve convergence.
// t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
// }
// else
// {
// // Bisection to guarantee progress.
// t = 0.5f * (a1 + a2);
// }
// ++rootIterCount;
// ++_toiRootIters;
// float s = fcn.Evaluate(indexA, indexB, t);
// if (Math.Abs(s - target) < tolerance)
// {
// // t2 holds a tentative value for t1
// t2 = t;
// break;
// }
// // Ensure we continue to bracket the root.
// if (s > target)
// {
// a1 = t;
// s1 = s;
// }
// else
// {
// a2 = t;
// s2 = s;
// }
// if (rootIterCount == 50)
// {
// break;
// }
// }
// _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);
// ++pushBackIter;
// if (pushBackIter == Settings._maxPolygonVertices)
// {
// break;
// }
// }
// ++iter;
// ++_toiIters;
// if (done)
// {
// break;
// }
// if (iter == k_maxIterations)
// {
// // Root finder got stuck. Semi-victory.
// output.state = TOIOutput.State.e_failed;
// output.t = t1;
// break;
// }
}
_toiMaxIters = Math.Max(_toiMaxIters, iter);
float time = timer.GetMilliseconds();
_toiMaxTime = Math.Max(_toiMaxTime, time);
_toiTime += time;
}
// TODO_ERIN might not need to return the separation
float Initialize(SimplexCache cache,
DistanceProxy proxyA, Sweep sweepA,
DistanceProxy proxyB, Sweep sweepB,
float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Utilities.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
Transform xfA, xfB;
m_sweepA.GetTransform(out xfA, t1);
m_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
m_type = SeparationType.e_points;
Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
Vec2 pointB = Utilities.Mul(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;
Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
Vec2 pointB = Utilities.Mul(xfB, m_localPoint);
Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vec2 pointA = Utilities.Mul(xfA, localPointA);
float s = Utilities.Dot(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;
Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
Vec2 normal = Utilities.Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
Vec2 pointA = Utilities.Mul(xfA, m_localPoint);
Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
Vec2 pointB = Utilities.Mul(xfB, localPointB);
float s = Utilities.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
