From 73f8fca2afaa182d93d9a11e9510d6d8141e5891 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 3 Jun 2013 13:00:54 -0700
Subject: [PATCH] More work on voxeltars; added rotations.

---
 interface/src/Avatar.cpp            |  8 +++-
 interface/src/AvatarVoxelSystem.cpp |  9 ++--
 interface/src/Skeleton.cpp          | 65 +++++++++++++--------------
 interface/src/Skeleton.h            | 68 +++++++++++++++--------------
 interface/src/Util.cpp              | 21 +++++++++
 interface/src/Util.h                |  2 +
 6 files changed, 103 insertions(+), 70 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 2250bda751..fa193d3a70 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -909,6 +909,8 @@ void Avatar::updateBodyBalls(float deltaTime) {
     if (glm::length(_position - _bodyBall[AVATAR_JOINT_PELVIS].position) > BEYOND_BODY_SPRING_RANGE) {
         resetBodyBalls();
     }
+    glm::quat orientation = getOrientation();
+    glm::vec3 jointDirection = orientation * JOINT_DIRECTION;
     for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
         glm::vec3 springVector(_bodyBall[b].position);
         
@@ -955,7 +957,11 @@ void Avatar::updateBodyBalls(float deltaTime) {
         _bodyBall[b].position += _bodyBall[b].velocity * deltaTime;
         
         // update rotation
-        _bodyBall[b].rotation = _skeleton.joint[b].rotation;
+        if (_skeleton.joint[b].parent == AVATAR_JOINT_NULL || length < 0.1f) {
+            _bodyBall[b].rotation = orientation * _skeleton.joint[b].absoluteBindPoseRotation;
+        } else {
+            _bodyBall[b].rotation = rotationBetween(jointDirection, springVector) * orientation;
+        }
     }
 }
 
diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index d6b51bec82..9e441c148c 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -144,10 +144,11 @@ void AvatarVoxelSystem::applyScaleAndBindProgram(bool texture) {
         glm::vec3 position;
         glm::quat orientation;
         _avatar->getBodyBallTransform((AvatarJointID)i, position, orientation);
-        boneMatrices[i].translate(position.x, position.y, position.z);
+        boneMatrices[i].translate(position.x, position.y, position.z);  
+        orientation = orientation * glm::inverse(_avatar->getSkeleton().joint[i].absoluteBindPoseRotation);
         boneMatrices[i].rotate(QQuaternion(orientation.w, orientation.x, orientation.y, orientation.z));
-        const glm::vec3& defaultPosition = _avatar->getSkeleton().joint[i].absoluteDefaultPosePosition;
-        boneMatrices[i].translate(-defaultPosition.x, -defaultPosition.y, -defaultPosition.z);
+        const glm::vec3& bindPosition = _avatar->getSkeleton().joint[i].absoluteBindPosePosition;
+        boneMatrices[i].translate(-bindPosition.x, -bindPosition.y, -bindPosition.z);
         boneMatrices[i] *= baseMatrix;
     } 
     _skinProgram->setUniformValueArray(_boneMatricesLocation, boneMatrices, NUM_AVATAR_JOINTS);
