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 static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray <IndexedVector3> worldVertsB1 = new ObjectArray <IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray <IndexedVector3> worldVertsB2 = new ObjectArray <IndexedVector3>();
ObjectArray <IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray <IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
{
return;
}
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS = planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray <IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth < -3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist < maxDist)
{
resultOut.AddContactPoint(ref separatingNormal, ref point, curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
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 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 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 virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(input, output, debugDraw);
}
public virtual void GetClosestPoints(ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(input, output, debugDraw, false);
}
private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
ref IndexedVector3 side1, ref IndexedVector3 p2,
float[] R2, ref IndexedVector3 side2,
ref IndexedVector3 normal, ref float depth, ref int return_code,
int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
{
Vector3 centerDifference = Vector3.Zero, ppv = Vector3.Zero;
float[] normalR = null;
int normalROffsetResult = 0;
IndexedVector3 A = side1 * 0.5f;
IndexedVector3 B = side2 * 0.5f;
int code;
bool invert_normal;
// get vector from centers of box 1 to box 2, relative to box 1
IndexedVector3 p = p2 - p1;
IndexedVector3 pp = new IndexedVector3();
DMULTIPLY1_331(ref pp, R1, ref p); // get pp = p relative to body 1
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
float R11 = DDOT44(R1, 0, R2, 0);
float R12 = DDOT44(R1, 0, R2, 1);
float R13 = DDOT44(R1, 0, R2, 2);
float R21 = DDOT44(R1, 1, R2, 0);
float R22 = DDOT44(R1, 1, R2, 1);
float R23 = DDOT44(R1, 1, R2, 2);
float R31 = DDOT44(R1, 2, R2, 0);
float R32 = DDOT44(R1, 2, R2, 1);
float R33 = DDOT44(R1, 2, R2, 2);
float Q11 = System.Math.Abs(R11);
float Q12 = System.Math.Abs(R12);
float Q13 = System.Math.Abs(R13);
float Q21 = System.Math.Abs(R21);
float Q22 = System.Math.Abs(R22);
float Q23 = System.Math.Abs(R23);
float Q31 = System.Math.Abs(R31);
float Q32 = System.Math.Abs(R32);
float Q33 = System.Math.Abs(R33);
float s = -float.MaxValue;
invert_normal = false;
code = 0;
int normalROffset = 0;
// separating axis = u1,u2,u3
if (TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal)) return 0;
// separating axis = v1,v2,v3
if (TST(DDOT41(R2, 0, ref p, 0), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 1, ref p, 0), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 2, ref p, 0), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal)) return 0;
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
// separating axis = u1 x (v1,v2,v3)
//private static bool TST2(float expr1,float expr2,ref Vector3 normal, ref Vector3 normalC,int cc,ref int code)
IndexedVector3 normalC = new IndexedVector3();
// separating axis = u1 x (v1,v2,v3)
if (TST2(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u2 x (v1,v2,v3)
if (TST2(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u3 x (v1,v2,v3)
if (TST2(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal)) return 0;
if (code == 0)
{
return 0;
}
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR != null)
{
normal[0] = normalR[0 + normalROffset];
normal[1] = normalR[4 + normalROffset];
normal[2] = normalR[8 + normalROffset];
}
else
{
DMULTIPLY0_331(ref normal, R1, ref normalC);
}
if (invert_normal)
{
normal = -normal;
}
depth = -s;
// compute contact point(s)
if (code > 6)
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
IndexedVector3 pa1 = p1;
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
for (int i = 0; i < 3; i++)
{
pa1[i] += sign * A[j] * R1[i * 4 + j];
}
}
// find a point pb on the intersecting edge of box 2
IndexedVector3 pb1 = p2;
//for (i = 0; i < 3; i++) pb[i] = p2[i];
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
for (int i = 0; i < 3; i++)
{
pb1[i] += sign * B[j] * R2[i * 4 + j];
}
}
float alpha = 0f, beta = 0f;
IndexedVector3 ua = new IndexedVector3();
IndexedVector3 ub = new IndexedVector3();
for (int i = 0; i < 3; i++)
{
ua[i] = R1[((code) - 7) / 3 + i * 4];
}
for (int i = 0; i < 3; i++)
{
ub[i] = R2[((code) - 7) % 3 + i * 4];
}
DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, ref alpha, ref beta);
for (int i = 0; i < 3; i++)
{
pa1[i] += ua[i] * alpha;
}
for (int i = 0; i < 3; i++)
{
pb1[i] += ub[i] * beta;
}
{
//contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
#if USE_CENTER_POINT
pointInWorld = (pa + pb) * 0.5f;
output.addContactPoint(-normal,pointInWorld,-depth);
#else
Vector3 pbv = pb1.ToVector3();
output.AddContactPoint((-normal).ToVector3(), pbv, -depth);
#endif //
return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
float[] Ra;
float[] Rb;
IndexedVector3 pa;
IndexedVector3 pb;
IndexedVector3 Sa;
IndexedVector3 Sb;
if (code <= 3)
{
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
IndexedVector3 normal2;
IndexedVector3 nr = new IndexedVector3();
IndexedVector3 anr;
if (code <= 3)
{
normal2 = normal;
}
else
{
normal2 = -normal;
}
DMULTIPLY1_331(ref nr, Rb, ref normal2);
nr.Abs(out anr);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if (anr[1] > anr[0])
{
if (anr[1] > anr[2])
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if (anr[0] > anr[2])
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
IndexedVector3 center = new IndexedVector3(); ;
if (nr[lanr] < 0)
{
for (int i = 0; i < 3; i++)
{
center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
}
}
else
{
for (int i = 0; i < 3; i++)
{
center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
}
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if (code <= 3)
{
codeN = code - 1;
}
else
{
codeN = code - 4;
}
if (codeN == 0)
{
code1 = 1;
code2 = 2;
}
else if (codeN == 1)
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
float[] quad = s_quad; // 2D coordinate of incident face (x,y pairs)
float c1, c2, m11, m12, m21, m22;
c1 = DDOT14(ref center, 0, Ra, code1);
c2 = DDOT14(ref center, 0, Ra, code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = DDOT44(Ra, code1, Rb, a1);
m12 = DDOT44(Ra, code1, Rb, a2);
m21 = DDOT44(Ra, code2, Rb, a1);
m22 = DDOT44(Ra, code2, Rb, a2);
{
float k1 = m11 * Sb[a1];
float k2 = m21 * Sb[a1];
float k3 = m12 * Sb[a2];
float k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
float[] rect = new float[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
float[] ret = s_ret;
int n = IntersectRectQuad2(rect, quad, ret);
if (n < 1)
{
return 0; // this should never happen
}
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
float[] point = s_point; // penetrating contact points
float[] dep = s_dep; // depths for those points
float det1 = 1f / (m11 * m22 - m12 * m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (int j = 0; j < n; j++)
{
float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (int i = 0; i < 3; i++)
{
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
}
dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
if (dep[cnum] >= 0)
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if (cnum < 1)
{
return 0; // this should never happen
}
// we can't generate more contacts than we actually have
if (maxc > cnum)
{
maxc = cnum;
}
if (maxc < 1)
{
maxc = 1;
}
if (cnum <= maxc)
{
if (code < 4)
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorldFA[i] = point[j * 3 + i] + pa[i];
}
Vector3 pointInWorld = pointInWorldFA.ToVector3();
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[j]);
}
}
else
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorld = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorld[i] = point[j * 3 + i] + pa[i];
}
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld.ToVector3());
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld.ToVector3(), -dep[j]);
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
float maxdepth = dep[0];
for (int i = 1; i < cnum; i++)
{
if (dep[i] > maxdepth)
{
maxdepth = dep[i];
i1 = i;
}
}
int[] iret = new int[8];
CullPoints2(cnum, ret, maxc, i1, iret);
for (int j = 0; j < maxc; j++)
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
IndexedVector3 posInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
}
Vector3 pointInWorld = posInWorldFA.ToVector3();
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[iret[j]]);
}
cnum = maxc;
}
return_code = code;
return cnum;
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>();
ObjectArray<IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray<IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
return;
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray<IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(ref input, output, debugDraw, false);
}
public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(ref input, output, debugDraw);
}
private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
ref IndexedVector3 side1, ref IndexedVector3 p2,
float[] R2, ref IndexedVector3 side2,
ref IndexedVector3 normal, ref float depth, ref int return_code,
int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
{
//IndexedVector3 centerDifference = IndexedVector3.Zero, ppv = IndexedVector3.Zero;
float[] normalR = null;
int normalROffsetResult = 0;
IndexedVector3 A = side1 * 0.5f;
IndexedVector3 B = side2 * 0.5f;
int code;
bool invert_normal;
// get vector from centers of box 1 to box 2, relative to box 1
IndexedVector3 p = p2 - p1;
IndexedVector3 pp = new IndexedVector3();
DMULTIPLY1_331(ref pp, R1, ref p); // get pp = p relative to body 1
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
float R11 = DDOT44(R1, 0, R2, 0);
float R12 = DDOT44(R1, 0, R2, 1);
float R13 = DDOT44(R1, 0, R2, 2);
float R21 = DDOT44(R1, 1, R2, 0);
float R22 = DDOT44(R1, 1, R2, 1);
float R23 = DDOT44(R1, 1, R2, 2);
float R31 = DDOT44(R1, 2, R2, 0);
float R32 = DDOT44(R1, 2, R2, 1);
float R33 = DDOT44(R1, 2, R2, 2);
float Q11 = Math.Abs(R11);
float Q12 = Math.Abs(R12);
float Q13 = Math.Abs(R13);
float Q21 = Math.Abs(R21);
float Q22 = Math.Abs(R22);
float Q23 = Math.Abs(R23);
float Q31 = Math.Abs(R31);
float Q32 = Math.Abs(R32);
float Q33 = Math.Abs(R33);
float s = -float.MaxValue;
invert_normal = false;
code = 0;
int normalROffset = 0;
// separating axis = u1,u2,u3
if (TST(pp.X, (A.X + B.X * Q11 + B.Y * Q12 + B.Z * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST(pp.Y, (A.Y + B.X * Q21 + B.Y * Q22 + B.Z * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST(pp.Z, (A.Z + B.X * Q31 + B.Y * Q32 + B.Z * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal))
{
return(0);
}
// separating axis = v1,v2,v3
if (TST(DDOT41(R2, 0, ref p, 0), (A.X * Q11 + A.Y * Q21 + A.Z * Q31 + B.X), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST(DDOT41(R2, 1, ref p, 0), (A.X * Q12 + A.Y * Q22 + A.Z * Q32 + B.Y), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST(DDOT41(R2, 2, ref p, 0), (A.X * Q13 + A.Y * Q23 + A.Z * Q33 + B.Z), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal))
{
return(0);
}
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
// separating axis = u1 x (v1,v2,v3)
//private static bool TST2(float expr1,float expr2,ref IndexedVector3 normal, ref IndexedVector3 normalC,int cc,ref int code)
IndexedVector3 normalC = new IndexedVector3();
float fudge2 = 1.0e-5f;
Q11 += fudge2;
Q12 += fudge2;
Q13 += fudge2;
Q21 += fudge2;
Q22 += fudge2;
Q23 += fudge2;
Q31 += fudge2;
Q32 += fudge2;
Q33 += fudge2;
// separating axis = u1 x (v1,v2,v3)
if (TST2(pp.Z * R21 - pp.Y * R31, (A.Y * Q31 + A.Z * Q21 + B.Y * Q13 + B.Z * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.Z * R22 - pp.Y * R32, (A.Y * Q32 + A.Z * Q22 + B.X * Q13 + B.Z * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.Z * R23 - pp.Y * R33, (A.Y * Q33 + A.Z * Q23 + B.X * Q12 + B.Y * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal))
{
return(0);
}
// separating axis = u2 x (v1,v2,v3)
if (TST2(pp.X * R31 - pp.Z * R11, (A.X * Q31 + A.Z * Q11 + B.Y * Q23 + B.Z * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.X * R32 - pp.Z * R12, (A.X * Q32 + A.Z * Q12 + B.X * Q23 + B.Z * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.X * R33 - pp.Z * R13, (A.X * Q33 + A.Z * Q13 + B.X * Q22 + B.Y * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal))
{
return(0);
}
// separating axis = u3 x (v1,v2,v3)
if (TST2(pp.Y * R11 - pp.X * R21, (A.X * Q21 + A.Y * Q11 + B.Y * Q33 + B.Z * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.Y * R12 - pp.X * R22, (A.X * Q22 + A.Y * Q12 + B.X * Q33 + B.Z * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal))
{
return(0);
}
if (TST2(pp.Y * R13 - pp.X * R23, (A.X * Q23 + A.Y * Q13 + B.X * Q32 + B.Y * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal))
{
return(0);
}
if (code == 0)
{
return(0);
}
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR != null)
{
normal.X = normalR[0 + normalROffset];
normal.Y = normalR[4 + normalROffset];
normal.Z = normalR[8 + normalROffset];
}
else
{
DMULTIPLY0_331(ref normal, R1, ref normalC);
}
if (invert_normal)
{
normal = -normal;
}
depth = -s;
// compute contact point(s)
if (code > 6)
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
IndexedVector3 pa1 = p1;
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
for (int i = 0; i < 3; i++)
{
pa1[i] += sign * A[j] * R1[i * 4 + j];
}
}
// find a point pb on the intersecting edge of box 2
IndexedVector3 pb1 = p2;
//for (i = 0; i < 3; i++) pb[i] = p2[i];
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
for (int i = 0; i < 3; i++)
{
pb1[i] += sign * B[j] * R2[i * 4 + j];
}
}
float alpha, beta;
IndexedVector3 ua = new IndexedVector3();
IndexedVector3 ub = new IndexedVector3();
for (int i = 0; i < 3; i++)
{
ua[i] = R1[((code) - 7) / 3 + i * 4];
}
for (int i = 0; i < 3; i++)
{
ub[i] = R2[((code) - 7) % 3 + i * 4];
}
DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, out alpha, out beta);
for (int i = 0; i < 3; i++)
{
pa1[i] += ua[i] * alpha;
}
for (int i = 0; i < 3; i++)
{
pb1[i] += ub[i] * beta;
}
{
//contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
#if USE_CENTER_POINT
pointInWorld = (pa + pb) * 0.5f;
output.addContactPoint(-normal, pointInWorld, -depth);
#else
output.AddContactPoint(-normal, pb1, -depth);
#endif //
return_code = code;
}
return(1);
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
float[] Ra;
float[] Rb;
IndexedVector3 pa;
IndexedVector3 pb;
IndexedVector3 Sa;
IndexedVector3 Sb;
if (code <= 3)
{
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
IndexedVector3 normal2;
IndexedVector3 nr = new IndexedVector3();
IndexedVector3 anr;
if (code <= 3)
{
normal2 = normal;
}
else
{
normal2 = -normal;
}
DMULTIPLY1_331(ref nr, Rb, ref normal2);
nr.Abs(out anr);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if (anr.Y > anr.X)
{
if (anr.Y > anr.Z)
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if (anr.X > anr.Z)
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
IndexedVector3 center;
if (nr[lanr] < 0)
{
center = new IndexedVector3(
pb.X - pa.X + Sb[lanr] * Rb[lanr],
pb.Y - pa.Y + Sb[lanr] * Rb[lanr + 4],
pb.Z - pa.Z + Sb[lanr] * Rb[lanr + 8]
);
}
else
{
center = new IndexedVector3(
pb.X - pa.X - Sb[lanr] * Rb[lanr],
pb.Y - pa.Y - Sb[lanr] * Rb[lanr + 4],
pb.Z - pa.Z - Sb[lanr] * Rb[lanr + 8]
);
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if (code <= 3)
{
codeN = code - 1;
}
else
{
codeN = code - 4;
}
if (codeN == 0)
{
code1 = 1;
code2 = 2;
}
else if (codeN == 1)
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
float[] quad = s_quad; // 2D coordinate of incident face (x,y pairs)
float c1, c2, m11, m12, m21, m22;
c1 = DDOT14(ref center, 0, Ra, code1);
c2 = DDOT14(ref center, 0, Ra, code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = DDOT44(Ra, code1, Rb, a1);
m12 = DDOT44(Ra, code1, Rb, a2);
m21 = DDOT44(Ra, code2, Rb, a1);
m22 = DDOT44(Ra, code2, Rb, a2);
{
float k1 = m11 * Sb[a1];
float k2 = m21 * Sb[a1];
float k3 = m12 * Sb[a2];
float k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
//float[] s_rectReferenceFace = new float[2];
s_rectReferenceFace[0] = Sa[code1];
s_rectReferenceFace[1] = Sa[code2];
// intersect the incident and reference faces
float[] ret = s_ret;
int n = IntersectRectQuad2(s_rectReferenceFace, quad, ret);
if (n < 1)
{
return(0); // this should never happen
}
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
float[] point = s_point; // penetrating contact points
float[] dep = s_dep; // depths for those points
float det1 = 1f / (m11 * m22 - m12 * m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (int j = 0; j < n; j++)
{
float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (int i = 0; i < 3; i++)
{
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
}
dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
if (dep[cnum] >= 0)
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if (cnum < 1)
{
return(0); // this should never happen
}
// we can't generate more contacts than we actually have
if (maxc > cnum)
{
maxc = cnum;
}
if (maxc < 1)
{
maxc = 1;
}
if (cnum <= maxc)
{
if (code < 4)
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorldFA = new IndexedVector3();;
for (int i = 0; i < 3; i++)
{
pointInWorldFA[i] = point[j * 3 + i] + pa[i];
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorldFA);
}
output.AddContactPoint(-normal, pointInWorldFA, -dep[j]);
}
}
else
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorld = new IndexedVector3();;
for (int i = 0; i < 3; i++)
{
pointInWorld[i] = point[j * 3 + i] + pa[i];
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint(-normal, pointInWorld, -dep[j]);
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
float maxdepth = dep[0];
for (int i = 1; i < cnum; i++)
{
if (dep[i] > maxdepth)
{
maxdepth = dep[i];
i1 = i;
}
}
int[] iret = new int[8];
CullPoints2(cnum, ret, maxc, i1, iret);
for (int j = 0; j < maxc; j++)
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
IndexedVector3 posInWorldFA = new IndexedVector3();;
for (int i = 0; i < 3; i++)
{
posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
}
IndexedVector3 pointInWorld = posInWorldFA;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint((-normal), pointInWorld, -dep[iret[j]]);
}
cnum = maxc;
}
return_code = code;
return(cnum);
}
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);
}
}
}
