From c7bc7281d8502094cfb6b1ded01500713d6e40ff Mon Sep 17 00:00:00 2001
From: Robin Engman <robinengman@hotmail.com>
Date: Fri, 31 Jan 2014 13:41:06 +0100
Subject: [PATCH] Prefix for "sinking" objects

---
 .../Implementation/PhysicsAPI_Impl.cpp        | 25 +++++++++++--------
 Code/GamePhysics/PhysicsAPI.h                 |  2 +-
 Code/GamePhysics/PhysicsStructs.h             |  4 ++-
 Code/OysterPhysics3D/OysterCollision3D.cpp    |  4 +--
 Code/OysterPhysics3D/RigidBody.cpp            |  7 +++++-
 5 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp
index 6f164810..30ced129 100644
--- a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp
+++ b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp
@@ -26,7 +26,7 @@ namespace
 			ICustomBody::State protoState; proto->GetState( protoState );
 			ICustomBody::State deuterState; deuter->GetState( deuterState );
 
-			Float4 protoG = protoState.GetLinearMomentum( worldPointOfContact.xyz ),
+			Float4 protoG = protoState.GetLinearMomentum(worldPointOfContact.xyz ),
 					deuterG = deuterState.GetLinearMomentum( worldPointOfContact.xyz );
 
 			// calc from perspective of deuter
@@ -198,24 +198,27 @@ void API_Impl::Update()
 	proto = updateList.begin();
 	for( ; proto != updateList.end(); ++proto )
 	{
-		Float3 lM = state.GetLinearMomentum() + state.GetLinearImpulse();
+		(*proto)->GetState( state );
+		Float3 lM = state.GetLinearMomentum();
 
-		if( lM.x < this->epsilon )
+		//LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp(state.SetOrientation(, Float4(state.GetGravityNormal(), 0.0f), 1.0f);
+
+
+		if( abs(lM.x) < this->epsilon )
 		{
-			state.SetLinearMomentum( Float3(0, lM.y, lM.z) );
-			state.SetLinearImpulse( Float3(0, lM.y, lM.z) );
+			state.linearMomentum.x = 0;
 		}
-		if( lM.y < this->epsilon )
+		if( abs(lM.y) < this->epsilon )
 		{
-			state.SetLinearMomentum( Float3(lM.x, 0, lM.z) );
-			state.SetLinearImpulse( Float3(lM.x, 0, lM.z) );
+			state.linearMomentum.y = 0;
 		}
-		if( lM.z < this->epsilon )
+		if( abs(lM.z) < this->epsilon )
 		{
-			state.SetLinearMomentum( Float3(lM.x, lM.y, 0) );
-			state.SetLinearImpulse( Float3(lM.x, lM.y, 0) );
+			state.linearMomentum.z = 0;
 		}
 
+		(*proto)->SetState( state );
+
 		switch( (*proto)->Update(this->updateFrameLength) )
 		{
 		case UpdateState_altered:
diff --git a/Code/GamePhysics/PhysicsAPI.h b/Code/GamePhysics/PhysicsAPI.h
index 78b65f5e..080b8d30 100644
--- a/Code/GamePhysics/PhysicsAPI.h
+++ b/Code/GamePhysics/PhysicsAPI.h
@@ -34,7 +34,7 @@ namespace Oyster
 		namespace Constant
 		{
 			const float gravity_constant = (const float)6.67284e-11; //!< The _big_G_! ( N(m/kg)^2 ) Used in real gravityforcefields.
-			const float epsilon = (const float)1.0e-7;
+			const float epsilon = (const float)1.0e-3;
 		}
 
 		class PHYSICS_DLL_USAGE API
diff --git a/Code/GamePhysics/PhysicsStructs.h b/Code/GamePhysics/PhysicsStructs.h
index 00c07b2d..aef157a8 100644
--- a/Code/GamePhysics/PhysicsStructs.h
+++ b/Code/GamePhysics/PhysicsStructs.h
@@ -115,11 +115,13 @@ namespace Oyster { namespace Physics
 			bool IsDisturbed() const;
 			bool IsForwarded() const;
 
+			::Oyster::Math::Float3 linearMomentum;
+
 		private:
 			::Oyster::Math::Float mass, restitutionCoeff, staticFrictionCoeff, kineticFrictionCoeff;
 			::Oyster::Physics3D::MomentOfInertia inertiaTensor;
 			::Oyster::Math::Float3 reach, centerPos, angularAxis;
-			::Oyster::Math::Float3 linearMomentum, angularMomentum;
+			::Oyster::Math::Float3 angularMomentum;
 			::Oyster::Math::Float3 linearImpulse, angularImpulse;
 			::Oyster::Math::Float3 deltaPos, deltaAxis; // Forwarding data sum
 			::Oyster::Math::Float3 gravityNormal;
diff --git a/Code/OysterPhysics3D/OysterCollision3D.cpp b/Code/OysterPhysics3D/OysterCollision3D.cpp
index 169b5add..89b3af8a 100644
--- a/Code/OysterPhysics3D/OysterCollision3D.cpp
+++ b/Code/OysterPhysics3D/OysterCollision3D.cpp
@@ -784,8 +784,8 @@ namespace Oyster { namespace Collision3D { namespace Utility
 		if( Intersect(box, sphere) )
 		{
 			Float distance;
-			Ray ray( box.center, sphere.center - box.center );
-		
+			Ray ray( box.center, (sphere.center - box.center).Normalize() );
+			
 			Intersect( sphere, ray, distance );
 			worldPointOfContact = ray.origin + ray.direction*distance;
 			return true;
diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp
index 0c60c597..f053e8dd 100644
--- a/Code/OysterPhysics3D/RigidBody.cpp
+++ b/Code/OysterPhysics3D/RigidBody.cpp
@@ -50,7 +50,12 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength )
 	
 	// updating the linear
 	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
-	this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear );
+	Float3 deltaPos = ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear );
+	if( deltaPos.GetLength() < 0.001f )
+	{
+		deltaPos = Float3::null;
+	}
+	this->centerPos += deltaPos;
 	
 	// updating the angular
 	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H