Box updated

orientation Matrix have been split into a rotation Matrix and a
translation Vector

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 */
@@ -349,6 +408,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.

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,26 +96,65 @@ 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 & 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 )
 	{

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.
@@ -187,6 +214,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.

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( )
-Box::Box( const Float4x4 &o, const Float3 &s ) : ICollideable(Type_box), orientation(o), boundingOffset(s*0.5) {}
+	: 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
 {

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/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 );
+				tMin = Max( tMin, t1 );
-				tMax = ::Utility::Value::Min( tMax, t2 );
+				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.x * Abs(plane.normal.x); // Box max extending towards plane
-			boxExtend += h.y * ::Utility::Value::Abs(plane.normal.y);
+			boxExtend += h.y * Abs(plane.normal.y);
-			boxExtend += h.z * ::Utility::Value::Abs(plane.normal.z);
+			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.x * Abs(plane.normal.Dot(box.xAxis)); // Box max extending towards plane
-			boxExtend += box.boundingOffset.y * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[1].xyz));
+			boxExtend += box.boundingOffset.y * Abs(plane.normal.Dot(box.yAxis));
-			boxExtend += box.boundingOffset.z * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[2].xyz));
+			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();
+			 * Uses the Seperating Axis Theorem
-				absOrientationB.v[0] = ::Utility::Value::Abs(tO.v[0]);
+			 * if( |t dot s| > hA dot |s * RA| + hB dot |s * RB| ) then not intersecting
-				if( absOrientationB.v[0].w > boundingOffsetA.x + boundingOffsetB.Dot(absOrientationB.v[0].xyz) ) return false;
+			 *     |t dot s| > hA dot |s| + hB dot |s * RB| .. as RA = I
-				absOrientationB.v[1] = ::Utility::Value::Abs(tO.v[1]);
+			 *
-				if( absOrientationB.v[1].w > boundingOffsetA.y + boundingOffsetB.Dot(absOrientationB.v[1].xyz) ) return false;
+			 * t: objectB's offset from objectA														[worldOffset]
-				absOrientationB.v[2] = ::Utility::Value::Abs(tO.v[2]);
+			 * s: current comparison axis
-				if( absOrientationB.v[2].w > boundingOffsetA.z + boundingOffsetB.Dot(absOrientationB.v[2].xyz) ) return false;
+			 * 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();
+			// s = { 0, 1, 0 }	[ RA.v[1] ]
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[0])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[0].xyz) ) return false;
+			if( absWorldOffset.y > boundingOffsetA.y + boundingOffsetB.Dot(Float3(absRotationB.v[0].y, absRotationB.v[1].y, absRotationB.v[2].y)) )
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[1])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[1].xyz) ) return false;
+			{ // t.y > hA.y + hB dot |{RB.v[0].y, RB.v[1].y, RB.v[2].y}|
-			if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[2])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[2].xyz) ) return false;
+				return false;
+			}
 
