public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
return false;
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f;//minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return res.HasResult;
}
/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin = 0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon))
{
return(false);
}
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return(false);
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
}
return(false);
}
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
_simplexSolver.Reset();
// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
float boundingRadiusA = _convexA.GetAngularMotionDisc();
float boundingRadiusB = _convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
angVelB.Length() * boundingRadiusB;
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1f, 0f, 0f);
int maxIter = MaxIterations;
Vector3 n = new Vector3();
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin=0f;
//result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
gjk.setIgnoreMargin(true);
input.TransformA = fromA;
input.TransformB = fromB;
DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
hasResult = pointCollector1.HasResult;
c = pointCollector1.PointInWorld;
if (hasResult)
{
float dist;
dist = pointCollector1.Distance;
n = pointCollector1.NormalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
return false; //todo: report a failure
float dLambda = 0f;
//calculate safe moving fraction from distance / (linear+rotational velocity)
//float clippedDist = GEN_min(angularConservativeRadius,dist);
//float clippedDist = dist;
float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f) return false;
if (lambda < 0f) return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
break;
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = interpolatedTransA;
input.TransformB = interpolatedTransB;
// !!!!!!!!!!
r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0f)
{
//degenerate ?!
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
c = pointCollector.PointInWorld;
dist = pointCollector.Distance;
}
else
{
//??
return false;
}
}
result.Fraction = lambda;
result.Normal = n;
return true;
}
return false;
}
/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin=0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda) break;
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return false;
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
}
return false;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
{
return(false);
}
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f; //minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return(res.HasResult);
}
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
_simplexSolver.Reset();
// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
float boundingRadiusA = _convexA.GetAngularMotionDisc();
float boundingRadiusB = _convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
angVelB.Length() * boundingRadiusB;
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1f, 0f, 0f);
int maxIter = MaxIterations;
Vector3 n = new Vector3();
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin = 0f;
//result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
gjk.setIgnoreMargin(true);
input.TransformA = fromA;
input.TransformB = fromB;
DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
hasResult = pointCollector1.HasResult;
c = pointCollector1.PointInWorld;
if (hasResult)
{
float dist;
dist = pointCollector1.Distance;
n = pointCollector1.NormalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
//calculate safe moving fraction from distance / (linear+rotational velocity)
//float clippedDist = GEN_min(angularConservativeRadius,dist);
//float clippedDist = dist;
float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f)
{
return(false);
}
if (lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = interpolatedTransA;
input.TransformB = interpolatedTransB;
// !!!!!!!!!!
r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0f)
{
//degenerate ?!
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = pointCollector.Distance;
}
else
{
//??
return(false);
}
}
result.Fraction = lambda;
result.Normal = n;
return(true);
}
return(false);
}