From 1aa66dc79fc8c5646ad9d5daf878a855bce913ff Mon Sep 17 00:00:00 2001
From: Robin Engman <robinengman@hotmail.com>
Date: Tue, 11 Feb 2014 13:53:44 +0100
Subject: [PATCH] Quaternion to angular axis complete

---
 .../Implementation/SimpleRigidBody.cpp        | 22 +++++++++++++++++++
 .../Implementation/SimpleRigidBody.h          |  1 +
 Code/GamePhysics/PhysicsAPI.h                 |  9 ++++----
 3 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp
index 1201cad8..58f598ed 100644
--- a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp
+++ b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp
@@ -186,6 +186,28 @@ Float4x4 SimpleRigidBody::GetRotation() const
 {
 	return this->state.GetRotation();
 }
+Float4 SimpleRigidBody::GetRotationAsAngularAxis()
+{
+	Float4 axis = Float4::null;
+	Float s = sqrtf(1 - this->state.quaternion.real*this->state.quaternion.real);
+
+	axis.w = 2*acos(this->state.quaternion.real*this->state.quaternion.real);
+
+	if(1 - this->state.quaternion.real > 0.001f)
+	{
+		axis.x = this->state.quaternion.imaginary.x/s;
+		axis.y = this->state.quaternion.imaginary.y/s;
+		axis.z = this->state.quaternion.imaginary.z/s;
+	}
+	else
+	{
+		axis.x = this->state.quaternion.imaginary.x;
+		axis.y = this->state.quaternion.imaginary.y;
+		axis.z = this->state.quaternion.imaginary.z;
+	}
+
+	return axis;
+}
 
 Float4x4 SimpleRigidBody::GetOrientation() const
 {
diff --git a/Code/GamePhysics/Implementation/SimpleRigidBody.h b/Code/GamePhysics/Implementation/SimpleRigidBody.h
index 70709c83..039345eb 100644
--- a/Code/GamePhysics/Implementation/SimpleRigidBody.h
+++ b/Code/GamePhysics/Implementation/SimpleRigidBody.h
@@ -38,6 +38,7 @@ namespace Oyster
 			void SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 forward);
 
 			Math::Float4x4 GetRotation() const;
+			Math::Float4 GetRotationAsAngularAxis();
 			Math::Float4x4 GetOrientation() const;
 			Math::Float4x4 GetView() const;
 			Math::Float4x4 GetView( const ::Oyster::Math::Float3 &offset ) const;
diff --git a/Code/GamePhysics/PhysicsAPI.h b/Code/GamePhysics/PhysicsAPI.h
index ff2ba374..46f663d7 100644
--- a/Code/GamePhysics/PhysicsAPI.h
+++ b/Code/GamePhysics/PhysicsAPI.h
@@ -146,10 +146,11 @@ namespace Oyster
 			virtual void SetUpAndRight(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 right) = 0;
 			virtual void SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 forward) = 0;
 
-			::Oyster::Math::Float4x4 GetRotation() const;
-			::Oyster::Math::Float4x4 GetOrientation() const;
-			::Oyster::Math::Float4x4 GetView() const;
-			::Oyster::Math::Float4x4 GetView( const ::Oyster::Math::Float3 &offset ) const;
+			virtual ::Oyster::Math::Float4x4 GetRotation() const = 0;
+			virtual ::Oyster::Math::Float4 GetRotationAsAngularAxis() = 0;
+			virtual ::Oyster::Math::Float4x4 GetOrientation() const = 0;
+			virtual ::Oyster::Math::Float4x4 GetView() const = 0;
+			virtual ::Oyster::Math::Float4x4 GetView(const ::Oyster::Math::Float3 &offset) const = 0;
 
 			/********************************************************
 			 * @return the void pointer set by SetCustomTag.