RigidBody improved and Gimbal lock proofed

Code/GamePhysics/Implementation/SimpleRigidBody.cpp

@@ -43,7 +43,8 @@ namespace Private
 
 SimpleRigidBody::SimpleRigidBody()
 {
-	this->rigid = RigidBody( Box(Float4x4::identity, Float3::null, Float3(1.0f)), 16.0f, Float4x4::identity );
+	this->rigid = RigidBody();
+	this->rigid.SetMass_KeepMomentum( 16.0f );
 	this->gravityNormal = Float3::null;
 	this->collisionAction = Default::EventAction_Collision;
 	this->ignoreGravity = false;
@@ -52,9 +53,13 @@ SimpleRigidBody::SimpleRigidBody()
 
 SimpleRigidBody::SimpleRigidBody( const API::SimpleBodyDescription &desc )
 {
-	this->rigid = RigidBody( Box( desc.rotation, desc.centerPosition.xyz, desc.size.xyz  ),
+	this->rigid = RigidBody();
-							 desc.mass,
+	this->rigid.SetRotation( desc.rotation );
-							 desc.inertiaTensor );
+	this->rigid.centerPos = desc.centerPosition;
+	this->rigid.SetSize( desc.size );
+	this->rigid.SetMass_KeepMomentum( desc.mass );
+	this->rigid.SetMomentOfInertia_KeepMomentum( desc.inertiaTensor );
+
 	this->gravityNormal = Float3::null;
 	
 	if( desc.subscription )
@@ -81,31 +86,31 @@ SimpleRigidBody::State SimpleRigidBody::GetState() const
 {
 	return State( this->rigid.GetMass(), this->rigid.restitutionCoeff,
 				  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
-				  this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset,
+				  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
-				  this->rigid.box.center, AngularAxis(this->rigid.box.rotation),
+				  this->rigid.centerPos, this->rigid.axis,
-				  Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) );
+				  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
 }
 
 SimpleRigidBody::State & SimpleRigidBody::GetState( SimpleRigidBody::State &targetMem ) const
 {
 	return targetMem = State( this->rigid.GetMass(), this->rigid.restitutionCoeff,
 							  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
-							  this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset,
+							  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
-							  this->rigid.box.center, AngularAxis(this->rigid.box.rotation),
+							  this->rigid.centerPos, this->rigid.axis,
-							  Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) );
+							  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
 }
 
 void SimpleRigidBody::SetState( const SimpleRigidBody::State &state )
 {
-	this->rigid.box.boundingOffset = state.GetReach();
+	this->rigid.centerPos			  = state.GetCenterPosition();
-	this->rigid.box.center = state.GetCenterPosition();
+	this->rigid.SetRotation( state.GetRotation() );
-	this->rigid.box.rotation = state.GetRotation();
+	this->rigid.boundingReach		  = state.GetReach();
-	this->rigid.angularMomentum = state.GetAngularMomentum().xyz;
+	this->rigid.momentum_Linear		  = state.GetLinearMomentum();
-	this->rigid.linearMomentum = state.GetLinearMomentum().xyz;
+	this->rigid.momentum_Angular	  = state.GetAngularMomentum();
-	this->rigid.impulseTorqueSum += state.GetAngularImpulse().xyz;
+	this->rigid.impulse_Linear		 += state.GetLinearImpulse();
-	this->rigid.impulseForceSum += state.GetLinearImpulse().xyz;
+	this->rigid.impulse_Angular		 += state.GetAngularImpulse();
-	this->rigid.restitutionCoeff = state.GetRestitutionCoeff();
+	this->rigid.restitutionCoeff	  = state.GetRestitutionCoeff();
-	this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static();
+	this->rigid.frictionCoeff_Static  = state.GetFrictionCoeff_Static();
 	this->rigid.frictionCoeff_Kinetic = state.GetFrictionCoeff_Kinetic();
 
 	if( this->scene )
@@ -135,27 +140,27 @@ bool SimpleRigidBody::IsAffectedByGravity() const
 
 bool SimpleRigidBody::Intersects( const ICollideable &shape ) const
 {
-	return this->rigid.box.Intersects( shape );
+	return Box( this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize() ).Intersects( shape );
 }
 
 bool SimpleRigidBody::Intersects( const ICollideable &shape, Float4 &worldPointOfContact ) const
 {
-	return this->rigid.box.Intersects( shape, worldPointOfContact );
+	return Box( this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize() ).Intersects( shape, worldPointOfContact );
 }
 
 bool SimpleRigidBody::Intersects( const ICustomBody &object, Float4 &worldPointOfContact ) const
 {
-	return object.Intersects( this->rigid.box, worldPointOfContact );
+	return object.Intersects( Box(this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize()), worldPointOfContact );
 }
 
 Sphere & SimpleRigidBody::GetBoundingSphere( Sphere &targetMem ) const
 {
-	return targetMem = Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.GetMagnitude() );
+	return targetMem = Sphere( this->rigid.centerPos, this->rigid.boundingReach.GetMagnitude() );
 }
 
 Float4 & SimpleRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const
 {
-	Float4 offset = worldPos - this->rigid.box.center;
+	Float4 offset = worldPos - this->rigid.centerPos;
 	Float distance = offset.Dot( offset );
 	Float3 normal = Float3::null;
 
@@ -163,39 +168,40 @@ Float4 & SimpleRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem
 	{ // sanity check
 		Ray axis( Float4::standard_unit_w, offset / (Float)::std::sqrt(distance) );
 		Float minDistance = numeric_limits<Float>::max();
+		Float4x4 rotationMatrix = this->rigid.GetRotationMatrix();
 		
-		if( Private::Intersects(axis, Plane(this->rigid.box.xAxis, -this->rigid.box.boundingOffset.x), axis.collisionDistance) )
+		if( Private::Intersects(axis, Plane(rotationMatrix.v[0], -this->rigid.boundingReach.x), axis.collisionDistance) )
 		{ // check along x-axis
 			if( axis.collisionDistance < 0.0f )
-				normal = -this->rigid.box.xAxis.xyz;
+				normal = -rotationMatrix.v[0].xyz;
 			else
-				normal = this->rigid.box.xAxis.xyz;
+				normal = rotationMatrix.v[0].xyz;
 
 			minDistance = Abs( axis.collisionDistance );
 		}
 
-		if( Private::Intersects(axis, Plane(this->rigid.box.yAxis, -this->rigid.box.boundingOffset.y), axis.collisionDistance) )
+		if( Private::Intersects(axis, Plane(rotationMatrix.v[1], -this->rigid.boundingReach.y), axis.collisionDistance) )
 		{ // check along y-axis
 			distance = Abs( axis.collisionDistance ); // recycling memory
 			if( minDistance > distance )
 			{
 				if( axis.collisionDistance < 0.0f )
-					normal = -this->rigid.box.yAxis.xyz;
+					normal = -rotationMatrix.v[1].xyz;
 				else
-					normal = this->rigid.box.yAxis.xyz;
+					normal = rotationMatrix.v[1].xyz;
 
 				minDistance = distance;
 			}
 		}
 
-		if( Private::Intersects(axis, Plane(this->rigid.box.zAxis, -this->rigid.box.boundingOffset.z), axis.collisionDistance) )
+		if( Private::Intersects(axis, Plane(rotationMatrix.v[2], -this->rigid.boundingReach.z), axis.collisionDistance) )
 		{ // check along z-axis
 			if( minDistance > Abs( axis.collisionDistance ) )
 			{
 				if( axis.collisionDistance < 0.0f )
-					normal = -this->rigid.box.zAxis.xyz;
+					normal = -rotationMatrix.v[2].xyz;
 				else
-					normal = this->rigid.box.zAxis.xyz;
+					normal = rotationMatrix.v[2].xyz;
 			}
 		}
 	}
@@ -211,7 +217,7 @@ Float3 & SimpleRigidBody::GetGravityNormal( Float3 &targetMem ) const
 
 //Float3 & SimpleRigidBody::GetCenter( Float3 &targetMem ) const
 //{
-//	return targetMem = this->rigid.box.center;
+//	return targetMem = this->rigid.centerPos;
 //}
 //
 //Float4x4 & SimpleRigidBody::GetRotation( Float4x4 &targetMem ) const

Code/GamePhysics/Implementation/SphericalRigidBody.cpp

@@ -10,7 +10,8 @@ using namespace ::Utility::Value;
 
 SphericalRigidBody::SphericalRigidBody()
 {
-	this->rigid = RigidBody( Box(Float4x4::identity, Float3::null, Float3(1.0f)), 10.0f, Float4x4::identity );
+	this->rigid = RigidBody();
+	this->rigid.SetMass_KeepMomentum( 10.0f );
 	this->gravityNormal = Float3::null;
 	this->collisionAction = Default::EventAction_Collision;
 	this->ignoreGravity = false;
@@ -20,9 +21,13 @@ SphericalRigidBody::SphericalRigidBody()
 
 SphericalRigidBody::SphericalRigidBody( const API::SphericalBodyDescription &desc )
 {
-	this->rigid = RigidBody( Box( desc.rotation, desc.centerPosition.xyz, Float3(2.0f * desc.radius) ),
+	this->rigid = RigidBody();
-							 desc.mass,
+	this->rigid.SetRotation( desc.rotation );
-							 Formula::MomentOfInertia::CreateSphereMatrix( desc.mass, desc.radius ) );
+	this->rigid.centerPos = desc.centerPosition;
+	this->rigid.boundingReach = Float4( desc.radius, desc.radius, desc.radius, 0.0f );
+	this->rigid.SetMass_KeepMomentum( desc.mass );
+	this->rigid.SetMomentOfInertia_KeepMomentum( Formula::MomentOfInertia::CreateSphereMatrix( desc.mass, desc.radius ) );
+
 	this->gravityNormal = Float3::null;
 
 	if( desc.subscription )
@@ -50,31 +55,31 @@ SphericalRigidBody::State SphericalRigidBody::GetState() const
 {
 	return State( this->rigid.GetMass(), this->rigid.restitutionCoeff,
 				  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
-				  this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset,
+				  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
-				  this->rigid.box.center, AngularAxis(this->rigid.box.rotation),
+				  this->rigid.centerPos, this->rigid.axis,
-				  Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) );
+				  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
 }
 
 SphericalRigidBody::State & SphericalRigidBody::GetState( SphericalRigidBody::State &targetMem ) const
 {
 	return targetMem = State( this->rigid.GetMass(), this->rigid.restitutionCoeff,
 							  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
-							  this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset,
+							  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
-							  this->rigid.box.center, AngularAxis(this->rigid.box.rotation),
+							  this->rigid.centerPos, this->rigid.axis,
-							  Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) );
+							  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
 }
 
 void SphericalRigidBody::SetState( const SphericalRigidBody::State &state )
 {
-	this->rigid.box.boundingOffset = state.GetReach();
+	this->rigid.centerPos			  = state.GetCenterPosition();
-	this->rigid.box.center = state.GetCenterPosition();
+	this->rigid.SetRotation( state.GetRotation() );
-	this->rigid.box.rotation = state.GetRotation();
+	this->rigid.boundingReach		  = state.GetReach();
-	this->rigid.angularMomentum = state.GetAngularMomentum().xyz;
+	this->rigid.momentum_Linear		  = state.GetLinearMomentum();
-	this->rigid.linearMomentum = state.GetLinearMomentum().xyz;
+	this->rigid.momentum_Angular	  = state.GetAngularMomentum();
-	this->rigid.impulseTorqueSum += state.GetAngularImpulse().xyz;
+	this->rigid.impulse_Linear		 += state.GetLinearImpulse();
-	this->rigid.impulseForceSum += state.GetLinearImpulse().xyz;
+	this->rigid.impulse_Angular		 += state.GetAngularImpulse();
-	this->rigid.restitutionCoeff = state.GetRestitutionCoeff();
+	this->rigid.restitutionCoeff	  = state.GetRestitutionCoeff();
-	this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static();
+	this->rigid.frictionCoeff_Static  = state.GetFrictionCoeff_Static();
 	this->rigid.frictionCoeff_Kinetic = state.GetFrictionCoeff_Kinetic();
 
 	if( this->scene )
@@ -104,17 +109,17 @@ bool SphericalRigidBody::IsAffectedByGravity() const
 
 bool SphericalRigidBody::Intersects( const ICollideable &shape ) const
 {
-	return Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.x ).Intersects( shape );
+	return Sphere( this->rigid.centerPos, this->rigid.boundingReach.x ).Intersects( shape );
 }
 
 bool SphericalRigidBody::Intersects( const ICollideable &shape, Float4 &worldPointOfContact ) const
 {
-	return Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.x ).Intersects( shape, worldPointOfContact );
+	return Sphere( this->rigid.centerPos, this->rigid.boundingReach.x ).Intersects( shape, worldPointOfContact );
 }
 
 bool SphericalRigidBody::Intersects( const ICustomBody &object, Float4 &worldPointOfContact ) const
 {
-	return object.Intersects( Sphere(this->rigid.box.center, this->rigid.box.boundingOffset.x), worldPointOfContact );
+	return object.Intersects( Sphere(this->rigid.centerPos, this->rigid.boundingReach.x), worldPointOfContact );
 }
 
 Sphere & SphericalRigidBody::GetBoundingSphere( Sphere &targetMem ) const
