Merge remote-tracking branch 'origin/Physics' into GameLogicBranch

Code/GamePhysics/GamePhysics.vcxproj

@@ -145,7 +145,14 @@
     </ProjectReference>
   </ItemGroup>
   <ItemGroup>
-    <ClInclude Include="Include\GamePhysics.h" />
+    <ClInclude Include="Implementation\PhysicsAPI_Impl.h" />
+    <ClInclude Include="Implementation\SimpleRigidBody.h" />
+    <ClInclude Include="PhysicsAPI.h" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="Implementation\NullBody.cpp" />
+    <ClCompile Include="Implementation\PhysicsAPI_Impl.cpp" />
+    <ClCompile Include="Implementation\SimpleRigidBody.cpp" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">

Code/GamePhysics/GamePhysics.vcxproj.filters

@@ -16,16 +16,30 @@
     <Filter Include="Header Files\Implementation">
       <UniqueIdentifier>{f2cb55b8-47a0-45c7-8dce-5b93f945a57b}</UniqueIdentifier>
     </Filter>
-    <Filter Include="Source Files\Implementation">
-      <UniqueIdentifier>{cac9a78f-f09b-4850-b1aa-ea87e8368678}</UniqueIdentifier>
-    </Filter>
     <Filter Include="Header Files\Include">
       <UniqueIdentifier>{792daa4b-b2f7-4664-9529-71a929365274}</UniqueIdentifier>
     </Filter>
   </ItemGroup>
   <ItemGroup>
-    <ClInclude Include="Include\GamePhysics.h">
+    <ClInclude Include="PhysicsAPI.h">
       <Filter>Header Files\Include</Filter>
     </ClInclude>
+    <ClInclude Include="Implementation\PhysicsAPI_Impl.h">
+      <Filter>Header Files\Implementation</Filter>
+    </ClInclude>
+    <ClInclude Include="Implementation\SimpleRigidBody.h">
+      <Filter>Header Files\Implementation</Filter>
+    </ClInclude>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="Implementation\PhysicsAPI_Impl.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Implementation\SimpleRigidBody.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Implementation\NullBody.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>

Code/GamePhysics/Implementation/NullBody.cpp

@@ -0,0 +1,83 @@
+#include "..\PhysicsAPI.h"
+
+using namespace ::Oyster::Physics;
+using namespace ::Oyster::Physics::Error;
+using namespace ::Oyster::Math;
+using namespace ::Oyster::Collision3D;
+using namespace ::Utility::DynamicMemory;
+
+NullBody::~NullBody() {}
+
+UniquePointer<ICustomBody> NullBody::Clone() const	
+{
+	return new NullBody( *this );
+}
+
+bool NullBody::IsSubscribingCollisions() const
+{
+	return false;
+}
+
+bool NullBody::Intersects( const ICustomBody &object, Float &deltaWhen, Float3 &worldPointOfContact ) const
+{
+	return false;
+}
+
+bool NullBody::Intersects( const ICollideable &shape ) const
+{
+	return false;
+}
+
+unsigned int NullBody::GetReference() const
+{
+	return not_a_reference;
+}
+
+Sphere & NullBody::GetBoundingSphere( Sphere &targetMem ) const
+{
+	return targetMem = Sphere( Float3::null, 0.0f );
+}
+
+Float3 & NullBody::GetNormalAt( const Float3 &worldPos, Float3 &targetMem ) const
+{
+	return targetMem = Float3::standard_unit_z;
+}
+
+Float3 & NullBody::GetCenter( Float3 &targetMem ) const
+{
+	return targetMem = Float3::null;
+}
+
+Float4x4 & NullBody::GetRotation( Float4x4 &targetMem ) const
+{
+	return targetMem = Float4x4::identity;
+}
+
+Float4x4 & NullBody::GetOrientation( Float4x4 &targetMem ) const
+{
+	return targetMem = Float4x4::identity;
+}
+
+Float4x4 & NullBody::GetView( Float4x4 &targetMem ) const
+{
+	return targetMem = Float4x4::identity;
+}
+
+UpdateState NullBody::Update( Float timeStepLength )
+{
+	return resting;
+}
+
+void NullBody::SetMomentOfInertiaTensor_KeepVelocity( const Float4x4 &localI ) {}
+
+void NullBody::SetMomentOfInertiaTensor_KeepMomentum( const Float4x4 &localI ) {}
+
+void NullBody::SetMass_KeepVelocity( Float m ) {}
+
+void NullBody::SetMass_KeepMomentum( Float m ) {}
+
+void NullBody::SetCenter( const Float3 &worldPos ) {}
+
+void NullBody::SetRotation( const Float4x4 &rotation ) {}
+
+void NullBody::SetOrientation( const Float4x4 &orientation ) {}

Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp

@@ -0,0 +1,129 @@
+#include "PhysicsAPI_Impl.h"
+#include "SimpleRigidBody.h"
+
+using namespace ::Oyster::Physics;
+using namespace ::Oyster::Math;
+using namespace ::Oyster::Collision3D;
+using namespace ::Utility::DynamicMemory;
+
+API_Impl instance;  
+
+API & Instance()
+{
+	return instance;
+}
+
+API_Impl::API_Impl()
+{
+	/** @todo TODO: Fix this constructor.*/
+}
+
+API_Impl::~API_Impl()
+{
+	/** @todo TODO: Fix this destructor.*/
+}
+
+void API_Impl::SetDeltaTime( float deltaTime )
+{
+	/** @todo TODO: Fix this function.*/
+}
+void API_Impl::SetGravityConstant( float g )
+{
+	/** @todo TODO: Fix this function.*/
+}
+void API_Impl::SetAction( EventAction_Collision functionPointer )
+{
+	/** @todo TODO: Fix this function.*/
+}
+void API_Impl::SetAction( EventAction_Destruction functionPointer )
+{
+	/** @todo TODO: Fix this function.*/
+}
+
+void API_Impl::Update()
+{
+	/** @todo TODO: Fix this function.*/
+}
+
+bool API_Impl::IsInLimbo( unsigned int objRef )
+{
+	//! @todo TODO: implement stub
+	return true;
+}
+
+void API_Impl::MoveToLimbo( unsigned int objRef )
+{
+	/** @todo TODO: Fix this function.*/
+}
+void API_Impl::ReleaseFromLimbo( unsigned int objRef )
+{
+	/** @todo TODO: Fix this function.*/
+}
+
+unsigned int API_Impl::AddObject( ::Utility::DynamicMemory::UniquePointer<ICustomBody> handle )
+{
+	/** @todo TODO: Fix this function.*/
+
+	return 0;
+}
+
+::Utility::DynamicMemory::UniquePointer<ICustomBody> ExtractObject( unsigned int objRef )
+{
+	//! @todo TODO: implement stub
+	return NULL;
+}
+
+void API_Impl::DestroyObject( unsigned int objRef )
+{
+	/** @todo TODO: Fix this function.*/
+}
+
+void API_Impl::ApplyForceAt( unsigned int objRef, const Float3 &worldPos, const Float3 &worldF )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::ApplyCollisionResponse( unsigned int objRefA, unsigned int objRefB, Float &deltaWhen, Float3 &worldPointOfContact )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetMomentOfInertiaTensor_KeepVelocity( unsigned int objRef, const Float4x4 &localI )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetMomentOfInertiaTensor_KeepMomentum( unsigned int objRef, const Float4x4 &localI )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetMass_KeepVelocity( unsigned int objRef, Float m )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetMass_KeepMomentum( unsigned int objRef, Float m )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetCenter( unsigned int objRef, const Float3 &worldPos )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetRotation( unsigned int objRef, const Float4x4 &rotation )
+{
+	//! @todo TODO: implement stub
+}
+
+void API_Impl::SetOrientation( unsigned int objRef, const Float4x4 &orientation )
+{
+	//! @todo TODO: implement stub
+}
+
+UniquePointer<ICustomBody> API_Impl::CreateSimpleRigidBody() const
+{
+	return new SimpleRigidBody();
+}

Code/GamePhysics/Implementation/PhysicsAPI_Impl.h

@@ -0,0 +1,51 @@
+#ifndef PHYSICS_API_IMPL_H
+#define PHYSICS_API_IMPL_H
+
+#include "../PhysicsAPI.h"
+
+namespace Oyster
+{
+	namespace Physics
+	{
+		class API_Impl : public API
+		{
+		public:
+			API_Impl();
+			virtual ~API_Impl();
+
+			void SetDeltaTime( float deltaTime );
+			void SetGravityConstant( float g );
+			void SetAction( EventAction_Collision functionPointer );
+			void SetAction( EventAction_Destruction functionPointer );
+
+			void Update();
+
+			bool IsInLimbo( unsigned int objRef );
+			void MoveToLimbo( unsigned int objRef );
+			void ReleaseFromLimbo( unsigned int objRef );
+
+			unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer<ICustomBody> handle );
+			::Utility::DynamicMemory::UniquePointer<ICustomBody> ExtractObject( unsigned int objRef );
+			void DestroyObject( unsigned int objRef );
+
+			const ICustomBody & Peek( unsigned int objRef ) const;
+
+			void ApplyForceAt( unsigned int objRef, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &worldF );
+			void ApplyCollisionResponse( unsigned int objRefA, unsigned int objRefB, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact );
+
+			void SetMomentOfInertiaTensor_KeepVelocity( unsigned int objRef, const ::Oyster::Math::Float4x4 &localI );
+			void SetMomentOfInertiaTensor_KeepMomentum( unsigned int objRef, const ::Oyster::Math::Float4x4 &localI );
+			void SetMass_KeepVelocity( unsigned int objRef, ::Oyster::Math::Float m );
+			void SetMass_KeepMomentum( unsigned int objRef, ::Oyster::Math::Float m );
+			void SetCenter( unsigned int objRef, const ::Oyster::Math::Float3 &worldPos );
+			void SetRotation( unsigned int objRef, const ::Oyster::Math::Float4x4 &rotation );
+			void SetOrientation( unsigned int objRef, const ::Oyster::Math::Float4x4 &orientation );
+
+			::Utility::DynamicMemory::UniquePointer<ICustomBody> CreateSimpleRigidBody() const;
+		};
+	
+	}
+
+}
+
+#endif

Code/GamePhysics/Implementation/SimpleRigidBody.cpp

@@ -0,0 +1,122 @@
+#include "SimpleRigidBody.h"
+
+using namespace ::Oyster::Physics;
+using namespace ::Oyster::Math;
+using namespace ::Oyster::Collision3D;
+using namespace ::Utility::DynamicMemory;
+
+SimpleRigidBody::SimpleRigidBody()
+{
+	//! @todo TODO: implement stub
+}
+
+SimpleRigidBody::~SimpleRigidBody()
+{
+	//! @todo TODO: implement stub
+}
+
+UniquePointer<ICustomBody> SimpleRigidBody::Clone() const	
+{
+	return new SimpleRigidBody( *this );
+}
+
+bool SimpleRigidBody::IsSubscribingCollisions() const
+{
+	//! @todo TODO: implement stub
+	return false;
+}
+
+bool SimpleRigidBody::Intersects( const ICustomBody &object, Float &deltaWhen, Float3 &worldPointOfContact ) const
+{
+	//! @todo TODO: implement stub
+	return false;
+}
+
+bool SimpleRigidBody::Intersects( const ICollideable &shape ) const
+{
+	//! @todo TODO: implement stub
+	return false;
+}
+
+unsigned int SimpleRigidBody::GetReference() const
+{
+	//! @todo TODO: implement stub
+	return Error::not_a_reference;
+}
+
+Sphere & SimpleRigidBody::GetBoundingSphere( Sphere &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Sphere( Float3::null, 0.0f );
+}
+
+Float3 & SimpleRigidBody::GetNormalAt( const Float3 &worldPos, Float3 &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Float3::standard_unit_z;
+}
+
+Float3 & SimpleRigidBody::GetCenter( Float3 &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Float3::null;
+}
+
+Float4x4 & SimpleRigidBody::GetRotation( Float4x4 &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Float4x4::identity;
+}
+
+Float4x4 & SimpleRigidBody::GetOrientation( Float4x4 &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Float4x4::identity;
+}
+
+Float4x4 & SimpleRigidBody::GetView( Float4x4 &targetMem ) const
+{
+	//! @todo TODO: implement stub
+	return targetMem = Float4x4::identity;
+}
+
+UpdateState SimpleRigidBody::Update( Float timeStepLength )
+{
+	//! @todo TODO: implement stub
+	return resting;
+}
+
+void SimpleRigidBody::SetMomentOfInertiaTensor_KeepVelocity( const Float4x4 &localI )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetMomentOfInertiaTensor_KeepMomentum( const Float4x4 &localI )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetMass_KeepVelocity( Float m )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetMass_KeepMomentum( Float m )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetCenter( const Float3 &worldPos )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetRotation( const Float4x4 &rotation )
+{
+	//! @todo TODO: implement stub
+}
+
+void SimpleRigidBody::SetOrientation( const Float4x4 &orientation )
+{
+	//! @todo TODO: implement stub
+}

