From 1f0e3219803d62977b93944281029f97e39a2fc9 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 5 Jun 2013 17:54:14 -0700 Subject: [PATCH 1/4] removed Oculus Manager from camera object, using _tightness = 0.0 to shutoff camera movement smoothing --- interface/src/Application.cpp | 17 ++++------------- interface/src/Camera.cpp | 23 +++++++++-------------- 2 files changed, 13 insertions(+), 27 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 836eb5636b..f6dba4f97a 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -303,11 +303,12 @@ void Application::paintGL() { } else if (OculusManager::isConnected()) { _myCamera.setUpShift (0.0f); _myCamera.setDistance (0.0f); - _myCamera.setTightness (100.0f); + _myCamera.setTightness (0.0f); // Camera is directly connected to head without smoothing _myCamera.setTargetPosition(_myAvatar.getHeadJointPosition()); _myCamera.setTargetRotation(_myAvatar.getHead().getOrientation()); } 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.setTargetRotation(_myAvatar.getHead().getWorldAlignedOrientation()); @@ -316,19 +317,9 @@ void Application::paintGL() { _myCamera.setTargetRotation(_myAvatar.getHead().getWorldAlignedOrientation()); } - // important... + // Update camera position _myCamera.update( 1.f/_fps ); - // Render anything (like HUD items) that we want to be in 3D but not in worldspace - /* - const float HUD_Z_OFFSET = -5.f; - glPushMatrix(); - glm::vec3 test(0.5, 0.5, 0.5); - glTranslatef(1, 1, HUD_Z_OFFSET); - drawVector(&test); - glPopMatrix(); - */ - // Note: whichCamera is used to pick between the normal camera myCamera for our // main camera, vs, an alternate camera. The alternate camera we support right now @@ -1971,7 +1962,7 @@ void Application::displaySide(Camera& whichCamera) { } } agentList->unlock(); - + // Render my own Avatar _myAvatar.render(_lookingInMirror->isChecked(), _renderAvatarBalls->isChecked()); _myAvatar.setDisplayingLookatVectors(_renderLookatOn->isChecked()); diff --git a/interface/src/Camera.cpp b/interface/src/Camera.cpp index 4227e623ff..9fc0033fc4 100644 --- a/interface/src/Camera.cpp +++ b/interface/src/Camera.cpp @@ -7,9 +7,7 @@ #include #include #include -#include #include "Log.h" - #include "Camera.h" #include "Util.h" @@ -88,22 +86,19 @@ void Camera::updateFollowMode(float deltaTime) { t = 1.0; } - // update rotation (before position!) - if (_needsToInitialize || OculusManager::isConnected()) { + // Update position and rotation, setting directly if tightness is 0.0 + + if (_needsToInitialize || (_tightness == 0.0f)) { _rotation = _targetRotation; + _idealPosition = _targetPosition + _rotation * glm::vec3(0.0f, _upShift, _distance); + _position = _idealPosition; + _needsToInitialize = false; + } else { // pull rotation towards ideal _rotation = safeMix(_rotation, _targetRotation, t); - } - - _idealPosition = _targetPosition + _rotation * glm::vec3(0.0f, _upShift, _distance); - - if (_needsToInitialize) { - _position = _idealPosition; - _needsToInitialize = false; - } else { - // force position towards ideal position - _position += (_idealPosition - _position) * t; + _idealPosition = _targetPosition + _rotation * glm::vec3(0.0f, _upShift, _distance); + _position += (_idealPosition - _position) * t; } } From 1b20b016dd61e720a1bf483570bc3a29fa1f47f7 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 5 Jun 2013 20:04:30 -0700 Subject: [PATCH 2/4] Fixed acceleration checking bug that was limiting angular rotation, changed mouselook to work correctly/stable. --- interface/src/Application.cpp | 4 +-- interface/src/Avatar.cpp | 60 ++++++++++++++++------------------- interface/src/Avatar.h | 1 - 3 files changed, 30 insertions(+), 35 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index f6dba4f97a..5caf82faf7 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -1558,6 +1558,7 @@ void Application::init() { void Application::updateAvatar(float deltaTime) { + if (_serialHeadSensor.active) { // Update avatar head translation @@ -1591,7 +1592,6 @@ void Application::updateAvatar(float deltaTime) { } - if (OculusManager::isConnected()) { float yaw, pitch, roll; OculusManager::getEulerAngles(yaw, pitch, roll); @@ -1600,7 +1600,7 @@ void Application::updateAvatar(float deltaTime) { _myAvatar.getHead().setPitch(pitch); _myAvatar.getHead().setRoll(roll); } - + // Get audio loudness data from audio input device #ifndef _WIN32 _myAvatar.getHead().setAudioLoudness(_audio.getLastInputLoudness()); diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 04e462fd43..28e66e9a52 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -87,7 +87,6 @@ Avatar::Avatar(Agent* owningAgent) : _mouseRayOrigin(0.0f, 0.0f, 0.0f), _mouseRayDirection(0.0f, 0.0f, 0.0f), _interactingOther(NULL), - _cumulativeMouseYaw(0.0f), _isMouseTurningRight(false), _voxels(this) { @@ -288,6 +287,13 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa _head.addPitch(measuredPitchRate * AMPLIFY_PITCH * deltaTime); _head.addYaw(measuredYawRate * AMPLIFY_YAW * deltaTime); _head.addRoll(measuredRollRate * AMPLIFY_ROLL * deltaTime); + + /* + glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation(); + _head.setPitch(estimatedRotation.x * AMPLIFY_PITCH); + _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()); @@ -321,11 +327,10 @@ glm::quat Avatar::getWorldAlignedOrientation () const { } void Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight) { - // Update yaw based on mouse behavior + // Update head yaw and pitch based on mouse input const float MOUSE_MOVE_RADIUS = 0.15f; - const float MOUSE_ROTATE_SPEED = 3.0f; + const float MOUSE_ROTATE_SPEED = 4.0f; const float MOUSE_PITCH_SPEED = 1.5f; - const float MAX_YAW_TO_ADD = 180.f; const int TITLE_BAR_HEIGHT = 46; float mouseLocationX = (float)mouseX / (float)screenWidth - 0.5f; float mouseLocationY = (float)mouseY / (float)screenHeight - 0.5f; @@ -334,30 +339,18 @@ void Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int scree // // Mouse must be inside screen (not at edge) and not on title bar for movement to happen // - if (fabs(mouseLocationX) > MOUSE_MOVE_RADIUS) { - // Add Yaw - float mouseYawAdd = (fabs(mouseLocationX) - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_ROTATE_SPEED; - bool rightTurning = (mouseLocationX > 0.f); - if (_isMouseTurningRight == rightTurning) { - _cumulativeMouseYaw += mouseYawAdd; - } else { - _cumulativeMouseYaw = 0; - _isMouseTurningRight = rightTurning; - } - if (_cumulativeMouseYaw < MAX_YAW_TO_ADD) { - setBodyYaw(getBodyYaw() - (rightTurning ? mouseYawAdd : -mouseYawAdd)); - } - } else { - _cumulativeMouseYaw = 0; - } - if (fabs(mouseLocationY) > MOUSE_MOVE_RADIUS) { - float mousePitchAdd = (fabs(mouseLocationY) - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED; - bool downPitching = (mouseLocationY > 0.f); - _head.setPitch(_head.getPitch() + (downPitching ? -mousePitchAdd : mousePitchAdd)); + if (mouseLocationX > MOUSE_MOVE_RADIUS) { + _head.addYaw(-(mouseLocationX - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_ROTATE_SPEED); + } else if (mouseLocationX < -MOUSE_MOVE_RADIUS) { + _head.addYaw(-(mouseLocationX + MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_ROTATE_SPEED); } + if (mouseLocationY > MOUSE_MOVE_RADIUS) { + _head.addPitch(-(mouseLocationY - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED); + } else if (mouseLocationY < -MOUSE_MOVE_RADIUS) { + _head.addPitch(-(mouseLocationY + MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED); + } } - return; } @@ -538,20 +531,23 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { } } - // Compute instantaneous acceleration - float acceleration = glm::distance(getVelocity(), oldVelocity) / deltaTime; + // Compute instantaneous acceleration + ; + float fwdAcceleration = glm::length(glm::dot(getBodyFrontDirection(), getVelocity() - oldVelocity)) / deltaTime; const float ACCELERATION_PITCH_DECAY = 0.4f; const float ACCELERATION_YAW_DECAY = 0.4f; - + const float ACCELERATION_PULL_THRESHOLD = 0.2f; const float OCULUS_ACCELERATION_PULL_THRESHOLD = 1.0f; const int OCULUS_YAW_OFFSET_THRESHOLD = 10; - // Decay HeadPitch as a function of acceleration, so that you look straight ahead when + // Decay HeadPitch as a function of acceleration, so that you are pulled to look straight ahead when // you start moving, but don't do this with an HMD like the Oculus. if (!OculusManager::isConnected()) { - _head.setPitch(_head.getPitch() * (1.f - acceleration * ACCELERATION_PITCH_DECAY * deltaTime)); - _head.setYaw(_head.getYaw() * (1.f - acceleration * ACCELERATION_YAW_DECAY * deltaTime)); - } else if (fabsf(acceleration) > OCULUS_ACCELERATION_PULL_THRESHOLD + if (fwdAcceleration > ACCELERATION_PULL_THRESHOLD) { + _head.setPitch(_head.getPitch() * (1.f - fwdAcceleration * ACCELERATION_PITCH_DECAY * deltaTime)); + _head.setYaw(_head.getYaw() * (1.f - fwdAcceleration * ACCELERATION_YAW_DECAY * deltaTime)); + } + } else if (fabsf(fwdAcceleration) > OCULUS_ACCELERATION_PULL_THRESHOLD && fabs(_head.getYaw()) > OCULUS_YAW_OFFSET_THRESHOLD) { // if we're wearing the oculus // and this acceleration is above the pull threshold diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index c1fc353a34..0fc5b31286 100644 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -194,7 +194,6 @@ private: glm::vec3 _mouseRayOrigin; glm::vec3 _mouseRayDirection; Avatar* _interactingOther; - float _cumulativeMouseYaw; bool _isMouseTurningRight; AvatarVoxelSystem _voxels; From 24dcd1437176e28747237962f8bd7c26c948fbff Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 5 Jun 2013 23:53:01 -0700 Subject: [PATCH 3/4] Added sensor fusion for more stable gyro roll, pitch. --- interface/src/Application.cpp | 1 + interface/src/Avatar.cpp | 14 +++----------- interface/src/SerialInterface.cpp | 32 +++++++++++++++++++++++++++---- interface/src/Util.cpp | 4 +++- interface/src/Util.h | 2 ++ 5 files changed, 37 insertions(+), 16 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 5caf82faf7..5fadf3b297 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -19,6 +19,7 @@ #endif #include +#include #include #include diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 28e66e9a52..3eefd6e791 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -278,22 +279,13 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa const float AMPLIFY_PITCH = 2.f; const float AMPLIFY_YAW = 2.f; const float AMPLIFY_ROLL = 2.f; + - float measuredPitchRate = serialInterface->getLastPitchRate(); - float measuredYawRate = serialInterface->getLastYawRate(); - float measuredRollRate = serialInterface->getLastRollRate(); - - // Update avatar head position based on measured gyro rates - _head.addPitch(measuredPitchRate * AMPLIFY_PITCH * deltaTime); - _head.addYaw(measuredYawRate * AMPLIFY_YAW * deltaTime); - _head.addRoll(measuredRollRate * AMPLIFY_ROLL * deltaTime); - - /* glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation(); _head.setPitch(estimatedRotation.x * AMPLIFY_PITCH); _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()); diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 2fa078baf4..1fc993c174 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -6,6 +6,8 @@ // #include "SerialInterface.h" +#include +#include #ifdef __APPLE__ #include @@ -15,6 +17,7 @@ const short NO_READ_MAXIMUM_MSECS = 3000; const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values +const int SENSOR_FUSION_SAMPLES = 200; const int LONG_TERM_RATE_SAMPLES = 1000; const bool USING_INVENSENSE_MPU9150 = 1; @@ -227,6 +230,7 @@ void SerialInterface::readData(float deltaTime) { // Update raw rotation estimates _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates); + // Update estimated position and velocity float const DECAY_VELOCITY = 0.95f; @@ -235,10 +239,7 @@ void SerialInterface::readData(float deltaTime) { _estimatedPosition += deltaTime * _estimatedVelocity; _estimatedVelocity *= DECAY_VELOCITY; _estimatedPosition *= DECAY_POSITION; - - //glm::vec3 baseline = glm::normalize(_gravity); - //glm::vec3 current = glm::normalize(_lastAcceleration); - + // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { _averageRotationRates = _lastRotationRates; @@ -255,8 +256,31 @@ void SerialInterface::readData(float deltaTime) { if (totalSamples < GRAVITY_SAMPLES) { _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity + 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration; + } else { + // Use gravity reading to do sensor fusion on the pitch and roll estimation + float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)), + glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) * + ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0); + + float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)), + glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) * + ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0); + + // PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to + // be perfectly valid inputs. So I added these NaN tests, gotta fix. + if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) { + _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x + + 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle; + _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z + + 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle; + // Without a compass heading, always decay estimated Yaw slightly + const float YAW_DECAY = 0.995; + _estimatedRotation.y *= YAW_DECAY; + } + } } + totalSamples++; } diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index d16e6d36bb..9ad51cf483 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -57,7 +57,9 @@ void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * front, glm::vec3 * righ front->x = frontNew.x; front->y = frontNew.y; front->z = frontNew.z; } - +void printVector(glm::vec3 vec) { + printf("%4.2f, %4.2f, %4.2f\n", vec.x, vec.y, vec.z); +} // Return the azimuth angle in degrees between two points. float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos) { diff --git a/interface/src/Util.h b/interface/src/Util.h index 0187d93da5..05e7a21525 100644 --- a/interface/src/Util.h +++ b/interface/src/Util.h @@ -43,6 +43,8 @@ void noiseTest(int w, int h); void drawVector(glm::vec3* vector); +void printVector(glm::vec3 vec); + float angleBetween(const glm::vec3& v1, const glm::vec3& v2); glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2); From 61d4ac26751cd16d35f997dd99f6dc35832a14d0 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Thu, 6 Jun 2013 09:52:39 -0700 Subject: [PATCH 4/4] Fixes per code review. --- interface/src/Avatar.cpp | 12 ++++++------ interface/src/SerialInterface.cpp | 16 ++++++++-------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 3eefd6e791..00439986fa 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -524,8 +524,8 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { } // Compute instantaneous acceleration - ; - float fwdAcceleration = glm::length(glm::dot(getBodyFrontDirection(), getVelocity() - oldVelocity)) / deltaTime; + + float forwardAcceleration = glm::length(glm::dot(getBodyFrontDirection(), getVelocity() - oldVelocity)) / deltaTime; const float ACCELERATION_PITCH_DECAY = 0.4f; const float ACCELERATION_YAW_DECAY = 0.4f; const float ACCELERATION_PULL_THRESHOLD = 0.2f; @@ -535,11 +535,11 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { // Decay HeadPitch as a function of acceleration, so that you are pulled to look straight ahead when // you start moving, but don't do this with an HMD like the Oculus. if (!OculusManager::isConnected()) { - if (fwdAcceleration > ACCELERATION_PULL_THRESHOLD) { - _head.setPitch(_head.getPitch() * (1.f - fwdAcceleration * ACCELERATION_PITCH_DECAY * deltaTime)); - _head.setYaw(_head.getYaw() * (1.f - fwdAcceleration * ACCELERATION_YAW_DECAY * deltaTime)); + if (forwardAcceleration > ACCELERATION_PULL_THRESHOLD) { + _head.setPitch(_head.getPitch() * (1.f - forwardAcceleration * ACCELERATION_PITCH_DECAY * deltaTime)); + _head.setYaw(_head.getYaw() * (1.f - forwardAcceleration * ACCELERATION_YAW_DECAY * deltaTime)); } - } else if (fabsf(fwdAcceleration) > OCULUS_ACCELERATION_PULL_THRESHOLD + } else if (fabsf(forwardAcceleration) > OCULUS_ACCELERATION_PULL_THRESHOLD && fabs(_head.getYaw()) > OCULUS_YAW_OFFSET_THRESHOLD) { // if we're wearing the oculus // and this acceleration is above the pull threshold diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 1fc993c174..1e472c5a11 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -259,20 +259,20 @@ void SerialInterface::readData(float deltaTime) { } else { // Use gravity reading to do sensor fusion on the pitch and roll estimation float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)), - glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) * - ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0); + glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) + * ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0); float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)), - glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) * - ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0); + glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) + * ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0); // PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to // be perfectly valid inputs. So I added these NaN tests, gotta fix. if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) { - _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x + - 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle; - _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z + - 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle; + _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x + + 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle; + _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z + + 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle; // Without a compass heading, always decay estimated Yaw slightly const float YAW_DECAY = 0.995; _estimatedRotation.y *= YAW_DECAY;