@@ -124,7 +129,7 @@ Sphere & SphericalRigidBody::GetBoundingSphere( Sphere &targetMem ) const
 
 Float4 & SphericalRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const
 {
-	targetMem = worldPos - this->rigid.box.center;
+	targetMem = worldPos - this->rigid.centerPos;
 	Float magnitude = targetMem.GetMagnitude();
 	if( magnitude != 0.0f )
 	{ // sanity check
@@ -141,7 +146,7 @@ Float3 & SphericalRigidBody::GetGravityNormal( Float3 &targetMem ) const
 
 //Float3 & SphericalRigidBody::GetCenter( Float3 &targetMem ) const
 //{
-//	return targetMem = this->rigid.box.center;
+//	return targetMem = this->rigid.centerPos;
 //}
 //
 //Float4x4 & SphericalRigidBody::GetRotation( Float4x4 &targetMem ) const
@@ -167,7 +172,7 @@ Float3 SphericalRigidBody::GetRigidLinearVelocity() const
 UpdateState SphericalRigidBody::Update( Float timeStepLength )
 {
 	this->rigid.Update_LeapFrog( timeStepLength );
-	this->body.center = this->rigid.GetCenter();
+	this->body.center = this->rigid.centerPos;
 
 	// compare previous and new state and return result
 	//return this->current == this->previous ? UpdateState_resting : UpdateState_altered;

Code/OysterMath/LinearMath.h

@@ -281,6 +281,106 @@ namespace LinearAlgebra3D
 																   0, 0, 0, 1 );
 	}
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Quaternion<ScalarType> Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector3<ScalarType> &normalizedAxis )
+	{
+		ScalarType r = radian * 0.5f,
+				   s = std::sin( r ),
+				   c = std::cos( r );
+
+		return ::LinearAlgebra::Quaternion<ScalarType>( normalizedAxis * s, c );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Quaternion<ScalarType> Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
+	{
+		ScalarType r = radian * 0.5f,
+				   s = std::sin( r ),
+				   c = std::cos( r );
+
+		return ::LinearAlgebra::Quaternion<ScalarType>( (normalizedAxis * s).xyz, c );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Quaternion<ScalarType> Rotation( const ::LinearAlgebra::Vector3<ScalarType> &angularAxis )
+	{
+		ScalarType radius = angularAxis.Dot( angularAxis );
+		if( radius != 0 )
+		{
+			radius = (ScalarType)::std::sqrt( radius );
+			return Rotation( radius, angularAxis / radius );
+		}
+		else
+		{
+			return ::LinearAlgebra::Quaternion<ScalarType>::identity;
+		}
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Quaternion<ScalarType> Rotation( const ::LinearAlgebra::Vector4<ScalarType> &angularAxis )
+	{
+		ScalarType radius = angularAxis.Dot( angularAxis );
+		if( radius != 0 )
+		{
+			radius = (ScalarType)::std::sqrt( radius );
+			return Rotation( radius, angularAxis / radius );
+		}
+		else
+		{
+			return ::LinearAlgebra::Quaternion<ScalarType>::identity;
+		}
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & RotationMatrix( const ::LinearAlgebra::Quaternion<ScalarType> &rotationQuaternion, ::LinearAlgebra::Matrix4x4<ScalarType> targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
+	{
+		::LinearAlgebra::Quaternion<ScalarType> conjugate = rotationQuaternion.GetConjugate();
+
+		targetMem.v[0] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(1,0,0)*conjugate).imaginary, 0 );
+		targetMem.v[1] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,1,0)*conjugate).imaginary, 0 );
+		targetMem.v[2] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,0,1)*conjugate).imaginary, 0 );
+		targetMem.v[3] = ::LinearAlgebra::Vector4<ScalarType>::standard_unit_w;
+		return targetMem;
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & OrientationMatrix( const ::LinearAlgebra::Quaternion<ScalarType> &rotationQuaternion, const ::LinearAlgebra::Vector3<ScalarType> &translation, ::LinearAlgebra::Matrix4x4<ScalarType> targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
+	{
+		::LinearAlgebra::Quaternion<ScalarType> conjugate = rotationQuaternion.GetConjugate();
+
+		targetMem.v[0] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(1,0,0)*conjugate).imaginary, 0 );
+		targetMem.v[1] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,1,0)*conjugate).imaginary, 0 );
+		targetMem.v[2] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,0,1)*conjugate).imaginary, 0 );
+		targetMem.v[3] = ::LinearAlgebra::Vector4<ScalarType>( translation, 1 );
+		return targetMem;
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & OrientationMatrix( const ::LinearAlgebra::Quaternion<ScalarType> &rotationQuaternion, const ::LinearAlgebra::Vector4<ScalarType> &translation, ::LinearAlgebra::Matrix4x4<ScalarType> targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
+	{
+		::LinearAlgebra::Quaternion<ScalarType> conjugate = rotationQuaternion.GetConjugate();
+
+		targetMem.v[0] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(1,0,0)*conjugate).imaginary, 0 );
+		targetMem.v[1] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,1,0)*conjugate).imaginary, 0 );
+		targetMem.v[2] = ::LinearAlgebra::Vector4<ScalarType>( (rotationQuaternion*::LinearAlgebra::Vector3<ScalarType>(0,0,1)*conjugate).imaginary, 0 );
+		targetMem.v[3] = translation;
+		return targetMem;
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & ViewMatrix( const ::LinearAlgebra::Quaternion<ScalarType> &rotationQuaternion, const ::LinearAlgebra::Vector3<ScalarType> &translation, ::LinearAlgebra::Matrix4x4<ScalarType> targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
+	{
+		OrientationMatrix( rotationQuaternion, translation, targetMem );
+		return InverseOrientationMatrix( targetMem, targetMem );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & ViewMatrix( const ::LinearAlgebra::Quaternion<ScalarType> &rotationQuaternion, const ::LinearAlgebra::Vector4<ScalarType> &translation, ::LinearAlgebra::Matrix4x4<ScalarType> targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
+	{
+		OrientationMatrix( rotationQuaternion, translation, targetMem );
+		return InverseOrientationMatrix( targetMem, targetMem );
+	}
+
 	template<typename ScalarType>
 	inline ::LinearAlgebra::Matrix3x3<ScalarType> & RotationMatrix_AxisX( const ScalarType &radian, ::LinearAlgebra::Matrix3x3<ScalarType> &targetMem = ::LinearAlgebra::Matrix3x3<ScalarType>() )
 	{
@@ -557,9 +657,17 @@ namespace LinearAlgebra3D
 	inline ::LinearAlgebra::Vector3<ScalarType> VectorProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &axis )
 	{ return axis * ( vector.Dot(axis) / axis.Dot(axis) ); }
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Vector4<ScalarType> VectorProjection( const ::LinearAlgebra::Vector4<ScalarType> &vector, const ::LinearAlgebra::Vector4<ScalarType> &axis )
+	{ return axis * ( vector.Dot(axis) / axis.Dot(axis) ); }
+
 	template<typename ScalarType>
 	inline ::LinearAlgebra::Vector3<ScalarType> NormalProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &normalizedAxis )
 	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Vector4<ScalarType> NormalProjection( const ::LinearAlgebra::Vector4<ScalarType> &vector, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
+	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
 }
 
 #include "Utilities.h"

Code/OysterMath/OysterMath.cpp

@@ -101,6 +101,51 @@ namespace Oyster { namespace Math3D
 		return ::LinearAlgebra3D::TranslationMatrix( position, targetMem );
 	}
 
+	Quaternion Rotation( Float radian, const Float3 &normalizedAxis )
+	{
+		return ::LinearAlgebra3D::Rotation( radian, normalizedAxis );
+	}
+
+	Quaternion Rotation( Float radian, const Float4 &normalizedAxis )
+	{
+		return ::LinearAlgebra3D::Rotation( radian, normalizedAxis );
+	}
+
+	Quaternion Rotation( const Float3 &angularAxis )
+	{
+		return ::LinearAlgebra3D::Rotation( angularAxis );
+	}
+
+	Quaternion Rotation( const Float4 &angularAxis )
+	{
+		return ::LinearAlgebra3D::Rotation( angularAxis );
+	}
+
+	Float4x4 & RotationMatrix( const Quaternion &rotationQuaternion, Float4x4 &targetMem )
+	{
+		return ::LinearAlgebra3D::RotationMatrix( rotationQuaternion, targetMem );
+	}
+
+	Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem )
+	{
+		return ::LinearAlgebra3D::OrientationMatrix( rotationQuaternion, translation, targetMem );
+	}
+
+	Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem )
+	{
+		return ::LinearAlgebra3D::OrientationMatrix( rotationQuaternion, translation, targetMem );
+	}
+
+	Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem )
+	{
+		return ::LinearAlgebra3D::ViewMatrix( rotationQuaternion, translation, targetMem );
+	}
+
+	Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem )
+	{
+		return ::LinearAlgebra3D::ViewMatrix( rotationQuaternion, translation, targetMem );
+	}
+
 	Float4x4 & RotationMatrix_AxisX( const Float &radian, Float4x4 &targetMem )
 	{
 		return ::LinearAlgebra3D::RotationMatrix_AxisX( radian, targetMem );
@@ -217,8 +262,18 @@ namespace Oyster { namespace Math3D
 		return ::LinearAlgebra3D::VectorProjection( vector, axis );
 	}
 
+	Float4 VectorProjection( const Float4 &vector, const Float4 &axis )
+	{
+		return ::LinearAlgebra3D::VectorProjection( vector, axis );
+	}
+
 	Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis )
 	{
 		return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis );
 	}