Code/GamePhysics/Implementation/SimpleRigidBody.h

@@ -0,0 +1,42 @@
+#ifndef OYSTER_PHYSICS_SIMPLE_RIGIDBODY_H
+#define OYSTER_PHYSICS_SIMPLE_RIGIDBODY_H
+
+#include "..\PhysicsAPI.h"
+
+namespace Oyster { namespace Physics
+{
+	class SimpleRigidBody : public ICustomBody
+	{
+	public:
+		SimpleRigidBody();
+		virtual ~SimpleRigidBody();
+
+		::Utility::DynamicMemory::UniquePointer<ICustomBody> Clone() const;
+		
+		bool IsSubscribingCollisions() const;
+		bool Intersects( const ICustomBody &object, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact ) const;
+		bool Intersects( const ::Oyster::Collision3D::ICollideable &shape ) const;
+
+		unsigned int					GetReference() const;
+		::Oyster::Collision3D::Sphere &	GetBoundingSphere( ::Oyster::Collision3D::Sphere &targetMem = ::Oyster::Collision3D::Sphere() ) const;
+		::Oyster::Math::Float3 &		GetNormalAt( const ::Oyster::Math::Float3 &worldPos, ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const;
+		::Oyster::Math::Float3 &		GetCenter( ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const;
+		::Oyster::Math::Float4x4 &		GetRotation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+		::Oyster::Math::Float4x4 &		GetOrientation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+		::Oyster::Math::Float4x4 &		GetView( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+
+		UpdateState Update( ::Oyster::Math::Float timeStepLength );
+
+		void SetMomentOfInertiaTensor_KeepVelocity( const ::Oyster::Math::Float4x4 &localI );
+		void SetMomentOfInertiaTensor_KeepMomentum( const ::Oyster::Math::Float4x4 &localI );
+		void SetMass_KeepVelocity( ::Oyster::Math::Float m );
+		void SetMass_KeepMomentum( ::Oyster::Math::Float m );
+		void SetCenter( const ::Oyster::Math::Float3 &worldPos );
+		void SetRotation( const ::Oyster::Math::Float4x4 &rotation );
+		void SetOrientation( const ::Oyster::Math::Float4x4 &orientation );
+
+	private:
+	};
+} }
+
+#endif

Code/GamePhysics/Include/PhysicsAPI.h

@@ -1,36 +0,0 @@
-#ifndef PHYSICS_API_H
-#define PHYSICS_API_H
-
-#include "OysterMath.h"
-
-namespace Oyster
-{
-	namespace Physics
-	{
-		class API;
-		class IRigidBody;
-		class IParticle;	
-
-		class API
-		{
-		public:
-			static API & Instance();
-		};
-
-		class IRigidBody
-		{
-		public:
-
-		};
-
-		class IParticle
-		{
-		public:
-
-		};
-	}
-
-	namespace Collision
-	{}
-}
-#endif

Code/GamePhysics/PhysicsAPI.h

@@ -0,0 +1,135 @@
+#ifndef PHYSICS_API_H
+#define PHYSICS_API_H
+
+#include "OysterCollision3D.h"
+#include "OysterMath.h"
+#include "Utilities.h"
+
+namespace Oyster
+{
+	namespace Physics
+	{
+		class API;
+		class ICustomBody;
+
+		enum UpdateState
+		{
+			resting,
+			altered
+		};
+
+		namespace Constant
+		{
+			const float gravity_constant = (const float)6.67284e-11; // The _big_G_! ( N(m/kg)^2 ) Used in real gravityforcefields.
+		}
+
+		class API
+		{
+		public:
+			typedef void (*EventAction_Collision)( unsigned int, unsigned int );
+			typedef void (*EventAction_Destruction)( unsigned int, ::Utility::DynamicMemory::UniquePointer<ICustomBody> );
+
+			static API & Instance();
+
+			virtual void SetDeltaTime( float deltaTime ) = 0;
+			virtual void SetGravityConstant( float g ) = 0;
+			virtual void SetAction( EventAction_Collision functionPointer ) = 0;
+			virtual void SetAction( EventAction_Destruction functionPointer ) = 0;
+
+			virtual void Update() = 0;
+
+			virtual bool IsInLimbo( unsigned int objRef ) = 0;
+			virtual void MoveToLimbo( unsigned int objRef ) = 0;
+			virtual void ReleaseFromLimbo( unsigned int objRef ) = 0;
+
+			virtual unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer<ICustomBody> handle ) = 0;
+			virtual ::Utility::DynamicMemory::UniquePointer<ICustomBody> ExtractObject( unsigned int objRef ) = 0;
+			virtual void DestroyObject( unsigned int objRef ) = 0;
+
+			virtual const ICustomBody & Peek( unsigned int objRef ) const = 0;
+
+			virtual void ApplyForceAt( unsigned int objRef, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &worldF ) = 0;
+			virtual void ApplyCollisionResponse( unsigned int objRefA, unsigned int objRefB, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact ) = 0;
+
+			virtual void SetMomentOfInertiaTensor_KeepVelocity( unsigned int objRef, const ::Oyster::Math::Float4x4 &localI ) = 0;
+			virtual void SetMomentOfInertiaTensor_KeepMomentum( unsigned int objRef, const ::Oyster::Math::Float4x4 &localI ) = 0;
+			virtual void SetMass_KeepVelocity( unsigned int objRef, ::Oyster::Math::Float m ) = 0;
+			virtual void SetMass_KeepMomentum( unsigned int objRef, ::Oyster::Math::Float m ) = 0;
+			virtual void SetCenter( unsigned int objRef, const ::Oyster::Math::Float3 &worldPos ) = 0;
+			virtual void SetRotation( unsigned int objRef, const ::Oyster::Math::Float4x4 &rotation ) = 0;
+			virtual void SetOrientation( unsigned int objRef, const ::Oyster::Math::Float4x4 &orientation ) = 0;
+
+			virtual ::Utility::DynamicMemory::UniquePointer<ICustomBody> CreateSimpleRigidBody() const = 0;
+		
+		protected:
+			virtual ~API() {}
+		};
+	
+		class ICustomBody
+		{
+		public:
+			virtual ~ICustomBody() {};
+
+			virtual ::Utility::DynamicMemory::UniquePointer<ICustomBody> Clone() const = 0;
+			
+			virtual bool IsSubscribingCollisions() const = 0;
+			virtual bool Intersects( const ICustomBody &object, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact ) const = 0;
+			virtual bool Intersects( const ::Oyster::Collision3D::ICollideable &shape ) const = 0;
+
+			virtual unsigned int					GetReference() const = 0;
+			virtual ::Oyster::Collision3D::Sphere &	GetBoundingSphere( ::Oyster::Collision3D::Sphere &targetMem = ::Oyster::Collision3D::Sphere() ) const = 0;
+			virtual ::Oyster::Math::Float3 &		GetNormalAt( const ::Oyster::Math::Float3 &worldPos, ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const = 0;
+			virtual ::Oyster::Math::Float3 &		GetCenter( ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const = 0;
+			virtual ::Oyster::Math::Float4x4 &		GetRotation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const = 0;
+			virtual ::Oyster::Math::Float4x4 &		GetOrientation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const = 0;
+			virtual ::Oyster::Math::Float4x4 &		GetView( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const = 0;
+
+			virtual UpdateState Update( ::Oyster::Math::Float timeStepLength ) = 0;
+
+			virtual void SetMomentOfInertiaTensor_KeepVelocity( const ::Oyster::Math::Float4x4 &localI ) = 0;
+			virtual void SetMomentOfInertiaTensor_KeepMomentum( const ::Oyster::Math::Float4x4 &localI ) = 0;
+			virtual void SetMass_KeepVelocity( ::Oyster::Math::Float m ) = 0;
+			virtual void SetMass_KeepMomentum( ::Oyster::Math::Float m ) = 0;
+			virtual void SetCenter( const ::Oyster::Math::Float3 &worldPos ) = 0;
+			virtual void SetRotation( const ::Oyster::Math::Float4x4 &rotation ) = 0;
+			virtual void SetOrientation( const ::Oyster::Math::Float4x4 &orientation ) = 0;
+		};
+
+		namespace Error
+		{
+			const unsigned int not_a_reference = ::Utility::Value::numeric_limits<unsigned int>::max();
+
+			class NullBody : public ICustomBody
+			{
+			public:
+				virtual ~NullBody();
+
+				::Utility::DynamicMemory::UniquePointer<ICustomBody> Clone() const;
+			
+				bool IsSubscribingCollisions() const;
+				bool Intersects( const ICustomBody &object, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact ) const;
+				bool Intersects( const ::Oyster::Collision3D::ICollideable &shape ) const;
+
+				unsigned int					GetReference() const;
+				::Oyster::Collision3D::Sphere &	GetBoundingSphere( ::Oyster::Collision3D::Sphere &targetMem = ::Oyster::Collision3D::Sphere() ) const;
+				::Oyster::Math::Float3 &		GetNormalAt( const ::Oyster::Math::Float3 &worldPos, ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const;
+				::Oyster::Math::Float3 &		GetCenter( ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const;
+				::Oyster::Math::Float4x4 &		GetRotation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+				::Oyster::Math::Float4x4 &		GetOrientation( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+				::Oyster::Math::Float4x4 &		GetView( ::Oyster::Math::Float4x4 &targetMem = ::Oyster::Math::Float4x4() ) const;
+
+				UpdateState Update( ::Oyster::Math::Float timeStepLength );
+
+				void SetMomentOfInertiaTensor_KeepVelocity( const ::Oyster::Math::Float4x4 &localI );
+				void SetMomentOfInertiaTensor_KeepMomentum( const ::Oyster::Math::Float4x4 &localI );
+				void SetMass_KeepVelocity( ::Oyster::Math::Float m );
+				void SetMass_KeepMomentum( ::Oyster::Math::Float m );
+				void SetCenter( const ::Oyster::Math::Float3 &worldPos );
+				void SetRotation( const ::Oyster::Math::Float4x4 &rotation );
+				void SetOrientation( const ::Oyster::Math::Float4x4 &orientation );
+
+			} const nobody;
+		}
+	}
+}
+#endif

Code/OysterMath/LinearMath.h

@@ -161,6 +161,32 @@ namespace LinearAlgebra2D
 		return targetMem = ::LinearAlgebra::Matrix3x3<ScalarType>( c,-s, 0, s, c, 0, 0, 0, 1 );
 	}
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix2x2<ScalarType> & InverseRotationMatrix( const ::LinearAlgebra::Matrix2x2<ScalarType> &rotationMatrix )
+	{
+		return rotationMatrix.GetTranspose();
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix3x3<ScalarType> & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotationMatrix )
+	{
+		return rotationMatrix.GetTranspose();
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix3x3<ScalarType> OrientationMatrix( const ::LinearAlgebra::Matrix2x2<ScalarType> &rotation, const ::LinearAlgebra::Vector2<ScalarType> &translation )
+	{
+		return ::LinearAlgebra::Matrix3x3<ScalarType>( ::LinearAlgebra::Vector3<ScalarType>(rotation.v[0], 0),
+													   ::LinearAlgebra::Vector3<ScalarType>(rotation.v[1], 0),
+													   ::LinearAlgebra::Vector3<ScalarType>(translation, 1) );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix3x3<ScalarType> OrientationMatrix( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotation, const ::LinearAlgebra::Vector2<ScalarType> &translation )
+	{
+		return ::LinearAlgebra::Matrix3x3<ScalarType>( rotation.v[0], rotation.v[1], ::LinearAlgebra::Vector3<ScalarType>(translation, 1) );
+	}
+
 	template<typename ScalarType>
 	inline ::LinearAlgebra::Matrix3x3<ScalarType> & OrientationMatrix( const ScalarType &radian, const ::LinearAlgebra::Vector2<ScalarType> &position, ::LinearAlgebra::Matrix3x3<ScalarType> &targetMem = ::LinearAlgebra::Matrix3x3<ScalarType>() )
 	{
@@ -197,6 +223,12 @@ namespace LinearAlgebra2D
 																   orientationMatrix.m12, orientationMatrix.m22, -orientationMatrix.v[1].xy.Dot(orientationMatrix.v[2].xy),
 																   0, 0, 1 );
 	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix3x3<ScalarType> ExtractRotationMatrix( const ::LinearAlgebra::Matrix3x3<ScalarType> &orientationMatrix )
+	{
+		return ::LinearAlgebra::Matrix3x3<ScalarType>( orientationMatrix.v[0], orientationMatrix.v[1], ::LinearAlgebra::Vector3<ScalarType>::standard_unit_z );
+	}
 }
 
 namespace LinearAlgebra3D
@@ -283,6 +315,33 @@ namespace LinearAlgebra3D
 		return targetMem;
 	}
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix3x3<ScalarType> & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotationMatrix )
+	{
+		return rotationMatrix.GetTranspose();
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> & InverseRotationMatrix( const ::LinearAlgebra::Matrix4x4<ScalarType> &rotationMatrix )
+	{
+		return rotationMatrix.GetTranspose();
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> OrientationMatrix( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotation, const ::LinearAlgebra::Vector3<ScalarType> &translation )
+	{
+		return ::LinearAlgebra::Matrix4x4<ScalarType>( ::LinearAlgebra::Vector4<ScalarType>(rotation.v[0], 0),
+													   ::LinearAlgebra::Vector4<ScalarType>(rotation.v[1], 0),
+													   ::LinearAlgebra::Vector4<ScalarType>(rotation.v[2], 0),
+													   ::LinearAlgebra::Vector4<ScalarType>(translation, 1) );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> OrientationMatrix( const ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector3<ScalarType> &translation )
+	{
+		return ::LinearAlgebra::Matrix4x4<ScalarType>( rotation.v[0], rotation.v[1], rotation.v[2], ::LinearAlgebra::Vector4<ScalarType>(translation, 1) );
+	}
+
 	template<typename ScalarType>
 	::LinearAlgebra::Matrix4x4<ScalarType> & OrientationMatrix( const ::LinearAlgebra::Vector3<ScalarType> &normalizedAxis, const ScalarType &deltaRadian, const ::LinearAlgebra::Vector3<ScalarType> &sumTranslation, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
 	{ /** @todo TODO: not tested */
@@ -327,6 +386,23 @@ namespace LinearAlgebra3D
 		return targetMem;
 	}
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> OrientationMatrix_LookAtDirection( const ::LinearAlgebra::Vector3<ScalarType> &normalizedDirection, const ::LinearAlgebra::Vector3<ScalarType> &normalizedUpVector, const ::LinearAlgebra::Vector3<ScalarType> &worldPos )
+	{ // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis.
+		::LinearAlgebra::Vector3<ScalarType> right = normalizedDirection.Cross( normalizedUpVector ).GetNormalized();
+		return ::LinearAlgebra::Matrix4x4<ScalarType>( ::LinearAlgebra::Vector4<ScalarType>( right, 0.0f ),
+													   ::LinearAlgebra::Vector4<ScalarType>( right.Cross( normalizedDirection ), 0.0f ),
+													   ::LinearAlgebra::Vector4<ScalarType>( -normalizedDirection, 0.0f ),
+													   ::LinearAlgebra::Vector4<ScalarType>( worldPos, 1.0f ) );
+	}
+
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> OrientationMatrix_LookAtPos( const ::LinearAlgebra::Vector3<ScalarType> &worldLookAt, const ::LinearAlgebra::Vector3<ScalarType> &normalizedUpVector, const ::LinearAlgebra::Vector3<ScalarType> &worldPos )
+	{ // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis.
+		::LinearAlgebra::Vector3<ScalarType> direction = ( worldLookAt - worldPos ).GetNormalized();
+		return OrientationMatrix_LookAtDirection( direction, normalizedUpVector, worldPos );
+	}
+
 	template<typename ScalarType>
 	inline ::LinearAlgebra::Matrix4x4<ScalarType> & InverseOrientationMatrix( const ::LinearAlgebra::Matrix4x4<ScalarType> &orientationMatrix, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
 	{
@@ -349,6 +425,12 @@ namespace LinearAlgebra3D
 		return orientationMatrix;
 	}
 
+	template<typename ScalarType>
+	inline ::LinearAlgebra::Matrix4x4<ScalarType> ExtractRotationMatrix( const ::LinearAlgebra::Matrix4x4<ScalarType> &orientationMatrix )
+	{
+		return ::LinearAlgebra::Matrix4x4<ScalarType>( orientationMatrix.v[0], orientationMatrix.v[1], orientationMatrix.v[2], ::LinearAlgebra::Vector4<ScalarType>::standard_unit_w );
+	}
+
 	/*	Creates an orthographic projection matrix designed for DirectX enviroment.
 		width; of the projection sample volume.
 		height; of the projection sample volume.
@@ -390,6 +472,10 @@ namespace LinearAlgebra3D
 	template<typename ScalarType>
 	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::Vector3<ScalarType> NormalProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &normalizedAxis )
+	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
 }
 
 #include "Utilities.h"

Code/OysterMath/OysterMath.cpp

@@ -33,17 +33,50 @@ namespace Oyster { namespace Math2D
 	Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem )
 	{ return ::LinearAlgebra2D::RotationMatrix( radian, targetMem ); }
 	
+	Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation );
+	}
+
+	Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation );
+	}
+
+	Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation );
+	}
+
+	Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation );
+	}
+
 	Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem )
-	{ return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem ); }
+	{
+		return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem );
+	}
 
 	Float3x3 & OrientationMatrix( const Float2 &position, const Float2 &lookAt, Float3x3 &targetMem )
-	{ return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem ); }
+	{
+		return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem );
+	}
 
 	Float3x3 & OrientationMatrix( const Float2 &position, Float radian, const Float2 &localCenterOfRotation, Float3x3 &targetMem )
-	{ return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem ); }
+	{
+		return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem );
+	}
 
 	Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem )
-	{ return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem ); }
+	{
+		return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem );
+	}
+
+	Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra2D::ExtractRotationMatrix( orientation );
+	}
 } }
 
 namespace Oyster { namespace Math3D
@@ -63,29 +96,83 @@ namespace Oyster { namespace Math3D
 	Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem )
 	{ return ::LinearAlgebra3D::RotationMatrix( normalizedAxis, radian, targetMem ); }
 	
+	Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation );
+	}
+	
+	Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation );
+	}
+
+	Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation );
+	}
+
+	Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation );
+	}
+
 	Float4x4 & OrientationMatrix( const Float3 &normalizedAxis, const Float & deltaRadian, const Float3 &sumTranslation, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem ); }
+	{
+		return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem );
+	}
 
 	Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem ); }
+	{
+		return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem );
+	}
 
 	Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, const Float3 &centerOfMass, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); }
+	{
+		return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem );
+	}
+
+	Float4x4 & OrientationMatrix_LookAtDirection( const Float3 &normalizedDirection, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::OrientationMatrix_LookAtDirection( normalizedDirection, normalizedUpVector, worldPos );
+	}
+
+	Float4x4 & OrientationMatrix_LookAtPos( const Float3 &worldLookAt, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::OrientationMatrix_LookAtPos( worldLookAt, normalizedUpVector, worldPos );
+	}
 
 	Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); }
+	{
+		return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem );
+	}
 
 	Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix )
-	{ return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix ); }
+	{
+		return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix );
+	}
+
+	Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem )
+	{
+		return targetMem = ::LinearAlgebra3D::ExtractRotationMatrix( orientation );
+	}
 
 	Float4x4 & ProjectionMatrix_Orthographic( const Float &width, const Float &height, const Float &nearClip, const Float &farClip, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem ); }
+	{
+		return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem );
+	}
 
 	Float4x4 & ProjectionMatrix_Perspective( const Float &verticalFoV, const Float &aspectRatio, const Float &nearClip, const Float &farClip, Float4x4 &targetMem )
-	{ return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem ); }
+	{
+		return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem );
+	}
 
 	Float3 VectorProjection( const Float3 &vector, const Float3 &axis )
 	{
 		return ::LinearAlgebra3D::VectorProjection( vector, axis );
 	}
+
+	Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis )
+	{
+		return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis );
+	}
 } }

Code/OysterMath/OysterMath.h

@@ -114,6 +114,18 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized
 	/// 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.
+	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.
+	Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() );
+
+	/// 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
+	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
 	Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem = Float3x3() );
 
@@ -126,6 +138,9 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized
 
 	/// 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.
+	Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem = Float3x3() );
 } }
 
 namespace Oyster { namespace Math3D /// Oyster's native math library specialized for 3D
@@ -148,6 +163,18 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	/// 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.
+	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.
+	Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem = Float4x4() );
+	
+	/// 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
+	Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() );
+
 	/*******************************************************************
 	 * Sets and returns targetMem as an orientation Matrix
 	 * @param normalizedAxis: The normalized vector parallell with the rotationAxis.
@@ -180,6 +207,12 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	 *******************************************************************/
 	Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, const Float3 &centerOfMass, Float4x4 &targetMem = Float4x4() );
 
+	//! @todo TODO: Add documentation and not tested
+	Float4x4 & OrientationMatrix_LookAtDirection( const Float3 &normalizedDirection, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem = Float4x4() );
+
+	//! @todo TODO: Add documentation and not tested
+	Float4x4 & OrientationMatrix_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.
 	Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem = Float4x4() );
 
@@ -187,6 +220,9 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	// 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.
+	Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem = Float4x4() );
+
 	/*******************************************************************
 	 *	Creates an orthographic projection matrix designed for DirectX enviroment.
 	 *	@param width; of the projection sample volume.
@@ -214,14 +250,21 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
 	/// 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.
+	Float3 NormalProjection( const Float3 &vector, const Float3 &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; }
 
-	/// Helper inline function that sets and then returns targetMem = transformer * transformee
-	inline Float4x4 & TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee, Float4x4 &targetMem = Float4x4() )
+	/** Helper inline function that sets and then returns targetMem = transformer * transformee */
+	inline Float4x4 & TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee, Float4x4 &targetMem )
 	{ return targetMem = transformer * transformee; }
 
+	/** Helper inline function that sets and then returns transformer * transformee */
+	inline Float4x4 TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee )
+	{ return 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/OysterPhysics3D/Box.cpp

@@ -8,19 +8,28 @@
 using namespace ::Oyster::Collision3D;
 using namespace ::Oyster::Math3D;
 
-Box::Box( ) : ICollideable(Type_box), orientation(Float4x4::identity), boundingOffset() {}
-Box::Box( const Float4x4 &o, const Float3 &s ) : ICollideable(Type_box), orientation(o), boundingOffset(s*0.5) {}
+Box::Box( )
+	: ICollideable(Type_box), rotation(Float4x4::identity), center(0.0f), boundingOffset(0.5f)
+{}
+
+Box::Box( const Float4x4 &r, const Float3 &p, const Float3 &s )
+	: ICollideable(Type_box), rotation(r), center(p), boundingOffset(s*0.5)
+{}
+
 Box::~Box( ) {}
 
 Box & Box::operator = ( const Box &box )
 {
-	this->orientation = box.orientation;
+	this->rotation = box.rotation;
+	this->center = box.center;
 	this->boundingOffset = box.boundingOffset;
 	return *this;
 }
 
 ::Utility::DynamicMemory::UniquePointer<ICollideable> Box::Clone( ) const
-{ return ::Utility::DynamicMemory::UniquePointer<ICollideable>( new Box(*this) ); }
+{
+	return ::Utility::DynamicMemory::UniquePointer<ICollideable>( new Box(*this) );
+}
 
 bool Box::Intersects( const ICollideable *target ) const
 {
@@ -31,10 +40,10 @@ bool Box::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance );
 	case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target );
 	case Type_plane: return Utility::Intersect( *this, *(Plane*)target );
-	case Type_triangle: return false; // TODO: : 
+	// case Type_triangle: return false; // TODO: : 
 	case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target );
 	case Type_box: return Utility::Intersect( *this, *(Box*)target );
-	case Type_frustrum: return false; // TODO: : 
+	// case Type_frustrum: return false; // TODO: : 
 	default: return false;
 	}
 }
@@ -44,11 +53,11 @@ bool Box::Contains( const ICollideable *target ) const
 	switch( target->type )
 	{
 	case Type_point: return Utility::Intersect( *this, *(Point*)target );
-	case Type_sphere: return false; // TODO: 
-	case Type_triangle: return false; // TODO: 
-	case Type_box_axis_aligned: return false; // TODO: 
-	case Type_box: return false; // TODO: 
-	case Type_frustrum: return false; // TODO: 
+	// case Type_sphere: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
+	// case Type_box_axis_aligned: return false; // TODO: 
+	// case Type_box: return false; // TODO: 
+	// case Type_frustrum: return false; // TODO: 
 	default: return false;
 	}
 }

Code/OysterPhysics3D/Box.h

@@ -16,19 +16,18 @@ namespace Oyster { namespace Collision3D
 	public:
 		union
 		{
-			struct{ ::Oyster::Math::Float4x4 orientation; ::Oyster::Math::Float3 boundingOffset; };
+			struct{ ::Oyster::Math::Float4x4 rotation; ::Oyster::Math::Float3 center, boundingOffset; };
 			struct
 			{
 				::Oyster::Math::Float3 xAxis; ::Oyster::Math::Float paddingA;
 				::Oyster::Math::Float3 yAxis; ::Oyster::Math::Float paddingB;
 				::Oyster::Math::Float3 zAxis; ::Oyster::Math::Float paddingC;
-				::Oyster::Math::Float3 center;
 			};
-			char byte[sizeof(::Oyster::Math::Float4x4) + sizeof(::Oyster::Math::Float3)];
+			char byte[sizeof(::Oyster::Math::Float4x4) + 2*sizeof(::Oyster::Math::Float3)];
 		};
 
 		Box( );
-		Box( const ::Oyster::Math::Float4x4 &orientation, const ::Oyster::Math::Float3 &size );
+		Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &size );
 		virtual ~Box( );
 
 		Box & operator = ( const Box &box );

Code/OysterPhysics3D/BoxAxisAligned.cpp

@@ -33,10 +33,10 @@ bool BoxAxisAligned::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance );
 	case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target );
 	case Type_plane: return Utility::Intersect( *this, *(Plane*)target );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target );
