/*************************************************************************
*                                                                       *
* Open Physics Abstraction Layer                                        *
* Copyright (C) 2004-2005                                               *
* Alan Fischer  alan.fischer@gmail.com                                  *
* Andres Reinot  andres@reinot.com                                      *
* Tyler Streeter  tylerstreeter@gmail.com                               *
* All rights reserved.                                                  *
* Web: opal.sourceforge.net                                             *
*                                                                       *
* This library is free software; you can redistribute it and/or         *
* modify it under the terms of EITHER:                                  *
*   (1) The GNU Lesser General Public License as published by the Free  *
*       Software Foundation; either version 2.1 of the License, or (at  *
*       your option) any later version. The text of the GNU Lesser      *
*       General Public License is included with this library in the     *
*       file license-LGPL.txt.                                          *
*   (2) The BSD-style license that is included with this library in     *
*       the file license-BSD.txt.                                       *
*                                                                       *
* This library is distributed in the hope that it will be useful,       *
* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
* license-LGPL.txt and license-BSD.txt for more details.                *
*                                                                       *
*************************************************************************/

// class headers
#include "ODESolid.h"

// project headers
#include "ODESpace.h"
#include "ODETools.h"
#include "../BoxShapeData.h"
#include "../SphereShapeData.h"
#include "../CapsuleShapeData.h"
#include "../PlaneShapeData.h"
#include "../MeshShapeData.h"
#include "../Mass.h"

namespace opal
{
	ODESolid::ODESolid( dWorldID worldID, dSpaceID spaceID )
			: Solid()
	{
		mWorldID = worldID;
		mSpaceID = spaceID;
		mIsPlaceable = true;
		mCollisionCount = 0;
		mNonSymmetricInertia = false;
		mIsFreelySpinning = true;
		mPrevAngVelMagSquared = 0;

		if ( !mData.isStatic )
		{
			// Create an ODE body with default ODE mass parameters (total
			// mass = 1).  This should have an initial mass of 0 until shapes
			// are added, but ODE won't allow a mass of 0.  This will be
			// adjusted appropriately when the first shape is added.
			mBodyID = dBodyCreate( mWorldID );
		}

		init( mData );
	}

	ODESolid::~ODESolid()
	{
		destroyGeoms();

		if ( !mData.isStatic )
		{
			// This is a dynamic solid, so it has an ODE body that needs
			// to be destroyed.
			dBodyDestroy( mBodyID );
		}
	}

	void ODESolid::clearShapes()
	{
		destroyGeoms();
		resetAABB();
		mData.destroyShapes();
	}

	void ODESolid::destroyGeoms()
	{
		for ( size_t i = 0; i < mGeomDataList.size(); ++i )
		{
			if ( 0 != mGeomDataList[ i ] ->transformID )
			{
				dGeomDestroy( mGeomDataList[ i ] ->transformID );
			}

			if ( 0 != mGeomDataList[ i ] ->trimeshDataID )
			{
				// This geom uses a trimesh.  Destroy it.  (This does NOT
				// touch the user's mesh data, just internal ODE trimesh
				// data.)
				dGeomTriMeshDataDestroy( mGeomDataList[ i ] ->trimeshDataID );
			}

			// Destroy the ODE geom.
			dGeomDestroy( mGeomDataList[ i ] ->geomID );

			// Delete the geom data object.
			delete mGeomDataList[ i ];
		}
		mGeomDataList.clear();
	}

	void ODESolid::init( const SolidData& data )
	{
		// The order of function calls here is important.

		// Destroy the old Shapes.
		mData.destroyShapes();

		// Destroy the old ODE geoms.
		destroyGeoms();

		// Set whether the Solid is static.
		setStatic( data.isStatic );

		// Set the new transform.
		setTransform( data.transform );

		// Add the new Shapes.
		for ( unsigned int i = 0; i < data.getNumShapes(); ++i )
		{
			addShape( *( data.getShapeData( i ) ) );
		}

		// Set whether the Solid is sleeping.
		setSleeping( data.sleeping );

		// Set the Solid's sleepiness level.
		setSleepiness( data.sleepiness );

		// Set whether the Solid is enabled.
		setEnabled( data.enabled );

		// Set damping levels.
		setLinearDamping( data.linearDamping );
		setAngularDamping( data.angularDamping );

		// Set velocities.
		setGlobalLinearVel( data.globalLinearVel );
		setGlobalAngularVel( data.globalAngularVel );

		// Set the Solid's name.
		setName( data.name );
	}