+
+	Float4 NormalProjection( const Float4 &vector, const Float4 &normalizedAxis )
+	{
+		return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis );
+	}
 } }

Code/OysterMath/OysterMath.h

@@ -9,17 +9,19 @@
 #include "LinearMath.h"
 #include <limits>
 
-namespace Oyster { namespace Math /// Oyster's native math library
+namespace Oyster { namespace Math //! Oyster's native math library
 {
-	typedef float Float; /// Oyster's native scalar is float
+	typedef float Float; //!< Oyster's native scalar is float
 
-	typedef ::LinearAlgebra::Vector2<Float> Float2; /// 2D Linear Vector for Oyster
+	typedef ::LinearAlgebra::Vector2<Float> Float2; //!< 2D Linear Vector for Oyster
-	typedef ::LinearAlgebra::Vector3<Float> Float3; /// 3D Linear Vector for Oyster
+	typedef ::LinearAlgebra::Vector3<Float> Float3; //!< 3D Linear Vector for Oyster
-	typedef ::LinearAlgebra::Vector4<Float> Float4; /// 4D Linear Vector for Oyster
+	typedef ::LinearAlgebra::Vector4<Float> Float4; //!< 4D Linear Vector for Oyster
 
-	typedef ::LinearAlgebra::Matrix2x2<Float> Float2x2; /// 2x2 Linear Matrix for Oyster
+	typedef ::LinearAlgebra::Matrix2x2<Float> Float2x2; //!< 2x2 Linear Matrix for Oyster
-	typedef ::LinearAlgebra::Matrix3x3<Float> Float3x3; /// 3x3 Linear Matrix for Oyster
+	typedef ::LinearAlgebra::Matrix3x3<Float> Float3x3; //!< 3x3 Linear Matrix for Oyster
-	typedef ::LinearAlgebra::Matrix4x4<Float> Float4x4; /// 4x4 Linear Matrix for Oyster
+	typedef ::LinearAlgebra::Matrix4x4<Float> Float4x4; //!< 4x4 Linear Matrix for Oyster
+
+	typedef ::LinearAlgebra::Quaternion<Float> Quaternion; //!< Quaternion for Oyster
 
 	typedef Float4x4 Matrix; // by popular demand
 	typedef Float2 Vector2;  // by popular demand
@@ -28,20 +30,20 @@ namespace Oyster { namespace Math /// Oyster's native math library
 
 	const Float pi = 3.1415926535897932384626433832795f;
 
-	/// Function Highly recommended to check at start, just in case current version is using a feature that might be available.
+ //! Function Highly recommended to check at start, just in case current version is using a feature that might be available.
-	/// @todo TODO: create a template UniquePointer to use here
+ //! @todo TODO: create a template UniquePointer to use here
 	bool IsSupported();
 
-	/// Creates a solution matrix for 'out´= 'targetMem' * 'in'.
+ //! Creates a solution matrix for 'out´= 'targetMem' * 'in'.
-	/// Returns false if there is no explicit solution.
+ //! Returns false if there is no explicit solution.
 	bool SuperpositionMatrix( const Float2x2 &in, const Float2x2 &out, Float2x2 &targetMem );
 
-	/// Creates a solution matrix for 'out´= 'targetMem' * 'in'.
+ //! Creates a solution matrix for 'out´= 'targetMem' * 'in'.
-	/// Returns false if there is no explicit solution.
+ //! Returns false if there is no explicit solution.
 	bool SuperpositionMatrix( const Float3x3 &in, const Float3x3 &out, Float3x3 &targetMem );
 
-	/// Creates a solution matrix for 'out´= 'targetMem' * 'in'.
+ //! Creates a solution matrix for 'out´= 'targetMem' * 'in'.
-	/// Returns false if there is no explicit solution.
+ //! Returns false if there is no explicit solution.
 	bool SuperpositionMatrix( const Float4x4 &in, const Float4x4 &out, Float4x4 &targetMem );
 } }
 
@@ -96,54 +98,54 @@ inline ::Oyster::Math::Float3x3 operator * ( const ::Oyster::Math::Float &left,
 inline ::Oyster::Math::Float4x4 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float4x4 &right )
 { return ::Oyster::Math::Float4x4(right) *= left; }
 
-namespace Oyster { namespace Math2D /// Oyster's native math library specialized for 2D
+namespace Oyster { namespace Math2D //! Oyster's native math library specialized for 2D
 {
 	using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace
 
-	/// If there is an Y-axis on a 2D plane, then there is an explicit X-axis on and that is what is returned.
+	//! If there is an Y-axis on a 2D plane, then there is an explicit X-axis on and that is what is returned.
-	/// Recommended too make sure that yAxis is normalized.
+	//! Recommended too make sure that yAxis is normalized.
 	Float2 X_AxisTo( const Float2 &yAxis );
 
-	/// If there is an X-axis on a 2D plane, then there is an explicit Y-axis and that is what is returned.
+	//! If there is an X-axis on a 2D plane, then there is an explicit Y-axis and that is what is returned.
-	/// Recommended too make sure that yAxis is normalized.
+	//! Recommended too make sure that yAxis is normalized.
 	Float2 Y_AxisTo( const Float2 &xAxis );
 
-	/// Sets and returns targetMem to a translationMatrix with position as translation. 
+	//! Sets and returns targetMem to a translationMatrix with position as translation. 
 	Float3x3 & TranslationMatrix( const Float2 &position, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as a counterclockwise rotationMatrix
+	//! Sets and returns targetMem as a counterclockwise rotationMatrix
 	Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem = Float3x3() );
 
-	/// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
+	//! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
 	Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem = Float2x2() );
 	
-	/// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
+	//! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
 	Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
+	//! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
 	Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
+	//! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
 	Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as an orientation Matrix with position as translation and radian rotation
+	//! Sets and returns targetMem as an orientation Matrix with position as translation and radian rotation
 	Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as an orientation Matrix with position as translation and local y-axis directed at lookAt
+	//! Sets and returns targetMem as an orientation Matrix with position as translation and local y-axis directed at lookAt
 	Float3x3 & OrientationMatrix( const Float2 &position, const Float2 &lookAt, Float3x3 &targetMem = Float3x3() );
 
-	/// Sets and returns targetMem as an orientation Matrix that is rotated around localCenterOfRotation and then translated with position.
+	//! Sets and returns targetMem as an orientation Matrix that is rotated around localCenterOfRotation and then translated with position.
-	/// TODO: not tested
+	//! TODO: not tested
 	Float3x3 & OrientationMatrix( const Float2 &position, Float radian, const Float2 &localCenterOfRotation, Float3x3 &targetMem = Float3x3() );
 
-	/// If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method.
+	//! If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method.
 	Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem = Float3x3() );
 
-	/// Returns targetmem after writing the rotation data from orientation, into it.
+	//! Returns targetmem after writing the rotation data from orientation, into it.
 	Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem = Float3x3() );
 } }
 
