/*
===========================================================================

Doom 3 GPL Source Code
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.

This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").

Doom 3 Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 Source Code 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
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

===========================================================================
*/

#ifndef __PHYSICS_AF_H__
#define __PHYSICS_AF_H__

#include "idlib/math/Lcp.h"

#include "physics/Physics_Base.h"

/*
===================================================================================

	Articulated Figure physics

	Employs a constraint force based dynamic simulation using a lagrangian
	multiplier method to solve for the constraint forces.

===================================================================================
*/

class idAFConstraint;
class idAFConstraint_Fixed;
class idAFConstraint_BallAndSocketJoint;
class idAFConstraint_BallAndSocketJointFriction;
class idAFConstraint_UniversalJoint;
class idAFConstraint_UniversalJointFriction;
class idAFConstraint_CylindricalJoint;
class idAFConstraint_Hinge;
class idAFConstraint_HingeFriction;
class idAFConstraint_HingeSteering;
class idAFConstraint_Slider;
class idAFConstraint_Line;
class idAFConstraint_Plane;
class idAFConstraint_Spring;
class idAFConstraint_Contact;
class idAFConstraint_ContactFriction;
class idAFConstraint_ConeLimit;
class idAFConstraint_PyramidLimit;
class idAFConstraint_Suspension;
class idAFBody;
class idAFTree;
class idPhysics_AF;

typedef enum {
	CONSTRAINT_INVALID,
	CONSTRAINT_FIXED,
	CONSTRAINT_BALLANDSOCKETJOINT,
	CONSTRAINT_UNIVERSALJOINT,
	CONSTRAINT_HINGE,
	CONSTRAINT_HINGESTEERING,
	CONSTRAINT_SLIDER,
	CONSTRAINT_CYLINDRICALJOINT,
	CONSTRAINT_LINE,
	CONSTRAINT_PLANE,
	CONSTRAINT_SPRING,
	CONSTRAINT_CONTACT,
	CONSTRAINT_FRICTION,
	CONSTRAINT_CONELIMIT,
	CONSTRAINT_PYRAMIDLIMIT,
	CONSTRAINT_SUSPENSION
} constraintType_t;


//===============================================================
//
//	idAFConstraint
//
//===============================================================

// base class for all constraints
class idAFConstraint {

	friend class idPhysics_AF;
	friend class idAFTree;

public:
							idAFConstraint( void );
	virtual					~idAFConstraint( void );
	constraintType_t		GetType( void ) const { return type; }
	const idStr &			GetName( void ) const { return name; }
	idAFBody *				GetBody1( void ) const { return body1; }
	idAFBody *				GetBody2( void ) const { return body2; }
	void					SetPhysics( idPhysics_AF *p ) { physics = p; }
	const idVecX &			GetMultiplier( void );
	virtual void			SetBody1( idAFBody *body );
	virtual void			SetBody2( idAFBody *body );
	virtual void			DebugDraw( void );
	virtual void			GetForce( idAFBody *body, idVec6 &force );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	constraintType_t		type;						// constraint type
	idStr					name;						// name of constraint
	idAFBody *				body1;						// first constrained body
	idAFBody *				body2;						// second constrained body, NULL for world
	idPhysics_AF *			physics;					// for adding additional constraints like limits

							// simulation variables set by Evaluate
	idMatX					J1, J2;						// matrix with left hand side of constraint equations
	idVecX					c1, c2;						// right hand side of constraint equations
	idVecX					lo, hi, e;					// low and high bounds and lcp epsilon
	idAFConstraint *		boxConstraint;				// constraint the boxIndex refers to
	int						boxIndex[6];				// indexes for special box constrained variables

							// simulation variables used during calculations
	idMatX					invI;						// transformed inertia
	idMatX					J;							// transformed constraint matrix
	idVecX					s;							// temp solution
	idVecX					lm;							// lagrange multipliers
	int						firstIndex;					// index of the first constraint row in the lcp matrix

	struct constraintFlags_s {
		bool				allowPrimary		: 1;	// true if the constraint can be used as a primary constraint
		bool				frameConstraint		: 1;	// true if this constraint is added to the frame constraints
		bool				noCollision			: 1;	// true if body1 and body2 never collide with each other
		bool				isPrimary			: 1;	// true if this is a primary constraint
		bool				isZero				: 1;	// true if 's' is zero during calculations
	} fl;

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
	void					InitSize( int size );
};

