/*****************************************************************************
The Dark Mod GPL Source Code

This file is part of the The Dark Mod Source Code, originally based
on the Doom 3 GPL Source Code as published in 2011.

The Dark Mod 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. For details, see LICENSE.TXT.

Project: The Dark Mod (http://www.thedarkmod.com/)

******************************************************************************/

#include "precompiled.h"
#pragma hdrstop



#include "../Game_local.h"
#include "../Grabber.h"

CLASS_DECLARATION( idPhysics_Base, idPhysics_AF )
END_CLASS

const float ERROR_REDUCTION					= 0.5f;
const float ERROR_REDUCTION_MAX				= 256.0f;
const float LIMIT_ERROR_REDUCTION			= 0.3f;
const float LCP_EPSILON						= 1e-7f;
const float LIMIT_LCP_EPSILON				= 1e-4f;
const float CONTACT_LCP_EPSILON				= 1e-6f;
const float CENTER_OF_MASS_EPSILON			= 1e-2f;
#ifdef MOD_WATERPHYSICS
const float NO_MOVE_TIME					= 2.0f;
// ishtvan test: move impulse threshold back to D3 default or below
// const float IMPULSE_THRESHOLD				= 1500.0f;
const float IMPULSE_THRESHOLD				= 250.0f;
const float WATER_FRICTION                  = 0.0f;     // we need AF friction to be a little bigger than RB water friction, we add this value
const float DEFAULT_LIQUID_SCALAR           = -0.28f;
const float DEFAULT_LIQUID_DENSITY          = 0.005f;
const float LIQUID_MASS_MUL                 = 3.0f;     // I'm not sure how to explain this, without it body bob way too quickly
#else
const float NO_MOVE_TIME					= 1.0f;
const float IMPULSE_THRESHOLD				= 500.0f;
#endif
const float NO_MOVE_TRANSLATION_TOLERANCE	= 10.0f;
const float NO_MOVE_ROTATION_TOLERANCE		= 10.0f;
const float MIN_MOVE_TIME					= -1.0f;
const float MAX_MOVE_TIME					= -1.0f;
const float SUSPEND_LINEAR_VELOCITY			= 10.0f;
const float SUSPEND_ANGULAR_VELOCITY		= 15.0f;
const float SUSPEND_LINEAR_ACCELERATION		= 20.0f;
const float SUSPEND_ANGULAR_ACCELERATION	= 30.0f;
const idVec6 vec6_lcp_epsilon				= idVec6( LCP_EPSILON, LCP_EPSILON, LCP_EPSILON,
													 LCP_EPSILON, LCP_EPSILON, LCP_EPSILON );

static const float MAX_GRABBER_EXT_VELOCITY		= 120.0f;
static const float MAX_GRABBER_EXT_ANGVEL		= 5.0f;

#define AF_TIMINGS

#ifdef AF_TIMINGS
static int lastTimerReset = 0;
static int numArticulatedFigures = 0;
static idTimer timer_total, timer_pc, timer_ac, timer_collision, timer_lcp;
#endif



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

/*
================
idAFConstraint::idAFConstraint
================
*/
idAFConstraint::idAFConstraint( void ) {
	type				= CONSTRAINT_INVALID;
	name				= "noname";
	body1				= NULL;
	body2				= NULL;
	physics				= NULL;

	lo.Zero( 6 );
	lo.SubVec6(0)		= -vec6_infinity;
	hi.Zero( 6 );
	hi.SubVec6(0)		= vec6_infinity;
	e.SetSize( 6 );
	e.SubVec6(0)		= vec6_lcp_epsilon;

	boxConstraint		= NULL;
	boxIndex[0]			= -1;
	boxIndex[1]			= -1;
	boxIndex[2]			= -1;
	boxIndex[3]			= -1;
	boxIndex[4]			= -1;
	boxIndex[5]			= -1;

	firstIndex			= 0;

	memset( &fl, 0, sizeof( fl ) );
}

/*
================
idAFConstraint::~idAFConstraint
================
*/
idAFConstraint::~idAFConstraint( void ) {
}

/*
================
idAFConstraint::SetBody1
================
*/
void idAFConstraint::SetBody1( idAFBody *body ) {
	if ( body1 != body) {
		body1 = body;
		if ( physics ) {
			physics->SetChanged();
		}
	}
}

/*
================
idAFConstraint::SetBody2
================
*/
void idAFConstraint::SetBody2( idAFBody *body ) {
	if ( body2 != body ) {
		body2 = body;
		if ( physics ) {
			physics->SetChanged();
		}
	}
}

/*
================
idAFConstraint::GetMultiplier
================
*/
const idVecX &idAFConstraint::GetMultiplier( void ) {
	return lm;
}

/*
================
idAFConstraint::Evaluate
================
*/
void idAFConstraint::Evaluate( float invTimeStep ) {
	assert( 0 );
}

/*
================
idAFConstraint::ApplyFriction
================
*/
void idAFConstraint::ApplyFriction( float invTimeStep ) {
}

/*
================
idAFConstraint::GetForce
================
*/
void idAFConstraint::GetForce( idAFBody *body, idVec6 &force ) {
	idVecX v;

	v.SetData( 6, VECX_ALLOCA( 6 ) );
	if ( body == body1 ) {
		J1.TransposeMultiply( v, lm );
	}
	else if ( body == body2 ) {
		J2.TransposeMultiply( v, lm );
	}
	else {
		v.Zero();
	}
	force[0] = v[0]; force[1] = v[1]; force[2] = v[2]; force[3] = v[3]; force[4] = v[4]; force[5] = v[5];
}

/*
================
idAFConstraint::Translate
================
*/
void idAFConstraint::Translate( const idVec3 &translation ) {
	assert( 0 );
}

/*
================
idAFConstraint::Rotate
================
*/
void idAFConstraint::Rotate( const idRotation &rotation ) {
	assert( 0 );
}

/*
================
idAFConstraint::GetCenter
================
*/
void idAFConstraint::GetCenter( idVec3 &center ) {
	center.Zero();
}

/*
================
idAFConstraint::DebugDraw
================
*/
void idAFConstraint::DebugDraw( void ) {
}

/*
================
idAFConstraint::InitSize
================
*/
void idAFConstraint::InitSize( int size ) {
	J1.Zero( size, 6 );
	J2.Zero( size, 6 );
	c1.Zero( size );
	c2.Zero( size );
	s.Zero( size );
	lm.Zero( size );
}

/*
================
idAFConstraint::Save
================
*/
void idAFConstraint::Save( idSaveGame *saveFile ) const {
	saveFile->WriteInt( type );
}

/*
================
idAFConstraint::Restore
================
*/
void idAFConstraint::Restore( idRestoreGame *saveFile ) {
	constraintType_t t;
	saveFile->ReadInt( (int &)t );
	assert( t == type );
}


//===============================================================
//
//	idAFConstraint_Fixed
//
//===============================================================

/*
================
idAFConstraint_Fixed::idAFConstraint_Fixed
================
*/
idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_FIXED;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 6 );
	fl.allowPrimary = true;
	fl.noCollision = true;

	InitOffset();
}

/*
================
idAFConstraint_Fixed::InitOffset
================
*/
void idAFConstraint_Fixed::InitOffset( void ) {
	if ( body2 ) {
		offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
		relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
	}
	else {
		offset = body1->GetWorldOrigin();
		relAxis = body1->GetWorldAxis();
	}
}

/*
================
idAFConstraint_Fixed::SetBody1
================
*/
void idAFConstraint_Fixed::SetBody1( idAFBody *body ) {
	if ( body1 != body) {
		body1 = body;
		InitOffset();
		if ( physics ) {
			physics->SetChanged();
		}
	}
}

/*
================
idAFConstraint_Fixed::SetBody2
================
*/
void idAFConstraint_Fixed::SetBody2( idAFBody *body ) {
	if ( body2 != body ) {
		body2 = body;
		InitOffset();
		if ( physics ) {
			physics->SetChanged();
		}
	}
}

/*
================
idAFConstraint_Fixed::Evaluate
================
*/
void idAFConstraint_Fixed::Evaluate( float invTimeStep ) {
	idVec3 ofs, a2;
	idMat3 ax;
	idRotation r;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		a2 = offset * master->GetWorldAxis();
		ofs = a2 + master->GetWorldOrigin();
		ax = relAxis * master->GetWorldAxis();
	}
	else {
		a2.Zero();
		ofs = offset;
		ax = relAxis;
	}

	J1.Set(	mat3_identity, mat3_zero,
				mat3_zero, mat3_identity );

	if ( body2 ) {
		J2.Set(	-mat3_identity, SkewSymmetric( a2 ),
					mat3_zero, -mat3_identity );
	}
	else {
		J2.Zero( 6, 6 );
	}

	c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( ofs - body1->GetWorldOrigin() );
	r = ( body1->GetWorldAxis().Transpose() * ax ).ToRotation();
	c1.SubVec3(1) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * -(float) DEG2RAD( r.GetAngle() ) );

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
}

/*
================
idAFConstraint_Fixed::ApplyFriction
================
*/
void idAFConstraint_Fixed::ApplyFriction( float invTimeStep ) {
	// no friction
}

/*
================
idAFConstraint_Fixed::Translate
================
*/
void idAFConstraint_Fixed::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		offset += translation;
	}
}

/*
================
idAFConstraint_Fixed::Rotate
================
*/
void idAFConstraint_Fixed::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		offset *= rotation;
		relAxis *= rotation.ToMat3();
	}
}

/*
================
idAFConstraint_Fixed::GetCenter
================
*/
void idAFConstraint_Fixed::GetCenter( idVec3 &center ) {
	center = body1->GetWorldOrigin();
}

/*
================
idAFConstraint_Fixed::DebugDraw
================
*/
void idAFConstraint_Fixed::DebugDraw( void ) {
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();
	if ( master ) {
		gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin() );
	}
	else {
		gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), vec3_origin );
	}
}

/*
================
idAFConstraint_Fixed::Save
================
*/
void idAFConstraint_Fixed::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( offset );
	saveFile->WriteMat3( relAxis );
}

/*
================
idAFConstraint_Fixed::Restore
================
*/
void idAFConstraint_Fixed::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( offset );
	saveFile->ReadMat3( relAxis );
}


//===============================================================
//
//	idAFConstraint_BallAndSocketJoint
//
//===============================================================

/*
================
idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint
================
*/
idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_BALLANDSOCKETJOINT;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 3 );
	coneLimit = NULL;
	pyramidLimit = NULL;
	friction = 0.0f;
	fc = NULL;
	fl.allowPrimary = true;
	fl.noCollision = true;
}

/*
================
idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint
================
*/
idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint( void ) {
	if ( coneLimit ) {
		delete coneLimit;
	}
	if ( pyramidLimit ) {
		delete pyramidLimit;
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::SetAnchor
================
*/
void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3 &worldPosition ) {

	// get anchor relative to center of mass of body1
	anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get anchor relative to center of mass of body2
		anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
	}
	else {
		anchor2 = worldPosition;
	}

	if ( coneLimit ) {
		coneLimit->SetAnchor( anchor2 );
	}
	if ( pyramidLimit ) {
		pyramidLimit->SetAnchor( anchor2 );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::GetAnchor
================
*/
idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor( void ) const {
	if ( body2 ) {
		return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
	}
	return anchor2;
}

/*
================
idAFConstraint_BallAndSocketJoint::SetNoLimit
================
*/
void idAFConstraint_BallAndSocketJoint::SetNoLimit( void ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
	if ( pyramidLimit ) {
		delete pyramidLimit;
		pyramidLimit = NULL;
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::SetConeLimit
================
*/
void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) {
	if ( pyramidLimit ) {
		delete pyramidLimit;
		pyramidLimit = NULL;
	}
	if ( !coneLimit ) {
		coneLimit = new idAFConstraint_ConeLimit;
		coneLimit->SetPhysics( physics );
	}
	if ( body2 ) {
		coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
	}
	else {
		coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::SetPyramidLimit
================
*/
void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
														const float angle1, const float angle2, const idVec3 &body1Axis ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
	if ( !pyramidLimit ) {
		pyramidLimit = new idAFConstraint_PyramidLimit;
		pyramidLimit->SetPhysics( physics );
	}
	if ( body2 ) {
		pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
									baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2,
											body1Axis * body1->GetWorldAxis().Transpose() );
	}
	else {
		pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2,
											body1Axis * body1->GetWorldAxis().Transpose() );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::SetLimitEpsilon
================
*/
void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e ) {
	if ( coneLimit ) {
		coneLimit->SetEpsilon( e );
	}
	if ( pyramidLimit ) {
		pyramidLimit->SetEpsilon( e );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::GetFriction
================
*/
float idAFConstraint_BallAndSocketJoint::GetFriction( void ) const {
	if ( af_forceFriction.GetFloat() > 0.0f ) {
		return af_forceFriction.GetFloat();
	}
	return friction * physics->GetJointFrictionScale();
}

/*
================
idAFConstraint_BallAndSocketJoint::Evaluate
================
*/
void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep ) {
	idVec3 a1, a2;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = anchor1 * body1->GetWorldAxis();

	if ( master ) {
		a2 = anchor2 * master->GetWorldAxis();
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
	}
	else {
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( anchor2 - ( a1 + body1->GetWorldOrigin() ) );
	}

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );

	J1.Set( mat3_identity, -SkewSymmetric( a1 ) );

	if ( body2 ) {
		J2.Set( -mat3_identity, SkewSymmetric( a2 ) );
	}
	else {
		J2.Zero( 3, 6 );
	}

	if ( coneLimit ) {
		coneLimit->Add( physics, invTimeStep );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::ApplyFriction
================
*/
void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep ) {
	idVec3 angular;
	float invMass, currentFriction;

	currentFriction = GetFriction();

	if ( currentFriction <= 0.0f ) {
		return;
	}

	if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {

		angular = body1->GetAngularVelocity();
		invMass = body1->GetInverseMass();
		if ( body2 ) {
			angular -= body2->GetAngularVelocity();
			invMass += body2->GetInverseMass();
		}

		angular *= currentFriction / invMass;

		body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
		if ( body2 ) {
			body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
		}
	}
	else {
		if ( !fc ) {
			fc = new idAFConstraint_BallAndSocketJointFriction;
			fc->Setup( this );
		}

		fc->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::GetForce
================
*/
void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody *body, idVec6 &force ) {
	idAFConstraint::GetForce( body, force );
	// FIXME: add limit force
}

/*
================
idAFConstraint_BallAndSocketJoint::Translate
================
*/
void idAFConstraint_BallAndSocketJoint::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		anchor2 += translation;
	}
	if ( coneLimit ) {
		coneLimit->Translate( translation );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Translate( translation );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::Rotate
================
*/
void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		anchor2 *= rotation;
	}
	if ( coneLimit ) {
		coneLimit->Rotate( rotation );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Rotate( rotation );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::GetCenter
================
*/
void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3 &center ) {
	center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
}

/*
================
idAFConstraint_BallAndSocketJoint::DebugDraw
================
*/
void idAFConstraint_BallAndSocketJoint::DebugDraw( void ) {
	idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 5, 0, 0 ), a1 + idVec3( 5, 0, 0 ) );
	gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 5, 0 ), a1 + idVec3( 0, 5, 0 ) );
	gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 0, 5 ), a1 + idVec3( 0, 0, 5 ) );

	if ( af_showLimits.GetBool() ) {
		if ( coneLimit ) {
			coneLimit->DebugDraw();
		}
		if ( pyramidLimit ) {
			pyramidLimit->DebugDraw();
		}
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::Save
================
*/
void idAFConstraint_BallAndSocketJoint::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( anchor1 );
	saveFile->WriteVec3( anchor2 );
	saveFile->WriteFloat( friction );
	if ( coneLimit ) {
		coneLimit->Save( saveFile );
	}
	if ( pyramidLimit ) {
		pyramidLimit->Save( saveFile );
	}
}

/*
================
idAFConstraint_BallAndSocketJoint::Restore
================
*/
void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( anchor1 );
	saveFile->ReadVec3( anchor2 );
	saveFile->ReadFloat( friction );
	if ( coneLimit ) {
		coneLimit->Restore( saveFile );
	}
	if ( pyramidLimit ) {
		pyramidLimit->Restore( saveFile );
	}
}


//===============================================================
//
//	idAFConstraint_BallAndSocketJointFriction
//
//===============================================================

/*
================
idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction
================
*/
idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction( void ) {
	type = CONSTRAINT_FRICTION;
	name = "ballAndSocketJointFriction";
	InitSize( 3 );
	joint = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_BallAndSocketJointFriction::Setup
================
*/
void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSocketJoint *bsj ) {
	this->joint = bsj;
	body1 = bsj->GetBody1();
	body2 = bsj->GetBody2();
}

/*
================
idAFConstraint_BallAndSocketJointFriction::Evaluate
================
*/
void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_BallAndSocketJointFriction::ApplyFriction
================
*/
void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_BallAndSocketJointFriction::Add
================
*/
bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
	float f;

	physics = phys;

	f = joint->GetFriction() * joint->GetMultiplier().Length();
	if ( f == 0.0f ) {
		return false;
	}

	lo[0] = lo[1] = lo[2] = -f;
	hi[0] = hi[1] = hi[2] = f;

	J1.Zero( 3, 6 );
	J1[0][3] = J1[1][4] = J1[2][5] = 1.0f;

	if ( body2 ) {

		J2.Zero( 3, 6 );
		J2[0][3] = J2[1][4] = J2[2][5] = 1.0f;
	}

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_BallAndSocketJointFriction::Translate
================
*/
void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_BallAndSocketJointFriction::Rotate
================
*/
void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation &rotation ) {
}


//===============================================================
//
//	idAFConstraint_UniversalJoint
//
//===============================================================

/*
================
idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint
================
*/
idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_UNIVERSALJOINT;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 4 );
	coneLimit = NULL;
	pyramidLimit = NULL;
	friction = 0.0f;
	fc = NULL;
	fl.allowPrimary = true;
	fl.noCollision = true;
}

/*
================
idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint
================
*/
idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint( void ) {
	if ( coneLimit ) {
		delete coneLimit;
	}
	if ( pyramidLimit ) {
		delete pyramidLimit;
	}
	if ( fc ) {
		delete fc;
	}
}

/*
================
idAFConstraint_UniversalJoint::SetAnchor
================
*/
void idAFConstraint_UniversalJoint::SetAnchor( const idVec3 &worldPosition ) {

	// get anchor relative to center of mass of body1
	anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get anchor relative to center of mass of body2
		anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
	}
	else {
		anchor2 = worldPosition;
	}

	if ( coneLimit ) {
		coneLimit->SetAnchor( anchor2 );
	}
	if ( pyramidLimit ) {
		pyramidLimit->SetAnchor( anchor2 );
	}
}

/*
================
idAFConstraint_UniversalJoint::GetAnchor
================
*/
idVec3 idAFConstraint_UniversalJoint::GetAnchor( void ) const {
	if ( body2 ) {
		return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
	}
	return anchor2;
}

/*
================
idAFConstraint_UniversalJoint::SetShafts
================
*/
void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 ) {
	idVec3 cardanAxis;
	float l;

	shaft1 = cardanShaft1;
	l = shaft1.Normalize();
	assert( l != 0.0f );
	shaft2 = cardanShaft2;
	l = shaft2.Normalize();
	assert( l != 0.0f );

	// the cardan axis is a vector orthogonal to both cardan shafts
	cardanAxis = shaft1.Cross( shaft2 );
	if ( cardanAxis.Normalize() == 0.0f ) {
		idVec3 vecY;
		shaft1.OrthogonalBasis( cardanAxis, vecY );
		cardanAxis.Normalize();
	}

	shaft1 *= body1->GetWorldAxis().Transpose();
	axis1 = cardanAxis * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		shaft2 *= body2->GetWorldAxis().Transpose();
		axis2 = cardanAxis * body2->GetWorldAxis().Transpose();
	}
	else {
		axis2 = cardanAxis;
	}

	if ( coneLimit ) {
		coneLimit->SetBody1Axis( shaft1 );
	}
	if ( pyramidLimit ) {
		pyramidLimit->SetBody1Axis( shaft1 );
	}
}

/*
================
idAFConstraint_UniversalJoint::SetNoLimit
================
*/
void idAFConstraint_UniversalJoint::SetNoLimit( void ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
	if ( pyramidLimit ) {
		delete pyramidLimit;
		pyramidLimit = NULL;
	}
}

/*
================
idAFConstraint_UniversalJoint::SetConeLimit
================
*/
void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle ) {
	if ( pyramidLimit ) {
		delete pyramidLimit;
		pyramidLimit = NULL;
	}
	if ( !coneLimit ) {
		coneLimit = new idAFConstraint_ConeLimit;
		coneLimit->SetPhysics( physics );
	}
	if ( body2 ) {
		coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1 );
	}
	else {
		coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, shaft1 );
	}
}

/*
================
idAFConstraint_UniversalJoint::SetPyramidLimit
================
*/
void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
														const float angle1, const float angle2 ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
	if ( !pyramidLimit ) {
		pyramidLimit = new idAFConstraint_PyramidLimit;
		pyramidLimit->SetPhysics( physics );
	}
	if ( body2 ) {
		pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
									baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1 );
	}
	else {
		pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1 );
	}
}

/*
================
idAFConstraint_UniversalJoint::SetLimitEpsilon
================
*/
void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e ) {
	if ( coneLimit ) {
		coneLimit->SetEpsilon( e );
	}
	if ( pyramidLimit ) {
		pyramidLimit->SetEpsilon( e );
	}
}

/*
================
idAFConstraint_UniversalJoint::GetFriction
================
*/
float idAFConstraint_UniversalJoint::GetFriction( void ) const {
	if ( af_forceFriction.GetFloat() > 0.0f ) {
		return af_forceFriction.GetFloat();
	}
	return friction * physics->GetJointFrictionScale();
}

/*
================
idAFConstraint_UniversalJoint::Evaluate

  NOTE: this joint is homokinetic
================
*/
void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep ) {
	idVec3 a1, a2, s1, s2, d1, d2, v;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = anchor1 * body1->GetWorldAxis();
	s1 = shaft1 * body1->GetWorldAxis();
	d1 = s1.Cross( axis1 * body1->GetWorldAxis() );

	if ( master ) {
		a2 = anchor2 * master->GetWorldAxis();
		s2 = shaft2 * master->GetWorldAxis();
		d2 = axis2 * master->GetWorldAxis();
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
	}
	else {
		a2 = anchor2;
		s2 = shaft2;
		d2 = axis2;
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
	}

	J1.Set(	mat3_identity,	-SkewSymmetric( a1 ),
				mat3_zero,		idMat3( s1[0], s1[1], s1[2],
										0.0f, 0.0f, 0.0f,
										0.0f, 0.0f, 0.0f ) );
	J1.SetSize( 4, 6 );

	if ( body2 ) {
		J2.Set(	-mat3_identity,	SkewSymmetric( a2 ),
					mat3_zero,		idMat3( s2[0], s2[1], s2[2],
											0.0f, 0.0f, 0.0f,
											0.0f, 0.0f, 0.0f ) );
		J2.SetSize( 4, 6 );
	}
	else {
		J2.Zero( 4, 6 );
	}

	v = s1.Cross( s2 );
	if ( v.Normalize() != 0.0f ) {
		idMat3 m1, m2;

		m1[0] = s1;
		m1[1] = v;
		m1[2] = v.Cross( m1[0] );

		m2[0] = -s2;
		m2[1] = v;
		m2[2] = v.Cross( m2[0] );

		d2 *= m2.Transpose() * m1;
	}

	c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( d1 * d2 );

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );

	if ( coneLimit ) {
		coneLimit->Add( physics, invTimeStep );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_UniversalJoint::ApplyFriction
================
*/
void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep ) {
	idVec3 angular;
	float invMass, currentFriction;

	currentFriction = GetFriction();

	if ( currentFriction <= 0.0f ) {
		return;
	}

	if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {

		angular = body1->GetAngularVelocity();
		invMass = body1->GetInverseMass();
		if ( body2 ) {
			angular -= body2->GetAngularVelocity();
			invMass += body2->GetInverseMass();
		}

		angular *= currentFriction / invMass;

		body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
		if ( body2 ) {
			body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
		}
	}
	else {
		if ( !fc ) {
			fc = new idAFConstraint_UniversalJointFriction;
			fc->Setup( this );
		}

		fc->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_UniversalJoint::GetForce
================
*/
void idAFConstraint_UniversalJoint::GetForce( idAFBody *body, idVec6 &force ) {
	idAFConstraint::GetForce( body, force );
	// FIXME: add limit force
}

/*
================
idAFConstraint_UniversalJoint::Translate
================
*/
void idAFConstraint_UniversalJoint::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		anchor2 += translation;
	}
	if ( coneLimit ) {
		coneLimit->Translate( translation );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Translate( translation );
	}
}

