ODE flat cylinder collider readme by Nguyen Binh (www.coderfarm.com)

------------------------------------------------------------------------------------------------------
Note: 
+ Those colliders are originated from Alen Lavadac @ Croteam. He generously contributed to ODE.
The licence of those code follow ODE's.
+ Any bugs/ feedback are welcomed. Please feel free to poke me at ode@coderfarm.com or better in 
ODE's mailling-list.
+ There are two cylinder colliders left:
	. Cylinder vs Cylinder : there is a bug in my code, I'm trying to fix it now
	. Cylinder vs Capsule : in progress.

------------------------------------------------------------------------------------------------------
HISTORY:
+ [19 mAy 2004]
 These release contains 3 colliders :
	+ Cylinder vs Box in collision_cylinder_box.cpp
	+ Cylinder vs Sphere in collision_cylinder_sphere.cpp
	+ Cylinder vs Trimesh in collision_cylinder_trimesh.cpp


ODE INTEGRATION GUIDES
You could just unpack these files into ODE main directory and say yes to overwrite question :)
Or follow these steps if you are paranoid and don't like to overwrite anything.

** NOTE FROM ADAM M.: dcylinder2.patch contains a patch that will apply this
** contrib to the main tree in a more futureproof way than copying files over
** and a more foolproof way than applying the below changes manually.

Here are steps to put flat cylinder collider into ODE (apply to current CVS version)

1.Modify collision_std.h
Add to the end of the file two prototype of cylinder vs box and sphere

// Cylinder - Box by (C) CroTeam
// Ported by Nguyen Binh
int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);

// Cylinder - Sphere (C) by CroTeam
// Ported by Nguyen Binh
int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, int flags, dContactGeom 
						   *contact, int skip);


2.Modify collision_trimesh_internal.h

Add these lines after #define _ODE_COLLISION_TRIMESH_INTERNAL_H_, which is at the beginning of the file

// cylinder - trimesh
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);

3. Modify collision.h in main ODE include directory to support flat cylinder

// Original code
dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length);
void dGeomCCylinderSetParams (dGeomID ccylinder, dReal radius, dReal length);
void dGeomCCylinderGetParams (dGeomID ccylinder, dReal *radius, dReal *length);
dReal dGeomCCylinderPointDepth (dGeomID ccylinder, dReal x, dReal y, dReal z);

// Add these
dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length);
void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length);
void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length);
//End

//Original code
dGeomID dCreateRay (dSpaceID space, dReal length);
void dGeomRaySetLength (dGeomID ray, dReal length);
dReal dGeomRayGetLength (dGeomID ray);
void dGeomRaySet (dGeomID ray, dReal px, dReal py, dReal pz,
		  dReal dx, dReal dy, dReal dz);
void dGeomRayGet (dGeomID ray, dVector3 start, dVector3 dir);


4. Modify collision_kernel.cpp

In function initColliders()

#ifdef dTRIMESH_ENABLED
 // original code
  setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
  setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
  setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
  setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
  setCollider (dTriMeshClass,dCCylinderClass,&dCollideCCTL);
   
  // Add this for cylinder-trimesh   
  // 11 May, Croteam's collider
  setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
#endif
  // Add these for cylinder vs box and sphere
  // 23 Apr 2004
  setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox);
  setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere);
  //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder);
  
  // END
  
And these portion of implementation code of flat cylinder geom

//START


//****************************************************************************
// flat cylinder public API

dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
	dAASSERT (_radius > 0 && _length > 0);
	type = dCylinderClass;
	radius = _radius;
	lz = _length;
}


void dxCylinder::computeAABB()
{
	dReal xrange = dFabs (R[0] * radius) +	 dFabs (R[1] * radius) + REAL(0.5)* dFabs (R[2] * 
		lz);
	dReal yrange = dFabs (R[4] * radius) +   dFabs (R[5] * radius) + REAL(0.5)* dFabs (R[6] * 
		lz);
	dReal zrange = dFabs (R[8] * radius) +	 dFabs (R[9] * radius) + REAL(0.5)* dFabs (R[10] * 
		lz);
	aabb[0] = pos[0] - xrange;
	aabb[1] = pos[0] + xrange;
	aabb[2] = pos[1] - yrange;
	aabb[3] = pos[1] + yrange;
	aabb[4] = pos[2] - zrange;
	aabb[5] = pos[2] + zrange;
}


dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length)
{
	return new dxCylinder (space,radius,length);
}

void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length)
{
	dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
	dAASSERT (radius > 0 && length > 0);
	dxCylinder *c = (dxCylinder*) cylinder;
	c->radius = radius;
	c->lz = length;
	dGeomMoved (cylinder);
}

