public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
IndexedVector3 point, normal;
float timeOfImpact = 1f;
float depth = 0f;
// output.m_distance = float(1e30);
//move sphere into triangle space
IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA);
IndexedVector3 temp = sphereInTr._origin;
if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold))
{
if (swapResults)
{
IndexedVector3 normalOnB = transformB._basis * normal;
IndexedVector3 normalOnA = -normalOnB;
IndexedVector3 pointOnA = transformB * point + normalOnB * depth;
output.AddContactPoint(ref normalOnA, ref pointOnA, depth);
}
else
{
IndexedVector3 p = transformB._basis * normal;
IndexedVector3 p2 = transformB * point;
output.AddContactPoint(ref p, ref p2, depth);
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = new ManifoldResult();
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_swapped ? body1 : body0;
CollisionObject triObj = m_swapped ? body0 : body1;
SphereShape sphere = sphereObj.GetCollisionShape() as SphereShape;
TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
{
detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
sphereObj.GetWorldTransform(out input.m_transformA);
triObj.GetWorldTransform(out input.m_transformB);
bool swapResults = m_swapped;
detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB = IndexedVector3.Zero;
IndexedVector3 pointOnBWorld = IndexedVector3.Zero;
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
#endif
}
//BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
//resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = col0.GetCollisionShape() as BoxShape;
BoxShape box1 = col1.GetCollisionShape() as BoxShape;
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector.GetClosestPoints(box0, box1, ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
{
if (m_convexB1 != null)
{
m_simplexSolver.Reset();
GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.GetClosestPoints(ref input, pointCollector, null);
}
else
{
//convex versus plane
ConvexShape convexShape = m_convexA;
StaticPlaneShape planeShape = m_planeShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = transA;
IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform;
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal;
pointCollector.AddContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance);
}
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(ref input, output, debugDraw);
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(ref input, output, debugDraw, false);
}
public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
IndexedVector3 normalInB = IndexedVector3.Zero;
IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
IndexedMatrix localTransA = input.m_transformA;
IndexedMatrix localTransB = input.m_transformB;
IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;
IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin, ref positionOffset);
IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
//localTransB._origin -= positionOffset;
bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();
float marginA = m_marginA;
float marginB = m_marginB;
#if TEST_NON_VIRTUAL
float marginAv = m_minkowskiA.getMarginNonVirtual();
float marginBv = m_minkowskiB.getMarginNonVirtual();
Debug.Assert(marginA == marginAv);
Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL
gNumGjkChecks++;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
}
{
float squaredDistance = MathUtil.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.Reset();
int count = 0;
for (; ;)
//while (true)
{
count++;
IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;
IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
IndexedVector3 pWorld = localTransA * pInA;
IndexedVector3 qWorld = localTransB * qInB;
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X, pWorld.Y - qWorld.Y, pWorld.Z - qWorld.Z);
IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w, out delta);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
}
// potential exit, they don't overlap
if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.InSimplex(ref w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);
//calculate the closest point to the origin (update vector v)
IndexedVector3 newCachedSeparatingAxis;
if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.LengthSquared();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
}
#if false
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance > previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
{
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
//#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
// printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
// printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
// m_cachedSeparatingAxis.getX(),
// m_cachedSeparatingAxis.getY(),
// m_cachedSeparatingAxis.getZ(),
// squaredDistance,
// m_minkowskiA->getShapeType(),
// m_minkowskiB->getShapeType());
//#endif
break;
}
bool check = (!m_simplexSolver.FullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = m_cachedSeparatingAxis;
float lenSqr = m_cachedSeparatingAxis.LengthSquared();
//valid normal
if (lenSqr < 0.0001f)
{
m_degenerateSimplex = 5;
}
if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
//float rlen = 1 / normalInB.Length();
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
//normalInB.Normalize();
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
Debug.Assert(s > 0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail ref
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis = IndexedVector3.Zero;
bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
debugDraw
);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
}
if (isValid2)
{
IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.LengthSquared();
if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.LengthSquared();
}
if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(lenSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
//FIXME! check2d THIS
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
//isValid = false;
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.LengthSquared() > 0f)
{
float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA;
pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.Normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
}
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
IndexedVector3 temp = pointOnB + positionOffset;
output.AddContactPoint(
ref normalInB,
ref temp,
distance);
}
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
IndexedVector3 linVelA, linVelB;
linVelA = toA._origin - fromA._origin;
linVelB = toB._origin - fromB._origin;
float radius = 0.001f;
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
IndexedVector3 r = (linVelA - linVelB);
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
IndexedMatrix identityTrans = IndexedMatrix.Identity;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector = new PointCollector();
using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
{
gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(ref input, pointCollector, null, false);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
float dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
float projectedLinearVelocity = IndexedVector3.Dot(r, n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > 1f || lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return(false);
//n.setValue(0,0,0);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw(lambda);
input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
gjk.GetClosestPoints(ref input, pointCollector, null, false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return(true);
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
return(false);
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
{
return(false);
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return(true);
}
}
return(false);
}
//public BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1,BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
#endif
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
IndexedVector3 point, normal;
float timeOfImpact = 1f;
float depth = 0f;
// output.m_distance = float(1e30);
//move sphere into triangle space
IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA);
IndexedVector3 temp = sphereInTr._origin;
if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold))
{
if (swapResults)
{
IndexedVector3 normalOnB = transformB._basis * normal;
IndexedVector3 normalOnA = -normalOnB;
IndexedVector3 pointOnA = transformB * point + normalOnB * depth;
output.AddContactPoint(ref normalOnA, ref pointOnA, depth);
}
else
{
IndexedVector3 p = transformB._basis * normal;
IndexedVector3 p2 = transformB * point;
output.AddContactPoint(ref p, ref p2, depth);
}
}
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(ref input, output, debugDraw);
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(ref input, output, debugDraw, false);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist > MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return(false);
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}
//public BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
IndexedVector3 normalInB = IndexedVector3.Zero;
IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
IndexedMatrix localTransA = input.m_transformA;
IndexedMatrix localTransB = input.m_transformB;
IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;
IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin,ref positionOffset);
IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
//localTransB._origin -= positionOffset;
bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();
float marginA = m_marginA;
float marginB = m_marginB;
#if TEST_NON_VIRTUAL
float marginAv = m_minkowskiA.getMarginNonVirtual();
float marginBv = m_minkowskiB.getMarginNonVirtual();
Debug.Assert(marginA == marginAv);
Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL
gNumGjkChecks++;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
}
#endif
{
float squaredDistance = MathUtil.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.Reset();
int count = 0;
for (; ; )
//while (true)
{
count++;
IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;
IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
IndexedVector3 pWorld = localTransA * pInA;
IndexedVector3 qWorld = localTransB * qInB;
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X,pWorld.Y - qWorld.Y,pWorld.Z - qWorld.Z);
delta = IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
}
#endif
// potential exit, they don't overlap
if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.InSimplex(ref w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);
//calculate the closest point to the origin (update vector v)
IndexedVector3 newCachedSeparatingAxis;
if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.LengthSquared();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
}
#endif
#if false
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
{
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
//#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
// printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
// printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
// m_cachedSeparatingAxis.getX(),
// m_cachedSeparatingAxis.getY(),
// m_cachedSeparatingAxis.getZ(),
// squaredDistance,
// m_minkowskiA->getShapeType(),
// m_minkowskiB->getShapeType());
//#endif
break;
}
bool check = (!m_simplexSolver.FullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = m_cachedSeparatingAxis;
float lenSqr = m_cachedSeparatingAxis.LengthSquared();
//valid normal
if (lenSqr < 0.0001f)
{
m_degenerateSimplex = 5;
}
if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
//float rlen = 1 / normalInB.Length();
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
//normalInB.Normalize();
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
Debug.Assert(s > 0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail ref
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis = IndexedVector3.Zero;
bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
debugDraw
);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
}
#endif
if (isValid2)
{
IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.LengthSquared();
if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.LengthSquared();
}
if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(lenSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
//FIXME! check2d THIS
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
//isValid = false;
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.LengthSquared() > 0f)
{
float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA;
pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.Normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
#if DEBUG
if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
}
#endif
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
IndexedVector3 temp = pointOnB + positionOffset;
output.AddContactPoint(
ref normalInB,
ref temp,
distance);
}
}
