From a22cd3680a8b1b9146215f5eb069fd573f8c1d1f Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Mon, 25 Feb 2013 14:31:56 -0800
Subject: [PATCH] Added angular velocity and angular movement to hand, changed
 hand to block.

---
 interface/src/Hand.cpp | 57 ++++++++++++++++++++++++++++++++++++------
 interface/src/Hand.h   |  4 ++-
 2 files changed, 52 insertions(+), 9 deletions(-)

diff --git a/interface/src/Hand.cpp b/interface/src/Hand.cpp
index 89209d309a..16698a74d1 100644
--- a/interface/src/Hand.cpp
+++ b/interface/src/Hand.cpp
@@ -24,14 +24,24 @@ Hand::Hand(glm::vec3 initcolor)
     scale.z = scale.y * 1.0;
 }
 
+void Hand::addAngularVelocity (float pRate, float yRate, float rRate) {
+    pitchRate += pRate;
+    yawRate += yRate;
+    rollRate += rRate;
+}
+
 void Hand::render()
 {
-//    glPushMatrix();
-//    glTranslatef(position.x, position.y, position.z);
-//    glColor3f(color.x, color.y, color.z);
-//    glScalef(scale.x, scale.y, scale.z);
-//    glutSolidSphere(1.5, 20, 20);
-//    glPopMatrix();
+    glPushMatrix();
+    glTranslatef(position.x, position.y, position.z);
+    glRotatef(yaw, 0, 1, 0);
+    glRotatef(pitch, 1, 0, 0);
+    glRotatef(roll, 0, 0, 1);
+    glColor3f(color.x, color.y, color.z);
+    glScalef(scale.x, scale.y, scale.z);
+    //glutSolidSphere(1.5, 20, 20);
+    glutSolidCube(1.0);
+    glPopMatrix();
 }
 
 void Hand::reset()
@@ -39,17 +49,48 @@ void Hand::reset()
     position.x = DEFAULT_X;
     position.y = DEFAULT_Y;
     position.z = DEFAULT_Z;
+    pitch = yaw = roll = 0;
+    pitchRate = yawRate = rollRate = 0;
     setTarget(position);
     velocity.x = velocity.y = velocity.z = 0;
 }
 
 void Hand::simulate(float deltaTime)
 {
-    //  If noise, add wandering movement
+    const float VNOISE = 0.1;
+    const float RSPRING = 0.01;
+    const float RNOISE = 0.1;
+
+    //  If noise, add a bit of random velocity
     if (noise) {
-        position += noise * 0.1f * glm::vec3(randFloat() - 0.5, randFloat() - 0.5, randFloat() - 0.5);
+        glm::vec3 nVel(randFloat() - 0.5f, randFloat() - 0.5f, randFloat() - 0.5f);
+        nVel *= VNOISE;
+        addVelocity(nVel);
+        
+        addAngularVelocity(RNOISE*(randFloat() - 0.5f),
+                           RNOISE*(randFloat() - 0.5f),
+                           RNOISE*(randFloat() - 0.5f));        
     }
+    position += velocity*deltaTime;
+    
+    pitch += pitchRate;
+    yaw += yawRate;
+    roll += rollRate;
+    
     //  Decay position of hand toward target
     position -= deltaTime*(position - target);
     
+    //  Decay velocity
+    velocity *= 1.0 - deltaTime;
+    
+    //  Decay Angular Velocity
+    pitchRate *= 1.0 - deltaTime;
+    yawRate *= 1.0 - deltaTime;
+    rollRate *= 1.0 - deltaTime;
+    
+    //  Add spring effect to return hand rotation to zero
+    pitchRate -= pitch * RSPRING;
+    yawRate -= yaw * RSPRING;
+    rollRate -= roll * RSPRING;
+    
 }
\ No newline at end of file
diff --git a/interface/src/Hand.h b/interface/src/Hand.h
index 9f64343673..01478839ee 100644
--- a/interface/src/Hand.h
+++ b/interface/src/Hand.h
@@ -23,12 +23,14 @@ public:
     void render ();
     void reset ();
     void setNoise (float mag) { noise = mag; };
-    void addVel (glm::vec3 v) { velocity += v; };
+    void addVelocity (glm::vec3 v) { velocity += v; };
+    void addAngularVelocity (float pRate, float yRate, float rRate);
     glm::vec3 getPos() { return position; };
     void setPos(glm::vec3 p) { position = p; };
     void setTarget(glm::vec3 t) { target = t; };
 private:
     glm::vec3 position, target, velocity, color, scale;
+    float pitch, yaw, roll, pitchRate, yawRate, rollRate;
     float noise;
     
 };