Fixed player going bonkers on lower hemisphere

Code/Game/GameLogic/Player.cpp

@@ -80,12 +80,8 @@ void Player::BeginFrame()
 	Oyster::Math::Float maxSpeed = 30;
 
 	// Rotate player accordingly
+	this->rigidBody->AddRotationAroundY(this->rotationUp);
 	this->rigidBody->SetUp(this->rigidBody->GetState().centerPos.GetNormalized());
-	Oyster::Math::Quaternion firstUp = this->rigidBody->GetState().quaternion;
-	this->rigidBody->SetRotationAsAngularAxis(Oyster::Math3D::Float4(this->rigidBody->GetState().centerPos.GetNormalized(), this->rotationUp));
-	Oyster::Math::Quaternion secondTurn = this->rigidBody->GetState().quaternion;
-
-	this->rigidBody->SetRotation(secondTurn*firstUp);
 
 	// Direction data
 	Oyster::Math::Float4x4 xform;
@@ -272,7 +268,7 @@ void Player::SetLookDir(const Oyster::Math3D::Float3& lookDir)
 }
 void Player::TurnLeft(Oyster::Math3D::Float deltaRadians)
 {
-	this->rotationUp += deltaRadians;
+	this->rotationUp = deltaRadians;
 }
 
 void Player::Jump()

Code/Physics/GamePhysics/Implementation/SimpleRigidBody.cpp

@@ -163,25 +163,15 @@ void SimpleRigidBody::SetRotation(::Oyster::Math::Float4x4 rotation)
 	this->state.quaternion = Quaternion(Float3(quaternion.x(), quaternion.y(), quaternion.z()), quaternion.w());
 }
 
-void SimpleRigidBody::SetRotationAsAngularAxis(::Oyster::Math::Float4 angularAxis)
+void SimpleRigidBody::AddRotationAroundY(::Oyster::Math::Float angle)
 {
-	if(angularAxis.xyz.GetMagnitude() == 0)
-	{
-		return;
-	}
-
-	float s = sin(angularAxis.w/2);
-	float x = angularAxis.x * s;
-	float y = angularAxis.y * s;
-	float z = angularAxis.z * s;
-	float w = cos(angularAxis.w/2);
-
 	btTransform trans;
-	btVector3 vector(angularAxis.x, angularAxis.y, angularAxis.z);
+	btQuaternion quaternion;
-	btQuaternion quaternion(x,y,z,w);
 
 	trans = this->rigidBody->getWorldTransform();
-	trans.setRotation(quaternion);
+
+	quaternion = btQuaternion(trans.getBasis().getColumn(1), angle);
+	trans.setRotation(quaternion*trans.getRotation());
 	this->rigidBody->setWorldTransform(trans);
 
 	this->state.quaternion = Quaternion(Float3(quaternion.x(), quaternion.y(), quaternion.z()), quaternion.w());
@@ -245,23 +235,45 @@ void SimpleRigidBody::SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math:
 
 void SimpleRigidBody::SetUp(::Oyster::Math::Float3 up)
 {
-	Float3 vector = Float3(0, 1, 0).Cross(up);
+	btQuaternion newRotation;
-
-	if(vector == Float3::null)
-	{
-		return;
-	}
-
-	Float sine = vector.GetLength();
-	Float cosine = acos(Float3(0, 1, 0).Dot(up));
-
-	btQuaternion quaternion(btVector3(vector.x, vector.y, vector.z),cosine);
-
 	btTransform trans;
 	trans = this->rigidBody->getWorldTransform();
-	trans.setRotation(quaternion);
+
+	btVector3 v1 = trans.getBasis().getColumn(1);
+	btVector3 v2(up.x, up.y, up.z);
+
+	btQuaternion q;
+	btVector3 a = v1.cross(v2);
+	
+	if (v1.dot(v2) < -0.999999) 
+	{
+		btVector3 xCrossPre = btVector3(1, 0 ,0).cross(v1);
+		if(xCrossPre.length() < 0.000001)
+			xCrossPre = btVector3(0, 1 ,0).cross(v1);
+		xCrossPre.normalize();
+		q.setRotation(xCrossPre, 3.1415);
+    }
+	else if (v1.dot(v2) > 0.999999) 
+	{
+           q = btQuaternion(0, 0, 0, 1);
+	}
+	else
+	{
+		q.setX(a.x());
+		q.setY(a.y());
+		q.setZ(a.z());
+
+		q.setW(1 + v1.dot(v2));
+
+		q.normalize();
+	}
+
+	newRotation = q*trans.getRotation();
+
+	trans.setRotation(newRotation);
 	this->rigidBody->setWorldTransform(trans);
-	this->state.quaternion = Quaternion(Float3(quaternion.x(), quaternion.y(), quaternion.z()), quaternion.w());
+
+	this->state.quaternion = Quaternion(Float3(newRotation.x(), newRotation.y(), newRotation.z()), newRotation.w());
 }
 
 Float4x4 SimpleRigidBody::GetRotation() const

Code/Physics/GamePhysics/Implementation/SimpleRigidBody.h

@@ -29,7 +29,7 @@ namespace Oyster
 			void SetRotation(Math::Quaternion quaternion);
 			void SetRotation(Math::Float3 eulerAngles);
 			void SetRotation(::Oyster::Math::Float4x4 rotation);
-			void SetRotationAsAngularAxis(Math::Float4 angularAxis);
+			void AddRotationAroundY(Math::Float angle);
 			void SetAngularFactor(Math::Float factor);
 			void SetMass(Math::Float mass);
 

Code/Physics/GamePhysics/PhysicsAPI.h

@@ -147,7 +147,7 @@ namespace Oyster
 			virtual void SetRotation(::Oyster::Math::Quaternion quaternion) = 0;
 			virtual void SetRotation(::Oyster::Math::Float3 eulerAngles) = 0;
 			virtual void SetRotation(::Oyster::Math::Float4x4 rotation) = 0;
-			virtual void SetRotationAsAngularAxis(::Oyster::Math::Float4 angularAxis) = 0;
+			virtual void AddRotationAroundY(::Oyster::Math::Float angle) = 0;
 			virtual void SetAngularFactor(::Oyster::Math::Float factor) = 0;
 			virtual void SetMass(::Oyster::Math::Float mass) = 0;