	void ODESolid::setEnabled( bool e )
	{
		if ( e )
		{
			// If this Solid is dynamic, enable the ODE body.
			if ( !mData.isStatic )
			{
				dBodyEnable( mBodyID );
			}

			// Enable the ODE geoms.
			std::vector<GeomData*>::iterator iter;
			for ( iter = mGeomDataList.begin();
			        iter != mGeomDataList.end(); ++iter )
			{
				dGeomEnable( ( *iter ) ->geomID );
			}
		}
		else // Disable the Solid.
		{
			// If this Solid is dynamic, disable the ODE body.
			if ( !mData.isStatic )
			{
				dBodyDisable( mBodyID );
			}

			// Disable the ODE geoms.
			std::vector<GeomData*>::iterator iter;
			for ( iter = mGeomDataList.begin();
			        iter != mGeomDataList.end(); ++iter )
			{
				dGeomDisable( ( *iter ) ->geomID );
			}
		}
	}

	void ODESolid::internal_updateOPALTransform()
	{
		const real * R = ( const real* ) dBodyGetRotation( mBodyID );
		const real* P = ( const real* ) dBodyGetPosition( mBodyID );

		mData.transform.set( R[ 0 ], R[ 1 ], R[ 2 ], P[ 0 ],
		                     R[ 4 ], R[ 5 ], R[ 6 ], P[ 1 ],
		                     R[ 8 ], R[ 9 ], R[ 10 ], P[ 2 ],
		                     0, 0, 0, 1 );
	}

	void ODESolid::internal_updateEngineTransform()
	{
		dMatrix3 R = {mData.transform[ 0 ], mData.transform[ 4 ],
		              mData.transform[ 8 ], 0, mData.transform[ 1 ], mData.transform[ 5 ],
		              mData.transform[ 9 ], 0, mData.transform[ 2 ], mData.transform[ 6 ],
		              mData.transform[ 10 ], 0};

		if ( !mData.isStatic )
		{
			dBodySetRotation( mBodyID, R );
			dBodySetPosition( mBodyID, mData.transform[ 12 ],
			                  mData.transform[ 13 ], mData.transform[ 14 ] );
		}
		else if ( mIsPlaceable )
		{
			std::vector<GeomData*>::iterator iter;
			for ( iter = mGeomDataList.begin(); iter != mGeomDataList.end();
			        ++iter )
			{
				GeomData* geomData = ( *iter );
				if ( 0 == geomData->transformID )
				{
					// No geom transform.
					dGeomSetRotation( geomData->geomID, R );
					dGeomSetPosition( geomData->geomID, mData.transform[ 12 ],
					                  mData.transform[ 13 ], mData.transform[ 14 ] );
				}
				else
				{
					// Using geom transform.
					dGeomSetRotation( geomData->transformID, R );
					dGeomSetPosition( geomData->transformID,
					                  mData.transform[ 12 ], mData.transform[ 13 ],
					                  mData.transform[ 14 ] );
				}
			}
		}
	}

	void ODESolid::setSpace( Space* newSpace )
	{
		// update solid's space which will be used for future shapes
		mSpaceID = ( ( ODESpace* ) newSpace ) ->internal_getSpaceID();

		moveToSpace();
	}

	void ODESolid::setStatic( bool s )
	{
		if ( true == mData.isStatic )
		{
			if ( false == s )
			{
				// Make this object dynamic.
				mData.isStatic = false;

				// Create a new body.
				mBodyID = dBodyCreate( mWorldID );			

				// Set the ODE sleepiness params using the stored Solid
				// sleepiness param.
				setSleepiness( mData.sleepiness );

				// Set the position of the new body.
				dMatrix3 R =
				    {mData.transform[ 0 ], mData.transform[ 4 ],
				     mData.transform[ 8 ], 0, mData.transform[ 1 ],
				     mData.transform[ 5 ], mData.transform[ 9 ], 0,
				     mData.transform[ 2 ], mData.transform[ 6 ],
				     mData.transform[ 10 ], 0};
				dBodySetRotation( mBodyID, R );
				dBodySetPosition( mBodyID, mData.transform[ 12 ],
				                  mData.transform[ 13 ], mData.transform[ 14 ] );

				// Loop over the geoms.
				std::vector<GeomData*>::iterator iter;
				for ( iter = mGeomDataList.begin();
				        iter != mGeomDataList.end(); ++iter )
				{
					dMass newMass;

					// Get a pointer to this geom's ShapeData.
					ShapeData* shapeData = ( *iter ) ->shape;

					// Setup mass.
					switch ( shapeData->getType() )
					{
						case BOX_SHAPE:
							{
								BoxShapeData * boxData = ( BoxShapeData* ) shapeData;
								dMassSetBox( &newMass,
								             shapeData->material.density,
								             boxData->dimensions[ 0 ],
								             boxData->dimensions[ 1 ],
								             boxData->dimensions[ 2 ] );
								break;
							}
						case SPHERE_SHAPE:
							{
								SphereShapeData* sphereData =
								    ( SphereShapeData* ) shapeData;
								dMassSetSphere( &newMass,
								                shapeData->material.density,
								                sphereData->radius );
								break;
							}
						case CAPSULE_SHAPE:
							{
								CapsuleShapeData* capsuleData =
								    ( CapsuleShapeData* ) shapeData;
								dMassSetCappedCylinder( &newMass,
								                        shapeData->material.density, 3,
								                        capsuleData->radius,
								                        capsuleData->length );
								break;
							}
						case PLANE_SHAPE:
							// Planes have no mass.
							break;
							//case RAY_SHAPE:
							//	// Rays have no mass.
							//	break;
						case MESH_SHAPE:
							// This is a simple way to set the mass of an
							// arbitrary mesh.  Ideally we would compute
							// the exact volume and intertia tensor.
							// Instead we just use a box inertia tensor
							// from its axis-aligned bounding box.
							dReal aabb[ 6 ];
							dGeomGetAABB( ( *iter ) ->geomID, aabb );
							dMassSetBox( &newMass,
							             shapeData->material.density,
							             aabb[ 1 ] - aabb[ 0 ], aabb[ 3 ] - aabb[ 2 ],
							             aabb[ 5 ] - aabb[ 4 ] );
							break;
						default:
							assert( false );
					}

					// Setup the new mass.
					if ( iter == mGeomDataList.begin() )
						setMass( newMass, shapeData->offset );
					else
						addMass( newMass, shapeData->offset );

					// Add each geom to the new body.
					if ( 0 == ( *iter ) ->transformID )
					{
						// No geom transform.
						dGeomSetBody( ( *iter ) ->geomID, mBodyID );
					}
					else
					{
						// Use geom transform.
						dGeomSetBody( ( *iter ) ->transformID, mBodyID );
					}
				}

				moveToSpace();
			}
			else
			{
				// do nothing
			}
		}
		else // this object is not static
		{
			if ( true == s )
			{
				// make this object static
				mData.isStatic = true;

				// destroy the body
				dBodyDestroy( mBodyID );

				moveToSpace();
			}
			else
			{
				// do nothing
			}
		}
	}

