Beispiel #1
0
        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
        }
Beispiel #2
0
		public void setMotorTargetVelocity( btScalar motorTargetVelocity ) { m_motorTargetVelocity = motorTargetVelocity; }