doom3/neo/game/physics/Physics_AF.cpp

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

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

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

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

Doom 3 Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

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

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

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

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

#include "../../idlib/precompiled.h"
#pragma hdrstop

#include "../Game_local.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-4f;
const float NO_MOVE_TIME					= 1.0f;
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 IMPULSE_THRESHOLD				= 500.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 );

#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.0f );
	gameRenderWorld->DebugArrow( colorBlue, a2, a2 + s2 * 5.0f, 1.0f );
	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, dir1, dir2;
	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;

	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;
}

/*
================
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::SetDentity: center of mass not at origin for body '%s'", 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();
	}
}

/*
================
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 );

	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 );
	saveFile->WriteVec3( atRestOrigin );
	saveFile->WriteMat3( atRestAxis );
}

/*
================
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 );

	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 );
	saveFile->ReadVec3( atRestOrigin );
	saveFile->ReadMat3( atRestAxis );
}



//===============================================================
//                                                        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;
	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;
	idMat3 axis;

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

		// 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
			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,
											mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() );

			body->fl.spatialInertiaSparse = true;
		}
		else {
			idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass );

			// spatial inertia in world space
			body->I.Set( body->mass * mat3_identity, massMoment,
								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;
	idVec3 vec;
	idAFBody *body;
	idVec6 force;
	idRotation rotation;
	float vSqr, maxLinearVelocity, maxAngularVelocity;
	
	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();

	// 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
		body->next->spatialVelocity.SubVec3(0) -= body->linearFriction * body->next->spatialVelocity.SubVec3(0);
		body->next->spatialVelocity.SubVec3(1) -= 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;
	impactInfo_t info;
	idEntity *ent;

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

	// get info from other entity involved
	ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info );
	// 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 );
	impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal );
	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 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;
	contactInfo_t contactInfo[10];
	idEntity *passEntity;
	idVecX dir( 6, VECX_ALLOCA( 6 ) );

	// 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, 10, 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::ClearExternalForce
================
*/
void idPhysics_AF::ClearExternalForce( void ) {
	int i;
	idAFBody *body;

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

		// clear external force
		body->current->externalForce.Zero();
		body->next->externalForce.Zero();
	}
}

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

	for ( i = 0; i < bodies.Num(); i++ ) {
		body = bodies[i];
		// add gravitational force
		body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
	}
}

/*
================
idPhysics_AF::SwapStates
================
*/
void idPhysics_AF::SwapStates( void ) {
	int i;
	idAFBody *body;
	AFBodyPState_t *swap;

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

		body = bodies[i];

		// swap the current and next state for next simulation step
		swap = body->current;
		body->current = body->next;
		body->next = swap;
	}
}

/*
================
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;
	}

	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
	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;
		}
	}

	// 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]->current->externalForce.Zero();
	}

	self->BecomeInactive( TH_PHYSICS );
}

/*
================
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() ) {
		return bodies[id]->mass;
	}
	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;


	// 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 );

#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
	ClearExternalForce();

	// 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

	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->DrawText( 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];
			gameRenderWorld->DrawText( 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();
		gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
	}

	if ( af_showInertia.GetBool() ) {
		for ( i = 0; i < bodies.Num(); i++ ) {
			body = bodies[i];
			idMat3 &I = body->inertiaTensor;
			gameRenderWorld->DrawText( 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->DrawText( 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->DrawText( 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;

	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;

#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;

	// the articulated figure structure is handled by the owner

	idPhysics_AF_SavePState( saveFile, current );
	idPhysics_AF_SavePState( saveFile, saved );

	saveFile->WriteInt( bodies.Num() );
	for ( i = 0; i < bodies.Num(); i++ ) {
		bodies[i]->Save( saveFile );
	}
	if ( masterBody ) {
		saveFile->WriteBool( true );
		masterBody->Save( saveFile );
	} else {
		saveFile->WriteBool( false );
	}

	saveFile->WriteInt( constraints.Num() );
	for ( i = 0; i < constraints.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 );

	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 );

	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() );
	}

	if ( GetBody( 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\n", id );
		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;
	}
	info->invMass = 1.0f / bodies[id]->mass;
	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;
	bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse;
	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 ) {
	if ( noImpact ) {
		return;
	}
	if ( id < 0 || id >= bodies.Num() ) {
		return;
	}
	bodies[id]->current->externalForce.SubVec3( 0 ) += force;
	bodies[id]->current->externalForce.SubVec3( 1 ) += (point - bodies[id]->current->worldOrigin).Cross( force );
	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++ ) {
		memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) );
	}
}

/*
================
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;

	if ( !worldConstraintsLocked ) {
		// rotate constraints attached to the world
		for ( i = 0; i < constraints.Num(); i++ ) {
			constraints[i]->Rotate( rotation );
		}
	}

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

		body->current->worldOrigin *= rotation;
		body->current->worldAxis *= rotation.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();
}