-namespace Oyster { namespace Math3D /// Oyster's native math library specialized for 3D
+namespace Oyster { namespace Math3D //! Oyster's native math library specialized for 3D
 {
 	using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace
 
@@ -156,35 +158,62 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	//! Extracts the angularAxis from orientationMatrix
 	Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix );
 
-	/// Sets and returns targetMem to a translationMatrix with position as translation. 
+	//! Sets and returns targetMem to a translationMatrix with position as translation. 
 	Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an counterclockwise rotation matrix around the global X-axis
+	/** @todo TODO: add doc */
+	Quaternion Rotation( Float radian, const Float3 &normalizedAxis );
+
+	/** @todo TODO: add doc */
+	Quaternion Rotation( Float radian, const Float3 &normalizedAxis );
+
+	/** @todo TODO: add doc */
+	Quaternion Rotation( const Float3 &angularAxis );
+
+	/** @todo TODO: add doc */
+	Quaternion Rotation( const Float4 &angularAxis );
+
+	/** @todo TODO: add doc */
+	Float4x4 & RotationMatrix( const Quaternion &rotationQuaternion, Float4x4 &targetMem = Float4x4() );
+
+	/** @todo TODO: add doc */
+	Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem = Float4x4() );
+
+	/** @todo TODO: add doc */
+	Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem = Float4x4() );
+
+	/** @todo TODO: add doc */
+	Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem = Float4x4() );
+	
+	/** @todo TODO: add doc */
+	Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem = Float4x4() );
+
+	//! Sets and returns targetMem as an counterclockwise rotation matrix around the global X-axis
 	Float4x4 & RotationMatrix_AxisX( const Float &radian, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an counterclockwise rotation matrix around the global Y-axis
+	//! Sets and returns targetMem as an counterclockwise rotation matrix around the global Y-axis
 	Float4x4 & RotationMatrix_AxisY( const Float &radian, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an counterclockwise rotation matrix around the global Z-axis
+	 //! Sets and returns targetMem as an counterclockwise rotation matrix around the global Z-axis
 	Float4x4 & RotationMatrix_AxisZ( const Float &radian, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an counterclockwise rotation matrix around the angularAxis.
+	 //! Sets and returns targetMem as an counterclockwise rotation matrix around the angularAxis.
 	Float4x4 & RotationMatrix( const Float3 &angularAxis, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an counterclockwise rotation matrix around the normalizedAxis.
+	 //! Sets and returns targetMem as an counterclockwise rotation matrix around the normalizedAxis.
-	/// Please make sure normalizedAxis is normalized.
+	 //! Please make sure normalizedAxis is normalized.
 	Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem = Float4x4() );
 
-	/// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
+	 //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
 	Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() );
 
-	/// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
+	 //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method.
 	Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem = Float4x4() );
 	
-	/// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
+	 //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
 	Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() );
 
-	/// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
+	 //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector
 	Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() );
 
 	/*******************************************************************
@@ -238,14 +267,14 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	//! @todo TODO: Add documentation and not tested
 	Float4x4 & ViewMatrix_LookAtPos( const Float3 &worldLookAt, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem = Float4x4() );
 
-	/// If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method.
+	//! If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method.
 	Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem = Float4x4() );
 
 	// O0 = T0 * R0
 	// O1 = T1 * T0 * R1 * R0
 	Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix );
 
-	/// Returns targetmem after writing the rotation data from orientation, into it.
+	//! Returns targetmem after writing the rotation data from orientation, into it.
 	Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem = Float4x4() );
 
 	/*******************************************************************
@@ -272,13 +301,19 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	 *******************************************************************/
 	Float4x4 & ProjectionMatrix_Perspective( const Float &verticalFoV, const Float &aspectRatio, const Float &nearClip = ::std::numeric_limits<Float>::epsilon(), const Float &farClip = ::std::numeric_limits<Float>::max(), Float4x4 &targetMem = Float4x4() );
 
-	/// returns the component vector of vector that is parallell with axis
+	//! returns the component vector of vector that is parallell with axis
 	Float3 VectorProjection( const Float3 &vector, const Float3 &axis );
 
-	/// returns the component vector of vector that is parallell with axis. Faster than VectorProjection.
+	//! returns the component vector of vector that is parallell with axis
+	Float4 VectorProjection( const Float4 &vector, const Float4 &axis );
+
+	//! returns the component vector of vector that is parallell with axis. Faster than VectorProjection.
 	Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis );
 
-	/// Helper inline function that sets and then returns targetMem = projection * view
+	//! returns the component vector of vector that is parallell with axis. Faster than VectorProjection.
+	Float4 NormalProjection( const Float4 &vector, const Float4 &normalizedAxis );
+
+	//! Helper inline function that sets and then returns targetMem = projection * view
 	inline Float4x4 & ViewProjectionMatrix( const Float4x4 &view, const Float4x4 &projection, Float4x4 &targetMem = Float4x4() )
 	{ return targetMem = projection * view; }
 
@@ -290,7 +325,7 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	inline Float4x4 TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee )
 	{ return transformer * transformee; }
 
-	/// Helper inline function that sets and then returns targetMem = transformer * transformee
+	//! Helper inline function that sets and then returns targetMem = transformer * transformee
 	inline Float4 & TransformVector( const Float4x4 &transformer, const Float4 &transformee, Float4 &targetMem = Float4() )
 	{ return targetMem = transformer * transformee; }
 } }

Code/OysterMath/Quaternion.h

@@ -22,8 +22,10 @@ namespace LinearAlgebra
 			char byte[sizeof(ScalarType[2])];
 		};
 
+		static const Quaternion<ScalarType> null;
+		static const Quaternion<ScalarType> identity;
+
 		Quaternion( );
-		Quaternion( const Quaternion &quaternion );
 		Quaternion( const Vector3<ScalarType> &imaginary, const ScalarType &real );
 
 		operator ScalarType* ( );
@@ -53,40 +55,54 @@ namespace LinearAlgebra
 //	Body
 ///////////////////////////////////////////////////////////////////////////////////
 	
