// helper for less key strokes
// r = ( (v1 - v2) cross v3 ) cross v3
inline void _CalculateAxis(const dVector3& v1,
                           const dVector3& v2,
                           const dVector3& v3,
                           dVector3& r)
{
    dVector3 t1;
    dVector3 t2;

    dVector3Subtract(v1,v2,t1);
    dVector3Cross(t1,v3,t2);
    dVector3Cross(t2,v3,r);
}
Ejemplo n.º 2
0
// check for separation between box edge and cylinder circle edge
int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
							const dVector3 &vCenterPoint, 
							const dVector3 &vVx0, const dVector3 &vVx1, 
							int iAxis ) 
{
	// calculate direction of edge
	dVector3 vDirEdge;
	dVector3Subtract(vVx1,vVx0,vDirEdge);
	dNormalize3(vDirEdge);
	// starting point of edge 
	dVector3 vEStart;
	dVector3Copy(vVx0,vEStart);;

	// calculate angle cosine between cylinder axis and edge
	dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);

	// if edge is perpendicular to cylinder axis
	if(dFabs(fdot2) < 1e-5f) 
	{
		// this can't be separating axis, because edge is parallel to circle plane
		return 1;
	}

	// find point of intersection between edge line and circle plane
	dVector3 vTemp1;
	dVector3Subtract(vCenterPoint,vEStart,vTemp1);
	dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
	dVector3 vpnt;
	vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
	vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
	vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);

	// find tangent vector on circle with same center (vCenterPoint) that
	// touches point of intersection (vpnt)
	dVector3 vTangent;
	dVector3Subtract(vCenterPoint,vpnt,vTemp1);
	dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
	
	// find vector orthogonal both to tangent and edge direction
	dVector3 vAxis;
	dVector3Cross(vTangent,vDirEdge,vAxis);

	// use that vector as separating axis
	return _cldTestAxis( cData, vAxis, iAxis );
}
// intersection test between edge and circle
bool _cldTestCircleToEdgeAxis(sData& cData,
                              const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
                              const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
                              const dVector3 &vVx0, const dVector3 &vVx1, int iAxis)
{
    // calculate direction of edge
    dVector3 vkl;
    dVector3Subtract( vVx1 , vVx0 , vkl);
    dNormalize3(vkl);
    // starting point of edge
    dVector3 vol;
    dVector3Copy(vVx0,vol);

    // calculate angle cosine between cylinder axis and edge
    dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);

    // if edge is perpendicular to cylinder axis
    if(dFabs(fdot2)<REAL(1e-5))
    {
        // this can't be separating axis, because edge is parallel to circle plane
        return true;
    }

    // find point of intersection between edge line and circle plane
    dVector3 vTemp;
    dVector3Subtract(vCenterPoint,vol,vTemp);
    dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
    dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
    vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
    vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
    vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;

    // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
    dVector3 vTangent;
    dVector3Subtract(vCenterPoint,vpnt,vTemp);
    dVector3Cross(vTemp,vCylinderAxis1,vTangent);

    // find vector orthogonal both to tangent and edge direction
    dVector3 vAxis;
    dVector3Cross(vTangent,vkl,vAxis);

    // use that vector as separating axis
    return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis );
}
void TestOneTriangleVsCylinder(   sData& cData,
                                  const dVector3 &v0,
                                  const dVector3 &v1,
                                  const dVector3 &v2,
                                  const bool bDoubleSided)
{

    // calculate triangle normal
    dVector3Subtract( v2 , v1 ,cData.vE1);
    dVector3 vTemp;
    dVector3Subtract( v0 , v1 ,vTemp);
    dVector3Cross(cData.vE1 , vTemp , cData.vNormal );

    dNormalize3( cData.vNormal);

    // create plane from triangle
    //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 );
    dReal plDistance = -dVector3Dot(v0, cData.vNormal);
    dVector4 plTrianglePlane;
    dConstructPlane( cData.vNormal,plDistance,plTrianglePlane);

    // calculate sphere distance to plane
    dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane);

    // Sphere must be over positive side of triangle
    if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided)
    {
        // if not don't generate contacts
        return;
    }

    dVector3 vPnt0;
    dVector3 vPnt1;
    dVector3 vPnt2;

    if (fDistanceCylinderCenterToPlane < REAL(0.0) )
    {
        // flip it
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt2);
        dVector3Copy(v2 , vPnt1);
    }
    else
    {
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt1);
        dVector3Copy(v2 , vPnt2);
    }

    cData.fBestDepth = MAX_REAL;

    // do intersection test and find best separating axis
    if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) )
    {
        // if not found do nothing
        return;
    }

    // if best separation axis is not found
    if ( cData.iBestAxis == 0 )
    {
        // this should not happen (we should already exit in that case)
        dIASSERT(false);
        // do nothing
        return;
    }

    dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis );

    // choose which clipping method are we going to apply
    if (dFabs(fdot) < REAL(0.9) )
    {
        if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2))
        {
            return;
        }
    }
    else
    {
        _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2);
    }

}
bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
{
    // translate cylinder
    dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal);
    dVector3 vN2;
    vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp;
    vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp;
    vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp;

    fTemp = dVector3Length(vN2);
    if (fTemp < REAL(1e-5))
    {
        return false;
    }

    // Normalize it
    vN2[0] /= fTemp;
    vN2[1] /= fTemp;
    vN2[2] /= fTemp;

    // calculate caps centers in absolute space
    dVector3 vCposTrans;
    vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius;
    vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius;
    vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius;

    dVector3 vCEdgePoint0;
    vCEdgePoint0[0]  = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
    vCEdgePoint0[1]  = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
    vCEdgePoint0[2]  = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));

    dVector3 vCEdgePoint1;
    vCEdgePoint1[0]  = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
    vCEdgePoint1[1]  = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
    vCEdgePoint1[2]  = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));

    // transform cylinder edge points into triangle space
    vCEdgePoint0[0] -= v0[0];
    vCEdgePoint0[1] -= v0[1];
    vCEdgePoint0[2] -= v0[2];

    vCEdgePoint1[0] -= v0[0];
    vCEdgePoint1[1] -= v0[1];
    vCEdgePoint1[2] -= v0[2];

    dVector4 plPlane;
    dVector3 vPlaneNormal;

    // triangle plane
    //plPlane = Plane4f( -cData.vNormal, 0);
    vPlaneNormal[0] = -cData.vNormal[0];
    vPlaneNormal[1] = -cData.vNormal[1];
    vPlaneNormal[2] = -cData.vNormal[2];
    dConstructPlane(vPlaneNormal,REAL(0.0),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
    {
        return false;
    }

    // plane with edge 0
    //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5));
    dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal);
    dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
    {
        return false;
    }

    // plane with edge 1
    //dVector3 vTemp = ( cData.vNormal cross cData.vE1 );
    dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal);
    fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5);
    //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5)));
    dConstructPlane(vPlaneNormal,-fTemp,plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
    {
        return false;
    }

    // plane with edge 2
    // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5));
    dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal);
    dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
    {
        return false;
    }

    // return capsule edge points into absolute space
    vCEdgePoint0[0] += v0[0];
    vCEdgePoint0[1] += v0[1];
    vCEdgePoint0[2] += v0[2];

    vCEdgePoint1[0] += v0[0];
    vCEdgePoint1[1] += v0[1];
    vCEdgePoint1[2] += v0[2];

    // calculate depths for both contact points
    dVector3 vTemp;
    dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp);
    dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
    dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp);
    dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;

    dReal fDepth0 = cData.fBestDepth - (fRestDepth0);
    dReal fDepth1 = cData.fBestDepth - (fRestDepth1);

    // clamp depths to zero
    if(fDepth0 < REAL(0.0) )
    {
        fDepth0 = REAL(0.0);
    }

    if(fDepth1<REAL(0.0))
    {
        fDepth1 = REAL(0.0);
    }

    // Generate contact 0
    {
        cData.gLocalContacts[cData.nContacts].fDepth = fDepth0;
        dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
        dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos);
        cData.gLocalContacts[cData.nContacts].nFlags = 1;
        cData.nContacts++;
        if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
            return true;
    }

    // Generate contact 1
    {
        // generate contacts
        cData.gLocalContacts[cData.nContacts].fDepth = fDepth1;
        dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
        dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos);
        cData.gLocalContacts[cData.nContacts].nFlags = 1;
        cData.nContacts++;
    }

    return true;
}
bool _cldTestSeparatingAxes(sData& cData,
                            const dVector3 &v0,
                            const dVector3 &v1,
                            const dVector3 &v2)
{

    // calculate edge vectors
    dVector3Subtract(v1 ,v0 , cData.vE0);
    // cData.vE1 has been calculated before -> so save some cycles here
    dVector3Subtract(v0 ,v2 , cData.vE2);

    // calculate caps centers in absolute space
    dVector3 vCp0;
    vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
    vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
    vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));

    dVector3 vCp1;
    vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
    vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
    vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));

    // reset best axis
    cData.iBestAxis = 0;
    dVector3 vAxis;

    // axis cData.vNormal
    //vAxis = -cData.vNormal;
    vAxis[0] = -cData.vNormal[0];
    vAxis[1] = -cData.vNormal[1];
    vAxis[2] = -cData.vNormal[2];
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true))
    {
        return false;
    }

    // axis CxE0
    // vAxis = ( cData.vCylinderAxis cross cData.vE0 );
    dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis);
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2))
    {
        return false;
    }

    // axis CxE1
    // vAxis = ( cData.vCylinderAxis cross cData.vE1 );
    dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis);
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3))
    {
        return false;
    }

    // axis CxE2
    // vAxis = ( cData.vCylinderAxis cross cData.vE2 );
    dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis);
    if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4))
    {
        return false;
    }

    // first vertex on triangle
    // axis ((V0-Cp0) x C) x C
    //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
    _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis);
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11))
    {
        return false;
    }

    // second vertex on triangle
    // axis ((V1-Cp0) x C) x C
    // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
    _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis);
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12))
    {
        return false;
    }

    // third vertex on triangle
    // axis ((V2-Cp0) x C) x C
    //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
    _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis);
    if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13))
    {
        return false;
    }

    // test cylinder axis
    // vAxis = cData.vCylinderAxis;
    dVector3Copy(cData.vCylinderAxis , vAxis);
    if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14))
    {
        return false;
    }

    // Test top and bottom circle ring of cylinder for separation
    dVector3 vccATop;
    vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
    vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
    vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));

    dVector3 vccABottom;
    vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
    vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
    vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));


    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15))
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16))
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17))
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18))
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19))
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20))
    {
        return false;
    }

    return true;
}
Ejemplo n.º 7
0
// Test separating axis for collision
int _cldTestSeparatingAxes(sCylinderBoxData& cData) 
{
	// reset best axis
	cData.fBestDepth = MAX_FLOAT;
	cData.iBestAxis = 0;
	cData.fBestrb = 0;
	cData.fBestrc = 0;
	cData.nContacts = 0;

	dVector3  vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};

	// Epsilon value for checking axis vector length 
	const dReal fEpsilon = 1e-6f;

	// axis A0
	dMat3GetCol(cData.mBoxRot, 0 , vAxis);
	if (!_cldTestAxis( cData, vAxis, 1 )) 
	{
		return 0;
	}

	// axis A1
	dMat3GetCol(cData.mBoxRot, 1 , vAxis);
	if (!_cldTestAxis( cData, vAxis, 2 )) 
	{
		return 0;
	}

	// axis A2
	dMat3GetCol(cData.mBoxRot, 2 , vAxis);
	if (!_cldTestAxis( cData, vAxis, 3 )) 
	{
		return 0;
	}

	// axis C - Cylinder Axis
	//vAxis = vCylinderAxis;
	dVector3Copy(cData.vCylinderAxis , vAxis);
	if (!_cldTestAxis( cData, vAxis, 4 )) 
	{
		return 0;
	}

	// axis CxA0
	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));
	dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);
	if(dVector3Length2( vAxis ) > fEpsilon ) 
	{
		if (!_cldTestAxis( cData, vAxis, 5 ))
		{
			return 0;
		}
	}

	// axis CxA1
	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));
	dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);
	if(dVector3Length2( vAxis ) > fEpsilon ) 
	{
		if (!_cldTestAxis( cData, vAxis, 6 )) 
		{
			return 0;
		}
	}

	// axis CxA2
	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));
	dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);
	if(dVector3Length2( vAxis ) > fEpsilon ) 
	{
		if (!_cldTestAxis( cData, vAxis, 7 ))
		{
			return 0;
		}
	}

	int i = 0;
	dVector3	vTemp1;
	dVector3	vTemp2;
	// here we check box's vertices axis
	for(i=0; i< 8; i++) 
	{
		//vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));
		dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);
		dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);
		//vAxis = ( vCylinderAxis cross vAxis );
		dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);
		if(dVector3Length2( vAxis ) > fEpsilon ) 
		{
			if (!_cldTestAxis( cData, vAxis, 8 + i ))
			{
				return 0;
			}
		}
	}

	// ************************************
	// this is defined for first 12 axes
	// normal of plane that contains top circle of cylinder
	// center of top circle of cylinder
	dVector3 vcc;
	vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
	vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
	vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
	// ************************************

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) 
	{
		return 0;
	}


	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) 
	{
		return 0;
	}

	// ************************************
	// this is defined for second 12 axes
	// normal of plane that contains bottom circle of cylinder
	// center of bottom circle of cylinder
	//	vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5));
	vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
	vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
	vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
	// ************************************

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) 
	{
		return 0;
	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) 
	{
		return 0;
	}

	return 1;
}
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (Cylinder->type == dCylinderClass);
	dIASSERT (Plane->type == dPlaneClass);
	dIASSERT ((flags & NUMC_MASK) >= 1);

	int GeomCount = 0; // count of used contactgeoms

