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); } } }
/// <summary> /// 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 GjkPairDetector. /// </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 convex = new MinkowskiSumShape(_convexA, _convexB); Matrix rayFromLocalA; Matrix rayToLocalA; rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; rayToLocalA = MathHelper.InvertMatrix(toA) * toB; _simplexSolver.Reset(); convex.TransformB = rayFromLocalA; float lambda = 0; //todo: need to verify this: //because of minkowski difference, we need the inverse direction Vector3 s = -rayFromLocalA.Translation; Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); Vector3 x = s; Vector3 v; Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); v = x - arbitraryPoint; int maxIter = MaxIterations; Vector3 n = new Vector3(); float lastLambda = lambda; float dist2 = v.LengthSquared(); float epsilon = 0.0001f; Vector3 w, p; float VdotR; while ((dist2 > epsilon) && (maxIter-- != 0)) { p = convex.LocalGetSupportingVertex(v); w = x - p; float VdotW = Vector3.Dot(v, w); if (VdotW > 0) { VdotR = Vector3.Dot(v, r); if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) { return(false); } else { lambda = lambda - VdotW / VdotR; x = s + lambda * r; _simplexSolver.Reset(); //check next line w = x - p; lastLambda = lambda; n = v; } } _simplexSolver.AddVertex(w, x, p); if (_simplexSolver.Closest(out v)) { dist2 = v.LengthSquared(); } else { dist2 = 0f; } } result.Fraction = lambda; result.Normal = n; return(true); }