Renegade/Code/wwphys/rbody.cpp

/*
**	Command & Conquer Renegade(tm)
**	Copyright 2025 Electronic Arts Inc.
**
**	This program 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.
**
**	This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
*/

/***********************************************************************************************
 ***              C O N F I D E N T I A L  ---  W E S T W O O D  S T U D I O S               ***
 ***********************************************************************************************
 *                                                                                             *
 *                 Project Name : WWPhys                                                       *
 *                                                                                             *
 *                     $Archive:: /Commando/Code/wwphys/rbody.cpp                             $*
 *                                                                                             *
 *                       Author:: Greg Hjelstrom                                               *
 *                                                                                             *
 *                     $Modtime:: 3/28/02 11:00a                                              $*
 *                                                                                             *
 *                    $Revision:: 119                                                         $*
 *                                                                                             *
 *---------------------------------------------------------------------------------------------*
 * Functions:                                                                                  *
 *   RigidBodyClass::RigidBodyClass -- Constructor for RigidBodyClass                          *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */


#include "rbody.h"
#include "pscene.h"
#include "boxrobj.h"
#include "physcoltest.h"
#include "physinttest.h"
#include "physcon.h"
#include "octbox.h"
#include "bitstream.h"
#include "persistfactory.h"
#include "simpledefinitionfactory.h"
#include "wwphysids.h"
#include "wwhack.h"
#include "wwprofile.h"
#include "hlod.h"
#include "physcontrol.h"
#include "phys3.h"
#include <stdio.h>


DECLARE_FORCE_LINK(rbody);


#define RBODY_DEBUGGING					0
#define RBODY_DEBUG_FILTER				(stricmp(Model->Get_Name(),"V_GDI_ORCA_M") == 0) && (PhysicsSceneClass::Get_Instance()->Is_Debug_Display_Enabled())

#define JITTER_ELIMINATION_CODE		0


float RigidBodyClass::_CorrectionTime = 1.0f;

/***********************************************************************************************
**
** RBodyHistoryClass Implementation
**
***********************************************************************************************/
																						
/*
** RBodyHistoryClass Parameters
** Control the implementation of the phys3 history tracking system with the following 
** parameters.
*/
const int		RBODY_SNAPSHOT_COUNT = 16;					// must be power of 2!
const float		RBODY_HISTORY_MIN_TIME = 0.75f;			// seconds of history to store

const int		RBODY_SNAPSHOT_MASK = RBODY_SNAPSHOT_COUNT - 1; 
const float		RBODY_SNAPSHOT_INTERVAL = RBODY_HISTORY_MIN_TIME / RBODY_SNAPSHOT_COUNT;

#define RBODYHISTORY_NO_CORRECTION			0
#define RBODYHISTORY_ABSOLUTE_CORRECTION	0
#define RBODYHISTORY_LERP_CORRECTION		1


/**
** RBodyHistoryClass
** This class is used to store a history of the state of a RBody object.  The network
** update code uses this history to intelligently update the state of the object when
** a packet is received.  
*/
class RBodyHistoryClass
{
public:
	RBodyHistoryClass(void);
	~RBodyHistoryClass(void);

	void		Init(const RigidBodyStateStruct & state);
	void		Update_History(const RigidBodyStateStruct & state, float dt);
	void		Compute_Old_State(float dt,RigidBodyStateStruct * set_state);
	void		Apply_Correction(const RigidBodyStateStruct & error,float fraction);

	int		History_Count(void) { return RBODY_SNAPSHOT_COUNT; }
	const RigidBodyStateStruct & Get_Historical_State(int i) { return SnapshotArray[Wrap_Index(HeadIndex + i)]; }
	const Vector3 & Get_Historical_Position(int i) { return SnapshotArray[Wrap_Index(HeadIndex + i)].Position; }

	void		Find_Nearest_State(const RigidBodyStateStruct & input,RigidBodyStateStruct * output);

private:

	int		Wrap_Index(int index) { return (index + RBODY_SNAPSHOT_COUNT) & RBODY_SNAPSHOT_MASK; }
	
	struct StateSnapshotStruct : public RigidBodyStateStruct
	{
	public:
		StateSnapshotStruct(void) : Age(0) { }
		StateSnapshotStruct(const StateSnapshotStruct & that) : RigidBodyStateStruct(that) { Age = that.Age; }
		StateSnapshotStruct & operator = (const StateSnapshotStruct & that) { RigidBodyStateStruct::operator = (that); Age = that.Age; return *this; }
		StateSnapshotStruct & operator = (const RigidBodyStateStruct & that) { RigidBodyStateStruct::operator = (that); Age = 0.0f; return *this; }

		void					Lerp(const StateSnapshotStruct & a, const StateSnapshotStruct & b, float fraction);
		void					Update_Age(float dt) { Age += dt; }

		float					Age;
	};

	StateSnapshotStruct *	SnapshotArray;
	int							HeadIndex;			// history buffer is circular, this is the "head"
};

RBodyHistoryClass::RBodyHistoryClass(void) :
	SnapshotArray(NULL),
	HeadIndex(0)
{
	SnapshotArray = new StateSnapshotStruct[RBODY_SNAPSHOT_COUNT];
}

RBodyHistoryClass::~RBodyHistoryClass(void)
{
	if (SnapshotArray != NULL) {
		delete[] SnapshotArray;
		SnapshotArray = NULL;
	}
}

void RBodyHistoryClass::Init(const RigidBodyStateStruct & state)
{
	int next_older_index = Wrap_Index(HeadIndex + 1);

	SnapshotArray[HeadIndex] = state;
	SnapshotArray[HeadIndex].Age = 0.0f;
	SnapshotArray[next_older_index] = state;
	SnapshotArray[next_older_index].Age = 1000.0f;
}

void RBodyHistoryClass::Update_History(const RigidBodyStateStruct & state, float dt)
{
	int next_older_index = Wrap_Index(HeadIndex + 1);

	/*
	** See if enough time has passed so we need to ratchet forward in the circular buffer
	*/
	if (SnapshotArray[next_older_index].Age + dt > RBODY_SNAPSHOT_INTERVAL) {
		HeadIndex = Wrap_Index(HeadIndex - 1);
		SnapshotArray[HeadIndex].Age = 0;
	}

	SnapshotArray[HeadIndex] = state;

	/*
	** Add age to all existing snapshots
	*/
	for (int i=0; i<RBODY_SNAPSHOT_COUNT; i++) {
		if (i != HeadIndex) {
			SnapshotArray[i].Update_Age(dt);
		}
	}
}

void RBodyHistoryClass::Compute_Old_State(float t,RigidBodyStateStruct * set_state)
{
	WWASSERT(set_state != NULL);
	int index = HeadIndex;
	bool done = false;
	
	while (!done) {
		if (SnapshotArray[index].Age <= t) {
			index = Wrap_Index(index + 1);
			
			/*
			** past the end of our history, just return the oldest known state
			*/
			if (index == HeadIndex) {
				int tail_index = Wrap_Index(HeadIndex - 1);
				*set_state = SnapshotArray[tail_index];
				return;
			}
		
		} else {
			done = true;
		}
	}
	
	int prev_index = Wrap_Index(index - 1);
	int next_index = index;

	float fraction = (t - SnapshotArray[prev_index].Age) / (SnapshotArray[next_index].Age - SnapshotArray[prev_index].Age);
	RigidBodyStateStruct::Lerp(SnapshotArray[prev_index],SnapshotArray[next_index],fraction,set_state);
}

void RBodyHistoryClass::Apply_Correction(const RigidBodyStateStruct & error,float fraction)
{
#if (!RBODY_HISTORY_NO_CORRECTION)
	for (int counter=0; counter<RBODY_SNAPSHOT_COUNT; counter++) {
		int index = Wrap_Index(HeadIndex + counter);

		RigidBodyStateStruct ideal_state = SnapshotArray[index];
		ideal_state.Position += error.Position;
		ideal_state.Orientation = ideal_state.Orientation * error.Orientation;
		ideal_state.LMomentum += error.LMomentum;
		ideal_state.AMomentum += error.AMomentum;

		float lerp_fraction = 0.0f;
#if RBODYHISTORY_ABSOLUTE_CORRECTION
		lerp_fraction = fraction;
#endif
#if RBODYHISTORY_LERP_CORRECTION
		lerp_fraction = fraction * (RBODY_HISTORY_MIN_TIME - SnapshotArray[index].Age) / RBODY_HISTORY_MIN_TIME;
#endif
		RigidBodyStateStruct::Lerp(SnapshotArray[index],ideal_state,lerp_fraction,&(SnapshotArray[index]));
	}
#endif
}

void RBodyHistoryClass::Find_Nearest_State(const RigidBodyStateStruct & input,RigidBodyStateStruct * output)
{
	/*
	** Find the nearest line segment
	*/
	float min_dist = 100000.0f;
	float result_fraction = 0.0f;
	int result_index0 = -1;
	int result_index1 = -1;

	for (int counter=0; counter<RBODY_SNAPSHOT_COUNT-1; counter++) {
		int index0 = Wrap_Index(HeadIndex + counter);
		int index1 = Wrap_Index(index0 + 1);

		LineSegClass segment(SnapshotArray[index0].Position,SnapshotArray[index1].Position);

		Vector3 point = segment.Find_Point_Closest_To(input.Position);
		float dist = (point - input.Position).Length();
		float fraction = 0.0f;
		if (segment.Get_Length() > 0.0f) {
			fraction = (point - segment.Get_P0()).Length() / segment.Get_Length();
		}

#if 0
		/*
		** Ignore points with velocity more than 90 deg away from server vel
		*/
		float vdot = 0.0f;
		Vector3 history_vel;
		Vector3::Lerp(SnapshotArray[index0].Velocity,SnapshotArray[index1].Velocity,fraction,&history_vel);
		vdot = Vector3::Dot_Product(vel,history_vel); 
#endif
		
		if ((dist < min_dist) /*&& (vdot >= 0.0f)*/) {
			min_dist = dist;
			result_fraction = fraction;
			result_index0 = index0;
			result_index1 = index1;
		}
	}
	
	WWASSERT((result_index0 >= 0) && (result_index0 < RBODY_SNAPSHOT_COUNT));
	WWASSERT((result_index1 >= 0) && (result_index1 < RBODY_SNAPSHOT_COUNT));
	WWASSERT(output != NULL);
	RigidBodyStateStruct::Lerp(SnapshotArray[result_index0],SnapshotArray[result_index1],result_fraction,output);
}


