diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 71769b6be2..4c37be98c3 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -308,7 +308,7 @@ void Application::paintGL() { if (_myCamera.getMode() == CAMERA_MODE_MIRROR) { _myCamera.setTightness (100.0f); - _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE)); + _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition()); _myCamera.setTargetRotation(_myAvatar.getWorldAlignedOrientation() * glm::quat(glm::vec3(0.0f, PIf, 0.0f))); } else if (OculusManager::isConnected()) { @@ -320,11 +320,11 @@ void Application::paintGL() { } else if (_myCamera.getMode() == CAMERA_MODE_FIRST_PERSON) { _myCamera.setTightness(0.0f); // In first person, camera follows head exactly without delay - _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE)); + _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition()); _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale)); } else if (_myCamera.getMode() == CAMERA_MODE_THIRD_PERSON) { - _myCamera.setTargetPosition(_myAvatar.getHeadJointPosition()); + _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition()); _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale)); } diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index f336da2d8c..30337e73ad 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -103,9 +103,11 @@ Avatar::Avatar(Agent* owningAgent) : initializeBodyBalls(); _height = _skeleton.getHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius + _bodyBall[ BODY_BALL_HEAD_BASE ].radius; + _maxArmLength = _skeleton.getArmLength(); _pelvisStandingHeight = _skeleton.getPelvisStandingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius; _pelvisFloatingHeight = _skeleton.getPelvisFloatingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius; + _pelvisToHeadLength = _skeleton.getPelvisToHeadLength(); _avatarTouch.setReachableRadius(PERIPERSONAL_RADIUS); @@ -284,19 +286,12 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa _head.setYaw(estimatedRotation.y * AMPLIFY_YAW); _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL); - // Update head lean distance based on accelerometer data - glm::vec3 headRotationRates(_head.getPitch(), _head.getYaw(), _head.getRoll()); - - glm::vec3 leaning = (serialInterface->getLastAcceleration() - serialInterface->getGravity()) - * LEAN_SENSITIVITY - * (1.f - fminf(glm::length(headRotationRates), HEAD_RATE_MAX) / HEAD_RATE_MAX); - leaning.y = 0.f; - if (glm::length(leaning) < LEAN_MAX) { - _head.setLeanForward(_head.getLeanForward() * (1.f - LEAN_AVERAGING * deltaTime) + - (LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY); - _head.setLeanSideways(_head.getLeanSideways() * (1.f - LEAN_AVERAGING * deltaTime) + - (LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY); - } + // Update torso lean distance based on accelerometer data + glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition(); + const float TORSO_LENGTH = 0.5f; + const float MAX_LEAN = 45.0f; + _head.setLeanSideways(glm::clamp(glm::degrees(atanf(-estimatedPosition.x / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN)); + _head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN)); } float Avatar::getAbsoluteHeadYaw() const { @@ -315,6 +310,10 @@ glm::quat Avatar::getWorldAlignedOrientation () const { return computeRotationFromBodyToWorldUp() * getOrientation(); } +glm::vec3 Avatar::getUprightHeadPosition() const { + return _position + getWorldAlignedOrientation() * glm::vec3(0.0f, _pelvisToHeadLength, 0.0f); +} + void Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight) { // Update head yaw and pitch based on mouse input const float MOUSE_ROTATE_SPEED = 0.01f; @@ -347,6 +346,10 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { // update balls if (_balls) { _balls->simulate(deltaTime); } + // update torso rotation based on head lean + _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::radians(glm::vec3( + _head.getLeanForward(), 0.0f, _head.getLeanSideways()))); + // update avatar skeleton _skeleton.update(deltaTime, getOrientation(), _position); @@ -1124,32 +1127,29 @@ glm::quat Avatar::computeRotationFromBodyToWorldUp(float proportion) const { return glm::angleAxis(angle * proportion, axis); } -void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) { - +float Avatar::getBallRenderAlpha(int ball, bool lookingInMirror) const { const float RENDER_OPAQUE_BEYOND = 1.0f; // Meters beyond which body is shown opaque const float RENDER_TRANSLUCENT_BEYOND = 0.5f; + float distanceToCamera = glm::length(_cameraPosition - _bodyBall[ball].position); + return (lookingInMirror || _owningAgent) ? 1.0f : glm::clamp( + (distanceToCamera - RENDER_TRANSLUCENT_BEYOND) / (RENDER_OPAQUE_BEYOND - RENDER_TRANSLUCENT_BEYOND), 0.f, 1.f); +} + +void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) { + + // Render the body as balls and cones if (renderAvatarBalls || !_voxels.getVoxelURL().isValid()) { for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) { - float distanceToCamera = glm::length(_cameraPosition - _bodyBall[b].position); - - float alpha = lookingInMirror ? 1.0f : glm::clamp((distanceToCamera - RENDER_TRANSLUCENT_BEYOND) / - (RENDER_OPAQUE_BEYOND - RENDER_TRANSLUCENT_BEYOND), 0.f, 1.f); - - if (lookingInMirror || _owningAgent) { - alpha = 1.0f; - } + float alpha = getBallRenderAlpha(b, lookingInMirror); // Always render other people, and render myself when beyond threshold distance if (b == BODY_BALL_HEAD_BASE) { // the head is rendered as a special - if (lookingInMirror || _owningAgent || distanceToCamera > RENDER_OPAQUE_BEYOND * 0.5) { + if (alpha > 0.0f) { _head.render(lookingInMirror, _cameraPosition, alpha); } - } else if (_owningAgent || distanceToCamera > RENDER_TRANSLUCENT_BEYOND - || b == BODY_BALL_RIGHT_ELBOW - || b == BODY_BALL_RIGHT_WRIST - || b == BODY_BALL_RIGHT_FINGERTIPS ) { + } else if (alpha > 0.0f) { // Render the body ball sphere if (_owningAgent || b == BODY_BALL_RIGHT_ELBOW || b == BODY_BALL_RIGHT_WRIST @@ -1201,7 +1201,10 @@ void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) { } } else { // Render the body's voxels - _voxels.render(false); + float alpha = getBallRenderAlpha(BODY_BALL_HEAD_BASE, lookingInMirror); + if (alpha > 0.0f) { + _voxels.render(false); + } } } diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index 0fc5b31286..de76092328 100644 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -121,6 +121,8 @@ public: glm::quat getOrientation () const; glm::quat getWorldAlignedOrientation() const; + glm::vec3 getUprightHeadPosition() const; + AvatarVoxelSystem* getVoxels() { return &_voxels; } // Set what driving keys are being pressed to control thrust levels @@ -185,6 +187,7 @@ private: int _driveKeys[MAX_DRIVE_KEYS]; float _pelvisStandingHeight; float _pelvisFloatingHeight; + float _pelvisToHeadLength; float _height; Balls* _balls; AvatarTouch _avatarTouch; @@ -201,6 +204,7 @@ private: // private methods... glm::vec3 caclulateAverageEyePosition() { return _head.caclulateAverageEyePosition(); } // get the position smack-dab between the eyes (for lookat) glm::quat computeRotationFromBodyToWorldUp(float proportion = 1.0f) const; + float getBallRenderAlpha(int ball, bool lookingInMirror) const; void renderBody(bool lookingInMirror, bool renderAvatarBalls); void initializeBodyBalls(); void resetBodyBalls(); diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp index 8a7708587f..dc3b937da9 100644 --- a/interface/src/AvatarVoxelSystem.cpp +++ b/interface/src/AvatarVoxelSystem.cpp @@ -17,7 +17,7 @@ #include "renderer/ProgramObject.h" const float AVATAR_TREE_SCALE = 1.0f; -const int MAX_VOXELS_PER_AVATAR = 2000; +const int MAX_VOXELS_PER_AVATAR = 10000; const int BONE_ELEMENTS_PER_VOXEL = BONE_ELEMENTS_PER_VERTEX * VERTICES_PER_VOXEL; AvatarVoxelSystem::AvatarVoxelSystem(Avatar* avatar) : diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 28ae7eceb4..76487bb668 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -6,6 +6,7 @@ // #include "SerialInterface.h" +#include "SharedUtil.h" #include "Util.h" #include #include @@ -155,11 +156,11 @@ void SerialInterface::renderLevels(int width, int height) { // Acceleration rates glColor4f(1, 1, 1, 1); glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42); - glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.x * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42); glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57); - glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.y * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57); glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72); - glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.z * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72); // Estimated Position glColor4f(0, 1, 1, 1); @@ -217,6 +218,7 @@ void SerialInterface::readData(float deltaTime) { _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2; + int rollRate, yawRate, pitchRate; convertHexToInt(sensorBuffer + 22, rollRate); @@ -225,35 +227,87 @@ void SerialInterface::readData(float deltaTime) { // Convert the integer rates to floats const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. - _lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; - _lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; - _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; + glm::vec3 rotationRates; + rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; + // update and subtract the long term average + _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates + + 1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates; + rotationRates -= _averageRotationRates; + + // compute the angular acceleration + glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime; + _lastRotationRates = rotationRates; + // Update raw rotation estimates glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * - glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates))); + glm::quat(glm::radians(deltaTime * _lastRotationRates)); + + // Update acceleration estimate: first, subtract gravity as rotated into current frame + _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() : + _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; + + // update and subtract the long term average + _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration + + 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration; + _estimatedAcceleration -= _averageAcceleration; + + // Consider updating our angular velocity/acceleration to linear acceleration mapping + if (glm::length(_estimatedAcceleration) > EPSILON && + (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) { + // compute predicted linear acceleration, find error between actual and predicted + glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates + + _angularAccelToLinearAccel * angularAcceleration; + glm::vec3 error = _estimatedAcceleration - predictedAcceleration; + + // the "error" is actually what we want: the linear acceleration minus rotational influences + _estimatedAcceleration = error; + + // adjust according to error in each dimension, in proportion to input magnitudes + for (int i = 0; i < 3; i++) { + if (fabsf(error[i]) < EPSILON) { + continue; + } + const float LEARNING_RATE = 0.001f; + float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z); + if (rateSum > EPSILON) { + for (int j = 0; j < 3; j++) { + float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum; + if (proportion > EPSILON) { + _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j]; + } + } + } + float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z); + if (accelSum > EPSILON) { + for (int j = 0; j < 3; j++) { + float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum; + if (proportion > EPSILON) { + _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j]; + } + } + } + } + } + + // rotate estimated acceleration into global rotation frame + _estimatedAcceleration = estimatedRotation * _estimatedAcceleration; // Update estimated position and velocity - float const DECAY_VELOCITY = 0.95f; - float const DECAY_POSITION = 0.95f; - _estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration); + float const DECAY_VELOCITY = 0.975f; + float const DECAY_POSITION = 0.975f; + _estimatedVelocity += deltaTime * _estimatedAcceleration; _estimatedPosition += deltaTime * _estimatedVelocity; _estimatedVelocity *= DECAY_VELOCITY; _estimatedPosition *= DECAY_POSITION; // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { - _averageRotationRates = _lastRotationRates; - _averageAcceleration = _lastAcceleration; _gravity = _lastAcceleration; } else { - // Cumulate long term average to (hopefully) take DC bias out of rotation rates - _averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates - + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates; - _averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration - + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration; - if (totalSamples < GRAVITY_SAMPLES) { _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity + 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration; @@ -299,6 +353,7 @@ void SerialInterface::resetAverages() { _estimatedRotation = glm::vec3(0, 0, 0); _estimatedPosition = glm::vec3(0, 0, 0); _estimatedVelocity = glm::vec3(0, 0, 0); + _estimatedAcceleration = glm::vec3(0, 0, 0); } void SerialInterface::resetSerial() { diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 3fc9ea920a..8c918e65ff 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -32,18 +32,27 @@ public: _estimatedPosition(0, 0, 0), _estimatedVelocity(0, 0, 0), _lastAcceleration(0, 0, 0), - _lastRotationRates(0, 0, 0) + _lastRotationRates(0, 0, 0), + _angularVelocityToLinearAccel( // experimentally derived initial values + 0.003f, -0.001f, -0.006f, + -0.005f, -0.001f, -0.006f, + 0.010f, 0.004f, 0.007f), + _angularAccelToLinearAccel( // experimentally derived initial values + 0.0f, 0.0f, 0.002f, + 0.0f, 0.0f, 0.001f, + -0.002f, -0.002f, 0.0f) {} void pair(); void readData(float deltaTime); - const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; } - const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; } - const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; } + const float getLastPitchRate() const { return _lastRotationRates[0]; } + const float getLastYawRate() const { return _lastRotationRates[1]; } + const float getLastRollRate() const { return _lastRotationRates[2]; } const glm::vec3& getLastRotationRates() const { return _lastRotationRates; }; const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; }; const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; }; const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; }; + const glm::vec3& getEstimatedAcceleration() const { return _estimatedAcceleration; }; const glm::vec3& getLastAcceleration() const { return _lastAcceleration; }; const glm::vec3& getGravity() const { return _gravity; }; @@ -64,8 +73,12 @@ private: glm::vec3 _estimatedRotation; glm::vec3 _estimatedPosition; glm::vec3 _estimatedVelocity; + glm::vec3 _estimatedAcceleration; glm::vec3 _lastAcceleration; glm::vec3 _lastRotationRates; + + glm::mat3 _angularVelocityToLinearAccel; + glm::mat3 _angularAccelToLinearAccel; }; #endif diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp index 2fa43c9010..1167194534 100644 --- a/interface/src/Skeleton.cpp +++ b/interface/src/Skeleton.cpp @@ -132,15 +132,15 @@ void Skeleton::update(float deltaTime, const glm::quat& orientation, glm::vec3 p for (int b = 0; b < NUM_AVATAR_JOINTS; b++) { if (joint[b].parent == AVATAR_JOINT_NULL) { - joint[b].rotation = orientation; + joint[b].absoluteRotation = orientation * joint[b].rotation; joint[b].position = position; } else { - joint[b].rotation = joint[ joint[b].parent ].rotation; + joint[b].absoluteRotation = joint[ joint[b].parent ].absoluteRotation * joint[b].rotation; joint[b].position = joint[ joint[b].parent ].position; } - glm::vec3 rotatedJointVector = joint[b].rotation * joint[b].defaultPosePosition; + glm::vec3 rotatedJointVector = joint[b].absoluteRotation * joint[b].defaultPosePosition; joint[b].position += rotatedJointVector; } } @@ -174,6 +174,13 @@ float Skeleton::getPelvisFloatingHeight() { FLOATING_HEIGHT; } +float Skeleton::getPelvisToHeadLength() { + return + joint[ AVATAR_JOINT_TORSO ].length + + joint[ AVATAR_JOINT_CHEST ].length + + joint[ AVATAR_JOINT_NECK_BASE ].length + + joint[ AVATAR_JOINT_HEAD_BASE ].length; +} diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h index e98c2e7b12..bb953fe947 100644 --- a/interface/src/Skeleton.h +++ b/interface/src/Skeleton.h @@ -56,6 +56,7 @@ public: float getHeight(); float getPelvisStandingHeight(); float getPelvisFloatingHeight(); + float getPelvisToHeadLength(); //glm::vec3 getJointVectorFromParent(AvatarJointID jointID) {return joint[jointID].position - joint[joint[jointID].parent].position; } struct AvatarJoint @@ -68,6 +69,7 @@ public: glm::quat absoluteBindPoseRotation; // the absolute rotation when the avatar is in the "T-pose" float bindRadius; // the radius of the bone capsule that envelops the vertices to bind glm::quat rotation; // the parent-relative rotation (orientation) of the joint as a quaternion + glm::quat absoluteRotation; // the absolute rotation of the joint as a quaternion float length; // the length of vector connecting the joint and its parent }; diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp index 30aa75b461..697719b36d 100644 --- a/libraries/shared/src/SharedUtil.cpp +++ b/libraries/shared/src/SharedUtil.cpp @@ -102,12 +102,12 @@ void setAtBit(unsigned char& byte, int bitIndex) { } int getSemiNibbleAt(unsigned char& byte, int bitIndex) { - return (byte >> (7 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11 + return (byte >> (6 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11 } void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) { //assert(value <= 3 && value >= 0); - byte += ((value & 3) << (7 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11 + byte += ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11 }