-			// ( 1,0,0 ) x orientationB.v[0].xyz:
+			// s = { 0, 0, 1 }	[ RA.v[2] ]
-			Float d = boundingOffsetA.y * absOrientationB.v[0].z;
+			if( absWorldOffset.z > boundingOffsetA.z + boundingOffsetB.Dot(Float3(absRotationB.v[0].z, absRotationB.v[1].z, absRotationB.v[2].z)) )
-			d += boundingOffsetA.z * absOrientationB.v[0].y;
+			{ // t.z > hA.z + hB dot |{RB.v[0].z, RB.v[1].z, RB.v[2].z}|
-			d += boundingOffsetB.y * absOrientationB.v[2].x;
+				return false;
-			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;
 
-			// ( 1,0,0 ) x orientationB.v[1].xyz:
+			// s = RB.v[0].xyz
-			d  = boundingOffsetA.y * absOrientationB.v[1].z;
+			if( Abs(worldOffset.Dot(rotationB.v[0].xyz)) > boundingOffsetA.Dot(absRotationB.v[0].xyz) + boundingOffsetB.x )
-			d += boundingOffsetA.z * absOrientationB.v[1].y;
+			{ // |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
-			d += boundingOffsetB.x * absOrientationB.v[2].x;
+				return false;
-			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;
 
-			// ( 1,0,0 ) x orientationB.v[2].xyz:
+			// s = RB.v[1].xyz
-			d  = boundingOffsetA.y * absOrientationB.v[2].z;
+			if( Abs(worldOffset.Dot(rotationB.v[1].xyz)) > boundingOffsetA.Dot(absRotationB.v[1].xyz) + boundingOffsetB.y )
-			d += boundingOffsetA.z * absOrientationB.v[2].y;
+			{ // |t dot s| > hA dot |s| + hB.y
-			d += boundingOffsetB.x * absOrientationB.v[1].x;
+				return false;
-			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;
 
-			// ( 0,1,0 ) x orientationB.v[0].xyz:
+			// s = RB.v[2].xyz
-			d  = boundingOffsetA.x * absOrientationB.v[0].z;
+			if( Abs(worldOffset.Dot(rotationB.v[2].xyz)) > boundingOffsetA.Dot(absRotationB.v[2].xyz) + boundingOffsetB.z )
-			d += boundingOffsetA.z * absOrientationB.v[0].x;
+			{ // |t dot s| > hA dot |s| + hB.z
-			d += boundingOffsetB.y * absOrientationB.v[2].y;
+				return false;
-			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;
 
-			// ( 0,1,0 ) x orientationB.v[1].xyz:
+			// s = ( 1,0,0 ) x rotationB.v[0].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[1].z;
+			Float d = boundingOffsetA.y * absRotationB.v[0].z;
-			d += boundingOffsetA.z * absOrientationB.v[1].x;
+			d += boundingOffsetA.z * absRotationB.v[0].y;
-			d += boundingOffsetB.x * absOrientationB.v[2].y;
+			d += boundingOffsetB.y * absRotationB.v[2].x;
-			d += boundingOffsetB.z * absOrientationB.v[0].y;
+			d += boundingOffsetB.z * absRotationB.v[1].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[1].z - orientationB.v[3].z*orientationB.v[1].x) > d ) return false;
+			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:
+			// s = ( 1,0,0 ) x rotationB.v[1].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[2].z;
+			d  = boundingOffsetA.y * absRotationB.v[1].z;
-			d += boundingOffsetA.z * absOrientationB.v[2].x;
+			d += boundingOffsetA.z * absRotationB.v[1].y;
-			d += boundingOffsetB.x * absOrientationB.v[1].y;
+			d += boundingOffsetB.x * absRotationB.v[2].x;
-			d += boundingOffsetB.y * absOrientationB.v[0].y;
+			d += boundingOffsetB.z * absRotationB.v[0].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[2].z - orientationB.v[3].z*orientationB.v[2].x) > d ) return false;
+			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:
+			// s = ( 1,0,0 ) x rotationB.v[2].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[0].y;
+			d  = boundingOffsetA.y * absRotationB.v[2].z;
-			d += boundingOffsetA.y * absOrientationB.v[0].x;
+			d += boundingOffsetA.z * absRotationB.v[2].y;
-			d += boundingOffsetB.y * absOrientationB.v[2].z;
+			d += boundingOffsetB.x * absRotationB.v[1].x;
-			d += boundingOffsetB.z * absOrientationB.v[1].z;
+			d += boundingOffsetB.y * absRotationB.v[0].x;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[0].x - orientationB.v[3].x*orientationB.v[0].y) > d ) return false;
+			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:
+			// s = ( 0,1,0 ) x rotationB.v[0].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[1].y;
+			d  = boundingOffsetA.x * absRotationB.v[0].z;
-			d += boundingOffsetA.y * absOrientationB.v[1].x;
+			d += boundingOffsetA.z * absRotationB.v[0].x;
-			d += boundingOffsetB.x * absOrientationB.v[2].z;
+			d += boundingOffsetB.y * absRotationB.v[2].y;
-			d += boundingOffsetB.z * absOrientationB.v[0].z;
+			d += boundingOffsetB.z * absRotationB.v[1].y;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[1].x - orientationB.v[3].x*orientationB.v[1].y) > d ) return false;
+			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:
+			// s = ( 0,1,0 ) x rotationB.v[1].xyz:
-			d  = boundingOffsetA.x * absOrientationB.v[2].y;
+			d  = boundingOffsetA.x * absRotationB.v[1].z;
-			d += boundingOffsetA.y * absOrientationB.v[2].x;
+			d += boundingOffsetA.z * absRotationB.v[1].x;
-			d += boundingOffsetB.x * absOrientationB.v[1].z;
+			d += boundingOffsetB.x * absRotationB.v[2].y;
-			d += boundingOffsetB.y * absOrientationB.v[0].z;
+			d += boundingOffsetB.z * absRotationB.v[0].y;
-			if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[2].x - orientationB.v[3].x*orientationB.v[2].y) > d ) return false;
+			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 );
+		Float3 e = Max( box.minVertex - sphere.center, Float3::null );
-		e += ::Utility::Value::Max( sphere.center - box.maxVertex, 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,
+		Float3 e = sphere.center - box.center,
-			   centerL = Float3( e.Dot(box.orientation.v[0].xyz), e.Dot(box.orientation.v[1].xyz), e.Dot(box.orientation.v[2].xyz) );
+			   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  = Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null );
-		e += ::Utility::Value::Max( centerL - box.boundingOffset, 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;
+		Float3 alignedOffsetBoundaries = (boxB.maxVertex - boxB.minVertex) * 0.5f,
-		Float4x4 translated = boxA.orientation;
+			   offset = boxA.center - Average( boxB.minVertex, boxB.maxVertex );
-		translated.v[3].xyz -= boxB.minVertex;
+		return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( alignedOffsetBoundaries, boxA.boundingOffset, boxA.rotation, offset );
-		translated.v[3].xyz += alignedOffsetBoundaries * 0.5f;
-		alignedOffsetBoundaries = ::Utility::Value::Abs(alignedOffsetBoundaries);
-		return Private::FifteenAxisVsAlignedAxisOverlappingChecks( alignedOffsetBoundaries, boxA.boundingOffset, translated );
 	}
 
 	bool Intersect( const Box &boxA, const Box &boxB )
 	{ // by Dan Andersson
-		Float4x4 M;
+		Float4x4 orientationA = OrientationMatrix(boxA.rotation, boxA.center),
-		InverseOrientationMatrix( boxA.orientation, M );
+				 orientationB = OrientationMatrix(boxB.rotation, boxB.center),
-		TransformMatrix( M, boxB.orientation, M ); /// TODO: not verified
+				 invOrientationA = InverseOrientationMatrix( orientationA );
-		return Private::FifteenAxisVsAlignedAxisOverlappingChecks( boxA.boundingOffset, boxB.boundingOffset, M );
+
+		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/RigidBody.cpp

@@ -40,7 +40,7 @@ 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.orientation, this->momentOfInertiaTensor );
+	Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor );
 
 	// updating the linear
 	// dv = dt * a = dt * F / m
@@ -63,10 +63,9 @@ 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
+		// using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box
-		UpdateOrientationMatrix( deltaPos, RotationMatrix(deltaRadian, rotationAxis), this->box.orientation );
+		this->box.center += deltaPos;
-
+		TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation );
-		/** @todo TODO: ISSUE! how is momentOfInertiaTensor related to the orientation of the RigidBody? */
 	}
 	else
 	{ // no rotation, only use deltaPos to translate the RigidBody

Code/OysterPhysics3D/RigidBody.h

@@ -34,8 +34,6 @@ namespace Oyster { namespace Physics3D
 
 		// 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();
@@ -46,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;