diff --git a/interface/src/Hand.cpp b/interface/src/Hand.cpp index 89209d309a..9ba98c8e12 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,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; } \ 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; }; diff --git a/interface/src/Head.h b/interface/src/Head.h index 94714d97b0..b554d4541e 100644 --- a/interface/src/Head.h +++ b/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 \ No newline at end of file diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 6d19004bfe..e6901a2829 100644 --- a/interface/src/main.cpp +++ b/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)