public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
Matrix transformA = input.TransformA;
Matrix transformB = input.TransformB;
Vector3 point = new Vector3();
Vector3 normal = new Vector3();
Single timeOfImpact = 1.0f;
Single depth = 0.0f;
//move sphere into triangle space
Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
{
output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
}
}
public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
float distance = 0;
Vector3 normalInB = new Vector3();
Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
Matrix localTransA = input.TransformA;
Matrix localTransB = input.TransformB;
Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
localTransA.Translation -= positionOffset;
localTransB.Translation -= positionOffset;
float marginA = _minkowskiA.Margin;
float marginB = _minkowskiB.Margin;
_numGjkChecks++;
if (_ignoreMargin)
{
marginA = 0;
marginB = 0;
}
_currentIteration = 0;
int gjkMaxIter = 1000;
_cachedSeparatingAxis = new Vector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
_degenerateSimplex = 0;
_lastUsedMethod = -1;
{
float squaredDistance = MathHelper.Infinity;
float delta = 0;
float margin = marginA + marginB;
_simplexSolver.Reset();
while (true)
{
Matrix transABasis = input.TransformA;
transABasis.Translation = Vector3.Zero;
Matrix transBBasis = input.TransformB;
transBBasis.Translation = Vector3.Zero;
Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
Vector3 w = pWorld - qWorld;
delta = Vector3.Dot(_cachedSeparatingAxis, w);
if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
{
checkPenetration = false;
break;
}
if (_simplexSolver.InSimplex(w))
{
_degenerateSimplex = 1;
checkSimplex = true;
break;
}
float f0 = squaredDistance - delta;
float f1 = squaredDistance * RelativeError2;
if (f0 <= f1)
{
if (f0 <= 0.0f)
{
_degenerateSimplex = 2;
}
checkSimplex = true;
break;
}
_simplexSolver.AddVertex(w, pWorld, qWorld);
if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
{
_degenerateSimplex = 3;
checkSimplex = true;
break;
}
float previouseSquaredDistance = squaredDistance;
squaredDistance = _cachedSeparatingAxis.LengthSquared();
if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
checkSimplex = true;
break;
}
if (_currentIteration++ > gjkMaxIter)
{
#if DEBUG
Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
_cachedSeparatingAxis.X,
_cachedSeparatingAxis.Y,
_cachedSeparatingAxis.Z,
squaredDistance,
_minkowskiA.ShapeType,
_minkowskiB.ShapeType
);
#endif
break;
}
bool check = (!_simplexSolver.FullSimplex);
if (!check)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
break;
}
}
if (checkSimplex)
{
_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = _cachedSeparatingAxis.LengthSquared();
if (lenSqr < 0.0001f)
{
_degenerateSimplex = 5;
}
if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
{
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
BulletDebug.Assert(s > 0);
pointOnA -= _cachedSeparatingAxis * (marginA / s);
pointOnB += _cachedSeparatingAxis * (marginB / s);
distance = ((1 / rlen) - margin);
isValid = true;
_lastUsedMethod = 1;
}
else
{
_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
#warning Check this
if (_penetrationDepthSolver != null)
{
Vector3 tmpPointOnA, tmpPointOnB;
_numDeepPenetrationChecks++;
bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
_simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
if (isValid2)
{
Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lengSqr = tmpNormalInB.LengthSquared();
if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
{
tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
_lastUsedMethod = 3;
}
else
{
}
}
else
{
_lastUsedMethod = 4;
}
}
else
{
_lastUsedMethod = 5;
}
}
}
if (isValid)
{
output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
}
}
}
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);
}