// fixed or rigid joint which allows zero degrees of freedom
// constrains body1 to have a fixed position and orientation relative to body2
class idAFConstraint_Fixed : public idAFConstraint {

public:
							idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 );
	void					SetRelativeOrigin( const idVec3 &origin ) { this->offset = origin; }
	void					SetRelativeAxis( const idMat3 &axis ) { this->relAxis = axis; }
	virtual void			SetBody1( idAFBody *body );
	virtual void			SetBody2( idAFBody *body );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					offset;						// offset of body1 relative to body2 in body2 space
	idMat3					relAxis;					// rotation of body1 relative to body2

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
	void					InitOffset( void );
};

// ball and socket or spherical joint which allows 3 degrees of freedom
// constrains body1 relative to body2 with a ball and socket joint
class idAFConstraint_BallAndSocketJoint : public idAFConstraint {

public:
							idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
							~idAFConstraint_BallAndSocketJoint( void );
	void					SetAnchor( const idVec3 &worldPosition );
	idVec3					GetAnchor( void ) const;
	void					SetNoLimit( void );
	void					SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis );
	void					SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
											const float angle1, const float angle2, const idVec3 &body1Axis );
	void					SetLimitEpsilon( const float e );
	void					SetFriction( const float f ) { friction = f; }
	float					GetFriction( void ) const;
	virtual void			DebugDraw( void );
	virtual void			GetForce( idAFBody *body, idVec6 &force );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					anchor1;					// anchor in body1 space
	idVec3					anchor2;					// anchor in body2 space
	float					friction;					// joint friction
	idAFConstraint_ConeLimit *coneLimit;				// cone shaped limit
	idAFConstraint_PyramidLimit *pyramidLimit;			// pyramid shaped limit
	idAFConstraint_BallAndSocketJointFriction *fc;		// friction constraint

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// ball and socket joint friction
class idAFConstraint_BallAndSocketJointFriction : public idAFConstraint {

public:
							idAFConstraint_BallAndSocketJointFriction( void );
	void					Setup( idAFConstraint_BallAndSocketJoint *cc );
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:
	idAFConstraint_BallAndSocketJoint *joint;

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// universal, Cardan or Hooke joint which allows 2 degrees of freedom
// like a ball and socket joint but also constrains the rotation about the cardan shafts
class idAFConstraint_UniversalJoint : public idAFConstraint {

public:
							idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
							~idAFConstraint_UniversalJoint( void );
	void					SetAnchor( const idVec3 &worldPosition );
	idVec3					GetAnchor( void ) const;
	void					SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 );
	void					GetShafts( idVec3 &cardanShaft1, idVec3 &cardanShaft2 ) { cardanShaft1 = shaft1; cardanShaft2 = shaft2; }
	void					SetNoLimit( void );
	void					SetConeLimit( const idVec3 &coneAxis, const float coneAngle );
	void					SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
											const float angle1, const float angle2 );
	void					SetLimitEpsilon( const float e );
	void					SetFriction( const float f ) { friction = f; }
	float					GetFriction( void ) const;
	virtual void			DebugDraw( void );
	virtual void			GetForce( idAFBody *body, idVec6 &force );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					anchor1;					// anchor in body1 space
	idVec3					anchor2;					// anchor in body2 space
	idVec3					shaft1;						// body1 cardan shaft in body1 space
	idVec3					shaft2;						// body2 cardan shaft in body2 space
	idVec3					axis1;						// cardan axis in body1 space
	idVec3					axis2;						// cardan axis in body2 space
	float					friction;					// joint friction
	idAFConstraint_ConeLimit *coneLimit;				// cone shaped limit
	idAFConstraint_PyramidLimit *pyramidLimit;			// pyramid shaped limit
	idAFConstraint_UniversalJointFriction *fc;			// friction constraint

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// universal joint friction
class idAFConstraint_UniversalJointFriction : public idAFConstraint {

public:
							idAFConstraint_UniversalJointFriction( void );
	void					Setup( idAFConstraint_UniversalJoint *cc );
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:
	idAFConstraint_UniversalJoint *joint;			// universal joint

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// cylindrical joint which allows 2 degrees of freedom
// constrains body1 to lie on a line relative to body2 and allows only translation along and rotation about the line
class idAFConstraint_CylindricalJoint : public idAFConstraint {

public:
							idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// hinge, revolute or pin joint which allows 1 degree of freedom
// constrains all motion of body1 relative to body2 except the rotation about the hinge axis
class idAFConstraint_Hinge : public idAFConstraint {

public:
							idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 );
							~idAFConstraint_Hinge( void );
	void					SetAnchor( const idVec3 &worldPosition );
	idVec3					GetAnchor( void ) const;
	void					SetAxis( const idVec3 &axis );
	void					GetAxis( idVec3 &a1, idVec3 &a2 ) const { a1 = axis1; a2 = axis2; }
	idVec3					GetAxis( void ) const;
	void					SetNoLimit( void );
	void					SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis );
	void					SetLimitEpsilon( const float e );
	float					GetAngle( void ) const;
	void					SetSteerAngle( const float degrees );
	void					SetSteerSpeed( const float speed );
	void					SetFriction( const float f ) { friction = f; }
	float					GetFriction( void ) const;
	virtual void			DebugDraw( void );
	virtual void			GetForce( idAFBody *body, idVec6 &force );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					anchor1;					// anchor in body1 space
	idVec3					anchor2;					// anchor in body2 space
	idVec3					axis1;						// axis in body1 space
	idVec3					axis2;						// axis in body2 space
	idMat3					initialAxis;				// initial axis of body1 relative to body2
	float					friction;					// hinge friction
	idAFConstraint_ConeLimit *coneLimit;				// cone limit
	idAFConstraint_HingeSteering *steering;				// steering
	idAFConstraint_HingeFriction *fc;					// friction constraint

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// hinge joint friction
class idAFConstraint_HingeFriction : public idAFConstraint {

public:
							idAFConstraint_HingeFriction( void );
	void					Setup( idAFConstraint_Hinge *cc );
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:
	idAFConstraint_Hinge *	hinge;						// hinge

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// constrains two bodies attached to each other with a hinge to get a specified relative orientation
class idAFConstraint_HingeSteering : public idAFConstraint {

public:
							idAFConstraint_HingeSteering( void );
	void					Setup( idAFConstraint_Hinge *cc );
	void					SetSteerAngle( const float degrees ) { steerAngle = degrees; }
	void					SetSteerSpeed( const float speed ) { steerSpeed = speed; }
	void					SetEpsilon( const float e ) { epsilon = e; }
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idAFConstraint_Hinge *	hinge;						// hinge
	float					steerAngle;					// desired steer angle in degrees
	float					steerSpeed;					// steer speed
	float					epsilon;					// lcp epsilon

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// slider, prismatic or translational constraint which allows 1 degree of freedom
// constrains body1 to lie on a line relative to body2, the orientation is also fixed relative to body2
class idAFConstraint_Slider : public idAFConstraint {

public:
							idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 );
	void					SetAxis( const idVec3 &ax );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					axis;						// axis along which body1 slides in body2 space
	idVec3					offset;						// offset of body1 relative to body2
	idMat3					relAxis;					// rotation of body1 relative to body2

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// line constraint which allows 4 degrees of freedom
// constrains body1 to lie on a line relative to body2, does not constrain the orientation.
class idAFConstraint_Line : public idAFConstraint {

public:
							idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// plane constraint which allows 5 degrees of freedom
// constrains body1 to lie in a plane relative to body2, does not constrain the orientation.
class idAFConstraint_Plane : public idAFConstraint {

public:
							idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 );
	void					SetPlane( const idVec3 &normal, const idVec3 &anchor );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					anchor1;					// anchor in body1 space
	idVec3					anchor2;					// anchor in body2 space
	idVec3					planeNormal;				// plane normal in body2 space

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// spring constraint which allows 6 or 5 degrees of freedom based on the spring limits
// constrains body1 relative to body2 with a spring
class idAFConstraint_Spring : public idAFConstraint {

public:
							idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 );
	void					SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 );
	void					SetSpring( const float stretch, const float compress, const float damping, const float restLength );
	void					SetLimit( const float minLength, const float maxLength );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					anchor1;					// anchor in body1 space
	idVec3					anchor2;					// anchor in body2 space
	float					kstretch;					// spring constant when stretched
	float					kcompress;					// spring constant when compressed
	float					damping;					// spring damping
	float					restLength;					// rest length of spring
	float					minLength;					// minimum spring length
	float					maxLength;					// maximum spring length

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// constrains body1 to either be in contact with or move away from body2
class idAFConstraint_Contact : public idAFConstraint {

public:
							idAFConstraint_Contact( void );
							~idAFConstraint_Contact( void );
	void					Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c );
	const contactInfo_t &	GetContact( void ) const { return contact; }
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			GetCenter( idVec3 &center );

protected:
	contactInfo_t			contact;					// contact information
	idAFConstraint_ContactFriction *fc;					// contact friction

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// contact friction
class idAFConstraint_ContactFriction : public idAFConstraint {

public:
							idAFConstraint_ContactFriction( void );
	void					Setup( idAFConstraint_Contact *cc );
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:
	idAFConstraint_Contact *cc;							// contact constraint

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// constrains an axis attached to body1 to be inside a cone relative to body2
class idAFConstraint_ConeLimit : public idAFConstraint {

public:
							idAFConstraint_ConeLimit( void );
	void					Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis,
									const float coneAngle, const idVec3 &body1Axis );
	void					SetAnchor( const idVec3 &coneAnchor );
	void					SetBody1Axis( const idVec3 &body1Axis );
	void					SetEpsilon( const float e ) { epsilon = e; }
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					coneAnchor;					// top of the cone in body2 space
	idVec3					coneAxis;					// cone axis in body2 space
	idVec3					body1Axis;					// axis in body1 space that should stay within the cone
	float					cosAngle;					// cos( coneAngle / 2 )
	float					sinHalfAngle;				// sin( coneAngle / 4 )
	float					cosHalfAngle;				// cos( coneAngle / 4 )
	float					epsilon;					// lcp epsilon

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// constrains an axis attached to body1 to be inside a pyramid relative to body2
class idAFConstraint_PyramidLimit : public idAFConstraint {

public:
							idAFConstraint_PyramidLimit( void );
	void					Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor,
									const idVec3 &pyramidAxis, const idVec3 &baseAxis,
									const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis );
	void					SetAnchor( const idVec3 &pyramidAxis );
	void					SetBody1Axis( const idVec3 &body1Axis );
	void					SetEpsilon( const float e ) { epsilon = e; }
	bool					Add( idPhysics_AF *phys, float invTimeStep );
	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );
	virtual void			Save( idSaveGame *saveFile ) const;
	virtual void			Restore( idRestoreGame *saveFile );

protected:
	idVec3					pyramidAnchor;				// top of the pyramid in body2 space
	idMat3					pyramidBasis;				// pyramid basis in body2 space with base[2] being the pyramid axis
	idVec3					body1Axis;					// axis in body1 space that should stay within the cone
	float					cosAngle[2];				// cos( pyramidAngle / 2 )
	float					sinHalfAngle[2];			// sin( pyramidAngle / 4 )
	float					cosHalfAngle[2];			// cos( pyramidAngle / 4 )
	float					epsilon;					// lcp epsilon

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};

// vehicle suspension
class idAFConstraint_Suspension : public idAFConstraint {

public:
							idAFConstraint_Suspension( void );

