Merge branch 'Physics' of https://github.com/dean11/Danbias into GameLogic

Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp

@@ -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.linearMomentum.x = 0;
-			state.SetLinearImpulse( Float3(0, lM.y, lM.z) );
 		}
-		if( lM.y < this->epsilon )
+		if( abs(lM.y) < this->epsilon )
 		{
-			state.SetLinearMomentum( Float3(lM.x, 0, lM.z) );
+			state.linearMomentum.y = 0;
-			state.SetLinearImpulse( Float3(lM.x, 0, lM.z) );
 		}
-		if( lM.z < this->epsilon )
+		if( abs(lM.z) < this->epsilon )
 		{
-			state.SetLinearMomentum( Float3(lM.x, lM.y, 0) );
+			state.linearMomentum.z = 0;
-			state.SetLinearImpulse( Float3(lM.x, lM.y, 0) );
 		}
 
+		(*proto)->SetState( state );
+
 		switch( (*proto)->Update(this->updateFrameLength) )
 		{
 		case UpdateState_altered:

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

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;

Code/OysterMath/LinearMath.h

@@ -325,6 +325,12 @@ namespace LinearAlgebra2D
 
 namespace LinearAlgebra3D
 {
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Vector3<ScalarType> WorldAxisOf( const ::LinearAlgebra::Quaternion<ScalarType> &rotation, const ::LinearAlgebra::Vector3<ScalarType> &localAxis )
+	{
+		return (rotation*localAxis*rotation.GetConjugate()).imaginary;
+	}
+
 	// All Matrix to AngularAxis conversions here is incorrect
 	//template<typename ScalarType>
 	//inline ::LinearAlgebra::Vector4<ScalarType> AngularAxis( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotationMatrix )
@@ -741,6 +747,23 @@ namespace LinearAlgebra3D
 	inline ::LinearAlgebra::Vector4<ScalarType> NormalProjection( const ::LinearAlgebra::Vector4<ScalarType> &vector, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
 	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
 
+	template<typename ScalarType>
+	::LinearAlgebra::Vector4<ScalarType> & SnapAngularAxis( ::LinearAlgebra::Vector4<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector4<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector4<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector4<ScalarType> &targetMem = ::LinearAlgebra::Vector4<ScalarType>() )
+	{
+		::LinearAlgebra::Vector4<ScalarType> worldStartNormal( WorldAxisOf(Rotation(startAngularAxis.xyz), localStartNormal), (ScalarType)0 );
+		targetMem = ::LinearAlgebra::Vector4<ScalarType>( worldStartNormal.xyz.Cross(worldEndNormal.xyz), (ScalarType)0);
+		targetMem *= (ScalarType)::std::acos( worldStartNormal.Dot(worldEndNormal) );
+		return targetMem += startAngularAxis;
+	}
+
+	template<typename ScalarType>
+	::LinearAlgebra::Vector3<ScalarType> & SnapAngularAxis( ::LinearAlgebra::Vector3<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector3<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector3<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector3<ScalarType> &targetMem = ::LinearAlgebra::Vector3<ScalarType>() )
+	{
+		return targetMem = SnapAngularAxis( ::LinearAlgebra::Vector4<ScalarType>(startAngularAxis, (ScalarType)0),
+											::LinearAlgebra::Vector4<ScalarType>(localStartNormal, (ScalarType)0),
+											::LinearAlgebra::Vector4<ScalarType>(worldEndNormal, (ScalarType)0) ).xyz;
+	}
+
 	template<typename ScalarType>
 	::LinearAlgebra::Matrix4x4<ScalarType> & SnapAxisYToNormal_UsingNlerp( ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
 	{

Code/OysterMath/OysterMath.h

@@ -324,6 +324,7 @@ namespace Oyster { namespace Math3D //! Oyster's native math library specialized
 	using ::LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp;
 	using ::LinearAlgebra3D::InterpolateOrientation_UsingNonRigidNlerp;
 	using ::LinearAlgebra3D::InterpolateOrientation_UsingSlerp;
+	using ::LinearAlgebra3D::SnapAngularAxis;
 } }
 
 #endif

Code/OysterPhysics3D/OysterCollision3D.cpp

@@ -784,7 +784,7 @@ 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;

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