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 bool TestSepAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 sep_axis, out float depth) { float Min0, Max0; float Min1, Max1; hullA.Project(ref transA, ref sep_axis, out Min0, out Max0); hullB.Project(ref transB, ref sep_axis, out Min1, out Max1); if (Max0 < Min1 || Max1 < Min0) { depth = 0; return(false); } float d0 = Max0 - Min1; Debug.Assert(d0 >= 0.0f); float d1 = Max1 - Min0; Debug.Assert(d1 >= 0.0f); depth = d0 < d1 ? d0 : d1; return(true); }
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) public virtual bool InitializePolyhedralFeatures() { #if USE_CONVEX_HULL_COMPUTER if (m_polyhedron != null) { m_polyhedron = null; } m_polyhedron = new ConvexPolyhedron(); ObjectArray <IndexedVector3> tmpVertices = new ObjectArray <IndexedVector3>(); for (int i = 0; i < GetNumVertices(); i++) { IndexedVector3 newVertex; GetVertex(i, out newVertex); tmpVertices.Add(newVertex); } ConvexHullComputer conv = new ConvexHullComputer(); //conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f); conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f); ObjectArray <IndexedVector3> faceNormals = new ObjectArray <IndexedVector3>(); int numFaces = conv.faces.size(); faceNormals.Resize(numFaces); ConvexHullComputer convexUtil = conv; m_polyhedron.m_faces.Resize(numFaces); int numVertices = convexUtil.vertices.Count; m_polyhedron.m_vertices.Resize(numVertices); for (int p = 0; p < numVertices; p++) { m_polyhedron.m_vertices[p] = convexUtil.vertices[p]; } for (int i = 0; i < numFaces; i++) { int face = convexUtil.faces[i]; //printf("face=%d\n",face); Edge firstEdge = convexUtil.edges[face]; Edge edge = firstEdge; IndexedVector3[] edges = new IndexedVector3[3]; int numEdges = 0; //compute face normals float maxCross2 = 0.0f; int chosenEdge = -1; do { int src = edge.GetSourceVertex(); m_polyhedron.m_faces[i].m_indices.Add(src); int targ = edge.GetTargetVertex(); IndexedVector3 wa = convexUtil.vertices[src]; IndexedVector3 wb = convexUtil.vertices[targ]; IndexedVector3 newEdge = wb - wa; newEdge.Normalize(); if (numEdges < 2) { edges[numEdges++] = newEdge; } edge = edge.GetNextEdgeOfFace(); } while (edge != firstEdge); float planeEq = 1e30f; if (numEdges == 2) { faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]); faceNormals[i].Normalize(); m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X; m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y; m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z; m_polyhedron.m_faces[i].m_plane[3] = planeEq; } else { Debug.Assert(false);//degenerate? faceNormals[i] = IndexedVector3.Zero; } for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++) { float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]); if (planeEq > eq) { planeEq = eq; } } m_polyhedron.m_faces[i].m_plane[3] = planeEq; } if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0) { for (int f = 0; f < m_polyhedron.m_faces.Count; f++) { IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]); float planeEq = m_polyhedron.m_faces[f].m_plane[3]; IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal); if (IndexedVector3.Dot(supVec, planeNormal) < planeEq) { m_polyhedron.m_faces[f].m_plane[0] *= -1; m_polyhedron.m_faces[f].m_plane[1] *= -1; m_polyhedron.m_faces[f].m_plane[2] *= -1; m_polyhedron.m_faces[f].m_plane[3] *= -1; int numVerts = m_polyhedron.m_faces[f].m_indices.Count; for (int v = 0; v < numVerts / 2; v++) { int temp = m_polyhedron.m_faces[f].m_indices[v]; m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v]; m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp; } } } } m_polyhedron.Initialize(); #endif return(true); }
public static bool TestInternalObjects(ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin) { float dp = delta_c.Dot(ref axis); IndexedVector3 localAxis0; InverseTransformPoint3x3(out localAxis0, ref axis, ref trans0); IndexedVector3 localAxis1; InverseTransformPoint3x3(out localAxis1, ref axis, ref trans1); IndexedVector3 p0; BoxSupport(ref convex0.m_extents, ref localAxis0, out p0); IndexedVector3 p1; BoxSupport(ref convex1.m_extents, ref localAxis1, out p1); float Radius0 = p0.X * localAxis0.X + p0.Y * localAxis0.Y + p0.Z * localAxis0.Z; float Radius1 = p1.X * localAxis1.X + p1.Y * localAxis1.Y + p1.Z * localAxis1.Z; float MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius; float MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius; float MinMaxRadius = MaxRadius + MinRadius; float d0 = MinMaxRadius + dp; float d1 = MinMaxRadius - dp; float depth = d0 < d1 ? d0:d1; if (depth > dmin) { return(false); } return(true); }
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, out IndexedVector3 sep) { gActualSATPairTests++; // dummy value to satisfy exit points. sep = new IndexedVector3(0, 1, 0); #if TEST_INTERNAL_OBJECTS IndexedVector3 c0 = transA * hullA.m_localCenter; IndexedVector3 c1 = transB * hullB.m_localCenter; IndexedVector3 DeltaC2 = c0 - c1; #endif float dmin = float.MaxValue; int curPlaneTests = 0; int numFacesA = hullA.m_faces.Count; // Test normals from hullA for (int i = 0; i < numFacesA; i++) { IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); IndexedVector3 faceANormalWS = transA._basis * Normal; curPlaneTests++; #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref faceANormalWS, hullA, hullB, dmin)) { continue; } gActualNbTests++; #endif float d; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref faceANormalWS, out d)) { return(false); } if (d < dmin) { dmin = d; sep = faceANormalWS; } } int numFacesB = hullB.m_faces.Count; // Test normals from hullB for (int i = 0; i < numFacesB; i++) { IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); IndexedVector3 WorldNormal = transB._basis * Normal; curPlaneTests++; #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref WorldNormal, hullA, hullB, dmin)) { continue; } gActualNbTests++; #endif float d; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref WorldNormal, out d)) { return(false); } if (d < dmin) { dmin = d; sep = WorldNormal; } } IndexedVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; int curEdgeEdge = 0; // Test edges for (int e0 = 0; e0 < hullA.m_uniqueEdges.Count; e0++) { IndexedVector3 edge0 = hullA.m_uniqueEdges[e0]; IndexedVector3 WorldEdge0 = transA._basis * edge0; for (int e1 = 0; e1 < hullB.m_uniqueEdges.Count; e1++) { IndexedVector3 edge1 = hullB.m_uniqueEdges[e1]; IndexedVector3 WorldEdge1 = transB._basis * edge1; IndexedVector3 Cross = IndexedVector3.Cross(WorldEdge0, WorldEdge1); curEdgeEdge++; if (!MathUtil.IsAlmostZero(ref Cross)) { Cross.Normalize(); #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref Cross, hullA, hullB, dmin)) { continue; } gActualNbTests++; #endif float dist; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref Cross, out dist)) { return(false); } if (dist < dmin) { dmin = dist; sep = Cross; } } } } IndexedVector3 deltaC = transB._origin - transA._origin; if ((IndexedVector3.Dot(deltaC, sep)) > 0.0f) { sep = -sep; } return(true); }
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, out IndexedVector3 sep) { return(FindSeparatingAxis(hullA, hullB, ref transA, ref transB, out sep)); }
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 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); }