void RBodyHistoryClass::StateSnapshotStruct::Lerp(const StateSnapshotStruct & a, const StateSnapshotStruct & b, float fraction)
{
	RigidBodyStateStruct::Lerp(a,b,fraction,this);
	Age = WWMath::Lerp(a.Age,b.Age,fraction);
}
																						
																						
																						

				
/***********************************************************************************************
**
** RigidBodyClass Implementation
**
***********************************************************************************************/
float DEFAULT_CONTACT_THICKNESS	= 0.2f;

#define IMPULSE_COLOR		Vector3(1,0,0)
#define LMOMENTUM_COLOR		Vector3(0,1,0)
#define AMOMENTUM_COLOR		Vector3(0,0,1)

												
/*
** Declare a PersistFactory for RigidBodyClasses
*/
SimplePersistFactoryClass<RigidBodyClass,PHYSICS_CHUNKID_RIGIDBODY>	_RigidBodyFactory;


/*
** Chunk ID's used by RigidBodyClass
*/
enum
{
	RBODY_CHUNK_MOVEABLE							= 0x00800900,
	RBODY_CHUNK_VARIABLES,

	RBODY_VARIABLE_ODESYSTEM_PTR				= 0x00,
	RBODY_VARIABLE_IBODY,
	RBODY_VARIABLE_IBODYINV,
	RBODY_VARIABLE_STATE_POSITION,
	RBODY_VARIABLE_STATE_ORIENTATION,
	RBODY_VARIABLE_STATE_LMOMENTUM,
	RBODY_VARIABLE_STATE_AMOMENTUM,
	RBODY_VARIABLE_CONTACT_STIFFNESS,		// OBSOLETE!
	RBODY_VARIABLE_CONTACT_DAMPING,			// OBSOLETE!
	RBODY_VARIABLE_CONTACT_LENGTH,
};										
						

/*
** Rigid Body Dynamics 
** 
** NOTES:
** - No matter what numerical integration technique I use, it seems that the 
**   orientation drifts.  I have to re-normalize frequently so the higher order 
**   integrators are not very attractive (more frequent calls to Compute_Forces...).  
** - Clamping angular velocity to an artificial maximum seems to cause problems 
**   as well.  I'm not sure why but in the test app, it causes "jumpiness" in
**   the simulation.
** - Impact/Contact plan:  Integrate a new state, ignoring collision but computing
**   penalty forces.  Then break that change in state into a change in orientation
**   along with a change in position.  Try the change in orientation, checking for
**   penetration; if penetration occurs, discard the orientation change.  Try
**   the translation, using the swept OBBox.  Compute impact impluse if contact
**   occurs.  When testing for penetration/impact use the normal collision box.
**   When computing penalty/contact forces use a slightly larger box and collect
**   the points inside the box and the box points "behind" the triangles.
**
** May 3, 1999
** - The idea about breaking the state update into separate orientation and translation
**   updates is completely flawed.  It just doesn't work.  Trying to find a new way
**   of detecting collisions without resorting to the binary search through time!
** - Binary searching for the time-of-collision requires a different sort of collision
**   query.  While iterating, it requires a simpler intersection query.  Once finished
**   iterating (found the TOC) it requires a query that will return what point is
**   causing the collision.
**
** June 25, 1999
** - Completely rigid motion may be less efficient in high-poly environments.  For
**   example, a rigid object sliding along the ground will experience a collision (and
**   therefore rewind the integrator) at every single new non-coplanar triangle.
** - On the other hand, the buggy seems to go over curvy surfaces fine since its springs
**   just adjust over time.  Maybe penalty forces are the way after all...  Going to have
**   to try the two-layer box idea...
** - Updating the integrator to a scheme that can handle multiple systems being combined
**   into a single coupled system.  
**
** August 16, 1999
** New Idea: Two-layer box with TOC searching and penalty based contact forces.  General 
** idea is that we let things interpenetrate their outer boxes but not their inner ones.
**
**   pseudo-code:
**   - collect all rigid bodies into a list (later on, we'll break into multiple lists)
**   - integrate them all, computing penalty or real contact forces at each active contact 
**	  - if interpenetration occurs:
**     - search for TOC
**     - rewind (interpolate) all objects to TOC
**     - find the colliding features on the objects involved (edge->edge or vertex->face)
**     - apply collision impulses to the two colliding objects
**     - add contact if resultant relative velocity is less than epsilon
**
**   Derivative calculations contact, constraint forces:
**   - zero each object's force accumulator (or reset it with gravity)
**   - apply all external forces (springs, wind, water)
**   - iterate over all contacts in the sytem, either putting them into a big contact solver
**     or computing a penalty based on their displacement.
**   - add the contact forces to all of the relavent objects
**   - have each object compute its derivatives and add them to the vector
**
**   Finding point of collision:
**   - binary search time until the objects are within some distance of each other
**   - perform an intersection check at time t+dt and record the polygons/boxes that penetrate
**     the outer skin
**   - perform distance calculations on all of those objects (polygons/boxes) at time t
**   - determine if the closest point is caused by a pair of edges or a vertex hitting a face
**   - at the collision point, compute and apply the necessary collision impulse
**   - if the resultant relative velocity is low enough, add a contact constraint/spring to the system
**
** Sept 23, 1999
** Another new idea: Padded boxes.
** - The collision box for an object has four pads on each face (for a total of 24 pads).  Each
**   of these pads can have one contact point.  
** - During each timestep (and sub-timestep) each pad will push out from its face until it hits something
**   or reaches the edge of the outer box.  If it hits something, a contact will be set up which records
**   the position, normal, velocity, and surface parameters.
** - Normally all contacts could behave like damped springs.
** - If the inner box is penetrated, we could search for a time when the outer is penetrated, the inner
**   isn't, and apply impulses to all incoming contact points.
** - Probably need to do some hierarchical culling to the contact checking, try the entire face and
**   if it hits something, then try the four sub-faces.
**
** Oct 14, 1999
** YAHPI	- Yet Another Hacky Physics Idea: Octant Boxes
** - Divide our collision box into 8 octants and detect one contact per octant.  
** - Contact detection will be achieved by using the box-sweeping code on each octant.
** - Octants start out inside the box and sweep along a diagonal some distance
** - At their starting positions these "octant boxes" need to overlap so that when at their
**   fully extended position there aren't gaps around the model.
** ALSO: a key idea is to *never* binary search for the "real" collision.  We just detect
** the collision, revert state, and divide our momentum down in the hopes that the forces
** will handle everything if we come in slower next frame.  This code will need to be improved...
**
** Oct 19, 1999
** Octant boxes are flawed because the contact point for each octant jitters around on the
** object when you're on flat ground.  Since the boxes overlap, the points can all jitter 
** to one side of the CM and then to another side.  This is not good :-)
** Solution: Add contact "hairs" to the corners of the box and "prefer" them whenever the 
** contact "compression" is close to that of its octant box. 
** Remaining/New Problems:
** - Need to compute parameters for these "oct-boxes" automatically which handle any combination
**   of OBBox + Mass
** - Objects can now "tunnel" need to do an extra collision check to prevent this.
** - Need to test the simulation code at the "worst-case" simulation framerate to see if it still
**   stabilizes...
** - Need to handle object-object collisions.  
**
** Oct 26,1999
** - Good success with the octbox + corner contacts idea.  Use both the corner contact and
**   the octant contact when both are present and the octant is closer than the corner.
** - Also still using the "divide down the momentum" approach when there is a collision.  Now
**   the code increases the divisor each frame that the object is stuck.  Not 100% sure if I
**   like this but it is so much faster than doing all of those binary searches through time...
** - Critically damped or overdamped contact springs make the system too stiff.  I use underdamped
**   springs (0.33*critical) to keep everything numerically stable.
** - Had to re-normalize the orientation on every sub-timestep.  It would be nice if we
**   didn't have to do this.  If the guts of the integrator was rotating the orientation
**   instead of adding to it we probably could avoid this.  
** - A first pass at (fake) friction is implemented but commented out.  I'm currently getting 
**   instability if I turn it on and it also never stops the object on slopes, etc.  
** TODO: write a custom integrator which rotates the orientation rather than doing the normal
** state += derivatives*dt...  Is this possible?  It is for Euler integration but we need
** at least MidPoint to keep the simulation stable.
** TODO: when a collision is detected, move the object to the extremeties of its contacts?  This
** should help smooth out collisions?  Will have to recursively try to move any objects in contact
** with this one too...  Probably will need this recursive "fake move" in order to handle obj-obj
** interaction anyway...  Hmmm, this needs more thought.
** TODO: handle obj-obj interaction :-)
**
** Oct 11, 2001
** - Latency handling network code.  Each packet causes us to compute an error between the 
** nearest point in their history; then in the timestep function we try to correct that error.
*/


/***********************************************************************************************
 * RigidBodyClass::RigidBodyClass -- Constructor for RigidBodyClass                            *
 *                                                                                             *
 * INPUT:                                                                                      *
 *                                                                                             *
 * OUTPUT:                                                                                     *
 *                                                                                             *
 * WARNINGS:                                                                                   *
 *                                                                                             *
 * HISTORY:                                                                                    *
 *   10/15/99   gth : Created.                                                                 *
 *=============================================================================================*/
RigidBodyClass::RigidBodyClass(void) : 
	Box(NULL),
	ContactBox(NULL),
	IBody(1),
	IBodyInv(1),
	Rotation(1),
	IInv(1),
	Velocity(0,0,0),
	AngularVelocity(0,0,0),
	ContactCount(0),
	StickCount(0),
	LastTimestep(0.0f),
	GoToSleepTimer(RBODY_SLEEP_DELAY),
	History(NULL)
{
	State.Position.Set(0,0,0);
	State.Orientation.Make_Identity();
	State.LMomentum.Set(0,0,0);
	State.AMomentum.Set(0,0,0);
	
	LatencyError.Position.Set(0,0,0);
	LatencyError.Orientation.Make_Identity();
	LatencyError.LMomentum.Set(0,0,0);
	LatencyError.AMomentum.Set(0,0,0);

	LastKnownState.Position.Set(0,0,0);
	LastKnownState.Orientation.Make_Identity();
	LastKnownState.LMomentum.Set(0,0,0);
	LastKnownState.AMomentum.Set(0,0,0);

	ContactThickness = DEFAULT_CONTACT_THICKNESS;
}