/*
================
idAFConstraint_UniversalJoint::Rotate
================
*/
void idAFConstraint_UniversalJoint::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		anchor2 *= rotation;
		shaft2 *= rotation.ToMat3();
		axis2 *= rotation.ToMat3();
	}
	if ( coneLimit ) {
		coneLimit->Rotate( rotation );
	}
	else if ( pyramidLimit ) {
		pyramidLimit->Rotate( rotation );
	}
}

/*
================
idAFConstraint_UniversalJoint::GetCenter
================
*/
void idAFConstraint_UniversalJoint::GetCenter( idVec3 &center ) {
	center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
}

/*
================
idAFConstraint_UniversalJoint::DebugDraw
================
*/
void idAFConstraint_UniversalJoint::DebugDraw( void ) {
	idVec3 a1, a2, s1, s2, d1, d2, v;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	s1 = shaft1 * body1->GetWorldAxis();
	d1 = axis1 * body1->GetWorldAxis();

	if ( master ) {
        a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
		s2 = shaft2 * master->GetWorldAxis();
		d2 = axis2 * master->GetWorldAxis();
	}
	else {
        a2 = anchor2;
		s2 = shaft2;
		d2 = axis2;
	}

	v = s1.Cross( s2 );
	if ( v.Normalize() != 0.0f ) {
		idMat3 m1, m2;

		m1[0] = s1;
		m1[1] = v;
		m1[2] = v.Cross( m1[0] );

		m2[0] = -s2;
		m2[1] = v;
		m2[2] = v.Cross( m2[0] );

		d2 *= m2.Transpose() * m1;
	}

	gameRenderWorld->DebugArrow( colorCyan, a1, a1 + s1 * 5.0f, 1 );
	gameRenderWorld->DebugArrow( colorBlue, a2, a2 + s2 * 5.0f, 1 );
	gameRenderWorld->DebugLine( colorGreen, a1, a1 + d1 * 5.0f );
	gameRenderWorld->DebugLine( colorGreen, a2, a2 + d2 * 5.0f );

	if ( af_showLimits.GetBool() ) {
		if ( coneLimit ) {
			coneLimit->DebugDraw();
		}
		if ( pyramidLimit ) {
			pyramidLimit->DebugDraw();
		}
	}
}

/*
================
idAFConstraint_UniversalJoint::Save
================
*/
void idAFConstraint_UniversalJoint::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( anchor1 );
	saveFile->WriteVec3( anchor2 );
	saveFile->WriteVec3( shaft1 );
	saveFile->WriteVec3( shaft2 );
	saveFile->WriteVec3( axis1 );
	saveFile->WriteVec3( axis2 );
	saveFile->WriteFloat( friction );
	if ( coneLimit ) {
		coneLimit->Save( saveFile );
	}
	if ( pyramidLimit ) {
		pyramidLimit->Save( saveFile );
	}
}

/*
================
idAFConstraint_UniversalJoint::Restore
================
*/
void idAFConstraint_UniversalJoint::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( anchor1 );
	saveFile->ReadVec3( anchor2 );
	saveFile->ReadVec3( shaft1 );
	saveFile->ReadVec3( shaft2 );
	saveFile->ReadVec3( axis1 );
	saveFile->ReadVec3( axis2 );
	saveFile->ReadFloat( friction );
	if ( coneLimit ) {
		coneLimit->Restore( saveFile );
	}
	if ( pyramidLimit ) {
		pyramidLimit->Restore( saveFile );
	}
}


//===============================================================
//
//	idAFConstraint_UniversalJointFriction
//
//===============================================================

/*
================
idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction
================
*/
idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction( void ) {
	type = CONSTRAINT_FRICTION;
	name = "universalJointFriction";
	InitSize( 2 );
	joint = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_UniversalJointFriction::Setup
================
*/
void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint *uj ) {
	this->joint = uj;
	body1 = uj->GetBody1();
	body2 = uj->GetBody2();
}

/*
================
idAFConstraint_UniversalJointFriction::Evaluate
================
*/
void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_UniversalJointFriction::ApplyFriction
================
*/
void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_UniversalJointFriction::Add
================
*/
bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
	idVec3 s1, s2, dir1, dir2;
	float f;

	physics = phys;

	f = joint->GetFriction() * joint->GetMultiplier().Length();
	if ( f == 0.0f ) {
		return false;
	}

	lo[0] = lo[1] = -f;
	hi[0] = hi[1] = f;

	joint->GetShafts( s1, s2 );

	s1 *= body1->GetWorldAxis();
	s1.NormalVectors( dir1, dir2 );

	J1.SetSize( 2, 6 );
	J1.SubVec6(0).SubVec3(0).Zero();
	J1.SubVec6(0).SubVec3(1) = dir1;
	J1.SubVec6(1).SubVec3(0).Zero();
	J1.SubVec6(1).SubVec3(1) = dir2;

	if ( body2 ) {

		J2.SetSize( 2, 6 );
		J2.SubVec6(0).SubVec3(0).Zero();
		J2.SubVec6(0).SubVec3(1) = -dir1;
		J2.SubVec6(1).SubVec3(0).Zero();
		J2.SubVec6(1).SubVec3(1) = -dir2;
	}

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_UniversalJointFriction::Translate
================
*/
void idAFConstraint_UniversalJointFriction::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_UniversalJointFriction::Rotate
================
*/
void idAFConstraint_UniversalJointFriction::Rotate( const idRotation &rotation ) {
}


//===============================================================
//
//	idAFConstraint_CylindricalJoint
//
//===============================================================

/*
================
idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint
================
*/
idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_CylindricalJoint::Evaluate
================
*/
void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_CylindricalJoint::ApplyFriction
================
*/
void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_CylindricalJoint::Translate
================
*/
void idAFConstraint_CylindricalJoint::Translate( const idVec3 &translation ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_CylindricalJoint::Rotate
================
*/
void idAFConstraint_CylindricalJoint::Rotate( const idRotation &rotation ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_CylindricalJoint::DebugDraw
================
*/
void idAFConstraint_CylindricalJoint::DebugDraw( void ) {
	assert( 0 );	// FIXME: implement
}


//===============================================================
//
//	idAFConstraint_Hinge
//
//===============================================================

/*
================
idAFConstraint_Hinge::idAFConstraint_Hinge
================
*/
idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_HINGE;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 5 );
	coneLimit = NULL;
	steering = NULL;
	friction = 0.0f;
	fc = NULL;
	fl.allowPrimary = true;
	fl.noCollision = true;
	initialAxis = body1->GetWorldAxis();
	if ( body2 ) {
		initialAxis *= body2->GetWorldAxis().Transpose();
	}
}

/*
================
idAFConstraint_Hinge::~idAFConstraint_Hinge
================
*/
idAFConstraint_Hinge::~idAFConstraint_Hinge( void ) {
	if ( coneLimit ) {
		delete coneLimit;
	}
	if ( fc ) {
		delete fc;
	}
	if ( steering ) {
		delete steering;
	}
}

/*
================
idAFConstraint_Hinge::SetAnchor
================
*/
void idAFConstraint_Hinge::SetAnchor( const idVec3 &worldPosition ) {
	// get anchor relative to center of mass of body1
	anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get anchor relative to center of mass of body2
		anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
	}
	else {
		anchor2 = worldPosition;
	}

	if ( coneLimit ) {
		coneLimit->SetAnchor( anchor2 );
	}
}

/*
================
idAFConstraint_Hinge::GetAnchor
================
*/
idVec3 idAFConstraint_Hinge::GetAnchor( void ) const {
	if ( body2 ) {
		return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
	}
	return anchor2;
}

/*
================
idAFConstraint_Hinge::SetAxis
================
*/
void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) {
	idVec3 normAxis;

	normAxis = axis;
	normAxis.Normalize();

	// get axis relative to body1
	axis1 = normAxis * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get axis relative to body2
		axis2 = normAxis * body2->GetWorldAxis().Transpose();
	}
	else {
		axis2 = normAxis;
	}
}

/*
================
idAFConstraint_Hinge::GetAxis
================
*/
idVec3 idAFConstraint_Hinge::GetAxis( void ) const {
	if ( body2 ) {
		return axis2 * body2->GetWorldAxis();
	}
	return axis2;
}

/*
================
idAFConstraint_Hinge::SetNoLimit
================
*/
void idAFConstraint_Hinge::SetNoLimit( void ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
}

/*
================
idAFConstraint_Hinge::SetLimit
================
*/
void idAFConstraint_Hinge::SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis ) {
	if ( !coneLimit ) {
		coneLimit = new idAFConstraint_ConeLimit;
		coneLimit->SetPhysics( physics );
	}
	if ( body2 ) {
		coneLimit->Setup( body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose() );
	}
	else {
		coneLimit->Setup( body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose() );
	}
}

/*
================
idAFConstraint_Hinge::SetLimitEpsilon
================
*/
void idAFConstraint_Hinge::SetLimitEpsilon( const float e ) {
	if ( coneLimit ) {
		coneLimit->SetEpsilon( e );
	}
}

/*
================
idAFConstraint_Hinge::GetFriction
================
*/
float idAFConstraint_Hinge::GetFriction( void ) const {
	if ( af_forceFriction.GetFloat() > 0.0f ) {
		return af_forceFriction.GetFloat();
	}
	return friction * physics->GetJointFrictionScale();
}

/*
================
idAFConstraint_Hinge::GetAngle
================
*/
float idAFConstraint_Hinge::GetAngle( void ) const {
	idMat3 axis;
	idRotation rotation;
	float angle;

	axis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose() * initialAxis.Transpose();
	rotation = axis.ToRotation();
	angle = rotation.GetAngle();
	if ( rotation.GetVec() * axis1 < 0.0f ) {
		return -angle;
	}
	return angle;
}

/*
================
idAFConstraint_Hinge::SetSteerAngle
================
*/
void idAFConstraint_Hinge::SetSteerAngle( const float degrees ) {
	if ( coneLimit ) {
		delete coneLimit;
		coneLimit = NULL;
	}
	if ( !steering ) {
		steering = new idAFConstraint_HingeSteering();
		steering->Setup( this );
	}
	steering->SetSteerAngle( degrees );
}

/*
================
idAFConstraint_Hinge::SetSteerSpeed
================
*/
void idAFConstraint_Hinge::SetSteerSpeed( const float speed ) {
	if ( steering ) {
		steering->SetSteerSpeed( speed );
	}
}

/*
================
idAFConstraint_Hinge::Evaluate
================
*/
void idAFConstraint_Hinge::Evaluate( float invTimeStep ) {
	idVec3 a1, a2;
	idVec3 x1, x2, cross;
	idVec3 vecX, vecY;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	x1 = axis1 * body1->GetWorldAxis();		// axis in body1 space
	x1.OrthogonalBasis( vecX, vecY );				// basis for axis in body1 space

	a1 = anchor1 * body1->GetWorldAxis();	// anchor in body1 space

	if ( master ) {
		a2 = anchor2 * master->GetWorldAxis();	// anchor in master space
		x2 = axis2 * master->GetWorldAxis();
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
	}
	else {
		a2 = anchor2;
		x2 = axis2;
		c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
	}

	J1.Set(	mat3_identity,	-SkewSymmetric( a1 ),
				mat3_zero,		idMat3(	vecX[0], vecX[1], vecX[2],
										vecY[0], vecY[1], vecY[2],
										0.0f, 0.0f, 0.0f ) );
	J1.SetSize( 5, 6 );

	if ( body2 ) {
		J2.Set(	-mat3_identity,	SkewSymmetric( a2 ),
					mat3_zero,		idMat3(	-vecX[0], -vecX[1], -vecX[2],
											-vecY[0], -vecY[1], -vecY[2],
											0.0f, 0.0f, 0.0f ) );
		J2.SetSize( 5, 6 );
	}
	else {
		J2.Zero( 5, 6 );
	}

	cross = x1.Cross( x2 );

	c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecX );
	c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecY );

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );

	if ( steering ) {
		steering->Add( physics, invTimeStep );
	}
	else if ( coneLimit ) {
		coneLimit->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_Hinge::ApplyFriction
================
*/
void idAFConstraint_Hinge::ApplyFriction( float invTimeStep ) {
	idVec3 angular;
	float invMass, currentFriction;

	currentFriction = GetFriction();

	if ( currentFriction <= 0.0f ) {
		return;
	}

	if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) {

		angular = body1->GetAngularVelocity();
		invMass = body1->GetInverseMass();
		if ( body2 ) {
			angular -= body2->GetAngularVelocity();
			invMass += body2->GetInverseMass();
		}

		angular *= currentFriction / invMass;

		body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
		if ( body2 ) {
			body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
		}
	}
	else {
		if ( !fc ) {
			fc = new idAFConstraint_HingeFriction;
			fc->Setup( this );
		}

		fc->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_Hinge::GetForce
================
*/
void idAFConstraint_Hinge::GetForce( idAFBody *body, idVec6 &force ) {
	idAFConstraint::GetForce( body, force );
	// FIXME: add limit force
}

/*
================
idAFConstraint_Hinge::Translate
================
*/
void idAFConstraint_Hinge::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		anchor2 += translation;
	}
	if ( coneLimit ) {
		coneLimit->Translate( translation );
	}
}

/*
================
idAFConstraint_Hinge::Rotate
================
*/
void idAFConstraint_Hinge::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		anchor2 *= rotation;
		axis2 *= rotation.ToMat3();
	}
	if ( coneLimit ) {
		coneLimit->Rotate( rotation );
	}
}

/*
================
idAFConstraint_Hinge::GetCenter
================
*/
void idAFConstraint_Hinge::GetCenter( idVec3 &center ) {
	center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
}

/*
================
idAFConstraint_Hinge::DebugDraw
================
*/
void idAFConstraint_Hinge::DebugDraw( void ) {
	idVec3 vecX, vecY;
	idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	idVec3 x1 = axis1 * body1->GetWorldAxis();
	x1.OrthogonalBasis( vecX, vecY );

	gameRenderWorld->DebugArrow( colorBlue, a1 - 4.0f * x1, a1 + 4.0f * x1, 1 );
	gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecX, a1 + 2.0f * vecX );
	gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecY, a1 + 2.0f * vecY );

	if ( af_showLimits.GetBool() ) {
		if ( coneLimit ) {
			coneLimit->DebugDraw();
		}
	}
}

/*
================
idAFConstraint_Hinge::Save
================
*/
void idAFConstraint_Hinge::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( anchor1 );
	saveFile->WriteVec3( anchor2 );
	saveFile->WriteVec3( axis1 );
	saveFile->WriteVec3( axis2 );
	saveFile->WriteMat3( initialAxis );
	saveFile->WriteFloat( friction );
	if ( coneLimit ) {
		saveFile->WriteBool( true );
		coneLimit->Save( saveFile );
	} else {
		saveFile->WriteBool( false );
	}
	if ( steering ) {
		saveFile->WriteBool( true );
		steering->Save( saveFile );
	} else {
		saveFile->WriteBool( false );
	}
	if ( fc ) {
		saveFile->WriteBool( true );
		fc->Save( saveFile );
	} else {
		saveFile->WriteBool( false );
	}
}

/*
================
idAFConstraint_Hinge::Restore
================
*/
void idAFConstraint_Hinge::Restore( idRestoreGame *saveFile ) {
	bool b;
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( anchor1 );
	saveFile->ReadVec3( anchor2 );
	saveFile->ReadVec3( axis1 );
	saveFile->ReadVec3( axis2 );
	saveFile->ReadMat3( initialAxis );
	saveFile->ReadFloat( friction );

	saveFile->ReadBool( b );
	if ( b ) {
		if ( !coneLimit ) {
			coneLimit = new idAFConstraint_ConeLimit;
		}
		coneLimit->SetPhysics( physics );
		coneLimit->Restore( saveFile );
	}
	saveFile->ReadBool( b );
	if ( b ) {
		if ( !steering ) {
			steering = new idAFConstraint_HingeSteering;
		}
		steering->Setup( this );
		steering->Restore( saveFile );
	}
	saveFile->ReadBool( b );
	if ( b ) {
		if ( !fc ) {
			fc = new idAFConstraint_HingeFriction;
		}
		fc->Setup( this );
		fc->Restore( saveFile );
	}
}


//===============================================================
//
//	idAFConstraint_HingeFriction
//
//===============================================================

/*
================
idAFConstraint_HingeFriction::idAFConstraint_HingeFriction
================
*/
idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( void ) {
	type = CONSTRAINT_FRICTION;
	name = "hingeFriction";
	InitSize( 1 );
	hinge = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_HingeFriction::Setup
================
*/
void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge *h ) {
	this->hinge = h;
	body1 = h->GetBody1();
	body2 = h->GetBody2();
}

/*
================
idAFConstraint_HingeFriction::Evaluate
================
*/
void idAFConstraint_HingeFriction::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_HingeFriction::ApplyFriction
================
*/
void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_HingeFriction::Add
================
*/
bool idAFConstraint_HingeFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
	idVec3 a1, a2;
	float f;

	physics = phys;

	f = hinge->GetFriction() * hinge->GetMultiplier().Length();
	if ( f == 0.0f ) {
		return false;
	}

	lo[0] = -f;
	hi[0] = f;

	hinge->GetAxis( a1, a2 );

	a1 *= body1->GetWorldAxis();

	J1.SetSize( 1, 6 );
	J1.SubVec6(0).SubVec3(0).Zero();
	J1.SubVec6(0).SubVec3(1) = a1;

	if ( body2 ) {
		a2 *= body2->GetWorldAxis();

		J2.SetSize( 1, 6 );
		J2.SubVec6(0).SubVec3(0).Zero();
		J2.SubVec6(0).SubVec3(1) = -a2;
	}

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_HingeFriction::Translate
================
*/
void idAFConstraint_HingeFriction::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_HingeFriction::Rotate
================
*/
void idAFConstraint_HingeFriction::Rotate( const idRotation &rotation ) {
}


//===============================================================
//
//	idAFConstraint_HingeSteering
//
//===============================================================

/*
================
idAFConstraint_HingeSteering::idAFConstraint_HingeSteering
================
*/
idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( void ) {
	type = CONSTRAINT_HINGESTEERING;
	name = "hingeFriction";
	InitSize( 1 );
	hinge = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
	steerSpeed = 0.0f;
	epsilon = LCP_EPSILON;
}

/*
================
idAFConstraint_HingeSteering::Save
================
*/
void idAFConstraint_HingeSteering::Save( idSaveGame *saveFile ) const {
	saveFile->WriteFloat(steerAngle);
	saveFile->WriteFloat(steerSpeed);
	saveFile->WriteFloat(epsilon);
}

/*
================
idAFConstraint_HingeSteering::Restore
================
*/
void idAFConstraint_HingeSteering::Restore( idRestoreGame *saveFile ) {
	saveFile->ReadFloat(steerAngle);
	saveFile->ReadFloat(steerSpeed);
	saveFile->ReadFloat(epsilon);
}

/*
================
idAFConstraint_HingeSteering::Setup
================
*/
void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge *h ) {
	this->hinge = h;
	body1 = h->GetBody1();
	body2 = h->GetBody2();
}

/*
================
idAFConstraint_HingeSteering::Evaluate
================
*/
void idAFConstraint_HingeSteering::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_HingeSteering::ApplyFriction
================
*/
void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_HingeSteering::Add
================
*/
bool idAFConstraint_HingeSteering::Add( idPhysics_AF *phys, float invTimeStep ) {
	float angle, speed;
	idVec3 a1, a2;

	physics = phys;

	hinge->GetAxis( a1, a2 );
	angle = hinge->GetAngle();

	a1 *= body1->GetWorldAxis();

	J1.SetSize( 1, 6 );
	J1.SubVec6(0).SubVec3(0).Zero();
	J1.SubVec6(0).SubVec3(1) = a1;

	if ( body2 ) {
		a2 *= body2->GetWorldAxis();

		J2.SetSize( 1, 6 );
		J2.SubVec6(0).SubVec3(0).Zero();
		J2.SubVec6(0).SubVec3(1) = -a2;
	}

	speed = steerAngle - angle;
	if ( steerSpeed != 0.0f ) {
		if ( speed > steerSpeed ) {
			speed = steerSpeed;
		}
		else if ( speed < -steerSpeed ) {
			speed = -steerSpeed;
		}
	}

	c1[0] = DEG2RAD( speed ) * invTimeStep;

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_HingeSteering::Translate
================
*/
void idAFConstraint_HingeSteering::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_HingeSteering::Rotate
================
*/
void idAFConstraint_HingeSteering::Rotate( const idRotation &rotation ) {
}


//===============================================================
//
//	idAFConstraint_Slider
//
//===============================================================

/*
================
idAFConstraint_Slider::idAFConstraint_Slider
================
*/
idAFConstraint_Slider::idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_SLIDER;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 5 );
	fl.allowPrimary = true;
	fl.noCollision = true;

	if ( body2 ) {
		offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
		relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
	}
	else {
		offset = body1->GetWorldOrigin();
		relAxis = body1->GetWorldAxis();
	}
}

/*
================
idAFConstraint_Slider::SetAxis
================
*/
void idAFConstraint_Slider::SetAxis( const idVec3 &ax ) {
	idVec3 normAxis;

	// get normalized axis relative to body1
	normAxis = ax;
	normAxis.Normalize();
	if ( body2 ) {
		axis = normAxis * body2->GetWorldAxis().Transpose();
	}
	else {
		axis = normAxis;
	}
}

/*
================
idAFConstraint_Slider::Evaluate
================
*/
void idAFConstraint_Slider::Evaluate( float invTimeStep ) {
	idVec3 vecX, vecY, ofs;
	idRotation r;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		(axis * master->GetWorldAxis()).OrthogonalBasis( vecX, vecY );
		ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
		r = ( body1->GetWorldAxis().Transpose() * (relAxis * master->GetWorldAxis()) ).ToRotation();
	}
	else {
		axis.OrthogonalBasis( vecX, vecY );
		ofs = offset - body1->GetWorldOrigin();
		r = ( body1->GetWorldAxis().Transpose() * relAxis ).ToRotation();
	}

	J1.Set(	mat3_zero, mat3_identity,
			idMat3( vecX, vecY, vec3_origin ), mat3_zero );
	J1.SetSize( 5, 6 );

	if ( body2 ) {

		J2.Set(	mat3_zero, -mat3_identity,
				idMat3( -vecX, -vecY, vec3_origin ), mat3_zero );
		J2.SetSize( 5, 6 );
	}
	else {
		J2.Zero( 5, 6 );
	}

	c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * - (float) DEG2RAD( r.GetAngle() ) );

	c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( vecX * ofs );
	c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( vecY * ofs );

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
}

/*
================
idAFConstraint_Slider::ApplyFriction
================
*/
void idAFConstraint_Slider::ApplyFriction( float invTimeStep ) {
	// no friction
}

/*
================
idAFConstraint_Slider::Translate
================
*/
void idAFConstraint_Slider::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		offset += translation;
	}
}

/*
================
idAFConstraint_Slider::Rotate
================
*/
void idAFConstraint_Slider::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		offset *= rotation;
	}
}

/*
================
idAFConstraint_Slider::GetCenter
================
*/
void idAFConstraint_Slider::GetCenter( idVec3 &center ) {
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();
	if ( master ) {
		center = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
	}
	else {
		center = offset - body1->GetWorldOrigin();
	}
}

/*
================
idAFConstraint_Slider::DebugDraw
================
*/
void idAFConstraint_Slider::DebugDraw( void ) {
	idVec3 ofs;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();
	if ( master ) {
		ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
	}
	else {
		ofs = offset - body1->GetWorldOrigin();
	}
	gameRenderWorld->DebugLine( colorGreen, ofs, ofs + axis * body1->GetWorldAxis() );
}

/*
================
idAFConstraint_Slider::Save
================
*/
void idAFConstraint_Slider::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( axis );
	saveFile->WriteVec3( offset );
	saveFile->WriteMat3( relAxis );
}

/*
================
idAFConstraint_Slider::Restore
================
*/
void idAFConstraint_Slider::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( axis );
	saveFile->ReadVec3( offset );
	saveFile->ReadMat3( relAxis );
}


//===============================================================
//
//	idAFConstraint_Line
//
//===============================================================

