Added head lean to broadcast data packets, lean now being set from invensense accelerometers.

interface/src/Avatar.cpp

@@ -266,7 +266,7 @@ void Avatar::reset() {
 
 
 //  Update avatar head rotation with sensor data
-void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
+void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface, glm::vec3* gravity) {
     float measuredPitchRate = 0.0f;
     float measuredRollRate = 0.0f;
     float measuredYawRate = 0.0f;
@@ -283,13 +283,34 @@ void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterfa
     const float MAX_ROLL = 50;
     const float MIN_ROLL = -50;
     
-    addHeadPitch(measuredPitchRate * frametime);
+    addHeadPitch(measuredPitchRate * deltaTime);
-    addHeadYaw(measuredYawRate * frametime);
+    addHeadYaw(measuredYawRate * deltaTime);
-    addHeadRoll(measuredRollRate * frametime);
+    addHeadRoll(measuredRollRate * deltaTime);
     
     setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
     setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
     setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
+    
+    //  Update head lean distance based on accelerometer data
+    const float LEAN_SENSITIVITY = 0.15;
+    const float LEAN_MAX = 0.45;
+    const float LEAN_AVERAGING = 10.0;
+    glm::vec3 headRotationRates(getHeadPitch(), getHeadYaw(), getHeadRoll());
+    float headRateMax = 50.f;
+    
+    
+    glm::vec3 leaning = (serialInterface->getLastAcceleration() -  serialInterface->getGravity())
+                        * LEAN_SENSITIVITY
+                        * (1.f - fminf(glm::length(headRotationRates), headRateMax) / headRateMax);
+    leaning.y = 0.f;
+    if (glm::length(leaning) < LEAN_MAX) {
+        _head.leanForward = _head.leanForward * (1.f - LEAN_AVERAGING * deltaTime) +
+                                (LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY;
+        _head.leanSideways = _head.leanSideways * (1.f - LEAN_AVERAGING * deltaTime) +
+                                (LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY;
+    }
+    setHeadLeanSideways(_head.leanSideways);
+    setHeadLeanForward(_head.leanForward); 
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
@@ -323,6 +344,8 @@ void Avatar::simulate(float deltaTime) {
     // update balls
     if (_balls) { _balls->simulate(deltaTime); }
     
+    // if other avatar, update head position from network data
+    
 	// update avatar skeleton
 	updateSkeleton();
 	
@@ -515,14 +538,12 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) {
 
 void Avatar::updateHead(float deltaTime) {
 
-    // hold on to this - used for testing....
+    // Get head position data from network for other people
-    /*
+    if (!_isMine) {
-    static float test = 0.0f;
+        _head.leanSideways = getHeadLeanSideways();
-    test += deltaTime;
+        _head.leanForward = getHeadLeanForward(); 
-    _head.leanForward  = 0.02 * sin( test * 0.2f );
+    }
-    _head.leanSideways = 0.02 * sin( test * 0.3f );
+    
-    */
-
     //apply the head lean values to the springy position...
     if (fabs(_head.leanSideways + _head.leanForward) > 0.0f) {
         glm::vec3 headLean = 
@@ -549,7 +570,7 @@ void Avatar::updateHead(float deltaTime) {
     }
 
     //  Decay head back to center if turned on
-    if (_returnHeadToCenter) {
+    if (_isMine && _returnHeadToCenter) {
         //  Decay back toward center
         _headPitch *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
         _headYaw   *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
@@ -557,13 +578,14 @@ void Avatar::updateHead(float deltaTime) {
     }
     
     //  For invensense gyro, decay only slightly when roughly centered
-    const float RETURN_RANGE = 15.0;
+    if (_isMine) {
-    const float RETURN_STRENGTH = 2.0;
+        const float RETURN_RANGE = 15.0;
-    if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        const float RETURN_STRENGTH = 2.0;
-    if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); }
-    if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); }
+    }
 
-    
     if (_head.noise) {
         //  Move toward new target
         _headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ;

interface/src/SerialInterface.h

@@ -50,16 +50,11 @@ public:
     float getLastYawRate() const { return _lastYawRate; }
     float getLastPitchRate() const { return _lastPitchRate; }
     float getLastRollRate() const { return _lastRollRate; }
-    
+    glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
-    //int getLED() {return LED;};
+    glm::vec3 getGravity() {return _gravity;};
-    //int getNumSamples() {return samplesAveraged;};
-    //int getValue(int num) {return lastMeasured[num];};
-    //int getRelativeValue(int num) {return static_cast<int>(lastMeasured[num] - trailingAverage[num]);};
-    //float getTrailingValue(int num) {return trailingAverage[num];};
     
     void renderLevels(int width, int height);
     bool active;
-    glm::vec3 getGravity() {return _gravity;};
     
 private:
     void initializePort(char* portname, int baud);

libraries/avatars/src/AvatarData.cpp

@@ -42,6 +42,8 @@ AvatarData::AvatarData() :
     _headYaw(0),
     _headPitch(0),
     _headRoll(0),
+    _headLeanSideways(0),
+    _headLeanForward(0),
     _handState(0),
     _cameraPosition(0,0,0),
     _cameraDirection(0,0,0),
@@ -84,7 +86,13 @@ int AvatarData::getBroadcastData(unsigned char* destinationBuffer) {
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headPitch);
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headRoll);
     
-    // Hand Position 
+    // Head lean X,Z (head lateral and fwd/back motion relative to torso)
+    memcpy(destinationBuffer, &_headLeanSideways, sizeof(float));
+    destinationBuffer += sizeof(float);
+    memcpy(destinationBuffer, &_headLeanForward, sizeof(float));
+    destinationBuffer += sizeof(float);
+
+    // Hand Position
     memcpy(destinationBuffer, &_handPosition, sizeof(float) * 3);
     destinationBuffer += sizeof(float) * 3;
     
@@ -150,6 +158,12 @@ int AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) {
     sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headPitch);
     sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headRoll);
 
+    //  Head position relative to pelvis
+    memcpy(&_headLeanSideways, sourceBuffer, sizeof(float));
+    sourceBuffer += sizeof(float);
+    memcpy(&_headLeanForward, sourceBuffer, sizeof(float));
+    sourceBuffer += sizeof(float);
+
     // Hand Position
     memcpy(&_handPosition, sourceBuffer, sizeof(float) * 3);
     sourceBuffer += sizeof(float) * 3;

libraries/avatars/src/AvatarData.h

@@ -55,6 +55,12 @@ public:
     void  addHeadYaw(float y){_headYaw -= y; }
     void  addHeadRoll(float r){_headRoll += r; }
 
+    //  Head vector deflection from pelvix in X,Z
+    void setHeadLeanSideways(float s) {_headLeanSideways = s; };
+    float getHeadLeanSideways() const { return _headLeanSideways; };
+    void setHeadLeanForward(float f) {_headLeanForward = f; };
+    float getHeadLeanForward() const { return _headLeanForward; };
+    
     //  Hand State
     void setHandState(char s) { _handState = s; };
     char getHandState() const {return _handState; };
@@ -104,6 +110,9 @@ protected:
     float _headYaw;
     float _headPitch;
     float _headRoll;
+    
+    float _headLeanSideways;
+    float _headLeanForward;
 
     //  Audio loudness (used to drive facial animation)
     float _audioLoudness;