void RigidBodyClass::Init(const RigidBodyDefClass & def)
{
	MoveablePhysClass::Init(def);

	State.Position.Set(0,0,0);
	State.Orientation.Make_Identity();
	State.LMomentum.Set(0,0,0);
	State.AMomentum.Set(0,0,0);

	LatencyError.Position.Set(0,0,0);
	LatencyError.Orientation.Make_Identity();
	LatencyError.LMomentum.Set(0,0,0);
	LatencyError.AMomentum.Set(0,0,0);

	LastKnownState.Position.Set(0,0,0);
	LastKnownState.Orientation.Make_Identity();
	LastKnownState.LMomentum.Set(0,0,0);
	LastKnownState.AMomentum.Set(0,0,0);

	ContactThickness = DEFAULT_CONTACT_THICKNESS;

	GoToSleepTimer = RBODY_SLEEP_DELAY;
}

RigidBodyClass::~RigidBodyClass(void)
{
	REF_PTR_RELEASE(Box);
	if (ContactBox != NULL) {
		delete ContactBox;
		ContactBox = NULL;
	}
}

const AABoxClass & RigidBodyClass::Get_Bounding_Box(void) const
{
	// TODO: propogate this into Phys?
	WWASSERT(Model);
	return Model->Get_Bounding_Box();
}

const Matrix3D & RigidBodyClass::Get_Transform(void) const
{
	// TODO: propogate this into Phys?
	WWASSERT(Model);
	return Model->Get_Transform();
}

void RigidBodyClass::Set_Transform(const Matrix3D & m)
{
	WWASSERT(Model);

	//TODO: rename this to Teleport!
	//TODO: warp model to desired position and verify that it is non-intersecting
	State.Orientation = Build_Quaternion(m);
	State.Position = m.Get_Translation();
	Model->Set_Transform(m);
	Set_Flag(ASLEEP,false);

	// each time the state changes, update the derived values
	Update_Auxiliary_State();
	Update_Cull_Box();
	Update_Visibility_Status();
}

bool RigidBodyClass::Cast_Ray(PhysRayCollisionTestClass & raytest)
{
	if (Model && Model->Cast_Ray(raytest)) {
		raytest.CollidedPhysObj = this;
		return true;
	}
	return false;
}

bool RigidBodyClass::Cast_AABox(PhysAABoxCollisionTestClass & boxtest)
{
	// only let others collide against what we use to collide with them...
	if (CollisionMath::Collide(boxtest.Box,boxtest.Move,ContactBox->Get_World_Inner_Box(),Vector3(0,0,0),boxtest.Result)) {
		boxtest.CollidedPhysObj = this;
		boxtest.CollidedRenderObj = Box;
		return true;
	}
	return false;
}

bool RigidBodyClass::Cast_OBBox(PhysOBBoxCollisionTestClass & boxtest)
{
	// only let others collide against what we use to collide with them...
	if (CollisionMath::Collide(boxtest.Box,boxtest.Move,ContactBox->Get_World_Inner_Box(),Vector3(0,0,0),boxtest.Result)) {
		boxtest.CollidedPhysObj = this;
		boxtest.CollidedRenderObj = Box;
		return true;
	}
	return false;
}


bool RigidBodyClass::Intersection_Test(PhysAABoxIntersectionTestClass & test)
{
	if (CollisionMath::Intersection_Test(ContactBox->Get_World_Inner_Box(),test.Box)) {
		test.Add_Intersected_Object(this);
		return true;
	} 
	return false;
}


bool RigidBodyClass::Intersection_Test(PhysOBBoxIntersectionTestClass & test)
{
	if (CollisionMath::Intersection_Test(ContactBox->Get_World_Inner_Box(),test.Box)) {
		test.Add_Intersected_Object(this);
		return true;
	}
	return false;
}


bool RigidBodyClass::Intersection_Test(PhysMeshIntersectionTestClass & test)
{
	// flip the test around and test our OBBox against the given mesh...
	OBBoxIntersectionTestClass our_test(ContactBox->Get_World_Inner_Box(),test.CollisionType);
	if (test.Mesh->Intersect_OBBox(our_test)) {
		test.Add_Intersected_Object(this);
		return true;
	}
	return false;
}

bool RigidBodyClass::Can_Teleport(const Matrix3D &new_tm, bool check_dyn_only,NonRefPhysListClass * result_list)
{
	bool intersecting = false;

	if (ContactBox) {
		ContactBox->Set_Transform(new_tm);
		intersecting = ContactBox->Is_Intersecting(result_list,!check_dyn_only,true);
		ContactBox->Set_Transform(Get_Transform());

	}
	return !intersecting;
}

bool RigidBodyClass::Can_Teleport_And_Stand(const Matrix3D &new_tm, Matrix3D *out)
{
	*out = new_tm;
	return Can_Teleport(new_tm);
}

void RigidBodyClass::Set_Model(RenderObjClass * model)
{
	MoveablePhysClass::Set_Model(model);
	Update_Cached_Model_Parameters();
}

void RigidBodyClass::Update_Cached_Model_Parameters(void)
{
	// if we don't have a model yet, just return
	if (Model == NULL) return;

	Set_Transform(Model->Get_Transform());

	// cache a pointer to the collision box...
	REF_PTR_RELEASE(Box);
	
	// Try to get the "WorldBox" from the model
	if (Model) {
		RenderObjClass * obj = Model->Get_Sub_Object_By_Name("WorldBox");
		if (obj && obj->Class_ID() == RenderObjClass::CLASSID_OBBOX) {
			REF_PTR_SET(Box,(OBBoxRenderObjClass *)obj);
		}
		REF_PTR_RELEASE(obj);

		// If we didn't finde WorldBox, try to find the LOD named "WorldBox" 
		// The LOD code generates a unique name for the mesh by appending A,B,C, etc to the name.
		// A is the lowest LOD, B is the next, and so on.  Our worldbox is specified in the highest
		// LOD so we have to construct the name by appending 'A'+LodCount to the name... icky
		if ((Box == NULL) && (Model->Class_ID() == RenderObjClass::CLASSID_HLOD)) {
			
			char namebuffer[64];
			sprintf(namebuffer,"WorldBox%c",'A' + ((HLodClass *)Model)->Get_Lod_Count() - 1);

			obj = Model->Get_Sub_Object_By_Name(namebuffer);
			if (obj && obj->Class_ID() == RenderObjClass::CLASSID_OBBOX) {
				REF_PTR_SET(Box,(OBBoxRenderObjClass *)obj);
			}
			REF_PTR_RELEASE(obj);
		}
	}
	
	// Otherwise just create one
	if (Box == NULL) {
		WWDEBUG_SAY(("Missing WorldBox in model: %s\r\n",Model->Get_Name()));
		Box = new OBBoxRenderObjClass(OBBoxClass(Vector3(0,0,0),Vector3(1,1,1)));
		Box->Set_Collision_Type(COLLISION_TYPE_PHYSICAL);
	}

	// Update our contact box
	Matrix3D tm = Get_Transform();
	Model->Set_Transform(Matrix3D(1));

	if (ContactBox != NULL) {
		delete ContactBox;
	}
	ContactBox = new OctBoxClass(*this,Box->Get_Box());
	ContactBox->Set_Thickness(ContactThickness);
	ContactBox->Update_Contact_Parameters();

	Model->Set_Transform(tm);
	ContactBox->Set_Transform(tm);

	// recompute our inertia tensor
	Compute_Inertia();

	// and update our auxiliary state (inertia effects it)
	Update_Auxiliary_State();

	// update our culling box
	Update_Cull_Box();
}

void RigidBodyClass::Get_Velocity(Vector3 * set_vel) const
{
	*set_vel = Velocity;
	Assert_State_Valid();
}

void RigidBodyClass::Get_Angular_Velocity(Vector3 * set_avel) const
{
	*set_avel = AngularVelocity;
	Assert_State_Valid();
}

void RigidBodyClass::Set_Velocity(const Vector3 & newvel)
{
	Assert_State_Valid();

#ifdef WWDEBUG 
	if (newvel.Is_Valid() != true) {
		WWDEBUG_SAY(("someone set an invalid velocity (%8.3f, %8.3f, %8.3f)\r\n",newvel.X,newvel.Y,newvel.Z));
	}
#endif

	Velocity = newvel;
	State.LMomentum = Velocity * Mass;

	Assert_State_Valid();
}

void RigidBodyClass::Set_Angular_Velocity(const Vector3 & newavel)
{
	WWASSERT(WWMath::Is_Valid_Float(newavel.X));
	WWASSERT(WWMath::Is_Valid_Float(newavel.Y));
	WWASSERT(WWMath::Is_Valid_Float(newavel.Z));

#ifdef WWDEBUG 
	if (newavel.Is_Valid() != true) {
		WWDEBUG_SAY(("someone set an invalid angular velocity (%8.3f, %8.3f, %8.3f)\r\n",newavel.X,newavel.Y,newavel.Z));
	}
#endif

	AngularVelocity = newavel;
	Matrix3 I = IInv.Inverse();
	State.AMomentum = I * newavel;
	Assert_State_Valid();
}

void RigidBodyClass::Network_Interpolate_State_Update
(
	const Vector3 & new_pos,
	const Quaternion & new_orientation,
	const Vector3 & new_vel,
	const Vector3 & new_avel,
	float fraction
)
{
	// If we are severely out of sync with the network update, we have to "pop"
	// to the new state.  If needed, we can add more criteria here in the future...
	float characteristic_length2 = 4.0f * ContactBox->Get_Extent_Length2();
	if ((new_pos - State.Position).Length2() > characteristic_length2) {
		fraction = 1.0f;
	}

	// interpolate a new state
	Vector3::Lerp(State.Position,new_pos,fraction,&(State.Position));
	::Fast_Slerp(State.Orientation,State.Orientation,new_orientation,fraction);
	
	Vector3 vel,avel;
	Vector3::Lerp(Velocity,new_vel,fraction,&vel);
	Vector3::Lerp(AngularVelocity,new_avel,fraction,&avel);

	Set_Velocity(vel);
	Set_Angular_Velocity(avel);
	Assert_State_Valid();

	// clear the latency error since we are not using it
	LatencyError.Reset();
	
	// each time the state changes, update the derived values
	Update_Auxiliary_State();
	Model->Set_Transform(Matrix3D(Rotation,State.Position));
	Update_Cull_Box();
	Update_Visibility_Status();
}