/*
================
idAFConstraint_Line::idAFConstraint_Line
================
*/
idAFConstraint_Line::idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_Line::Evaluate
================
*/
void idAFConstraint_Line::Evaluate( float invTimeStep ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_Line::ApplyFriction
================
*/
void idAFConstraint_Line::ApplyFriction( float invTimeStep ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_Line::Translate
================
*/
void idAFConstraint_Line::Translate( const idVec3 &translation ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_Line::Rotate
================
*/
void idAFConstraint_Line::Rotate( const idRotation &rotation ) {
	assert( 0 );	// FIXME: implement
}

/*
================
idAFConstraint_Line::DebugDraw
================
*/
void idAFConstraint_Line::DebugDraw( void ) {
	assert( 0 );	// FIXME: implement
}


//===============================================================
//
//	idAFConstraint_Plane
//
//===============================================================

/*
================
idAFConstraint_Plane::idAFConstraint_Plane
================
*/
idAFConstraint_Plane::idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_PLANE;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 1 );
	fl.allowPrimary = true;
	fl.noCollision = true;
}

/*
================
idAFConstraint_Plane::SetPlane
================
*/
void idAFConstraint_Plane::SetPlane( const idVec3 &normal, const idVec3 &anchor ) {
	// get anchor relative to center of mass of body1
	anchor1 = ( anchor - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get anchor relative to center of mass of body2
		anchor2 = ( anchor - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
		planeNormal = normal * body2->GetWorldAxis().Transpose();
	}
	else {
		anchor2 = anchor;
		planeNormal = normal;
	}
}

/*
================
idAFConstraint_Plane::Evaluate
================
*/
void idAFConstraint_Plane::Evaluate( float invTimeStep ) {
	idVec3 a1, a2, normal, p;
	idVec6 v;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	if ( master ) {
		a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
		normal = planeNormal * master->GetWorldAxis();
	}
	else {
		a2 = anchor2;
		normal = planeNormal;
	}

	p = a1 - body1->GetWorldOrigin();
	v.SubVec3(0) = normal;
	v.SubVec3(1) = p.Cross( normal );
	J1.Set( 1, 6, v.ToFloatPtr() );

	if ( body2 ) {
		p = a1 - body2->GetWorldOrigin();
		v.SubVec3(0) = -normal;
		v.SubVec3(1) = p.Cross( -normal );
		J2.Set( 1, 6, v.ToFloatPtr() );
	}

	c1[0] = -( invTimeStep * ERROR_REDUCTION ) * (a1 * normal - a2 * normal);

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
}

/*
================
idAFConstraint_Plane::ApplyFriction
================
*/
void idAFConstraint_Plane::ApplyFriction( float invTimeStep ) {
	// no friction
}

/*
================
idAFConstraint_Plane::Translate
================
*/
void idAFConstraint_Plane::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		anchor2 += translation;
	}
}

/*
================
idAFConstraint_Plane::Rotate
================
*/
void idAFConstraint_Plane::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		anchor2 *= rotation;
		planeNormal *= rotation.ToMat3();
	}
}

/*
================
idAFConstraint_Plane::DebugDraw
================
*/
void idAFConstraint_Plane::DebugDraw( void ) {
	idVec3 a1, normal, right, up;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	if ( master ) {
		normal = planeNormal * master->GetWorldAxis();
	}
	else {
		normal = planeNormal;
	}
	normal.NormalVectors( right, up );
	normal *= 4.0f;
	right *= 4.0f;
	up *= 4.0f;

	gameRenderWorld->DebugLine( colorCyan, a1 - right, a1 + right );
	gameRenderWorld->DebugLine( colorCyan, a1 - up, a1 + up );
	gameRenderWorld->DebugArrow( colorCyan, a1, a1 + normal, 1 );
}

/*
================
idAFConstraint_Plane::Save
================
*/
void idAFConstraint_Plane::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( anchor1 );
	saveFile->WriteVec3( anchor2 );
	saveFile->WriteVec3( planeNormal );
}

/*
================
idAFConstraint_Plane::Restore
================
*/
void idAFConstraint_Plane::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( anchor1 );
	saveFile->ReadVec3( anchor2 );
	saveFile->ReadVec3( planeNormal );
}


//===============================================================
//
//	idAFConstraint_Spring
//
//===============================================================

/*
================
idAFConstraint_Spring::idAFConstraint_Spring
================
*/
idAFConstraint_Spring::idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 ) {
	assert( body1 );
	type = CONSTRAINT_SPRING;
	this->name = name;
	this->body1 = body1;
	this->body2 = body2;
	InitSize( 1 );
	fl.allowPrimary = false;
	kstretch = kcompress = damping = 1.0f;
	minLength = maxLength = restLength = 0.0f;
}

/*
================
idAFConstraint_Spring::SetAnchor
================
*/
void idAFConstraint_Spring::SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 ) {
	// get anchor relative to center of mass of body1
	anchor1 = ( worldAnchor1 - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
	if ( body2 ) {
		// get anchor relative to center of mass of body2
		anchor2 = ( worldAnchor2 - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
	}
	else {
		anchor2 = worldAnchor2;
	}
}

/*
================
idAFConstraint_Spring::SetSpring
================
*/
void idAFConstraint_Spring::SetSpring( const float stretch, const float compress, const float damping, const float restLength ) {
	assert( stretch >= 0.0f && compress >= 0.0f && restLength >= 0.0f );
	this->kstretch = stretch;
	this->kcompress = compress;
	this->damping = damping;
	this->restLength = restLength;
}

/*
================
idAFConstraint_Spring::SetLimit
================
*/
void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLength ) {
	assert( minLength >= 0.0f && maxLength >= 0.0f && maxLength >= minLength );
	this->minLength = minLength;
	this->maxLength = maxLength;
}

/*
================
idAFConstraint_Spring::Evaluate
================
*/
void idAFConstraint_Spring::Evaluate( float invTimeStep ) {
	idVec3 a1, a2, velocity1, velocity2, force;
	idVec6 v1, v2;
	float d, dampingForce, length, error;
	bool limit;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	velocity1 = body1->GetPointVelocity( a1 );

	if ( master ) {
		a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
		velocity2 = master->GetPointVelocity( a2 );
	}
	else {
		a2 = anchor2;
		velocity2.Zero();
	}

	force = a2 - a1;
	d = force * force;
	if ( d != 0.0f ) {
		dampingForce = damping * idMath::Fabs( (velocity2 - velocity1) * force ) / d;
	}
	else {
		dampingForce = 0.0f;
	}
	length = force.Normalize();

	if ( length > restLength ) {
		if ( kstretch > 0.0f ) {
			idVec3 springForce = force * ( Square( length - restLength ) * kstretch - dampingForce );
			body1->AddForce( a1, springForce );
			if ( master ) {
				master->AddForce( a2, -springForce );
			}
		}
	}
	else {
		if ( kcompress > 0.0f ) {
			idVec3 springForce = force * -( Square( restLength - length ) * kcompress - dampingForce );
			body1->AddForce( a1, springForce );
			if ( master ) {
				master->AddForce( a2, -springForce );
			}
		}
	}

	// check for spring limits
	if ( length < minLength ) {
		force = -force;
		error = minLength - length;
		limit = true;
	}
	else if ( maxLength > 0.0f && length > maxLength ) {
		error = length - maxLength;
		limit = true;
	}
	else {
		error = 0.0f;
		limit = false;
	}

	if ( limit ) {
		a1 -= body1->GetWorldOrigin();
		v1.SubVec3(0) = force;
		v1.SubVec3(1) = a1.Cross( force );
		J1.Set( 1, 6, v1.ToFloatPtr() );
		if ( body2 ) {
			a2 -= body2->GetWorldOrigin();
			v2.SubVec3(0) = -force;
			v2.SubVec3(1) = a2.Cross( -force );
			J2.Set( 1, 6, v2.ToFloatPtr() );
		}
		c1[0] = -( invTimeStep * ERROR_REDUCTION ) * error;
		lo[0] = 0.0f;
	}
	else {
		J1.Zero( 0, 0 );
		J2.Zero( 0, 0 );
	}

	c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
}

/*
================
idAFConstraint_Spring::ApplyFriction
================
*/
void idAFConstraint_Spring::ApplyFriction( float invTimeStep ) {
	// no friction
}

/*
================
idAFConstraint_Spring::Translate
================
*/
void idAFConstraint_Spring::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		anchor2 += translation;
	}
}

/*
================
idAFConstraint_Spring::Rotate
================
*/
void idAFConstraint_Spring::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		anchor2 *= rotation;
	}
}

/*
================
idAFConstraint_Spring::GetCenter
================
*/
void idAFConstraint_Spring::GetCenter( idVec3 &center ) {
	idAFBody *master;
	idVec3 a1, a2;

	master = body2 ? body2 : physics->GetMasterBody();
	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	if ( master ) {
		a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
	}
	else {
		a2 = anchor2;
	}
	center = ( a1 + a2 ) * 0.5f;
}

/*
================
idAFConstraint_Spring::DebugDraw
================
*/
void idAFConstraint_Spring::DebugDraw( void ) {
	idAFBody *master;
	float length;
	idVec3 a1, a2, dir, mid, p;

	master = body2 ? body2 : physics->GetMasterBody();
	a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
	if ( master ) {
		a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
	}
	else {
		a2 = anchor2;
	}
	dir = a2 - a1;
	mid = a1 + 0.5f * dir;
	length = dir.Normalize();

	// draw spring
	gameRenderWorld->DebugLine( colorGreen, a1, a2 );

	// draw rest length
	p = restLength * 0.5f * dir;
	gameRenderWorld->DebugCircle( colorWhite, mid + p, dir, 1.0f, 10 );
	gameRenderWorld->DebugCircle( colorWhite, mid - p, dir, 1.0f, 10 );
	if ( restLength > length ) {
		gameRenderWorld->DebugLine( colorWhite, a2, mid + p );
		gameRenderWorld->DebugLine( colorWhite, a1, mid - p );
	}

	if ( minLength > 0.0f ) {
		// draw min length
		gameRenderWorld->DebugCircle( colorBlue, mid + minLength * 0.5f * dir, dir, 2.0f, 10 );
		gameRenderWorld->DebugCircle( colorBlue, mid - minLength * 0.5f * dir, dir, 2.0f, 10 );
	}

	if ( maxLength > 0.0f ) {
		// draw max length
		gameRenderWorld->DebugCircle( colorRed, mid + maxLength * 0.5f * dir, dir, 2.0f, 10 );
		gameRenderWorld->DebugCircle( colorRed, mid - maxLength * 0.5f * dir, dir, 2.0f, 10 );
	}
}

/*
================
idAFConstraint_Spring::Save
================
*/
void idAFConstraint_Spring::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( anchor1 );
	saveFile->WriteVec3( anchor2 );
	saveFile->WriteFloat( kstretch );
	saveFile->WriteFloat( kcompress );
	saveFile->WriteFloat( damping );
	saveFile->WriteFloat( restLength );
	saveFile->WriteFloat( minLength );
	saveFile->WriteFloat( maxLength );
}

/*
================
idAFConstraint_Spring::Restore
================
*/
void idAFConstraint_Spring::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( anchor1 );
	saveFile->ReadVec3( anchor2 );
	saveFile->ReadFloat( kstretch );
	saveFile->ReadFloat( kcompress );
	saveFile->ReadFloat( damping );
	saveFile->ReadFloat( restLength );
	saveFile->ReadFloat( minLength );
	saveFile->ReadFloat( maxLength );
}


//===============================================================
//
//	idAFConstraint_Contact
//
//===============================================================

/*
================
idAFConstraint_Contact::idAFConstraint_Contact
================
*/
idAFConstraint_Contact::idAFConstraint_Contact( void ) {
	name = "contact";
	type = CONSTRAINT_CONTACT;
	InitSize( 1 );
	fc = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_Contact::~idAFConstraint_Contact
================
*/
idAFConstraint_Contact::~idAFConstraint_Contact( void ) {
	if ( fc ) {
		delete fc;
	}
}

/*
================
idAFConstraint_Contact::Setup
================
*/
void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c ) {
	idVec3 p;
	idVec6 v;
	float vel;
	float minBounceVelocity = 2.0f;

	assert( b1 );

	body1 = b1;
	body2 = b2;
	contact = c;

	p = c.point - body1->GetWorldOrigin();
	v.SubVec3(0) = c.normal;
	v.SubVec3(1) = p.Cross( c.normal );
	J1.Set( 1, 6, v.ToFloatPtr() );
	vel = v.SubVec3(0) * body1->GetLinearVelocity() + v.SubVec3(1) * body1->GetAngularVelocity();

	if ( body2 ) {
		p = c.point - body2->GetWorldOrigin();
		v.SubVec3(0) = -c.normal;
		v.SubVec3(1) = p.Cross( -c.normal );
		J2.Set( 1, 6, v.ToFloatPtr() );
		vel += v.SubVec3(0) * body2->GetLinearVelocity() + v.SubVec3(1) * body2->GetAngularVelocity();
		c2[0] = 0.0f;
	}

	if ( body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity ) {
		c1[0] = body1->GetBouncyness() * vel;
	} else {
		c1[0] = 0.0f;
	}

	e[0] = CONTACT_LCP_EPSILON;
	lo[0] = 0.0f;
	hi[0] = idMath::INFINITY;
	boxConstraint = NULL;
	boxIndex[0] = -1;
}

/*
================
idAFConstraint_Contact::Evaluate
================
*/
void idAFConstraint_Contact::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_Contact::ApplyFriction
================
*/
void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) {
	idVec3 r, velocity, normal;
	float friction, magnitude, forceNumerator, forceDenominator;
	idVecX impulse, dv;

	friction = body1->GetContactFriction();
	if ( body2 && body2->GetContactFriction() < friction ) {
		friction = body2->GetContactFriction();
	}

	friction *= physics->GetContactFrictionScale();

	if ( friction <= 0.0f ) {
		return;
	}

	// seperate friction per contact is silly but it's fast and often looks close enough
	if ( af_useImpulseFriction.GetBool() ) {

		impulse.SetData( 6, VECX_ALLOCA( 6 ) );
		dv.SetData( 6, VECX_ALLOCA( 6 ) );

		// calculate velocity in the contact plane
		r = contact.point - body1->GetWorldOrigin();
		velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r );
		velocity -= contact.normal * velocity * contact.normal;

		// get normalized direction of friction and magnitude of velocity
		normal = -velocity;
		magnitude = normal.Normalize();

		forceNumerator = friction * magnitude;
		forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
		impulse.SubVec3(0) = (forceNumerator / forceDenominator) * normal;
		impulse.SubVec3(1) = r.Cross( impulse.SubVec3(0) );
		body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() );

		// modify velocity with friction force
		body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3(0) );
		body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3(1) );
	}
	else {

		if ( !fc ) {
			fc = new idAFConstraint_ContactFriction;
		}
		// call setup each frame because contact constraints are re-used for different bodies
		fc->Setup( this );
		fc->Add( physics, invTimeStep );
	}
}

/*
================
idAFConstraint_Contact::Translate
================
*/
void idAFConstraint_Contact::Translate( const idVec3 &translation ) {
	assert( 0 );	// contact should never be translated
}

/*
================
idAFConstraint_Contact::Rotate
================
*/
void idAFConstraint_Contact::Rotate( const idRotation &rotation ) {
	assert( 0 );	// contact should never be rotated
}

/*
================
idAFConstraint_Contact::GetCenter
================
*/
void idAFConstraint_Contact::GetCenter( idVec3 &center ) {
	center = contact.point;
}

/*
================
idAFConstraint_Contact::DebugDraw
================
*/
void idAFConstraint_Contact::DebugDraw( void ) {
	idVec3 x, y;
	contact.normal.NormalVectors( x, y );
	gameRenderWorld->DebugLine( colorWhite, contact.point, contact.point + 6.0f * contact.normal );
	gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x );
	gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y );
}


//===============================================================
//
//	idAFConstraint_ContactFriction
//
//===============================================================

/*
================
idAFConstraint_ContactFriction::idAFConstraint_ContactFriction
================
*/
idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) {
	type = CONSTRAINT_FRICTION;
	name = "contactFriction";
	InitSize( 2 );
	cc = NULL;
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_ContactFriction::Setup
================
*/
void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) {
	this->cc = cc;
	body1 = cc->GetBody1();
	body2 = cc->GetBody2();
}

/*
================
idAFConstraint_ContactFriction::Evaluate
================
*/
void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_ContactFriction::ApplyFriction
================
*/
void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_ContactFriction::Add
================
*/
bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep ) {
	idVec3 r, dir1, dir2;
	float friction;
	int newRow;

	physics = phys;

	friction = body1->GetContactFriction() * physics->GetContactFrictionScale();

	// if the body only has friction in one direction
	if ( body1->GetFrictionDirection( dir1 ) ) {
		// project the friction direction into the contact plane
		dir1 -= dir1 * cc->GetContact().normal * dir1;
		dir1.Normalize();

		r = cc->GetContact().point - body1->GetWorldOrigin();

		J1.SetSize( 1, 6 );
		J1.SubVec6(0).SubVec3(0) = dir1;
		J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
		c1.SetSize( 1 );
		c1[0] = 0.0f;

		if ( body2 ) {
			r = cc->GetContact().point - body2->GetWorldOrigin();

			J2.SetSize( 1, 6 );
			J2.SubVec6(0).SubVec3(0) = -dir1;
			J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
			c2.SetSize( 1 );
			c2[0] = 0.0f;
		}

		lo[0] = -friction;
		hi[0] = friction;
		boxConstraint = cc;
		boxIndex[0] = 0;
	}
	else {
		// get two friction directions orthogonal to contact normal
		cc->GetContact().normal.NormalVectors( dir1, dir2 );

		r = cc->GetContact().point - body1->GetWorldOrigin();

		J1.SetSize( 2, 6 );
		J1.SubVec6(0).SubVec3(0) = dir1;
		J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 );
		J1.SubVec6(1).SubVec3(0) = dir2;
		J1.SubVec6(1).SubVec3(1) = r.Cross( dir2 );
		c1.SetSize( 2 );
		c1[0] = c1[1] = 0.0f;

		if ( body2 ) {
			r = cc->GetContact().point - body2->GetWorldOrigin();

			J2.SetSize( 2, 6 );
			J2.SubVec6(0).SubVec3(0) = -dir1;
			J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 );
			J2.SubVec6(1).SubVec3(0) = -dir2;
			J2.SubVec6(1).SubVec3(1) = r.Cross( -dir2 );
			c2.SetSize( 2 );
			c2[0] = c2[1] = 0.0f;

			if ( body2->GetContactFriction() < friction ) {
				friction = body2->GetContactFriction();
			}
		}

		lo[0] = -friction;
		hi[0] = friction;
		boxConstraint = cc;
		boxIndex[0] = 0;
		lo[1] = -friction;
		hi[1] = friction;
		boxIndex[1] = 0;
	}

	if ( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f ) {
		// project the motor force direction into the contact plane
		dir1 -= dir1 * cc->GetContact().normal * dir1;
		dir1.Normalize();

		r = cc->GetContact().point - body1->GetWorldOrigin();

		newRow = J1.GetNumRows();
		J1.ChangeSize( newRow+1, J1.GetNumColumns() );
		J1.SubVec6(newRow).SubVec3(0) = -dir1;
		J1.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
		c1.ChangeSize( newRow+1 );
		c1[newRow] = body1->GetContactMotorVelocity();

		if ( body2 ) {
			r = cc->GetContact().point - body2->GetWorldOrigin();

			J2.ChangeSize( newRow+1, J2.GetNumColumns() );
			J2.SubVec6(newRow).SubVec3(0) = -dir1;
			J2.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 );
			c2.ChangeSize( newRow+1 );
			c2[newRow] = 0.0f;
		}

		lo[newRow] = -body1->GetContactMotorForce();
		hi[newRow] = body1->GetContactMotorForce();
		boxIndex[newRow] = -1;
	}

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_ContactFriction::Translate
================
*/
void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_ContactFriction::Rotate
================
*/
void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) {
}

/*
================
idAFConstraint_ContactFriction::DebugDraw
================
*/
void idAFConstraint_ContactFriction::DebugDraw( void ) {
}


//===============================================================
//
//	idAFConstraint_ConeLimit
//
//===============================================================

/*
================
idAFConstraint_ConeLimit::idAFConstraint_ConeLimit
================
*/
idAFConstraint_ConeLimit::idAFConstraint_ConeLimit( void ) {
	type = CONSTRAINT_CONELIMIT;
	name = "coneLimit";
	InitSize( 1 );
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_ConeLimit::Setup

  the coneAnchor is the top of the cone in body2 space
  the coneAxis is the axis of the cone in body2 space
  the coneAngle is the angle the cone hull makes at the top
  the body1Axis is the axis in body1 space that should stay within the cone
================
*/
void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) {
	this->body1 = b1;
	this->body2 = b2;
	this->coneAxis = coneAxis;
	this->coneAxis.Normalize();
	this->coneAnchor = coneAnchor;
	this->body1Axis = body1Axis;
	this->body1Axis.Normalize();
	this->cosAngle = (float) cos( DEG2RAD( coneAngle * 0.5f ) );
	this->sinHalfAngle = (float) sin( DEG2RAD( coneAngle * 0.25f ) );
	this->cosHalfAngle = (float) cos( DEG2RAD( coneAngle * 0.25f ) );
}

/*
================
idAFConstraint_ConeLimit::SetAnchor
================
*/
void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) {
	this->coneAnchor = coneAnchor;
}

/*
================
idAFConstraint_ConeLimit::SetBody1Axis
================
*/
void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) {
	this->body1Axis = body1Axis;
}

/*
================
idAFConstraint_ConeLimit::Evaluate
================
*/
void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_ConeLimit::ApplyFriction
================
*/
void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) {
}

/*
================
idAFConstraint_ConeLimit::Add
================
*/
bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
	float a;
	idVec6 J1row, J2row;
	idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2;
	idQuat q;
	idAFBody *master;

	if ( af_skipLimits.GetBool() ) {
		lm.Zero();	// constraint exerts no force
		return false;
	}

	physics = phys;

	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		ax = coneAxis * master->GetWorldAxis();
		anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
	}
	else {
		ax = coneAxis;
		anchor = coneAnchor;
	}

	body1ax = body1Axis * body1->GetWorldAxis();

	a = ax * body1ax;

	// if the body1 axis is inside the cone
	if ( a > cosAngle ) {
		lm.Zero();	// constraint exerts no force
		return false;
	}

	// calculate the inward cone normal for the position the body1 axis went outside the cone
	normal = body1ax.Cross( ax );
	normal.Normalize();
	q.x = normal.x * sinHalfAngle;
	q.y = normal.y * sinHalfAngle;
	q.z = normal.z * sinHalfAngle;
	q.w = cosHalfAngle;
	coneVector = ax * q.ToMat3();
	normal = coneVector.Cross( ax ).Cross( coneVector );
	normal.Normalize();

	p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin();

	J1row.SubVec3(0) = normal;
	J1row.SubVec3(1) = p1.Cross( normal );
	J1.Set( 1, 6, J1row.ToFloatPtr() );

	c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );

	if ( body2 ) {

		p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin();

		J2row.SubVec3(0) = -normal;
		J2row.SubVec3(1) = p2.Cross( -normal );
		J2.Set( 1, 6, J2row.ToFloatPtr() );

		c2[0] = 0.0f;
	}

	lo[0] = 0.0f;
	e[0] = LIMIT_LCP_EPSILON;

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_ConeLimit::Translate
================
*/
void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		coneAnchor += translation;
	}
}

/*
================
idAFConstraint_ConeLimit::Rotate
================
*/
void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		coneAnchor *= rotation;
		coneAxis *= rotation.ToMat3();
	}
}

/*
================
idAFConstraint_ConeLimit::DebugDraw
================
*/
void idAFConstraint_ConeLimit::DebugDraw( void ) {
	idVec3 ax, anchor, x, y, z, start, end;
	float sinAngle, a, size = 10.0f;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		ax = coneAxis * master->GetWorldAxis();
		anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
	}
	else {
		ax = coneAxis;
		anchor = coneAnchor;
	}

	// draw body1 axis
	gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );

	// draw cone
	ax.NormalVectors( x, y );
	sinAngle = idMath::Sqrt( 1.0f - cosAngle * cosAngle );
	x *= size * sinAngle;
	y *= size * sinAngle;
	z = anchor + ax * size * cosAngle;
	start = x + z;
	for ( a = 0.0f; a < 360.0f; a += 45.0f ) {
		end = x * (float) cos( DEG2RAD(a + 45.0f) ) + y * (float) sin( DEG2RAD(a + 45.0f) ) + z;
		gameRenderWorld->DebugLine( colorMagenta, anchor, start );
		gameRenderWorld->DebugLine( colorMagenta, start, end );
		start = end;
	}
}

/*
================
idAFConstraint_ConeLimit::Save
================
*/
void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( coneAnchor );
	saveFile->WriteVec3( coneAxis );
	saveFile->WriteVec3( body1Axis );
	saveFile->WriteFloat( cosAngle );
	saveFile->WriteFloat( sinHalfAngle );
	saveFile->WriteFloat( cosHalfAngle );
	saveFile->WriteFloat( epsilon );
}

/*
================
idAFConstraint_ConeLimit::Restore
================
*/
void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( coneAnchor );
	saveFile->ReadVec3( coneAxis );
	saveFile->ReadVec3( body1Axis );
	saveFile->ReadFloat( cosAngle );
	saveFile->ReadFloat( sinHalfAngle );
	saveFile->ReadFloat( cosHalfAngle );
	saveFile->ReadFloat( epsilon );
}


//===============================================================
//
//	idAFConstraint_PyramidLimit
//
//===============================================================

