Inertia big patch

It's ali-..WORKS! It works!

Code/GamePhysics/PhysicsStructs-Impl.h

@@ -15,7 +15,7 @@ namespace Oyster
 				this->rotation = ::Oyster::Math::Float4x4::identity;
 				this->centerPosition = ::Oyster::Math::Float3::null;
 				this->size = ::Oyster::Math::Float3( 1.0f );
-				this->mass = 12.0f;
+				this->mass = 6.0f;
 				this->restitutionCoeff = 1.0f;
 				this->frictionCoeff_Dynamic = 0.5f;
 				this->frictionCoeff_Static = 0.5f;
@@ -124,7 +124,7 @@ namespace Oyster
 
 			inline const ::Oyster::Math::Float3 & CustomBodyState::GetAngularAxis() const
 			{
-				return this->angularAxis;
+				return ::Utility::Value::Radian(this->angularAxis);
 			}
 
 			inline ::Oyster::Math::Float4x4 CustomBodyState::GetRotation() const
@@ -201,8 +201,6 @@ namespace Oyster
 			{
 				if( m != 0.0f )
 				{ // sanity block!
-					// Formula::LinearMomentum( m, Formula::LinearVelocity(this->mass, this->linearMomentum) )
-					// is the same as (this->linearMomentum / this->mass) * m = (m / this->mass) * this->linearMomentum
 					this->linearMomentum *= (m / this->mass);
 					this->mass = m;
 				}
@@ -263,19 +261,6 @@ namespace Oyster
 				this->isSpatiallyAltered = this->isDisturbed = true;
 			}
 
-			/*inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float4x4 &rotation )
-			{
-				this->SetRotation( ::Oyster::Math3D::AngularAxis(rotation) );
-			}
-
-			inline void CustomBodyState::SetOrientation( const ::Oyster::Math::Float4x4 &orientation )
-			{
-				this->SetRotation( ::Oyster::Math3D::ExtractAngularAxis(orientation) );
-				this->SetCenterPosition( orientation.v[3] );
-			}*/
-
-			
-
 			inline void CustomBodyState::SetLinearMomentum( const ::Oyster::Math::Float3 &g )
 			{
 				this->linearMomentum = g;
@@ -334,8 +319,8 @@ namespace Oyster
 				::Oyster::Math::Float3 offset = at - this->centerPos;
 				::Oyster::Math::Float3 deltaAngularImpulse = ::Oyster::Physics3D::Formula::AngularMomentum( j, offset );
 				this->linearImpulse += j - ::Oyster::Physics3D::Formula::TangentialLinearMomentum( deltaAngularImpulse, offset );
-				this->angularImpulse += deltaAngularImpulse;
 
+				this->angularImpulse += deltaAngularImpulse;
 				this->isDisturbed = true;
 			}
 

Code/OysterPhysics3D/Inertia.cpp

@@ -32,10 +32,17 @@ Float3 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, con
 }
 
 Float3 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h, Float3 &targetMem ) const
-{ // w = (R * I_R) * I_M^-1 * (R * I_R)^-1 * h
+{ // w = h * | (2/3) * I_M^-1 (R I_R)^-1 h | / |h|
-	Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation );
+	Float hMagnitudeSquared = h.Dot( h );
-	Float4 w = rotation.GetInverse() * Float4( h, 0.0f );
+	if( hMagnitudeSquared > 0.0f )
-	return targetMem = rotation * w.PiecewiseMultiplicationAdd( Float4(1.0f / this->magnitude.x, 1.0f / this->magnitude.y, 1.0f / this->magnitude.z, 0.0f) );
+	{
+		Float4x4 rotationInverse = (RotationMatrix( externR ) * RotationMatrix( this->rotation )).Transpose();
+		Float4 v = rotationInverse * Float4( h, 0.0f );
+		v.PiecewiseMultiplicationAdd( Float4((2.0f/3.0f) / this->magnitude.x, (2.0f/3.0f) / this->magnitude.y, (2.0f/3.0f) / this->magnitude.z, 0.0f) );
+		return targetMem = (Float4( h, 0.0f ) * ( v.GetMagnitude() / ( (Float)::std::sqrt(hMagnitudeSquared)) ) ).xyz;
+	}
+	else
+		return targetMem = Float3::null;
 }
 
 Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w ) const