	void					Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel );
	void					SetSuspension( const float up, const float down, const float k, const float d, const float f );

	void					SetSteerAngle( const float degrees ) { steerAngle = degrees; }
	void					EnableMotor( const bool enable ) { motorEnabled = enable; }
	void					SetMotorForce( const float force ) { motorForce = force; }
	void					SetMotorVelocity( const float vel ) { motorVelocity = vel; }
	void					SetEpsilon( const float e ) { epsilon = e; }
	const idVec3			GetWheelOrigin( void ) const;

	virtual void			DebugDraw( void );
	virtual void			Translate( const idVec3 &translation );
	virtual void			Rotate( const idRotation &rotation );

protected:
	idVec3					localOrigin;				// position of suspension relative to body1
	idMat3					localAxis;					// orientation of suspension relative to body1
	float					suspensionUp;				// suspension up movement
	float					suspensionDown;				// suspension down movement
	float					suspensionKCompress;		// spring compress constant
	float					suspensionDamping;			// spring damping
	float					steerAngle;					// desired steer angle in degrees
	float					friction;					// friction
	bool					motorEnabled;				// whether the motor is enabled or not
	float					motorForce;					// motor force
	float					motorVelocity;				// desired velocity
	idClipModel *			wheelModel;					// wheel model
	idVec3					wheelOffset;				// wheel position relative to body1
	trace_t					trace;						// contact point with the ground
	float					epsilon;					// lcp epsilon

protected:
	virtual void			Evaluate( float invTimeStep );
	virtual void			ApplyFriction( float invTimeStep );
};


//===============================================================
//
//	idAFBody
//
//===============================================================

typedef struct AFBodyPState_s {
	idVec3					worldOrigin;				// position in world space
	idMat3					worldAxis;					// axis at worldOrigin
	idVec6					spatialVelocity;			// linear and rotational velocity of body
	idVec6					externalForce;				// external force and torque applied to body
} AFBodyPState_t;


class idAFBody {