/*
================
idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit
================
*/
idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) {
	type = CONSTRAINT_PYRAMIDLIMIT;
	name = "pyramidLimit";
	InitSize( 1 );
	fl.allowPrimary = false;
	fl.frameConstraint = true;
}

/*
================
idAFConstraint_PyramidLimit::Setup
================
*/
void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor,
								const idVec3 &pyramidAxis, const idVec3 &baseAxis,
								const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ) {
	body1 = b1;
	body2 = b2;
	// setup the base and make sure the basis is orthonormal
	pyramidBasis[2] = pyramidAxis;
	pyramidBasis[2].Normalize();
	pyramidBasis[0] = baseAxis;
	pyramidBasis[0] -= pyramidBasis[2] * baseAxis * pyramidBasis[2];
	pyramidBasis[0].Normalize();
	pyramidBasis[1] = pyramidBasis[0].Cross( pyramidBasis[2] );
	// pyramid top
	this->pyramidAnchor = pyramidAnchor;
	// angles
	cosAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.5f ) );
	cosAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.5f ) );
	sinHalfAngle[0] = (float) sin( DEG2RAD( pyramidAngle1 * 0.25f ) );
	sinHalfAngle[1] = (float) sin( DEG2RAD( pyramidAngle2 * 0.25f ) );
	cosHalfAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.25f ) );
	cosHalfAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.25f ) );

	this->body1Axis = body1Axis;
}

/*
================
idAFConstraint_PyramidLimit::SetAnchor
================
*/
void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) {
	this->pyramidAnchor = pyramidAnchor;
}

/*
================
idAFConstraint_PyramidLimit::SetBody1Axis
================
*/
void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) {
	this->body1Axis = body1Axis;
}

/*
================
idAFConstraint_PyramidLimit::Evaluate
================
*/
void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_PyramidLimit::ApplyFriction
================
*/
void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) {
}

/*
================
idAFConstraint_PyramidLimit::Add
================
*/
bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) {
	int i;
	float a[2];
	idVec6 J1row, J2row;
	idMat3 worldBase;
	idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2;
	idQuat q;
	idAFBody *master;

	if ( af_skipLimits.GetBool() ) {
		lm.Zero();	// constraint exerts no force
		return false;
	}

	physics = phys;
	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
		worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
		worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
		anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
	}
	else {
		worldBase = pyramidBasis;
		anchor = pyramidAnchor;
	}

	body1ax = body1Axis * body1->GetWorldAxis();

	for ( i = 0; i < 2; i++ ) {
		ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i];
		ax[i].Normalize();
		a[i] = worldBase[2] * ax[i];
	}

	// if the body1 axis is inside the pyramid
	if ( a[0] > cosAngle[0] && a[1] > cosAngle[1] ) {
		lm.Zero();	// constraint exerts no force
		return false;
	}

	// calculate the inward pyramid normal for the position the body1 axis went outside the pyramid
	pyramidVector = worldBase[2];
	for ( i = 0; i < 2; i++ ) {
		if ( a[i] <= cosAngle[i] ) {
			v = ax[i].Cross( worldBase[2] );
			v.Normalize();
			q.x = v.x * sinHalfAngle[i];
			q.y = v.y * sinHalfAngle[i];
			q.z = v.z * sinHalfAngle[i];
			q.w = cosHalfAngle[i];
			pyramidVector *= q.ToMat3();
		}
	}
	normal = pyramidVector.Cross( worldBase[2] ).Cross( pyramidVector );
	normal.Normalize();

	p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin();

	J1row.SubVec3(0) = normal;
	J1row.SubVec3(1) = p1.Cross( normal );
	J1.Set( 1, 6, J1row.ToFloatPtr() );

	c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) );

	if ( body2 ) {

		p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin();

		J2row.SubVec3(0) = -normal;
		J2row.SubVec3(1) = p2.Cross( -normal );
		J2.Set( 1, 6, J2row.ToFloatPtr() );

		c2[0] = 0.0f;
	}

	lo[0] = 0.0f;
	e[0] = LIMIT_LCP_EPSILON;

	physics->AddFrameConstraint( this );

	return true;
}

/*
================
idAFConstraint_PyramidLimit::Translate
================
*/
void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) {
	if ( !body2 ) {
		pyramidAnchor += translation;
	}
}

/*
================
idAFConstraint_PyramidLimit::Rotate
================
*/
void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) {
	if ( !body2 ) {
		pyramidAnchor *= rotation;
		pyramidBasis[0] *= rotation.ToMat3();
		pyramidBasis[1] *= rotation.ToMat3();
		pyramidBasis[2] *= rotation.ToMat3();
	}
}

/*
================
idAFConstraint_PyramidLimit::DebugDraw
================
*/
void idAFConstraint_PyramidLimit::DebugDraw( void ) {
	int i;
	float size = 10.0f;
	idVec3 anchor, dir, p[4];
	idMat3 worldBase, m[2];
	idQuat q;
	idAFBody *master;

	master = body2 ? body2 : physics->GetMasterBody();

	if ( master ) {
		worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
		worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
		worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
		anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
	}
	else {
		worldBase = pyramidBasis;
		anchor = pyramidAnchor;
	}

	// draw body1 axis
	gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) );

	// draw the pyramid
	for ( i = 0; i < 2; i++ ) {
		q.x = worldBase[!i].x * sinHalfAngle[i];
		q.y = worldBase[!i].y * sinHalfAngle[i];
		q.z = worldBase[!i].z * sinHalfAngle[i];
		q.w = cosHalfAngle[i];
		m[i] = q.ToMat3();
	}

	dir = worldBase[2] * size;
	p[0] = anchor + m[0] * (m[1] * dir);
	p[1] = anchor + m[0] * (m[1].Transpose() * dir);
	p[2] = anchor + m[0].Transpose() * (m[1].Transpose() * dir);
	p[3] = anchor + m[0].Transpose() * (m[1] * dir);

	for ( i = 0; i < 4; i++ ) {
		gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] );
		gameRenderWorld->DebugLine( colorMagenta, p[i], p[(i+1)&3] );
	}
}

/*
================
idAFConstraint_PyramidLimit::Save
================
*/
void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const {
	idAFConstraint::Save( saveFile );
	saveFile->WriteVec3( pyramidAnchor );
	saveFile->WriteMat3( pyramidBasis );
	saveFile->WriteVec3( body1Axis );
	saveFile->WriteFloat( cosAngle[0] );
	saveFile->WriteFloat( cosAngle[1] );
	saveFile->WriteFloat( sinHalfAngle[0] );
	saveFile->WriteFloat( sinHalfAngle[1] );
	saveFile->WriteFloat( cosHalfAngle[0] );
	saveFile->WriteFloat( cosHalfAngle[1] );
	saveFile->WriteFloat( epsilon );
}

/*
================
idAFConstraint_PyramidLimit::Restore
================
*/
void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) {
	idAFConstraint::Restore( saveFile );
	saveFile->ReadVec3( pyramidAnchor );
	saveFile->ReadMat3( pyramidBasis );
	saveFile->ReadVec3( body1Axis );
	saveFile->ReadFloat( cosAngle[0] );
	saveFile->ReadFloat( cosAngle[1] );
	saveFile->ReadFloat( sinHalfAngle[0] );
	saveFile->ReadFloat( sinHalfAngle[1] );
	saveFile->ReadFloat( cosHalfAngle[0] );
	saveFile->ReadFloat( cosHalfAngle[1] );
	saveFile->ReadFloat( epsilon );
}


//===============================================================
//
//	idAFConstraint_Suspension
//
//===============================================================

/*
================
idAFConstraint_Suspension::idAFConstraint_Suspension
================
*/
idAFConstraint_Suspension::idAFConstraint_Suspension( void ) {
	type = CONSTRAINT_SUSPENSION;
	name = "suspension";
	InitSize( 3 );
	fl.allowPrimary = false;
	fl.frameConstraint = true;

	localOrigin.Zero();
	localAxis.Identity();
	suspensionUp = 0.0f;
	suspensionDown = 0.0f;
	suspensionKCompress = 0.0f;
	suspensionDamping = 0.0f;
	steerAngle = 0.0f;
	friction = 2.0f;
	motorEnabled = false;
	motorForce = 0.0f;
	motorVelocity = 0.0f;
	wheelModel = NULL;
	memset( &trace, 0, sizeof( trace ) );
	epsilon = LCP_EPSILON;
}

/*
================
idAFConstraint_Suspension::Setup
================
*/
void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ) {
	this->name = name;
	body1 = body;
	body2 = NULL;
	localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose();
	localAxis = axis * body->GetWorldAxis().Transpose();
	wheelModel = clipModel;
}

/*
================
idAFConstraint_Suspension::SetSuspension
================
*/
void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f ) {
	suspensionUp = up;
	suspensionDown = down;
	suspensionKCompress = k;
	suspensionDamping = d;
	friction = f;
}

/*
================
idAFConstraint_Suspension::GetWheelOrigin
================
*/
const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const {
	return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis();
}

/*
================
idAFConstraint_Suspension::Evaluate
================
*/
void idAFConstraint_Suspension::Evaluate( float invTimeStep ) {
	float velocity, suspensionLength, springLength, compression, dampingForce, springForce;
	idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir;
	idMat3 axis;
	idRotation rotation;

	axis = localAxis * body1->GetWorldAxis();
	origin = body1->GetWorldOrigin() + localOrigin * body1->GetWorldAxis();
	start = origin + suspensionUp * axis[2];
	end = origin - suspensionDown * axis[2];

	rotation.SetVec( axis[2] );
	rotation.SetAngle( steerAngle );

	axis *= rotation.ToMat3();

	gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL );

	wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();

	if ( trace.fraction >= 1.0f ) {
		J1.SetSize( 0, 6 );
		if ( body2 ) {
			J2.SetSize( 0, 6 );
		}
		return;
	}

	// calculate and add spring force
	vel1 = body1->GetPointVelocity( start );
	if ( body2 ) {
		vel2 = body2->GetPointVelocity( trace.c.point );
	} else {
		vel2.Zero();
	}

	suspensionLength = suspensionUp + suspensionDown;
	springDir = trace.endpos - start;
	springLength = trace.fraction * suspensionLength;
	dampingForce = suspensionDamping * idMath::Fabs( ( vel2 - vel1 ) * springDir ) / ( 1.0f + springLength * springLength );
	compression = suspensionLength - springLength;
	springForce = compression * compression * suspensionKCompress - dampingForce;

	r = trace.c.point - body1->GetWorldOrigin();
	J1.SetSize( 2, 6 );
	J1.SubVec6(0).SubVec3(0) = trace.c.normal;
	J1.SubVec6(0).SubVec3(1) = r.Cross( trace.c.normal );
	c1.SetSize( 2 );
	c1[0] = 0.0f;
	velocity = J1.SubVec6(0).SubVec3(0) * body1->GetLinearVelocity() + J1.SubVec6(0).SubVec3(1) * body1->GetAngularVelocity();

	if ( body2 ) {
		r = trace.c.point - body2->GetWorldOrigin();
		J2.SetSize( 2, 6 );
		J2.SubVec6(0).SubVec3(0) = -trace.c.normal;
		J2.SubVec6(0).SubVec3(1) = r.Cross( -trace.c.normal );
		c2.SetSize( 2 );
		c2[0] = 0.0f;
		velocity += J2.SubVec6(0).SubVec3(0) * body2->GetLinearVelocity() + J2.SubVec6(0).SubVec3(1) * body2->GetAngularVelocity();
	}

	c1[0] = -compression;		// + 0.5f * -velocity;

	e[0] = 1e-4f;
	lo[0] = 0.0f;
	hi[0] = springForce;
	boxConstraint = NULL;
	boxIndex[0] = -1;

	// project the friction direction into the contact plane
	frictionDir = axis[1] - axis[1] * trace.c.normal * axis[1];
	frictionDir.Normalize();

	r = trace.c.point - body1->GetWorldOrigin();

	J1.SubVec6(1).SubVec3(0) = frictionDir;
	J1.SubVec6(1).SubVec3(1) = r.Cross( frictionDir );
	c1[1] = 0.0f;

	if ( body2 ) {
		r = trace.c.point - body2->GetWorldOrigin();

		J2.SubVec6(1).SubVec3(0) = -frictionDir;
		J2.SubVec6(1).SubVec3(1) = r.Cross( -frictionDir );
		c2[1] = 0.0f;
	}

	lo[1] = -friction * physics->GetContactFrictionScale();
	hi[1] = friction * physics->GetContactFrictionScale();

	boxConstraint = this;
	boxIndex[1] = 0;


	if ( motorEnabled ) {
		// project the motor force direction into the contact plane
		motorDir = axis[0] - axis[0] * trace.c.normal * axis[0];
		motorDir.Normalize();

		r = trace.c.point - body1->GetWorldOrigin();

		J1.ChangeSize( 3, J1.GetNumColumns() );
		J1.SubVec6(2).SubVec3(0) = -motorDir;
		J1.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
		c1.ChangeSize( 3 );
		c1[2] = motorVelocity;

		if ( body2 ) {
			r = trace.c.point - body2->GetWorldOrigin();

			J2.ChangeSize( 3, J2.GetNumColumns() );
			J2.SubVec6(2).SubVec3(0) = -motorDir;
			J2.SubVec6(2).SubVec3(1) = r.Cross( -motorDir );
			c2.ChangeSize( 3 );
			c2[2] = 0.0f;
		}

		lo[2] = -motorForce;
		hi[2] = motorForce;
		boxIndex[2] = -1;
	}
}

/*
================
idAFConstraint_Suspension::ApplyFriction
================
*/
void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) {
	// do nothing
}

/*
================
idAFConstraint_Suspension::Translate
================
*/
void idAFConstraint_Suspension::Translate( const idVec3 &translation ) {
}

/*
================
idAFConstraint_Suspension::Rotate
================
*/
void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) {
}

/*
================
idAFConstraint_Suspension::DebugDraw
================
*/
void idAFConstraint_Suspension::DebugDraw( void ) {
	idVec3 origin;
	idMat3 axis;
	idRotation rotation;

	axis = localAxis * body1->GetWorldAxis();

	rotation.SetVec( axis[2] );
	rotation.SetAngle( steerAngle );

	axis *= rotation.ToMat3();

	if ( trace.fraction < 1.0f ) {
		origin = trace.c.point;

		gameRenderWorld->DebugLine( colorWhite, origin, origin + 6.0f * axis[2] );
		gameRenderWorld->DebugLine( colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0] );
		gameRenderWorld->DebugLine( colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1] );
	}
}


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

/*
================
idAFBody::idAFBody
================
*/
idAFBody::idAFBody( void ) {
	Init();
}

/*
================
idAFBody::idAFBody
================
*/
idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) {

	assert( clipModel );
	assert( clipModel->IsTraceModel() );

	Init();

	this->name = name;
	this->clipModel = NULL;

	SetClipModel( clipModel );
	SetDensity( density );

	current->worldOrigin = clipModel->GetOrigin();
	current->worldAxis = clipModel->GetAxis();
	*next = *current;

}

/*
================
idAFBody::~idAFBody
================
*/
idAFBody::~idAFBody( void ) {
	delete clipModel;
}

/*
================
idAFBody::Init
================
*/
void idAFBody::Init( void ) {
	name						= "noname";
	parent						= NULL;
	clipModel					= NULL;
	primaryConstraint			= NULL;
	tree						= NULL;

	linearFriction				= -1.0f;
	angularFriction				= -1.0f;
	contactFriction				= -1.0f;
	bouncyness					= -1.0f;
	clipMask					= 0;

	frictionDir					= vec3_zero;
	contactMotorDir				= vec3_zero;
	contactMotorVelocity		= 0.0f;
	contactMotorForce			= 0.0f;

	mass						= 1.0f;
	invMass						= 1.0f;
	centerOfMass				= vec3_zero;
	inertiaTensor				= mat3_identity;
	inverseInertiaTensor		= mat3_identity;
#ifdef MOD_WATERPHYSICS
	this->volume                = 1.0f;
	this->liquidMass            = 1.0f;
	this->invLiquidMass         = 1.0f;
	this->waterLevel            = 0.0f;
#endif

	current						= &state[0];
	next						= &state[1];
	current->worldOrigin		= vec3_zero;
	current->worldAxis			= mat3_identity;
	current->spatialVelocity	= vec6_zero;
	current->externalForce		= vec6_zero;
	*next						= *current;
	saved						= *current;
	atRestOrigin				= vec3_zero;
	atRestAxis					= mat3_identity;

	s.Zero( 6 );
	totalForce.Zero( 6 );
	auxForce.Zero( 6 );
	acceleration.Zero( 6 );

	response					= NULL;
	responseIndex				= NULL;
	numResponses				= 0;
	maxAuxiliaryIndex			= 0;
	maxSubTreeAuxiliaryIndex	= 0;

	memset( &fl, 0, sizeof( fl ) );

	fl.selfCollision			= true;
	fl.isZero					= true;

	m_RerouteEnt				= NULL;
}

/*
================
idAFBody::SetClipModel
================
*/
void idAFBody::SetClipModel( idClipModel *clipModel ) {
	if ( this->clipModel && this->clipModel != clipModel ) {
		delete this->clipModel;
	}
	this->clipModel = clipModel;
}

/*
================
idAFBody::SetFriction
================
*/
void idAFBody::SetFriction( float linear, float angular, float contact ) {
	if ( linear < 0.0f || linear > 1.0f ||
			angular < 0.0f || angular > 1.0f ||
				contact < 0.0f ) {
		gameLocal.Warning( "idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact );
		return;
	}
	linearFriction = linear;
	angularFriction = angular;
	contactFriction = contact;
}

/*
================
idAFBody::SetBouncyness
================
*/
void idAFBody::SetBouncyness( float bounce ) {
	if ( bounce < 0.0f || bounce > 1.0f ) {
		gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce );
		return;
	}
	bouncyness = bounce;
}

/*
================
idAFBody::SetDensity
================
*/
void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) {

	// get the body mass properties
	clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );

	// make sure we have a valid mass
	if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) {
		gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() );
		mass = 1.0f;
		centerOfMass.Zero();
		inertiaTensor.Identity();
	}

	// make sure the center of mass is at the body origin
	if ( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {
		gameLocal.Warning( "idAFBody::SetDensity: center of mass (%f, %f, %f) not at origin (%f, %f, %f) for body '%s'",
		 vec3_origin.x, vec3_origin.y, vec3_origin.z, 
		 centerOfMass.x, centerOfMass.y, centerOfMass.z, name.c_str() );
	}
	centerOfMass.Zero();

	// calculate the inverse mass and inverse inertia tensor
	invMass = 1.0f / mass;
	if ( inertiaScale != mat3_identity ) {
		inertiaTensor *= inertiaScale;
	}
	if ( inertiaTensor.IsDiagonal( 1e-3f ) ) {
		inertiaTensor[0][1] = inertiaTensor[0][2] = 0.0f;
		inertiaTensor[1][0] = inertiaTensor[1][2] = 0.0f;
		inertiaTensor[2][0] = inertiaTensor[2][1] = 0.0f;
		inverseInertiaTensor.Identity();
		inverseInertiaTensor[0][0] = 1.0f / inertiaTensor[0][0];
		inverseInertiaTensor[1][1] = 1.0f / inertiaTensor[1][1];
		inverseInertiaTensor[2][2] = 1.0f / inertiaTensor[2][2];
	}
	else {
		inverseInertiaTensor = inertiaTensor.Inverse();
	}
#ifdef MOD_WATERPHYSICS
	this->volume = mass / density;
	this->liquidMass = this->mass;
	this->invLiquidMass = this->invMass;
#endif
}

/*
================
idAFBody::SetFrictionDirection
================
*/
void idAFBody::SetFrictionDirection( const idVec3 &dir ) {
	frictionDir = dir * current->worldAxis.Transpose();
	fl.useFrictionDir = true;
}

/*
================
idAFBody::GetFrictionDirection
================
*/
bool idAFBody::GetFrictionDirection( idVec3 &dir ) const {
	if ( fl.useFrictionDir ) {
		dir = frictionDir * current->worldAxis;
		return true;
	}
	return false;
}

/*
================
idAFBody::SetContactMotorDirection
================
*/
void idAFBody::SetContactMotorDirection( const idVec3 &dir ) {
	contactMotorDir = dir * current->worldAxis.Transpose();
	fl.useContactMotorDir = true;
}

/*
================
idAFBody::GetContactMotorDirection
================
*/
bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const {
	if ( fl.useContactMotorDir ) {
		dir = contactMotorDir * current->worldAxis;
		return true;
	}
	return false;
}

/*
================
idAFBody::GetPointVelocity
================
*/
idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const {
	idVec3 r = point - current->worldOrigin;
	return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross( r );
}

/*
================
idAFBody::AddForce
================
*/
void idAFBody::AddForce( const idVec3 &point, const idVec3 &force ) {
	current->externalForce.SubVec3(0) += force;
	current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross( force );
}

/*
================
idAFBody::InverseWorldSpatialInertiaMultiply

  dst = this->inverseWorldSpatialInertia * v;
================
*/
ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const {
	const float *mPtr = inverseWorldSpatialInertia.ToFloatPtr();
	const float *vPtr = v;
	float *dstPtr = dst.ToFloatPtr();

	if ( fl.spatialInertiaSparse ) {
		dstPtr[0] = mPtr[0*6+0] * vPtr[0];
		dstPtr[1] = mPtr[1*6+1] * vPtr[1];
		dstPtr[2] = mPtr[2*6+2] * vPtr[2];
		dstPtr[3] = mPtr[3*6+3] * vPtr[3] + mPtr[3*6+4] * vPtr[4] + mPtr[3*6+5] * vPtr[5];
		dstPtr[4] = mPtr[4*6+3] * vPtr[3] + mPtr[4*6+4] * vPtr[4] + mPtr[4*6+5] * vPtr[5];
		dstPtr[5] = mPtr[5*6+3] * vPtr[3] + mPtr[5*6+4] * vPtr[4] + mPtr[5*6+5] * vPtr[5];
	} else {
		gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() );
	}
}

/*
================
idAFBody::Save
================
*/
void idAFBody::Save( idSaveGame *saveFile ) {
	saveFile->WriteFloat( linearFriction );
	saveFile->WriteFloat( angularFriction );
	saveFile->WriteFloat( contactFriction );
	saveFile->WriteFloat( bouncyness );
	saveFile->WriteInt( clipMask );
	saveFile->WriteVec3( frictionDir );
	saveFile->WriteVec3( contactMotorDir );
	saveFile->WriteFloat( contactMotorVelocity );
	saveFile->WriteFloat( contactMotorForce );

#ifdef MOD_WATERPHYSICS
	saveFile->WriteFloat( volume );
    saveFile->WriteFloat( liquidMass );
    saveFile->WriteFloat( invLiquidMass );
#endif
	saveFile->WriteFloat( mass );
	saveFile->WriteFloat( invMass );
	saveFile->WriteVec3( centerOfMass );
	saveFile->WriteMat3( inertiaTensor );
	saveFile->WriteMat3( inverseInertiaTensor );

	saveFile->WriteVec3( current->worldOrigin );
	saveFile->WriteMat3( current->worldAxis );
	saveFile->WriteVec6( current->spatialVelocity );
	saveFile->WriteVec6( current->externalForce );
	forceApplications.Save( saveFile );
	saveFile->WriteVec3( atRestOrigin );
	saveFile->WriteMat3( atRestAxis );
	m_RerouteEnt.Save( saveFile );
}

/*
================
idAFBody::Restore
================
*/
void idAFBody::Restore( idRestoreGame *saveFile ) {
	saveFile->ReadFloat( linearFriction );
	saveFile->ReadFloat( angularFriction );
	saveFile->ReadFloat( contactFriction );
	saveFile->ReadFloat( bouncyness );
	saveFile->ReadInt( clipMask );
	saveFile->ReadVec3( frictionDir );
	saveFile->ReadVec3( contactMotorDir );
	saveFile->ReadFloat( contactMotorVelocity );
	saveFile->ReadFloat( contactMotorForce );

#ifdef MOD_WATERPHYSICS
	saveFile->ReadFloat( volume );
	saveFile->ReadFloat( liquidMass );
	saveFile->ReadFloat( invLiquidMass );
#endif

	saveFile->ReadFloat( mass );
	saveFile->ReadFloat( invMass );
	saveFile->ReadVec3( centerOfMass );
	saveFile->ReadMat3( inertiaTensor );
	saveFile->ReadMat3( inverseInertiaTensor );

	saveFile->ReadVec3( current->worldOrigin );
	saveFile->ReadMat3( current->worldAxis );
	saveFile->ReadVec6( current->spatialVelocity );
	saveFile->ReadVec6( current->externalForce );
	forceApplications.Restore( saveFile );
	saveFile->ReadVec3( atRestOrigin );
	saveFile->ReadMat3( atRestAxis );
	m_RerouteEnt.Restore( saveFile );
}

idEntity *idAFBody::GetRerouteEnt( void )
{
	return m_RerouteEnt.GetEntity();
}