+	template<typename ScalarType> const Quaternion<ScalarType> Quaternion<ScalarType>::null = Quaternion<ScalarType>( Vector3<ScalarType>((ScalarType)0), (ScalarType)0 );
+	template<typename ScalarType> const Quaternion<ScalarType> Quaternion<ScalarType>::identity = Quaternion<ScalarType>( Vector3<ScalarType>((ScalarType)0), (ScalarType)1 );
+
 	template<typename ScalarType>
 	Quaternion<ScalarType>::Quaternion( ) : imaginary(), real() {}
 	
 	template<typename ScalarType>
-	Quaternion<ScalarType>::Quaternion( const Quaternion &quaternion )
+	Quaternion<ScalarType>::Quaternion( const Vector3<ScalarType> &imaginary, const ScalarType &real )
-	{ this->imaginary = quaternion.imaginary; this->real = quaternion.real; }
+	{
-	
+		this->imaginary = imaginary;
-	template<typename ScalarType>
+		this->real = real;
-	Quaternion<ScalarType>::Quaternion( const Vector3<ScalarType> &_imaginary, const ScalarType &_real )
+	}
-	{ this->imaginary = _imaginary; this->real = _real; }
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType>::operator ScalarType* ( )
-	{ return this->element; }
+	{
+		return this->element;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType>::operator const ScalarType* ( ) const
-	{ return this->element; }
+	{
+		return this->element;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType>::operator char* ( )
-	{ return this->byte; }
+	{
+		return this->byte;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType>::operator const char* ( ) const
-	{ return this->byte; }
+	{
+		return this->byte;
+	}
 	
 	template<typename ScalarType>
 	inline ScalarType & Quaternion<ScalarType>::operator [] ( int i )
-	{ return this->element[i]; }
+	{
+		return this->element[i];
+	}
 	
 	template<typename ScalarType>
 	inline const ScalarType & Quaternion<ScalarType>::operator [] ( int i ) const
-	{ return this->element[i]; }
+	{
+		return this->element[i];
+	}
 	
 	template<typename ScalarType>
 	Quaternion<ScalarType> & Quaternion<ScalarType>::operator = ( const Quaternion<ScalarType> &quaternion )
@@ -154,27 +170,39 @@ namespace LinearAlgebra
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::operator * ( const ScalarType &scalar ) const
-	{ return Quaternion<ScalarType>(*this) *= scalar; }
+	{
+		return Quaternion<ScalarType>(*this) *= scalar;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::operator /  ( const ScalarType &scalar ) const
-	{ return Quaternion<ScalarType>(*this) /= scalar; }
+	{
+		return Quaternion<ScalarType>(*this) /= scalar;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::operator + ( const Quaternion<ScalarType> &quaternion ) const
-	{ return Quaternion<ScalarType>(*this) += quaternion; }
+	{
+		return Quaternion<ScalarType>(*this) += quaternion;
+	}
 	
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::operator - ( const Quaternion<ScalarType> &quaternion ) const
-	{ return Quaternion<ScalarType>(*this) -= quaternion; }
+	{
+		return Quaternion<ScalarType>(*this) -= quaternion;
+	}
 
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::operator - ( ) const
-	{ return Quaternion<ScalarType>(-this->imaginary, -this->real); }
+	{
+		return Quaternion<ScalarType>(-this->imaginary, -this->real);
+	}
 
 	template<typename ScalarType>
 	inline Quaternion<ScalarType> Quaternion<ScalarType>::GetConjugate( ) const
-	{ return Quaternion<ScalarType>(-this->imaginary, this->real ); }
+	{
+		return Quaternion<ScalarType>(-this->imaginary, this->real );
+	}
 }
 
 #endif

Code/OysterPhysics3D/Box.cpp

@@ -22,6 +22,13 @@ Box::Box( const Float4x4 &r, const Float3 &p, const Float3 &s ) : ICollideable(T
 	this->boundingOffset = Float4( s * 0.5f , 0.0f);
 }
 
+Box::Box( const Float4x4 &r, const Float4 &p, const Float4 &s ) : ICollideable(Type_box)
+{
+	this->rotation = r;
+	this->center = p;
+	this->boundingOffset = s * 0.5f;
+}
+
 Box::~Box( ) {}
 
 Box & Box::operator = ( const Box &box )

Code/OysterPhysics3D/Box.h

@@ -28,6 +28,7 @@ namespace Oyster { namespace Collision3D
 
 		Box( );
 		Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &size );
+		Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float4 &worldPos, const ::Oyster::Math::Float4 &size );
 		virtual ~Box( );
 
 		Box & operator = ( const Box &box );

Code/OysterPhysics3D/OysterPhysics3D.h

@@ -192,6 +192,15 @@ namespace Oyster { namespace Physics3D
 			return worldOffset.Cross( linearVelocity );
 		}
 
+		/******************************************************************
+		 * Returns the world angular velocity of a mass in rotation.
+		 * @todo TODO: improve doc
+		 ******************************************************************/
+		inline ::Oyster::Math::Float4 AngularVelocity( const ::Oyster::Math::Float4 &linearVelocity, const ::Oyster::Math::Float4 &worldOffset )
+		{
+			return ::Oyster::Math::Float4( worldOffset.xyz.Cross( linearVelocity.xyz ), 0.0f );
+		}
+
 		/******************************************************************
 		 * Returns the world tangential velocity at worldPos, of a mass in rotation.
 		 * @todo TODO: improve doc
@@ -237,6 +246,15 @@ namespace Oyster { namespace Physics3D
 			return worldOffset.Cross( impulseForce );
 		}
 
+		/******************************************************************
+		 * 
+		 * @todo TODO: improve doc
+		 ******************************************************************/
+		inline ::Oyster::Math::Float4 ImpulseTorque( const ::Oyster::Math::Float4 & impulseForce, const ::Oyster::Math::Float4 &worldOffset )
+		{
+			return ::Oyster::Math::Float4( worldOffset.xyz.Cross(impulseForce.xyz), 0.0f );
+		}
+
 		/******************************************************************
 		 * T = I*a
 		 * @todo TODO: improve doc
@@ -246,6 +264,15 @@ namespace Oyster { namespace Physics3D
 			return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz;
 		}
 
+		/******************************************************************
+		 * T = I*a
+		 * @todo TODO: improve doc
+		 ******************************************************************/
+		inline ::Oyster::Math::Float4 ImpulseTorque( const ::Oyster::Math::Float4x4 & momentOfInertia, const ::Oyster::Math::Float4 &angularImpulseAcceleration )
+		{
+			return momentOfInertia * angularImpulseAcceleration;
+		}
+
 		namespace MomentOfInertia
 		{ /// Library of Formulas to calculate moment of inerta for simple shapes
 			/** @todo TODO: add MomentOfInertia tensor formulas */

Code/OysterPhysics3D/OysterPhysics3D.vcxproj

@@ -159,6 +159,7 @@
     <ClInclude Include="Point.h" />
     <ClInclude Include="Ray.h" />
     <ClInclude Include="RigidBody.h" />
+    <ClInclude Include="RigidBody_Inline.h" />
     <ClInclude Include="Sphere.h" />
     <ClInclude Include="Spring.h" />
     <ClInclude Include="Universe.h" />

Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters

@@ -75,6 +75,9 @@
     <ClInclude Include="RigidBody.h">
       <Filter>Header Files\Physics</Filter>
     </ClInclude>
+    <ClInclude Include="RigidBody_Inline.h">
+      <Filter>Header Files\Physics</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="Box.cpp">

Code/OysterPhysics3D/RigidBody.cpp

@@ -9,418 +9,201 @@ using namespace ::Oyster::Collision3D;
 using namespace ::Oyster::Physics3D;
 using namespace ::Oyster::Math3D;
 
-RigidBody::RigidBody( const Box &b, Float m, const Float4x4 &inertiaTensor )
+RigidBody::RigidBody( )
 { // by Dan Andersson
-	this->box = b;
+	this->centerPos = Float4::standard_unit_w;
-	this->angularMomentum = Float3::null;
+	this->axis = Float4::null;
-	this->linearMomentum = Float3::null;
+	this->boundingReach = Float4( 0.5f, 0.5f, 0.5f, 0.0f );
-	this->impulseTorqueSum = Float3::null;
+	this->momentum_Linear = Float4::null;
-	this->impulseForceSum = Float3::null;
+	this->momentum_Angular = Float4::null;
+	this->impulse_Linear = Float4::null;
+	this->impulse_Angular = Float4::null;
 	this->restitutionCoeff = 1.0f;
-	this->frictionCoeff_Static = 1.0f;
+	this->frictionCoeff_Static = 0.5f;
 	this->frictionCoeff_Kinetic = 1.0f;
-
+	this->mass = 10;
-	if( m != 0.0f )
+	this->momentOfInertiaTensor = Float4x4::identity;
-	{
+	this->rotation = Quaternion::identity;
-		this->mass = m;
-	}
-	else
-	{
-		this->mass = ::Utility::Value::numeric_limits<Float>::epsilon();
-	}
-
-	if( inertiaTensor.GetDeterminant() != 0.0f )
-	{
-		this->momentOfInertiaTensor = inertiaTensor;
-	}
-	else
-	{
-		this->momentOfInertiaTensor = Float4x4::identity;
-	}
 }
 
 RigidBody & RigidBody::operator = ( const RigidBody &body )
 { // by Dan Andersson
-	this->box = body.box;
+	this->centerPos = body.centerPos;
-	this->angularMomentum = body.angularMomentum;
+	this->axis = body.axis;
-	this->linearMomentum = body.linearMomentum;
+	this->boundingReach = body.boundingReach;
-	this->impulseTorqueSum = body.impulseTorqueSum;
+	this->momentum_Linear = body.momentum_Linear;
-	this->impulseForceSum = body.impulseForceSum;
+	this->momentum_Angular = body.momentum_Angular;
+	this->impulse_Linear = body.impulse_Linear;
+	this->impulse_Angular = body.impulse_Angular;
 	this->restitutionCoeff = body.restitutionCoeff;
 	this->frictionCoeff_Static = body.frictionCoeff_Static;
 	this->frictionCoeff_Kinetic = body.frictionCoeff_Kinetic;
 	this->mass = body.mass;
 	this->momentOfInertiaTensor = body.momentOfInertiaTensor;
+	this->rotation = body.rotation;
 	return *this;
 }
 
-bool RigidBody::operator == ( const RigidBody &body )
+void RigidBody::Update_LeapFrog( Float updateFrameLength )
-{
-	if( this->box.center != body.box.center )
-	{
-		return false;
-	}
-
-	if( this->box.rotation != body.box.rotation )
-	{
-		return false;
-	}
-
-	if( this->box.boundingOffset != body.box.boundingOffset )
-	{
-		return false;
-	}
-
-	if( this->angularMomentum != body.angularMomentum )
-	{
-		return false;
-	}
-
-	if( this->linearMomentum != body.linearMomentum )
-	{
-		return false;
-	}
-
-	if( this->impulseTorqueSum != body.impulseTorqueSum )
-	{
-		return false;
-	}
-
-	if( this->impulseForceSum != body.impulseForceSum )
-	{
-		return false;
-	}
-
-	return true;
-}
-
-bool RigidBody::operator != ( const RigidBody &body )
-{
-	return !this->operator==( body );
-}
-
-void RigidBody::Update_LeapFrog( Float deltaTime )
 { // by Dan Andersson: Euler leap frog update when Runga Kutta is not needed
 	
-	// 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( this->box.rotation, this->momentOfInertiaTensor ); // RI
-
 	// updating the linear
-	// dG = F  * dt
 	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
-	Float3 linearImpulse =  this->impulseForceSum; // HACK: * deltaTime; // aka deltaLinearMomentum
+	this->centerPos += ( updateFrameLength / this->mass ) * ::Utility::Value::AverageWithDelta( this->momentum_Linear, this->impulse_Linear );
-	Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse );
 
 	// updating the angular
-	// dH = T * dt
+	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
-	Float3 angularImpulse = this->impulseTorqueSum; // HACK: * deltaTime; // aka deltaAngularMomentum
+	this->axis += Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), ::Utility::Value::AverageWithDelta(this->momentum_Angular, this->impulse_Angular) );
-	Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(),
+	this->rotation = Rotation( this->axis );
-													::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) );
 
