public virtual bool CalcTimeOfImpact(IndexedMatrix fromA, IndexedMatrix toA, IndexedMatrix fromB, IndexedMatrix toB, CastResult result)
{
return CalcTimeOfImpact(ref fromA, ref toA, ref fromB, ref toB, result);
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
IndexedVector3 linVelA, linVelB;
linVelA = toA._origin - fromA._origin;
linVelB = toB._origin - fromB._origin;
float radius = 0.001f;
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
IndexedVector3 r = (linVelA - linVelB);
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
IndexedMatrix identityTrans = IndexedMatrix.Identity;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector = new PointCollector();
using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
{
gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(ref input, pointCollector, null, false);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
float dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
float dLambda = 0f;
float projectedLinearVelocity = IndexedVector3.Dot(r, n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > 1f || lambda < 0f)
{
return false;
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw(lambda);
input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
gjk.GetClosestPoints(ref input, pointCollector, null, false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
return false;
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
{
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
}
return false;
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
IndexedVector3 linVelA, linVelB;
linVelA = toA._origin - fromA._origin;
linVelB = toB._origin - fromB._origin;
float radius = 0.001f;
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
IndexedVector3 r = (linVelA - linVelB);
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
IndexedMatrix identityTrans = IndexedMatrix.Identity;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector = new PointCollector();
using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
{
gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(ref input, pointCollector, null, false);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
float dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
float projectedLinearVelocity = IndexedVector3.Dot(r, n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > 1f || lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return(false);
//n.setValue(0,0,0);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw(lambda);
input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
gjk.GetClosestPoints(ref input, pointCollector, null, false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return(true);
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
return(false);
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
{
return(false);
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return(true);
}
}
return(false);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return(resultFraction);
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return(resultFraction);
}
public virtual bool CalcTimeOfImpact(IndexedMatrix fromA, IndexedMatrix toA, IndexedMatrix fromB, IndexedMatrix toB, CastResult result)
{
return(CalcTimeOfImpact(ref fromA, ref toA, ref fromB, ref toB, result));
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
/// compute linear and angular velocity for this interval, to interpolate
IndexedVector3 linVelA, angVelA, linVelB, angVelB;
TransformUtil.CalculateVelocity(ref fromA, ref toA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocity(ref fromB, ref toB, 1f, out linVelB, out angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB1 != null ? m_convexB1.GetAngularMotionDisc() : 0.0f;
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return false;
}
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
float radius = 0.001f;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
{
ComputeClosestPoints(ref fromA, ref fromB, pointCollector1);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return false;
}
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
IndexedVector3 colour = new IndexedVector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
float dLambda = 0f;
projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return false;
}
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return false;
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
IndexedMatrix interpolatedTransA = IndexedMatrix.Identity, interpolatedTransB = IndexedMatrix.Identity, relativeTrans = IndexedMatrix.Identity;
TransformUtil.IntegrateTransform(ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = interpolatedTransB.InverseTimes(ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA._origin, 0.2f, new IndexedVector3(1, 0, 0));
}
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
ComputeClosestPoints(ref interpolatedTransA, ref interpolatedTransB, pointCollector);
if (pointCollector.m_hasResult)
{
dist = pointCollector.m_distance + result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
result.ReportFailure(-1, numIter);
return false;
}
numIter++;
if (numIter > maxIter)
{
result.ReportFailure(-2, numIter);
return false;
}
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
}
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
IndexedVector3 linVelA = toA._origin - fromA._origin;
IndexedVector3 linVelB = toB._origin - fromB._origin;
float lambda = 0f;
IndexedMatrix interpolatedTransA = fromA;
IndexedMatrix interpolatedTransB = fromB;
///take relative motion
IndexedVector3 r = (linVelA - linVelB);
IndexedVector3 v;
IndexedVector3 supVertexA = fromA * m_convexA.LocalGetSupportingVertex(-r * fromA._basis);
IndexedVector3 supVertexB = fromB * m_convexB.LocalGetSupportingVertex(r * fromB._basis);
v = supVertexA - supVertexB;
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
IndexedVector3 w, p;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- > 0))
{
supVertexA = interpolatedTransA * (m_convexA.LocalGetSupportingVertex(-v * interpolatedTransA._basis));
supVertexB = interpolatedTransB * (m_convexB.LocalGetSupportingVertex(v * interpolatedTransB._basis));
w = supVertexA - supVertexB;
float VdotW = IndexedVector3.Dot(v, w);
if (lambda > 1.0f)
{
return false;
}
if (VdotW > 0f)
{
VdotR = IndexedVector3.Dot(v, r);
if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
return false;
}
else
{
lambda = lambda - VdotW / VdotR;
//interpolate to next lambda
// x = s + lambda * r;
interpolatedTransA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
interpolatedTransB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
//m_simplexSolver.reset();
//check next line
w = supVertexA - supVertexB;
lastLambda = lambda;
n = v;
hasResult = true;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
if (!m_simplexSolver.InSimplex(ref w))
{
m_simplexSolver.AddVertex(ref w, ref supVertexA, ref supVertexB);
}
if (m_simplexSolver.Closest(out v))
{
dist2 = v.LengthSquared();
hasResult = true;
//todo: check this normal for validity
//n=v;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
//printf("DIST2=%f\n",dist2);
//printf("numverts = %i\n",m_simplexSolver.numVertices());
}
else
{
dist2 = 0f;
}
}
//int numiter = MAX_ITERATIONS - maxIter;
// printf("number of iterations: %d", numiter);
//don't report a time of impact when moving 'away' from the hitnormal
result.m_fraction = lambda;
if (n.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
result.m_normal = IndexedVector3.Normalize(n);
}
else
{
result.m_normal = IndexedVector3.Zero;
}
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(result.m_normal, r) >= -result.m_allowedPenetration)
{
return false;
}
IndexedVector3 hitA, hitB;
m_simplexSolver.ComputePoints(out hitA, out hitB);
result.m_hitPoint = hitB;
return true;
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
/// compute linear and angular velocity for this interval, to interpolate
IndexedVector3 linVelA, angVelA, linVelB, angVelB;
TransformUtil.CalculateVelocity(ref fromA, ref toA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocity(ref fromB, ref toB, 1f, out linVelB, out angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB1 != null?m_convexB1.GetAngularMotionDisc() : 0.0f;
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return(false);
}
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
float radius = 0.001f;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
{
ComputeClosestPoints(ref fromA, ref fromB, pointCollector1);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return(false);
}
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
IndexedVector3 colour = new IndexedVector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
float dLambda = 0f;
projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return(false);
}
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return(false);
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
IndexedMatrix interpolatedTransA = IndexedMatrix.Identity, interpolatedTransB = IndexedMatrix.Identity, relativeTrans = IndexedMatrix.Identity;
TransformUtil.IntegrateTransform(ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = interpolatedTransB.InverseTimes(ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA._origin, 0.2f, new IndexedVector3(1, 0, 0));
}
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
ComputeClosestPoints(ref interpolatedTransA, ref interpolatedTransB, pointCollector);
if (pointCollector.m_hasResult)
{
dist = pointCollector.m_distance + result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
result.ReportFailure(-1, numIter);
return(false);
}
numIter++;
if (numIter > maxIter)
{
result.ReportFailure(-2, numIter);
return(false);
}
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return(true);
}
return(false);
}
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
IndexedVector3 linVelA = toA._origin - fromA._origin;
IndexedVector3 linVelB = toB._origin - fromB._origin;
float lambda = 0f;
IndexedMatrix interpolatedTransA = fromA;
IndexedMatrix interpolatedTransB = fromB;
///take relative motion
IndexedVector3 r = (linVelA - linVelB);
IndexedVector3 v;
IndexedVector3 supVertexA = fromA * m_convexA.LocalGetSupportingVertex(-r * fromA._basis);
IndexedVector3 supVertexB = fromB * m_convexB.LocalGetSupportingVertex(r * fromB._basis);
v = supVertexA - supVertexB;
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
IndexedVector3 w, p;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- > 0))
{
supVertexA = interpolatedTransA * (m_convexA.LocalGetSupportingVertex(-v * interpolatedTransA._basis));
supVertexB = interpolatedTransB * (m_convexB.LocalGetSupportingVertex(v * interpolatedTransB._basis));
w = supVertexA - supVertexB;
float VdotW = IndexedVector3.Dot(v, w);
if (lambda > 1.0f)
{
return(false);
}
if (VdotW > 0f)
{
VdotR = IndexedVector3.Dot(v, r);
if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
//interpolate to next lambda
// x = s + lambda * r;
interpolatedTransA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
interpolatedTransB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
//m_simplexSolver.reset();
//check next line
w = supVertexA - supVertexB;
lastLambda = lambda;
n = v;
hasResult = true;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
if (!m_simplexSolver.InSimplex(ref w))
{
m_simplexSolver.AddVertex(ref w, ref supVertexA, ref supVertexB);
}
if (m_simplexSolver.Closest(out v))
{
dist2 = v.LengthSquared();
hasResult = true;
//todo: check this normal for validity
//n=v;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
//printf("DIST2=%f\n",dist2);
//printf("numverts = %i\n",m_simplexSolver.numVertices());
}
else
{
dist2 = 0f;
}
}
//int numiter = MAX_ITERATIONS - maxIter;
// printf("number of iterations: %d", numiter);
//don't report a time of impact when moving 'away' from the hitnormal
result.m_fraction = lambda;
if (n.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
result.m_normal = IndexedVector3.Normalize(n);
}
else
{
result.m_normal = IndexedVector3.Zero;
}
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(result.m_normal, r) >= -result.m_allowedPenetration)
{
return(false);
}
IndexedVector3 hitA, hitB;
m_simplexSolver.ComputePoints(out hitA, out hitB);
result.m_hitPoint = hitB;
return(true);
}