@@ -182,7 +183,7 @@ void AvatarVoxelSystem::computeBoneIndicesAndWeights(const glm::vec3& vertex, Bo
     // find the nearest four joints (TODO: use a better data structure for the pose positions to speed this up)
     IndexDistance nearest[BONE_ELEMENTS_PER_VERTEX];
     for (int i = 0; i < NUM_AVATAR_JOINTS; i++) {
-        float distance = glm::distance(jointVertex, _avatar->getSkeleton().joint[i].absoluteDefaultPosePosition);
+        float distance = glm::distance(jointVertex, _avatar->getSkeleton().joint[i].absoluteBindPosePosition);
         for (int j = 0; j < BONE_ELEMENTS_PER_VERTEX; j++) {
             if (distance < nearest[j].distance) {
                 // move the rest of the indices down
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index ae31810d8e..136cb6f619 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -5,6 +5,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
 #include "Skeleton.h"
+#include "Util.h"
 
 const float BODY_SPRING_DEFAULT_TIGHTNESS = 1000.0f;
 const float FLOATING_HEIGHT               = 0.13f;
@@ -17,7 +18,7 @@ void Skeleton::initialize() {
     for (int b=0; b<NUM_AVATAR_JOINTS; b++) {
         joint[b].parent              = AVATAR_JOINT_NULL;
         joint[b].position            = glm::vec3(0.0, 0.0, 0.0);
-        joint[b].defaultPosePosition = glm::vec3(0.0, 0.0, 0.0);
+        joint[b].bindPosePosition = glm::vec3(0.0, 0.0, 0.0);
         joint[b].rotation            = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
         joint[b].length              = 0.0;
     }
@@ -49,46 +50,46 @@ void Skeleton::initialize() {
     joint[ AVATAR_JOINT_RIGHT_TOES		  ].parent = AVATAR_JOINT_RIGHT_HEEL;
     
     // specify the default pose position
-    joint[ AVATAR_JOINT_PELVIS           ].defaultPosePosition = glm::vec3(  0.0,   0.0,    0.0  );
-    joint[ AVATAR_JOINT_TORSO            ].defaultPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
-    joint[ AVATAR_JOINT_CHEST            ].defaultPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
-    joint[ AVATAR_JOINT_NECK_BASE        ].defaultPosePosition = glm::vec3(  0.0,   0.14,   0.01 );
-    joint[ AVATAR_JOINT_HEAD_BASE        ].defaultPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
+    joint[ AVATAR_JOINT_PELVIS           ].bindPosePosition = glm::vec3(  0.0,   0.0,    0.0  );
+    joint[ AVATAR_JOINT_TORSO            ].bindPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
+    joint[ AVATAR_JOINT_CHEST            ].bindPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
+    joint[ AVATAR_JOINT_NECK_BASE        ].bindPosePosition = glm::vec3(  0.0,   0.14,   0.01 );
+    joint[ AVATAR_JOINT_HEAD_BASE        ].bindPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
     
-    joint[ AVATAR_JOINT_LEFT_COLLAR      ].defaultPosePosition = glm::vec3( -0.06,  0.04,   0.01 );
-    joint[ AVATAR_JOINT_LEFT_SHOULDER    ].defaultPosePosition = glm::vec3( -0.05,  0.0,    0.01 );
-    joint[ AVATAR_JOINT_LEFT_ELBOW       ].defaultPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
-    joint[ AVATAR_JOINT_LEFT_WRIST       ].defaultPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
-    joint[ AVATAR_JOINT_LEFT_FINGERTIPS  ].defaultPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
+    joint[ AVATAR_JOINT_LEFT_COLLAR      ].bindPosePosition = glm::vec3( -0.06,  0.04,   0.01 );
+    joint[ AVATAR_JOINT_LEFT_SHOULDER    ].bindPosePosition = glm::vec3( -0.05,  0.0,    0.01 );
+    joint[ AVATAR_JOINT_LEFT_ELBOW       ].bindPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
+    joint[ AVATAR_JOINT_LEFT_WRIST       ].bindPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
+    joint[ AVATAR_JOINT_LEFT_FINGERTIPS  ].bindPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
     
-    joint[ AVATAR_JOINT_RIGHT_COLLAR     ].defaultPosePosition = glm::vec3(  0.06,  0.04,   0.01 );
-    joint[ AVATAR_JOINT_RIGHT_SHOULDER   ].defaultPosePosition = glm::vec3(  0.05,  0.0,    0.01 );
-    joint[ AVATAR_JOINT_RIGHT_ELBOW      ].defaultPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
-    joint[ AVATAR_JOINT_RIGHT_WRIST      ].defaultPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
-    joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].defaultPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
+    joint[ AVATAR_JOINT_RIGHT_COLLAR     ].bindPosePosition = glm::vec3(  0.06,  0.04,   0.01 );
+    joint[ AVATAR_JOINT_RIGHT_SHOULDER   ].bindPosePosition = glm::vec3(  0.05,  0.0,    0.01 );
+    joint[ AVATAR_JOINT_RIGHT_ELBOW      ].bindPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
+    joint[ AVATAR_JOINT_RIGHT_WRIST      ].bindPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
+    joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].bindPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
     
-    joint[ AVATAR_JOINT_LEFT_HIP         ].defaultPosePosition = glm::vec3( -0.05,  0.0,    0.02 );
-    joint[ AVATAR_JOINT_LEFT_KNEE        ].defaultPosePosition = glm::vec3(  0.01, -0.25,  -0.03 );
-    joint[ AVATAR_JOINT_LEFT_HEEL        ].defaultPosePosition = glm::vec3(  0.01, -0.22,   0.08 );
-    joint[ AVATAR_JOINT_LEFT_TOES        ].defaultPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
+    joint[ AVATAR_JOINT_LEFT_HIP         ].bindPosePosition = glm::vec3( -0.05,  0.0,    0.02 );
+    joint[ AVATAR_JOINT_LEFT_KNEE        ].bindPosePosition = glm::vec3(  0.01, -0.25,  -0.03 );
+    joint[ AVATAR_JOINT_LEFT_HEEL        ].bindPosePosition = glm::vec3(  0.01, -0.22,   0.08 );
+    joint[ AVATAR_JOINT_LEFT_TOES        ].bindPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
     
-    joint[ AVATAR_JOINT_RIGHT_HIP        ].defaultPosePosition = glm::vec3(  0.05,  0.0,    0.02 );
-    joint[ AVATAR_JOINT_RIGHT_KNEE       ].defaultPosePosition = glm::vec3( -0.01, -0.25,  -0.03 );
-    joint[ AVATAR_JOINT_RIGHT_HEEL       ].defaultPosePosition = glm::vec3( -0.01, -0.22,   0.08 );
-    joint[ AVATAR_JOINT_RIGHT_TOES       ].defaultPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
+    joint[ AVATAR_JOINT_RIGHT_HIP        ].bindPosePosition = glm::vec3(  0.05,  0.0,    0.02 );
+    joint[ AVATAR_JOINT_RIGHT_KNEE       ].bindPosePosition = glm::vec3( -0.01, -0.25,  -0.03 );
+    joint[ AVATAR_JOINT_RIGHT_HEEL       ].bindPosePosition = glm::vec3( -0.01, -0.22,   0.08 );
+    joint[ AVATAR_JOINT_RIGHT_TOES       ].bindPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
              
-    // calculate bone length, absolute positions
+    // calculate bone length, absolute positions/rotations
     for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
-        joint[b].length = glm::length(joint[b].defaultPosePosition);
+        joint[b].length = glm::length(joint[b].bindPosePosition);
         
         if (joint[b].parent == AVATAR_JOINT_NULL) {
-            joint[b].absoluteDefaultPosePosition = glm::vec3(0.0f, 0.0f, 0.0f);
+            joint[b].absoluteBindPosePosition = joint[b].bindPosePosition;
+            joint[b].absoluteBindPoseRotation = glm::quat();
         } else {
-            joint[b].absoluteDefaultPosePosition = joint[ joint[b].parent ].absoluteDefaultPosePosition;
+            joint[b].absoluteBindPosePosition = joint[ joint[b].parent ].absoluteBindPosePosition +
+                joint[b].bindPosePosition;
+            joint[b].absoluteBindPoseRotation = rotationBetween(JOINT_DIRECTION, joint[b].bindPosePosition);
         }
-        
-        glm::vec3 rotatedJointVector = joint[b].defaultPosePosition;
-        joint[b].absoluteDefaultPosePosition += rotatedJointVector;
     }
 }
 
@@ -105,7 +106,7 @@ void Skeleton::update(float deltaTime, const glm::quat& orientation, glm::vec3 p
             joint[b].position = joint[ joint[b].parent ].position;
         }
 
-        glm::vec3 rotatedJointVector = joint[b].rotation * joint[b].defaultPosePosition;
+        glm::vec3 rotatedJointVector = joint[b].rotation * joint[b].bindPosePosition;
         joint[b].position += rotatedJointVector;
     }    
 }
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index 5602dfc136..78405b39ac 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -13,35 +13,36 @@
 
 enum AvatarJointID
 {
-	AVATAR_JOINT_NULL = -1,
-	AVATAR_JOINT_PELVIS,	
-	AVATAR_JOINT_TORSO,	
-	AVATAR_JOINT_CHEST,	
-	AVATAR_JOINT_NECK_BASE,	
-	AVATAR_JOINT_HEAD_BASE,	
-	AVATAR_JOINT_HEAD_TOP,	
-	AVATAR_JOINT_LEFT_COLLAR,
-	AVATAR_JOINT_LEFT_SHOULDER,
-	AVATAR_JOINT_LEFT_ELBOW,
-	AVATAR_JOINT_LEFT_WRIST,
-	AVATAR_JOINT_LEFT_FINGERTIPS,
-	AVATAR_JOINT_RIGHT_COLLAR,
-	AVATAR_JOINT_RIGHT_SHOULDER,
-	AVATAR_JOINT_RIGHT_ELBOW,
-	AVATAR_JOINT_RIGHT_WRIST,
-	AVATAR_JOINT_RIGHT_FINGERTIPS,
-	AVATAR_JOINT_LEFT_HIP,
-	AVATAR_JOINT_LEFT_KNEE,
-	AVATAR_JOINT_LEFT_HEEL,		
-	AVATAR_JOINT_LEFT_TOES,		
-	AVATAR_JOINT_RIGHT_HIP,	
-	AVATAR_JOINT_RIGHT_KNEE,	
-	AVATAR_JOINT_RIGHT_HEEL,	
-	AVATAR_JOINT_RIGHT_TOES,	
+    AVATAR_JOINT_NULL = -1,
+    AVATAR_JOINT_PELVIS,    
+    AVATAR_JOINT_TORSO,    
+    AVATAR_JOINT_CHEST,    
+    AVATAR_JOINT_NECK_BASE,    
+    AVATAR_JOINT_HEAD_BASE,    
+    AVATAR_JOINT_HEAD_TOP,    
+    AVATAR_JOINT_LEFT_COLLAR,
+    AVATAR_JOINT_LEFT_SHOULDER,
+    AVATAR_JOINT_LEFT_ELBOW,
+    AVATAR_JOINT_LEFT_WRIST,
+    AVATAR_JOINT_LEFT_FINGERTIPS,
+    AVATAR_JOINT_RIGHT_COLLAR,
+    AVATAR_JOINT_RIGHT_SHOULDER,
+    AVATAR_JOINT_RIGHT_ELBOW,
+    AVATAR_JOINT_RIGHT_WRIST,
+    AVATAR_JOINT_RIGHT_FINGERTIPS,
+    AVATAR_JOINT_LEFT_HIP,
+    AVATAR_JOINT_LEFT_KNEE,
+    AVATAR_JOINT_LEFT_HEEL,        
+    AVATAR_JOINT_LEFT_TOES,        
+    AVATAR_JOINT_RIGHT_HIP,    
+    AVATAR_JOINT_RIGHT_KNEE,    
+    AVATAR_JOINT_RIGHT_HEEL,    
+    AVATAR_JOINT_RIGHT_TOES,    
     
-	NUM_AVATAR_JOINTS
+    NUM_AVATAR_JOINTS
 };
 