void RigidBodyClass::Network_Latency_State_Update
(			
	const Vector3 & new_pos,
	const Quaternion & new_orientation,
	const Vector3 & new_vel,
	const Vector3 & new_avel
)
{
	OBBoxClass debug_box;
	ContactBox->Get_Inner_Box(&debug_box,new_orientation,new_pos);
	DEBUG_RENDER_OBBOX(debug_box,Vector3(1.0f,0,0),0.3f);

	/*
	** Set up the input state
	*/
	LastKnownState.Position = new_pos;
	LastKnownState.Orientation = new_orientation;
	LastKnownState.LMomentum = new_vel * Mass;
	
	Matrix3 I = IInv.Inverse();
	LastKnownState.AMomentum = I * new_avel;

	/*
	** Allocate a history object if needed
	*/
	if (History == NULL) {
		History = new RBodyHistoryClass;
		History->Init(LastKnownState);
	}
	WWASSERT(History != NULL);

	/*
	** Search our history to find the point nearest this server update
	*/
	RigidBodyStateStruct nearest_state;
	History->Find_Nearest_State(LastKnownState,&nearest_state);

	/*
	** Now, compute the correction to apply to our current state
	*/
	LatencyError.Position = LastKnownState.Position - nearest_state.Position;
	LatencyError.Orientation = LastKnownState.Orientation / nearest_state.Orientation;
	LatencyError.LMomentum = LastKnownState.LMomentum - nearest_state.LMomentum;
	LatencyError.AMomentum= LastKnownState.AMomentum - nearest_state.AMomentum;
}


void RigidBodyClass::Network_Latency_Error_Correction(float dt)
{
	if (LatencyError.Position.Length2() > 0.01f) {

#ifdef WWDEBUG
		if (!LatencyError.Is_Valid()) {
			WWDEBUG_SAY(("RigidBodyClass - Invalid Latency Error!\r\n"));
			WWDEBUG_SAY((" position error: %f, %f, %f\r\n",LatencyError.Position.X,LatencyError.Position.Y,LatencyError.Position.Z));
			WWDEBUG_SAY((" orientation error: %f, %f, %f, %f\r\n",LatencyError.Orientation.X, LatencyError.Orientation.Y,LatencyError.Orientation.Z,LatencyError.Orientation.W));
			WWDEBUG_SAY((" L momentum error: %f, %f, %f\r\n",LatencyError.LMomentum.X,LatencyError.LMomentum.Y,LatencyError.LMomentum.Z));
			WWDEBUG_SAY((" A momentum error: %f, %f, %f\r\n",LatencyError.AMomentum.X,LatencyError.AMomentum.Y,LatencyError.AMomentum.Z));
		}
#endif
		if (!LatencyError.Is_Valid()) {
			LatencyError.Reset();
		}

		/*
		** Compute the theoretically fully corrected state
		*/
		RigidBodyStateStruct ideal_state(State);
		ideal_state.Position += LatencyError.Position;
		ideal_state.Orientation = ideal_state.Orientation * LatencyError.Orientation;
		ideal_state.LMomentum += LatencyError.LMomentum;
		ideal_state.AMomentum += LatencyError.AMomentum;

#if 0
	WWDEBUG_SAY(("Rigid Body %s network correction\r\n",Model->Get_Name()));
	WWDEBUG_SAY(("                               position error: %f\r\n",LatencyError.Position.Length()));
	WWDEBUG_SAY(("                               orientation error: %f\r\n",WWMath::Acos(LatencyError.Orientation.W) * 2.0f));
#endif

		/*
		** Decide whether to do the normal smooth correction or to pop
		** We'll pop whenever the position error is large and the 
		** velocity sent from the server is small.
		*/
		const float POP_POSITION_ERROR2 = 3.0f * 3.0f;
		const float POP_MAX_VELOCITY2 = 0.5f * 0.5f;
		const float POP_ANGLE_ERROR = 0.966f;				// cos(theta/2) for 30 degrees

		bool pop = (	(LatencyError.Position.Length2() > POP_POSITION_ERROR2) &&
							((LastKnownState.LMomentum * MassInv).Length2() < POP_MAX_VELOCITY2));

		float cos_half_theta = LatencyError.Orientation.W;

		if (WWMath::Fabs(cos_half_theta) < POP_ANGLE_ERROR) {
			pop = true;			
		}

		if (pop == true) {

			/*
			** Jump to the last given state
			*/
			State = LastKnownState;
			History->Init(LastKnownState);

			/*
			** Clear the latency error
			*/
			LatencyError.Position.Set(0,0,0);
			LatencyError.Orientation.Set(0,0,0);
			LatencyError.LMomentum.Set(0,0,0);
			LatencyError.AMomentum.Set(0,0,0);

		} else {

			/*
			** Blend a fraction of this "ideal" state into the current state
			*/
			float fraction = 0.1f;
			if (_CorrectionTime <= dt) {
				fraction = 1.0f;
			} else {
				fraction = WWMath::Clamp(dt / _CorrectionTime);
			}
			fraction = WWMath::Clamp(fraction,0.0f,0.5f);

			RigidBodyStateStruct::Lerp(State,ideal_state,fraction,&State);
			History->Apply_Correction(LatencyError,fraction);

			/*
			** Recalculate the remaining error
			*/
			LatencyError.Position = ideal_state.Position - State.Position;
			LatencyError.Orientation = ideal_state.Orientation / State.Orientation;
			LatencyError.LMomentum = ideal_state.LMomentum - State.LMomentum;
			LatencyError.AMomentum = ideal_state.AMomentum - State.AMomentum;
		}

		/*
		** each time the state changes, update the derived values
		*/
		Assert_State_Valid();
		Update_Auxiliary_State();
		Model->Set_Transform(Matrix3D(Rotation,State.Position));
		Update_Cull_Box();
		Update_Visibility_Status();
	}
}


void RigidBodyClass::Apply_Impulse(const Vector3 & imp)
{
	// Impluse applied to center of mass simply adds to the linear momentum
	State.LMomentum += imp;
	Velocity = State.LMomentum * MassInv;

	if (Is_Asleep()) {
		Set_Flag(ASLEEP,false);
	}

	Assert_State_Valid();
	DEBUG_RENDER_VECTOR(State.Position,imp,IMPULSE_COLOR);
}

void RigidBodyClass::Apply_Impulse(const Vector3 & imp, const Vector3 & wpos)
{
	// Impluse applied off center, adds to the linear momentum and also
	// adds to the torque.
	Vector3 aimp;
	Vector3::Cross_Product((wpos - State.Position),imp,&aimp);
	
	State.LMomentum += imp;
	State.AMomentum += aimp;

	Velocity = MassInv * State.LMomentum;
	AngularVelocity = IInv * State.AMomentum;

	if (Is_Asleep()) {
		Set_Flag(ASLEEP,false);
	}

	Assert_State_Valid();
#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("Impulse applied: %10.10f %10.10f %10.10f\r\n",imp.X,imp.Y,imp.Z));
		DEBUG_RENDER_VECTOR(wpos,imp,IMPULSE_COLOR);
	}
#endif
}

void RigidBodyClass::Set_Mass(float mass)
{
	// In this function, we need to update the inertia tensor and
	// adjust the linear and angular momentum so that the object
	// retains the same velocity and angular velocity.
	Vector3 vel,avel;
	Get_Velocity(&vel);
	Get_Angular_Velocity(&avel);

	MoveablePhysClass::Set_Mass(mass);
	Compute_Inertia();
	Update_Auxiliary_State();

	Set_Velocity(vel);
	Set_Angular_Velocity(avel);

	ContactBox->Update_Contact_Parameters();
	Assert_State_Valid();
}

void RigidBodyClass::Get_Inertia_Inv(Matrix3 * set_I_inv)
{
	*set_I_inv = IInv;
}

void RigidBodyClass::Set_Inertia(const Matrix3 & I)
{
	Vector3 vel,avel;
	Get_Velocity(&vel);
	Get_Angular_Velocity(&avel);

	IBody = I;
	IBodyInv = IBody.Inverse();
	Update_Auxiliary_State();

	Set_Velocity(vel);
	Set_Angular_Velocity(avel);
}

void RigidBodyClass::Get_Inertia(Matrix3 * I)
{
	WWASSERT(I);
	*I = IBody;
}

void RigidBodyClass::Set_Contact_Parameters(float length)
{
	ContactThickness = length;
	ContactBox->Set_Thickness(ContactThickness);
}

void RigidBodyClass::Get_Contact_Parameters(float * stiffness,float * damping,float * length)
{
	if (stiffness) {
		if (ContactBox) {
			*stiffness = ContactBox->Get_Stiffness();
		} else {
			*stiffness = 0.0f;
		}
	}
	if (damping) {
		if (ContactBox) {
			*damping = ContactBox->Get_Damping();
		} else {
			*damping = 0.0f;
		}
	}
	if (length) {
		*length = ContactThickness;
	}
}



int RigidBodyClass::Compute_Derivatives
(
	float						t,
	StateVectorClass *	test_state,
	StateVectorClass *	dydt,
	int						index
)
{
	if (test_state) {
		Set_State(*test_state,index);
	}

#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("  compute_derivatives: t = %f\r\n",t));
		Dump_State();
	}
#endif

	// time derivitive of position
	(*dydt)[index++] = Velocity[0];
	(*dydt)[index++] = Velocity[1];
	(*dydt)[index++] = Velocity[2];

	// time derivitive of orientation
	Quaternion avel(AngularVelocity.X,AngularVelocity.Y,AngularVelocity.Z,0.0f);
	Quaternion qdot = 0.5 * avel * State.Orientation;
	(*dydt)[index++] = qdot[0];
	(*dydt)[index++] = qdot[1];
	(*dydt)[index++] = qdot[2];
	(*dydt)[index++] = qdot[3];

	// time derivitives of momentum and angular momentum (a.k.a. force and torque)
	Vector3 force(0,0,0);
	Vector3 torque(0,0,0);
	Compute_Force_And_Torque(&force,&torque);

	WWASSERT(force.Is_Valid());
	WWASSERT(torque.Is_Valid());