-	case Type_box: return false; // TODO: 
-	case Type_frustrum: return false; // TODO: 
+	// case Type_box: return false; // TODO: 
+	// case Type_frustrum: return false; // TODO: 
 	default: return false;
 	}
 }
@@ -45,12 +45,12 @@ bool BoxAxisAligned::Contains( const ICollideable *target ) const
 {
 	switch( target->type )
 	{
-	case Type_point: return false; // TODO: 
-	case Type_sphere: return false; // TODO: 
-	case Type_triangle: return false; // TODO: 
-	case Type_box_axis_aligned: return false; // TODO: 
-	case Type_box: return false; // TODO: 
-	case Type_frustrum: return false; // TODO: 
+	// case Type_point: return false; // TODO: 
+	// case Type_sphere: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
+	// case Type_box_axis_aligned: return false; // TODO: 
+	// case Type_box: return false; // TODO: 
+	// case Type_frustrum: return false; // TODO: 
 	default: return false;
 	}
 }

Code/OysterPhysics3D/Frustrum.cpp

@@ -200,11 +200,11 @@ bool Frustrum::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance );
 	case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target );
 	case Type_plane: return Utility::Intersect( *this, *(Plane*)target );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target );
 	case Type_box: return Utility::Intersect( *this, *(Box*)target );
 	case Type_frustrum: return Utility::Intersect( *this, *(Frustrum*)target );
