From 26a87b3b436b2a560cb155669bc60ee2300a02da Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Tue, 26 Feb 2013 09:41:16 -0800
Subject: [PATCH] Added critically damped spring behavior to hand, and added
 simulateHand() routine to main.cpp that uses mouse to correctly move hand
 controller.

---
 interface/src/Hand.cpp | 19 ++++++++++---------
 interface/src/main.cpp | 33 +++++++++++++++++----------------
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/interface/src/Hand.cpp b/interface/src/Hand.cpp
index dc6f73e456..9ba98c8e12 100644
--- a/interface/src/Hand.cpp
+++ b/interface/src/Hand.cpp
@@ -59,7 +59,8 @@ void Hand::simulate(float deltaTime)
 {
     const float VNOISE = 0.01;
     const float RSPRING = 0.01;
-    const float PSPRING = 0.4;
+    const float LINEAR_SPRING_CONSTANT = 500;
+    const float LINEAR_DAMPING_COEFFICIENT = 2.0*powf(LINEAR_SPRING_CONSTANT,0.5);
     const float RNOISE = 0.1;
     const float VDECAY = 5.0;
 
@@ -79,15 +80,15 @@ void Hand::simulate(float deltaTime)
     yaw += yawRate;
     roll += rollRate;
     
-    //  Spring effect to return hand to target;
-    glm::vec3 sVel = target - position;
-    sVel *= PSPRING;
-    addVelocity(sVel);
-    //  Decay position of hand toward target
-    //position -= deltaTime*(position - target);
+    //  Use a spring to attempt to return the hand to the target position
+    glm::vec3 springForce = target - position;
+    springForce *= LINEAR_SPRING_CONSTANT;
+    addVelocity(springForce * deltaTime);
     
-    //  Decay velocity
-    velocity *= 1.0 - deltaTime*VDECAY;
+    //  Critically damp the spring
+    glm::vec3 dampingForce(velocity);
+    dampingForce *= LINEAR_DAMPING_COEFFICIENT;
+    addVelocity(-dampingForce * deltaTime);
     
     //  Decay Angular Velocity
     pitchRate *= 1.0 - deltaTime;
diff --git a/interface/src/main.cpp b/interface/src/main.cpp
index 3ae6e4e411..e6901a2829 100644
--- a/interface/src/main.cpp
+++ b/interface/src/main.cpp
@@ -391,7 +391,20 @@ void reset_sensors()
     }
 }
 
-void update_pos(float frametime)
+void simulateHand(float deltaTime) {
+    //  If mouse is being dragged, send current force to the hand controller
+    if (mouse_pressed == 1)
+    {
+        //  Add a velocity to the hand corresponding to the detected size of the drag vector
+        const float MOUSE_HAND_FORCE = 3.0;
+        float dx = mouse_x - mouse_start_x;
+        float dy = mouse_y - mouse_start_y;
+        glm::vec3 vel(dx*MOUSE_HAND_FORCE, -dy*MOUSE_HAND_FORCE*(WIDTH/HEIGHT), 0);
+        myHead.hand->addVelocity(vel*deltaTime);
+    }
+}
+
+void simulateHead(float frametime)
 //  Using serial data, update avatar/render position and angles
 {
 //    float measured_pitch_rate = serialPort.getRelativeValue(PITCH_RATE);
@@ -766,7 +779,9 @@ void idle(void)
     {
         steps_per_frame++;
         //  Simulation
-        update_pos(1.f/FPS);
+        simulateHead(1.f/FPS);
+        simulateHand(1.f/FPS);
+        
         if (simulate_on) {
             field.simulate(1.f/FPS);
             myHead.simulate(1.f/FPS);
@@ -830,22 +845,8 @@ void motionFunc( int x, int y)
 {
 	mouse_x = x;
 	mouse_y = y;
-    if (mouse_pressed == 1)
-    {
-        //  Send network packet containing mouse location
-        char mouse_string[20];
-        sprintf(mouse_string, "M %d %d\n", mouse_x, mouse_y);
-        //network_send(UDP_socket, mouse_string, strlen(mouse_string));
-        
-        //  Send dragged mouse vector to the hand;
-        float dx = mouse_x - mouse_start_x;
-        float dy = mouse_y - mouse_start_y;
-        glm::vec3 vel(dx*0.003, -dy*0.003, 0);
-        myHead.hand->addVelocity(vel);
-    }
     
     lattice.mouseClick((float)x/(float)WIDTH,(float)y/(float)HEIGHT);
-	
 }
 
 void mouseoverFunc( int x, int y)