#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("Force:		%10.10f , %10.10f , %10.10f\r\n",force[0],force[1],force[2]));
		WWDEBUG_SAY(("Torque:	%10.10f , %10.10f , %10.10f\r\n",torque[0],torque[1],torque[2]));
	}
#endif
	
	(*dydt)[index++] = force[0];
	(*dydt)[index++] = force[1];
	(*dydt)[index++] = force[2];

	(*dydt)[index++] = torque[0];
	(*dydt)[index++] = torque[1];
	(*dydt)[index++] = torque[2];
	
#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("  done. \r\n\r\n"));
	}
#endif
	return index;
}

void RigidBodyClass::Get_State(StateVectorClass & set_state)
{
	State.To_Vector(set_state);
}

int RigidBodyClass::Set_State(const StateVectorClass & new_state,int index)
{
	WWPROFILE("RBody::Set_State");
	index = State.From_Vector(new_state,index);
	Update_Auxiliary_State();
	return index;
}

void RigidBodyClass::Set_State(const RigidBodyStateStruct & new_state)
{
	State = new_state;
	Update_Auxiliary_State();
}

void RigidBodyClass::Integrate(float time)
{
	Assert_State_Valid();

	//IntegrationSystem::Euler_Integrate(this,time);
	IntegrationSystem::Midpoint_Integrate(this,time);
	//IntegrationSystem::Runge_Kutta_Integrate(this,time);

	// normalize the orientation since it slowly drifts due to integrator error
	State.Orientation.Normalize();

	Update_Auxiliary_State();
	Assert_State_Valid();
}

void RigidBodyClass::Compute_Inertia(void)
{
	// I'm assuming that the CM is at the origin and the principal axes of inertia
	// are aligned with the coordinate system.  So I'm approximating I by using the 
	// formula for a box which extends both direction in the maximum that the actual
	// box entends in either...
	IBody.Make_Identity();

	if (Box) {
		AABoxClass objbox;
		Box->Get_Obj_Space_Bounding_Box(objbox);
		float dx = 2.0f * objbox.Extent.X + WWMath::Fabs(objbox.Center.X);
		float dy = 2.0f * objbox.Extent.Y + WWMath::Fabs(objbox.Center.Y);
		float dz = 2.0f * objbox.Extent.Z + WWMath::Fabs(objbox.Center.Z);
		IBody[0][0] = (1.0f / 12.0f) * Mass * (dz*dz + dy*dy);
		IBody[1][1] = (1.0f / 12.0f) * Mass * (dx*dx + dz*dz);
		IBody[2][2] = (1.0f / 12.0f) * Mass * (dx*dx + dy*dy);
	}
	IBodyInv = IBody.Inverse();
}

void RigidBodyClass::Update_Auxiliary_State(void)
{
	WWPROFILE("Rbody::Assert_State_Valid");
	Assert_State_Valid();

	State.Orientation.Normalize();

	// compute Rotation,IInv,Velocity,AngularVelocity
	Rotation = Build_Matrix3(State.Orientation);
	IInv = Rotation * IBodyInv * Rotation.Transpose();
	Velocity = MassInv * State.LMomentum;
	AngularVelocity = IInv * State.AMomentum;
	if (ContactBox) {
		ContactBox->Set_Transform(State.Orientation,State.Position);
	}

	Assert_State_Valid();
}

void RigidBodyClass::Compute_Force_And_Torque(Vector3 * force,Vector3 * torque)
{
	WWPROFILE("RigidBodyClass::Compute_Force_And_Torque");

	// NOTE: derived classes should perform their force calculations first and
	// then call us so because contact forces are computed at the end of
	// this routine.
	
	// NOTE: need to optimize this routine!

	// add in gravity
	*force += GravScale * Mass * PhysicsConstants::GravityAcceleration;


	// Add damping
	Vector3 vel_dir = Velocity;
	if (vel_dir.Length2() > WWMATH_EPSILON) {
		vel_dir.Normalize();
		
// DEBUG DEBUG
#pragma message ("(gth) HACK! zeroing bad velocities.")
const float MAX_VEL = 500.0f;
const float MAX_ACCEL = 100.0f;
if (	(Velocity.Is_Valid() == false) || 
		(force->Is_Valid() == false) ||
		(Velocity.Length2() > MAX_VEL * MAX_VEL) ||
		(force->Length() * MassInv > MAX_ACCEL) )
{
	WWDEBUG_SAY(("clearing velocity, model=%s vel=(%f,%f,%f)\r\n",Model->Get_Name(),Velocity.X,Velocity.Y,Velocity.Z));
	Velocity.Set(0,0,0);
	AngularVelocity.Set(0,0,0);
	State.LMomentum.Set(0,0,0);
	State.AMomentum.Set(0,0,0);
	force->Set(0,0,0);
	torque->Set(0,0,0);
	vel_dir.Set(0,0,0);
}

		RigidBodyDefClass * def = Get_RigidBodyDef();
		*force -= def->AerodynamicDragCoefficient * Vector3::Dot_Product(Velocity,Velocity) * vel_dir;

		WWASSERT(force->Is_Valid());
	}

	Vector3 a_dir = AngularVelocity;

// DEBUG DEBUG
#pragma message ("(gth) HACK! zeroing bad angular velocities.")
const float MAX_AVEL = 5.0f * 2.0f * WWMATH_PI;
if (a_dir.Length2() > MAX_AVEL * MAX_AVEL) {
	WWDEBUG_SAY(("clearing angluar velocity, model=%s avel=(%f,%f,%f)\r\n",Model->Get_Name(),AngularVelocity.X,AngularVelocity.Y,AngularVelocity.Z));
	Velocity.Set(0,0,0);
	AngularVelocity.Set(0,0,0);
	State.LMomentum.Set(0,0,0);
	State.AMomentum.Set(0,0,0);
	force->Set(0,0,0);
	torque->Set(0,0,0);
	a_dir.Set(0,0,0);
}

	if (a_dir.Length2() > WWMATH_EPSILON) {
		a_dir.Normalize();
		WWASSERT((a_dir.Length() - 1.0f) < WWMATH_EPSILON);
		*torque -= PhysicsConstants::AngularDamping * Vector3::Dot_Product(AngularVelocity,AngularVelocity) * a_dir;
		WWASSERT(torque->Is_Valid());
	}

	// TODO: boyancy forces, force fields
	// Detect contacts.
	if (Get_RigidBodyDef()->CollisionDisabled == false) {

		ContactBox->Compute_Contacts(false);
		
		for (int i=0; i<ContactBox->Get_Contact_Count(); i++) {

			const CastResultStruct & contact = ContactBox->Get_Contact(i);
			Vector3 r;
			Vector3::Subtract(contact.ContactPoint,State.Position,&r);

			/*
			** Compute Contact Force
			*/
			Vector3 pdot;
			Compute_Point_Velocity(contact.ContactPoint,&pdot);

			float dx = 1.0f - contact.Fraction;
			float dv = Vector3::Dot_Product(pdot,contact.Normal);			
			
			float force_magnitude = ContactBox->Get_Stiffness()*dx - ContactBox->Get_Damping()*dv;
			WWASSERT(WWMath::Is_Valid_Float(force_magnitude));
			Vector3 contact_force = force_magnitude * contact.Normal;
			
			Vector3 contact_torque;
			Vector3::Cross_Product(r,contact_force,&contact_torque);

#ifdef WWDEBUG
			if ((contact_force.Is_Valid() == false) || (contact_torque.Is_Valid() == false)) {
				WWDEBUG_SAY(("bad contact: normal=(%8.3f,%8.3f,%8.3f) r=(%8.3f,%8.3f,%8.3f) dx=%8.3f dv=%8.3f\n",
					contact.Normal.X,contact.Normal.Y,contact.Normal.Z,r.X,r.Y,r.Z,dx,dv));
				contact_force.Set(0,0,0);
				contact_torque.Set(0,0,0);
			}
#endif
	
			/*
			** If we are contacting a phys3 object, push it away from us.  If we
			** can't push it away, then exert a contact force.
			*/
			Phys3Class * p3obj = ContactBox->Peek_Contacted_Object(i)->As_Phys3Class();
			bool resolved = false;
			if (p3obj != NULL) {
				resolved = Push_Phys3_Object_Away(p3obj,contact);
			}
			if (resolved == false) {
				*force += contact_force;
				*torque += contact_torque;
			}

			/*
			** Friction/Drag to bring the object to rest when contacting the ground
			*/
			int contact_count = ContactBox->Get_Contact_Count();
			float contact_weight = Get_Weight() / contact_count;
			Vector3 gravity(0.0f,0.0f,-contact_weight);
			
			if (Get_Flag(FRICTION_DISABLED) == false) {
			
				Vector3 tan_vel = pdot - Vector3::Dot_Product(pdot,contact.Normal)*contact.Normal;
				float tan_vel_magnitude = tan_vel.Length2();

				float friction_coefficient =	PhysicsConstants::Get_Contact_Friction_Coefficient(
														PhysicsConstants::DYNAMIC_OBJ_TYPE_TIRE,
														contact.SurfaceType	);

				if (tan_vel_magnitude > WWMATH_EPSILON * WWMATH_EPSILON) {
					
					/*
					** Friction force points opposite the point's tangential motion
					*/
					tan_vel_magnitude = WWMath::Sqrt(tan_vel_magnitude);
					Vector3 tan_vel_dir = tan_vel / tan_vel_magnitude;
					
					/*
					** The magnitude is the friction coefficient times the portion of the weight supported
					** by this contact.  As the velocity approaches zero, this force approaches zero
					*/
					float friction_magnitude = friction_coefficient * WWMath::Min(0.1f * tan_vel_magnitude * contact_weight,contact_weight);
					Vector3 friction_force = -friction_magnitude * tan_vel_dir;

					/*
					** The active contacts divy up the gravitational force to oppose
					*/
					if (contact.Normal.Z > 0.0f) {
						friction_force -= gravity - Vector3::Dot_Product(gravity,contact.Normal) * contact.Normal;
					}

					/*
					** Finally, clamp the force to the friction circle
					*/
					float max_friction_force = friction_coefficient * contact_weight;
					if (friction_force.Length2() > max_friction_force * max_friction_force) {  
						float flen = friction_force.Length();
						friction_force /= flen;
						friction_force *= 0.5f * max_friction_force;
					}

					/*
					** Compute the torque and add the force and torque into the accumulators
					*/
					Vector3 friction_torque;
					Vector3::Cross_Product(r,friction_force,&friction_torque);

					*force += friction_force;
					*torque += friction_torque;
					DEBUG_RENDER_VECTOR(contact.ContactPoint,friction_force,Vector3(1,0,0));
				}
			}

			DEBUG_RENDER_VECTOR(contact.ContactPoint,contact_force / Get_Mass(),Vector3(0,1,0));
		}
	}

	/*
	** HACK!  Never let the force or torque completely reflect the angular or linear momentum
	** clamp them to a value that at most, drives the momentum to zero
	**
	** NOTE: this doesn't work, I'm just leaving it here in case I get a new idea
	** which will make it work...
	*/
#if JITTER_ELIMINATION_CODE
	Vector3 max_delta_lmomentum = - 1.0f * State.LMomentum / LastTimestep;
	Vector3 max_delta_amomentum = - 1.0f * State.AMomentum / LastTimestep;
	
	Vector3 old_force = *force;
	Vector3 old_torque = *torque;

	if ((max_delta_lmomentum.X < 0) && (force->X < max_delta_lmomentum.X)) force->X = max_delta_lmomentum.X;
	if ((max_delta_lmomentum.X > 0) && (force->X > max_delta_lmomentum.X)) force->X = max_delta_lmomentum.X;
	if ((max_delta_lmomentum.Y < 0) && (force->Y < max_delta_lmomentum.Y)) force->Y = max_delta_lmomentum.Y;
	if ((max_delta_lmomentum.Y > 0) && (force->Y > max_delta_lmomentum.Y)) force->Y = max_delta_lmomentum.Y;
	if ((max_delta_lmomentum.Z < 0) && (force->Z < max_delta_lmomentum.Z)) force->Z = max_delta_lmomentum.Z;
	if ((max_delta_lmomentum.Z > 0) && (force->Z > max_delta_lmomentum.Z)) force->Z = max_delta_lmomentum.Z;
	
	if ((max_delta_amomentum.X < 0) && (torque->X < max_delta_amomentum.X)) torque->X = max_delta_amomentum.X;
	if ((max_delta_amomentum.X > 0) && (torque->X > max_delta_amomentum.X)) torque->X = max_delta_amomentum.X;
	if ((max_delta_amomentum.Y < 0) && (torque->Y < max_delta_amomentum.Y)) torque->Y = max_delta_amomentum.Y;
	if ((max_delta_amomentum.Y > 0) && (torque->Y > max_delta_amomentum.Y)) torque->Y = max_delta_amomentum.Y;
	if ((max_delta_amomentum.Z < 0) && (torque->Z < max_delta_amomentum.Z)) torque->Z = max_delta_amomentum.Z;
	if ((max_delta_amomentum.Z > 0) && (torque->Z > max_delta_amomentum.Z)) torque->Z = max_delta_amomentum.Z;

	WWASSERT(1.00001f * old_force.Length2() >= force->Length2());
	WWASSERT(1.00001f * old_torque.Length2() >= torque->Length2());
#endif

	WWASSERT(WWMath::Is_Valid_Float(force->X));
	WWASSERT(WWMath::Is_Valid_Float(force->Y));
	WWASSERT(WWMath::Is_Valid_Float(force->Z));

}