void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length)
{
	dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
	dxCylinder *c = (dxCylinder*) cylinder;
	*radius = c->radius;
	*length = c->lz;
}

//END

5.Add this code to the end of collision_util.cpp - I send along a modified version, if can just replace the old file with
it.

// START

// clip polygon with plane and generate new polygon points
void		 dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn, 
							  dVector3 avArrayOut[], int &ctOut, 
							  const dVector4 &plPlane )
{
	// start with no output points
	ctOut = 0;

	int i0 = ctIn-1;

	// for each edge in input polygon
	for (int i1=0; i1<ctIn; i0=i1, i1++) {


		// calculate distance of edge points to plane
		dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
		dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );

		// if first point is in front of plane
		if( fDistance0 >= 0 ) {
			// emit point
			avArrayOut[ctOut][0] = avArrayIn[i0][0];
			avArrayOut[ctOut][1] = avArrayIn[i0][1];
			avArrayOut[ctOut][2] = avArrayIn[i0][2];
			ctOut++;
		}

		// if points are on different sides
		if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {

			// find intersection point of edge and plane
			dVector3 vIntersectionPoint;
			vIntersectionPoint[0]= avArrayIn[i0][0] - 
				(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
			vIntersectionPoint[1]= avArrayIn[i0][1] - 
				(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
			vIntersectionPoint[2]= avArrayIn[i0][2] - 
				(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);

			// emit intersection point
			avArrayOut[ctOut][0] = vIntersectionPoint[0];
			avArrayOut[ctOut][1] = vIntersectionPoint[1];
			avArrayOut[ctOut][2] = vIntersectionPoint[2];
			ctOut++;
		}
	}

}

void		 dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, 
							   dVector3 avArrayOut[], int &ctOut, 
							   const dVector4 &plPlane ,dReal fRadius)
{
	// start with no output points
	ctOut = 0;

	int i0 = ctIn-1;

	// for each edge in input polygon
	for (int i1=0; i1<ctIn; i0=i1, i1++) 
	{
		// calculate distance of edge points to plane
		dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
		dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );

		// if first point is in front of plane
		if( fDistance0 >= 0 ) 
		{
			// emit point
			if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
			{
				avArrayOut[ctOut][0] = avArrayIn[i0][0];
				avArrayOut[ctOut][1] = avArrayIn[i0][1];
				avArrayOut[ctOut][2] = avArrayIn[i0][2];
				ctOut++;
			}
		}

		// if points are on different sides
		if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) 
		{

			// find intersection point of edge and plane
			dVector3 vIntersectionPoint;
			vIntersectionPoint[0]= avArrayIn[i0][0] - 
				(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
			vIntersectionPoint[1]= avArrayIn[i0][1] - 
				(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
			vIntersectionPoint[2]= avArrayIn[i0][2] - 
				(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);

			// emit intersection point
			if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
			{
				avArrayOut[ctOut][0] = vIntersectionPoint[0];
				avArrayOut[ctOut][1] = vIntersectionPoint[1];
				avArrayOut[ctOut][2] = vIntersectionPoint[2];
				ctOut++;
			}
		}
	}	
}

// END

6. Modify collision_util.h
    Add this code to the end of collision_util.h - I send along a modified version, if can just replace the old file with
it.

// START

// 20 Apr 2004
// Start code by Nguyen Binh
int			dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
// clip polygon with plane and generate new polygon points
void		 dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, 
							  dVector3 avArrayOut[], int &ctOut, 
							  const dVector4 &plPlane );

void		 dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, 
							  dVector3 avArrayOut[], int &ctOut, 
							  const dVector4 &plPlane ,dReal fRadius);


// Some vector math
inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
{
	c[0] = a[0] - b[0];
	c[1] = a[1] - b[1];
	c[2] = a[2] - b[2];
}

// Some vector math
inline void dVector3Scale(dVector3& a,dReal nScale)
{
	a[0] *= nScale ;
	a[1] *= nScale ;
	a[2] *= nScale ;
}

inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
{
	c[0] = a[0] + b[0];
	c[1] = a[1] + b[1];
	c[2] = a[2] + b[2];
}

inline void dVector3Copy(const dVector3& a,dVector3& c)
{
	c[0] = a[0];
	c[1] = a[1];
	c[2] = a[2];
}

inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
{
	dCROSS(c,=,a,b);
}

inline dReal dVector3Length(const dVector3& a)
{
	return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
}

inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
{
	return dDOT(a,b);
}

inline void dVector3Inv(dVector3& a)
{
	a[0] = -a[0];
	a[1] = -a[1];
	a[2] = -a[2];
}

inline dReal dVector3Length2(const dVector3& a)
{
	return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
}

inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
{
	v[0] = m[col + 0];
	v[1] = m[col + 4];
	v[2] = m[col + 8];
}

inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
	r[0] =  v[1] * m[2*4 + col] - v[2] * m[1*4 + col]; 
	r[1] =  v[2] * m[0*4 + col] - v[0] * m[2*4 + col]; 
	r[2] =  v[0] * m[1*4 + col] - v[1] * m[0*4 + col];
}

inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
	r[0] =   v[2] * m[1*4 + col] - v[1] * m[2*4 + col]; 
	r[1] =   v[0] * m[2*4 + col] - v[2] * m[0*4 + col]; 
	r[2] =   v[1] * m[0*4 + col] - v[0] * m[1*4 + col];
}

inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
{
	dMULTIPLY0_331(r,m,v);
}

inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
{
	return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
}

inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
{
	plane[0] = normal[0];
	plane[1] = normal[1];
	plane[2] = normal[2];
	plane[3] = distance;
}

inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
{
	dest[0]	=	source[0];
	dest[1]	=	source[1];
	dest[2]	=	source[2];

	dest[4]	=	source[4];
	dest[5]	=	source[5];
	dest[6]	=	source[6];

	dest[8]	=	source[8];
	dest[9]	=	source[9];
	dest[10]=	source[10];
}

inline dReal dMatrix3Det( const dMatrix3& mat )
{
	dReal det;

	det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
		- mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
		+ mat[2] * ( mat[4]*mat[9]  - mat[8]*mat[5] );

	return( det );
}


inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
{
	dReal det = dMatrix3Det( ma );

	if ( dFabs( det ) < REAL(0.0005) )
	{
		dRSetIdentity( dst );
		return;
	}

	dst[0] =    ma[5]*ma[10] - ma[6]*ma[9]   / det;
	dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det;
	dst[2] =    ma[1]*ma[6]  - ma[5]*ma[2]   / det;

	dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det;
	dst[5] =    ma[0]*ma[10] - ma[8]*ma[2]   / det;
	dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det;

	dst[8] =    ma[4]*ma[9] - ma[8]*ma[5]   / det;
	dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det;
	dst[10] =    ma[0]*ma[5] - ma[1]*ma[4]   / det;
}

inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{

	// Nguyen Binh : this code seem to be the fastest.
	dReal x0 = 	source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
	dReal x1 = 	source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
	dReal x2 = 	source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
	dReal x3 = 	source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];

	dest[0]  = 	quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
	dest[1]  = 	quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
	dest[2]  = 	quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;

	/*
	// nVidia SDK implementation
	dVector3 uv, uuv; 
	dVector3 qvec;
	qvec[0] = quat[1];
	qvec[1] = quat[2];
	qvec[2] = quat[3];

	dVector3Cross(qvec,source,uv);
	dVector3Cross(qvec,uv,uuv);

	dVector3Scale(uv,REAL(2.0)*quat[0]);
	dVector3Scale(uuv,REAL(2.0));

	dest[0] = source[0] + uv[0] + uuv[0];
	dest[1] = source[1] + uv[1] + uuv[1];
	dest[2] = source[2] + uv[2] + uuv[2];   
	*/
}

inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{

	dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];

	if (norm > REAL(0.0))
	{
		dQuaternion invQuat;
		invQuat[0] =  quat[0] / norm;
		invQuat[1] = -quat[1] / norm;
		invQuat[2] = -quat[2] / norm;
		invQuat[3] = -quat[3] / norm;	
		
		dQuatTransform(invQuat,source,dest);

	}
	else
	{
		// Singular -> return identity
		dVector3Copy(source,dest);
	}
}

inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
{
	rY = asin(mRot[0 * 4 + 2]);
	if (rY < M_PI /2)
	{
		if (rY > -M_PI /2)
		{
			rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
			rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
		}
		else
		{
			// not unique
			rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
			rZ = REAL(0.0);
		}
	}
	else
	{
		// not unique
		rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
		rZ = REAL(0.0);
	}
}

inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
{
	dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];

	if (norm > 0.0f)
	{
		dest[0] = source[0] / norm;
		dest[1] = -source[1] / norm;
		dest[2] = -source[2] / norm;
		dest[3] = -source[3] / norm;	
	}
	else
	{
		// Singular -> return identity
		dest[0] = REAL(1.0);
		dest[1] = REAL(0.0);
		dest[2] = REAL(0.0);
		dest[3] = REAL(0.0);
	}
}

// Fetches a contact
inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
	dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
	return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
}

// END

7. Finally, copy 3 files :
	+ collision_cylinder_box.cpp
	+ collision_cylinder_sphere.cpp
	+ collision_cylinder_trimesh.cpp
	to [ode]/src directory and add them to your project

That's maybe all. :)

Ensembled by Nguyen Binh, 19-May-2004

