Пример #1
0
        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
        }