void RigidBodyClass::Compute_Point_Velocity(const Vector3 & p,Vector3 * pdot)
{
	// REMEMBER: p is assumed to be in world coordinates!  pdot is as well.
	// pdot = velocity + CROSS(angular_velocity , r)
	Vector3 r;
	
	Vector3::Subtract(p,State.Position,&r);
	Vector3::Cross_Product(AngularVelocity,r,pdot);
	Vector3::Add(*pdot,Velocity,pdot);
}

bool RigidBodyClass::Is_Colliding(const Vector3 & point, const Vector3 & normal)
{
	Vector3 padot;
	Compute_Point_Velocity(point,&padot);
	float vrel = Vector3::Dot_Product(normal,padot);
	return vrel < 0.0f;
}

void RigidBodyClass::Set_Moving_Collision_Region(float dt)
{
	AABoxClass region;
	ContactBox->Get_Outer_Bounds(&region);		// start with bounds of collision box

	// Recenter and enlarge to contain our velocity (scaled by a bit)
	Vector3 move = 1.5f * Velocity * dt;
	region.Center += 0.5f * move;
	region.Extent.X += WWMath::Fabs(move.X);
	region.Extent.Y += WWMath::Fabs(move.Y);
	region.Extent.Z += WWMath::Fabs(move.Z);

	// Now, scale to account for any rotational effects
	region.Extent *= 2.0f;

	PhysicsSceneClass::Get_Instance()->Set_Collision_Region(region,Get_Collision_Group());
}

void RigidBodyClass::Set_Stationary_Collision_Region(void)
{
	AABoxClass region;
	ContactBox->Get_Outer_Bounds(&region);		// start with bounds of collision box
	PhysicsSceneClass::Get_Instance()->Set_Collision_Region(region,Get_Collision_Group());
}


bool RigidBodyClass::Can_Go_To_Sleep(float dt)
{
	/*
	** RigidBodies go to sleep if their oct-box is resting on at least three contacts and
	** their velocities are below some thresh-hold and their controller isn't doing anything.
	*/
	if ((Controller != NULL) && (Controller->Is_Inactive() != true)) {
		GoToSleepTimer = RBODY_SLEEP_DELAY;
		return false;
	}
	
	const float VEL_THRESHHOLD = 0.05f;
	const float AVEL_THRESHHOLD = 0.05f;

	float max_lmomentum2 = Mass * VEL_THRESHHOLD * VEL_THRESHHOLD;
	float max_amomentum2 = IBody[1][1] * AVEL_THRESHHOLD * AVEL_THRESHHOLD;
	bool tried_to_sleep = false;

	if ((ContactBox->ContactCount >= 3) || (Get_RigidBodyDef()->CollisionDisabled)) {
		
		if ((State.LMomentum.Length2() < max_lmomentum2) && 
			(State.AMomentum.Length2() < max_amomentum2) ) 
		{
			tried_to_sleep = true;
			if (GoToSleepTimer < 0.0f) {
				return true;
			}
		} 
	}

	if (tried_to_sleep) {
		GoToSleepTimer -= dt;
	} else {
		GoToSleepTimer = RBODY_SLEEP_DELAY;
	}

	return false;
}

void RigidBodyClass::Compute_Impact(const CastResultStruct & result,Vector3 * impulse)
{
	Compute_Impact(result.ContactPoint,result.Normal,impulse);	
}

void RigidBodyClass::Compute_Impact(const Vector3 & point,const Vector3 & normal,Vector3 * impulse)
{
	// NOTE: this is only a temporary solution, it assumes we are colliding with
	// an immovable object (infinite mass)...
	Vector3 padot;
	Compute_Point_Velocity(point,&padot);
	float vrel = Vector3::Dot_Product(normal,padot);
	
	if (vrel > 0.0f) {
	
		// moving away
		impulse->Set(0,0,0);	

	} else {
		
		// collision
		float num = -(1.0f + Elasticity) * vrel;
		Vector3 ra = point - State.Position;
		
		Vector3 tmp1,tmp2;
		Vector3::Cross_Product(ra,normal,&tmp1);
		Matrix3::Rotate_Vector(IInv,tmp1,&tmp2);
		Vector3::Cross_Product(tmp2,ra,&tmp1);
		float den = MassInv + Vector3::Dot_Product(normal,tmp1);
		
		if (WWMath::Fabs(den) > WWMATH_EPSILON) {
			float mag = num / den;
			*impulse = mag * normal;
		} else {
			WWASSERT(0);
			impulse->Set(0,0,0);	
		}
	}

	WWASSERT(WWMath::Is_Valid_Float(impulse->X));
	WWASSERT(WWMath::Is_Valid_Float(impulse->Y));
	WWASSERT(WWMath::Is_Valid_Float(impulse->Z));

}

void RigidBodyClass::Assert_State_Valid(void) const
{
	WWASSERT(State.Position.Is_Valid());
	WWASSERT(State.Orientation.Is_Valid());
	WWASSERT(State.LMomentum.Is_Valid());
	WWASSERT(State.LMomentum.Is_Valid());
	WWASSERT(Velocity.Is_Valid());
	WWASSERT(AngularVelocity.Is_Valid());
}

inline void RigidBodyClass::Dump_State(void) const
{
	WWDEBUG_SAY(("Position:		%10.10f , %10.10f , %10.10f \r\n",State.Position.X,State.Position.Y,State.Position.Z));
	WWDEBUG_SAY(("Orientation:	%10.10f , %10.10f , %10.10f , %10.10f\r\n",State.Orientation.X,State.Orientation.Y,State.Orientation.Z,State.Orientation.W));
	WWDEBUG_SAY(("LMomentum:	%10.10f , %10.10f , %10.10f \r\n",State.LMomentum.X,State.LMomentum.Y,State.LMomentum.Z));
	WWDEBUG_SAY(("AMomentum:	%10.10f , %10.10f , %10.10f \r\n",State.AMomentum.X,State.AMomentum.Y,State.AMomentum.Z));
	WWDEBUG_SAY(("ContactBox intersecting: %d\r\n",(ContactBox->Is_Intersecting() ? 1 : 0)));
	WWDEBUG_SAY(("\r\n"));
}

void RigidBodyClass::Timestep(float dt)
{	
	WWPROFILE("RigidBody::Timestep");
	LastTimestep = dt;

// DEBUG DEBUG
Vector3 vel0 = Velocity;
Vector3 avel0 = AngularVelocity;

	/*
	** Update our history buffer
	*/
	if (History != NULL) {
		History->Update_History(State,dt);

		/*
		** Debugging, Draw our history if present
		*/
		OBBoxClass box;
		for (int i=1; i<History->History_Count(); i++) {
		
			const RigidBodyStateStruct & state = History->Get_Historical_State(i);
			ContactBox->Get_Inner_Box(&box,state.Orientation,state.Position);
			DEBUG_RENDER_OBBOX(box,Vector3(0,1.0f,0),0.05f);
		}
	}

	/*
	** If we have any latency error, attempt to correct it
	*/
	Assert_State_Valid();
	if (LatencyError.Position.Length2() > 0.001f) {
		Network_Latency_Error_Correction(dt);
	}
	Assert_State_Valid();


	/*
	** If we're currently asleep, see if we need to wake up.
	*/
	if (Is_Asleep()) {
		if ((Controller != NULL) && (Controller->Is_Inactive() == false)) {
			Set_Flag(ASLEEP,false);
		} else {
			return;
		}
	}

	if (Is_Immovable()) {
		return;
	}

	WWASSERT(Model);
	WWASSERT(ContactBox);

	Inc_Ignore_Counter();

#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("------------------------------\r\n"));
		WWDEBUG_SAY(("RigidBody Timestep Begin (dt=%f).\r\n",dt));
		WWDEBUG_SAY((" Beginning State:\r\n"));
		Dump_State();
	}
