diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index cc7df02659..7eaedabad1 100644 --- a/interface/src/Avatar.cpp +++ b/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); - addHeadYaw(measuredYawRate * frametime); - addHeadRoll(measuredRollRate * frametime); + addHeadPitch(measuredPitchRate * deltaTime); + addHeadYaw(measuredYawRate * deltaTime); + 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.... - /* - static float test = 0.0f; - test += deltaTime; - _head.leanForward = 0.02 * sin( test * 0.2f ); - _head.leanSideways = 0.02 * sin( test * 0.3f ); - */ - + // Get head position data from network for other people + if (!_isMine) { + _head.leanSideways = getHeadLeanSideways(); + _head.leanForward = getHeadLeanForward(); + } + //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; - const float RETURN_STRENGTH = 2.0; - if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (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 (_isMine) { + const float RETURN_RANGE = 15.0; + const float RETURN_STRENGTH = 2.0; + if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (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 + ; diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 2858d2aee7..8569129520 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -50,16 +50,11 @@ public: float getLastYawRate() const { return _lastYawRate; } float getLastPitchRate() const { return _lastPitchRate; } float getLastRollRate() const { return _lastRollRate; } - - //int getLED() {return LED;}; - //int getNumSamples() {return samplesAveraged;}; - //int getValue(int num) {return lastMeasured[num];}; - //int getRelativeValue(int num) {return static_cast(lastMeasured[num] - trailingAverage[num]);}; - //float getTrailingValue(int num) {return trailingAverage[num];}; + glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); }; + glm::vec3 getGravity() {return _gravity;}; void renderLevels(int width, int height); bool active; - glm::vec3 getGravity() {return _gravity;}; private: void initializePort(char* portname, int baud); diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 3b8e06e81e..8279851458 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/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; diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 88d5c1ac1f..9ce2bb2c77 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/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;