void idAFBody::SetRerouteEnt( idEntity *ent )
{
	m_RerouteEnt = ent;
}

#ifdef MOD_WATERPHYSICS
/*
================
idAFBody::GetWaterLevel
	returns the percent of the body in water (set by SetWaterLevel)
================
*/
float idAFBody::GetWaterLevel() const {
	return this->waterLevel;
}

/*
================
idAFBody::SetWaterLevel
	returns the percent of the body in water
	0.0f if out of water 
	
	Note we use the liquid's gravity normal for 
	floating because the idPhysics_AF gravity normal
	is really hard to get a hold of!
================
*/
float idAFBody::SetWaterLevel( idPhysics_Liquid *l, const idVec3 &gravityNormal, bool fixedDensityBuoyancy ) {
	if( l == NULL ) {
		this->waterLevel = 0.0f;
		this->m_fWaterMurkiness = 0.0f;
		return 0.0f;
	}

	if( !fixedDensityBuoyancy ) {
		const idBounds	&bounds = this->clipModel->GetBounds();
		idVec3 depth,point;
		float height, d;

		//
		// check if physics object is under water
		// and return the percentage of the object under water
		//
		point = this->GetWorldOrigin();

		depth = l->GetDepth(point);
	//	height = abs( (bounds[0] - bounds[1]) * gravityNormal ) * 0.5f;
	//	d = abs( depth * gravityNormal );
		height = fabs( bounds[0].z - bounds[1].z ) * 0.5f;
		d = depth.z;

		if( d < 0 )
			this->waterLevel = 0.0f;
		else if( d > height )
			this->waterLevel = 1.0f;
		else
			this->waterLevel = d / height;
	}
	else {
		idVec3 depth,bottom(this->current->worldOrigin);
		idBounds bounds = this->clipModel->GetBounds();
		float height,d;

		// offset and rotate the bounding box
		bounds += -centerOfMass;
		bounds *= this->current->worldAxis.Transpose();

		// gets the position of the object relative to the surface of the water
		height = fabs(bounds[1] * gravityNormal * 2);

		// calculates the depth of the bottom of the object
		bottom += (height * 0.5f) * gravityNormal;
		depth = l->GetDepth(bottom);
		d = fabs(depth * gravityNormal);

		if( d > height ) {
			// the body is totally submerged
			this->waterLevel = 1.0f;
		}
		else if( depth.x == -1 && depth.y == -1 && depth.z == -1 ) {
			this->waterLevel = 0.0f;
		}
		else {
			// the body is partly submerged
			this->waterLevel = d / height;
		}
	}
	return this->waterLevel;
}

#endif

//===============================================================
//                                                        M
//  idAFTree                                             MrE
//                                                        E
//===============================================================

/*
================
idAFTree::Factor

  factor matrix for the primary constraints in the tree
================
*/
void idAFTree::Factor( void ) const {
	int i, j;
	idAFBody *body;
	idAFConstraint *child(NULL);
	idMatX childI;

	childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) );

	// from the leaves up towards the root
	for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
		body = sortedBodies[i];

		if ( body->children.Num() ) {

			for ( j = 0; j < body->children.Num(); j++ ) {

				child = body->children[j]->primaryConstraint;

				// child->I = - child->body1->J.Transpose() * child->body1->I * child->body1->J;
				childI.SetSize( child->J1.GetNumRows(), child->J1.GetNumRows() );
				child->body1->J.TransposeMultiply( child->body1->I ).Multiply( childI, child->body1->J );
				childI.Negate();

				child->invI = childI;
				if ( !child->invI.InverseFastSelf() ) {
					gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'",
									child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() );
				}
				child->J = child->invI * child->J;

				body->I -= child->J.TransposeMultiply( childI ) * child->J;
			}

			body->invI = body->I;
			if ( !body->invI.InverseFastSelf() ) {
				gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s",
								child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() );
			}
			if ( body->primaryConstraint ) {
				body->J = body->invI * body->J;
			}
		}
		else if ( body->primaryConstraint ) {
			body->J = body->inverseWorldSpatialInertia * body->J;
		}
	}
}

/*
================
idAFTree::Solve

  solve for primary constraints in the tree
================
*/
void idAFTree::Solve( int auxiliaryIndex ) const {
	int i, j;
	idAFBody *body, *child;
	idAFConstraint *primaryConstraint;

	// from the leaves up towards the root
	for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
		body = sortedBodies[i];

		for ( j = 0; j < body->children.Num(); j++ ) {
			child = body->children[j];
			primaryConstraint = child->primaryConstraint;

			if ( !child->fl.isZero ) {
				child->J.TransposeMultiplySub( primaryConstraint->s, child->s );
				primaryConstraint->fl.isZero = false;
			}
			if ( !primaryConstraint->fl.isZero ) {
				primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s );
				body->fl.isZero = false;
			}
		}
	}

	bool useSymmetry = af_useSymmetry.GetBool();

	// from the root down towards the leaves
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];
		primaryConstraint = body->primaryConstraint;

		if ( primaryConstraint ) {

			if ( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
				continue;
			}

			if ( !primaryConstraint->fl.isZero ) {
				primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s;
			}
			primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s );

			primaryConstraint->lm = primaryConstraint->s;

			if ( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) {
				continue;
			}

			if ( body->children.Num() ) {
				if ( !body->fl.isZero ) {
					body->s = body->invI * body->s;
				}
				body->J.MultiplySub( body->s, primaryConstraint->s );
			}
		} else if ( body->children.Num() ) {
			body->s = body->invI * body->s;
		}
	}
}

/*
================
idAFTree::Response

  calculate body forces in the tree in response to a constraint force
================
*/
void idAFTree::Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const {
	int i, j;
	idAFBody *body;
	idAFConstraint *child, *primaryConstraint;
	idVecX v;

	// if a single body don't waste time because there aren't any primary constraints
	if ( sortedBodies.Num() == 1 ) {
		body = constraint->body1;
		if ( body->tree == this ) {
			body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
			body->responseIndex[body->numResponses++] = auxiliaryIndex;
		}
		else {
			body = constraint->body2;
			body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
			body->responseIndex[body->numResponses++] = auxiliaryIndex;
		}
		return;
	}

	v.SetData( 6, VECX_ALLOCA( 6 ) );

	// initialize right hand side to zero
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			primaryConstraint->s.Zero();
			primaryConstraint->fl.isZero = true;
		}
		body->s.Zero();
		body->fl.isZero = true;
		body->GetResponseForce( body->numResponses ).Zero();
	}

	// set right hand side for first constrained body
	body = constraint->body1;
	if ( body->tree == this ) {
		body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] );
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			primaryConstraint->J1.Multiply( primaryConstraint->s, v );
			primaryConstraint->fl.isZero = false;
		}
		for ( i = 0; i < body->children.Num(); i++ ) {
			child = body->children[i]->primaryConstraint;
			child->J2.Multiply( child->s, v );
			child->fl.isZero = false;
		}
		body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
	}

	// set right hand side for second constrained body
	body = constraint->body2;
	if ( body && body->tree == this ) {
		body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] );
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v );
			primaryConstraint->fl.isZero = false;
		}
		for ( i = 0; i < body->children.Num(); i++ ) {
			child = body->children[i]->primaryConstraint;
			child->J2.MultiplyAdd( child->s, v );
			child->fl.isZero = false;
		}
		body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
	}


	// solve for primary constraints
	Solve( auxiliaryIndex );

	bool useSymmetry = af_useSymmetry.GetBool();

	// store body forces in response to the constraint force
	idVecX force;
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];

		if ( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex ) {
			continue;
		}

		force.SetData( 6, body->response + body->numResponses * 8 );

		// add forces of all primary constraints acting on this body
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm );
		}
		for ( j = 0; j < body->children.Num(); j++ ) {
			child = body->children[j]->primaryConstraint;
			child->J2.TransposeMultiplyAdd( force, child->lm );
		}

		body->responseIndex[body->numResponses++] = auxiliaryIndex;
	}
}

/*
================
idAFTree::CalculateForces

  calculate forces on the bodies in the tree
================
*/
void idAFTree::CalculateForces( float timeStep ) const {
	int i, j;
	float invStep;
	idAFBody *body;
	idAFConstraint *child, *c, *primaryConstraint;

	// forces on bodies
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];

		body->totalForce.SubVec6(0) = body->current->externalForce + body->auxForce.SubVec6(0);
	}

	// if a single body don't waste time because there aren't any primary constraints
	if ( sortedBodies.Num() == 1 ) {
		return;
	}

	invStep = 1.0f / timeStep;

	// initialize right hand side
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];

		body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
		body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			// b = ( J * acc + c )
			c = primaryConstraint;
			c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * ( c->c1 + c->c2 );
			c->fl.isZero = false;
		}
		body->s.Zero();
		body->fl.isZero = true;
	}

	// solve for primary constraints
	Solve();

	// calculate forces on bodies after applying primary constraints
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];

		// add forces of all primary constraints acting on this body
		primaryConstraint = body->primaryConstraint;
		if ( primaryConstraint ) {
			primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm );
		}
		for ( j = 0; j < body->children.Num(); j++ ) {
			child = body->children[j]->primaryConstraint;
			child->J2.TransposeMultiplyAdd( body->totalForce, child->lm );
		}
	}
}

/*
================
idAFTree::SetMaxSubTreeAuxiliaryIndex
================
*/
void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) {
	int i, j;
	idAFBody *body, *child;

	// from the leaves up towards the root
	for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) {
		body = sortedBodies[i];

		body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex;
		for ( j = 0; j < body->children.Num(); j++ ) {
			child = body->children[j];
			if ( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex ) {
				body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex;
			}
		}
	}
}

/*
================
idAFTree::SortBodies_r
================
*/
void idAFTree::SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body ) {
	int i;

	for ( i = 0; i < body->children.Num(); i++ ) {
		sortedList.Append( body->children[i] );
	}
	for ( i = 0; i < body->children.Num(); i++ ) {
		SortBodies_r( sortedList, body->children[i] );
	}
}

/*
================
idAFTree::SortBodies

  sort body list to make sure parents come first
================
*/
void idAFTree::SortBodies( void ) {
	int i;
	idAFBody *body;

	// find the root
	for ( i = 0; i < sortedBodies.Num(); i++ ) {
		if ( !sortedBodies[i]->parent ) {
			break;
		}
	}

	if ( i >= sortedBodies.Num() ) {
		gameLocal.Error( "Articulated figure tree has no root." );
	}

	body = sortedBodies[i];
	sortedBodies.Clear();
	sortedBodies.Append( body );
	SortBodies_r( sortedBodies, body );
}

/*
================
idAFTree::DebugDraw
================
*/
void idAFTree::DebugDraw( const idVec4 &color ) const {
	int i;
	idAFBody *body;

	for ( i = 1; i < sortedBodies.Num(); i++ ) {
		body = sortedBodies[i];
		gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 );
	}
}


//===============================================================
//                                                        M
//  idPhysics_AF                                         MrE
//                                                        E
//===============================================================

/*
================
idPhysics_AF::EvaluateConstraints
================
*/
void idPhysics_AF::EvaluateConstraints( float timeStep ) {
	int i;
	float invTimeStep;
	idAFBody *body;
	idAFConstraint *c;

	invTimeStep = 1.0f / timeStep;

	// setup the constraint equations for the current position and orientation of the bodies
	for ( i = 0; i < primaryConstraints.Num(); i++ ) {
		c = primaryConstraints[i];
		c->Evaluate( invTimeStep );
		c->J = c->J2;
	}
	for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		auxiliaryConstraints[i]->Evaluate( invTimeStep );
	}

	// add contact constraints to the list with frame constraints
	for ( i = 0; i < contactConstraints.Num(); i++ ) {
		AddFrameConstraint( contactConstraints[i] );
	}

	// setup body primary constraint matrix
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->primaryConstraint ) {
			body->J = body->primaryConstraint->J1.Transpose();
		}
	}
}

/*
================
idPhysics_AF::EvaluateBodies
================
*/
void idPhysics_AF::EvaluateBodies( float timeStep ) {
	int i;
	idAFBody *body;
#ifdef MOD_WATERPHYSICS
	float bMass, invbMass;
#endif
	idMat3 axis;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
#ifdef MOD_WATERPHYSICS
		if( this->water != NULL && body->GetWaterLevel() > 0.0f ) {
			bMass = body->liquidMass;
			invbMass = body->invLiquidMass;
		}
		else {
			bMass = body->mass;
			invbMass = body->invMass;
		}
#endif
		// we transpose the axis before using it because idMat3 is column-major
		axis = body->current->worldAxis.Transpose();

		// if the center of mass is at the body point of reference
		if ( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) {

			// spatial inertia in world space
#ifdef MOD_WATERPHYSICS
			body->I.Set( bMass * mat3_identity, mat3_zero,
							mat3_zero, axis * body->inertiaTensor * axis.Transpose() );
			body->inverseWorldSpatialInertia.Set( invbMass * mat3_identity, mat3_zero,
#else
			body->I.Set( body->mass * mat3_identity, mat3_zero,
							mat3_zero, axis * body->inertiaTensor * axis.Transpose() );
			// inverse spatial inertia in world space
			body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero,
#endif
											mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() );

			body->fl.spatialInertiaSparse = true;
		}
		else {
#ifdef MOD_WATERPHYSICS
			idMat3 massMoment =bMass * SkewSymmetric( body->centerOfMass );
			body->I.Set( bMass * mat3_identity, massMoment,
#else
			idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass );
			// spatial inertia in world space
			body->I.Set( body->mass * mat3_identity, massMoment,
#endif
								massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() );

			// inverse spatial inertia in world space
			body->inverseWorldSpatialInertia = body->I.InverseFast();

			body->fl.spatialInertiaSparse = false;
		}

		// initialize auxiliary constraint force to zero
		body->auxForce.Zero();
	}
}

/*
================
idPhysics_AF::AddFrameConstraints
================
*/
void idPhysics_AF::AddFrameConstraints( void ) {
	int i;

	// add frame constraints to auxiliary constraints
	for ( i = 0; i < frameConstraints.Num(); i++ ) {
		auxiliaryConstraints.Append( frameConstraints[i] );
	}
}

/*
================
idPhysics_AF::RemoveFrameConstraints
================
*/
void idPhysics_AF::RemoveFrameConstraints( void ) {
	// remove all the frame constraints from the auxiliary constraints
	auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num(), false );
	frameConstraints.SetNum( 0, false );
}

/*
================
idPhysics_AF::ApplyFriction
================
*/
void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) {
	int i;
	float invTimeStep;

	if ( af_skipFriction.GetBool() ) {
		return;
	}

	if ( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
		float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f;
		if ( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
			jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime;
		} else {
			jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime;
		}
	} else {
		jointFrictionDentScale = 0.0f;
	}

	if ( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) ) {
		float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f;
		if ( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) {
			contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime;
		} else {
			contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime;
		}
	} else {
		contactFrictionDentScale = 0.0f;
	}

	invTimeStep = 1.0f / timeStep;

	for ( i = 0; i < primaryConstraints.Num(); i++ ) {
		primaryConstraints[i]->ApplyFriction( invTimeStep );
	}
	for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		auxiliaryConstraints[i]->ApplyFriction( invTimeStep );
	}
	for ( i = 0; i < frameConstraints.Num(); i++ ) {
		frameConstraints[i]->ApplyFriction( invTimeStep );
	}
}

/*
================
idPhysics_AF::PrimaryFactor
================
*/
void idPhysics_AF::PrimaryFactor( void ) {
	int i;

	for ( i = 0; i < trees.Num(); i++ ) {
		trees[i]->Factor();
	}
}

/*
================
idPhysics_AF::PrimaryForces
================
*/
void idPhysics_AF::PrimaryForces( float timeStep ) {
	int i;

	for ( i = 0; i < trees.Num(); i++ ) {
		trees[i]->CalculateForces( timeStep );
	}
}

/*
================
idPhysics_AF::AuxiliaryForces
================
*/
void idPhysics_AF::AuxiliaryForces( float timeStep ) {
	int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex;
	float *ptr, *j1, *j2, *dstPtr, *forcePtr;
	float invStep, u;
	idAFBody *body;
	idAFConstraint *constraint;
	idVecX tmp;
	idMatX jmk;
	idVecX rhs, w, lm, lo, hi;

	// get the number of one dimensional auxiliary constraints
	for ( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows();
	}

	if ( numAuxConstraints == 0 ) {
		return;
	}

	// allocate memory to store the body response to auxiliary constraint forces
	forcePtr = (float *) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) );
	index = (int *) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) );
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		body->response = forcePtr;
		body->responseIndex = index;
		body->numResponses = 0;
		body->maxAuxiliaryIndex = 0;
		forcePtr += numAuxConstraints * 8;
		index += numAuxConstraints;
	}

	// set on each body the largest index of an auxiliary constraint constraining the body
	if ( af_useSymmetry.GetBool() ) {
		for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
			constraint = auxiliaryConstraints[i];
			for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
				if ( k > constraint->body1->maxAuxiliaryIndex ) {
					constraint->body1->maxAuxiliaryIndex = k;
				}
				if ( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex ) {
					constraint->body2->maxAuxiliaryIndex = k;
				}
			}
		}
		for ( i = 0; i < trees.Num(); i++ ) {
			trees[i]->SetMaxSubTreeAuxiliaryIndex();
		}
	}

	// calculate forces of primary constraints in response to the auxiliary constraint forces
	for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		constraint = auxiliaryConstraints[i];

		for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {

			// calculate body forces in the tree in response to the constraint force
			constraint->body1->tree->Response( constraint, j, k );
			// if there is a second body which is part of a different tree
			if ( constraint->body2 && constraint->body2->tree != constraint->body1->tree ) {
				// calculate body forces in the second tree in response to the constraint force
				constraint->body2->tree->Response( constraint, j, k );
			}
		}
	}

	// NOTE: the rows are 16 byte padded
	jmk.SetData( numAuxConstraints, ((numAuxConstraints+3)&~3), MATX_ALLOCA( numAuxConstraints * ((numAuxConstraints+3)&~3) ) );
	tmp.SetData( 6, VECX_ALLOCA( 6 ) );

	// create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints
	for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		constraint = auxiliaryConstraints[i];

		for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
			constraint->body1->InverseWorldSpatialInertiaMultiply( tmp, constraint->J1[j] );
			j1 = tmp.ToFloatPtr();
			ptr = constraint->body1->response;
			index = constraint->body1->responseIndex;
			dstPtr = jmk[k];
			s = af_useSymmetry.GetBool() ? k + 1 : numAuxConstraints;
			for ( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] ) {
				while( l < m ) {
					dstPtr[l++] = 0.0f;
				}
				dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] +
								j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
				ptr += 8;
			}

			while( l < s ) {
				dstPtr[l++] = 0.0f;
			}

			if ( constraint->body2 ) {
				constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] );
				j2 = tmp.ToFloatPtr();
				ptr = constraint->body2->response;
				index = constraint->body2->responseIndex;
				for ( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] ) {
					dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] +
										j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
					ptr += 8;
				}
			}
		}
	}

	if ( af_useSymmetry.GetBool() ) {
		n = jmk.GetNumColumns();
		for ( i = 0; i < numAuxConstraints; i++ ) {
			ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i;
			dstPtr = jmk.ToFloatPtr() + i * n + i + 1;
			for ( j = i+1; j < numAuxConstraints; j++ ) {
				*dstPtr++ = *ptr;
				ptr += n;
			}
		}
	}

	invStep = 1.0f / timeStep;

	// calculate body acceleration
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
		body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep;
	}

	rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
	lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
	hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
	lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
	boxIndex = (int *) _alloca16( numAuxConstraints * sizeof( int ) );

	// set first index for special box constrained variables
	for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		auxiliaryConstraints[i]->firstIndex = k;
		k += auxiliaryConstraints[i]->J1.GetNumRows();
	}

	// initialize right hand side and low and high bounds for auxiliary constraints
	for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		constraint = auxiliaryConstraints[i];
		n = k;

		for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {

			j1 = constraint->J1[j];
			ptr = constraint->body1->acceleration.ToFloatPtr();
			rhs[k] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
			rhs[k] += constraint->c1[j] * invStep;

			if ( constraint->body2 ) {
				j2 = constraint->J2[j];
				ptr = constraint->body2->acceleration.ToFloatPtr();
				rhs[k] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
				rhs[k] += constraint->c2[j] * invStep;
			}

			rhs[k] = -rhs[k];
			lo[k] = constraint->lo[j];
			hi[k] = constraint->hi[j];

			if ( constraint->boxIndex[j] >= 0 ) {
				if ( constraint->boxConstraint->fl.isPrimary ) {
					gameLocal.Error( "cannot reference primary constraints for the box index" );
				}
				boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j];
			}
			else {
				boxIndex[k] = -1;
			}
			jmk[k][k] += constraint->e[j] * invStep;
		}
	}

#ifdef AF_TIMINGS
	timer_lcp.Start();
#endif

	// calculate lagrange multipliers for auxiliary constraints
	if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) {
		return;		// bad monkey!
	}

#ifdef AF_TIMINGS
	timer_lcp.Stop();
#endif

	// calculate auxiliary constraint forces
	for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		constraint = auxiliaryConstraints[i];

		for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) {
			constraint->lm[j] = u = lm[k];

			j1 = constraint->J1[j];
			ptr = constraint->body1->auxForce.ToFloatPtr();
			ptr[0] += j1[0] * u; ptr[1] += j1[1] * u; ptr[2] += j1[2] * u;
			ptr[3] += j1[3] * u; ptr[4] += j1[4] * u; ptr[5] += j1[5] * u;

			if ( constraint->body2 ) {
				j2 = constraint->J2[j];
				ptr = constraint->body2->auxForce.ToFloatPtr();
				ptr[0] += j2[0] * u; ptr[1] += j2[1] * u; ptr[2] += j2[2] * u;
				ptr[3] += j2[3] * u; ptr[4] += j2[4] * u; ptr[5] += j2[5] * u;
			}
		}
	}

	// recalculate primary constraint forces in response to auxiliary constraint forces
	PrimaryForces( timeStep );

	// clear pointers pointing to stack space so tools don't get confused
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		body->response = NULL;
		body->responseIndex = NULL;
	}
}

/*
================
idPhysics_AF::VerifyContactConstraints
================
*/
void idPhysics_AF::VerifyContactConstraints( void ) {
#if 0
	int i;
	float impulseNumerator, impulseDenominator;
	idVec3 r, velocity, normalVelocity, normal, impulse;
	idAFBody *body;

	for ( i = 0; i < contactConstraints.Num(); i++ ) {
		body = contactConstraints[i]->body1;
		const contactInfo_t &contact = contactConstraints[i]->GetContact();

		r = contact.point - body->GetCenterOfMass();

		// calculate velocity at contact point
		velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r );

		// velocity along normal vector
		normalVelocity = ( velocity * contact.normal ) * contact.normal;

		// if moving towards the surface at the contact point
		if ( normalVelocity * contact.normal < 0.0f ) {
			// calculate impulse
			normal = -normalVelocity;
			impulseNumerator = normal.Normalize();
			impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
			impulse = (impulseNumerator / impulseDenominator) * normal * 1.0001f;

			// apply impulse
			body->SetLinearVelocity( body->GetLinearVelocity() + impulse );
			body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) );
		}
	}
#else
	int i;
	idAFBody *body;
	idVec3 normal;

	for ( i = 0; i < contactConstraints.Num(); i++ ) {
		body = contactConstraints[i]->body1;
		normal = contactConstraints[i]->GetContact().normal;
		if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
			body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
		}
		body = contactConstraints[i]->body2;
		if ( !body ) {
			continue;
		}
		normal = -normal;
		if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) {
			body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal;
		}
	}
#endif
}

/*
================
idPhysics_AF::Evolve
================
*/
void idPhysics_AF::Evolve( float timeStep ) {
	int i;
	float angle;
#ifdef MOD_WATERPHYSICS
	float waterLevel;
#endif
	idVec3 vec;
	idAFBody *body;
	idRotation rotation;
	float vSqr, maxLinearVelocity, maxAngularVelocity;
	float frictionTickMul;
	
	maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep;
	maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		// calculate the spatial velocity for the next physics state
		body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
		body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6(0);

		if ( maxLinearVelocity > 0.0f ) {
			// cap the linear velocity
			vSqr = body->next->spatialVelocity.SubVec3(0).LengthSqr();
			if ( vSqr > Square( maxLinearVelocity ) ) {
				body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity;
			}
		}

		if ( maxAngularVelocity > 0.0f ) {
			// cap the angular velocity
			vSqr = body->next->spatialVelocity.SubVec3(1).LengthSqr();
			if ( vSqr > Square( maxAngularVelocity ) ) {
				body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity;
			}
		}
	}

	// make absolutely sure all contact constraints are satisfied
	VerifyContactConstraints();

	// make friction independent of the frametime (i.e. the time between two calls of this function)
	frictionTickMul = timeStep / MS2SEC( USERCMD_MSEC );

	// calculate the position of the bodies for the next physics state
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		// translate world origin
		body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 );

		// convert angular velocity to a rotation matrix
		vec = body->next->spatialVelocity.SubVec3( 1 );
		angle = -timeStep * (float) RAD2DEG( vec.Normalize() );
		rotation = idRotation( vec3_origin, vec, angle );
		rotation.Normalize180();

		// rotate world axis
		body->next->worldAxis = body->current->worldAxis * rotation.ToMat3();
		body->next->worldAxis.OrthoNormalizeSelf();

		// linear and angular friction