-	case Type_line: return false; // TODO: 
+	// case Type_line: return false; // TODO: 
 	default: return false;
 	}
 }
@@ -214,14 +214,14 @@ bool Frustrum::Contains( const ICollideable *target ) const
 	switch( target->type )
 	{
 	case Type_point: return Utility::Intersect( *this, *(Point*)target );
-	case Type_ray: return false; // TODO: 
-	case Type_sphere: return false; // TODO: 
-	case Type_plane: return false; // TODO: 
-	case Type_triangle: return false; // TODO: 
-	case Type_box_axis_aligned: return false; // TODO: 
-	case Type_box: return false; // TODO: 
-	case Type_frustrum: return false; // TODO: 
-	case Type_line: return false; // TODO: 
+	// case Type_ray: return false; // TODO: 
+	// case Type_sphere: return false; // TODO: 
+	// case Type_plane: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
+	// case Type_box_axis_aligned: return false; // TODO: 
+	// case Type_box: return false; // TODO: 
+	// case Type_frustrum: return false; // TODO: 
+	// case Type_line: return false; // TODO: 
 	default: return false;
 	}
 }

Code/OysterPhysics3D/ICollideable.h

@@ -21,7 +21,7 @@ namespace Oyster { namespace Collision3D /// Contains a collection of 3D shapes
 			Type_ray,
 			Type_sphere,
 			Type_plane,
-			Type_triangle,
+			// Type_triangle,
 			Type_box_axis_aligned,
 			Type_box,
 			Type_frustrum,

Code/OysterPhysics3D/OysterCollision3D.cpp

@@ -6,7 +6,8 @@
 #include "Utilities.h"
 #include <limits>
 
-using namespace Oyster::Math3D;
+using namespace ::Oyster::Math3D;
+using namespace ::Utility::Value;
 
 namespace Oyster { namespace Collision3D { namespace Utility
 {
@@ -19,13 +20,13 @@ namespace Oyster { namespace Collision3D { namespace Utility
 		// Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture
 		inline bool EqualsZero( const Float &value )
 		{ // by Dan Andersson
-			return ::Utility::Value::Abs( value ) < epsilon;
+			return Abs( value ) < epsilon;
 		}
 
 		// Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture
 		inline bool NotEqualsZero( const Float &value )
 		{ // by Dan Andersson
-			return ::Utility::Value::Abs( value ) > epsilon;
+			return Abs( value ) > epsilon;
 		}
 
 		// returns true if miss/reject
@@ -39,8 +40,8 @@ namespace Oyster { namespace Collision3D { namespace Utility
 					  t2 = e - boundingOffset;
 				t1 /= f; t2 /= f;
 				if( t1 > t2 ) ::Utility::Element::Swap( t1, t2 );
-				tMin = ::Utility::Value::Max( tMin, t1 );
-				tMax = ::Utility::Value::Min( tMax, t2 );
+				tMin = Max( tMin, t1 );
+				tMax = Min( tMax, t2 );
 				if( tMin > tMax ) return true;
 				if( tMax < 0.0f ) return true;
 			}
@@ -65,101 +66,140 @@ namespace Oyster { namespace Collision3D { namespace Utility
 		{ // by Dan Andersson
 			Float3 c = (box.maxVertex + box.minVertex) * 0.5f, // box.Center
 				   h = (box.maxVertex - box.minVertex) * 0.5f; // box.halfSize
-			boxExtend  = h.x * ::Utility::Value::Abs(plane.normal.x); // Box max extending towards plane
-			boxExtend += h.y * ::Utility::Value::Abs(plane.normal.y);
-			boxExtend += h.z * ::Utility::Value::Abs(plane.normal.z);
+			boxExtend  = h.x * Abs(plane.normal.x); // Box max extending towards plane
+			boxExtend += h.y * Abs(plane.normal.y);
+			boxExtend += h.z * Abs(plane.normal.z);
 			centerDistance = c.Dot(plane.normal) + plane.phasing; // distance between box center and plane
 		}
 
 		void Compare( Float &boxExtend, Float &centerDistance, const Plane &plane, const Box &box )
 		{ // by Dan Andersson
-			boxExtend  = box.boundingOffset.x * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[0].xyz)); // Box max extending towards plane
-			boxExtend += box.boundingOffset.y * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[1].xyz));
-			boxExtend += box.boundingOffset.z * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[2].xyz));
+			boxExtend  = box.boundingOffset.x * Abs(plane.normal.Dot(box.xAxis)); // Box max extending towards plane
+			boxExtend += box.boundingOffset.y * Abs(plane.normal.Dot(box.yAxis));
+			boxExtend += box.boundingOffset.z * Abs(plane.normal.Dot(box.zAxis));
 
-			centerDistance = box.orientation.v[3].xyz.Dot(plane.normal) + plane.phasing; // distance between box center and plane
+			centerDistance = box.center.Dot(plane.normal) + plane.phasing; // distance between box center and plane
 		}
 
-		bool FifteenAxisVsAlignedAxisOverlappingChecks( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &orientationB )
+		bool SeperatingAxisTest_AxisAlignedVsTransformedBox( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &rotationB, const Float3 &worldOffset )
 		{ // by Dan Andersson
-			Float4x4 absOrientationB;
-			{
-				Float4x4 tO = orientationB.GetTranspose();
-				absOrientationB.v[0] = ::Utility::Value::Abs(tO.v[0]);
-				if( absOrientationB.v[0].w > boundingOffsetA.x + boundingOffsetB.Dot(absOrientationB.v[0].xyz) ) return false;
-				absOrientationB.v[1] = ::Utility::Value::Abs(tO.v[1]);
-				if( absOrientationB.v[1].w > boundingOffsetA.y + boundingOffsetB.Dot(absOrientationB.v[1].xyz) ) return false;
-				absOrientationB.v[2] = ::Utility::Value::Abs(tO.v[2]);
-				if( absOrientationB.v[2].w > boundingOffsetA.z + boundingOffsetB.Dot(absOrientationB.v[2].xyz) ) return false;
+
+			/*****************************************************************
+			 * Uses the Seperating Axis Theorem
+			 * if( |t dot s| > hA dot |s * RA| + hB dot |s * RB| ) then not intersecting
+			 *     |t dot s| > hA dot |s| + hB dot |s * RB| .. as RA = I
+			 *
+			 * t: objectB's offset from objectA														[worldOffset]
+			 * s: current comparison axis
+			 * hA: boundingReach vector of objectA. Only absolute values is assumed.				[boundingOffsetA]
+			 * hB: boundingReach vector of objectB. Only absolute values is assumed.				[boundingOffsetB]
+			 * RA: rotation matrix of objectA. Is identity matrix here, thus omitted.
+			 * RB: rotation matrix of objectB. Is transformed into objectA's view at this point.	[rotationB]
+			 *
+			 * Note: s * RB = (RB^T * s)^T = (RB^-1 * s)^T .... vector == vector^T
+			 *****************************************************************/
+
+			Float4x4 absRotationB = Abs(rotationB);
+			Float3 absWorldOffset = Abs(worldOffset); // |t|: [absWorldOffset]
+
+			// s = { 1, 0, 0 }	[ RA.v[0] ]
+			if( absWorldOffset.x > boundingOffsetA.x + boundingOffsetB.Dot(Float3(absRotationB.v[0].x, absRotationB.v[1].x, absRotationB.v[2].x)) )
+			{ // |t dot s| > hA dot |s| + hB dot |s * RB| -->> t.x > hA.x + hB dot |{RB.v[0].x, RB.v[1].x, RB.v[2].x}| 
+				return false;
 			}
 
-			absOrientationB.Transpose();
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[0])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[0].xyz) ) return false;
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[1])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[1].xyz) ) return false;
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[2])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[2].xyz) ) return false;
+			// s = { 0, 1, 0 }	[ RA.v[1] ]
+			if( absWorldOffset.y > boundingOffsetA.y + boundingOffsetB.Dot(Float3(absRotationB.v[0].y, absRotationB.v[1].y, absRotationB.v[2].y)) )
+			{ // t.y > hA.y + hB dot |{RB.v[0].y, RB.v[1].y, RB.v[2].y}|
+				return false;
+			}
 
-			// ( 1,0,0 ) x orientationB.v[0].xyz:
-			Float d = boundingOffsetA.y * absOrientationB.v[0].z;
-			d += boundingOffsetA.z * absOrientationB.v[0].y;
-			d += boundingOffsetB.y * absOrientationB.v[2].x;
-			d += boundingOffsetB.z * absOrientationB.v[1].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[0].y - orientationB.v[3].y*orientationB.v[0].z) > d ) return false;
+			// s = { 0, 0, 1 }	[ RA.v[2] ]
+			if( absWorldOffset.z > boundingOffsetA.z + boundingOffsetB.Dot(Float3(absRotationB.v[0].z, absRotationB.v[1].z, absRotationB.v[2].z)) )
+			{ // t.z > hA.z + hB dot |{RB.v[0].z, RB.v[1].z, RB.v[2].z}|
+				return false;
+			}
 