	void ODESolid::moveToSpace()
	{
		// remove all current shapes from their spaces and add them to the
		// new one
		std::vector<GeomData*>::iterator iter;
		for ( iter = mGeomDataList.begin(); iter != mGeomDataList.end();
		        ++iter )
		{
			if ( 0 != ( *iter ) ->transformID )
			{
				// This geom uses a transform, so apply the new space only
				// to the transform geom.
				dSpaceRemove( ( *iter ) ->spaceID, ( *iter ) ->transformID );
				dSpaceAdd( mSpaceID, ( *iter ) ->transformID );
			}
			else
			{
				// Normal geom with no transform.
				dSpaceRemove( ( *iter ) ->spaceID, ( *iter ) ->geomID );
				dSpaceAdd( mSpaceID, ( *iter ) ->geomID );
			}

			( *iter ) ->spaceID = mSpaceID;
		}
	}

	void ODESolid::setSleeping( bool sleeping )
	{
		if ( mData.isStatic )
		{
			return ;
		}

		// Note: mData.sleeping gets updated in Solid::getData.

		if ( sleeping )
		{
			dBodyDisable( mBodyID );
		}
		else
		{
			dBodyEnable( mBodyID );
		}
	}

	bool ODESolid::isSleeping() const
	{
		if ( mData.isStatic )
		{
			return true;
		}

		// The ODE body may fall asleep at unknown times, so we need to
		// get the data straight from ODE.
		if ( dBodyIsEnabled( mBodyID ) )
		{
			return false;
		}
		else
		{
			return true;
		}
	}

	void ODESolid::setSleepiness( real s )
	{
		Solid::setSleepiness( s );

		/// If this Solid has no ODE body, just return.  The sleepiness
		/// level will still be saved and applied if this Solid ever
		/// becomes dynamic.
		if ( mData.isStatic )
		{
			return ;
		}

		if ( 0 == s )
		{
			// No sleeping at all for the Solid.
			dBodySetAutoDisableFlag( mBodyID, false );
		}
		else
		{
			// Enable sleeping for the Solid..
			dBodySetAutoDisableFlag( mBodyID, true );
		}

		// As value goes from 0.0 to 1.0:
		// AutoDisableLinearThreshold goes from min to max,
		// AutoDisableAngularThreshold goes from min to max,
		// AutoDisableSteps goes from max to min,
		// AutoDisableTime goes from max to min.

		real range = defaults::ode::autoDisableLinearMax -
		             defaults::ode::autoDisableLinearMin;
		dBodySetAutoDisableLinearThreshold( mBodyID, s * range +
		                                    defaults::ode::autoDisableLinearMin );

		range = defaults::ode::autoDisableAngularMax -
		        defaults::ode::autoDisableAngularMin;
		dBodySetAutoDisableAngularThreshold( mBodyID, s * range +
		                                     defaults::ode::autoDisableAngularMin );

		range = ( real ) ( defaults::ode::autoDisableStepsMax -
		                   defaults::ode::autoDisableStepsMin );
		dBodySetAutoDisableSteps( mBodyID,
		                          ( int ) ( ( real ) defaults::ode::autoDisableStepsMax - s * range ) );

		range = defaults::ode::autoDisableTimeMax -
		        defaults::ode::autoDisableTimeMin;
		dBodySetAutoDisableTime( mBodyID,
		                         defaults::ode::autoDisableTimeMax - s * range );
	}