#ifdef dSINGLE
	const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
	const dReal toleranz = REAL(0.0000001);
#endif

	// Get the properties of the cylinder (length+radius)
	dReal radius, length;
	dGeomCylinderGetParams(Cylinder, &radius, &length);
	dVector3 &cylpos = Cylinder->final_posr->pos;
	// and the plane
	dVector4 planevec;
	dGeomPlaneGetParams(Plane, planevec);
	dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
	dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};

	dVector3 G1Pos1, G1Pos2, vDir1;
	vDir1[0] = Cylinder->final_posr->R[2];
	vDir1[1] = Cylinder->final_posr->R[6];
	vDir1[2] = Cylinder->final_posr->R[10];

	dReal s;
	s = length * REAL(0.5);
	G1Pos2[0] = vDir1[0] * s + cylpos[0];
	G1Pos2[1] = vDir1[1] * s + cylpos[1];
	G1Pos2[2] = vDir1[2] * s + cylpos[2];

	G1Pos1[0] = vDir1[0] * -s + cylpos[0];
	G1Pos1[1] = vDir1[1] * -s + cylpos[1];
	G1Pos1[2] = vDir1[2] * -s + cylpos[2];

	dVector3 C;

	// parallel-check
	s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
	if(s < 0)
		s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
	else
		s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
	if(s < toleranz && s > (-toleranz))
	{
		// discs are parallel to the plane

		// 1.compute if, and where contacts are
		dVector3 P;
		s = planevec[3] - dVector3Dot(planevec, G1Pos1);
		dReal t;
		t = planevec[3] - dVector3Dot(planevec, G1Pos2);
		if(s >= t) // s == t does never happen, 
		{
			if(s >= 0)
			{
				// 1. Disc
				dVector3Copy(G1Pos1, P);
			}
			else
				return GeomCount; // no contacts
		}
		else
		{
			if(t >= 0)
			{
				// 2. Disc
				dVector3Copy(G1Pos2, P);
			}
			else
				return GeomCount; // no contacts
		}

		// 2. generate a coordinate-system on the disc
		dVector3 V1, V2;
		if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
		{
			// not x-axis
			V1[0] = vDir1[0] + REAL(1.0); // random value
			V1[1] = vDir1[1];
			V1[2] = vDir1[2];
		}
		else
		{
			// maybe x-axis
			V1[0] = vDir1[0];
			V1[1] = vDir1[1] + REAL(1.0); // random value
			V1[2] = vDir1[2];
		}
		// V1 is now another direction than vDir1
		// Cross-product
	    dVector3Cross(V1, vDir1, V2);
		// make unit V2
		t = dVector3Length(V2);
		t = radius / t;
		dVector3Scale(V2, t);
		// cross again
		dVector3Cross(V2, vDir1, V1);
		// |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
		// V1 = first axis
		// V2 = second axis

		// 3. generate contactpoints

		// Potential contact 1
		dVector3Add(P, V1, contact->pos);
		contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
		if(contact->depth > 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}

		// Potential contact 2
		dVector3Subtract(P, V1, contact->pos);
		contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
		if(contact->depth > 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}

		// Potential contact 3
		dVector3Add(P, V2, contact->pos);
		contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
		if(contact->depth > 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}

		// Potential contact 4
		dVector3Subtract(P, V2, contact->pos);
		contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
		if(contact->depth > 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}
	}
	else
	{
		dReal t = dVector3Dot(PlaneNormal, vDir1);
		C[0] = vDir1[0] * t - PlaneNormal[0];
		C[1] = vDir1[1] * t - PlaneNormal[1];
		C[2] = vDir1[2] * t - PlaneNormal[2];
		s = dVector3Length(C);
		// move C onto the circle
		s = radius / s;
		dVector3Scale(C, s);

		// deepest point of disc 1
		dVector3Add(C, G1Pos1, contact->pos);

		// depth of the deepest point
		contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
		if(contact->depth >= 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}

		// C is still computed

		// deepest point of disc 2
		dVector3Add(C, G1Pos2, contact->pos);

		// depth of the deepest point
		contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
		if(contact->depth >= 0)
		{
			dVector3Copy(PlaneNormal, contact->normal);
			contact->g1 = Cylinder;
			contact->g2 = Plane;
			GeomCount++;
			if( GeomCount >= (flags & NUMC_MASK))
				return GeomCount; // enough contactgeoms
			contact = (dContactGeom *)((char *)contact + skip);
		}
	}
	return GeomCount;
}