-			// ( 1,0,0 ) x orientationB.v[1].xyz:
-			d  = boundingOffsetA.y * absOrientationB.v[1].z;
-			d += boundingOffsetA.z * absOrientationB.v[1].y;
-			d += boundingOffsetB.x * absOrientationB.v[2].x;
-			d += boundingOffsetB.z * absOrientationB.v[0].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[1].y - orientationB.v[3].y*orientationB.v[1].z) > d ) return false;
+			// s = RB.v[0].xyz
+			if( Abs(worldOffset.Dot(rotationB.v[0].xyz)) > boundingOffsetA.Dot(absRotationB.v[0].xyz) + boundingOffsetB.x )
+			{ // |t dot s| > hA dot |s| + hB dot |s * RB| -->> |t dot s| > hA dot |s| + hB dot |{1, 0, 0}| -->> |t dot s| > hA dot |s| + hB.x
+				return false;
+			}
 
-			// ( 1,0,0 ) x orientationB.v[2].xyz:
-			d  = boundingOffsetA.y * absOrientationB.v[2].z;
-			d += boundingOffsetA.z * absOrientationB.v[2].y;
-			d += boundingOffsetB.x * absOrientationB.v[1].x;
-			d += boundingOffsetB.y * absOrientationB.v[0].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[2].y - orientationB.v[3].y*orientationB.v[2].z) > d ) return false;
+			// s = RB.v[1].xyz
+			if( Abs(worldOffset.Dot(rotationB.v[1].xyz)) > boundingOffsetA.Dot(absRotationB.v[1].xyz) + boundingOffsetB.y )
+			{ // |t dot s| > hA dot |s| + hB.y
+				return false;
+			}
 
-			// ( 0,1,0 ) x orientationB.v[0].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[0].z;
-			d += boundingOffsetA.z * absOrientationB.v[0].x;
-			d += boundingOffsetB.y * absOrientationB.v[2].y;
-			d += boundingOffsetB.z * absOrientationB.v[1].y;
-			if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[0].z - orientationB.v[3].z*orientationB.v[0].x) > d ) return false;
+			// s = RB.v[2].xyz
+			if( Abs(worldOffset.Dot(rotationB.v[2].xyz)) > boundingOffsetA.Dot(absRotationB.v[2].xyz) + boundingOffsetB.z )
+			{ // |t dot s| > hA dot |s| + hB.z
+				return false;
+			}
 
-			// ( 0,1,0 ) x orientationB.v[1].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[1].z;
-			d += boundingOffsetA.z * absOrientationB.v[1].x;
-			d += boundingOffsetB.x * absOrientationB.v[2].y;
-			d += boundingOffsetB.z * absOrientationB.v[0].y;
-			if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[1].z - orientationB.v[3].z*orientationB.v[1].x) > d ) return false;
+			// s = ( 1,0,0 ) x rotationB.v[0].xyz:
+			Float d = boundingOffsetA.y * absRotationB.v[0].z;
+			d += boundingOffsetA.z * absRotationB.v[0].y;
+			d += boundingOffsetB.y * absRotationB.v[2].x;
+			d += boundingOffsetB.z * absRotationB.v[1].x;
+			if( Abs(worldOffset.z*rotationB.v[0].y - worldOffset.y*rotationB.v[0].z) > d ) return false;
 
-			// ( 0,1,0 ) x orientationB.v[2].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[2].z;
-			d += boundingOffsetA.z * absOrientationB.v[2].x;
-			d += boundingOffsetB.x * absOrientationB.v[1].y;
-			d += boundingOffsetB.y * absOrientationB.v[0].y;
-			if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[2].z - orientationB.v[3].z*orientationB.v[2].x) > d ) return false;
+			// s = ( 1,0,0 ) x rotationB.v[1].xyz:
+			d  = boundingOffsetA.y * absRotationB.v[1].z;
+			d += boundingOffsetA.z * absRotationB.v[1].y;
+			d += boundingOffsetB.x * absRotationB.v[2].x;
+			d += boundingOffsetB.z * absRotationB.v[0].x;
+			if( Abs(worldOffset.z*rotationB.v[1].y - worldOffset.y*rotationB.v[1].z) > d ) return false;
 
-			// ( 0,0,1 ) x orientationB.v[0].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[0].y;
-			d += boundingOffsetA.y * absOrientationB.v[0].x;
-			d += boundingOffsetB.y * absOrientationB.v[2].z;
-			d += boundingOffsetB.z * absOrientationB.v[1].z;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[0].x - orientationB.v[3].x*orientationB.v[0].y) > d ) return false;
+			// s = ( 1,0,0 ) x rotationB.v[2].xyz:
+			d  = boundingOffsetA.y * absRotationB.v[2].z;
+			d += boundingOffsetA.z * absRotationB.v[2].y;
+			d += boundingOffsetB.x * absRotationB.v[1].x;
+			d += boundingOffsetB.y * absRotationB.v[0].x;
+			if( Abs(worldOffset.z*rotationB.v[2].y - worldOffset.y*rotationB.v[2].z) > d ) return false;
 
-			// ( 0,0,1 ) x orientationB.v[1].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[1].y;
-			d += boundingOffsetA.y * absOrientationB.v[1].x;
-			d += boundingOffsetB.x * absOrientationB.v[2].z;
-			d += boundingOffsetB.z * absOrientationB.v[0].z;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[1].x - orientationB.v[3].x*orientationB.v[1].y) > d ) return false;
+			// s = ( 0,1,0 ) x rotationB.v[0].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[0].z;
+			d += boundingOffsetA.z * absRotationB.v[0].x;
+			d += boundingOffsetB.y * absRotationB.v[2].y;
+			d += boundingOffsetB.z * absRotationB.v[1].y;
+			if( Abs(worldOffset.x*rotationB.v[0].z - worldOffset.z*rotationB.v[0].x) > d ) return false;
 		
-			// ( 0,0,1 ) x orientationB.v[2].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[2].y;
-			d += boundingOffsetA.y * absOrientationB.v[2].x;
-			d += boundingOffsetB.x * absOrientationB.v[1].z;
-			d += boundingOffsetB.y * absOrientationB.v[0].z;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[2].x - orientationB.v[3].x*orientationB.v[2].y) > d ) return false;
+			// s = ( 0,1,0 ) x rotationB.v[1].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[1].z;
+			d += boundingOffsetA.z * absRotationB.v[1].x;
+			d += boundingOffsetB.x * absRotationB.v[2].y;
+			d += boundingOffsetB.z * absRotationB.v[0].y;
+			if( Abs(worldOffset.x*rotationB.v[1].z - worldOffset.z*rotationB.v[1].x) > d ) return false;
+
+			// s = ( 0,1,0 ) x rotationB.v[2].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[2].z;
+			d += boundingOffsetA.z * absRotationB.v[2].x;
+			d += boundingOffsetB.x * absRotationB.v[1].y;
+			d += boundingOffsetB.y * absRotationB.v[0].y;
+			if( Abs(worldOffset.x*rotationB.v[2].z - worldOffset.z*rotationB.v[2].x) > d ) return false;
+
+			// s = ( 0,0,1 ) x rotationB.v[0].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[0].y;
+			d += boundingOffsetA.y * absRotationB.v[0].x;
+			d += boundingOffsetB.y * absRotationB.v[2].z;
+			d += boundingOffsetB.z * absRotationB.v[1].z;
+			if( Abs(worldOffset.y*rotationB.v[0].x - worldOffset.x*rotationB.v[0].y) > d ) return false;
+
+			// s = ( 0,0,1 ) x rotationB.v[1].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[1].y;
+			d += boundingOffsetA.y * absRotationB.v[1].x;
+			d += boundingOffsetB.x * absRotationB.v[2].z;
+			d += boundingOffsetB.z * absRotationB.v[0].z;
+			if( Abs(worldOffset.y*rotationB.v[1].x - worldOffset.x*rotationB.v[1].y) > d ) return false;
+
+			// s = ( 0,0,1 ) x rotationB.v[2].xyz:
+			d  = boundingOffsetA.x * absRotationB.v[2].y;
+			d += boundingOffsetA.y * absRotationB.v[2].x;
+			d += boundingOffsetB.x * absRotationB.v[1].z;
+			d += boundingOffsetB.y * absRotationB.v[0].z;
+			if( Abs(worldOffset.y*rotationB.v[2].x - worldOffset.x*rotationB.v[2].y) > d ) return false;
 
 			return true;
 		}