	void ODESolid::translateMass( const Vec3r& offset )
	{
		if ( !mData.isStatic )
		{
			dMass m;
			dBodyGetMass( mBodyID, &m );
			dMassTranslate( &m, offset[ 0 ], offset[ 1 ], offset[ 2 ] );
			dBodySetMass( mBodyID, &m );

			// Update this since the mass changed.
			mNonSymmetricInertia = isInertiaNonSymmetric( m );
		}
	}

	void ODESolid::addShape( ShapeData& data )
	{
		assert( data.material.density >= 0 );

		dGeomID newGeomID = NULL;
		dGeomID newTransformID = NULL;
		dTriMeshDataID newTrimeshDataID = NULL;
		dSpaceID spaceID = NULL;
		dMass newMass;

		if ( Matrix44r() == data.offset )
		{
			// No offset transform.
			spaceID = mSpaceID;
			newTransformID = 0;
		}
		else
		{
			// Use ODE's geom transform object.
			spaceID = 0;
			newTransformID = dCreateGeomTransform( mSpaceID );
		}

		// Allocate a new GeomData object.
		GeomData* newGeomData = new GeomData;

		switch ( data.getType() )
		{
			case BOX_SHAPE:
				{
					BoxShapeData & boxData = ( BoxShapeData& ) data;
					newGeomID = dCreateBox( spaceID,
					                        ( dReal ) boxData.dimensions[ 0 ],
					                        ( dReal ) boxData.dimensions[ 1 ],
					                        ( dReal ) boxData.dimensions[ 2 ] );
					dMassSetBox( &newMass, ( dReal ) data.material.density,
					             ( dReal ) boxData.dimensions[ 0 ],
					             ( dReal ) boxData.dimensions[ 1 ],
					             ( dReal ) boxData.dimensions[ 2 ] );
					break;
				}
			case SPHERE_SHAPE:
				{
					SphereShapeData& sphereData = ( SphereShapeData& ) data;
					newGeomID = dCreateSphere( spaceID,
					                           ( dReal ) sphereData.radius );
					dMassSetSphere( &newMass, ( dReal ) data.material.density,
					                ( dReal ) sphereData.radius );
					break;
				}
			case CAPSULE_SHAPE:
				{
					CapsuleShapeData& capsuleData = ( CapsuleShapeData& ) data;
					newGeomID = dCreateCCylinder( spaceID,
					                              ( dReal ) capsuleData.radius, ( dReal ) capsuleData.length );

					// The "direction" parameter orients the mass along one of the
					// body's local axes; x=1, y=2, z=3.  This axis MUST
					// correspond with the axis along which the capsule is
					// initially aligned, which is the capsule's local Z axis.
					dMassSetCappedCylinder( &newMass, ( dReal ) data.material.density,
					                        3, ( dReal ) capsuleData.radius, ( dReal ) capsuleData.length );
					break;
				}
			case PLANE_SHAPE:
				{
					PlaneShapeData& planeData = ( PlaneShapeData& ) data;
					if ( !mData.isStatic )
					{
						OPAL_LOGGER( "warning" ) << "opal::ODESolid::addPlane: " <<
						"Plane Shape added to a non-static Solid.  " <<
						"The Solid will be made static." << std::endl;

						// ODE planes can't have bodies, so make it static.
						setStatic( true );
					}

					// TODO: make this fail gracefully and print warning: plane
					// offset transform ignored.
					assert( !newTransformID );

					// ODE planes must have their normal vector (abc) normalized.
					Vec3r normal( planeData.abcd[ 0 ], planeData.abcd[ 1 ],
					              planeData.abcd[ 2 ] );
					normal.normalize();

					newGeomID = dCreatePlane( spaceID, ( dReal ) normal[ 0 ],
					                          ( dReal ) normal[ 1 ], ( dReal ) normal[ 2 ],
					                          ( dReal ) planeData.abcd[ 3 ] );

					// Note: ODE planes cannot have mass, but this is already
					// handled since static Solids ignore mass.

					// Solids with planes are the only non-"placeable" Solids.
					mIsPlaceable = false;
					break;
				}
				//case RAY_SHAPE:
				//{
				//	RayShapeData& rayData = (RayShapeData&)data;
				//	newGeomID = dCreateRay(spaceID,
				//		(dReal)rayData.ray.getLength());
				//	Point3r origin = rayData.ray.getOrigin();
				//	Vec3r dir = rayData.ray.getDir();
				//	dGeomRaySet(newGeomID, (dReal)origin[0], (dReal)origin[1],
				//		(dReal)origin[2], (dReal)dir[0], (dReal)dir[1],
				//		(dReal)dir[2]);
				//	// Note: rays don't have mass.
				//	break;
				//}
			case MESH_SHAPE:
				{
					MeshShapeData& meshData = ( MeshShapeData& ) data;

					// Setup trimesh data pointer.  It is critical that the
					// size of OPAL reals at this point match the size of ODE's
					// dReals.
					newTrimeshDataID = dGeomTriMeshDataCreate();

					// Old way... This is problematic because ODE interprets
					// the vertex array as an array of dVector3s which each
					// have 4 elements.
					//dGeomTriMeshDataBuildSimple(newTrimeshDataID,
					//	(dReal*)meshData.vertexArray, meshData.numVertices,
					//	(int*)meshData.triangleArray, 3 * meshData.numTriangles);

#ifdef dSINGLE
					dGeomTriMeshDataBuildSingle( newTrimeshDataID,
					                             ( void* ) meshData.vertexArray, 3 * sizeof( real ),
					                             meshData.numVertices, ( void* ) meshData.triangleArray,
					                             3 * meshData.numTriangles, 3 * sizeof( unsigned int ) );
#else
					dGeomTriMeshDataBuildDouble( newTrimeshDataID,
												 ( void* ) meshData.vertexArray, 3 * sizeof( real ),
												 meshData.numVertices, ( void* ) meshData.triangleArray,
												 3 * meshData.numTriangles, 3 * sizeof( unsigned int ) );
#endif

					newGeomID = dCreateTriMesh( spaceID,
					                            newTrimeshDataID, NULL, NULL, NULL );

					// This is a simple way to set the mass of an arbitrary
					// mesh.  Ideally we would compute the exact volume and
					// intertia tensor.  Instead we just use a box
					// inertia tensor from its axis-aligned bounding box.
					dReal aabb[ 6 ];
					dGeomGetAABB( newGeomID, aabb );
					dMassSetBox( &newMass, ( dReal ) data.material.density,
					             aabb[ 1 ] - aabb[ 0 ], aabb[ 3 ] - aabb[ 2 ],
					             aabb[ 5 ] - aabb[ 4 ] );
					break;
				}
			default:
				assert( false );
		}

		// This will do nothing if this is a static Solid.
		addMass( newMass, data.offset );

		// Store new Shape.
		mData.addShape( data );

		// Update the Solid's local AABB.  The new shape's local AABB
		// must be transformed using the shape's offset from the Solid.
		real shapeAABB[ 6 ] = {0, 0, 0, 0, 0, 0};
		data.getLocalAABB( shapeAABB );
		Point3r minExtents( shapeAABB[ 0 ], shapeAABB[ 2 ], shapeAABB[ 4 ] );
		Point3r maxExtents( shapeAABB[ 1 ], shapeAABB[ 3 ], shapeAABB[ 5 ] );
		minExtents = data.offset * minExtents;
		maxExtents = data.offset * maxExtents;
		shapeAABB[ 0 ] = minExtents[ 0 ];
		shapeAABB[ 1 ] = maxExtents[ 0 ];
		shapeAABB[ 2 ] = minExtents[ 1 ];
		shapeAABB[ 3 ] = maxExtents[ 1 ];
		shapeAABB[ 4 ] = minExtents[ 2 ];
		shapeAABB[ 5 ] = maxExtents[ 2 ];
		addToLocalAABB( shapeAABB );

		// Setup GeomData.
		newGeomData->solid = this;
		newGeomData->shape = mData.getShapeData( mData.getNumShapes() - 1 );
		newGeomData->geomID = newGeomID;
		newGeomData->transformID = newTransformID;
		newGeomData->spaceID = mSpaceID;
		newGeomData->trimeshDataID = newTrimeshDataID;

		// Setup the geom.
		setupNewGeom( newGeomData );
	}