#ifdef MOD_WATERPHYSICS
		// apply a higher friction value if the AF is underwater
		waterLevel = body->GetWaterLevel();
		if( waterLevel == 0.0f || this->water == NULL ) {
			body->next->spatialVelocity.SubVec3(0) -= frictionTickMul * body->linearFriction * body->next->spatialVelocity.SubVec3(0);
		}
		else {
			body->next->spatialVelocity.SubVec3(0) -= frictionTickMul * (body->linearFriction * (this->water->GetViscosity() + WATER_FRICTION) * waterLevel) * body->next->spatialVelocity.SubVec3(0);
		}
#else
		body->next->spatialVelocity.SubVec3(0) -= frictionTickMul * body->linearFriction * body->next->spatialVelocity.SubVec3(0);
#endif	
		body->next->spatialVelocity.SubVec3(1) -= frictionTickMul * body->angularFriction * body->next->spatialVelocity.SubVec3(1);
	}
}

/*
================
idPhysics_AF::CollisionImpulse

  apply impulse to the colliding bodies
  the current state of the body should be set to the moment of impact
  this is silly as it doesn't take the AF structure into account
================
*/
bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ) {
	idVec3 r, velocity, impulse;
	idMat3 inverseWorldInertiaTensor;
	float impulseNumerator, impulseDenominator;
#ifdef MOD_WATERPHYSICS
	float invMass;
#endif
	impactInfo_t info;
	idEntity *ent;

	ent = gameLocal.entities[collision.c.entityNum];
	if ( ent == self ) {
		return false;
	}

#ifdef MOD_WATERPHYSICS
	if( this->water != NULL ) {
		invMass = body->invLiquidMass;
	} else {
		invMass = body->invMass;
	}
#endif

	// get info from other entity involved
	ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );

	// Update moved by and set in motion by actor
	if ( self->m_SetInMotionByActor.GetEntity() && !ent->m_droppedByAI ) // grayman #3075 - don't pass these settings along if dropped by an AI
	{
		ent->m_SetInMotionByActor = self->m_SetInMotionByActor.GetEntity();
		ent->m_MovedByActor = self->m_MovedByActor.GetEntity();
	}

	// Note: Actors should not overwrite the moved by other actors when they are hit with something
	// So only overwrite if MovedByActor is NULL
	if ( ent->IsType(idActor::Type) 
		&& self->m_SetInMotionByActor.GetEntity() == NULL
		&& !(static_cast<idActor *>(ent)->IsKnockedOut() || ent->health < 0) )
	{
		self->m_SetInMotionByActor = (idActor *) ent;
		self->m_MovedByActor = (idActor *) ent;
	}

	if ( self->IsType(idActor::Type) 
		&& ent->m_SetInMotionByActor.GetEntity() == NULL
		&& !(static_cast<idActor *>(self)->IsKnockedOut() || self->health < 0) )
	{
		ent->m_SetInMotionByActor = (idActor *) self;
		ent->m_MovedByActor = (idActor *) self;
	}

	// collision point relative to the body center of mass
	r = collision.c.point - (body->current->worldOrigin + body->centerOfMass * body->current->worldAxis);
	// the velocity at the collision point
	velocity = body->current->spatialVelocity.SubVec3(0) + body->current->spatialVelocity.SubVec3(1).Cross(r);
	// subtract velocity of other entity
	velocity -= info.velocity;
	// never stick
	if ( velocity * collision.c.normal > 0.0f )
	{
		velocity = collision.c.normal;
	}
	inverseWorldInertiaTensor = body->current->worldAxis.Transpose() * body->inverseInertiaTensor * body->current->worldAxis;
	impulseNumerator = -( 1.0f + body->bouncyness ) * ( velocity * collision.c.normal );
#ifdef MOD_WATERPHYSICS
	impulseDenominator = invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
#else
	impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
#endif
	if ( info.invMass )
	{
		impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal );
	}
	impulse = (impulseNumerator / impulseDenominator) * collision.c.normal;

	// apply impact to other entity
	ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );

	// callback to self to let the entity know about the impact
	return self->Collide( collision, velocity );
}

/*
================
idPhysics_AF::ApplyCollisions
================
*/
bool idPhysics_AF::ApplyCollisions( float timeStep ) {
	int i;

	for ( i = 0; i < collisions.Num(); i++ ) {
		if ( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) ) {
			return true;
		}
	}
	return false;
}

/*
================
idPhysics_AF::SetupCollisionForBody
================
*/
idEntity *idPhysics_AF::SetupCollisionForBody( idAFBody *body ) const {
	int i;
	idAFBody *b;
	idEntity *passEntity;

	passEntity = NULL;

	if ( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() ) {

		// disable all bodies
		for ( i = 0; i < bodies.Num(); i++ ) {
			bodies[i]->clipModel->Disable();
		}

		// don't collide with world collision model if attached to the world
		for ( i = 0; i < body->constraints.Num(); i++ ) {
			if ( !body->constraints[i]->fl.noCollision ) {
				continue;
			}
			// if this constraint attaches the body to the world
			if ( body->constraints[i]->body2 == NULL ) {
				// don't collide with the world collision model
				passEntity = gameLocal.world;
			}
		}

	} else {

		// enable all bodies that have self collision
		for ( i = 0; i < bodies.Num(); i++ ) {
			if ( bodies[i]->fl.selfCollision ) {
				bodies[i]->clipModel->Enable();
			} else {
				bodies[i]->clipModel->Disable();
			}
		}

		// don't let the body collide with itself
		body->clipModel->Disable();

		// disable any bodies attached with constraints
		for ( i = 0; i < body->constraints.Num(); i++ ) {
			if ( !body->constraints[i]->fl.noCollision ) {
				continue;
			}
			// if this constraint attaches the body to the world
			if ( body->constraints[i]->body2 == NULL ) {
				// don't collide with the world collision model
				passEntity = gameLocal.world;
			} else {
				if ( body->constraints[i]->body1 == body ) {
					b = body->constraints[i]->body2;
				} else if ( body->constraints[i]->body2 == body ) {
					b = body->constraints[i]->body1;
				} else {
					continue;
				}
				// don't collide with this body
				b->clipModel->Disable();
			}
		}
	}

	return passEntity;
}

/*
================
idPhysics_AF::CheckForCollisions

  check for collisions between the current and next state
  if there is a collision the next state is set to the state at the moment of impact
  assumes all bodies are linked for collision detection and relinks all bodies after moving them
================
*/
void idPhysics_AF::CheckForCollisions( float timeStep ) {
//	#define TEST_COLLISION_DETECTION
	int i, index;
	idAFBody *body;
	idMat3 axis;
	idRotation rotation;
	trace_t collision;
	idEntity *passEntity;

	// clear list with collisions
	collisions.SetNum( 0, false );

	if ( !enableCollision ) {
		return;
	}

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->clipMask != 0 ) {

			passEntity = SetupCollisionForBody( body );

#ifdef TEST_COLLISION_DETECTION
			bool startsolid = false;
			if ( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel,
															body->current->worldAxis, body->clipMask, passEntity ) ) {
				startsolid = true;
			}
#endif

			TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis );
			rotation = axis.ToRotation();
			rotation.SetOrigin( body->current->worldOrigin );

			// if there was a collision
			if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation,
										body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) {

				// set the next state to the state at the moment of impact
				body->next->worldOrigin = collision.endpos;
				body->next->worldAxis = collision.endAxis;

				// add collision to the list
				index = collisions.Num();
				collisions.SetNum( index + 1, false );
				collisions[index].trace = collision;
				collisions[index].body = body;
			}

#ifdef MOD_WATERPHYSICS
			// Check for water collision
			// ideally we could do this check in one step but if a body moves quickly in shallow water
			// they will occasionally clip through a solid entity (ie. fall through the floor)
			if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation,
										body->clipModel, body->current->worldAxis, MASK_WATER, passEntity ) ) {
				idEntity *ent = gameLocal.entities[collision.c.entityNum];

				// if the object collides with something with a physics_liquid
				if( ent->GetPhysics()->IsType( idPhysics_Liquid::Type ) ) {
					idPhysics_Liquid *liquid = static_cast<idPhysics_Liquid *>(ent->GetPhysics());
					impactInfo_t info;

					this->self->GetImpactInfo(ent,collision.c.id,collision.c.point,&info);

					this->SetWater(liquid, ent->spawnArgs.GetFloat("murkiness", "0") );
					this->water->Splash(this->self,body->GetVolume(),info,collision);
				}
			}
#endif

		
#ifdef TEST_COLLISION_DETECTION
			if ( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel,
														body->next->worldAxis, body->clipMask, passEntity ) ) {
				if ( !startsolid ) {
					int bah = 1;
				}
			}
#endif
		}

		body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis );
	}
}

/*
================
idPhysics_AF::EvaluateContacts
================
*/
bool idPhysics_AF::EvaluateContacts( void ) {
	int i, j, k, numContacts, numBodyContacts;
	idAFBody *body;
	idEntity *passEntity;
	idVecX dir( 6, VECX_ALLOCA( 6 ) );
	contactInfo_t contactInfo[CONTACTS_MAX_NUMBER];

	// evaluate bodies
	EvaluateBodies( current.lastTimeStep );

	// remove all existing contacts
	ClearContacts();

	contactBodies.SetNum( 0, false );

	if ( !enableCollision ) {
		return false;
	}

	// find all the contacts
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->clipMask == 0 ) {
			continue;
		}

		passEntity = SetupCollisionForBody( body );

		body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() );
		dir.SubVec6(0) = body->current->spatialVelocity + current.lastTimeStep * dir.SubVec6(0);
		dir.SubVec3(0).Normalize();
		dir.SubVec3(1).Normalize();

		numContacts = gameLocal.clip.Contacts(
			contactInfo, CONTACTS_MAX_NUMBER, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON,
			body->clipModel, body->current->worldAxis, body->clipMask, passEntity
		);

#if 1
		// merge nearby contacts between the same bodies
		// and assure there are at most three planar contacts between any pair of bodies
		for ( j = 0; j < numContacts; j++ ) {

			numBodyContacts = 0;
			for ( k = 0; k < contacts.Num(); k++ ) {
				if ( contacts[k].entityNum == contactInfo[j].entityNum ) {
					if ( ( contacts[k].id == i && contactInfo[j].id == contactBodies[k] ) ||
							( contactBodies[k] == i && contacts[k].id == contactInfo[j].id ) ) {

						if ( ( contacts[k].point - contactInfo[j].point ).LengthSqr() < Square( 2.0f ) ) {
							break;
						}
						if ( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f ) {
							numBodyContacts++;
						}
					}
				}
			}

			if ( k >= contacts.Num() && numBodyContacts < 3 ) {
				contacts.Append( contactInfo[j] );
				contactBodies.Append( i );
			}
		}

#else

		for ( j = 0; j < numContacts; j++ ) {
			contacts.Append( contactInfo[j] );
			contactBodies.Append( i );
		}
#endif

	}

	AddContactEntitiesForContacts();

	return ( contacts.Num() != 0 );
}

/*
================
idPhysics_AF::SetupContactConstraints
================
*/
void idPhysics_AF::SetupContactConstraints( void ) {
	int i;

	// make sure enough contact constraints are allocated
	contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement<idAFConstraint_Contact> );
	contactConstraints.SetNum( contacts.Num(), false );

	// setup contact constraints
	for ( i = 0; i < contacts.Num(); i++ ) {
		// add contact constraint
		contactConstraints[i]->physics = this;
		if ( contacts[i].entityNum == self->entityNumber ) {
			contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] );
		}
		else {
			contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] );
		}
	}
}

/*
================
idPhysics_AF::ApplyContactForces
================
*/
void idPhysics_AF::ApplyContactForces( void ) {
#if 0
	int i;
	idEntity *ent;
	idVec3 force;

	for ( i = 0; i < contactConstraints.Num(); i++ ) {
		if ( contactConstraints[i]->body2 != NULL ) {
			continue;
		}
		const contactInfo_t &contact = contactConstraints[i]->GetContact();
		ent = gameLocal.entities[contact.entityNum];
		if ( !ent ) {
			continue;
		}
		force.Zero();
		ent->AddForce( self, contact.id, contact.point, force );
	}
#endif
}

/*
================
idPhysics_AF::AddGravity
================
*/
void idPhysics_AF::AddGravity( void ) {
	int i;
	idAFBody *body;
#ifdef MOD_WATERPHYSICS
	idVec3 grav( this->liquidDensity * this->gravityVector );
	float waterLevel,wDensity = 0;
	bool inWater,bodyBuoyancy = 0;

	if( this->SetWaterLevelf() == 1.0f ) {
		wDensity = this->water->GetDensity();
		bodyBuoyancy = af_useBodyDensityBuoyancy.GetBool();
	}

	inWater = false;
#endif

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
#ifdef MOD_WATERPHYSICS
		// add gravitational force
		waterLevel = body->SetWaterLevel(this->water,this->gravityNormal,this->fixedDensityBuoyancy);
		if( waterLevel > 0.0f ) {
			if( !this->fixedDensityBuoyancy && !bodyBuoyancy )
			{
				body->liquidMass = body->mass;
				body->invLiquidMass = body->invMass;
			}
			else {
				body->liquidMass = body->volume * this->liquidDensity * LIQUID_MASS_MUL;
				body->invLiquidMass = 1 / body->liquidMass;
			}

			// we float the body in water	
			if( bodyBuoyancy ) 
				body->current->externalForce.SubVec3( 0 ) += (body->mass - (body->volume * wDensity * waterLevel)) * gravityVector;
			else if( this->fixedDensityBuoyancy )
				body->current->externalForce.SubVec3( 0 ) += body->volume * ( this->liquidDensity - (wDensity * waterLevel) ) * gravityVector;
			else
				body->current->externalForce.SubVec3( 0 ) += body->mass * grav * waterLevel;

			inWater = true;
		}
		else {
			body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
		}
#else
		// add gravitational force
		body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
#endif
	}

#ifdef MOD_WATERPHYSICS
// if all AFBodies are not in the water, we assume the whole entity is not in water so
// we clear the water flag
	if( !inWater )
	{
		this->water = NULL;
		this->m_fWaterMurkiness = 0.0f;
	}
#endif
}

/*
================
idPhysics_AF::SwapStates
================
*/
void idPhysics_AF::SwapStates( void ) {
	for ( int i = 0; i < bodies.Num(); i++ ) {
		idAFBody *body = bodies[i];
		// swap the current and next state for next simulation step
		idSwap(body->current, body->next);
	}
}

/*
================
idPhysics_AF::UpdateClipModels
================
*/
void idPhysics_AF::UpdateClipModels( void ) {
	int i;
	idAFBody *body;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis );
	}
}

/*
================
idPhysics_AF::SetSuspendSpeed
================
*/
void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ) {
	this->suspendVelocity = velocity;
	this->suspendAcceleration = acceleration;
}

/*
================
idPhysics_AF::SetSuspendTime
================
*/
void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) {
	this->minMoveTime = minTime;
	this->maxMoveTime = maxTime;
}

/*
================
idPhysics_AF::SetSuspendTolerance
================
*/
void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation ) {
	this->noMoveTime = noMoveTime;
	this->noMoveTranslation = noMoveTranslation;
	this->noMoveRotation = noMoveRotation;
}

/*
================
idPhysics_AF::SetTimeScaleRamp
================
*/
void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) {
	timeScaleRampStart = start;
	timeScaleRampEnd = end;
}

/*
================
idPhysics_AF::SetJointFrictionDent
================
*/
void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end ) {
	jointFrictionDent = dent;
	jointFrictionDentStart = start;
	jointFrictionDentEnd = end;
}

/*
================
idPhysics_AF::GetJointFrictionScale
================
*/
float idPhysics_AF::GetJointFrictionScale( void ) const {
	if ( jointFrictionDentScale > 0.0f ) {
		return jointFrictionDentScale;
	} else if ( jointFrictionScale > 0.0f ) {
		return jointFrictionScale;
	} else if ( af_jointFrictionScale.GetFloat() > 0.0f ) {
		return af_jointFrictionScale.GetFloat();
	}
	return 1.0f;
}

/*
================
idPhysics_AF::SetContactFrictionDent
================
*/
void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end ) {
	contactFrictionDent = dent;
	contactFrictionDentStart = start;
	contactFrictionDentEnd = end;
}

/*
================
idPhysics_AF::GetContactFrictionScale
================
*/
float idPhysics_AF::GetContactFrictionScale( void ) const {
	if ( contactFrictionDentScale > 0.0f ) {
		return contactFrictionDentScale;
	} else if ( contactFrictionScale > 0.0f ) {
		return contactFrictionScale;
	} else if ( af_contactFrictionScale.GetFloat() > 0.0f ) {
		return af_contactFrictionScale.GetFloat();
	}
	return 1.0f;
}

/*
================
idPhysics_AF::TestIfAtRest
================
*/
bool idPhysics_AF::TestIfAtRest( float timeStep ) {
	int i;
	float translationSqr, maxTranslationSqr, rotation, maxRotation;
	idAFBody *body;

	if ( current.atRest >= 0 ) {
		return true;
	}

#ifdef MOD_WATERPHYSICS
// prevent bodies from going in active after floating.  You don't really want bodies to
// go inactive if they're in water (sometimes they just have a long way to go before surfacing)
	if( this->water != NULL ) current.activateTime = 0.0f;
#endif

	current.activateTime += timeStep;

	// if the simulation should never be suspended before a certaint amount of time passed
	if ( minMoveTime > 0.0f && current.activateTime < minMoveTime ) {
		return false;
	}

	// if the simulation should always be suspended after a certain amount time passed
	if ( maxMoveTime > 0.0f && current.activateTime > maxMoveTime ) {
		return true;
	}

	// test if all bodies hardly moved over a period of time
	if ( current.noMoveTime == 0.0f ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
			body->atRestOrigin = body->current->worldOrigin;
			body->atRestAxis = body->current->worldAxis;
		}
		current.noMoveTime += timeStep;
	}
	else if ( current.noMoveTime > noMoveTime ) {
		current.noMoveTime = 0.0f;
		maxTranslationSqr = 0.0f;
		maxRotation = 0.0f;
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];

			translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr();
			if ( translationSqr > maxTranslationSqr ) {
				maxTranslationSqr = translationSqr;
			}
			rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle();
			if ( rotation > maxRotation ) {
				maxRotation = rotation;
			}
		}

		if ( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation ) {
			// hardly moved over a period of time so the articulated figure may come to rest
			return true;
		}
	} else {
		current.noMoveTime += timeStep;
	}

	// test if the velocity or acceleration of any body is still too large to come to rest
#ifdef MOD_WATERPHYSICS
	if( this->water == NULL ) {
#endif
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) {
			return false;
		}
		if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) {
			return false;
		}
		if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) {
			return false;
		}
		if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) {
			return false;
		}
	}
#ifdef MOD_WATERPHYSICS
	} else {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];

			if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) {
				return false;
			}
			if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) {
				return false;
			}
			if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) {
				return false;
			}
			if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) {
				return false;
			}
		}
	}
#endif

	// all bodies have a velocity and acceleration small enough to come to rest
	return true;
}

/*
================
idPhysics_AF::Rest
================
*/
void idPhysics_AF::Rest( void ) {
	int i;

	current.atRest = gameLocal.time;
	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->current->spatialVelocity.Zero();
		bodies[i]->forceApplications.Clear();
		bodies[i]->current->externalForce.Zero();
	}

	// grayman #4609 - AF has come to rest. If this is at mission start,
	// set nextSoundTime so it becomes okay for him to
	// make suspicious propagated sounds from this time forward.

	idAFEntity_Base *af = static_cast<idAFEntity_Base *>(self);
	if (( gameLocal.time > 0) && (af->nextSoundTime == NO_PROP_SOUND ))
	{
		af->nextSoundTime = gameLocal.time;
	}

	self->BecomeInactive( TH_PHYSICS );
	self->m_SetInMotionByActor = NULL;
	//self->m_droppedByAI = false; // grayman #1330 // grayman #3075 - keep the designation for use while resting
	
	// grayman #3075 - set m_MovedByActor to NULL here
	self->m_MovedByActor = NULL;
}

/*
================
idPhysics_AF::Activate
================
*/
void idPhysics_AF::Activate( void ) {
	// if the articulated figure was at rest
	if ( current.atRest >= 0 ) {
		// normally gravity is added at the end of a simulation frame
		// if the figure was at rest add gravity here so it is applied this simulation frame
		AddGravity();
		// reset the active time for the max move time
		current.activateTime = 0.0f;
	}
	current.atRest = -1;
	current.noMoveTime = 0.0f;
	self->BecomeActive( TH_PHYSICS );
}

/*
================
idPhysics_AF::PutToRest

  put to rest untill something collides with this physics object
================
*/
void idPhysics_AF::PutToRest( void ) {
	Rest();
}

/*
================
idPhysics_AF::EnableImpact
================
*/
void idPhysics_AF::EnableImpact( void ) {
	noImpact = false;
}

/*
================
idPhysics_AF::DisableImpact
================
*/
void idPhysics_AF::DisableImpact( void ) {
	noImpact = true;
}

/*
================
idPhysics_AF::AddPushVelocity
================
*/
void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) {
	int i;

	if ( pushVelocity != vec6_origin ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			bodies[i]->current->spatialVelocity += pushVelocity;
		}
	}
}

/*
================
idPhysics_AF::SetClipModel
================
*/
void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) {
}

/*
================
idPhysics_AF::GetClipModel
================
*/
idClipModel *idPhysics_AF::GetClipModel( int id ) const {
	if ( id >= 0 && id < bodies.Num() ) {
		return bodies[id]->GetClipModel();
	}
	return NULL;
}

/*
================
idPhysics_AF::GetNumClipModels
================
*/
int idPhysics_AF::GetNumClipModels( void ) const {
	return bodies.Num();
}

/*
================
idPhysics_AF::SetMass
================
*/
void idPhysics_AF::SetMass( float mass, int id ) {
	if ( id >= 0 && id < bodies.Num() ) {
	}
	else {
		forceTotalMass = mass;
	}
	SetChanged();
}

/*
================
idPhysics_AF::GetMass
================
*/
float idPhysics_AF::GetMass( int id ) const {
	if ( id >= 0 && id < bodies.Num() ) {
#ifdef MOD_WATERPHYSICS
		if ( bodies[id]->GetWaterLevel() > 0.0f )
			return bodies[id]->liquidMass;
		else
#endif
			return bodies[id]->mass;
	}

#ifdef MOD_WATERPHYSICS
	// if body in water, we have to recompute the total mass
	if( this->water != NULL ) {
		int i;
		float waterMass = 0.0f;

		for( i = 0; i < this->bodies.Num(); i++ ) {
			waterMass += this->bodies[i]->liquidMass;
		}
	
		return waterMass;
	}
#endif

	return totalMass;
}

/*
================
idPhysics_AF::SetContents
================
*/
void idPhysics_AF::SetContents( int contents, int id ) {
	int i;

	if ( id >= 0 && id < bodies.Num() ) {
		bodies[id]->GetClipModel()->SetContents( contents );
	}
	else {
		for ( i = 0; i < bodies.Num(); i++ ) {
			bodies[i]->GetClipModel()->SetContents( contents );
		}
	}
}

/*
================
idPhysics_AF::GetContents
================
*/
int idPhysics_AF::GetContents( int id ) const {
	int i, contents;

	if ( id >= 0 && id < bodies.Num() ) {
		return bodies[id]->GetClipModel()->GetContents();
	}
	else {
		contents = 0;
		for ( i = 0; i < bodies.Num(); i++ ) {
			contents |= bodies[i]->GetClipModel()->GetContents();
		}
		return contents;
	}
}

/*
================
idPhysics_AF::GetBounds
================
*/
const idBounds &idPhysics_AF::GetBounds( int id ) const {
	int i;
	static idBounds relBounds;

	if ( id >= 0 && id < bodies.Num() ) {
		return bodies[id]->GetClipModel()->GetBounds();
	}
	else if ( !bodies.Num() ) {
		relBounds.Zero();
		return relBounds;
	}
	else {
		relBounds = bodies[0]->GetClipModel()->GetBounds();
		for ( i = 1; i < bodies.Num(); i++ ) {
			idBounds bounds;
			idVec3 origin = ( bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin() ) * bodies[0]->GetWorldAxis().Transpose();
			idMat3 axis = bodies[i]->GetWorldAxis() * bodies[0]->GetWorldAxis().Transpose();
			bounds.FromTransformedBounds( bodies[i]->GetClipModel()->GetBounds(), origin, axis );
			relBounds += bounds;
		}
		return relBounds;
	}
}

