From 4cac35293a00d6b01b77ed7b516866671d7862a5 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 10:08:15 -0700 Subject: [PATCH 01/10] Refer to body balls parents, not joint parents. --- interface/src/Avatar.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 04e462fd43..82336dec76 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1082,9 +1082,9 @@ void Avatar::updateBodyBalls(float deltaTime) { if (_skeleton.joint[b].parent == AVATAR_JOINT_NULL || length < SMALL_SPRING_LENGTH) { _bodyBall[b].rotation = orientation * _skeleton.joint[_bodyBall[b].parentJoint].absoluteBindPoseRotation; } else { - glm::vec3 parentDirection = _bodyBall[ _skeleton.joint[b].parent ].rotation * JOINT_DIRECTION; + glm::vec3 parentDirection = _bodyBall[ _bodyBall[b].parentBall ].rotation * JOINT_DIRECTION; _bodyBall[b].rotation = rotationBetween(parentDirection, springVector) * - _bodyBall[ _skeleton.joint[b].parent ].rotation; + _bodyBall[ _bodyBall[b].parentBall ].rotation; } } } From e0189c9834f64512548cadf3ba1d3794046f3533 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 10:17:57 -0700 Subject: [PATCH 02/10] Build fix, render body as balls if we don't have an avatar. --- audio-mixer/src/InjectedAudioRingBuffer.cpp | 4 +++- audio-mixer/src/PositionalAudioRingBuffer.cpp | 4 +++- interface/src/Avatar.cpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/audio-mixer/src/InjectedAudioRingBuffer.cpp b/audio-mixer/src/InjectedAudioRingBuffer.cpp index b866c2650e..426bbf361d 100644 --- a/audio-mixer/src/InjectedAudioRingBuffer.cpp +++ b/audio-mixer/src/InjectedAudioRingBuffer.cpp @@ -6,6 +6,8 @@ // Copyright (c) 2013 HighFidelity, Inc. All rights reserved. // +#include + #include #include "InjectedAudioRingBuffer.h" @@ -38,4 +40,4 @@ int InjectedAudioRingBuffer::parseData(unsigned char* sourceBuffer, int numBytes currentBuffer += parseAudioSamples(currentBuffer, numBytes - (currentBuffer - sourceBuffer)); return currentBuffer - sourceBuffer; -} \ No newline at end of file +} diff --git a/audio-mixer/src/PositionalAudioRingBuffer.cpp b/audio-mixer/src/PositionalAudioRingBuffer.cpp index 81911fc9df..2e83d3a4f6 100644 --- a/audio-mixer/src/PositionalAudioRingBuffer.cpp +++ b/audio-mixer/src/PositionalAudioRingBuffer.cpp @@ -6,6 +6,8 @@ // Copyright (c) 2013 HighFidelity, Inc. All rights reserved. // +#include + #include #include "PositionalAudioRingBuffer.h" @@ -64,4 +66,4 @@ bool PositionalAudioRingBuffer::shouldBeAddedToMix(int numJitterBufferSamples) { } return false; -} \ No newline at end of file +} diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 4be98641b2..83adb8a62e 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1133,7 +1133,7 @@ void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) { const float RENDER_TRANSLUCENT_BEYOND = 0.5f; // Render the body as balls and cones - if (renderAvatarBalls) { + if (renderAvatarBalls || !_voxels.getVoxelURL().isValid()) { for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) { float distanceToCamera = glm::length(_cameraPosition - _bodyBall[b].position); From 2cc640b6f67bed9a211ef0369abf622d72dc4859 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 10:26:26 -0700 Subject: [PATCH 03/10] Set head bone rotation from head orientation. --- interface/src/Avatar.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 83adb8a62e..79a51a96ff 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1073,6 +1073,9 @@ void Avatar::updateBodyBalls(float deltaTime) { _bodyBall[ _bodyBall[b].parentBall ].rotation; } } + + // copy the head's rotation + _bodyBall[BODY_BALL_HEAD_TOP].rotation = _head.getOrientation(); } void Avatar::updateArmIKAndConstraints(float deltaTime) { From 27717bfcd427eee5dff247e918f4cbda33fd9bb2 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 10:48:35 -0700 Subject: [PATCH 04/10] Removed unused variables. --- interface/src/Avatar.cpp | 1 - interface/src/VoxelSystem.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 79a51a96ff..c468514bb3 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1008,7 +1008,6 @@ void Avatar::updateBodyBalls(float deltaTime) { resetBodyBalls(); } glm::quat orientation = getOrientation(); - glm::vec3 jointDirection = orientation * JOINT_DIRECTION; for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) { glm::vec3 springVector; diff --git a/interface/src/VoxelSystem.cpp b/interface/src/VoxelSystem.cpp index 7e179c0919..9cde8906be 100644 --- a/interface/src/VoxelSystem.cpp +++ b/interface/src/VoxelSystem.cpp @@ -256,7 +256,6 @@ void VoxelSystem::copyWrittenDataToReadArraysFullVBOs() { void VoxelSystem::copyWrittenDataToReadArraysPartialVBOs() { glBufferIndex segmentStart = 0; - glBufferIndex segmentEnd = 0; bool inSegment = false; for (glBufferIndex i = 0; i < _voxelsInWriteArrays; i++) { bool thisVoxelDirty = _writeVoxelDirtyArray[i]; @@ -560,7 +559,6 @@ void VoxelSystem::updateFullVBOs() { void VoxelSystem::updatePartialVBOs() { glBufferIndex segmentStart = 0; - glBufferIndex segmentEnd = 0; bool inSegment = false; for (glBufferIndex i = 0; i < _voxelsInReadArrays; i++) { bool thisVoxelDirty = _readVoxelDirtyArray[i]; From d9d6891cc8db1939e710c016fa4f754f12fa7cf3 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 10:54:28 -0700 Subject: [PATCH 05/10] Use the head rotation to adjust the position, too. --- interface/src/Avatar.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index c468514bb3..b4e770d664 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1075,6 +1075,8 @@ void Avatar::updateBodyBalls(float deltaTime) { // copy the head's rotation _bodyBall[BODY_BALL_HEAD_TOP].rotation = _head.getOrientation(); + _bodyBall[BODY_BALL_HEAD_TOP].position = _bodyBall[BODY_BALL_HEAD_BASE].position + + _bodyBall[BODY_BALL_HEAD_TOP].rotation * _skeleton.joint[BODY_BALL_HEAD_TOP].bindPosePosition; } void Avatar::updateArmIKAndConstraints(float deltaTime) { From 56b4c04a18b468ed6752676d9e3c03d72f455171 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 11:09:09 -0700 Subject: [PATCH 06/10] Adjusted head base/top transforms. --- interface/src/Avatar.cpp | 4 +++- interface/src/Skeleton.cpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index b4e770d664..143b88aaa7 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -1074,7 +1074,9 @@ void Avatar::updateBodyBalls(float deltaTime) { } // copy the head's rotation - _bodyBall[BODY_BALL_HEAD_TOP].rotation = _head.getOrientation(); + _bodyBall[BODY_BALL_HEAD_BASE].rotation = _bodyBall[BODY_BALL_HEAD_TOP].rotation = _head.getOrientation(); + _bodyBall[BODY_BALL_HEAD_BASE].position = _bodyBall[BODY_BALL_NECK_BASE].position + + _bodyBall[BODY_BALL_HEAD_BASE].rotation * _skeleton.joint[BODY_BALL_HEAD_BASE].bindPosePosition; _bodyBall[BODY_BALL_HEAD_TOP].position = _bodyBall[BODY_BALL_HEAD_BASE].position + _bodyBall[BODY_BALL_HEAD_TOP].rotation * _skeleton.joint[BODY_BALL_HEAD_TOP].bindPosePosition; } diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp index 92dba252a1..3b3e42a4f4 100644 --- a/interface/src/Skeleton.cpp +++ b/interface/src/Skeleton.cpp @@ -56,6 +56,7 @@ void Skeleton::initialize() { joint[ AVATAR_JOINT_CHEST ].bindPosePosition = glm::vec3( 0.0, 0.09, -0.01 ); joint[ AVATAR_JOINT_NECK_BASE ].bindPosePosition = glm::vec3( 0.0, 0.14, 0.01 ); joint[ AVATAR_JOINT_HEAD_BASE ].bindPosePosition = glm::vec3( 0.0, 0.04, 0.00 ); + joint[ AVATAR_JOINT_HEAD_TOP ].bindPosePosition = glm::vec3( 0.0, 0.04, 0.00 ); joint[ AVATAR_JOINT_LEFT_COLLAR ].bindPosePosition = glm::vec3( -0.06, 0.04, 0.01 ); joint[ AVATAR_JOINT_LEFT_SHOULDER ].bindPosePosition = glm::vec3( -0.05, 0.0, 0.01 ); From b13f22be26e4af52642878ef4254b206fc137ab2 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 11:14:57 -0700 Subject: [PATCH 07/10] Forgot to update the default pose position. --- interface/src/Skeleton.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp index 3b3e42a4f4..2fa43c9010 100644 --- a/interface/src/Skeleton.cpp +++ b/interface/src/Skeleton.cpp @@ -86,6 +86,7 @@ void Skeleton::initialize() { joint[ AVATAR_JOINT_CHEST ].defaultPosePosition = glm::vec3( 0.0, 0.09, -0.01 ); joint[ AVATAR_JOINT_NECK_BASE ].defaultPosePosition = glm::vec3( 0.0, 0.14, 0.01 ); joint[ AVATAR_JOINT_HEAD_BASE ].defaultPosePosition = glm::vec3( 0.0, 0.04, 0.00 ); + joint[ AVATAR_JOINT_HEAD_TOP ].defaultPosePosition = glm::vec3( 0.0, 0.04, 0.00 ); joint[ AVATAR_JOINT_LEFT_COLLAR ].defaultPosePosition = glm::vec3( -0.06, 0.04, 0.01 ); joint[ AVATAR_JOINT_LEFT_SHOULDER ].defaultPosePosition = glm::vec3( -0.05, 0.0, 0.01 ); From a85149968417d87780c1d15b6a4387181ae8a265 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 12:03:50 -0700 Subject: [PATCH 08/10] Rework rotation estimation bits to use quaternions. --- interface/src/SerialInterface.cpp | 29 ++++++----------------------- 1 file changed, 6 insertions(+), 23 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 1e472c5a11..ad23051a1b 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -229,8 +229,8 @@ void SerialInterface::readData(float deltaTime) { _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; // Update raw rotation estimates - _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates); - + glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * + glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates))); // Update estimated position and velocity float const DECAY_VELOCITY = 0.95f; @@ -258,29 +258,12 @@ void SerialInterface::readData(float deltaTime) { 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; - } - + _estimatedRotation = safeMix(_estimatedRotation, + rotationBetween(_gravity, _lastAcceleration) * _estimatedRotation, 1.0f / SENSOR_FUSION_SAMPLES); } } - + + _estimatedRotation = safeEulerAngles(estimatedRotation); totalSamples++; } From 14f331b07db2c3b22edd258e5d6427458453f41b Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 12:05:52 -0700 Subject: [PATCH 09/10] Scratch that; need to do this in a branch. --- interface/src/SerialInterface.cpp | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index ad23051a1b..1e472c5a11 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -229,8 +229,8 @@ void SerialInterface::readData(float deltaTime) { _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; // Update raw rotation estimates - glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * - glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates))); + _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates); + // Update estimated position and velocity float const DECAY_VELOCITY = 0.95f; @@ -258,12 +258,29 @@ void SerialInterface::readData(float deltaTime) { 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration; } else { // Use gravity reading to do sensor fusion on the pitch and roll estimation - _estimatedRotation = safeMix(_estimatedRotation, - rotationBetween(_gravity, _lastAcceleration) * _estimatedRotation, 1.0f / SENSOR_FUSION_SAMPLES); + 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; + } + } } - - _estimatedRotation = safeEulerAngles(estimatedRotation); + totalSamples++; } From 650981abfae985fa7aa6dcfc15e3d6b94bf6f106 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 6 Jun 2013 13:41:22 -0700 Subject: [PATCH 10/10] Remove the bounce term; it causes fluctuations in velocity. --- interface/src/Avatar.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 143b88aaa7..c0be97aebb 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -806,13 +806,12 @@ void Avatar::applyCollisionWithScene(const glm::vec3& penetration) { static float STATIC_FRICTION_VELOCITY = 0.15f; static float STATIC_FRICTION_DAMPING = 0.0f; static float KINETIC_FRICTION_DAMPING = 0.95f; - const float BOUNCE = 0.3f; - // reflect the velocity component in the direction of penetration + // cancel out the velocity component in the direction of penetration float penetrationLength = glm::length(penetration); if (penetrationLength > EPSILON) { glm::vec3 direction = penetration / penetrationLength; - _velocity -= 2.0f * glm::dot(_velocity, direction) * direction * BOUNCE; + _velocity -= glm::dot(_velocity, direction) * direction; _velocity *= KINETIC_FRICTION_DAMPING; // If velocity is quite low, apply static friction that takes away energy if (glm::length(_velocity) < STATIC_FRICTION_VELOCITY) {