-	Float deltaRadian = rotationAxis.Dot( rotationAxis );
+	// update momentums and clear impulse_Linear and impulse_Angular
-	if( deltaRadian != 0.0f )
+	this->momentum_Linear += this->impulse_Linear;
-	{ // branch depending if there is rotation
+	this->impulse_Linear = Float4::null;
-		deltaRadian = ::std::sqrt( deltaRadian );
+	this->momentum_Angular += this->impulse_Angular;
-		rotationAxis /= deltaRadian;
+	this->impulse_Angular = Float4::null;
-
-		// using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box
-		this->box.center.xyz += deltaPos;
-		TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation );
-	}
-	else
-	{ // no rotation, only use deltaPos to translate the RigidBody
-		this->box.center.xyz += deltaPos;
-	}
-
-	// update momentums and clear impulseForceSum and impulseTorqueSum
-	this->linearMomentum += linearImpulse;
-	this->impulseForceSum = Float3::null;
-	this->angularMomentum += angularImpulse;
-	this->impulseTorqueSum = Float3::null;
 }
 
-void RigidBody::ApplyImpulseForce( const Float3 &worldF )
+void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos )
 { // by Dan Andersson
-	this->impulseForceSum += worldF;
+	Float4 worldOffset = atWorldPos - this->centerPos;
-}
+	if( worldOffset != Float4::null )
-
-void RigidBody::ApplyImpulseForceAt( const Float3 &worldF, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	if( worldOffset != Float3::null )
 	{
-		this->impulseForceSum += VectorProjection( worldF, worldOffset );
+		this->impulse_Linear += VectorProjection( worldJ, atWorldPos );
-		this->impulseTorqueSum += Formula::ImpulseTorque( worldF, worldOffset );
+		this->impulse_Angular += Formula::ImpulseTorque( worldJ, atWorldPos );
 	}
 	else
 	{
-		this->impulseForceSum += worldF;
+		this->impulse_Linear += worldJ;
 	}
 }
 
-void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &worldA )
-{ // by Dan Andersson
-	this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA );
-}
-
-void RigidBody::ApplyLinearImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	if( worldOffset != Float3::null )
-	{
-		this->impulseForceSum += Formula::ImpulseForce( this->mass, VectorProjection(worldA, worldOffset) );
-
-		// tanAcc = angularAcc x localPosition
-		// angularAcc = localPosition x tanAcc = localPosition x linearAcc
-		// T = I * angularAcc
-		this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, Formula::AngularImpulseAcceleration(worldA, worldOffset) );
-	}
-	else
-	{
-		this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA );
-	}
-}
-
-void RigidBody::ApplyImpulseTorque( const Float3 &worldT )
-{ // by Dan Andersson
-	this->impulseTorqueSum += worldT;
-}
-
-void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &worldA )
-{ // by Dan Andersson
-	this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA );
-}
-
-Float3 & RigidBody::AccessBoundingReach()
-{ // by Dan Andersson
-	return this->box.boundingOffset.xyz;
-}
-
-const Float3 & RigidBody::AccessBoundingReach() const
-{ // by Dan Andersson
-	return this->box.boundingOffset.xyz;
-}
-
-Float3 & RigidBody::AccessCenter()
-{ // by Dan Andersson
-	return this->box.center.xyz;
-}
-
-const Float3 & RigidBody::AccessCenter() const
-{ // by Dan Andersson
-	return this->box.center.xyz;
-}
-
 const Float4x4 & RigidBody::GetMomentOfInertia() const
 { // by Dan Andersson
 	return this->momentOfInertiaTensor;
 }
 
-const Float & RigidBody::GetMass() const
+Float RigidBody::GetMass() const
 { // by Dan Andersson
 	return this->mass;
 }
 
-const Float4x4 RigidBody::GetOrientation() const
+const Quaternion & RigidBody::GetRotation() const
 { // by Dan Andersson
-	return OrientationMatrix( this->box.rotation, this->box.center.xyz );
+	return this->rotation;
+}
+
+Float4x4 RigidBody::GetRotationMatrix() const
+{ // by Dan Andersson
+	return RotationMatrix( this->rotation );
+}
+
+Float4x4 RigidBody::GetOrientation() const
+{ // by Dan Andersson
+	return ::Oyster::Math3D::OrientationMatrix( this->rotation, this->centerPos );
 }
 
 Float4x4 RigidBody::GetView() const
 { // by Dan Andersson
-	return InverseOrientationMatrix( this->GetOrientation() );
+	return ViewMatrix( this->rotation, this->centerPos );
 }
 
-const Float3 & RigidBody::GetBoundingReach() const
+Float4 RigidBody::GetVelocity_Linear() const
 { // by Dan Andersson
-	return this->box.boundingOffset.xyz;
+	return Formula::LinearVelocity( this->mass, this->momentum_Linear );
 }
 
-Float3 RigidBody::GetSize() const
+Float4 RigidBody::GetVelocity_Angular() const
 { // by Dan Andersson
-	return 2.0f * this->box.boundingOffset.xyz;
+	return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->momentum_Angular );
 }
 
-const Float3 & RigidBody::GetCenter() const
+Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const
 { // by Dan Andersson
-	return this->box.center.xyz;
+	return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos );
 }
 