	friend class idPhysics_AF;
	friend class idAFTree;

public:
							idAFBody( void );
							idAFBody( const idStr &name, idClipModel *clipModel, float density );
							~idAFBody( void );

	void					Init( void );
	const idStr &			GetName( void ) const { return name; }
	const idVec3 &			GetWorldOrigin( void ) const { return current->worldOrigin; }
	const idMat3 &			GetWorldAxis( void ) const { return current->worldAxis; }
	const idVec3 &			GetLinearVelocity( void ) const { return current->spatialVelocity.SubVec3(0); }
	const idVec3 &			GetAngularVelocity( void ) const { return current->spatialVelocity.SubVec3(1); }
	idVec3					GetPointVelocity( const idVec3 &point ) const;
	const idVec3 &			GetCenterOfMass( void ) const { return centerOfMass; }
	void					SetClipModel( idClipModel *clipModel );
	idClipModel *			GetClipModel( void ) const { return clipModel; }
	void					SetClipMask( const int mask ) { clipMask = mask; fl.clipMaskSet = true; }
	int						GetClipMask( void ) const { return clipMask; }
	void					SetSelfCollision( const bool enable ) { fl.selfCollision = enable; }
	void					SetWorldOrigin( const idVec3 &origin ) { current->worldOrigin = origin; }
	void					SetWorldAxis( const idMat3 &axis ) { current->worldAxis = axis; }
	void					SetLinearVelocity( const idVec3 &linear ) const { current->spatialVelocity.SubVec3(0) = linear; }
	void					SetAngularVelocity( const idVec3 &angular ) const { current->spatialVelocity.SubVec3(1) = angular; }
	void					SetFriction( float linear, float angular, float contact );
	float					GetContactFriction( void ) const { return contactFriction; }
	void					SetBouncyness( float bounce );
	float					GetBouncyness( void ) const { return bouncyness; }
	void					SetDensity( float density, const idMat3 &inertiaScale = mat3_identity );
	float					GetInverseMass( void ) const { return invMass; }
	idMat3					GetInverseWorldInertia( void ) const { return current->worldAxis.Transpose() * inverseInertiaTensor * current->worldAxis; }

