public static b2TOIInput Create()
{
var toi = new b2TOIInput();
toi.proxyA = b2DistanceProxy.Create();
toi.proxyB = b2DistanceProxy.Create();
toi.sweepA = new b2Sweep();
toi.sweepB = new b2Sweep();
return(toi);
}
// 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];
public static b2TOIInput Create()
{
var toi = new b2TOIInput();
toi.proxyA = b2DistanceProxy.Create();
toi.proxyB = b2DistanceProxy.Create();
toi.sweepA = new b2Sweep();
toi.sweepB = new b2Sweep();
return toi;
}
// 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];
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);
}