-const Float3 & RigidBody::GetImpulsTorque() const
+void RigidBody::SetMomentOfInertia_KeepVelocity( const Float4x4 &localTensorI )
 { // by Dan Andersson
-	return this->impulseTorqueSum;
+	if( localTensorI.GetDeterminant() != 0.0f )
-}
+	{ // insanity check! MomentOfInertiaTensor must be invertable
+		Float4x4 rotationMatrix; RotationMatrix( this->rotation, rotationMatrix );
 
-const Float3 & RigidBody::GetAngularMomentum() const
+		Float4 w = Formula::AngularVelocity( (rotationMatrix * this->momentOfInertiaTensor).GetInverse(), this->momentum_Angular );
-{ // by Dan Andersson
+		this->momentOfInertiaTensor = localTensorI;
-	return this->angularMomentum;
+		this->momentum_Angular = Formula::AngularMomentum( rotationMatrix * localTensorI, w );
-}
-
-Float3 RigidBody::GetAngularImpulseAcceleration() const
-{ // by Dan Andersson
-	return Formula::AngularImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum );
-}
-
-Float3 RigidBody::GetAngularVelocity() const
-{ // by Dan Andersson
-	return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum );
-}
-
-const Float3 & RigidBody::GetImpulseForce() const
-{ // by Dan Andersson
-	return this->impulseForceSum;
-}
-
-const Float3 & RigidBody::GetLinearMomentum() const
-{ // by Dan Andersson
-	return this->linearMomentum;
-}
-
-Float3 RigidBody::GetLinearImpulseAcceleration() const
-{ // by Dan Andersson
-	return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum );
-}
-
-Float3 RigidBody::GetLinearVelocity() const
-{ // by Dan Andersson
-	return Formula::LinearVelocity( this->mass, this->linearMomentum );
-}
-
-void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	Float3 momentum = Formula::TangentialLinearMomentum( this->angularMomentum, worldOffset );
-	momentum += this->linearMomentum;
-
-	normalMomentum = NormalProjection( momentum, surfaceNormal );
-	tangentialMomentum = momentum - normalMomentum;
-}
-
-void RigidBody::SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI )
-{ // by Dan Andersson
-	if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
-	{
-		Float3 w = Formula::AngularVelocity( (this->box.rotation * this->momentOfInertiaTensor).GetInverse(),
-											 this->angularMomentum );
-		this->momentOfInertiaTensor = localI;
-		this->angularMomentum = Formula::AngularMomentum( this->box.rotation*localI, w );
 	}
 }
 
-void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localI )
+void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localTensorI )
 { // by Dan Andersson
-	if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
+	if( localTensorI.GetDeterminant() != 0.0f )
-	{
+	{ // insanity check! MomentOfInertiaTensor must be invertable
-		this->momentOfInertiaTensor = localI;
+		this->momentOfInertiaTensor = localTensorI;
 	}
 }
 
 void RigidBody::SetMass_KeepVelocity( const Float &m )
 { // by Dan Andersson
-	if( m != 0.0f ) // insanitycheck! mass must be invertable
+	if( m != 0.0f )
-	{
+	{ // insanity check! Mass must be invertable
-		Float3 v = Formula::LinearVelocity( this->mass, this->linearMomentum );
+		Float4 v = Formula::LinearVelocity( this->mass, this->momentum_Linear );
 		this->mass = m;
-		this->linearMomentum = Formula::LinearMomentum( this->mass, v );
+		this->momentum_Linear = Formula::LinearMomentum( this->mass, v );
 	}
 }
 
 void RigidBody::SetMass_KeepMomentum( const Float &m )
 { // by Dan Anderson
-	if( m != 0.0f ) // insanitycheck! mass must be invertable
+	if( m != 0.0f )
-	{
+	{ // insanity check! Mass must be invertable
 		this->mass = m;
 	}
 }
 
 void RigidBody::SetOrientation( const Float4x4 &o )
 { // by Dan Andersson
-	 ExtractRotationMatrix( o, this->box.rotation );
+	this->axis = ExtractAngularAxis( o );
-	 this->box.center = o.v[3].xyz;
+	this->rotation = Rotation( this->axis );
-}
+	this->centerPos = o.v[3].xyz;
-
-void RigidBody::SetSize( const Float3 &widthHeight )
-{ // by Dan Andersson
-	this->box.boundingOffset = 0.5f * widthHeight;
-}
-
-void RigidBody::SetCenter( const Float3 &worldPos )
-{ // by Dan Andersson
-	this->box.center = worldPos;
 }
 
 void RigidBody::SetRotation( const Float4x4 &r )
 { // by Dan Andersson
-	this->box.rotation = r;
+	this->axis = ExtractAngularAxis( r );
+	this->rotation = Rotation( this->axis );
 }
 
-void RigidBody::SetImpulseTorque( const Float3 &worldT )
+void RigidBody::SetMomentum_Linear( const Float4 &worldG, const Float4 &atWorldPos )
 { // by Dan Andersson
-	this->impulseTorqueSum = worldT;
+	Float4 worldOffset = atWorldPos - this->centerPos;
+	this->momentum_Linear = VectorProjection( worldG, worldOffset );
+	this->momentum_Angular = Formula::AngularMomentum( worldG, worldOffset );
 }
 
-void RigidBody::SetAngularMomentum( const Float3 &worldH )
+void RigidBody::SetVelocity_Linear( const Float4 &worldV )
 { // by Dan Andersson
-	this->angularMomentum = worldH;
+	this->momentum_Linear = Formula::LinearMomentum( this->mass, worldV );
 }
 
-void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA )
+void RigidBody::SetVelocity_Linear( const Float4 &worldV, const Float4 &atWorldPos )
 { // by Dan Andersson
-	this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA );
+	Float4 worldOffset = atWorldPos - this->centerPos;
+	this->momentum_Linear = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) );
+	this->momentum_Angular = Formula::AngularMomentum( RotationMatrix(this->rotation) * this->momentOfInertiaTensor, Formula::AngularVelocity(worldV, worldOffset) );
 }
 
-void RigidBody::SetAngularVelocity( const Float3 &worldW )
+void RigidBody::SetVelocity_Angular( const Float4 &worldW )
 { // by Dan Andersson
-	this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW );
+	this->momentum_Angular = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW );
 }
 
-void RigidBody::SetImpulseForce( const Float3 &worldF )
+void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos )
 { // by Dan Andersson
-	this->impulseForceSum = worldF;
+	Float4 worldOffset = atWorldPos - this->centerPos;
-}
+	this->impulse_Linear = VectorProjection( worldJ, worldOffset );
-
+	this->impulse_Angular = Formula::ImpulseTorque( worldJ, worldOffset );
-void RigidBody::SetLinearMomentum( const Float3 &worldG )
-{ // by Dan Andersson
-	this->linearMomentum = worldG;
-}
-
-void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA )
-{ // by Dan Andersson
-	this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA );
-}
-
-void RigidBody::SetLinearVelocity( const Float3 &worldV )
-{ // by Dan Andersson
-	this->linearMomentum = Formula::LinearMomentum( this->mass, worldV );
-}
-
-void RigidBody::SetImpulseForceAt( const Float3 &worldForce, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	this->impulseForceSum = VectorProjection( worldForce, worldOffset );
-	this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset );
-}
-
-void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	this->linearMomentum = VectorProjection( worldG, worldOffset );
-	this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset );
-}
-
-void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	this->impulseForceSum = Formula::ImpulseForce( this->mass, VectorProjection(worldA, worldOffset) );
-	this->impulseTorqueSum = Formula::ImpulseTorque( this->box.rotation * this->momentOfInertiaTensor,
-													 Formula::AngularImpulseAcceleration(worldA, worldOffset) );
-}
-
-void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &worldPos )
-{ // by Dan Andersson
-	Float3 worldOffset = worldPos - this->box.center.xyz;
-	this->linearMomentum = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) );
-	this->angularMomentum = Formula::AngularMomentum( this->box.rotation * this->momentOfInertiaTensor,
-													  Formula::AngularVelocity(worldV, worldOffset) );
 }

Code/OysterPhysics3D/RigidBody.h