#endif

	/*
	** Set our active collision region
	*/
	Set_Moving_Collision_Region(dt);

	/*
	** Repeat until we eat all of the time
	*/
	const int MAX_COLLISIONS = 10;
	int collisions = 0;
	float remaining_time = dt;
	float timestep;
	
	while ((remaining_time > 0.0f) && (collisions < MAX_COLLISIONS)) {

		Assert_State_Valid();
		WWPROFILE("RigidBodyClass integration loop");
		timestep = remaining_time;

		/*
		** Integrate the state of the object
		*/
		RigidBodyStateStruct oldstate = State;
		Integrate(timestep);

		/*
		** Check the final state of the object for collision.
		*/
		if (!ContactBox->Is_Intersecting()) {
		
			/*
			** Not intersecting so we're accepting the entire move
			*/
			remaining_time -= timestep;
			StickCount = 0;

		} else {

			WWPROFILE("Impulsive Collision Handling");

			StickCount++;

			/*
			** Search for a state where we can get valid contact points
			*/
			bool found = Find_State_In_Contact_Zone(oldstate);

			/*
			** Ad-hoc impact algorithm.  Killing angular momentum, clipping
			** linear momentum to the contact normals.
			*/
			State.AMomentum /= (float)StickCount+1;

			if (found) {
				ContactBox->Compute_Contacts();
				
				if (ContactBox->Get_Contact_Count() > 0) {

					Vector3 contact_centroid(0,0,0);
					
					for (int ci=0; ci<ContactBox->Get_Contact_Count(); ci++) {
			
						const CastResultStruct & contact = ContactBox->Get_Contact(ci);

						/*
						** Eliminate any momentum not tangential to this contact normal
						*/
						float dot = Vector3::Dot_Product(State.LMomentum,contact.Normal);
						if (dot < 0.0f) {
							
							Vector3 impulse = -1.01f * dot * contact.Normal;

							/*
							** If the object we collided with is a dynamic object, apply an impulse
							** to it as well.
							*/
							PhysClass * other_obj = ContactBox->Peek_Contacted_Object(ci);

							if ((other_obj != NULL) && (other_obj->As_RigidBodyClass() != NULL)) {
							
								RigidBodyClass * other_rbody = other_obj->As_RigidBodyClass();
								float fraction = Mass / (Mass + other_rbody->Get_Mass());
									
								State.LMomentum += fraction * impulse;
								other_rbody->Apply_Impulse(-(1.0f - fraction) * impulse, contact.ContactPoint);

							} else if ((other_obj != NULL) && (other_obj->As_Phys3Class() != NULL)) {
								
								if (!Push_Phys3_Object_Away(other_obj->As_Phys3Class(),contact)) {
									State.LMomentum += impulse;
								}
								remaining_time = 0.0f;	// I don't want to resolve a crowd chain reaction all in one frame

							} else {

								State.LMomentum += impulse;

							}

						}

						/*
						** Accumulate the contact centroid
						*/
						contact_centroid += contact.ContactPoint;
					}

					/*
					** Apply an instantaneous change to the angular momentum to make the physics
					** feel more realistic.  Compute the impluse that was applied to the linear 
					** momentum and apply a fraction of it to the angular momentum at the contact 
					** centroid.  (of course, this is all "ad-hoc", not true rigid-body dynamics...)
					*/
					Vector3 impulse = 0.3f * (State.LMomentum - oldstate.LMomentum);
					contact_centroid /= ContactBox->Get_Contact_Count();
					
					Vector3 r = contact_centroid - State.Position;
					Vector3 a_impulse;
					Vector3::Cross_Product(r,impulse,&a_impulse);

					State.AMomentum += a_impulse;


					/*
					** Done, update the rest of the rigid body state
					*/
					Update_Auxiliary_State();

#if RBODY_DEBUGGING
				if (RBODY_DEBUG_FILTER) {
					WWDEBUG_SAY((" Intersection occured, found state in contact zone.\r\n"));
					WWDEBUG_SAY((" Resulting new state:\r\n"));
					Dump_State();
				}
#endif
				}

			} else {
				
				/*
				** If all else fails: 
				** Fall back to the old, kill the momentum and try to let the
				** contact spring forces sort it out.
				*/
#if RBODY_DEBUGGING
				if (RBODY_DEBUG_FILTER) {
					WWDEBUG_SAY(("Rigid Body Object: %s is intersecting (%f,%f,%f).\n",Model->Get_Name(),State.Position.X,State.Position.Y,State.Position.Z));
					WWDEBUG_SAY(("Reverting to previous position: (%f,%f,%f)\r\n",oldstate.Position.X,oldstate.Position.Y,oldstate.Position.Z));
				}
#endif

				State.LMomentum /= (float)(StickCount + 1);
				State.Position = oldstate.Position;
				State.Orientation = oldstate.Orientation;
				remaining_time = 0.0f;
				Update_Auxiliary_State();

				// We've reverted the state, now display the total force and torque that is causing us to "stick"
				#ifdef WWDEBUG
				Vector3 force(0,0,0);
				Vector3 torque(0,0,0);
				Compute_Force_And_Torque(&force,&torque);
			
				DEBUG_RENDER_VECTOR(State.Position,force,Vector3(0,1,0));
				DEBUG_RENDER_VECTOR(State.Position,force,Vector3(0,0,1));
				#endif	

#if RBODY_DEBUGGING
				if (RBODY_DEBUG_FILTER) {
					WWDEBUG_SAY((" Un-handled intersection!  StickCount = %d\r\n",StickCount));
					WWDEBUG_SAY((" Reverted State:\r\n"));
					Dump_State();
				}
#endif

			}
		}
		collisions++;
	}

#if 0 // DEBUG DEBUG DEBUG
	if (ContactBox->Is_Intersecting()) {
		State = BackupState;
		goto doitagain;
	}
#endif

	DEBUG_RENDER_VECTOR(State.Position,Velocity,LMOMENTUM_COLOR);
	DEBUG_RENDER_VECTOR(State.Position,AngularVelocity,AMOMENTUM_COLOR);	

#ifdef WWDEBUG
	if (ContactBox->Is_Intersecting()) {
		OBBoxClass box;
		ContactBox->Get_Inner_Box(&box);
		DEBUG_RENDER_OBBOX(box,Vector3(1,0,0),0.3f);
	}
#endif

	Dec_Ignore_Counter();
	Model->Set_Transform(Matrix3D(Rotation,State.Position));
	Update_Cull_Box();
	Update_Visibility_Status();

	/*
	** Release our active collision region
	*/
	PhysicsSceneClass::Get_Instance()->Release_Collision_Region();

	/*
	** See if we can go to sleep and stop simulating!
	*/
	if (Can_Go_To_Sleep(dt)) {
		Set_Flag(ASLEEP,true);
	}

// DEBUG DEBUG	
	Vector3 vel_change = Velocity - vel0;
	Vector3 avel_change = AngularVelocity - avel0;
	const float VEL_CHANGE_SPIKE = 10.0f;
	const float AVEL_CHANGE_SPIKE = 10.0f;

	if (	(vel_change.Length() > VEL_CHANGE_SPIKE) ||
			(avel_change.Length() > AVEL_CHANGE_SPIKE))
	{
		WWDEBUG_SAY(("***** Velocity spike detected during RBody::Timestep!  "));
		WWDEBUG_SAY(("initial vel: %f    final vel: %f\r\n",vel0.Length(),Velocity.Length()));
		WWDEBUG_SAY(("initial avel: %f   final avel: %f\r\n",avel0.Length(),AngularVelocity.Length()));
	}
}


bool RigidBodyClass::Push_Phys3_Object_Away(Phys3Class * p3obj,const CastResultStruct & contact)
{
	/*
	** Notify any observer of the phys3 object that this impact has happened
	*/
	CollisionEventClass event;
	event.CollisionResult = &contact;
	event.CollidedRenderObj = NULL;
	event.OtherObj = this;
	p3obj->Collision_Occurred(event);

	/*
	** Compute a movement vector to push the phy3 object away
	*/
	Vector3 move = p3obj->Get_Transform().Get_Translation() - State.Position;
	move.Normalize();

	Vector3 point_vel;
	Compute_Point_Velocity(p3obj->Get_Transform().Get_Translation(),&point_vel);
	float pvel_relative = Vector3::Dot_Product(point_vel,move);
	if (pvel_relative > 0.0f) {
		pvel_relative *= LastTimestep;
	} else {
		pvel_relative = 0.0f;
	}

	move *= ContactBox->Get_Thickness() * ((1.0f - contact.Fraction) + pvel_relative);
	
	Dec_Ignore_Counter();
	p3obj->Collide(move);
	Inc_Ignore_Counter();

#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("  Pushing Phys3 Object %x away (%f, %f, %f)\r\n",p3obj,move.X,move.Y,move.Z));
	}
#endif
	
	/*
	** Check if the object is now out of collision with us.  If the external game
	** logic kills the phys3 object, its collision group will be changed so we check
	** for that first.
	*/
	if (PhysicsSceneClass::Get_Instance()->Do_Groups_Collide(p3obj->Get_Collision_Group(),Get_Collision_Group())) {
		AABoxClass p3box;
		p3obj->Get_Collision_Box(&p3box);
		OBBoxClass rbox;
		ContactBox->Get_Outer_Box(&rbox);

#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("  completely clear of our outer collision box\r\n"));
	}
#endif
		return (CollisionMath::Overlap_Test(rbox,p3box) == CollisionMath::OUTSIDE);

	} else {

#if RBODY_DEBUGGING
	if (RBODY_DEBUG_FILTER) {
		WWDEBUG_SAY(("  still intersecting our outer collision box\r\n"));
	}
#endif
		return true;
	}
}