+const glm::vec3 JOINT_DIRECTION = glm::vec3(1.0f, 0.0f, 0.0f);
 
 class Skeleton {
 public:
@@ -58,15 +59,16 @@ public:
     
     struct AvatarJoint
     {
-        AvatarJointID parent;                       // which joint is this joint connected to?
-        glm::vec3	  position;				        // the position at the "end" of the joint - in global space
-        glm::vec3	  defaultPosePosition;	        // the parent relative position when the avatar is in the "T-pose"
-        glm::vec3     absoluteDefaultPosePosition;  // the absolute position when the avatar is in the "T-pose"
-        glm::quat     rotation;                     // the parent-relative rotation (orientation) of the joint as a quaternion
-        float		  length;				        // the length of vector connecting the joint and its parent
+        AvatarJointID parent;                    // which joint is this joint connected to?
+        glm::vec3     position;                  // the position at the "end" of the joint - in global space
+        glm::vec3     bindPosePosition;          // the parent relative position when the avatar is in the "T-pose"
+        glm::vec3     absoluteBindPosePosition;  // the absolute position when the avatar is in the "T-pose"
+        glm::quat     absoluteBindPoseRotation;  // the absolute rotation when the avatar is in the "T-pose"
+        glm::quat     rotation;                  // the parent-relative rotation (orientation) of the joint as a quaternion
+        float         length;                    // the length of vector connecting the joint and its parent
     };
 
-    AvatarJoint	joint[ NUM_AVATAR_JOINTS ];        
+    AvatarJoint    joint[ NUM_AVATAR_JOINTS ];        
  };
 
 #endif
diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp
index 05dfc50502..08ec6cf011 100644
--- a/interface/src/Util.cpp
+++ b/interface/src/Util.cpp
@@ -72,6 +72,27 @@ float angleBetween(const glm::vec3& v1, const glm::vec3& v2) {
     return acos((glm::dot(v1, v2)) / (glm::length(v1) * glm::length(v2))) * 180.f / PI;
 }
 
+//  Helper function return the rotation from the first vector onto the second
+glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2) {
+    float angle = angleBetween(v1, v2);
+    if (isnan(angle) || angle < EPSILON) {
+        return glm::quat();
+    }
+    glm::vec3 axis = glm::cross(v1, v2);
+    if (angle > 179.99f) { // 180 degree rotation; must use another axis
+        axis = glm::cross(v1, glm::vec3(1.0f, 0.0f, 0.0f));
+        float axisLength = glm::length(axis);
+        if (axisLength < EPSILON) { // parallel to x; y will work
+            axis = glm::normalize(glm::cross(v1, glm::vec3(0.0f, 1.0f, 0.0f)));
+        } else {
+            axis /= axisLength;
+        }        
+    } else {
+        axis = glm::normalize(glm::cross(v1, v2));
+    }
+    return glm::angleAxis(angle, axis);
+}
+
 //  Safe version of glm::eulerAngles; uses the factorization method described in David Eberly's
 //  http://www.geometrictools.com/Documentation/EulerAngles.pdf (via Clyde,
 // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java)
diff --git a/interface/src/Util.h b/interface/src/Util.h
index 0823ac405b..0187d93da5 100644
--- a/interface/src/Util.h
+++ b/interface/src/Util.h
@@ -45,6 +45,8 @@ void drawVector(glm::vec3* vector);
 
 float angleBetween(const glm::vec3& v1, const glm::vec3& v2); 
 
+glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2);
+
 glm::vec3 safeEulerAngles(const glm::quat& q);
 
 glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha);