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 = (SphereShape)sphereObj.GetCollisionShape();
TriangleShape triangle = (TriangleShape)triObj.GetCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector = new SphereTriangleDetector(sphere,triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = new ClosestPointInput();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = sphereObj.GetWorldTransform();
input.m_transformB = triObj.GetWorldTransform();
bool swapResults = m_swapped;
detector.GetClosestPoints(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 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Zero;
Vector3 pointOnBWorld = Vector3.Zero;
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
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);
}
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public void GetClosestPointsNonVirtual(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
Vector3 normalInB = Vector3.Zero;
Vector3 pointOnA = Vector3.Zero, pointOnB = Vector3.Zero;
Matrix localTransA = input.m_transformA;
Matrix localTransB = input.m_transformB;
Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * .5f;
localTransA.Translation -= positionOffset;
localTransB.Translation -= 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 Vector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
if (BulletGlobals.g_streamWriter != null && debugGJK)
{
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++;
if (gNumGjkChecks == 3 && count == 4)
{
int ibreak = 0;
}
Vector3 seperatingAxisInA = MathUtil.TransposeTransformNormal(-m_cachedSeparatingAxis, input.m_transformA);
Vector3 seperatingAxisInB = MathUtil.TransposeTransformNormal(m_cachedSeparatingAxis, input.m_transformB);
#if true
Vector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
Vector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
#else
Vector3 pInA = m_minkowskiA.localGetSupportingVertexWithoutMargin(ref seperatingAxisInA);
Vector3 qInB = m_minkowskiB.localGetSupportingVertexWithoutMargin(ref seperatingAxisInB);
#if TEST_NON_VIRTUAL
Vector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
Vector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
Debug.Assert((pInAv-pInA).Length() < 0.0001);
Debug.Assert((qInBv-qInB).Length() < 0.0001);
#endif //
#endif
Vector3 pWorld = Vector3.Transform(pInA, localTransA);
Vector3 qWorld = Vector3.Transform(qInB, localTransB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
Vector3 w = pWorld - qWorld;
delta = Vector3.Dot(m_cachedSeparatingAxis, w);
if (BulletGlobals.g_streamWriter != null && debugGJK)
{
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);
}
if (m_simplexSolver.NumVertices() == 3)
{
int ibreak = 0;
}
// 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)
Vector3 newCachedSeparatingAxis = new Vector3();
if (!m_simplexSolver.Closest(ref 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 && debugGJK)
{
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 //
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//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;
}
//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(ref pointOnA, ref pointOnB);
normalInB = pointOnA - pointOnB;
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)System.Math.Sqrt((float)lenSqr );
//normalInB.Normalize();
normalInB *= rlen;
float s = (float)System.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.
Vector3 tmpPointOnA = Vector3.Zero, tmpPointOnB = Vector3.Zero;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis = Vector3.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 && debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
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)
{
Vector3 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.Normalize();
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;
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 (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
Vector3 temp = pointOnB + positionOffset;
output.AddContactPoint(
ref normalInB,
ref temp,
distance);
}
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();
float minProj = float.MaxValue;
Vector3 minNorm = Vector3.Zero;
Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm = Vector3.Up;
convexA.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm = Vector3.Up;
convexB.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, 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++)
{
Vector3 norm = Vector3.Zero;
convexA.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
Vector3 norm = Vector3.Zero;
convexB.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
seperatingAxisInB = Vector3.TransformNormal(norm, transB);
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = Vector3.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)
{
Vector3 color = new Vector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new Vector3(1,1,1);
Vector3 vec = minB-minA;
float prj2 = Vector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
ClosestPointInput input = new ClosestPointInput();
Vector3 newOrg = transA.Translation + offset;
Matrix displacedTrans = transA;
displacedTrans.Translation = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
Vector3 temp = -minNorm;
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(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)
{
Vector3 color = new Vector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
public virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(input, output, debugDraw, false);
}
public virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(input, output, debugDraw);
}
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 = (BoxShape)col0.GetCollisionShape();
BoxShape box1 = (BoxShape)col1.GetCollisionShape();
//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 = new ClosestPointInput();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector detector = new BoxBoxDetector(box0,box1);
detector.GetClosestPoints(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
}
// Work in progress to copy redo the box detector to remove un-necessary allocations
#if true
public virtual void GetClosestPoints(ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
Matrix transformA = input.m_transformA;
Matrix transformB = input.m_transformB;
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
int skip = 0;
Object contact = null;
Matrix rotateA = Matrix.Identity;
rotateA.Backward = transformA.Backward;
rotateA.Right = transformA.Right;
rotateA.Up = transformA.Up;
Matrix rotateB = Matrix.Identity;
rotateB.Backward = transformB.Backward;
rotateB.Right = transformB.Right;
rotateB.Up = transformB.Up;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA.Translation);
IndexedVector3 translationB = new IndexedVector3(transformB.Translation);
Vector3 debugExtents = new Vector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * m_box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * m_box2.GetHalfExtentsWithMargin());
//Vector3 box1Margin = 2f * debugExtents;
//Vector3 box2Margin = 2f * debugExtents;
rotateA = Matrix.Transpose(rotateA);
rotateB = Matrix.Transpose(rotateB);
float[] temp1 = s_temp1;
float[] temp2 = s_temp2;
temp1[0] = rotateA.M11;
temp1[1] = rotateA.M12;
temp1[2] = rotateA.M13;
temp1[4] = rotateA.M21;
temp1[5] = rotateA.M22;
temp1[6] = rotateA.M23;
temp1[8] = rotateA.M31;
temp1[9] = rotateA.M32;
temp1[10] = rotateA.M33;
temp2[0] = rotateB.M11;
temp2[1] = rotateB.M12;
temp2[2] = rotateB.M13;
temp2[4] = rotateB.M21;
temp2[5] = rotateB.M22;
temp2[6] = rotateB.M23;
temp2[8] = rotateB.M31;
temp2[9] = rotateB.M32;
temp2[10] = rotateB.M33;
DBoxBox2(ref translationA,
temp1,
ref box1Margin,
ref translationB,
temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
Matrix transformA = input.m_transformA;
Matrix transformB = input.m_transformB;
Vector3 point = Vector3.Zero, normal = Vector3.Up;
float timeOfImpact = 1f;
float depth = 0f;
// output.m_distance = float(1e30);
//move sphere into triangle space
Matrix sphereInTr = MathUtil.InverseTimes(transformB,transformA);
Vector3 temp = sphereInTr.Translation;
if (Collide(ref temp,ref point,ref normal,ref depth,ref timeOfImpact,m_contactBreakingThreshold))
{
if (swapResults)
{
Vector3 normalOnB = Vector3.TransformNormal(normal,transformB);
Vector3 normalOnA = -normalOnB;
Vector3 pointOnA = Vector3.Transform(point,transformB)+normalOnB*depth;
output.AddContactPoint(ref normalOnA,ref pointOnA,depth);
}
else
{
Vector3 p = Vector3.TransformNormal(normal, transformB);
Vector3 p2 = Vector3.Transform(point, transformB);
output.AddContactPoint(ref p,ref p2,depth);
}
}
}
public virtual bool CalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = Vector3.Zero, angVelA = Vector3.Zero, linVelB = Vector3.Zero, angVelB = Vector3.Zero;
TransformUtil.CalculateVelocity(ref fromA,ref toA,1f,ref linVelA,ref angVelA);
TransformUtil.CalculateVelocity(ref fromB,ref toB,1f,ref linVelB,ref angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
Vector3 relLinVel = (linVelB-linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return false;
}
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1,0,0);
int maxIter = MAX_ITERATIONS;
Vector3 n = Vector3.Zero;
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
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(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(input,pointCollector1,null,false);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
Vector3 colour = new Vector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
float dLambda = 0f;
projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//calculate safe moving fraction from distance / (linear+rotational velocity)
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=MathUtil.SIMD_EPSILON)
{
return false;
}
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
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);
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = Matrix.Identity, interpolatedTransB = Matrix.Identity, relativeTrans = Matrix.Identity;
TransformUtil.IntegrateTransform(ref fromA,ref linVelA,ref angVelA,lambda,ref interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB,ref linVelB,ref angVelB,lambda,ref interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = MathUtil.InverseTimes(ref interpolatedTransB, ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA.Translation, 0.2f, new Vector3(1, 0, 0));
}
result.DebugDraw( lambda );
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.GetClosestPoints(input,pointCollector,null,false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
//degenerate ?!
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;//.setValue(1,1,1);// = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= result.m_allowedPenetration)//SIMD_EPSILON)
{
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
/*
//todo:
//if movement away from normal, discard result
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < btScalar(1.))
{
if (move.dot(result.m_normal) <= btScalar(0.))
{
}
}
*/
}
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 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Up;
Vector3 pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape) min0;
CapsuleShape capsuleB = (CapsuleShape) min1;
Vector3 localScalingA = capsuleA.GetLocalScaling();
Vector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(ref normalOnB,ref 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 = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(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.Margin + min1.Margin + 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();
gjkPairDetector.GetClosestPoints(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)
{
Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;
Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref 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;
}
Matrix unPerturbedTransform = Matrix.Identity;
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)
{
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
//input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.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.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#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(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();
}
}
