void _cldClipCylinderToTriangle(sData& cData,const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
{
    int i = 0;
    dVector3 avPoints[3];
    dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
    dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];

    dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
    dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);

    // setup array of triangle vertices
    dVector3Copy(v0,avPoints[0]);
    dVector3Copy(v1,avPoints[1]);
    dVector3Copy(v2,avPoints[2]);

    dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
    dSetZero(vCylinderCircleNormal_Rel,4);
    // check which circle from cylinder we take for clipping
    if ( dVector3Dot(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0))
    {
        // get top circle
        vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
        vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
        vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

        vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
    }
    else
    {
        // get bottom circle
        vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
        vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
        vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

        vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
    }

    dVector3 vTemp;
    dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot);
    // transform triangle points to space of cylinder circle
    for(i=0; i<3; i++)
    {
        dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp);
        dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]);
    }

    int iTmpCounter1 = 0;
    int iTmpCounter2 = 0;
    dVector4 plPlane;

    // plane of cylinder that contains circle for intersection
    //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f );
    dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
    dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane);

    // Body of base circle of Cylinder
    int nCircleSegment = 0;
    for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++)
    {
        dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);

        if (0 == (nCircleSegment % 2))
        {
            dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
        }
        else
        {
            dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
        }

        dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
        dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
    }

    // back transform clipped points to absolute space
    dReal ftmpdot;
    dReal fTempDepth;
    dVector3 vPoint;

    if (nCircleSegment %2)
    {
        for( i=0; i<iTmpCounter2; i++)
        {
            dQuatTransform(cData.qCylinderRot,avTempArray2[i], vPoint);
            vPoint[0] += vCylinderCirclePos[0];
            vPoint[1] += vCylinderCirclePos[1];
            vPoint[2] += vCylinderCirclePos[2];

            dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
            ftmpdot	 = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
            fTempDepth = cData.fBestrt - ftmpdot;
            // Depth must be positive
            if (fTempDepth > REAL(0.0))
            {
                cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
                dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
                dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
                cData.gLocalContacts[cData.nContacts].nFlags = 1;
                cData.nContacts++;
                if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
                    return;;
            }
        }
    }
    else
    {
        for( i=0; i<iTmpCounter1; i++)
        {
            dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint);
            vPoint[0] += vCylinderCirclePos[0];
            vPoint[1] += vCylinderCirclePos[1];
            vPoint[2] += vCylinderCirclePos[2];

            dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
            ftmpdot	 = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
            fTempDepth = cData.fBestrt - ftmpdot;
            // Depth must be positive
            if (fTempDepth > REAL(0.0))
            {
                cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
                dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
                dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
                cData.gLocalContacts[cData.nContacts].nFlags = 1;
                cData.nContacts++;
                if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
                    return;;
            }
        }
    }
}
Ejemplo n.º 2
0
void _cldClipBoxToCylinder(sCylinderBoxData& cData ) 
{
	dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
	// check which circle from cylinder we take for clipping
	if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) 
	{
		// get top circle
		vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

		vCylinderCircleNormal_Rel[0] = REAL(0.0);
		vCylinderCircleNormal_Rel[1] = REAL(0.0);
		vCylinderCircleNormal_Rel[2] = REAL(0.0);
		vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
	}
	else 
	{
		// get bottom circle
		vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

		vCylinderCircleNormal_Rel[0] = REAL(0.0);
		vCylinderCircleNormal_Rel[1] = REAL(0.0);
		vCylinderCircleNormal_Rel[2] = REAL(0.0);
		vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
	}

	// vNr is normal in Box frame, pointing from Cylinder to Box
	dVector3 vNr;
	dMatrix3 mBoxInv;

	// Find a way to use quaternion
	dMatrix3Inv(cData.mBoxRot,mBoxInv);
	dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);

	dVector3 vAbsNormal;

	vAbsNormal[0] = dFabs( vNr[0] );
	vAbsNormal[1] = dFabs( vNr[1] );
	vAbsNormal[2] = dFabs( vNr[2] );

	// find which face in box is closest to cylinder
	int iB0, iB1, iB2;

	// Different from Croteam's code
	if (vAbsNormal[1] > vAbsNormal[0]) 
	{
		// 1 > 0
		if (vAbsNormal[0]> vAbsNormal[2]) 
		{
			// 0 > 2 -> 1 > 0 >2
			iB0 = 1; iB1 = 0; iB2 = 2;
		} 
		else 
		{
			// 2 > 0-> Must compare 1 and 2
			if (vAbsNormal[1] > vAbsNormal[2])
			{
				// 1 > 2 -> 1 > 2 > 0
				iB0 = 1; iB1 = 2; iB2 = 0;
			}
			else
			{
				// 2 > 1 -> 2 > 1 > 0;
				iB0 = 2; iB1 = 1; iB2 = 0;
			}			
		}
	} 
	else 
	{
		// 0 > 1
		if (vAbsNormal[1] > vAbsNormal[2]) 
		{
			// 1 > 2 -> 0 > 1 > 2
			iB0 = 0; iB1 = 1; iB2 = 2;
		}
		else 
		{
			// 2 > 1 -> Must compare 0 and 2
			if (vAbsNormal[0] > vAbsNormal[2])
			{
				// 0 > 2 -> 0 > 2 > 1;
				iB0 = 0; iB1 = 2; iB2 = 1;
			}
			else
			{
				// 2 > 0 -> 2 > 0 > 1;
				iB0 = 2; iB1 = 0; iB2 = 1;
			}		
		}
	}

	dVector3 vCenter;
	// find center of box polygon
	dVector3 vTemp;
	if (vNr[iB0] > 0) 
	{
		dMat3GetCol(cData.mBoxRot,iB0,vTemp);
		vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
		vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
		vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
	}
	else 
	{
		dMat3GetCol(cData.mBoxRot,iB0,vTemp);
		vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
		vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
		vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2];
	}

	// find the vertices of box polygon
	dVector3 avPoints[4];
	dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
	dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];

	int i=0;
	for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) 
	{
		avTempArray1[i][0] = REAL(0.0);
		avTempArray1[i][1] = REAL(0.0);
		avTempArray1[i][2] = REAL(0.0);

		avTempArray2[i][0] = REAL(0.0);
		avTempArray2[i][1] = REAL(0.0);
		avTempArray2[i][2] = REAL(0.0);
	}

	dVector3 vAxis1, vAxis2;

	dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
	dMat3GetCol(cData.mBoxRot,iB2,vAxis2);

	avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
	avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
	avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];

	avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
	avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
	avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];

	avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
	avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
	avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];

	avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
	avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
	avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];

	// transform box points to space of cylinder circle
	dMatrix3 mCylinderInv;
	dMatrix3Inv(cData.mCylinderRot,mCylinderInv);

	for(i=0; i<4; i++) 
	{
		dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
		dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
	}

	int iTmpCounter1 = 0;
	int iTmpCounter2 = 0;
	dVector4 plPlane;

	// plane of cylinder that contains circle for intersection
	dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
	dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);


	// Body of base circle of Cylinder
	int nCircleSegment = 0;
	for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
	{
		dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);

		if (0 == (nCircleSegment % 2))
		{
			dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
		}
		else
		{
			dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
		}

		dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS );
		dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
	}
	
	// back transform clipped points to absolute space
	dReal ftmpdot;	
	dReal fTempDepth;
	dVector3 vPoint;

	if (nCircleSegment %2)
	{
		for( i=0; i<iTmpCounter2; i++)
		{
			dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
			vPoint[0] += vCylinderCirclePos[0];
			vPoint[1] += vCylinderCirclePos[1];
			vPoint[2] += vCylinderCirclePos[2];

			dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
			ftmpdot	 = dVector3Dot(vTemp, cData.vNormal);
			fTempDepth = cData.fBestrc - ftmpdot;
			// Depth must be positive
			if (fTempDepth > REAL(0.0))
			{
				// generate contacts
				dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
				Contact0->depth = fTempDepth;
				dVector3Copy(cData.vNormal,Contact0->normal);
				dVector3Copy(vPoint,Contact0->pos);
				Contact0->g1 = cData.gCylinder;
				Contact0->g2 = cData.gBox;
				dVector3Inv(Contact0->normal);
				cData.nContacts++;
			}
		}
	}
	else
	{
		for( i=0; i<iTmpCounter1; i++)
		{
			dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
			vPoint[0] += vCylinderCirclePos[0];
			vPoint[1] += vCylinderCirclePos[1];
			vPoint[2] += vCylinderCirclePos[2];

			dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
			ftmpdot	 = dVector3Dot(vTemp, cData.vNormal);
			fTempDepth = cData.fBestrc - ftmpdot;
			// Depth must be positive
			if (fTempDepth > REAL(0.0))
			{
				// generate contacts
				dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
				Contact0->depth = fTempDepth;
				dVector3Copy(cData.vNormal,Contact0->normal);
				dVector3Copy(vPoint,Contact0->pos);
				Contact0->g1 = cData.gCylinder;
				Contact0->g2 = cData.gBox;
				dVector3Inv(Contact0->normal);
				cData.nContacts++;
			}
		}
	}
}