From 30958e3ce61ee8127bbb22e2e9507b3752910156 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 22 May 2013 12:49:25 -0700
Subject: [PATCH] Working on reorientation.

---
 interface/src/Avatar.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index cdb9040d20..9e72e1ca90 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -6,6 +6,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
 #include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
 #include <vector>
 #include <lodepng.h>
 #include <SharedUtil.h>
@@ -324,7 +325,22 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     _bodyRoll  *= tiltDecay;
     
     //the following will be used to make the avatar upright no matter what gravity is
-    //float f = angleBetween(_orientation.getUp(), _gravity);
+    float gravityLength = glm::length(_gravity);
+    if (gravityLength > 0.0f) {
+        glm::vec3 targetUp = _gravity / -gravityLength;
+        const glm::vec3& currentUp = _orientation.getUp();
+        float angle = glm::degrees(acosf(glm::dot(currentUp, targetUp)));
+        if (angle > 0.0f) {
+            glm::vec3 axis;
+            if (angle > 180.0f - EPSILON) { // 180 degree rotation; must use another axis
+                axis = _orientation.getRight();
+            } else {
+                axis = glm::normalize(glm::cross(currentUp, targetUp));
+            }
+            _orientation.rotate(glm::angleAxis(angle, axis));
+            
+        }
+    }
     
     // update position by velocity
     _position += _velocity * deltaTime;