public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (_manifold == null) { //swapped? _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); _ownManifold = true; } resultOut.SetPersistentManifold(_manifold); ConvexShape min0 = bodyA.CollisionShape as ConvexShape; ConvexShape min1 = bodyB.CollisionShape as ConvexShape; GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); //TODO: if (dispatchInfo.m_useContinuous) _gjkPairDetector.setMinkowskiA(min0); _gjkPairDetector.setMinkowskiB(min1); input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; input.MaximumDistanceSquared *= input.MaximumDistanceSquared; // input.m_maximumDistanceSquared = 1e30f; input.TransformA = bodyA.WorldTransform; input.TransformB = bodyB.WorldTransform; _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); }
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (_manifold == null) { //swapped? _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); _ownManifold = true; } resultOut.SetPersistentManifold(_manifold); ConvexShape min0 = bodyA.CollisionShape as ConvexShape; ConvexShape min1 = bodyB.CollisionShape as ConvexShape; GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); //TODO: if (dispatchInfo.m_useContinuous) _gjkPairDetector.setMinkowskiA(min0); _gjkPairDetector.setMinkowskiB(min1); input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; input.MaximumDistanceSquared *= input.MaximumDistanceSquared; // input.m_maximumDistanceSquared = 1e30f; input.TransformA = bodyA.WorldTransform; input.TransformB = bodyB.WorldTransform; _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); }
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 ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (_manifold == null) return; SphereShape sphere = bodyA.CollisionShape as SphereShape; TriangleShape triangle = bodyB.CollisionShape as TriangleShape; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.SetPersistentManifold(_manifold); SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds input.TransformA = bodyA.WorldTransform; input.TransformB = bodyB.WorldTransform; detector.GetClosestPoints(input, resultOut, null); }
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (_manifold == null) { return; } SphereShape sphere = bodyA.CollisionShape as SphereShape; TriangleShape triangle = bodyB.CollisionShape as TriangleShape; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.SetPersistentManifold(_manifold); SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); input.MaximumDistanceSquared = 1e30f; //todo: tighter bounds input.TransformA = bodyA.WorldTransform; input.TransformB = bodyB.WorldTransform; detector.GetClosestPoints(input, resultOut, null); }
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; }
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> /// 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); }