//===========================================================================//
// File:        mover.cc                                                     //
// Project: MUNGA           Brick: Model Manager                             //
// Contents: Implementation details of moving entity class                   //
//---------------------------------------------------------------------------//
//   Date    Who  Modification                                               //
// --------  ---  ---------------------------------------------------------- //
// 01/21/95  JMA  Initial coding.                                            //
//---------------------------------------------------------------------------//
// Copyright (C) 1995, Virtual World Entertainment, Inc.                     //
// All Rights reserved worldwide                                             //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL               //
//===========================================================================//

#include "AdeptHeaders.hpp"
#include "Mover.hpp"
#include "EntityManager.hpp"

//#############################################################################
//##################    Mover::ExecutionStateEngine    #####################
//#############################################################################

const StateEngine::StateEntry
	Mover::ExecutionStateEngine::StateEntries[]=
{
	STATE_ENTRY(Mover__ExecutionStateEngine, StraightLineMotion),
	STATE_ENTRY(Mover__ExecutionStateEngine, LinearDragMotion)
};

Mover::ExecutionStateEngine::ClassData*
	Mover::ExecutionStateEngine::DefaultData = NULL;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ExecutionStateEngine::InitializeClass()
{
	Check_Object(StateEngine::DefaultData);
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			Mover__ExecutionStateEngineClassID,
			"Adept::Mover::ExecutionStateEngine",
			BaseClass::DefaultData,
			ELEMENTS(StateEntries), StateEntries,
			(Entity::ExecutionStateEngine::Factory)Make,
			(Entity::ExecutionStateEngine::FactoryRequest::Factory)
				&FactoryRequest::ConstructFactoryRequest
		);
	Register_Object(DefaultData);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ExecutionStateEngine::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Mover::ExecutionStateEngine*
	Mover::ExecutionStateEngine::Make(
		Mover *mover,
		FactoryRequest *request
	)
{
	Check_Object(mover);
	Check_Object(request);
	Mover::ExecutionStateEngine *engine =
		new Mover::ExecutionStateEngine(DefaultData, mover, request);
	return engine;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ExecutionStateEngine::TestInstance()
{
	Verify(IsDerivedFrom(DefaultData));
}

//#############################################################################
//###############################    Mover    #################################
//#############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Mover::ClassData*
	Mover::DefaultData = NULL;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::InitializeClass()
{
	Check_Object(ExecutionStateEngine::DefaultData);
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			MoverClassID,
			"Adept::Mover",
			BaseClass::DefaultData,
			0, NULL,
			(Entity::Factory)Make,
			(Entity::CreateMessage::Factory)CreateMessage::ConstructCreateMessage,
			ExecutionStateEngine::DefaultData,
			(Entity::GameModel::Factory)GameModel::ConstructGameModel,
			NULL,
			(Entity::GameModel::ReadAndVerifier)GameModel::ReadAndVerify,
			(Entity::GameModel::ModelWrite)GameModel::WriteToText,
			(Entity::GameModel::ModelSave)GameModel::SaveGameModel
		);
	Register_Object(DefaultData);

	DIRECT_ATTRIBUTE(
		Mover, 
		LocalSpaceVelocity,		
		localSpaceVelocity,
		Motion3D
	);
	DIRECT_ATTRIBUTE(
		Mover, 
		LocalSpaceAcceleration,	
		localSpaceAcceleration,
		Motion3D
	);
	DIRECT_ATTRIBUTE(
		Mover,	
		WorldSpaceVelocity,		
		worldSpaceVelocity,
		Motion3D
	);
	DIRECT_ATTRIBUTE(
		Mover, 
		WorldSpaceAcceleration,	
		worldSpaceAcceleration,
		Motion3D
	);
	INDIRECT_STATE_ATTRIBUTE(
		Mover,
		ExecutionState,
		executionState,
		Mover__ExecutionStateEngine
	);		
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		MoverMass,
		moverMass,
		Scalar
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		MomentOfInertia,
		momentOfInertia,
		Vector3D
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		LinearDragCoefficients,
		linearDragCoefficients,
		Vector3D
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		AngularDragCoefficients,
		angularDragCoefficients,
		Vector3D
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		FrictionCoefficient,
		frictionCoefficient,
		Scalar
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		ElasticityCoefficient,
		elasticityCoefficient,
		Scalar
	);
	DIRECT_GAME_MODEL_ATTRIBUTE(
		Mover__GameModel,
		MinimumBounceSpeed,
		minimumBounceSpeed,
		Scalar
	);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Mover*
	Mover::Make(
		const CreateMessage *message,
		ReplicatorID *base_id
	)
{
	Check_Object(message);
	gos_PushCurrentHeap(g_LibraryHeap);
	Mover *new_entity = new Mover(DefaultData, message, base_id, NULL);
	Check_Object(new_entity);
	gos_PopCurrentHeap();
	return new_entity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Mover::Mover(
	ClassData *class_data,
	const CreateMessage *message,
	ReplicatorID *base_id,
	ElementRenderer::Element *element
):
	Entity(class_data, message, base_id, element)
{
	Check_Pointer(this);
	Check_Object(message);

	//
	//------------------------------------------------------------------------
	// Initialize the motion vectors.  Note that this assignment only works
	// because at the time of construction, there is no parent so parent space
	// is equivalant to world space
	//------------------------------------------------------------------------
	//
	initialWorldSpaceVelocity = message->worldSpaceVelocity;
	initialWorldSpaceAcceleration = message->worldSpaceAcceleration;

	//
	//----------------------------------------------------------------------
	// Look at our initial state and see how to compute the current position
	//----------------------------------------------------------------------
	//
	if (message->initialAge > 0.0f)
	{
		Check_Object(executionState);
		int execution_state = executionState->GetState();
		switch (execution_state)
		{
		case ExecutionStateEngine::StraightLineMotionState:
			StraightLineMotionSimulation(gos_GetElapsedTime());
			break;

		case ExecutionStateEngine::LinearDragMotionState:
			{
				//
				//--------------------------------------------
				// figure out the drag numbers in parent space
				//--------------------------------------------
				//
				const GameModel *model = GetGameModel();
				Check_Object(model);
				localSpaceDrag.linearMotion = model->linearDragCoefficients;
				localSpaceDrag.angularMotion = model->angularDragCoefficients;
				LinearMatrix4D world_to_local;
				world_to_local.Invert(GetLocalToWorld());
				localSpaceAcceleration.linearMotion.Multiply(
					worldSpaceAcceleration.linearMotion,
					world_to_local
				);
				localSpaceAcceleration.angularMotion.Multiply(
					worldSpaceAcceleration.angularMotion,
					world_to_local
				);
				LinearDragMotionSimulation(gos_GetElapsedTime());
			}
			break;

		default:
			goto T_is_T0;
		}
	}

	//
	//----------------------------------------------------------------
	// If we are at the beginning of the time, just copy the variables
	//----------------------------------------------------------------
	//
	else
	{
T_is_T0:
		worldSpaceVelocity = message->worldSpaceVelocity;
		worldSpaceAcceleration = message->worldSpaceAcceleration;
	}

	//
	//-------------------------------------
	// Initialize the local velocity vector
	//-------------------------------------
	//
	LinearMatrix4D world_to_local;
	world_to_local.Invert(initialLocalToParent);
	localSpaceVelocity.linearMotion.Multiply(
		worldSpaceVelocity.linearMotion,
		world_to_local
	);
	localSpaceVelocity.angularMotion.Multiply(
		worldSpaceVelocity.angularMotion,
		world_to_local
	);
	localSpaceAcceleration.linearMotion.Multiply(
		worldSpaceAcceleration.linearMotion,
		world_to_local
	);
	localSpaceAcceleration.angularMotion.Multiply(
		worldSpaceAcceleration.angularMotion,
		world_to_local
	);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
	void Mover::Respawn(Entity::CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);

	BaseClass::Respawn(message);

	CreateMessage *mover_message = Cast_Pointer(CreateMessage *, message);
	//
	//------------------------------------------------------------------------
	// Initialize the motion vectors.  Note that this assignment only works
	// because at the time of construction, there is no parent so parent space
	// is equivalant to world space
	//------------------------------------------------------------------------
	//
	initialWorldSpaceVelocity = mover_message->worldSpaceVelocity;
	initialWorldSpaceAcceleration = mover_message->worldSpaceAcceleration;

	//
	//----------------------------------------------------------------------
	// Look at our initial state and see how to compute the current position
	//----------------------------------------------------------------------
	//
	if (mover_message->initialAge > 0.0f)
	{
		Check_Object(executionState);
		int execution_state = executionState->GetState();
		switch (execution_state)
		{
		case ExecutionStateEngine::StraightLineMotionState:
			StraightLineMotionSimulation(gos_GetElapsedTime());
			break;

		case ExecutionStateEngine::LinearDragMotionState:
			{
				//
				//--------------------------------------------
				// figure out the drag numbers in parent space
				//--------------------------------------------
				//
				const GameModel *model = GetGameModel();
				Check_Object(model);
				localSpaceDrag.linearMotion = model->linearDragCoefficients;
				localSpaceDrag.angularMotion = model->angularDragCoefficients;
				LinearMatrix4D world_to_local;
				world_to_local.Invert(GetLocalToWorld());
				localSpaceAcceleration.linearMotion.Multiply(
					worldSpaceAcceleration.linearMotion,
					world_to_local
				);
				localSpaceAcceleration.angularMotion.Multiply(
					worldSpaceAcceleration.angularMotion,
					world_to_local
				);
				LinearDragMotionSimulation(gos_GetElapsedTime());
			}
			break;

		default:
			goto T_is_T0;
		}
	}

	//
	//----------------------------------------------------------------
	// If we are at the beginning of the time, just copy the variables
	//----------------------------------------------------------------
	//
	else
	{
T_is_T0:
		worldSpaceVelocity = mover_message->worldSpaceVelocity;
		worldSpaceAcceleration = mover_message->worldSpaceAcceleration;
	}

	//
	//-------------------------------------
	// Initialize the local velocity vector
	//-------------------------------------
	//
	LinearMatrix4D world_to_local;
	world_to_local.Invert(initialLocalToParent);
	localSpaceVelocity.linearMotion.Multiply(
		worldSpaceVelocity.linearMotion,
		world_to_local
	);
	localSpaceVelocity.angularMotion.Multiply(
		worldSpaceVelocity.angularMotion,
		world_to_local
	);
	localSpaceAcceleration.linearMotion.Multiply(
		worldSpaceAcceleration.linearMotion,
		world_to_local
	);
	localSpaceAcceleration.angularMotion.Multiply(
		worldSpaceAcceleration.angularMotion,
		world_to_local
	);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::Reuse(
		const CreateMessage *message,
		ReplicatorID *base_id
	)
{
	Check_Pointer(this);
	Check_Object(message);
	BaseClass::Reuse(message, base_id);

	//
	//------------------------------------------------------------------------
	// Initialize the motion vectors.  Note that this assignment only works
	// because at the time of construction, there is no parent so parent space
	// is equivalant to world space
	//------------------------------------------------------------------------
	//
	initialWorldSpaceVelocity = message->worldSpaceVelocity;
	initialWorldSpaceAcceleration = message->worldSpaceAcceleration;

	//
	//----------------------------------------------------------------------
	// Look at our initial state and see how to compute the current position
	//----------------------------------------------------------------------
	//
	if (message->initialAge > 0.0f)
	{
		Check_Object(executionState);
		int execution_state = executionState->GetState();
		switch (execution_state)
		{
		case ExecutionStateEngine::StraightLineMotionState:
			StraightLineMotionSimulation(gos_GetElapsedTime());
			break;

		case ExecutionStateEngine::LinearDragMotionState:
			{
				//
				//--------------------------------------------
				// figure out the drag numbers in parent space
				//--------------------------------------------
				//
				const GameModel *model = GetGameModel();
				Check_Object(model);
				localSpaceDrag.linearMotion = model->linearDragCoefficients;
				localSpaceDrag.angularMotion = model->angularDragCoefficients;
				LinearMatrix4D world_to_local;
				world_to_local.Invert(GetLocalToWorld());
				localSpaceAcceleration.linearMotion.Multiply(
					worldSpaceAcceleration.linearMotion,
					world_to_local
				);
				localSpaceAcceleration.angularMotion.Multiply(
					worldSpaceAcceleration.angularMotion,
					world_to_local
				);
				LinearDragMotionSimulation(gos_GetElapsedTime());
			}
			break;

		default:
			goto T_is_T0;
		}
	}

	//
	//----------------------------------------------------------------
	// If we are at the beginning of the time, just copy the variables
	//----------------------------------------------------------------
	//
	else
	{
T_is_T0:
		worldSpaceVelocity = message->worldSpaceVelocity;
		worldSpaceAcceleration = message->worldSpaceAcceleration;
	}

	//
	//-------------------------------------
	// Initialize the local velocity vector
	//-------------------------------------
	//
	LinearMatrix4D world_to_local;
	world_to_local.Invert(initialLocalToParent);
	localSpaceVelocity.linearMotion.Multiply(
		worldSpaceVelocity.linearMotion,
		world_to_local
	);
	localSpaceVelocity.angularMotion.Multiply(
		worldSpaceVelocity.angularMotion,
		world_to_local
	);
	localSpaceAcceleration.linearMotion.Multiply(
		worldSpaceAcceleration.linearMotion,
		world_to_local
	);
	localSpaceAcceleration.angularMotion.Multiply(
		worldSpaceAcceleration.angularMotion,
		world_to_local
	);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Replicator::CreateMessage*
	Mover::SaveMakeMessage(MemoryStream *stream, ResourceFile *res_file)
{
	Check_Object(this);
	Check_Object(stream);

	//
	//---------------------------------------------------------------------
	// Make sure there is enough room for the factory request on the stream
	//---------------------------------------------------------------------
	//
	stream->AllocateBytes(sizeof(CreateMessage));
	BaseClass::SaveMakeMessage(stream, res_file);
	CreateMessage *message =
		Cast_Pointer(CreateMessage*, stream->GetPointer());
	message->messageLength = sizeof(*message);

	//
	//-------------------------------------------------------------------------
	// Set up the motion.  This will absolutely not work if the age is anything
	// other than zero
	//-------------------------------------------------------------------------
	//
	if(parentEntity)
	{
		Check_Object(parentEntity);
		LinearMatrix4D parent_to_world = parentEntity->GetLocalToWorld();
		message->worldSpaceVelocity.linearMotion.Multiply(
			initialWorldSpaceVelocity.linearMotion,
			parent_to_world
		);
		message->worldSpaceVelocity.angularMotion.Multiply(
			initialWorldSpaceVelocity.angularMotion,
			parent_to_world
		);
		message->worldSpaceAcceleration.linearMotion.Multiply(
			initialWorldSpaceAcceleration.linearMotion,
			parent_to_world
		);
		message->worldSpaceAcceleration.angularMotion.Multiply(
			initialWorldSpaceAcceleration.angularMotion,
			parent_to_world
		);
	}
	else
	{
		message->worldSpaceVelocity = Motion3D::Identity;
		message->worldSpaceAcceleration = Motion3D::Identity;
	}

	return message;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::PreCollisionExecute(Time till)
{
	Check_Object(this);
	PRECOLLISION_LOGIC("Mover");

	//
	//---------------------------------
	// If we aren't executing, stop now
	//---------------------------------
	//
	Check_Object(executionState);
	int execution_state = executionState->GetState();
	Verify(execution_state != ExecutionStateEngine::NeverExecuteState);

	//
	//---------------------------------------------
	// Make sure that the local velocity is current
	//---------------------------------------------
	//
	#if defined(_ARMOR)
	{
		const LinearMatrix4D &local_to_world = GetLocalToWorld();
		Check_Object(&local_to_world);
		LinearMatrix4D world_to_local;
		world_to_local.Invert(local_to_world);
		Vector3D velocity;
		velocity.Multiply(worldSpaceVelocity.linearMotion, world_to_local);
		Verify(velocity == localSpaceVelocity.linearMotion);
		velocity.Multiply(worldSpaceVelocity.angularMotion, world_to_local);
		Verify(velocity == localSpaceVelocity.angularMotion);
	}
	#endif

	//
	//---------------------------
	// Set up the mover variables
	//---------------------------
	//
	Verify(Close_Enough(GetLocalToParent(), GetLocalToWorld()));
	Verify(IsUsingPostCollision());
	const GameModel *model = GetGameModel();
	Check_Object(model);

	//
	//--------------------------
	// Now do something for real
	//--------------------------
	//
	switch (execution_state)
	{
	case ExecutionStateEngine::StraightLineMotionState:
		StraightLineMotionSimulation(till);
		break;

	//
	//------------------------------------------------------------------------
	// Linear drag model requires us to rotate the initial world accelerations
	// into local space each frame
	//------------------------------------------------------------------------
	//
	case ExecutionStateEngine::LinearDragMotionState:
		{
			localSpaceDrag.linearMotion = model->linearDragCoefficients;
			localSpaceDrag.angularMotion = model->angularDragCoefficients;
			LinearMatrix4D world_to_local;
			world_to_local.Invert(GetLocalToWorld());
			localSpaceAcceleration.linearMotion.Multiply(
				worldSpaceAcceleration.linearMotion,
				world_to_local
			);
			localSpaceAcceleration.angularMotion.Multiply(
				worldSpaceAcceleration.angularMotion,
				world_to_local
			);
			LinearDragMotionSimulation(till);
		}
		break;

	default:
		localSpaceAcceleration = Motion3D::Identity;
		localSpaceDrag.linearMotion = model->linearDragCoefficients;
		localSpaceDrag.angularMotion = model->angularDragCoefficients;
		break;
	}

	//
	//--------------------------------------------------
	// For pre-collision execution, call our parent last
	//--------------------------------------------------
	//
	BaseClass::PreCollisionExecute(till);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::PostCollisionExecute(Time till)
{
	Check_Object(this);
	Verify(Close_Enough(GetLocalToParent(), GetLocalToWorld()));
	POSTCOLLISION_LOGIC("Mover");

	//
	//----------------------------
	// Update the local velocities
	//----------------------------
	//
	LinearMatrix4D world_to_local;
	world_to_local.Invert(GetLocalToWorld());
	localSpaceVelocity.linearMotion.Multiply(
		worldSpaceVelocity.linearMotion,
		world_to_local
	);
	localSpaceVelocity.angularMotion.Multiply(
		worldSpaceVelocity.angularMotion,
		world_to_local
	);

	//
	//--------------------------------------------------
	// Reparameterize if we are in the linear drag state
	//--------------------------------------------------
	//
	Check_Object(executionState);
	int execution_state = executionState->GetState();
	

	//this is an invalid check...
	// it is possible that we were just set to this and are post executing...
//	Verify(execution_state != ExecutionStateEngine::NeverExecuteState);


	if (execution_state == ExecutionStateEngine::LinearDragMotionState)
	{
		lastParameterization = till;
		initialLocalToParent = GetLocalToParent();
		initialWorldSpaceVelocity = worldSpaceVelocity,
		initialWorldSpaceAcceleration = worldSpaceAcceleration;
	}
	
	BaseClass::PostCollisionExecute(till);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::StraightLineMotionSimulation(Time till)
{
	Check_Object(this);
	Verify(Close_Enough(GetLocalToParent(), GetLocalToWorld()));

	//
	//-------------------------
	// See how long to move for
	//-------------------------
	//
	Scalar t = GetTimeParameter(till);

	//
	//----------------------------------------------------
	// Add the world linear velocity to the world position
	//----------------------------------------------------
	//
	Point3D old_translation(initialLocalToParent);
	Point3D new_translation;
	new_translation.AddScaled(
		old_translation,
		worldSpaceVelocity.linearMotion,
		t
	);

	//
	//---------------------------------------------------
	// Make a matrix representing the rotation this frame
	//---------------------------------------------------
	//
	Vector3D spin_axis;
	spin_axis.Multiply(worldSpaceVelocity.angularMotion, t);
	UnitQuaternion spin;
	spin = spin_axis;

	//
	//-------------------------------------------------------------------------
	// Convert the EulerAngles into a matrix and create the new rotation matrix
	// through concatenation
	//-------------------------------------------------------------------------
	//
	UnitQuaternion rotation;
	rotation = initialLocalToParent;
	UnitQuaternion desire;
	desire.Multiply(rotation, spin);
	LinearMatrix4D new_position;
	new_position.BuildTranslation(new_translation);
	new_position.BuildRotation(desire);
	SetNewLocalToParent(new_position);
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::LinearDragMotionSimulation(Time till)
{



	Check_Object(this);
	Verify(Close_Enough(GetLocalToParent(), GetLocalToWorld()));
	Verify(IsUsingPostCollision());

	//
	//-------------------------
	// See how long to move for
	//-------------------------
	//
	Scalar t = GetTimeParameter(till);

	//
	//---------------------------------
	// Call the linear drag computation
	//---------------------------------
	//
	LinearMatrix4D origin_to_local;
	Motion3D new_v;
	CalculateMotionWithLinearDrag(
		t,
		&origin_to_local,
		&new_v,
		LinearMatrix4D::Identity,
		localSpaceVelocity,
		localSpaceAcceleration,
		localSpaceDrag
	);

	//
	//--------------------------------------------------
	// Now rotate the motions and delta into world space
	//--------------------------------------------------
	//
	const LinearMatrix4D &local_to_world = GetLocalToWorld();
	worldSpaceVelocity.linearMotion.Multiply(new_v.linearMotion, local_to_world);
	worldSpaceVelocity.angularMotion.Multiply(new_v.angularMotion, local_to_world);
	LinearMatrix4D new_local_to_parent;
	new_local_to_parent.Multiply(origin_to_local, local_to_world);
	new_local_to_parent.Normalize();

	//
	//--------------------------------
	// Set the position of the locator
	//--------------------------------
	//
	SetNewLocalToParent(new_local_to_parent);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::PlaceOnEntity(
		LinearMatrix4D *position,
		Motion3D *velocity,
		const LinearMatrix4D& initial_offset,
		const Motion3D& initial_velocity
	)
{
	Check_Object(this);
	Check_Object(position);
	Check_Object(velocity);

	BaseClass::PlaceOnEntity(
		position,
		velocity,
		initial_offset,
		initial_velocity
	);
	STOP(("Not implemented"));
	velocity->linearMotion += worldSpaceVelocity.linearMotion;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::CalculateMotionWithLinearDrag(
		Scalar time_slice,
		LinearMatrix4D *new_local_to_parent,
		Motion3D *new_velocity,
		const LinearMatrix4D &old_local_to_parent,
		const Motion3D &old_velocity,
		const Motion3D &acceleration,
		const Motion3D &drag
	)
{
	Check_Pointer(new_local_to_parent);
	Check_Pointer(new_velocity);
	Check_Object(&old_local_to_parent);
	Check_Object(&old_velocity);
	Check_Object(&acceleration);
	Check_Object(&drag);
	Verify(&old_local_to_parent != new_local_to_parent);

	//
	//-------------------------
	// Extract the translations
	//-------------------------
	//
	Point3D old_translation;
	old_translation = old_local_to_parent;
	Point3D new_translation;

	//
	//-----------------------------------------------------
	// Spin through each axis and compute the linear motion
	//-----------------------------------------------------
	//
	for (int axis=X_Axis; axis <= Z_Axis; ++axis)
	{
		//
		//-----------------------------------------------------------------
		// If there is no drag, use the x += v*t + 0.5*a*t*t, v += a*t form
		//-----------------------------------------------------------------
		//
		Scalar c = drag.linearMotion[axis];
		if (Small_Enough(c))
		{
			Scalar delta_v = acceleration.linearMotion[axis]*time_slice;
			new_translation[axis] =
				old_translation[axis]
				 + old_velocity.linearMotion[axis]*time_slice
				 + 0.5f*delta_v*time_slice;
			new_velocity->linearMotion[axis] =
				old_velocity.linearMotion[axis] + delta_v;
		}

		//
		//-----------------------------------
		// Otherwise, use the continuous form
		//-----------------------------------
		//
		else
		{
			Verify(c > 0.0f);
			Scalar terminal_v = acceleration.linearMotion[axis] / c;
			Scalar v_range = old_velocity.linearMotion[axis] - terminal_v;
			Scalar delta_v = v_range * Exp(-time_slice*c);
			new_velocity->linearMotion[axis] = terminal_v + delta_v;
			new_translation[axis] =
				old_translation[axis] 
				 + terminal_v*time_slice
				 + (v_range - delta_v) / c;
		}
	}

	//
	//------------------------------------------------------
	// Spin through each axis and compute the angular motion
	//------------------------------------------------------
	//
	Verify(time_slice > 0.0f);
	Vector3D delta_q;
	for (axis=X_Axis; axis <= Z_Axis; ++axis)
	{
		//
		//-----------------------------------------------------------------
		// If there is no drag, use the x += v*t + 0.5*a*t*t, v += a*t form
		//-----------------------------------------------------------------
		//
		Scalar c = drag.angularMotion[axis];
		if (Small_Enough(c))
		{
			Scalar delta_v = acceleration.angularMotion[axis]*time_slice;
			delta_q[axis] =
				 old_velocity.angularMotion[axis]*time_slice
				 + 0.5f*delta_v*time_slice;
			new_velocity->angularMotion[axis] =
				old_velocity.angularMotion[axis] + delta_v;
		}

		//
		//-----------------------------------
		// Otherwise, use the continuous form
		//-----------------------------------
		//
		else
		{
			Verify(c > 0.0f);
			Scalar terminal_v = acceleration.angularMotion[axis] / c;
			Scalar v_range = old_velocity.angularMotion[axis] - terminal_v;
			Scalar delta_v = v_range * Exp(-time_slice*c);
			new_velocity->angularMotion[axis] = terminal_v + delta_v;
			delta_q[axis] = terminal_v*time_slice + (v_range - delta_v) / c;
		}
	}

	//
	//------------------------------------------------------------------------
	// Now add this motion to the origin.  For the angular motion, convert the
	// delta into a quaternion first
	//------------------------------------------------------------------------
	//
	LinearMatrix4D d;
	d.BuildRotation(delta_q);
	d.BuildTranslation(Point3D::Identity);
	new_local_to_parent->Multiply(old_local_to_parent, d);
	new_local_to_parent->BuildTranslation(new_translation);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ApplyLocalAccelerationToTorque(
		const Vector3D &acceleration,
		const Vector3D &moment
	)
{
	Check_Object(this);
	Check_Object(&acceleration);
	Check_Object(&moment);

	Vector3D torque;
	torque.Cross(moment, acceleration);
	const GameModel *model = GetGameModel();
	Check_Pointer(model);
	torque *= model->momentOfInertia;
	localSpaceAcceleration.angularMotion += torque;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ApplyLocalForceToTorque(
		const Vector3D &force,
		const Vector3D &moment
	)
{
	Check_Object(this);
	Check_Object(&force);
	Check_Object(&moment);

	const GameModel *model = GetGameModel();
	Check_Pointer(model);
	Verify(!Small_Enough(model->moverMass));
	Vector3D acceleration;
	acceleration.Divide(force, model->moverMass);
	ApplyLocalAccelerationToTorque(acceleration, moment);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ApplyLocalAcceleration(
		const Vector3D &acceleration,
		const Vector3D &moment
	)
{
	Check_Object(this);
	Check_Object(&acceleration);
	Check_Object(&moment);

	localSpaceAcceleration.linearMotion += acceleration;
	ApplyLocalAccelerationToTorque(acceleration, moment);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::ApplyLocalForce(
		const Vector3D &force,
		const Vector3D &moment
	)
{
	Check_Object(this);
	Check_Object(&force);
	Check_Object(&moment);

	const GameModel *model = GetGameModel();
	Check_Pointer(model);
	Verify(!Small_Enough(model->moverMass));
	Vector3D acceleration;
	acceleration.Divide(force, model->moverMass);
	ApplyLocalAcceleration(acceleration, moment);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Mover::StaticBounce(
		const Point3D &, //old_position,
		Scalar delta_t,
		Scalar penetration,
		const Normal3D &normal,
		Scalar *elasticity,
		Scalar bounce_min,
		Scalar *friction
	)
{
	Check_Object(this);
	Check_Object(&normal);
	Check_Pointer(elasticity);
	Check_Pointer(friction);

	Verify(penetration >= 0.0f && penetration <= 1.0f);
	Verify(*elasticity >= 0.0f && *elasticity <= 1.0f);
	Verify(*friction >= 0.0f);
	Verify(delta_t > SMALL);

//      penetration = 1.0f;             // HACK - should keep stuff from going through the floor

	//
	//-----------------------------------------------------------------------
	// Calculate the impact speed and vectors.  If we didn't hit fast enough,
	// don't do any bounce
	//-----------------------------------------------------------------------
	//
	Scalar impact = worldSpaceVelocity.linearMotion * normal;
	Vector3D vn,vp;
	vn.Multiply(normal, impact);
	vp.Subtract(worldSpaceVelocity.linearMotion, vn);
	if (impact > 0.0f)
	{
		return 0.0f;
	}
	if (-impact <= bounce_min * delta_t)
	{
		*elasticity = 0.0f;
	}

	//
	//--------------------------------------
	// Calculate the energy lost to friction
	//--------------------------------------
	//
	Scalar resistance = vp.GetLength();
	if (Small_Enough(resistance))
	{
		*friction = resistance = 0.0f;
	}
	else
	{
		resistance =
			1.0f + *friction * (1.0f + *elasticity) * impact / resistance;
		if (resistance < 0.0f)
		{
			*friction = resistance = 0.0f;
		}
	}

	//
	//----------------------------------------------------
	// Compute the velocity delta created by the collision
	//----------------------------------------------------
	//
	Scalar temp = resistance - 1.0f;
	Vector3D delta_v;
	delta_v.Multiply(worldSpaceVelocity.linearMotion, temp);
	temp = resistance + *elasticity;
	delta_v.AddScaled(delta_v, vn, -temp);

	//
	//------------------------------------
	// Figure out the kinetic energy stuff
	//------------------------------------
	//
	temp = -1.0f - *elasticity;
	vn *= temp;
	vp.AddScaled(vn, worldSpaceVelocity.linearMotion, 2.0f);

	//
	// Reflect the velocity vector
	//
	worldSpaceVelocity.linearMotion += delta_v;
	temp = penetration * delta_t;
	delta_v *= temp;
	STOP(("Not implemented"));
//	localOrigin.linearPosition += delta_v;

	//
	// Compute the kinetic energy loss
	//
	const GameModel *model = GetGameModel();
	Check_Pointer(model);
	return -0.0005f * (vn * vp) * model->moverMass;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Mover::DynamicBounce(
		Mover *other,
		Scalar delta_t,
		Scalar penetration,
		const Normal3D &normal,
		Scalar *elasticity
	)
{
	Check_Object(this);
	Check_Object(other);
	Check_Object(&normal);
	Check_Pointer(elasticity);

	Verify(penetration >= 0.0f && penetration <= 1.0f);
	Verify(*elasticity >= 0.0f && *elasticity <= 1.0f);
	Verify(delta_t > SMALL);

	//
	//------------------------------------------------------------------------
	// Get the relative velocity of the other guy, and figure out the velocity
	// delta along the normal
	//------------------------------------------------------------------------
	//
	Scalar k1 = worldSpaceVelocity.linearMotion.GetLengthSquared();
	Scalar k2 = other->worldSpaceVelocity.linearMotion.GetLengthSquared();

	const GameModel *model = GetGameModel();
	Check_Pointer(model);
	const GameModel *other_model = other->GetGameModel();
	Check_Pointer(model);
	Scalar mass_ratio =
		other_model->moverMass / (model->moverMass + other_model->moverMass);
	Vector3D v;
	v.Subtract(other->worldSpaceVelocity.linearMotion, worldSpaceVelocity.linearMotion);
	Scalar temp = (1.0f + *elasticity) * (v*normal);
	Vector3D delta_v;
	delta_v.Multiply(normal, temp);

	//
	//-------------------------------------------------------------------------
	// Figure out the kinetic energy loss in kilojoules, and bounce the primary
	// mover
	//
	// There was an additional multiplication by mass ratio in system 3 code...
	//   we should make sure it is really needed...
	//-------------------------------------------------------------------------
	//
	v.AddScaled(delta_v, v, -2.0f);
	worldSpaceVelocity.linearMotion.AddScaled(
		worldSpaceVelocity.linearMotion,
		delta_v,
		mass_ratio
	);
	STOP(("Not implemented"));
#if 0
	localOrigin.linearPosition.AddScaled(
		localOrigin.linearPosition,
		delta_v,
		delta_t * penetration
	);
#endif

	//
	//----------------------------------------------------------
	// Bounce the second object, and reset it's update values...
	//----------------------------------------------------------
	//
	other->worldSpaceVelocity.linearMotion.AddScaled(
		other->worldSpaceVelocity.linearMotion,
		delta_v,
		mass_ratio - 1.0f
	);
#if 0
	other->localOrigin.linearPosition.AddScaled(
		other->localOrigin.linearPosition,
		delta_v,
		delta_t
	);
#endif

	//
	//--------------------------------
	// Return the result in kilojoules
	//--------------------------------
	//
	k1 -= worldSpaceVelocity.linearMotion.GetLengthSquared();
	k2 -= other->worldSpaceVelocity.linearMotion.GetLengthSquared();
	return
		0.0005f * mass_ratio
		 * (model->moverMass * k1 + other_model->moverMass * k2);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Mover::TestInstance()
{
	Verify(IsDerivedFrom(DefaultData));
}