@@ -353,8 +393,8 @@ namespace Oyster { namespace Collision3D { namespace Utility
 
 	bool Intersect( const BoxAxisAligned &box, const Sphere &sphere )
 	{ // by Dan Andersson
-		Float3 e = ::Utility::Value::Max( box.minVertex - sphere.center, Float3::null );
-		e += ::Utility::Value::Max( sphere.center - box.maxVertex, Float3::null );
+		Float3 e = Max( box.minVertex - sphere.center, Float3::null );
+		e += Max( sphere.center - box.maxVertex, Float3::null );
 
 		if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false;
 		return true;
@@ -385,17 +425,17 @@ namespace Oyster { namespace Collision3D { namespace Utility
 
 	bool Intersect( const Box &box, const Point &point )
 	{ // by Dan Andersson
-		Float3 dPos = point.center - box.orientation.v[3].xyz;
+		Float3 dPos = point.center - box.center;
 
-		Float coordinate = dPos.Dot( box.orientation.v[0].xyz );
+		Float coordinate = dPos.Dot( box.xAxis );
 		if( coordinate > box.boundingOffset.x ) return false;
 		if( coordinate < -box.boundingOffset.x ) return false;
 
-		coordinate = dPos.Dot( box.orientation.v[1].xyz );
+		coordinate = dPos.Dot( box.yAxis );
 		if( coordinate > box.boundingOffset.y ) return false;
 		if( coordinate < -box.boundingOffset.y ) return false;
 
-		coordinate = dPos.Dot( box.orientation.v[2].xyz );
+		coordinate = dPos.Dot( box.zAxis );
 		if( coordinate > box.boundingOffset.z ) return false;
 		if( coordinate < -box.boundingOffset.z ) return false;
 
@@ -419,11 +459,11 @@ namespace Oyster { namespace Collision3D { namespace Utility
 
 	bool Intersect( const Box &box, const Sphere &sphere )
 	{ // by Dan Andersson
-		Float3 e = sphere.center - box.orientation.v[3].xyz,
-			   centerL = Float3( e.Dot(box.orientation.v[0].xyz), e.Dot(box.orientation.v[1].xyz), e.Dot(box.orientation.v[2].xyz) );
+		Float3 e = sphere.center - box.center,
+			   centerL = Float3( e.Dot(box.xAxis), e.Dot(box.yAxis), e.Dot(box.zAxis) );
 		
-		e  = ::Utility::Value::Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null );
-		e += ::Utility::Value::Max( centerL - box.boundingOffset, Float3::null );
+		e  = Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null );
+		e += Max( centerL - box.boundingOffset, Float3::null );
 		
 		if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false;
 		return true;
@@ -440,20 +480,20 @@ namespace Oyster { namespace Collision3D { namespace Utility
 
 	bool Intersect( const Box &boxA, const BoxAxisAligned &boxB )
 	{ // by Dan Andersson
-		Float3 alignedOffsetBoundaries = boxB.maxVertex - boxB.minVertex;
-		Float4x4 translated = boxA.orientation;
-		translated.v[3].xyz -= boxB.minVertex;
-		translated.v[3].xyz += alignedOffsetBoundaries * 0.5f;
-		alignedOffsetBoundaries = ::Utility::Value::Abs(alignedOffsetBoundaries);
-		return Private::FifteenAxisVsAlignedAxisOverlappingChecks( alignedOffsetBoundaries, boxA.boundingOffset, translated );
+		Float3 alignedOffsetBoundaries = (boxB.maxVertex - boxB.minVertex) * 0.5f,
+			   offset = boxA.center - Average( boxB.minVertex, boxB.maxVertex );
+		return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( alignedOffsetBoundaries, boxA.boundingOffset, boxA.rotation, offset );
 	}
 
 	bool Intersect( const Box &boxA, const Box &boxB )
 	{ // by Dan Andersson
-		Float4x4 M;
-		InverseOrientationMatrix( boxA.orientation, M );
-		TransformMatrix( M, boxB.orientation, M ); /// TODO: not verified
-		return Private::FifteenAxisVsAlignedAxisOverlappingChecks( boxA.boundingOffset, boxB.boundingOffset, M );
+		Float4x4 orientationA = OrientationMatrix(boxA.rotation, boxA.center),
+				 orientationB = OrientationMatrix(boxB.rotation, boxB.center),
+				 invOrientationA = InverseOrientationMatrix( orientationA );
+
+		orientationB = TransformMatrix( invOrientationA, orientationB );
+
+		return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( boxA.boundingOffset, boxB.boundingOffset, ExtractRotationMatrix(orientationB), orientationB.v[3].xyz );
 	}
 
 	bool Intersect( const Frustrum &frustrum, const Point &point )
@@ -490,37 +530,37 @@ namespace Oyster { namespace Collision3D { namespace Utility
 		if( Intersect(frustrum.leftPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( Intersect(frustrum.rightPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( Intersect(frustrum.bottomPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( Intersect(frustrum.topPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( Intersect(frustrum.nearPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( Intersect(frustrum.farPlane, ray, distance) )
 		{
 			intersected = true;
-			connectDistance = ::Utility::Value::Min( connectDistance, distance );
+			connectDistance = Min( connectDistance, distance );
 		}
 
 		if( intersected ) return true;

Code/OysterPhysics3D/OysterPhysics3D.h

@@ -49,7 +49,7 @@ namespace Oyster { namespace Physics3D
 		}
 
 		/******************************************************************
-		 * Returns the local angular momentum of a mass in rotation.
+		 * Returns the world angular momentum of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
 		inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity )
@@ -58,39 +58,39 @@ namespace Oyster { namespace Physics3D
 		}
 
 		/******************************************************************
-		 * Returns the local angular momentum of a mass in rotation.
+		 * Returns the world angular momentum of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &offset )
+		inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return offset.Cross( linearMomentum );
+			return worldOffset.Cross( linearMomentum );
 		}
 
 		/******************************************************************
-		 * Returns the local tangential momentum at localPos, of a mass in rotation.
+		 * Returns the world tangential momentum at worldPos, of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &localOffset )
+		inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return angularMomentum.Cross( localOffset );
+			return angularMomentum.Cross( worldOffset );
 		}
 
 		/******************************************************************
-		 * Returns the local tangential momentum at localPos, of a mass in rotation.
+		 * Returns the world tangential momentum at worldPos, of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &localOffset )
+		inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return TangentialLinearMomentum( AngularMomentum(momentOfInertia, angularVelocity), localOffset );
+			return TangentialLinearMomentum( AngularMomentum(momentOfInertia, angularVelocity), worldOffset );
 		}
 
 		/******************************************************************
-		 * Returns the local impulse force at localPos, of a mass in angular acceleration.
+		 * Returns the world impulse force at worldPos, of a mass in angular acceleration.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialImpulseForce( const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &localOffset )
+		inline ::Oyster::Math::Float3 TangentialImpulseForce( const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return impulseTorque.Cross( localOffset );
+			return impulseTorque.Cross( worldOffset );
 		}
 
 		/******************************************************************
@@ -106,22 +106,22 @@ namespace Oyster { namespace Physics3D
 		 * 
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &offset )
+		inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return offset.Cross( linearImpulseAcceleration );
+			return worldOffset.Cross( linearImpulseAcceleration );
 		}
 
 		/******************************************************************
-		 * Returns the local impulse acceleration at localPos, of a mass in angular acceleration.
+		 * Returns the world impulse acceleration at ( worldOffset = worldPos - body's center of gravity ), of a mass in angular acceleration.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialImpulseAcceleration( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &localOffset )
+		inline ::Oyster::Math::Float3 TangentialImpulseAcceleration( const ::Oyster::Math::Float4x4 &worldMomentOfInertiaInversed, const ::Oyster::Math::Float3 &worldImpulseTorque, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return AngularImpulseAcceleration( momentOfInertiaInversed, impulseTorque ).Cross( localOffset );
+			return AngularImpulseAcceleration( worldMomentOfInertiaInversed, worldImpulseTorque ).Cross( worldOffset );
 		}
 
 		/******************************************************************
-		 * Returns the local angular velocity of a mass in rotation.
+		 * Returns the world angular velocity of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
 		inline ::Oyster::Math::Float3 AngularVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum )
@@ -130,21 +130,30 @@ namespace Oyster { namespace Physics3D
 		}
 
 		/******************************************************************
-		 * Returns the local tangential velocity at localPos, of a mass in rotation.
+		 * Returns the world angular velocity of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &offset )
+		inline ::Oyster::Math::Float3 AngularVelocity( const ::Oyster::Math::Float3 &linearVelocity, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return angularVelocity.Cross( offset );
+			return worldOffset.Cross( linearVelocity );
 		}
 
 		/******************************************************************
-		 * Returns the local tangential velocity at localPos, of a mass in rotation.
+		 * Returns the world tangential velocity at worldPos, of a mass in rotation.
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &offset )
+		inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return TangentialLinearVelocity( AngularVelocity(momentOfInertiaInversed, angularMomentum), offset );
+			return angularVelocity.Cross( worldOffset );
+		}
+
+		/******************************************************************
+		 * Returns the world tangential velocity at worldPos, of a mass in rotation.
+		 * @todo TODO: improve doc
+		 ******************************************************************/
+		inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &worldOffset )
+		{
+			return TangentialLinearVelocity( AngularVelocity(momentOfInertiaInversed, angularMomentum), worldOffset );
 		}
 
 		/******************************************************************
@@ -169,9 +178,9 @@ namespace Oyster { namespace Physics3D
 		 * 
 		 * @todo TODO: improve doc
 		 ******************************************************************/
-		inline ::Oyster::Math::Float3 ImpulseTorque( const ::Oyster::Math::Float3 & impulseForce, const ::Oyster::Math::Float3 &offset )
+		inline ::Oyster::Math::Float3 ImpulseTorque( const ::Oyster::Math::Float3 & impulseForce, const ::Oyster::Math::Float3 &worldOffset )
 		{
-			return offset.Cross( impulseForce );
+			return worldOffset.Cross( impulseForce );
 		}
 
 		/******************************************************************
@@ -183,9 +192,105 @@ namespace Oyster { namespace Physics3D
 			return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz;
 		}
 
+		inline ::Oyster::Math::Float3 Impulse(  )
+		{
+			//! @todo TODO: implement calculation for impulse. Hijack Mattias Eriksson
+			return ::Oyster::Math::Float3::null;
+		}
+
 		namespace MomentOfInertia
 		{ /// Library of Formulas to calculate moment of inerta for simple shapes
 			/** @todo TODO: add MomentOfInertia tensor formulas */
+			inline ::Oyster::Math::Float CalculateSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius )
+			{
+				return (2.0f/5.0f)*mass*radius*radius;
+			}
+
+			inline ::Oyster::Math::Float4x4 Sphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius )
+			{
+				::Oyster::Math::Float4x4 inertia = ::Oyster::Math::Float4x4::identity;
+				inertia.m[0][0] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateSphere( mass , radius );
+				inertia.m[1][1] = inertia.m[0][0];
+				inertia.m[2][2] = inertia.m[0][0];
+
+				return inertia;
+			}
+
+			inline ::Oyster::Math::Float CalculateHollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius )
+			{
+				return (2.0f/3.0f)*mass*radius*radius;
+			}
+
+			inline ::Oyster::Math::Float4x4 HollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius )
+			{
+				::Oyster::Math::Float4x4 inertia = ::Oyster::Math::Float4x4::identity;
+				inertia.m[0][0] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateHollowSphere( mass, radius );
+				inertia.m[1][1] = inertia.m[0][0];
+				inertia.m[2][2] = inertia.m[0][0];
+
+				return inertia;
+			}
+
+			inline ::Oyster::Math::Float CalculateCuboidX( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float depth )
+			{
+				return (1.0f/12.0f)*mass*(height*height + depth*depth);
+			}
+
+			inline ::Oyster::Math::Float CalculateCuboidY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth )
+			{
+				return (1.0f/12.0f)*mass*(width*width + depth*depth);
+			}
+
+			inline ::Oyster::Math::Float CalculateCuboidZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float height )
+			{
+				return (1.0f/12.0f)*mass*(height*height + width*width);
+			}
+
+			inline ::Oyster::Math::Float4x4 Cuboid( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth )
+			{
+				::Oyster::Math::Float4x4 inertia = ::Oyster::Math::Float4x4::identity;
+				inertia.m[0][0] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateCuboidX( mass , height, depth );
+				inertia.m[1][1] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateCuboidY( mass , width, depth  );
+				inertia.m[2][2] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateCuboidZ( mass , height, width );
+
+				return inertia;
+			}
+
+			inline ::Oyster::Math::Float CalculateRodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length )
+			{
+				return (1.0f/12.0f)*mass*(length*length);
+			}
+
+			inline ::Oyster::Math::Float4x4 RodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length )
+			{
+				::Oyster::Math::Float4x4 inertia = ::Oyster::Math::Float4x4::identity;
+				inertia.m[0][0] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateRodCenter( mass , length );
+				inertia.m[1][1] = inertia.m[0][0];
+				inertia.m[2][2] = inertia.m[0][0];
+
+				return inertia;
+			}
+
+			inline ::Oyster::Math::Float CalculateCylinderXY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius )
+			{
+				return (1.0f/12.0f)*mass*(3.0f*radius*radius + height*height);
+			}
+
+			inline ::Oyster::Math::Float CalculateCylinderZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius )
+			{
+				return 0.5f*mass*(radius*radius);
+			}
+			
+			inline ::Oyster::Math::Float4x4 Cylinder( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius )
+			{
+				::Oyster::Math::Float4x4 inertia = ::Oyster::Math::Float4x4::identity;
+				inertia.m[0][0] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateCylinderXY( mass , height, radius );
+				inertia.m[1][1] = inertia.m[0][0];
+				inertia.m[2][2] = ::Oyster::Physics3D::Formula::MomentOfInertia::CalculateCylinderZ( mass , radius );
+
+				return inertia;
+			}
+		
 		}
 	}
 } }

Code/OysterPhysics3D/Plane.cpp

@@ -31,7 +31,7 @@ bool Plane::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance );
 	case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target );
 	case Type_plane: return Utility::Intersect( *this, *(Plane*)target );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *(BoxAxisAligned*)target, *this );
 	case Type_box: return Utility::Intersect( *(Box*)target, *this );
 	case Type_frustrum: return false; // TODO: 
@@ -47,7 +47,7 @@ bool Plane::Contains( const ICollideable *target ) const
 	case Type_point: return Utility::Intersect( *this, *(Point*)target );
 	case Type_ray: return Utility::Contains( *this, *(Ray*)target );
 	case Type_plane: return Utility::Contains( *this, *(Plane*)target );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	default: return false;
 	}
 }

Code/OysterPhysics3D/Point.cpp

@@ -30,7 +30,7 @@ bool Point::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *(Ray*)target, *this, ((Ray*)target)->collisionDistance );
 	case Type_sphere: Utility::Intersect( *(Sphere*)target, *this );
 	case Type_plane: return Utility::Intersect( *(Plane*)target, *this );
-	case Type_triangle: return false; // TODO: 
+	 //case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *(BoxAxisAligned*)target, *this );
 	case Type_box: return Utility::Intersect( *(Box*)target, *this );
 	case Type_frustrum: return false; // TODO: 

Code/OysterPhysics3D/Ray.cpp

@@ -33,7 +33,7 @@ bool Ray::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, this->collisionDistance, ((Ray*)target)->collisionDistance );
 	case Type_sphere: return Utility::Intersect( *(Sphere*)target, *this, this->collisionDistance );
 	case Type_plane: return Utility::Intersect( *(Plane*)target, *this, this->collisionDistance );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *(BoxAxisAligned*)target, *this, this->collisionDistance );
 	case Type_box: return Utility::Intersect( *(Box*)target, *this, this->collisionDistance );
 	case Type_frustrum: return false; // TODO: 

Code/OysterPhysics3D/RigidBody.cpp

@@ -39,20 +39,21 @@ RigidBody & RigidBody::operator = ( const RigidBody &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
-	// dv = dt * a = dt * F / m
-	// ds = dt * avg_v
-	Float3 deltaLinearVelocity = this->impulseForceSum;
-	deltaLinearVelocity *= (deltaTime / this->mass);
-	Float3 deltaPos = deltaTime * ::Utility::Value::AverageWithDelta( Formula::LinearVelocity(this->mass, this->linearMomentum), deltaLinearVelocity );
+	// dG = F  * dt
+	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
+	Float3 linearImpulse =  this->impulseForceSum * deltaTime; // aka deltaLinearMomentum
+	Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse );
 
 	// updating the angular