@@ -12,110 +12,74 @@
 namespace Oyster { namespace Physics3D
 {
 	struct RigidBody
-	{	/// A struct of a simple rigid body.
+	{ //! A struct of a simple rigid body.
 	public:
-		::Oyster::Collision3D::Box box;				/** Contains data representing physical presence.					(worldValue) */
+		::Oyster::Math::Float4		centerPos,				//!< Location of the body's center in the world.
-		::Oyster::Math::Float3 angularMomentum,		/** The angular momentum H (Nm*s) around an parallell axis.			(worldValue) */
+									axis,					//!< Euler rotationAxis of the body.
-							   linearMomentum,		/** The linear momentum G (kg*m/s).									(worldValue) */
+									boundingReach,			//!< 
-							   impulseTorqueSum,	/** The impulse torque T (Nm) that will be consumed each update.	(worldValue) */
+									momentum_Linear,		//!< The linear momentum G (kg*m/s).
-							   impulseForceSum;		/** The impulse force F (N) that will be consumed each update.		(worldValue) */
+									momentum_Angular,		//!< The angular momentum H (Nm*s) around an parallell axis.
-		::Oyster::Math::Float restitutionCoeff, frictionCoeff_Static, frictionCoeff_Kinetic;
+									impulse_Linear,			//!< The linear impulse sum Jl (kg*m/s) that will be consumed each update.
+									impulse_Angular;		//!< The angular impulse sum Ja (kg*m^2/s) that will be consumed each update.		
+		::Oyster::Math::Float		restitutionCoeff,		//!< 
+									frictionCoeff_Static,	//!< 
+									frictionCoeff_Kinetic;	//!< 
 
-		RigidBody( const ::Oyster::Collision3D::Box &box = ::Oyster::Collision3D::Box(),
+		RigidBody();
-				   ::Oyster::Math::Float mass = 12.0f,
-				   const ::Oyster::Math::Float4x4 &inertiaTensor = ::Oyster::Math::Float4x4::identity );
 
 		RigidBody & operator = ( const RigidBody &body );
 
-		bool operator == ( const RigidBody &body );
+		void Update_LeapFrog( ::Oyster::Math::Float updateFrameLength );
-		bool operator != ( const RigidBody &body );
 
-		void Update_LeapFrog( ::Oyster::Math::Float deltaTime );
+		void ApplyImpulse( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos );
-		void ApplyImpulseForce( const ::Oyster::Math::Float3 &worldF );
+		void ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ );
-		void ApplyImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos );
+		void ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ );
-		void ApplyLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
+		void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength );
-		void ApplyLinearImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos );
+		void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos );
-		void ApplyImpulseTorque( const ::Oyster::Math::Float3 &worldT );
-		void ApplyAngularImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
-
-		// ACCESS METHODS /////////////////////////////
-
-		::Oyster::Math::Float3 &		 AccessBoundingReach();
-		const ::Oyster::Math::Float3 &	 AccessBoundingReach() const;
-		::Oyster::Math::Float3 &		 AccessCenter();
-		const ::Oyster::Math::Float3 &	 AccessCenter() const;
 
 		// GET METHODS ////////////////////////////////
 
-		const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const;
+		const ::Oyster::Math::Float4x4 &   GetMomentOfInertia() const;
-		const ::Oyster::Math::Float &	 GetMass() const;
+			  ::Oyster::Math::Float		   GetMass() const;
-
+		const ::Oyster::Math::Quaternion & GetRotation() const;
-		const ::Oyster::Math::Float4x4   GetOrientation() const;
+			  ::Oyster::Math::Float4x4	   GetRotationMatrix() const;
-		::Oyster::Math::Float4x4		 GetView() const;
+			  ::Oyster::Math::Float4x4	   GetOrientation() const;
- 		const ::Oyster::Math::Float4x4 & GetToWorldMatrix() const;
+			  ::Oyster::Math::Float4x4	   GetView() const;
-		::Oyster::Math::Float4x4		 GetToLocalMatrix() const;
+ 			  ::Oyster::Math::Float4x4	   GetToWorldMatrix() const;
-
+			  ::Oyster::Math::Float4x4	   GetToLocalMatrix() const;
-		const ::Oyster::Math::Float3 &	 GetBoundingReach() const;
+			  ::Oyster::Math::Float4	   GetSize() const;
-		::Oyster::Math::Float3			 GetSize() const;
+			  ::Oyster::Math::Float4	   GetVelocity_Linear() const;
-
+			  ::Oyster::Math::Float4	   GetVelocity_Angular() const;
-		const ::Oyster::Math::Float3 &	 GetCenter() const;
+			  ::Oyster::Math::Float4	   GetLinearMomentum( const ::Oyster::Math::Float4 &atWorldPos ) const;
-
-		const ::Oyster::Math::Float3 &	 GetImpulsTorque() const;
-		const ::Oyster::Math::Float3 &	 GetAngularMomentum() const;
-		::Oyster::Math::Float3			 GetAngularImpulseAcceleration() const;
-		::Oyster::Math::Float3			 GetAngularVelocity() const;
-		
-		const ::Oyster::Math::Float3 &	 GetImpulseForce() const;
-		const ::Oyster::Math::Float3 &	 GetLinearMomentum() const;
-		::Oyster::Math::Float3			 GetLinearImpulseAcceleration() const;
-		::Oyster::Math::Float3			 GetLinearVelocity() const;
-
-		void GetMomentumAt( const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &surfaceNormal, ::Oyster::Math::Float3 &normalMomentum, ::Oyster::Math::Float3 &tangentialMomentum ) const;
 
 		// SET METHODS ////////////////////////////////
 
-		void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI );
+		void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localTensorI );
-		void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localI );
+		void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localTensorI );
 		void SetMass_KeepVelocity( const ::Oyster::Math::Float &m );
 		void SetMass_KeepMomentum( const ::Oyster::Math::Float &m );
 
 		void SetOrientation( const ::Oyster::Math::Float4x4 &o );
-		void SetSize( const ::Oyster::Math::Float3 &widthHeight );
-		void SetCenter( const ::Oyster::Math::Float3 &worldPos );
 		void SetRotation( const ::Oyster::Math::Float4x4 &r );
+		void SetSize( const ::Oyster::Math::Float4 &widthHeight );
 
-		void SetImpulseTorque( const ::Oyster::Math::Float3 &worldT );
+		void SetMomentum_Linear( const ::Oyster::Math::Float4 &worldG, const ::Oyster::Math::Float4 &atWorldPos );
-		void SetAngularMomentum( const ::Oyster::Math::Float3 &worldH );
-		void SetAngularImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
-		void SetAngularVelocity( const ::Oyster::Math::Float3 &worldW );
 
-		void SetImpulseForce( const ::Oyster::Math::Float3 &worldF );
+		void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV );
-		void SetLinearMomentum( const ::Oyster::Math::Float3 &worldG );
+		void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV, const ::Oyster::Math::Float4 &atWorldPos );
-		void SetLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
+		void SetVelocity_Angular( const ::Oyster::Math::Float4 &worldW );
-		void SetLinearVelocity( const ::Oyster::Math::Float3 &worldV );
 		
-		void SetImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos );
+		void SetImpulse_Linear( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos );
-		void SetLinearMomentumAt( const ::Oyster::Math::Float3 &worldG, const ::Oyster::Math::Float3 &worldPos );
+		void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength );
-		void SetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos );
+		void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos );
-		void SetLinearVelocityAt( const ::Oyster::Math::Float3 &worldV, const ::Oyster::Math::Float3 &worldPos );
 
 	private:
-		::Oyster::Math::Float mass; /** m (kg) */
+		::Oyster::Math::Float mass;						//!< m (kg)
-		::Oyster::Math::Float4x4 momentOfInertiaTensor; /** I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration )	(localValue) */
+		::Oyster::Math::Float4x4 momentOfInertiaTensor;	//!< I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration )	(localValue)
+		::Oyster::Math::Quaternion rotation;			//!< RotationAxis of the body.
 	};
-
-	// INLINE IMPLEMENTATIONS ///////////////////////////////////////
-
-	inline const ::Oyster::Math::Float4x4 & RigidBody::GetToWorldMatrix() const
-	{
-		return this->GetOrientation();
-	}
-
-	inline ::Oyster::Math::Float4x4 RigidBody::GetToLocalMatrix() const
-	{
-		return this->GetView();
-	}
-
 } }
 
+#include "RigidBody_Inline.h"
+
 #endif

Code/OysterPhysics3D/RigidBody_Inline.h

@@ -0,0 +1,65 @@
+/////////////////////////////////////////////////////////////////////
+// INLINE IMPLEMENTATIONS
+// Created by Dan Andersson 2013
+/////////////////////////////////////////////////////////////////////
+
+#ifndef OYSTER_PHYSICS_3D_RIGIDBODY_INLINE_H
+#define OYSTER_PHYSICS_3D_RIGIDBODY_INLINE_H
+
+#include "RigidBody.h"
+
+namespace Oyster { namespace Physics3D
+{
+	inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ )
+	{
+		this->impulse_Linear += worldJ;
+	}
+
+	inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ )
+	{
+		this->impulse_Angular += worldJ;
+	}
+
+	inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength )
+	{
+		this->impulse_Linear += worldF * updateFrameLength;
+	}
+
+	inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos )
+	{
+		this->ApplyImpulse( worldF * updateFrameLength, atWorldPos );
+	}
+
+	inline ::Oyster::Math::Float4x4 RigidBody::GetToWorldMatrix() const
+	{
+		return this->GetOrientation();
+	}
+
+	inline ::Oyster::Math::Float4x4 RigidBody::GetToLocalMatrix() const
+	{
+		return this->GetView();
+	}
+
+	inline ::Oyster::Math::Float4 RigidBody::GetSize() const
+	{
+		return 2.0f * this->boundingReach;
+	}
+
+	inline void RigidBody::SetSize( const ::Oyster::Math::Float4 &widthHeight )
+	{
+		this->boundingReach = ::Utility::Value::Abs( 0.5f * widthHeight );
+	}
+
+	inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength )
+	{
+		this->impulse_Linear = worldF * updateFrameLength;
+	}
+
+	inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos )
+	{
+		this->SetImpulse_Linear( worldF * updateFrameLength, atWorldPos );
+	}
+
+} }
+
+#endif