void RigidBodyClass::Assert_Not_Intersecting(void)
{
	Inc_Ignore_Counter();

	OBBoxClass obbox = Box->Get_Box();
	CastResultStruct result;
	PhysOBBoxCollisionTestClass test(	obbox,
													Vector3(0,0,0),
													&result,
													Get_Collision_Group(),
													COLLISION_TYPE_PHYSICAL);
	PhysicsSceneClass::Get_Instance()->Cast_OBBox(test);
//	WWASSERT(result.StartBad != true);			
	if (result.StartBad) {
		WWDEBUG_SAY(("   !!!!!!!!!!!!!!!!!!!!!!!!!   Rigid Body %s intersecting!\r\n",Model->Get_Name()));
	} else {
		WWDEBUG_SAY(("   not intersecting\r\n"));
	}

	Dec_Ignore_Counter();
}

void RigidBodyClass::Get_Shadow_Blob_Box(AABoxClass * set_obj_space_box)
{
	WWASSERT(set_obj_space_box != NULL);
	if (Box) {
		Box->Get_Obj_Space_Bounding_Box(*set_obj_space_box);
	} else {
		MoveablePhysClass::Get_Shadow_Blob_Box(set_obj_space_box);
	}
}


bool RigidBodyClass::Find_State_In_Contact_Zone
(
	const RigidBodyStateStruct & oldstate
)
{
	RigidBodyStateStruct state0 = oldstate;
	RigidBodyStateStruct state1 = State;
	RigidBodyStateStruct teststate;
	
	bool done = false;
	bool success = false;
	int iteration_count = 0;
	const int MAX_ITERATIONS = 5;

	/*
	** Binary search for a state where the box is close enough to 
	** the terrain to generate contacts but the inner box is not
	** intersecting.
	*/
	while (!done) {
		RigidBodyStateStruct::Lerp(state0,state1,0.5f,&teststate);
		Set_State(teststate);
		
		if (ContactBox->Is_In_Contact_Zone()) {
			if (ContactBox->Is_Intersecting()) {
				state1 = teststate;
			} else {
				success = true;
				done = true;
			}
		} else {
			state0 = teststate;
		}
		iteration_count++;
		if (iteration_count > MAX_ITERATIONS) {
			done = true;
		}
	}
	
	return success;
}


bool RigidBodyClass::Push(const Vector3 & move)
{
	Vector3 old_pos = State.Position;

	/*
	** Create a collision test which sweeps our collision box along the given move vector
	*/
	OBBoxClass box = ContactBox->Get_World_Inner_Box();
	CastResultStruct result;
	PhysOBBoxCollisionTestClass test(	box,
													move,
													&result,
													Get_Collision_Group(),
													COLLISION_TYPE_PHYSICAL );
		
 	/*
	** Sweep the box
	*/
	Inc_Ignore_Counter();
	PhysicsSceneClass::Get_Instance()->Cast_OBBox(test);
	Dec_Ignore_Counter();

	/*
	** Move as far as we were allowed
	*/
	if (!result.StartBad) {
		
		if (result.Fraction > 0.0f) {
			State.Position += result.Fraction * move;
		}

		/*
		** Update the rest of our parameters (TODO: clean this process up)
		*/
		Set_Flag(ASLEEP,false);
		Update_Auxiliary_State();
		Update_Visibility_Status();

		Matrix3D m(Rotation,State.Position);
		Model->Set_Transform(m);
		if (ContactBox) {
			ContactBox->Set_Transform(m);
		}

		return (result.Fraction == 1.0f);
	
	} else {
	
		return false;

	}
}


/***********************************************************************************
**
** Save-Load System
**
***********************************************************************************/
const PersistFactoryClass & RigidBodyClass::Get_Factory (void) const
{
	return _RigidBodyFactory;
}

bool RigidBodyClass::Save (ChunkSaveClass &csave)
{
	csave.Begin_Chunk(RBODY_CHUNK_MOVEABLE);
	MoveablePhysClass::Save(csave);
	csave.End_Chunk();
	
	ODESystemClass * ode_ptr = (ODESystemClass *)this;
	csave.Begin_Chunk(RBODY_CHUNK_VARIABLES);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_ODESYSTEM_PTR,ode_ptr);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_IBODY,IBody);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_IBODYINV,IBodyInv);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_STATE_POSITION,State.Position);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_STATE_ORIENTATION,State.Orientation);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_STATE_LMOMENTUM,State.LMomentum);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_STATE_AMOMENTUM,State.AMomentum);
	WRITE_MICRO_CHUNK(csave,RBODY_VARIABLE_CONTACT_LENGTH,ContactThickness);
	csave.End_Chunk();
	return true;
}

bool RigidBodyClass::Load (ChunkLoadClass &cload)
{
	ODESystemClass * odesys = NULL;

	while (cload.Open_Chunk()) {
		
		switch(cload.Cur_Chunk_ID()) 
		{
			case RBODY_CHUNK_MOVEABLE:
				MoveablePhysClass::Load(cload);
				break;

			case RBODY_CHUNK_VARIABLES:
			
				while (cload.Open_Micro_Chunk()) {
					switch(cload.Cur_Micro_Chunk_ID()) {
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_ODESYSTEM_PTR,odesys);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_IBODY,IBody);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_IBODYINV,IBodyInv);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_STATE_POSITION,State.Position);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_STATE_ORIENTATION,State.Orientation);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_STATE_LMOMENTUM,State.LMomentum);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_STATE_AMOMENTUM,State.AMomentum);
						READ_MICRO_CHUNK(cload,RBODY_VARIABLE_CONTACT_LENGTH,ContactThickness);

					}
					cload.Close_Micro_Chunk();	
				}
				break;

			default:
				WWDEBUG_SAY(("Unhandled Chunk: 0x%X File: %s Line: %d\r\n",cload.Cur_Chunk_ID(),__FILE__,__LINE__));
				break;
		}
		
		cload.Close_Chunk();
	}

	
	if (odesys != NULL) {
		SaveLoadSystemClass::Register_Pointer(odesys,(ODESystemClass *)this);
	}

	SaveLoadSystemClass::Register_Post_Load_Callback(this);
	Update_Auxiliary_State();

	return true;
}


void RigidBodyClass::On_Post_Load(void)
{
	Update_Cached_Model_Parameters();
}


/***********************************************************************************************
**
** RigidBodyDefClass Implementation
**
***********************************************************************************************/

/*
** Persist factory for RigidBodyDefClass's
*/
SimplePersistFactoryClass<RigidBodyDefClass,PHYSICS_CHUNKID_RIGIDBODYDEF>	_RigidBodyDefFactory;

/*
** Definition factory for RigidBodyDefClass.  This makes it show up in the editor
*/
DECLARE_DEFINITION_FACTORY(RigidBodyDefClass, CLASSID_RIGIDBODYDEF, "RigidBody") _RigidBodyDefDefFactory;

/*
** Chunk ID's used by RigidBodyDefClass
*/
enum 
{
	RIGIDBODYDEF_CHUNK_MOVEABLEPHYSDEF						= 0x01106650,			// (parent class)
	RIGIDBODYDEF_CHUNK_VARIABLES,

	RIGIDBODYDEF_VARIABLE_AERODYNAMICDRAGCOEFFICIENT	= 0x00,
	RIGIDBODYDEF_VARIABLE_COLLISIONDISABLED,
};


RigidBodyDefClass::RigidBodyDefClass(void) :
	AerodynamicDragCoefficient(0.0f),
	CollisionDisabled(false)
{
	// make our parameters editable
	FLOAT_EDITABLE_PARAM(RigidBodyDefClass, AerodynamicDragCoefficient, 0.0f, 100.0f);
}

uint32 RigidBodyDefClass::Get_Class_ID (void) const	
{ 
	return CLASSID_RIGIDBODYDEF; 
}

PersistClass * RigidBodyDefClass::Create(void) const
{
	RigidBodyClass * obj = NEW_REF(RigidBodyClass,());
	obj->Init(*this);
	return obj;
}

const PersistFactoryClass & RigidBodyDefClass::Get_Factory (void) const
{
	return _RigidBodyDefFactory;
}

bool RigidBodyDefClass::Save(ChunkSaveClass &csave)
{
	csave.Begin_Chunk(RIGIDBODYDEF_CHUNK_MOVEABLEPHYSDEF);
	MoveablePhysDefClass::Save(csave);
	csave.End_Chunk();
	
	csave.Begin_Chunk(RIGIDBODYDEF_CHUNK_VARIABLES);
	WRITE_MICRO_CHUNK(csave,RIGIDBODYDEF_VARIABLE_AERODYNAMICDRAGCOEFFICIENT,AerodynamicDragCoefficient);
	WRITE_MICRO_CHUNK(csave,RIGIDBODYDEF_VARIABLE_COLLISIONDISABLED,CollisionDisabled);
	csave.End_Chunk();
	return true;
}

bool RigidBodyDefClass::Load(ChunkLoadClass &cload)
{
	while (cload.Open_Chunk()) {

		switch(cload.Cur_Chunk_ID()) {

			case RIGIDBODYDEF_CHUNK_MOVEABLEPHYSDEF:
				MoveablePhysDefClass::Load(cload);
				break;

			case RIGIDBODYDEF_CHUNK_VARIABLES:
				while (cload.Open_Micro_Chunk()) {
					switch(cload.Cur_Micro_Chunk_ID()) {
						READ_MICRO_CHUNK(cload,RIGIDBODYDEF_VARIABLE_AERODYNAMICDRAGCOEFFICIENT,AerodynamicDragCoefficient);
						READ_MICRO_CHUNK(cload,RIGIDBODYDEF_VARIABLE_COLLISIONDISABLED,CollisionDisabled);
					}
					cload.Close_Micro_Chunk();
				}
				break;


			default:
				WWDEBUG_SAY(("Unhandled Chunk: 0x%X File: %s Line: %d\r\n",cload.Cur_Chunk_ID(),__FILE__,__LINE__));
				break;
		}

		cload.Close_Chunk();
	}
	return true;
}

bool RigidBodyDefClass::Is_Type(const char * type_name)
{
	if (stricmp(type_name,RigidBodyDefClass::Get_Type_Name()) == 0) {
		return true;
	} else {
		return MoveablePhysDefClass::Is_Type(type_name);
	}
}