	void ODESolid::setLocalLinearVel( const Vec3r& vel )
	{
		if ( !mData.isStatic )
		{
			dVector3 worldVel;
			dBodyVectorToWorld( mBodyID, vel[ 0 ], vel[ 1 ], vel[ 2 ], worldVel );
			dBodySetLinearVel( mBodyID, worldVel[ 0 ], worldVel[ 1 ], worldVel[ 2 ] );

			// Invalidate the "freely-spinning" parameter.
			internal_setFreelySpinning( false );
		}
	}

	Vec3r ODESolid::getLocalLinearVel() const
	{
		if ( !mData.isStatic )
		{
			const dReal * vel = dBodyGetLinearVel( mBodyID );

			// Convert the vector from global to local coordinates.
			dVector3 localVel;
			dBodyVectorFromWorld( mBodyID, vel[ 0 ], vel[ 1 ], vel[ 2 ], localVel );
			return toVec3r( localVel );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	Vec3r ODESolid::getLocalLinearVelAtLocalPos( const Point3r& p ) const
	{
		if ( !mData.isStatic )
		{
			// First find the global velocity at the given point.
			dVector3 result;
			dBodyGetRelPointVel( mBodyID, p[ 0 ], p[ 1 ], p[ 2 ], result );

			// Now convert the velocity from global to local coordinates.
			dBodyVectorFromWorld( mBodyID, result[ 0 ], result[ 1 ], result[ 2 ],
			                      result );
			return toVec3r( result );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	void ODESolid::setLocalAngularVel( const Vec3r& vel )
	{
		if ( !mData.isStatic )
		{
			dVector3 worldVel;
			dBodyVectorToWorld( mBodyID, vel[ 0 ], vel[ 1 ], vel[ 2 ], worldVel );
			Vec3r velRad = toVec3r_DegToRad( worldVel );
			dBodySetAngularVel( mBodyID, velRad[ 0 ], velRad[ 1 ], velRad[ 2 ] );

			// Invalidate the "freely-spinning" parameter.
			internal_setFreelySpinning( false );
		}
	}

	Vec3r ODESolid::getLocalAngularVel() const
	{
		if ( !mData.isStatic )
		{
			const dReal * vel = dBodyGetAngularVel( mBodyID );

			// Convert the vector from global to local coordinates.
			dVector3 localVel;
			dBodyVectorFromWorld( mBodyID, vel[ 0 ], vel[ 1 ], vel[ 2 ], localVel );
			return toVec3r_RadToDeg( localVel );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	void ODESolid::setGlobalLinearVel( const Vec3r& vel )
	{
		if ( !mData.isStatic )
		{
			dBodySetLinearVel( mBodyID, vel[ 0 ], vel[ 1 ], vel[ 2 ] );

			// Invalidate the "freely-spinning" parameter.
			internal_setFreelySpinning( false );
		}
	}

	Vec3r ODESolid::getGlobalLinearVel() const
	{
		if ( !mData.isStatic )
		{
			const dReal * vel = dBodyGetLinearVel( mBodyID );
			return toVec3r( vel );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	Vec3r ODESolid::getGlobalLinearVelAtLocalPos( const Point3r& p ) const
	{
		if ( !mData.isStatic )
		{
			// First find the global velocity at the given point.
			dVector3 result;
			dBodyGetRelPointVel( mBodyID, p[ 0 ], p[ 1 ], p[ 2 ], result );
			return toVec3r( result );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	void ODESolid::setGlobalAngularVel( const Vec3r& vel )
	{
		if ( !mData.isStatic )
		{
			Vec3r velRad( degToRad( vel[ 0 ] ), degToRad( vel[ 1 ] ), degToRad( vel[ 2 ] ) );
			dBodySetAngularVel( mBodyID, velRad[ 0 ], velRad[ 1 ], velRad[ 2 ] );

			// Invalidate the "freely-spinning" parameter.
			internal_setFreelySpinning( false );
		}
	}

	Vec3r ODESolid::getGlobalAngularVel() const
	{
		if ( !mData.isStatic )
		{
			const dReal * vel = dBodyGetAngularVel( mBodyID );
			return toVec3r_RadToDeg( vel );
		}
		else
		{
			return Vec3r( 0, 0, 0 );
		}
	}

	dBodyID ODESolid::internal_getBodyID() const
	{
		return mBodyID;
	}

	void ODESolid::internal_setCollisionCount( long int count )
	{
		mCollisionCount = count;
	}

	long int ODESolid::internal_getCollisionCount() const
	{
		return mCollisionCount;
	}

	//void ODESolid::setFastRotation(bool fast)
	//{
	//	if (mStatic)
	//	{
	//		return;
	//	}
	//
	//	if (fast)
	//	{
	//		//if used, call dBodySetFiniteRotationAxis every step; see ODE docs
	//		dBodySetFiniteRotationMode(mBodyID, 1);
	//	}
	//	else
	//	{
	//		dBodySetFiniteRotationMode(mBodyID, 0);
	//	}
	//}

	//bool ODESolid::getFastRotation()const
	//{
	//	if (mStatic)
	//	{
	//		return false;
	//	}
	//
	//	if(1 == dBodyGetFiniteRotationMode(mBodyID))
	//	{
	//		return true;
	//	}
	//	else
	//	{
	//		return false;
	//	}
	//}

	//void ODESolid::setFastRotationAxis(Vec3r axis)
	//{
	//	dBodySetFiniteRotationAxis(mBodyID, axis[0], axis[1], axis[2]);
	//}

	void ODESolid::zeroForces()
	{
		if ( !mData.isStatic )
		{
			dBodySetForce( mBodyID, 0, 0, 0 );
			dBodySetTorque( mBodyID, 0, 0, 0 );

			while ( !mForceList.empty() )
			{
				mForceList.pop_back();
			}
		}
	}

	real ODESolid::getMass() const
	{
		if ( mData.isStatic )
		{
			return 0;
		}
		else
		{
			dMass mass;
			dBodyGetMass( mBodyID, &mass );
			return ( real ) mass.mass;
		}
	}

	Matrix44r ODESolid::getInertiaTensor() const
	{
		Matrix44r m;

		if ( mData.isStatic )
		{
			return m;
		}
		else
		{
			dMass mass;
			dBodyGetMass( mBodyID, &mass );
			m.set(
			    ( real ) mass.I[ 0 ], ( real ) mass.I[ 1 ], ( real ) mass.I[ 2 ], 0,
			    ( real ) mass.I[ 4 ], ( real ) mass.I[ 5 ], ( real ) mass.I[ 6 ], 0,
			    ( real ) mass.I[ 8 ], ( real ) mass.I[ 9 ], ( real ) mass.I[ 10 ], 0,
			    0, 0, 0, 1 );

			return m;
		}
	}

	void ODESolid::setupNewGeom( GeomData* newGeom )
	{
		if ( !mData.isStatic )
		{
			if ( 0 == newGeom->transformID )
			{
				// No geom transform.
				dGeomSetBody( newGeom->geomID, mBodyID );
			}
			else
			{
				// Use geom transform.
				dGeomSetBody( newGeom->transformID, mBodyID );
			}
		}

		if ( 0 != newGeom->transformID )
		{
			// Setup geom transform.
			dGeomTransformSetGeom( newGeom->transformID, newGeom->geomID );
			dMatrix3 R =
			    {newGeom->shape->offset[ 0 ], newGeom->shape->offset[ 4 ],
			     newGeom->shape->offset[ 8 ], 0,
			     newGeom->shape->offset[ 1 ], newGeom->shape->offset[ 5 ],
			     newGeom->shape->offset[ 9 ], 0,
			     newGeom->shape->offset[ 2 ], newGeom->shape->offset[ 6 ],
			     newGeom->shape->offset[ 10 ], 0};
			dGeomSetRotation( newGeom->geomID, R );
			dGeomSetPosition( newGeom->geomID, newGeom->shape->offset[ 12 ],
			                  newGeom->shape->offset[ 13 ],
			                  newGeom->shape->offset[ 14 ] );
		}

		// Set the GeomData reference for later use (e.g. in collision
		// handling).
		if ( 0 == newGeom->transformID )
		{
			// No geom transform.
			dGeomSetData( newGeom->geomID, newGeom );
		}
		else
		{
			// Using geom transform.
			dGeomSetData( newGeom->transformID, newGeom );
		}

		// Store the GeomData pointer.
		mGeomDataList.push_back( newGeom );

		// Make sure the initial transform is setup; this needs to come after
		// the geom data has been added.
		if ( mData.isStatic && mIsPlaceable )
		{
			setTransform( mData.transform );
		}
	}

	void ODESolid::applyForce( const Force& f )
	{
		switch ( f.type )
		{
			case LOCAL_FORCE:
				dBodyAddRelForce( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ] );
				break;
			case GLOBAL_FORCE:
				dBodyAddForce( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ] );
				break;
			case LOCAL_TORQUE:
				dBodyAddRelTorque( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ] );
				break;
			case GLOBAL_TORQUE:
				dBodyAddTorque( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ] );
				break;
			case LOCAL_FORCE_AT_LOCAL_POS:
				dBodyAddRelForceAtRelPos( mBodyID, f.vec[ 0 ], f.vec[ 1 ],
				                          f.vec[ 2 ], f.pos[ 0 ], f.pos[ 1 ], f.pos[ 2 ] );
				break;
			case LOCAL_FORCE_AT_GLOBAL_POS:
				dBodyAddRelForceAtPos( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ],
				                       f.pos[ 0 ], f.pos[ 1 ], f.pos[ 2 ] );
				break;
			case GLOBAL_FORCE_AT_LOCAL_POS:
				dBodyAddForceAtRelPos( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ],
				                       f.pos[ 0 ], f.pos[ 1 ], f.pos[ 2 ] );
				break;
			case GLOBAL_FORCE_AT_GLOBAL_POS:
				dBodyAddForceAtPos( mBodyID, f.vec[ 0 ], f.vec[ 1 ], f.vec[ 2 ],
				                    f.pos[ 0 ], f.pos[ 1 ], f.pos[ 2 ] );
				break;
		}

		// Invalidate the "freely-spinning" parameter.
		internal_setFreelySpinning( false );
	}

	void ODESolid::setMass( const Mass & newmass, const Matrix44r & offset )
	{
		if ( mData.isStatic )
		{
			return ;
		}

		// first create mass object
		dMass newMass;
		newMass.setParameters( newmass.mass,
		                       newmass.center.x,
		                       newmass.center.y,
		                       newmass.center.z,
		                       newmass.inertia[ 0 ],
		                       newmass.inertia[ 5 ],
		                       newmass.inertia[ 10 ],
		                       newmass.inertia[ 1 ],
		                       newmass.inertia[ 2 ],
		                       newmass.inertia[ 6 ] );

		// First rotate and translate the new mass.
		dMatrix3 R = {offset[ 0 ], offset[ 4 ], offset[ 8 ], 0,
		              offset[ 1 ], offset[ 5 ], offset[ 9 ], 0,
		              offset[ 2 ], offset[ 6 ], offset[ 10 ], 0};

		dMassRotate( &newMass, R );
		dMassTranslate( &newMass, offset[ 12 ], offset[ 13 ], offset[ 14 ] );

		dBodySetMass( mBodyID, &newMass );

		// Update this since the mass changed.
		dMass m;
		dBodyGetMass( mBodyID, &m );
		mNonSymmetricInertia = isInertiaNonSymmetric( m );
	}

	void ODESolid::addMass( dMass& newMass, const Matrix44r& offset )
	{
		if ( mData.isStatic )
		{
			return ;
		}

		// First rotate and translate the new mass.
		dMatrix3 R = {offset[ 0 ], offset[ 4 ], offset[ 8 ], 0,
		              offset[ 1 ], offset[ 5 ], offset[ 9 ], 0,
		              offset[ 2 ], offset[ 6 ], offset[ 10 ], 0};

		dMassRotate( &newMass, R );
		dMassTranslate( &newMass, offset[ 12 ], offset[ 13 ], offset[ 14 ] );

		// If this mass is for the first Shape, just set the Solid's mass
		// equal to the new one.  ODE bodies start with an initial mass
		// already setup, but we want to ignore that, not add to it.
		if ( 0 == mGeomDataList.size() )
		{
			dBodySetMass( mBodyID, &newMass );
		}
		else
		{
			// Add new mass to the Solid's existing mass.  First get the
			// existing mass struct from ODE.
			dMass totalMass;
			dBodyGetMass( mBodyID, &totalMass );
			dMassAdd( &totalMass, &newMass );
			dBodySetMass( mBodyID, &totalMass );
		}

		// Update this since the mass changed.
		dMass m;
		dBodyGetMass( mBodyID, &m );
		mNonSymmetricInertia = isInertiaNonSymmetric( m );
	}

	void ODESolid::setMass( dMass& newMass, const Matrix44r& offset )
	{
		if ( mData.isStatic )
		{
			return ;
		}

		// First rotate and translate the new mass.
		dMatrix3 R = {offset[ 0 ], offset[ 4 ], offset[ 8 ], 0,
		              offset[ 1 ], offset[ 5 ], offset[ 9 ], 0,
		              offset[ 2 ], offset[ 6 ], offset[ 10 ], 0};

		dMassRotate( &newMass, R );
		dMassTranslate( &newMass, offset[ 12 ], offset[ 13 ], offset[ 14 ] );

		dBodySetMass( mBodyID, &newMass );

		// Update this since the mass changed.
		dMass m;
		dBodyGetMass( mBodyID, &m );
		mNonSymmetricInertia = isInertiaNonSymmetric( m );
	}

	const std::vector<GeomData*>* ODESolid::internal_getGeomDataList() const
	{
		return & mGeomDataList;
	}

	void ODESolid::internal_doAngularVelFix()
	{
		if ( mNonSymmetricInertia )
		{
			Vec3r vel = getGlobalAngularVel();
			real currentAngVelMagSquared = vel.lengthSquared();

			if ( mIsFreelySpinning )
			{
				// If the current angular velocity magnitude is greater than
				// that of the previous step, scale it by that of the previous
				// step; otherwise, update the previous value to that of the
				// current step.  This ensures that angular velocity never
				// increases for freely-spinning objects.

				if ( currentAngVelMagSquared > mPrevAngVelMagSquared )
				{
					real currentAngVelMag = sqrt( currentAngVelMagSquared );
					vel = vel / currentAngVelMag;
					// Vel is now a unit vector.  Next, scale this vector
					// by the previous angular velocity magnitude.
					real prevAngVelMag = sqrt( mPrevAngVelMagSquared );
					setGlobalAngularVel( vel * prevAngVelMag );
				}
			}

			mPrevAngVelMagSquared = currentAngVelMagSquared;
		}

		// Reset the "freely-spinning" parameter for the next time step.
		internal_setFreelySpinning( true );
	}

	void ODESolid::internal_setFreelySpinning( bool fs )
	{
		mIsFreelySpinning = fs;
	}

	bool ODESolid::isInertiaNonSymmetric( const dMass& mass ) const
	{
		if ( !areEqual( ( real ) mass.I[ 0 ], ( real ) mass.I[ 5 ] )
		        || !areEqual( ( real ) mass.I[ 5 ], ( real ) mass.I[ 10 ] ) )
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}
