public void Project(ref IndexedMatrix trans, ref IndexedVector3 dir, ref float min, ref float max) { #if true min = float.MaxValue; max = float.MinValue; IndexedVector3 witnesPtMin; IndexedVector3 witnesPtMax; int numVerts = m_unscaledPoints.Count; for(int i=0;i<numVerts;i++) { IndexedVector3 vtx = m_unscaledPoints[i] * m_localScaling; IndexedVector3 pt = trans * vtx; float dp = pt.Dot(dir); if(dp < min) { min = dp; witnesPtMin = pt; } if(dp > max) { max = dp; witnesPtMax=pt; } } #else btVector3 localAxis = dir*trans.getBasis(); btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); min = vtx1.dot(dir); max = vtx2.dot(dir); #endif if(min>max) { float tmp = min; min = max; max = tmp; } }
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 override void GetAabbSlow(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { #if true IndexedVector3[] _directions = new IndexedVector3[] { new IndexedVector3( 1.0f, 0.0f, 0.0f), new IndexedVector3( 0.0f, 1.0f, 0.0f), new IndexedVector3( 0.0f, 0.0f, 1.0f), new IndexedVector3( -1.0f, 0.0f, 0.0f), new IndexedVector3( 0.0f, -1.0f, 0.0f), new IndexedVector3( 0.0f, 0.0f, -1.0f) }; IndexedVector4[] _supporting = new IndexedVector4[] { IndexedVector4.Zero, IndexedVector4.Zero, IndexedVector4.Zero, IndexedVector4.Zero, IndexedVector4.Zero, IndexedVector4.Zero }; for (int i = 0; i < 6; i++) { _directions[i] = _directions[i] * t._basis; } ObjectArray<IndexedVector4> tempSupporting = new ObjectArray<IndexedVector4>(6); BatchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); IndexedVector3 aabbMin1 = new IndexedVector3(0,0,0),aabbMax1 = new IndexedVector3(0,0,0); for ( int i = 0; i < 3; ++i ) { IndexedVector3 temp = new IndexedVector3(_supporting[i].X, _supporting[i].Y, _supporting[i].Z); aabbMax1[i] = (t *temp)[i]; temp = new IndexedVector3(_supporting[i+3].X, _supporting[i+3].Y, _supporting[i+3].Z); aabbMin1[i] = (t * temp)[i]; } IndexedVector3 marginVec = new IndexedVector3(GetMargin()); aabbMin = aabbMin1-marginVec; aabbMax = aabbMax1+marginVec; #else float margin = getMargin(); for (int i=0;i<3;i++) { IndexedVector3 vec(float(0.),float(0.),float(0.)); vec[i] = float(1.); IndexedVector3 sv = localGetSupportingVertex(vec*t.getBasis()); IndexedVector3 tmp = t(sv); aabbMax[i] = tmp[i]+margin; vec[i] = float(-1.); sv = localGetSupportingVertex(vec*t.getBasis()); tmp = t(sv); aabbMin[i] = tmp[i]-margin; } #endif }