From 648aacb53d8f6ce0075b44c64b45aa2a5bedbdcf Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 17:26:41 -0700
Subject: [PATCH] Let's use a fixed vector for the camera position so that the
 target doesn't move with lean.

---
 interface/src/Application.cpp     | 6 +++---
 interface/src/Avatar.cpp          | 6 ++++++
 interface/src/Avatar.h            | 3 +++
 interface/src/SerialInterface.cpp | 4 ++--
 interface/src/Skeleton.cpp        | 7 +++++++
 interface/src/Skeleton.h          | 1 +
 6 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 51c6e65cb9..489150c8a1 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -308,7 +308,7 @@ void Application::paintGL() {
     
     if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
         _myCamera.setTightness     (100.0f); 
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getWorldAlignedOrientation() * glm::quat(glm::vec3(0.0f, PIf, 0.0f)));
         
     } else if (OculusManager::isConnected()) {
@@ -320,11 +320,11 @@ void Application::paintGL() {
     
     } else if (_myCamera.getMode() == CAMERA_MODE_FIRST_PERSON) {
         _myCamera.setTightness(0.0f);  //  In first person, camera follows head exactly without delay
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
         
     } else if (_myCamera.getMode() == CAMERA_MODE_THIRD_PERSON) {
-        _myCamera.setTargetPosition(_myAvatar.getHeadJointPosition());
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
     }
     
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 5547fe5e43..7d6e0aad37 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -103,9 +103,11 @@ Avatar::Avatar(Agent* owningAgent) :
     initializeBodyBalls();
     
     _height               = _skeleton.getHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius + _bodyBall[ BODY_BALL_HEAD_BASE ].radius;
+    
     _maxArmLength         = _skeleton.getArmLength();
     _pelvisStandingHeight = _skeleton.getPelvisStandingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
     _pelvisFloatingHeight = _skeleton.getPelvisFloatingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
+    _pelvisToHeadLength   = _skeleton.getPelvisToHeadLength();
     
     _avatarTouch.setReachableRadius(PERIPERSONAL_RADIUS);
     
@@ -310,6 +312,10 @@ glm::quat Avatar::getWorldAlignedOrientation () const {
     return computeRotationFromBodyToWorldUp() * getOrientation();
 }
 
+glm::vec3 Avatar::getUprightHeadPosition() const {
+    return _position + getWorldAlignedOrientation() * glm::vec3(0.0f, _pelvisToHeadLength, 0.0f);
+}
+
 void  Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight) {
     //  Update head yaw and pitch based on mouse input
     const float MOUSE_MOVE_RADIUS = 0.3f;
diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h
index 0fc5b31286..82d98a3a02 100644
--- a/interface/src/Avatar.h
+++ b/interface/src/Avatar.h
@@ -121,6 +121,8 @@ public:
     glm::quat        getOrientation            () const;
     glm::quat        getWorldAlignedOrientation() const;
     
+    glm::vec3 getUprightHeadPosition() const;
+    
     AvatarVoxelSystem* getVoxels() { return &_voxels; }
     
     //  Set what driving keys are being pressed to control thrust levels
@@ -185,6 +187,7 @@ private:
     int         _driveKeys[MAX_DRIVE_KEYS];
     float       _pelvisStandingHeight;
     float       _pelvisFloatingHeight;
+    float       _pelvisToHeadLength;
     float       _height;
     Balls*      _balls;
     AvatarTouch _avatarTouch;
diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 86755e28b5..76487bb668 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -296,8 +296,8 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
         
         //  Update estimated position and velocity
-        float const DECAY_VELOCITY = 0.95f;
-        float const DECAY_POSITION = 0.95f;
+        float const DECAY_VELOCITY = 0.975f;
+        float const DECAY_POSITION = 0.975f;
         _estimatedVelocity += deltaTime * _estimatedAcceleration;
         _estimatedPosition += deltaTime * _estimatedVelocity;
         _estimatedVelocity *= DECAY_VELOCITY;
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index fcec90b542..1167194534 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -174,6 +174,13 @@ float Skeleton::getPelvisFloatingHeight() {
            FLOATING_HEIGHT;
 }
 
+float Skeleton::getPelvisToHeadLength() {
+    return 
+        joint[ AVATAR_JOINT_TORSO     ].length +
+        joint[ AVATAR_JOINT_CHEST     ].length +
+        joint[ AVATAR_JOINT_NECK_BASE ].length +
+        joint[ AVATAR_JOINT_HEAD_BASE ].length; 
+}
 
 
 
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index ec40f18cb3..bb953fe947 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -56,6 +56,7 @@ public:
     float getHeight();
     float getPelvisStandingHeight();
     float getPelvisFloatingHeight();
+    float getPelvisToHeadLength();
     //glm::vec3 getJointVectorFromParent(AvatarJointID jointID) {return joint[jointID].position - joint[joint[jointID].parent].position; }
     
     struct AvatarJoint