	void					SetFrictionDirection( const idVec3 &dir );
	bool					GetFrictionDirection( idVec3 &dir ) const;

	void					SetContactMotorDirection( const idVec3 &dir );
	bool					GetContactMotorDirection( idVec3 &dir ) const;
	void					SetContactMotorVelocity( float vel ) { contactMotorVelocity = vel; }
	float					GetContactMotorVelocity( void ) const { return contactMotorVelocity; }
	void					SetContactMotorForce( float force ) { contactMotorForce = force; }
	float					GetContactMotorForce( void ) const { return contactMotorForce; }

	void					AddForce( const idVec3 &point, const idVec3 &force );
	void					InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const;
	idVec6 &				GetResponseForce( int index ) { return reinterpret_cast<idVec6 &>(response[ index * 8 ]); }

	void					Save( idSaveGame *saveFile );
	void					Restore( idRestoreGame *saveFile );

private:
							// properties
	idStr					name;						// name of body
	idAFBody *				parent;						// parent of this body
	idList<idAFBody *>		children;					// children of this body
	idClipModel *			clipModel;					// model used for collision detection
	idAFConstraint *		primaryConstraint;			// primary constraint (this->constraint->body1 = this)
	idList<idAFConstraint *>constraints;				// all constraints attached to this body
	idAFTree *				tree;						// tree structure this body is part of
	float					linearFriction;				// translational friction
	float					angularFriction;			// rotational friction
	float					contactFriction;			// friction with contact surfaces
	float					bouncyness;					// bounce
	int						clipMask;					// contents this body collides with
	idVec3					frictionDir;				// specifies a single direction of friction in body space
	idVec3					contactMotorDir;			// contact motor direction
	float					contactMotorVelocity;		// contact motor velocity
	float					contactMotorForce;			// maximum force applied to reach the motor velocity

							// derived properties
	float					mass;						// mass of body
	float					invMass;					// inverse mass
	idVec3					centerOfMass;				// center of mass of body
	idMat3					inertiaTensor;				// inertia tensor
	idMat3					inverseInertiaTensor;		// inverse inertia tensor

							// physics state
	AFBodyPState_t			state[2];
	AFBodyPState_t *		current;					// current physics state
	AFBodyPState_t *		next;						// next physics state
	AFBodyPState_t			saved;						// saved physics state
	idVec3					atRestOrigin;				// origin at rest
	idMat3					atRestAxis;					// axis at rest

							// simulation variables used during calculations
	idMatX					inverseWorldSpatialInertia;	// inverse spatial inertia in world space
	idMatX					I, invI;					// transformed inertia
	idMatX					J;							// transformed constraint matrix
	idVecX					s;							// temp solution
	idVecX					totalForce;					// total force acting on body
	idVecX					auxForce;					// force from auxiliary constraints
	idVecX					acceleration;				// acceleration
	float *					response;					// forces on body in response to auxiliary constraint forces
	int *					responseIndex;				// index to response forces
	int						numResponses;				// number of response forces
	int						maxAuxiliaryIndex;			// largest index of an auxiliary constraint constraining this body
	int						maxSubTreeAuxiliaryIndex;	// largest index of an auxiliary constraint constraining this body or one of it's children

	struct bodyFlags_s {
		bool				clipMaskSet			: 1;	// true if this body has a clip mask set
		bool				selfCollision		: 1;	// true if this body can collide with other bodies of this AF
		bool				spatialInertiaSparse: 1;	// true if the spatial inertia matrix is sparse
		bool				useFrictionDir		: 1;	// true if a single friction direction should be used
		bool				useContactMotorDir	: 1;	// true if a contact motor should be used
		bool				isZero				: 1;	// true if 's' is zero during calculations
	} fl;
};


//===============================================================
//
//	idAFTree
//
//===============================================================

class idAFTree {
	friend class idPhysics_AF;

public:
	void					Factor( void ) const;
	void					Solve( int auxiliaryIndex = 0 ) const;
	void					Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const;
	void					CalculateForces( float timeStep ) const;
	void					SetMaxSubTreeAuxiliaryIndex( void );
	void					SortBodies( void );
	void					SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body );
	void					DebugDraw( const idVec4 &color ) const;

private:
	idList<idAFBody *>		sortedBodies;
};


//===============================================================
//
//	idPhysics_AF
//
//===============================================================

typedef struct AFPState_s {
	int						atRest;						// >= 0 if articulated figure is at rest
	float					noMoveTime;					// time the articulated figure is hardly moving
	float					activateTime;				// time since last activation
	float					lastTimeStep;				// last time step
	idVec6					pushVelocity;				// velocity with which the af is pushed
} AFPState_t;

typedef struct AFCollision_s {
	trace_t					trace;
	idAFBody *				body;
} AFCollision_t;


class idPhysics_AF : public idPhysics_Base {

public:
	CLASS_PROTOTYPE( idPhysics_AF );

							idPhysics_AF( void );
							~idPhysics_AF( void );

	void					Save( idSaveGame *savefile ) const;
	void					Restore( idRestoreGame *savefile );

							// initialisation
	int						AddBody( idAFBody *body );	// returns body id
	void					AddConstraint( idAFConstraint *constraint );
	void					AddFrameConstraint( idAFConstraint *constraint );
							// force a body to have a certain id
	void					ForceBodyId( idAFBody *body, int newId );
							// get body or constraint id
	int						GetBodyId( idAFBody *body ) const;
	int						GetBodyId( const char *bodyName ) const;
	int						GetConstraintId( idAFConstraint *constraint ) const;
	int						GetConstraintId( const char *constraintName ) const;
							// number of bodies and constraints
	int						GetNumBodies( void ) const;
	int						GetNumConstraints( void ) const;
							// retrieve body or constraint
	idAFBody *				GetBody( const char *bodyName ) const;
	idAFBody *				GetBody( const int id ) const;
	idAFBody *				GetMasterBody( void ) const { return masterBody; }
	idAFConstraint *		GetConstraint( const char *constraintName ) const;
	idAFConstraint *		GetConstraint( const int id ) const;
							// delete body or constraint
	void					DeleteBody( const char *bodyName );
	void					DeleteBody( const int id );
	void					DeleteConstraint( const char *constraintName );
	void					DeleteConstraint( const int id );
							// get all the contact constraints acting on the body
	int						GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const;
							// set the default friction for bodies
	void					SetDefaultFriction( float linear, float angular, float contact );
							// suspend settings
	void					SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration );
							// set the time and tolerances used to determine if the simulation can be suspended when the figure hardly moves for a while
	void					SetSuspendTolerance( const float noMoveTime, const float translationTolerance, const float rotationTolerance );
							// set minimum and maximum simulation time in seconds
	void					SetSuspendTime( const float minTime, const float maxTime );
							// set the time scale value
	void					SetTimeScale( const float ts ) { timeScale = ts; }
							// set time scale ramp
	void					SetTimeScaleRamp( const float start, const float end );
							// set the joint friction scale
	void					SetJointFrictionScale( const float scale ) { jointFrictionScale = scale; }
							// set joint friction dent
	void					SetJointFrictionDent( const float dent, const float start, const float end );
							// get the current joint friction scale
	float					GetJointFrictionScale( void ) const;
							// set the contact friction scale
	void					SetContactFrictionScale( const float scale ) { contactFrictionScale = scale; }
							// set contact friction dent
	void					SetContactFrictionDent( const float dent, const float start, const float end );
							// get the current contact friction scale
	float					GetContactFrictionScale( void ) const;
							// enable or disable collision detection
	void					SetCollision( const bool enable ) { enableCollision = enable; }
							// enable or disable self collision
	void					SetSelfCollision( const bool enable ) { selfCollision = enable; }
							// enable or disable coming to a dead stop
	void					SetComeToRest( bool enable ) { comeToRest = enable; }
							// call when structure of articulated figure changes
	void					SetChanged( void ) { changedAF = true; }
							// enable/disable activation by impact
	void					EnableImpact( void );
	void					DisableImpact( void );
							// lock of unlock the world constraints
	void					LockWorldConstraints( const bool lock ) { worldConstraintsLocked = lock; }
							// set force pushable
	void					SetForcePushable( const bool enable ) { forcePushable = enable; }
							// update the clip model positions
	void					UpdateClipModels( void );

public:	// common physics interface
	void					SetClipModel( idClipModel *model, float density, int id = 0, bool freeOld = true );
	idClipModel *			GetClipModel( int id = 0 ) const;
	int						GetNumClipModels( void ) const;

	void					SetMass( float mass, int id = -1 );
	float					GetMass( int id = -1 ) const;

	void					SetContents( int contents, int id = -1 );
	int						GetContents( int id = -1 ) const;

	const idBounds &		GetBounds( int id = -1 ) const;
	const idBounds &		GetAbsBounds( int id = -1 ) const;

	bool					Evaluate( int timeStepMSec, int endTimeMSec );
	void					UpdateTime( int endTimeMSec );
	int						GetTime( void ) const;

	void					GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const;
	void					ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse );
	void					AddForce( const int id, const idVec3 &point, const idVec3 &force );
	bool					IsAtRest( void ) const;
	int						GetRestStartTime( void ) const;
	void					Activate( void );
	void					PutToRest( void );
	bool					IsPushable( void ) const;

	void					SaveState( void );
	void					RestoreState( void );

	void					SetOrigin( const idVec3 &newOrigin, int id = -1 );
	void					SetAxis( const idMat3 &newAxis, int id = -1 );

	void					Translate( const idVec3 &translation, int id = -1 );
	void					Rotate( const idRotation &rotation, int id = -1 );

	const idVec3 &			GetOrigin( int id = 0 ) const;
	const idMat3 &			GetAxis( int id = 0 ) const;

	void					SetLinearVelocity( const idVec3 &newLinearVelocity, int id = 0 );
	void					SetAngularVelocity( const idVec3 &newAngularVelocity, int id = 0 );

	const idVec3 &			GetLinearVelocity( int id = 0 ) const;
	const idVec3 &			GetAngularVelocity( int id = 0 ) const;

	void					ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const;
	void					ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const;
	int						ClipContents( const idClipModel *model ) const;

	void					DisableClip( void );
	void					EnableClip( void );

	void					UnlinkClip( void );
	void					LinkClip( void );

	bool					EvaluateContacts( void );

	void					SetPushed( int deltaTime );
	const idVec3 &			GetPushedLinearVelocity( const int id = 0 ) const;
	const idVec3 &			GetPushedAngularVelocity( const int id = 0 ) const;

	void					SetMaster( idEntity *master, const bool orientated = true );

	void					WriteToSnapshot( idBitMsgDelta &msg ) const;
	void					ReadFromSnapshot( const idBitMsgDelta &msg );

private:
							// articulated figure
	idList<idAFTree *>		trees;							// tree structures
	idList<idAFBody *>		bodies;							// all bodies
	idList<idAFConstraint *>constraints;					// all frame independent constraints
	idList<idAFConstraint *>primaryConstraints;				// list with primary constraints
	idList<idAFConstraint *>auxiliaryConstraints;			// list with auxiliary constraints
	idList<idAFConstraint *>frameConstraints;				// constraints that only live one frame
	idList<idAFConstraint_Contact *>contactConstraints;		// contact constraints
	idList<int>				contactBodies;					// body id for each contact
	idList<AFCollision_t>	collisions;						// collisions
	bool					changedAF;						// true when the articulated figure just changed

							// properties
	float					linearFriction;					// default translational friction
	float					angularFriction;				// default rotational friction
	float					contactFriction;				// default friction with contact surfaces
	float					bouncyness;						// default bouncyness
	float					totalMass;						// total mass of articulated figure
	float					forceTotalMass;					// force this total mass

	idVec2					suspendVelocity;				// simulation may not be suspended if a body has more velocity
	idVec2					suspendAcceleration;			// simulation may not be suspended if a body has more acceleration
	float					noMoveTime;						// suspend simulation if hardly any movement for this many seconds
	float					noMoveTranslation;				// maximum translation considered no movement
	float					noMoveRotation;					// maximum rotation considered no movement
	float					minMoveTime;					// if > 0 the simulation is never suspended before running this many seconds
	float					maxMoveTime;					// if > 0 the simulation is always suspeded after running this many seconds
	float					impulseThreshold;				// threshold below which impulses are ignored to avoid continuous activation

	float					timeScale;						// the time is scaled with this value for slow motion effects
	float					timeScaleRampStart;				// start of time scale change
	float					timeScaleRampEnd;				// end of time scale change

	float					jointFrictionScale;				// joint friction scale
	float					jointFrictionDent;				// joint friction dives from 1 to this value and goes up again
	float					jointFrictionDentStart;			// start time of joint friction dent
	float					jointFrictionDentEnd;			// end time of joint friction dent
	float					jointFrictionDentScale;			// dent scale

	float					contactFrictionScale;			// contact friction scale
	float					contactFrictionDent;			// contact friction dives from 1 to this value and goes up again
	float					contactFrictionDentStart;		// start time of contact friction dent
	float					contactFrictionDentEnd;			// end time of contact friction dent
	float					contactFrictionDentScale;		// dent scale

	bool					enableCollision;				// if true collision detection is enabled
	bool					selfCollision;					// if true the self collision is allowed
	bool					comeToRest;						// if true the figure can come to rest
	bool					linearTime;						// if true use the linear time algorithm
	bool					noImpact;						// if true do not activate when another object collides
	bool					worldConstraintsLocked;			// if true world constraints cannot be moved
	bool					forcePushable;					// if true can be pushed even when bound to a master

							// physics state
	AFPState_t				current;
	AFPState_t				saved;

	idAFBody *				masterBody;						// master body
	idLCP *					lcp;							// linear complementarity problem solver

private:
	void					BuildTrees( void );
	bool					IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const;
	void					PrimaryFactor( void );
	void					EvaluateBodies( float timeStep );
	void					EvaluateConstraints( float timeStep );
	void					AddFrameConstraints( void );
	void					RemoveFrameConstraints( void );
	void					ApplyFriction( float timeStep, float endTimeMSec );
	void					PrimaryForces( float timeStep  );
	void					AuxiliaryForces( float timeStep );
	void					VerifyContactConstraints( void );
	void					SetupContactConstraints( void );
	void					ApplyContactForces( void );
	void					Evolve( float timeStep );
	idEntity *				SetupCollisionForBody( idAFBody *body ) const;
	bool					CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision );
	bool					ApplyCollisions( float timeStep );
	void					CheckForCollisions( float timeStep );
	void					ClearExternalForce( void );
	void					AddGravity( void );
	void					SwapStates( void );
	bool					TestIfAtRest( float timeStep );
	void					Rest( void );
	void					AddPushVelocity( const idVec6 &pushVelocity );
	void					DebugDraw( void );
};

#endif /* !__PHYSICS_AF_H__ */