Merge branch 'master' of github.com:worklist/hifi

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,55 @@ 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.01;
+    const float RSPRING = 0.01;
+    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;
+
+    //  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));        
     }
-    //  Decay position of hand toward target
-    position -= deltaTime*(position - target);
+    position += velocity*deltaTime;
+    
+    pitch += pitchRate;
+    yaw += yawRate;
+    roll += rollRate;
+    
+    //  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);
+    
+    //  Critically damp the spring
+    glm::vec3 dampingForce(velocity);
+    dampingForce *= LINEAR_DAMPING_COEFFICIENT;
+    addVelocity(-dampingForce * 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;
     
 }

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;
     
 };

interface/src/Head.h

@@ -62,6 +62,9 @@ class Head : public AgentData {
         void SetNewHeadTarget(float, float);
         glm::vec3 getPos() { return position; };
         void setPos(glm::vec3 newpos) { position = newpos; };
+    
+        Hand * hand;
+    
     private:
         float noise;
         float Pitch;
@@ -101,7 +104,6 @@ class Head : public AgentData {
         void readSensors();
         float renderYaw, renderPitch;       //   Pitch from view frustum when this is own head.
     
-        Hand * hand;
 };
 
 #endif

interface/src/main.cpp

@@ -144,14 +144,15 @@ int display_head_mouse = 1;         //  Display sample mouse pointer controlled
 int head_mouse_x, head_mouse_y; 
 int head_lean_x, head_lean_y;
 
-int mouse_x, mouse_y;				//  Where is the mouse 
+int mouse_x, mouse_y;				//  Where is the mouse
+int mouse_start_x, mouse_start_y;   //  Mouse location at start of last down click
 int mouse_pressed = 0;				//  true if mouse has been pressed (clear when finished)
 
 int nearbyAgents = 0;               //  How many other people near you is the domain server reporting?
 
 int speed;
 
-//  
+//
 //  Serial USB Variables
 // 
 
@@ -390,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);
@@ -765,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);
@@ -813,6 +829,8 @@ void mouseFunc( int button, int state, int x, int y )
 		mouse_y = y;
 		mouse_pressed = 1;
         lattice.mouseClick((float)x/(float)WIDTH,(float)y/(float)HEIGHT);
+        mouse_start_x = x;
+        mouse_start_y = y;
     }
 	if( button == GLUT_LEFT_BUTTON && state == GLUT_UP )
     {
@@ -827,16 +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));
-    }
     
     lattice.mouseClick((float)x/(float)WIDTH,(float)y/(float)HEIGHT);
-	
 }
 
 void mouseoverFunc( int x, int y)