@@ -44,8 +51,15 @@ Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, con
 }
 
 Float3 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w, Float3 &targetMem ) const
-{ // h = (R * I_R) * I_M * (R * I_R)^-1 * w
+{ // h = w * | (3/2) * I_M (R I_R)^-1 w | / |w|
-	Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation );
+	Float wMagnitudeSquared = w.Dot( w );
-	Float4 h = rotation.GetInverse() * Float4( w, 0.0f );
+	if( wMagnitudeSquared > 0.0f )
-	return targetMem = rotation * h.PiecewiseMultiplicationAdd( Float4(this->magnitude.x, this->magnitude.y, this->magnitude.z, 0.0f) );
+	{
+		Float4x4 rotationInverse = (RotationMatrix( externR ) * RotationMatrix( this->rotation )).Transpose();
+		Float4 v = rotationInverse * Float4( w, 0.0f );
+		v.PiecewiseMultiplicationAdd( Float4((3.0f/2.0f) * this->magnitude.x, (3.0f/2.0f) * this->magnitude.y, (3.0f/2.0f) * this->magnitude.z, 0.0f) );
+		return targetMem = (Float4( w, 0.0f ) * ( v.GetMagnitude() / (Float)::std::sqrt(wMagnitudeSquared) ) ).xyz;
+	}
+	else
+		return targetMem = Float3::null;
 }

Code/OysterPhysics3D/OysterPhysics3D.h

@@ -113,6 +113,7 @@ namespace Oyster { namespace Physics3D
 
 		/******************************************************************
 		 * Returns the world angular momentum of a mass in rotation.
+		 * H = r x G
 		 * @todo TODO: improve doc
 		 ******************************************************************/
 		inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &worldOffset )
@@ -381,7 +382,6 @@ namespace Oyster { namespace Physics3D
 
 				return inertia;
 			}
-		
 		}
 	}
 } }

Code/OysterPhysics3D/RigidBody.cpp

@@ -53,22 +53,15 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength )
 	this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear );
 	
 	// updating the angular
-	//Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix );
-	// Important! The member data is all world data except the Inertia tensor. Thus a new InertiaTensor needs to be created to be compatible with the rest of the world data.
-	//Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI
-
 	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H	
-	
+	this->axis += updateFrameLength * this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) );
-	//! HACK: @todo Rotation temporary disabled
-	//this->axis += Radian( Formula::AngularVelocity(wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular)) );
-	this->axis += this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) );
 	this->rotation = Rotation( this->axis );
 
 	// update momentums and clear impulse_Linear and impulse_Angular
 	this->momentum_Linear += this->impulse_Linear;
 	this->impulse_Linear = Float4::null;
 
-	//this->momentum_Angular += this->impulse_Angular; //! HACK: @todo Rotation temporary disabled
+	this->momentum_Angular += this->impulse_Angular; //! HACK: @todo Rotation temporary disabled
 	this->impulse_Angular = Float4::null;
 }
 
@@ -183,19 +176,6 @@ void RigidBody::SetMass_KeepMomentum( const Float &m )
 	}
 }
 
-//void RigidBody::SetOrientation( const Float4x4 &o )
-//{ // by Dan Andersson
-//	this->axis = ExtractAngularAxis( o );
-//	this->rotation = Rotation( this->axis );
-//	this->centerPos = o.v[3].xyz;
-//}
-//
-//void RigidBody::SetRotation( const Float4x4 &r )
-//{ // by Dan Andersson
-//	this->axis = ExtractAngularAxis( r );
-//	this->rotation = Rotation( this->axis );
-//}
-
 void RigidBody::SetMomentum_Linear( const Float3 &worldG, const Float3 &atWorldPos )
 { // by Dan Andersson
 	Float3 worldOffset = atWorldPos - this->centerPos;