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); }