/*
================
idPhysics_AF::GetAbsBounds
================
*/
const idBounds &idPhysics_AF::GetAbsBounds( int id ) const {
	int i;
	static idBounds absBounds;

	if ( id >= 0 && id < bodies.Num() ) {
		return bodies[id]->GetClipModel()->GetAbsBounds();
	}
	else if ( !bodies.Num() ) {
		absBounds.Zero();
		return absBounds;
	}
	else {
		absBounds = bodies[0]->GetClipModel()->GetAbsBounds();
		for ( i = 1; i < bodies.Num(); i++ ) {
			absBounds += bodies[i]->GetClipModel()->GetAbsBounds();
		}
		return absBounds;
	}
}

/*
================
idPhysics_AF::Evaluate
================
*/
bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) 
{
	float timeStep;

	if ( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) ) {
		timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart );
	} else if ( af_timeScale.GetFloat() != 1.0f ) {
		timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat();
	} else {
		timeStep = MS2SEC( timeStepMSec ) * timeScale;
	}
	current.lastTimeStep = timeStep;

	// stgatilov #5992: compute total force/torque for all bodies
	for ( int i = 0; i < bodies.Num(); i++ ) {
		// gravity and contact forces are added to externalForce at the end of Evaluate,
		// i.e. after it has already taken effect in simulation
		// so here we need to add them instead of just clearing totals
		// note: I don't know whether it is important of gravity/contacts can be moved to start...
		idVec6 externalFromLastEvaluate = bodies[i]->current->externalForce;
		bodies[i]->forceApplications.ComputeTotal(
			bodies[i]->current->worldOrigin,
			&bodies[i]->current->externalForce.SubVec3(0),
			&bodies[i]->current->externalForce.SubVec3(1)
		);
		bodies[i]->current->externalForce += externalFromLastEvaluate;
	}

	// if the articulated figure changed
	if ( changedAF || ( linearTime != af_useLinearTime.GetBool() ) ) {
		BuildTrees();
		changedAF = false;
		linearTime = af_useLinearTime.GetBool();
	}

	// get the new master position
	if ( masterBody ) {
		idVec3 masterOrigin;
		idMat3 masterAxis;
		self->GetMasterPosition( masterOrigin, masterAxis );
		if ( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) ) {
			Activate();
		}
		masterBody->current->worldOrigin = masterOrigin;
		masterBody->current->worldAxis = masterAxis;
	}

	// if the simulation is suspended because the figure is at rest
	if ( current.atRest >= 0 || timeStep <= 0.0f ) {
		DebugDraw();
		return false;
	}

	// move the af velocity into the frame of a pusher
	AddPushVelocity( -current.pushVelocity );

	// TDM: Enable the clipmodels of all team members for collisions
	idEntity *part = NULL;
	
	idList<bool> InitClipStates;
	bool PartClipState = false;

	if( ((idAFEntity_Base *) self )->CollidesWithTeam() )
	{
		for ( part = self->GetTeamMaster(); part != NULL; part = part->GetNextTeamEntity() ) 
		{
			if ( part != self && part->GetPhysics() ) 
			{
				PartClipState = part->GetPhysics()->GetClipModel()->IsEnabled();
				InitClipStates.Append( PartClipState );

				part->GetPhysics()->EnableClip();
			}
		}
	}

#ifdef AF_TIMINGS
	timer_total.Start();
#endif

#ifdef AF_TIMINGS
	timer_collision.Start();
#endif

	// evaluate contacts
	EvaluateContacts();

	// setup contact constraints
	SetupContactConstraints();

#ifdef AF_TIMINGS
	timer_collision.Stop();
#endif

	// evaluate constraint equations
	EvaluateConstraints( timeStep );

	// apply friction
	ApplyFriction( timeStep, endTimeMSec );

	// add frame constraints
	AddFrameConstraints();

#ifdef AF_TIMINGS
	int i, numPrimary = 0, numAuxiliary = 0;
	for ( i = 0; i < primaryConstraints.Num(); i++ ) {
		numPrimary += primaryConstraints[i]->J1.GetNumRows();
	}
	for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
		numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows();
	}
	timer_pc.Start();
#endif

	// factor matrices for primary constraints
	PrimaryFactor();

	// calculate forces on bodies after applying primary constraints
	PrimaryForces( timeStep );

#ifdef AF_TIMINGS
	timer_pc.Stop();
	timer_ac.Start();
#endif

	// calculate and apply auxiliary constraint forces
	AuxiliaryForces( timeStep );

#ifdef AF_TIMINGS
	timer_ac.Stop();
#endif

	// evolve current state to next state
	Evolve( timeStep );

	// debug graphics
	DebugDraw();

	// clear external forces on all bodies
	for ( int i = 0; i < bodies.Num(); i++ ) {
		idAFBody *body = bodies[i];
		// stgatilov #5992: clear total force to zero
		// the remaining part of Evaluate will add gravity and contact forces to it
		// we'll take them into account on the next physics subtic
		body->current->externalForce.Zero();
		body->next->externalForce.Zero();
		// drop list of forces
		body->forceApplications.Clear();
	}

	// apply contact force to other entities
	ApplyContactForces();

	// remove all frame constraints
	RemoveFrameConstraints();

#ifdef AF_TIMINGS
	timer_collision.Start();
#endif

	// check for collisions between current and next state
	CheckForCollisions( timeStep );

#ifdef AF_TIMINGS
	timer_collision.Stop();
#endif

	// swap the current and next state
	SwapStates();

	// make sure all clip models are disabled in case they were enabled for self collision
	if ( selfCollision && !af_skipSelfCollision.GetBool() ) {
		DisableClip();
	}

	// apply collision impulses
	if ( ApplyCollisions( timeStep ) ) {
		current.atRest = gameLocal.time;
		comeToRest = true;
	}

	// test if the simulation can be suspended because the whole figure is at rest
	if ( comeToRest && TestIfAtRest( timeStep ) ) {
		Rest();
	} else {
		ActivateContactEntities();
	}

	// add gravitational force
	AddGravity();

	// move the af velocity back into the world frame
	AddPushVelocity( current.pushVelocity );
	current.pushVelocity.Zero();

	if ( IsOutsideWorld() ) {
		gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)",
							self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0) );
		Rest();
	}

#ifdef AF_TIMINGS
	timer_total.Stop();

	if ( af_showTimings.GetInteger() == 1 ) {
		gameLocal.Printf( "%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
						self->name.c_str(),
						timer_total.Milliseconds(),
						numPrimary, timer_pc.Milliseconds(),
						numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
						timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
	}
	else if ( af_showTimings.GetInteger() == 2 ) {
		numArticulatedFigures++;
		if ( endTimeMSec > lastTimerReset ) {
			gameLocal.Printf( "af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
							numArticulatedFigures,
							timer_total.Milliseconds(),
							numPrimary, timer_pc.Milliseconds(),
							numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
							timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
		}
	}

	if ( endTimeMSec > lastTimerReset ) {
		lastTimerReset = endTimeMSec;
		numArticulatedFigures = 0;
		timer_total.Clear();
		timer_pc.Clear();
		timer_ac.Clear();
		timer_collision.Clear();
		timer_lcp.Clear();
	}
#endif


	// TDM: Disable the clipmodels that we enabled above
	int count = 0;

	if( ((idAFEntity_Base *) self )->CollidesWithTeam() )
	{
		for ( part = self->GetTeamMaster(); part != NULL; part = part->GetNextTeamEntity() )  
		{
			if ( part != self && part->GetPhysics() ) 
			{
				if( !InitClipStates[count] )
					part->GetPhysics()->DisableClip();

				count++;
			}
		}
	}

	return true;
}

/*
================
idPhysics_AF::UpdateTime
================
*/
void idPhysics_AF::UpdateTime( int endTimeMSec ) {
}

/*
================
idPhysics_AF::GetTime
================
*/
int idPhysics_AF::GetTime( void ) const {
	return gameLocal.time;
}

/*
================
DrawTraceModelSilhouette
================
*/
void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel *clipModel ) {
	int i, numSilEdges;
	int silEdges[MAX_TRACEMODEL_EDGES];
	idVec3 v1, v2;
	const idTraceModel *trm = clipModel->GetTraceModel();
	const idVec3 &origin = clipModel->GetOrigin();
	const idMat3 &axis = clipModel->GetAxis();

	numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges );
	for ( i = 0; i < numSilEdges; i++ ) {
		v1 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITSET( silEdges[i] ) ] ];
		v2 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITNOTSET( silEdges[i] ) ] ];
		gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 );
	}
}

/*
================
idPhysics_AF::DebugDraw
================
*/
void idPhysics_AF::DebugDraw( void ) {
	int i;
	idAFBody *body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL;
	idAFConstraint *constraint;
	idVec3 center;
	idMat3 axis;

	if ( af_highlightConstraint.GetString()[0] ) {
		constraint = GetConstraint( af_highlightConstraint.GetString() );
		if ( constraint ) {
			constraint->GetCenter( center );
			axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
			gameRenderWorld->DebugCone( colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0 );

			if ( af_showConstrainedBodies.GetBool() ) {
				cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) );
				constrainedBody1 = constraint->body1;
				if ( constrainedBody1 ) {
					collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(),
											constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f );
				}
				cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) );
				constrainedBody2 = constraint->body2;
				if ( constrainedBody2 ) {
					collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(),
											constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f );
				}
				cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
			}
		}
	}

	if ( af_highlightBody.GetString()[0] ) {
		highlightBody = GetBody( af_highlightBody.GetString() );
		if ( highlightBody ) {
			cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) );
			collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(),
									highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f );
			cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
		}
	}

	if ( af_showBodies.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
			if ( body == constrainedBody1 || body == constrainedBody2 ) {
				continue;
			}
			if ( body == highlightBody ) {
				continue;
			}
			collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(),
										body->clipModel->GetAxis(), vec3_origin, 0.0f );
			//DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel );
		}
	}

	if ( af_showBodyNames.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
			gameRenderWorld->DebugText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
		}
	}

	if ( af_showMass.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
#ifdef MOD_WATERPHYSICS
			if( body->GetWaterLevel() > 0.0f )
				gameRenderWorld->DebugText( va( "\n%1.2f", body->liquidMass ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
			else
#endif
			gameRenderWorld->DebugText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
		}
	}

	if ( af_showTotalMass.GetBool() ) {
		axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
#ifdef MOD_WATERPHYSICS
		gameRenderWorld->DebugText( va( "\n%1.2f", this->GetMass() ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
#else
		gameRenderWorld->DebugText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
#endif
	}

	if ( af_showInertia.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
			idMat3 &I = body->inertiaTensor;
			gameRenderWorld->DebugText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
										I[0].x, I[0].y, I[0].z,
										I[1].x, I[1].y, I[1].z,
										I[2].x, I[2].y, I[2].z ),
										body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
		}
	}

	if ( af_showVelocity.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f );
		}
	}

	if ( af_showConstraints.GetBool() ) {
		for ( i = 0; i < primaryConstraints.Num(); i++ ) {
			constraint = primaryConstraints[i];
			constraint->DebugDraw();
		}
		if ( !af_showPrimaryOnly.GetBool() ) {
			for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
				constraint = auxiliaryConstraints[i];
				constraint->DebugDraw();
			}
		}
	}

	if ( af_showConstraintNames.GetBool() ) {
		for ( i = 0; i < primaryConstraints.Num(); i++ ) {
			constraint = primaryConstraints[i];
			constraint->GetCenter( center );
			gameRenderWorld->DebugText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
		}
		if ( !af_showPrimaryOnly.GetBool() ) {
			for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) {
				constraint = auxiliaryConstraints[i];
				constraint->GetCenter( center );
				gameRenderWorld->DebugText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
			}
		}
	}

	if ( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) ) {
		for ( i = 0; i < trees.Num(); i++ ) {
			trees[i]->DebugDraw( idStr::ColorForIndex( i+3 ) );
		}
	}
}

/*
================
idPhysics_AF::idPhysics_AF
================
*/
idPhysics_AF::idPhysics_AF( void ) {
	trees.Clear();
	bodies.Clear();
	constraints.Clear();
	primaryConstraints.Clear();
	auxiliaryConstraints.Clear();
	frameConstraints.Clear();
	contacts.Clear();
	collisions.Clear();
	changedAF = true;
	masterBody = NULL;

	lcp = idLCP::AllocSymmetric();

	memset( &current, 0, sizeof( current ) );
	current.atRest = -1;
	current.lastTimeStep = USERCMD_MSEC;
	saved = current;

	linearFriction = 0.005f;
	angularFriction = 0.005f;
	contactFriction = 0.8f;
	bouncyness = 0.4f;
	totalMass = 0.0f;
	forceTotalMass = -1.0f;

#ifdef MOD_WATERPHYSICS
	// sets default buoyancy property based on CVar
	if( af_useFixedDensityBuoyancy.GetBool() ) {
		this->fixedDensityBuoyancy = true;
		this->liquidDensity = DEFAULT_LIQUID_DENSITY;
	}
	else {
		this->fixedDensityBuoyancy = false;
		this->liquidDensity = DEFAULT_LIQUID_SCALAR;
	}
	this->water = NULL;
	this->m_fWaterMurkiness = 0.0f;
#endif

	suspendVelocity.Set( SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY );
	suspendAcceleration.Set( SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION );
	noMoveTime = NO_MOVE_TIME;
	noMoveTranslation = NO_MOVE_TRANSLATION_TOLERANCE;
	noMoveRotation = NO_MOVE_ROTATION_TOLERANCE;
	minMoveTime = MIN_MOVE_TIME;
	maxMoveTime = MAX_MOVE_TIME;
	impulseThreshold = IMPULSE_THRESHOLD;

	timeScale = 1.0f;
	timeScaleRampStart = 0.0f;
	timeScaleRampEnd = 0.0f;

	jointFrictionScale = 0.0f;
	jointFrictionDent = 0.0f;
	jointFrictionDentStart = 0.0f;
	jointFrictionDentEnd = 0.0f;
	jointFrictionDentScale = 0.0f;

	contactFrictionScale = 0.0f;
	contactFrictionDent = 0.0f;
	contactFrictionDentStart = 0.0f;
	contactFrictionDentEnd = 0.0f;
	contactFrictionDentScale = 0.0f;

	enableCollision = true;
	selfCollision = true;
	comeToRest = true;
	linearTime = true;
	noImpact = false;
	worldConstraintsLocked = false;
	forcePushable = false;

	// Init these to -1, if they are still -1 on save time, 
	// save just uses the actual numbers in the lists
	m_NumOrigBodies = -1;
	m_NumOrigConstraints = -1;

#ifdef AF_TIMINGS
	lastTimerReset = 0;
#endif
}

/*
================
idPhysics_AF::~idPhysics_AF
================
*/
idPhysics_AF::~idPhysics_AF( void ) {
	int i;

	trees.DeleteContents( true );

	for ( i = 0; i < bodies.Num(); i++ ) {
		delete bodies[i];
	}

	for ( i = 0; i < constraints.Num(); i++ ) {
		delete constraints[i];
	}

	contactConstraints.SetNum( contactConstraints.NumAllocated(), false );
	for ( i = 0; i < contactConstraints.NumAllocated(); i++ ) {
		delete contactConstraints[i];
	}

	delete lcp;

	if ( masterBody ) {
		delete masterBody;
	}
}

/*
================
idPhysics_AF_SavePState
================
*/
void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) {
	saveFile->WriteInt( state.atRest );
	saveFile->WriteFloat( state.noMoveTime );
	saveFile->WriteFloat( state.activateTime );
	saveFile->WriteFloat( state.lastTimeStep );
	saveFile->WriteVec6( state.pushVelocity );
}

/*
================
idPhysics_AF_RestorePState
================
*/
void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) {
	saveFile->ReadInt( state.atRest );
	saveFile->ReadFloat( state.noMoveTime );
	saveFile->ReadFloat( state.activateTime );
	saveFile->ReadFloat( state.lastTimeStep );
	saveFile->ReadVec6( state.pushVelocity );
}

/*
================
idPhysics_AF::Save
================
*/
void idPhysics_AF::Save( idSaveGame *saveFile ) const 
{
	int i, num;

	// the articulated figure structure is handled by the owner

	idPhysics_AF_SavePState( saveFile, current );
	idPhysics_AF_SavePState( saveFile, saved );

	if( m_NumOrigBodies < 0 )
		num = bodies.Num();
	else
		num = m_NumOrigBodies;

	saveFile->WriteInt( num );
	for ( i = 0; i < num; i++ ) {
		bodies[i]->Save( saveFile );
	}
	if ( masterBody ) {
		saveFile->WriteBool( true );
		masterBody->Save( saveFile );
	} else {
		saveFile->WriteBool( false );
	}

	if( m_NumOrigConstraints < 0 )
		num = constraints.Num();
	else
		num = m_NumOrigConstraints;

	saveFile->WriteInt( num );
	for ( i = 0; i < num; i++ ) {
		constraints[i]->Save( saveFile );
	}

	saveFile->WriteBool( changedAF );

	saveFile->WriteFloat( linearFriction );
	saveFile->WriteFloat( angularFriction );
	saveFile->WriteFloat( contactFriction );
	saveFile->WriteFloat( bouncyness );
	saveFile->WriteFloat( totalMass );
	saveFile->WriteFloat( forceTotalMass );

#ifdef MOD_WATERPHYSICS
	saveFile->WriteBool( this->fixedDensityBuoyancy );
	saveFile->WriteFloat( this->liquidDensity );
#endif

	saveFile->WriteVec2( suspendVelocity );
	saveFile->WriteVec2( suspendAcceleration );
	saveFile->WriteFloat( noMoveTime );
	saveFile->WriteFloat( noMoveTranslation );
	saveFile->WriteFloat( noMoveRotation );
	saveFile->WriteFloat( minMoveTime );
	saveFile->WriteFloat( maxMoveTime );
	saveFile->WriteFloat( impulseThreshold );

	saveFile->WriteFloat( timeScale );
	saveFile->WriteFloat( timeScaleRampStart );
	saveFile->WriteFloat( timeScaleRampEnd );

	saveFile->WriteFloat( jointFrictionScale );
	saveFile->WriteFloat( jointFrictionDent );
	saveFile->WriteFloat( jointFrictionDentStart );
	saveFile->WriteFloat( jointFrictionDentEnd );
	saveFile->WriteFloat( jointFrictionDentScale );

	saveFile->WriteFloat( contactFrictionScale );
	saveFile->WriteFloat( contactFrictionDent );
	saveFile->WriteFloat( contactFrictionDentStart );
	saveFile->WriteFloat( contactFrictionDentEnd );
	saveFile->WriteFloat( contactFrictionDentScale );

	saveFile->WriteBool( enableCollision );
	saveFile->WriteBool( selfCollision );
	saveFile->WriteBool( comeToRest );
	saveFile->WriteBool( linearTime );
	saveFile->WriteBool( noImpact );
	saveFile->WriteBool( worldConstraintsLocked );
	saveFile->WriteBool( forcePushable );
}

/*
================
idPhysics_AF::Restore
================
*/
void idPhysics_AF::Restore( idRestoreGame *saveFile ) {
	int i, num;
	bool hasMaster;

	// the articulated figure structure should have already been restored

	idPhysics_AF_RestorePState( saveFile, current );
	idPhysics_AF_RestorePState( saveFile, saved );

	saveFile->ReadInt( num );
	assert( num == bodies.Num() );
	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->Restore( saveFile );
	}
	saveFile->ReadBool( hasMaster );
	if ( hasMaster ) {
		masterBody = new idAFBody();
		masterBody->Restore( saveFile );
	}

	saveFile->ReadInt( num );
	assert( num == constraints.Num() );
	for ( i = 0; i < constraints.Num(); i++ ) {
		constraints[i]->Restore( saveFile );
	}

	saveFile->ReadBool( changedAF );

	saveFile->ReadFloat( linearFriction );
	saveFile->ReadFloat( angularFriction );
	saveFile->ReadFloat( contactFriction );
	saveFile->ReadFloat( bouncyness );
	saveFile->ReadFloat( totalMass );
	saveFile->ReadFloat( forceTotalMass );

#ifdef MOD_WATERPHYSICS
	saveFile->ReadBool( this->fixedDensityBuoyancy );
	saveFile->ReadFloat( this->liquidDensity );
#endif

	saveFile->ReadVec2( suspendVelocity );
	saveFile->ReadVec2( suspendAcceleration );
	saveFile->ReadFloat( noMoveTime );
	saveFile->ReadFloat( noMoveTranslation );
	saveFile->ReadFloat( noMoveRotation );
	saveFile->ReadFloat( minMoveTime );
	saveFile->ReadFloat( maxMoveTime );
	saveFile->ReadFloat( impulseThreshold );

	saveFile->ReadFloat( timeScale );
	saveFile->ReadFloat( timeScaleRampStart );
	saveFile->ReadFloat( timeScaleRampEnd );

	saveFile->ReadFloat( jointFrictionScale );
	saveFile->ReadFloat( jointFrictionDent );
	saveFile->ReadFloat( jointFrictionDentStart );
	saveFile->ReadFloat( jointFrictionDentEnd );
	saveFile->ReadFloat( jointFrictionDentScale );

	saveFile->ReadFloat( contactFrictionScale );
	saveFile->ReadFloat( contactFrictionDent );
	saveFile->ReadFloat( contactFrictionDentStart );
	saveFile->ReadFloat( contactFrictionDentEnd );
	saveFile->ReadFloat( contactFrictionDentScale );

	saveFile->ReadBool( enableCollision );
	saveFile->ReadBool( selfCollision );
	saveFile->ReadBool( comeToRest );
	saveFile->ReadBool( linearTime );
	saveFile->ReadBool( noImpact );
	saveFile->ReadBool( worldConstraintsLocked );
	saveFile->ReadBool( forcePushable );

	changedAF = true;

	UpdateClipModels();
}

/*
================
idPhysics_AF::IsClosedLoop
================
*/
bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const {
	const idAFBody *b1, *b2;

	for ( b1 = body1; b1->parent; b1 = b1->parent ) {
	}
	for ( b2 = body2; b2->parent; b2 = b2->parent ) {
	}
	return ( b1 == b2 );
}

/*
================
idPhysics_AF::BuildTrees
================
*/
void idPhysics_AF::BuildTrees( void ) {
	int i;
	float scale;
	idAFBody *b;
	idAFConstraint *c;
	idAFTree *tree;

	primaryConstraints.Clear();
	auxiliaryConstraints.Clear();
	trees.DeleteContents( true );

	totalMass = 0.0f;
	for ( i = 0; i < bodies.Num(); i++ ) {
		b = bodies[i];
		b->parent = NULL;
		b->primaryConstraint = NULL;
		b->constraints.SetNum( 0, false );
		b->children.Clear();
		b->tree = NULL;
		totalMass += b->mass;
	}

	if ( forceTotalMass > 0.0f ) {
		scale = forceTotalMass / totalMass;
		for ( i = 0; i < bodies.Num(); i++ ) {
			b = bodies[i];
			b->mass *= scale;
			b->invMass = 1.0f / b->mass;
			b->inertiaTensor *= scale;
			b->inverseInertiaTensor = b->inertiaTensor.Inverse();
		}
		totalMass = forceTotalMass;
	}

	if ( af_useLinearTime.GetBool() ) {

		for ( i = 0; i < constraints.Num(); i++ ) {
			c = constraints[i];

			c->body1->constraints.Append( c );
			if ( c->body2 ) {
				c->body2->constraints.Append( c );
			}

			// only bilateral constraints between two non-world bodies that do not
			// create loops can be used as primary constraints
			if ( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) ) {
				c->body1->primaryConstraint = c;
				c->body1->parent = c->body2;
				c->body2->children.Append( c->body1 );
				c->fl.isPrimary = true;
				c->firstIndex = 0;
				primaryConstraints.Append( c );
			} else {
				c->fl.isPrimary = false;
				auxiliaryConstraints.Append( c );
			}
		}

		// create trees for all parent bodies
		for ( i = 0; i < bodies.Num(); i++ ) {
			if ( !bodies[i]->parent ) {
				tree = new idAFTree();
				tree->sortedBodies.Clear();
				tree->sortedBodies.Append( bodies[i] );
				bodies[i]->tree = tree;
				trees.Append( tree );
			}
		}

		// add each child body to the appropriate tree
		for ( i = 0; i < bodies.Num(); i++ ) {
			if ( bodies[i]->parent ) {
				for ( b = bodies[i]->parent; !b->tree; b = b->parent ) {
				}
				b->tree->sortedBodies.Append( bodies[i] );
				bodies[i]->tree = b->tree;
			}
		}

		if ( trees.Num() > 1 ) {
			gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.",
								self->name.c_str(), self->GetType()->classname );
		}

		// sort bodies in each tree to make sure parents come first
		for ( i = 0; i < trees.Num(); i++ ) {
			trees[i]->SortBodies();
		}

	} else {

		// create a tree for each body
		for ( i = 0; i < bodies.Num(); i++ ) {
			tree = new idAFTree();
			tree->sortedBodies.Clear();
			tree->sortedBodies.Append( bodies[i] );
			bodies[i]->tree = tree;
			trees.Append( tree );
		}

		for ( i = 0; i < constraints.Num(); i++ ) {
			c = constraints[i];

			c->body1->constraints.Append( c );
			if ( c->body2 ) {
				c->body2->constraints.Append( c );
			}

			c->fl.isPrimary = false;
			auxiliaryConstraints.Append( c );
		}
	}
}