-	// dw = dt * a = dt * ( I^-1 * T )
-	// rotation = dt * avg_w
-	Float4x4 inversedMomentOfInertiaTensor = this->momentOfInertiaTensor.GetInverse();
-	Float3 deltaAngularVelocity = Formula::AngularImpulseAcceleration( inversedMomentOfInertiaTensor, this->impulseTorqueSum ); // I^-1 * T
-	deltaAngularVelocity *= deltaTime; 
-	Float3 rotationAxis = ::Utility::Value::AverageWithDelta( Formula::AngularVelocity(inversedMomentOfInertiaTensor,this->angularMomentum), deltaAngularVelocity );
+	// dH = T * dt
+	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H
+	Float3 angularImpulse = this->impulseTorqueSum * deltaTime; // aka deltaAngularMomentum
+	Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(),
+													::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) );
 	
 	Float deltaRadian = rotationAxis.Dot( rotationAxis );
 	if( deltaRadian != 0.0f )
@@ -60,95 +61,72 @@ void RigidBody::Update_LeapFrog( Float deltaTime )
 		deltaRadian = ::std::sqrt( deltaRadian );
 		rotationAxis /= deltaRadian;
 
-		// using rotationAxis, deltaRadian and deltaPos to create a matrix to update the orientation matrix
-		UpdateOrientationMatrix( deltaPos, RotationMatrix(deltaRadian, rotationAxis), this->box.orientation );
-
-		/** @todo TODO: ISSUE! how is momentOfInertiaTensor related to the orientation of the RigidBody? */
+		// using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box
+		this->box.center += deltaPos;
+		TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation );
 	}
 	else
 	{ // no rotation, only use deltaPos to translate the RigidBody
 		this->box.center += deltaPos;
 	}
 
-	// update movements and clear impulses
-	this->linearMomentum += Formula::LinearMomentum( this->mass, deltaLinearVelocity );
+	// update momentums and clear impulseForceSum and impulseTorqueSum
+	this->linearMomentum += linearImpulse;
 	this->impulseForceSum = Float3::null;
-	this->angularMomentum += Formula::AngularMomentum( this->momentOfInertiaTensor, deltaAngularVelocity );
+	this->angularMomentum += angularImpulse;
 	this->impulseTorqueSum = Float3::null;
 }
 
-void RigidBody::ApplyImpulseForce( const Float3 &f )
+void RigidBody::ApplyImpulseForce( const Float3 &worldF )
 { // by Dan Andersson
-	this->impulseForceSum += f;
+	this->impulseForceSum += worldF;
 }
 
-void RigidBody::ApplyImpulseForceAt_Local( const Float3 &localForce, const Float3 &localOffset )
+void RigidBody::ApplyImpulseForceAt( const Float3 &worldF, const Float3 &worldPos )
 { // by Dan Andersson
-	if( localOffset != Float3::null )
+	Float3 worldOffset = worldPos - this->box.center;
+	if( worldOffset != Float3::null )
 	{
-		this->impulseForceSum += VectorProjection( localForce, localOffset );
-		this->impulseTorqueSum += Formula::ImpulseTorque( localForce, localOffset );
+		this->impulseForceSum += VectorProjection( worldF, worldOffset );
+		this->impulseTorqueSum += Formula::ImpulseTorque( worldF, worldOffset );
 	}
 	else
 	{
-		this->impulseForceSum += localForce;
+		this->impulseForceSum += worldF;
 	}
 }
 
-void RigidBody::ApplyImpulseForceAt_World( const Float3 &worldForce, const Float3 &worldPos )
+void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &worldA )
 { // by Dan Andersson
-	Float4x4 view = this->GetView();
-	this->ApplyImpulseForceAt_Local( (view * Float4(worldForce, 0.0f)).xyz,
-									 (view * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
+	this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA );
 }
 
-void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &a )
+void RigidBody::ApplyLinearImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos )
 { // by Dan Andersson
-	this->impulseForceSum += Formula::ImpulseForce( this->mass, a );
-}
-
-void RigidBody::ApplyLinearImpulseAccelerationAt_Local( const Float3 &localImpulseLinearAcc, const Float3 &localOffset )
-{ // by Dan Andersson
-	if( localOffset != Float3::null )
+	Float3 worldOffset = worldPos - this->box.center;
+	if( worldOffset != Float3::null )
 	{
-		this->impulseForceSum += Formula::ImpulseForce( this->mass, VectorProjection(localImpulseLinearAcc, localOffset) );
+		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(localImpulseLinearAcc, localOffset) );
+		this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, Formula::AngularImpulseAcceleration(worldA, worldOffset) );
 	}
 	else
 	{
-		this->impulseForceSum += Formula::ImpulseForce( this->mass, localImpulseLinearAcc );
+		this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA );
 	}
 }
 
-void RigidBody::ApplyLinearImpulseAccelerationAt_World( const Float3 &worldImpulseLinearAcc, const Float3 &worldPos )
+void RigidBody::ApplyImpulseTorque( const Float3 &worldT )
 { // by Dan Andersson
-	Float4x4 view = this->GetView();
-	this->ApplyLinearImpulseAccelerationAt_Local( (view * Float4(worldImpulseLinearAcc, 0.0f)).xyz,
-												  (view * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
+	this->impulseTorqueSum += worldT;
 }
 
-void RigidBody::ApplyImpulseTorque( const Float3 &t )
+void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &worldA )
 { // by Dan Andersson
-	this->impulseTorqueSum += t;
-}
-
-void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &a )
-{ // by Dan Andersson
-	this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, a );
-}
-
-Float4x4 & RigidBody::AccessOrientation()
-{ // by Dan Andersson
-	return this->box.orientation;
-}
-
-const Float4x4 & RigidBody::AccessOrientation() const
-{ // by Dan Andersson
-	return this->box.orientation;
+	this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA );
 }
 
 Float3 & RigidBody::AccessBoundingReach()
@@ -181,14 +159,14 @@ const Float & RigidBody::GetMass() const
 	return this->mass;
 }
 
-const Float4x4 & RigidBody::GetOrientation() const
+const Float4x4 RigidBody::GetOrientation() const
 { // by Dan Andersson
-	return this->box.orientation;
+	return OrientationMatrix( this->box.rotation, this->box.center );
 }
 
 Float4x4 RigidBody::GetView() const
 { // by Dan Andersson
-	return InverseOrientationMatrix( this->box.orientation );
+	return InverseOrientationMatrix( this->GetOrientation() );
 }
 
 const Float3 & RigidBody::GetBoundingReach() const
@@ -246,106 +224,32 @@ Float3 RigidBody::GetLinearVelocity() const
 	return Formula::LinearVelocity( this->mass, this->linearMomentum );
 }
 
-Float3 RigidBody::GetTangentialImpulseForceAt_Local( const Float3 &localPos ) const
+void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const
 { // by Dan Andersson
-	return Formula::TangentialImpulseForce( this->impulseTorqueSum, localPos );
+	Float3 worldOffset = worldPos - this->box.center;
+	Float3 momentum = Formula::TangentialLinearMomentum( this->angularMomentum, worldOffset );
+	momentum += this->linearMomentum;
+
+	normalMomentum = NormalProjection( momentum, surfaceNormal );
+	tangentialMomentum = momentum - normalMomentum;
 }
 
-Float3 RigidBody::GetTangentialImpulseForceAt_World( const Float3 &worldPos ) const
+void RigidBody::SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI )
 { // by Dan Andersson
-	return this->GetTangentialImpulseForceAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
-}
-
-Float3 RigidBody::GetTangentialLinearMomentumAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	return Formula::TangentialLinearMomentum( this->angularMomentum, localPos );
-}
-
-Float3 RigidBody::GetTangentialLinearMomentumAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	return this->GetTangentialLinearMomentumAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
-}
-
-Float3 RigidBody::GetTangentialImpulseAccelerationAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	return Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, localPos );
-}
-
-Float3 RigidBody::GetTangentialImpulseAccelerationAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	return this->GetTangentialImpulseAccelerationAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
-}
-
-Float3 RigidBody::GetTangentialLinearVelocityAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	return Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, localPos );
-}
-
-Float3 RigidBody::GetTangentialLinearVelocityAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	return this->GetTangentialLinearVelocityAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f
-}
-
-Float3 RigidBody::GetImpulseForceAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	return this->impulseForceSum + Formula::TangentialImpulseForce( this->impulseForceSum, localPos );
-}
-
-Float3 RigidBody::GetImpulseForceAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	Float4 localForce = Float4( this->GetImpulseForceAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f
-	return (this->box.orientation * localForce).xyz; // should not be any disform thus result.w = 0.0f
-}
-
-Float3 RigidBody::GetLinearMomentumAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	// Reminder! Momentum is a world value.
-	return Float3::null; // TODO: 
-}
-
-Float3 RigidBody::GetLinearMomentumAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	// Reminder! Momentum is a world value.
-	Float4 localMomentum = Float4( this->GetLinearMomentumAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f
-	return (this->box.orientation * localMomentum).xyz; // should not be any disform thus result.w = 0.0f
-
-	// TODO: angularMomentum is a local value!!
-	return this->linearMomentum + Formula::TangentialLinearMomentum( this->angularMomentum, worldPos );
-}
-
-Float3 RigidBody::GetImpulseAccelerationAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	// Reminder! Acceleration is a world value.
-	Float4 worldAccel = Float4( this->GetImpulseAccelerationAt_Local((this->box.orientation * Float4(localPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f
-	return (this->GetView() * worldAccel).xyz; // should not be any disform thus result.w = 0.0f
-}
-
-Float3 RigidBody::GetImpulseAccelerationAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	// Reminder! Acceleration is a world value.
-	return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum )
-		 + Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, worldPos );
-}
-
-Float3 RigidBody::GetLinearVelocityAt_Local( const Float3 &localPos ) const
-{ // by Dan Andersson
-	// Reminder! Velocity is a world value.
-	Float4 worldV = Float4( this->GetLinearVelocityAt_Local((this->box.orientation * Float4(localPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f
-	return (this->GetView() * worldV).xyz; // should not be any disform thus result.w = 0.0f
-}
-
-Float3 RigidBody::GetLinearVelocityAt_World( const Float3 &worldPos ) const
-{ // by Dan Andersson
-	// Reminder! Velocity is a world value.
-	return Formula::LinearVelocity( this->mass, this->linearMomentum )
-		 + Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, worldPos );
-}
-
-void RigidBody::SetMomentOfInertia( const Float4x4 &i )
-{ // by Dan Andersson
-	if( i.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
+	if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
 	{
-		this->momentOfInertiaTensor = i;
+		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 )
+{ // by Dan Andersson
+	if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
+	{
+		this->momentOfInertiaTensor = localI;
 	}
 }
 
@@ -353,9 +257,9 @@ void RigidBody::SetMass_KeepVelocity( const Float &m )
 { // by Dan Andersson
 	if( m != 0.0f ) // insanitycheck! mass must be invertable
 	{
-		Float3 velocity = Formula::LinearVelocity( this->mass, this->linearMomentum );
+		Float3 v = Formula::LinearVelocity( this->mass, this->linearMomentum );
 		this->mass = m;
-		this->linearMomentum = Formula::LinearMomentum( this->mass, velocity );
+		this->linearMomentum = Formula::LinearMomentum( this->mass, v );
 	}
 }
 
@@ -369,7 +273,8 @@ void RigidBody::SetMass_KeepMomentum( const Float &m )
 
 void RigidBody::SetOrientation( const Float4x4 &o )
 { // by Dan Andersson
-	this->box.orientation = o;
+	 ExtractRotationMatrix( o, this->box.rotation );
+	 this->box.center = o.v[3].xyz;
 }
 
 void RigidBody::SetSize( const Float3 &widthHeight )
@@ -377,98 +282,77 @@ void RigidBody::SetSize( const Float3 &widthHeight )
 	this->box.boundingOffset = 0.5f * widthHeight;
 }
 
-void RigidBody::SetCenter( const Float3 &p )
+void RigidBody::SetCenter( const Float3 &worldPos )
 { // by Dan Andersson
-	this->box.center = p;
+	this->box.center = worldPos;
 }
 
-void RigidBody::SetImpulseTorque( const Float3 &t )
+void RigidBody::SetImpulseTorque( const Float3 &worldT )
 { // by Dan Andersson
-	this->impulseTorqueSum = t;
+	this->impulseTorqueSum = worldT;
 }
 
-void RigidBody::SetAngularMomentum( const Float3 &h )
+void RigidBody::SetAngularMomentum( const Float3 &worldH )
 { // by Dan Andersson
-	this->angularMomentum = h;
+	this->angularMomentum = worldH;
 }
 
-void RigidBody::SetAngularImpulseAcceleration( const Float3 &a )
+void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA )
 { // by Dan Andersson
-	this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, a );
+	this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA );
 }
 
-void RigidBody::SetAngularVelocity( const Float3 &w )
+void RigidBody::SetAngularVelocity( const Float3 &worldW )
 { // by Dan Andersson
-	this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, w );
+	this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW );
 }
 
