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