/*
================
idPhysics_AF::AddBody

  bodies get an id in the order they are added starting at zero
  as such the first body added will get id zero
================
*/
int idPhysics_AF::AddBody( idAFBody *body )
{
	int id = 0;

	if ( !body->clipModel )
	{
		gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() );
	}

	if ( bodies.Find( body ) )
	{
		gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() );
	}

	/* grayman #3591 - We've only seen this problem with the card players, when the game is saved around
	the frame when the card player draws a card from his deck. In theory, it could also happen when 
	any objects are spawned by anim frame commands, i.e. the flint used to relight doused lights. The
	problem occurs because the list of attached objects is saved during a savegame, but during restore
	the list isn't recreated until after all objects have been spawned (idAFEntity_Base::RestoreAddedEnts()).

	But when objects are all spawned, each object is allowed to think and update its animation. For some
	reason, the card player's animation	replays the frame command to get a card and puts that into the
	attached list. When the delayed list of attached objects is allowed to be re-added to the attached list,
	the card is already there with the same name, because all of this happens in the same frame. I wasn't
	able to figure out why the frame command was running twice, but we've had problems with this in the past.
	So I'm taking the simple way out, which is to delete the object already in the list and append the new
	one to the list.
	
	Change the error message to a warning.
	*/

	if ( GetBody( body->name ) )
	{
		gameLocal.Warning( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() );
		DeleteBody(body->name);
		//gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() );
	}

	id = bodies.Num();
	body->clipModel->SetId( id );
	if ( body->linearFriction < 0.0f )
	{
		body->linearFriction = linearFriction;
		body->angularFriction = angularFriction;
		body->contactFriction = contactFriction;
	}

	if ( body->bouncyness < 0.0f )
	{
		body->bouncyness = bouncyness;
	}

	if ( !body->fl.clipMaskSet )
	{
		body->clipMask = clipMask;
	}

	bodies.Append( body );

	changedAF = true;

	return id;
}

/*
================
idPhysics_AF::AddConstraint
================
*/
void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) {

	if ( constraints.Find( constraint ) ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() );
	}
	if ( GetConstraint( constraint->name ) ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() );
	}
	if ( !constraint->body1 ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() );
	}
	if ( !bodies.Find( constraint->body1 ) ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
	}
	if ( constraint->body2 && !bodies.Find( constraint->body2 ) ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
	}
	if ( constraint->body1 == constraint->body2 ) {
		gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() );
	}

	constraints.Append( constraint );
	constraint->physics = this;

	changedAF = true;
}

/*
================
idPhysics_AF::AddFrameConstraint
================
*/
void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) {
	frameConstraints.Append( constraint );
	constraint->physics = this;
}

/*
================
idPhysics_AF::ForceBodyId
================
*/
void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) {
	int id;

	id = bodies.FindIndex( body );
	if ( id == -1 ) {
		gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
	}
	if ( id != newId ) {
		idAFBody *b = bodies[newId];
		bodies[newId] = bodies[id];
		bodies[id] = b;
		changedAF = true;
	}
}

/*
================
idPhysics_AF::GetBodyId
================
*/
int idPhysics_AF::GetBodyId( idAFBody *body ) const {
	int id;

	id = bodies.FindIndex( body );
	if ( id == -1 && body ) {
		gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
	}
	return id;
}

/*
================
idPhysics_AF::GetBodyId
================
*/
int idPhysics_AF::GetBodyId( const char *bodyName ) const {
	int i;

	for ( i = 0; i < bodies.Num(); i++ ) {
		if ( !bodies[i]->name.Icmp( bodyName ) ) {
			return i;
		}
	}
	gameLocal.Error( "GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName );
	return 0;
}

/*
================
idPhysics_AF::GetConstraintId
================
*/
int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const {
	int id;

	id = constraints.FindIndex( constraint );
	if ( id == -1 && constraint ) {
		gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() );
	}
	return id;
}

/*
================
idPhysics_AF::GetConstraintId
================
*/
int idPhysics_AF::GetConstraintId( const char *constraintName ) const {
	int i;

	for ( i = 0; i < constraints.Num(); i++ ) {
		if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
			return i;
		}
	}
	gameLocal.Error( "GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName );
	return 0;
}

/*
================
idPhysics_AF::GetNumBodies
================
*/
int idPhysics_AF::GetNumBodies( void ) const {
	return bodies.Num();
}

/*
================
idPhysics_AF::GetNumConstraints
================
*/
int idPhysics_AF::GetNumConstraints( void ) const {
	return constraints.Num();
}

/*
================
idPhysics_AF::GetBody
================
*/
idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const {
	int i;

	for ( i = 0; i < bodies.Num(); i++ ) {
		if ( !bodies[i]->name.Icmp( bodyName ) ) {
			return bodies[i];
		}
	}

	return NULL;
}

/*
================
idPhysics_AF::GetBody
================
*/
idAFBody *idPhysics_AF::GetBody( const int id ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		gameLocal.Error( "GetBody: no body with id %d exists on ent %s\n", id, self->name.c_str() );
		return NULL;
	}
	return bodies[id];
}

/*
================
idPhysics_AF::GetConstraint
================
*/
idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const {
	int i;

	for ( i = 0; i < constraints.Num(); i++ ) {
		if ( constraints[i]->name.Icmp( constraintName ) == 0 ) {
			return constraints[i];
		}
	}

	return NULL;
}

/*
================
idPhysics_AF::GetConstraint
================
*/
idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const {
	if ( id < 0 || id >= constraints.Num() ) {
		gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id );
		return NULL;
	}
	return constraints[id];
}

/*
================
idPhysics_AF::DeleteBody
================
*/
void idPhysics_AF::DeleteBody( const char *bodyName ) {
	int i;

	// find the body with the given name
	for ( i = 0; i < bodies.Num(); i++ ) {
		if ( !bodies[i]->name.Icmp( bodyName ) ) {
			break;
		}
	}

	if ( i >= bodies.Num() ) {
		gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
							bodyName, self->name.c_str(), self->GetType()->classname );
		return;
	}

	DeleteBody( i );
}

/*
================
idPhysics_AF::DeleteBody
================
*/
void idPhysics_AF::DeleteBody( const int id ) {
	int j;

	if ( id < 0 || id > bodies.Num() ) {
		gameLocal.Error( "DeleteBody: no body with id %d.", id );
		return;
	}

	// remove any constraints attached to this body
	for ( j = 0; j < constraints.Num(); j++ ) {
		if ( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] ) {
			delete constraints[j];
			constraints.RemoveIndex( j );
			j--;
		}
	}

	// remove the body
	delete bodies[id];
	bodies.RemoveIndex( id );

	// set new body ids
	for ( j = 0; j < bodies.Num(); j++ ) {
		bodies[j]->clipModel->SetId( j );
	}

	changedAF = true;
}

/*
================
idPhysics_AF::DeleteConstraint
================
*/
void idPhysics_AF::DeleteConstraint( const char *constraintName ) {
	int i;

	// find the constraint with the given name
	for ( i = 0; i < constraints.Num(); i++ ) {
		if ( !constraints[i]->name.Icmp( constraintName ) ) {
			break;
		}
	}

	if ( i >= constraints.Num() ) {
		gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
							constraintName, self->name.c_str(), self->GetType()->classname );
		return;
	}

	DeleteConstraint( i );
}

/*
================
idPhysics_AF::DeleteConstraint
================
*/
void idPhysics_AF::DeleteConstraint( const int id ) {

	if ( id < 0 || id >= constraints.Num() ) {
		gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id );
		return;
	}

	// remove the constraint
	delete constraints[id];
	constraints.RemoveIndex( id );

	changedAF = true;
}

/*
================
idPhysics_AF::GetBodyContactConstraints
================
*/
int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const {
	int i, numContacts;
	idAFBody *body;
	idAFConstraint_Contact *contact;

	if ( id < 0 || id >= bodies.Num() || maxContacts <= 0 ) {
		return 0;
	}

	numContacts = 0;
	body = bodies[id];
	for ( i = 0; i < contactConstraints.Num(); i++ ) {
		contact = contactConstraints[i];
		if ( contact->body1 == body || contact->body2 == body ) {
			contacts[numContacts++] = contact;
			if ( numContacts >= maxContacts ) {
				return numContacts;
			}
		}
	}
	return numContacts;
}

/*
================
idPhysics_AF::SetDefaultFriction
================
*/
void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact ) {
	if (	linear < 0.0f || linear > 1.0f ||
			angular < 0.0f || angular > 1.0f ||
			contact < 0.0f || contact > 1.0f ) {
		return;
	}
	linearFriction = linear;
	angularFriction = angular;
	contactFriction = contact;
}

/*
================
idPhysics_AF::GetImpactInfo
================
*/
void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		memset( info, 0, sizeof( *info ) );
		return;
	}
#ifdef MOD_WATERPHYSICS
	if( this->water != NULL )
		info->invMass = bodies[id]->invLiquidMass;
	else
		info->invMass = bodies[id]->invMass;
#else
	info->invMass = 1.0f / bodies[id]->mass;
#endif
	info->invInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
	info->position = point - bodies[id]->current->worldOrigin;
	info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross( info->position );
}

/*
================
idPhysics_AF::ApplyImpulse
================
*/
void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {

	if ( id < 0 || id >= bodies.Num() ) {
		return;
	}
	if ( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) ) {
		return;
	}
	idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
#ifdef MOD_WATERPHYSICS
	if( this->water != NULL )
		bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invLiquidMass * impulse;
	else
		bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse;
#else
	bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse;
#endif
	bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross( impulse );
	Activate();
}

/*
================
idPhysics_AF::AddForce
================
*/
void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &force, const idForceApplicationId &applId ) {
	if ( noImpact ) {
		return;
	}
	if ( id < 0 || id >= bodies.Num() ) {
		return;
	}
	bodies[id]->forceApplications.Add( point, force, applId );
	Activate();
}

/*
================
idPhysics_AF::IsAtRest
================
*/
bool idPhysics_AF::IsAtRest( void ) const {
	return current.atRest >= 0;
}

/*
================
idPhysics_AF::GetRestStartTime
================
*/
int idPhysics_AF::GetRestStartTime( void ) const {
	return current.atRest;
}

/*
================
idPhysics_AF::IsPushable
================
*/
bool idPhysics_AF::IsPushable( void ) const {
	return ( !noImpact && ( masterBody == NULL || forcePushable ) );
}

/*
================
idPhysics_AF::SaveState
================
*/
void idPhysics_AF::SaveState( void ) {
	int i;

	saved = current;

	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->saved = *bodies[i]->current;
	}
}

/*
================
idPhysics_AF::RestoreState
================
*/
void idPhysics_AF::RestoreState( void ) {
	int i;

	current = saved;

	for ( i = 0; i < bodies.Num(); i++ ) {
		*(bodies[i]->current) = bodies[i]->saved;
	}

	EvaluateContacts();
}

/*
================
idPhysics_AF::SetOrigin
================
*/
void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) {
	if ( masterBody ) {
		Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin );
	} else {
		Translate( newOrigin - bodies[0]->current->worldOrigin );
	}
}

/*
================
idPhysics_AF::SetAxis
================
*/
void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) {
	idMat3 axis;
	idRotation rotation;

	if ( masterBody ) {
		axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis );
	} else {
		axis = bodies[0]->current->worldAxis.Transpose() * newAxis;
	}
	rotation = axis.ToRotation();
	rotation.SetOrigin( bodies[0]->current->worldOrigin );

	Rotate( rotation );
}

/*
================
idPhysics_AF::Translate
================
*/
void idPhysics_AF::Translate( const idVec3 &translation, int id ) {
	int i;
	idAFBody *body;

	if ( !worldConstraintsLocked ) {
		// translate constraints attached to the world
		for ( i = 0; i < constraints.Num(); i++ ) {
			constraints[i]->Translate( translation );
		}
	}

	// translate all the bodies
	for ( i = 0; i < bodies.Num(); i++ ) {

		body = bodies[i];
		body->current->worldOrigin += translation;
	}

	Activate();

	UpdateClipModels();
}

/*
================
idPhysics_AF::Rotate
================
*/
void idPhysics_AF::Rotate( const idRotation &rotation, int id ) {
	int i;
	idAFBody *body;

	// ishtvan: Stop the stretching error due to too many
	// mat3 to rotation to mat3 conversions
	idRotation rotationC = rotation;
	rotationC.ReCalculateMatrix();

	if ( !worldConstraintsLocked ) {
		// rotate constraints attached to the world
		for ( i = 0; i < constraints.Num(); i++ ) {
			constraints[i]->Rotate( rotationC );
		}
	}

	// rotate all the bodies
	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		body->current->worldOrigin *= rotationC;
		body->current->worldAxis *= rotationC.ToMat3();
	}

	Activate();

	UpdateClipModels();
}

/*
================
idPhysics_AF::GetOrigin
================
*/
const idVec3 &idPhysics_AF::GetOrigin( int id ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		return vec3_origin;
	}
	else {
		return bodies[id]->current->worldOrigin;
	}
}

/*
================
idPhysics_AF::GetAxis
================
*/
const idMat3 &idPhysics_AF::GetAxis( int id ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		return mat3_identity;
	}
	else {
		return bodies[id]->current->worldAxis;
	}
}

/*
================
idPhysics_AF::SetLinearVelocity
================
*/
void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
	if ( id < 0 || id >= bodies.Num() ) {
		return;
	}
	bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity;
	Activate();
}

/*
================
idPhysics_AF::SetAngularVelocity
================
*/
void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) {
	if ( id < 0 || id >= bodies.Num() ) {
		return;
	}
	bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity;
	Activate();
}

/*
================
idPhysics_AF::GetLinearVelocity
================
*/
const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		return vec3_origin;
	}
	else {
		return bodies[id]->current->spatialVelocity.SubVec3( 0 );
	}
}

/*
================
idPhysics_AF::GetAngularVelocity
================
*/
const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const {
	if ( id < 0 || id >= bodies.Num() ) {
		return vec3_origin;
	}
	else {
		return bodies[id]->current->spatialVelocity.SubVec3( 1 );
	}
}

/*
================
idPhysics_AF::ClipTranslation
================
*/
void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const {
	int i;
	idAFBody *body;
	trace_t bodyResults;

	results.fraction = 1.0f;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->clipModel->IsTraceModel() ) {
			if ( model ) {
				gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
									body->clipModel, body->current->worldAxis, body->clipMask,
										model->Handle(), model->GetOrigin(), model->GetAxis() );
			}
			else {
				gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
									body->clipModel, body->current->worldAxis, body->clipMask, self );
			}
			if ( bodyResults.fraction < results.fraction ) {
				results = bodyResults;
			}
		}
	}

	results.endpos = bodies[0]->current->worldOrigin + results.fraction * translation;
	results.endAxis = bodies[0]->current->worldAxis;
}

/*
================
idPhysics_AF::ClipRotation
================
*/
void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const {
	int i;
	idAFBody *body;
	trace_t bodyResults;
	idRotation partialRotation;

	results.fraction = 1.0f;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->clipModel->IsTraceModel() ) {
			if ( model ) {
				gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation,
									body->clipModel, body->current->worldAxis, body->clipMask,
										model->Handle(), model->GetOrigin(), model->GetAxis() );
			}
			else {
				gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation,
									body->clipModel, body->current->worldAxis, body->clipMask, self );
			}
			if ( bodyResults.fraction < results.fraction ) {
				results = bodyResults;
			}
		}
	}

	partialRotation = rotation * results.fraction;
	results.endpos = bodies[0]->current->worldOrigin * partialRotation;
	results.endAxis = bodies[0]->current->worldAxis * partialRotation.ToMat3();
}

/*
================
idPhysics_AF::ClipContents
================
*/
int idPhysics_AF::ClipContents( const idClipModel *model ) const {
	int i, contents;
	idAFBody *body;

	contents = 0;

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];

		if ( body->clipModel->IsTraceModel() ) {
			if ( model ) {
				contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin,
									body->clipModel, body->current->worldAxis, -1,
										model->Handle(), model->GetOrigin(), model->GetAxis() );
			}
			else {
				contents |= gameLocal.clip.Contents( body->current->worldOrigin,
									body->clipModel, body->current->worldAxis, -1, NULL );
			}
		}
	}

	return contents;
}

/*
================
idPhysics_AF::DisableClip
================
*/
void idPhysics_AF::DisableClip( void ) {
	int i;

	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->clipModel->Disable();
	}
}

/*
================
idPhysics_AF::EnableClip
================
*/
void idPhysics_AF::EnableClip( void ) {
	int i;

	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->clipModel->Enable();
	}
}

/*
================
idPhysics_AF::UnlinkClip
================
*/
void idPhysics_AF::UnlinkClip( void ) {
	int i;

	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->clipModel->Unlink();
	}
}

/*
================
idPhysics_AF::LinkClip
================
*/
void idPhysics_AF::LinkClip( void ) {
	UpdateClipModels();
}

/*
================
idPhysics_AF::SetPushed
================
*/
void idPhysics_AF::SetPushed( int deltaTime ) {
	idAFBody *body;
	idRotation rotation;

	if ( bodies.Num() ) {
		body = bodies[0];
		rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation();

		// velocity with which the af is pushed
		current.pushVelocity.SubVec3(0) += ( body->current->worldOrigin - body->saved.worldOrigin ) / ( deltaTime * idMath::M_MS2SEC );
		current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC );
	}
}

/*
================
idPhysics_AF::GetPushedLinearVelocity
================
*/
const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const {
	return current.pushVelocity.SubVec3(0);
}

/*
================
idPhysics_AF::GetPushedAngularVelocity
================
*/
const idVec3 &idPhysics_AF::GetPushedAngularVelocity( const int id ) const {
	return current.pushVelocity.SubVec3(1);
}

/*
================
idPhysics_AF::SetMaster

   the binding is orientated based on the constraints being used
================
*/
void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) {
	int i;
	idVec3 masterOrigin;
	idMat3 masterAxis;
	idRotation rotation;

	if ( master ) {
		self->GetMasterPosition( masterOrigin, masterAxis );
		if ( !masterBody ) {
			masterBody = new idAFBody();
			// translate and rotate all the constraints with body2 == NULL from world space to master space
			rotation = masterAxis.Transpose().ToRotation();
			for ( i = 0; i < constraints.Num(); i++ ) {
				if ( constraints[i]->GetBody2() == NULL ) {
					constraints[i]->Translate( -masterOrigin );
					constraints[i]->Rotate( rotation );
				}
			}
			Activate();
		}
		masterBody->current->worldOrigin = masterOrigin;
		masterBody->current->worldAxis = masterAxis;
	}
	else {
		if ( masterBody ) {
			// translate and rotate all the constraints with body2 == NULL from master space to world space
			rotation = masterBody->current->worldAxis.ToRotation();
			for ( i = 0; i < constraints.Num(); i++ ) {
				if ( constraints[i]->GetBody2() == NULL ) {
					constraints[i]->Rotate( rotation );
					constraints[i]->Translate( masterBody->current->worldOrigin );
				}
			}
			delete masterBody;
			masterBody = NULL;
			Activate();
		}
	}
}


const float	AF_VELOCITY_MAX				= 16000;
const int	AF_VELOCITY_TOTAL_BITS		= 16;
const int	AF_VELOCITY_EXPONENT_BITS	= idMath::BitsForInteger( idMath::BitsForFloat( AF_VELOCITY_MAX ) ) + 1;
const int	AF_VELOCITY_MANTISSA_BITS	= AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS;
const float	AF_FORCE_MAX				= 1e20f;
const int	AF_FORCE_TOTAL_BITS			= 16;
const int	AF_FORCE_EXPONENT_BITS		= idMath::BitsForInteger( idMath::BitsForFloat( AF_FORCE_MAX ) ) + 1;
const int	AF_FORCE_MANTISSA_BITS		= AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS;

/*
================
idPhysics_AF::WriteToSnapshot
================
*/
void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const {
	int i;
	idCQuat quat;

	msg.WriteLong( current.atRest );
	msg.WriteFloat( current.noMoveTime );
	msg.WriteFloat( current.activateTime );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );

	msg.WriteByte( bodies.Num() );

	for ( i = 0; i < bodies.Num(); i++ ) {
		AFBodyPState_t *state = bodies[i]->current;
		quat = state->worldAxis.ToCQuat();

		msg.WriteFloat( state->worldOrigin[0] );
		msg.WriteFloat( state->worldOrigin[1] );
		msg.WriteFloat( state->worldOrigin[2] );
		msg.WriteFloat( quat.x );
		msg.WriteFloat( quat.y );
		msg.WriteFloat( quat.z );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
/*		msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
*/
	}
}

/*
================
idPhysics_AF::ReadFromSnapshot
================
*/
void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) {
	int i, num;
	idCQuat quat;

	current.atRest = msg.ReadLong();
	current.noMoveTime = msg.ReadFloat();
	current.activateTime = msg.ReadFloat();
	current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );

	num = msg.ReadByte();
	assert( num == bodies.Num() );

	for ( i = 0; i < bodies.Num(); i++ ) {
		AFBodyPState_t *state = bodies[i]->current;

		state->worldOrigin[0] = msg.ReadFloat();
		state->worldOrigin[1] = msg.ReadFloat();
		state->worldOrigin[2] = msg.ReadFloat();
		quat.x = msg.ReadFloat();
		quat.y = msg.ReadFloat();
		quat.z = msg.ReadFloat();
		state->spatialVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		state->spatialVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		state->spatialVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		state->spatialVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		state->spatialVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
		state->spatialVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS );
/*		state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
		state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS );
*/
		state->worldAxis = quat.ToMat3();
	}

	UpdateClipModels();
}
/*
================
idPhysics_AF::NearestBodyOrig

Linear search to find the body origin nearest to the given point
================
*/
idVec3 idPhysics_AF::NearestBodyOrig( idVec3 point, int *bodyID )
{
	idVec3 bodyOrigin, delta, returnVec;
	float tempDist;
	float minDist = idMath::INFINITY;
	int numBods(0), returnBody(-1);

	// return (0,0,0) if something unexpected happens
	returnVec.Zero();

	numBods = GetNumBodies();
	for( int i = 0; i < numBods; i++ )
	{
		if( GetBody( i ) )
		{
			bodyOrigin = GetBody( i )->GetWorldOrigin();
			delta = bodyOrigin - point;

			tempDist = delta.LengthSqr();
			if (tempDist < minDist)
			{
				minDist = tempDist;
				returnVec = bodyOrigin;
				returnBody = i;
			}
		}
	}

	if( bodyID )
		*bodyID = returnBody;

	return returnVec;
}

bool idPhysics_AF::HasGroundContactsAtJoint( int id )
{
	int numContacts;
	idAFConstraint_Contact *contacts[5];
	bool bReturnVal(false);
	idEntity *contactEnt(NULL);

	numContacts = GetBodyContactConstraints( id, contacts, 5 );

	for ( int i = 0; i < numContacts; i++ ) 
	{
		contactEnt = gameLocal.entities[ contacts[i]->GetContact().entityNum ];

		if( contacts[i]->GetContact().normal * -gravityNormal > 0.0f
			&& contactEnt && contactEnt != self )
		{
			bReturnVal = true;
			goto Quit;
		}
	}

Quit:
	return bReturnVal;
}

#ifdef MOD_WATERPHYSICS
/*
================
idPhysics_AF::SetLiquidDensity
================
*/
void idPhysics_AF::SetLiquidDensity( float density )
{
	this->liquidDensity = density;
}

/*
================
idPhysics_AF::GetLiquidDensity
================
*/
float idPhysics_AF::GetLiquidDensity() const
{
	return this->liquidDensity;
}

/*
================
idPhysics_AF::SetFixedDensityBuoyancy	
	This will reset the liquid density to the default value depending on the mode.
================
*/
void idPhysics_AF::SetFixedDensityBuoyancy( bool fixed )
{
	this->fixedDensityBuoyancy = fixed;
	if( this->fixedDensityBuoyancy )
		this->liquidDensity = DEFAULT_LIQUID_DENSITY;
	else
		this->liquidDensity = DEFAULT_LIQUID_SCALAR;
}

/*
================
idPhysics_AF::GetFixedDensityBuoyancy
================
*/
bool idPhysics_AF::GetFixedDensityBuoyancy() const
{
	return this->fixedDensityBuoyancy;
}
#endif