-void RigidBody::SetImpulseForce( const Float3 &f )
+void RigidBody::SetImpulseForce( const Float3 &worldF )
 { // by Dan Andersson
-	this->impulseForceSum = f;
+	this->impulseForceSum = worldF;
 }
 
-void RigidBody::SetLinearMomentum( const Float3 &g )
+void RigidBody::SetLinearMomentum( const Float3 &worldG )
 { // by Dan Andersson
-	this->linearMomentum = g;
+	this->linearMomentum = worldG;
 }
 
-void RigidBody::SetLinearImpulseAcceleration( const Float3 &a )
+void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA )
 { // by Dan Andersson
-	this->impulseForceSum = Formula::ImpulseForce( this->mass, a );
+	this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA );
 }
 
-void RigidBody::SetLinearVelocity( const Float3 &v )
+void RigidBody::SetLinearVelocity( const Float3 &worldV )
 { // by Dan Andersson
-	this->linearMomentum = Formula::LinearMomentum( this->mass, v );
+	this->linearMomentum = Formula::LinearMomentum( this->mass, worldV );
 }
 
-void RigidBody::SetImpulseForceAt_Local( const Float3 &localForce, const Float3 &localPos )
+void RigidBody::SetImpulseForceAt( const Float3 &worldForce, const Float3 &worldPos )
 { // by Dan Andersson
-	// Reminder! Impulse force and torque is world values.
-	Float3 worldForce = ( this->box.orientation * Float4(localForce, 0.0f) ).xyz,
-		   worldPos = ( this->box.orientation * Float4(localPos, 1.0f) ).xyz;
-	this->SetImpulseForceAt_World( worldForce, worldPos );
-
+	Float3 worldOffset = worldPos - this->box.center;
+	this->impulseForceSum = VectorProjection( worldForce, worldOffset );
+	this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset );
 }
 
-void RigidBody::SetImpulseForceAt_World( const Float3 &worldForce, const Float3 &worldPos )
+void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos )
 { // by Dan Andersson
-	// Reminder! Impulse force and torque is world values.
-	this->impulseForceSum = VectorProjection( worldForce, worldPos );
-	this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldPos );
+	Float3 worldOffset = worldPos - this->box.center;
+	this->linearMomentum = VectorProjection( worldG, worldOffset );
+	this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset );
 }
 
-void RigidBody::SetLinearMomentumAt_Local( const Float3 &localG, const Float3 &localPos )
+void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos )
 { // by Dan Andersson
-	// Reminder! Linear and angular momentum is world values.
-	Float3 worldG = ( this->box.orientation * Float4(localG, 0.0f) ).xyz,
-		   worldPos = ( this->box.orientation * Float4(localPos, 1.0f) ).xyz;
-	this->SetLinearMomentumAt_World( worldG, worldPos );
+	Float3 worldOffset = worldPos - this->box.center;
+	this->impulseForceSum = Formula::ImpulseForce( this->mass, VectorProjection(worldA, worldOffset) );
+	this->impulseTorqueSum = Formula::ImpulseTorque( this->box.rotation * this->momentOfInertiaTensor,
+													 Formula::AngularImpulseAcceleration(worldA, worldOffset) );
 }
 
-void RigidBody::SetLinearMomentumAt_World( const Float3 &worldG, const Float3 &worldPos )
+void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &worldPos )
 { // by Dan Andersson
-	// Reminder! Linear and angular momentum is world values.
-	this->linearMomentum = VectorProjection( worldG, worldPos );
-	this->angularMomentum = Formula::AngularMomentum( worldG, worldPos );
-}
-
-void RigidBody::SetImpulseAccelerationAt_Local( const Float3 &a, const Float3 &pos )
-{ // by Dan Andersson
-
-}
-
-void RigidBody::SetImpulseAccelerationAt_World( const Float3 &a, const Float3 &pos )
-{ // by 
-
-}
-
-void RigidBody::SetLinearVelocityAt_Local( const Float3 &v, const Float3 &pos )
-{ // by 
-
-}
-
-void RigidBody::SetLinearVelocityAt_World( const Float3 &v, const Float3 &pos )
-{ // by 
-
+	Float3 worldOffset = worldPos - this->box.center;
+	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

@@ -14,30 +14,26 @@ namespace Oyster { namespace Physics3D
 	struct RigidBody
 	{	/// A struct of a simple rigid body.
 	public:
-		::Oyster::Collision3D::Box box;				/// Contains data representing physical presence.
-		::Oyster::Math::Float3 angularMomentum,		/// The angular momentum H (Nm*s) around an parallell axis.
-							   linearMomentum,		/// The linear momentum G (kg*m/s).
-							   impulseTorqueSum,	/// The impulse torque T (Nm) that will be consumed each update.
-							   impulseForceSum;		/// The impulse force F (N) that will be consumed each update.
+		::Oyster::Collision3D::Box box;				/** Contains data representing physical presence.					(worldValue) */
+		::Oyster::Math::Float3 angularMomentum,		/** The angular momentum H (Nm*s) around an parallell axis.			(worldValue) */
+							   linearMomentum,		/** The linear momentum G (kg*m/s).									(worldValue) */
+							   impulseTorqueSum,	/** The impulse torque T (Nm) that will be consumed each update.	(worldValue) */
+							   impulseForceSum;		/** The impulse force F (N) that will be consumed each update.		(worldValue) */
 
 		RigidBody( const ::Oyster::Collision3D::Box &box = ::Oyster::Collision3D::Box(), ::Oyster::Math::Float mass = 1.0f );
 
 		RigidBody & operator = ( const RigidBody &body );
 
 		void Update_LeapFrog( ::Oyster::Math::Float deltaTime );
-		void ApplyImpulseForce( const ::Oyster::Math::Float3 &f );
-		void ApplyImpulseForceAt_Local( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos );
-		void ApplyImpulseForceAt_World( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos ); /// ApplyImpulseForce_LocalPos is preferred
-		void ApplyLinearImpulseAcceleration( const ::Oyster::Math::Float3 &a );
-		void ApplyLinearImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos );
-		void ApplyLinearImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos ); /// ApplyLinearImpulseAcceleration_LocalPos is preferred
-		void ApplyImpulseTorque( const ::Oyster::Math::Float3 &t );
-		void ApplyAngularImpulseAcceleration( const ::Oyster::Math::Float3 &a );
+		void ApplyImpulseForce( const ::Oyster::Math::Float3 &worldF );
+		void ApplyImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos );
+		void ApplyLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
+		void ApplyLinearImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos );
+		void ApplyImpulseTorque( const ::Oyster::Math::Float3 &worldT );
+		void ApplyAngularImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
 
 		// ACCESS METHODS /////////////////////////////
 
-		::Oyster::Math::Float4x4 &		 AccessOrientation();
-		const ::Oyster::Math::Float4x4 & AccessOrientation() const;
 		::Oyster::Math::Float3 &		 AccessBoundingReach();
 		const ::Oyster::Math::Float3 &	 AccessBoundingReach() const;
 		::Oyster::Math::Float3 &		 AccessCenter();
@@ -48,7 +44,7 @@ namespace Oyster { namespace Physics3D
 		const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const;
 		const ::Oyster::Math::Float &	 GetMass() const;
 
-		const ::Oyster::Math::Float4x4 & GetOrientation() const;
+		const ::Oyster::Math::Float4x4   GetOrientation() const;
 		::Oyster::Math::Float4x4		 GetView() const;
  		const ::Oyster::Math::Float4x4 & GetToWorldMatrix() const;
 		::Oyster::Math::Float4x4		 GetToLocalMatrix() const;
@@ -68,56 +64,37 @@ namespace Oyster { namespace Physics3D
 		::Oyster::Math::Float3			 GetLinearImpulseAcceleration() const;
 		::Oyster::Math::Float3			 GetLinearVelocity() const;
 
-		::Oyster::Math::Float3			 GetTangentialImpulseForceAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialImpulseForceAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialLinearMomentumAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialLinearMomentumAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialLinearVelocityAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetTangentialLinearVelocityAt_World( const ::Oyster::Math::Float3 &pos ) const;
-
-		::Oyster::Math::Float3			 GetImpulseForceAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetImpulseForceAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetLinearMomentumAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetLinearMomentumAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetLinearVelocityAt_Local( const ::Oyster::Math::Float3 &pos ) const;
-		::Oyster::Math::Float3			 GetLinearVelocityAt_World( const ::Oyster::Math::Float3 &pos ) 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( const ::Oyster::Math::Float4x4 &i );
+		void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI );
+		void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localI );
 		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 &p );
+		void SetCenter( const ::Oyster::Math::Float3 &worldPos );
 		
-		void SetImpulseTorque( const ::Oyster::Math::Float3 &t );
-		void SetAngularMomentum( const ::Oyster::Math::Float3 &h );
-		void SetAngularImpulseAcceleration( const ::Oyster::Math::Float3 &a );
-		void SetAngularVelocity( const ::Oyster::Math::Float3 &w );
+		void SetImpulseTorque( const ::Oyster::Math::Float3 &worldT );
+		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 &f );
-		void SetLinearMomentum( const ::Oyster::Math::Float3 &g );
-		void SetLinearImpulseAcceleration( const ::Oyster::Math::Float3 &a );
-		void SetLinearVelocity( const ::Oyster::Math::Float3 &v );
+		void SetImpulseForce( const ::Oyster::Math::Float3 &worldF );
+		void SetLinearMomentum( const ::Oyster::Math::Float3 &worldG );
+		void SetLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA );
+		void SetLinearVelocity( const ::Oyster::Math::Float3 &worldV );
 
-		void SetImpulseForceAt_Local( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos );
-		void SetImpulseForceAt_World( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos );
-		void SetLinearMomentumAt_Local( const ::Oyster::Math::Float3 &g, const ::Oyster::Math::Float3 &pos );
-		void SetLinearMomentumAt_World( const ::Oyster::Math::Float3 &g, const ::Oyster::Math::Float3 &pos );
-		void SetImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos );
-		void SetImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos );
-		void SetLinearVelocityAt_Local( const ::Oyster::Math::Float3 &v, const ::Oyster::Math::Float3 &pos );
-		void SetLinearVelocityAt_World( const ::Oyster::Math::Float3 &v, const ::Oyster::Math::Float3 &pos );
+		void SetImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos );
+		void SetLinearMomentumAt( const ::Oyster::Math::Float3 &worldG, const ::Oyster::Math::Float3 &worldPos );
+		void SetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos );
+		void SetLinearVelocityAt( const ::Oyster::Math::Float3 &worldV, const ::Oyster::Math::Float3 &worldPos );
 
 	private:
-		::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 )
+		::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) */
 	};
 
 	// INLINE IMPLEMENTATIONS ///////////////////////////////////////

Code/OysterPhysics3D/Sphere.cpp

@@ -27,7 +27,7 @@ bool Sphere::Intersects( const ICollideable *target ) const
 	case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance );
 	case Type_sphere: Utility::Intersect( *this, *(Sphere*)target );
 	case Type_plane: return Utility::Intersect( *(Plane*)target, *this );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return Utility::Intersect( *(BoxAxisAligned*)target, *this );
 	case Type_box: return Utility::Intersect( *(Box*)target, *this );
 	case Type_frustrum: return false; // TODO: 
@@ -41,7 +41,7 @@ bool Sphere::Contains( const ICollideable *target ) const
 	{
 	case Type_point:  return Utility::Intersect( *this, *(Point*)target );
 	case Type_sphere: return Utility::Contains( *this, *(Sphere*)target );
-	case Type_triangle: return false; // TODO: 
+	// case Type_triangle: return false; // TODO: 
 	case Type_box_axis_aligned: return false; // TODO: 
 	case Type_box: return false; // TODO: 
 	case Type_frustrum: return false; // TODO: