From 30f77975cf93e96dc7005aa5443af17b55942ab8 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 3 Jul 2013 17:07:22 -0700 Subject: [PATCH] Trying to get the right rotations, send them on the wire. --- interface/src/Avatar.cpp | 208 ++++++++++----------------- interface/src/Avatar.h | 2 +- interface/src/Head.cpp | 7 +- interface/src/Webcam.cpp | 54 ++++++- libraries/avatars/src/AvatarData.cpp | 17 +++ libraries/avatars/src/AvatarData.h | 1 - 6 files changed, 151 insertions(+), 138 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 5f7725c2de..27ccf0db4b 100755 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -148,120 +148,120 @@ void Avatar::initializeBodyBalls() { _bodyBall[ BODY_BALL_HEAD_BASE ].radius = 0.07; _bodyBall[ BODY_BALL_LEFT_COLLAR ].radius = 0.04; _bodyBall[ BODY_BALL_LEFT_SHOULDER ].radius = 0.03; - _bodyBall[ BODY_BALL_LEFT_ELBOW ].radius = 0.02; + _bodyBall[ BODY_BALL_LEFT_ELBOW ].radius = 0.02; _bodyBall[ BODY_BALL_LEFT_WRIST ].radius = 0.02; _bodyBall[ BODY_BALL_LEFT_FINGERTIPS ].radius = 0.01; _bodyBall[ BODY_BALL_RIGHT_COLLAR ].radius = 0.04; - _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].radius = 0.03; - _bodyBall[ BODY_BALL_RIGHT_ELBOW ].radius = 0.02; + _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].radius = 0.03; + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].radius = 0.02; _bodyBall[ BODY_BALL_RIGHT_WRIST ].radius = 0.02; _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS ].radius = 0.01; - _bodyBall[ BODY_BALL_LEFT_HIP ].radius = 0.04; + _bodyBall[ BODY_BALL_LEFT_HIP ].radius = 0.04; //_bodyBall[ BODY_BALL_LEFT_MID_THIGH ].radius = 0.03; - _bodyBall[ BODY_BALL_LEFT_KNEE ].radius = 0.025; - _bodyBall[ BODY_BALL_LEFT_HEEL ].radius = 0.025; - _bodyBall[ BODY_BALL_LEFT_TOES ].radius = 0.025; - _bodyBall[ BODY_BALL_RIGHT_HIP ].radius = 0.04; - _bodyBall[ BODY_BALL_RIGHT_KNEE ].radius = 0.025; - _bodyBall[ BODY_BALL_RIGHT_HEEL ].radius = 0.025; - _bodyBall[ BODY_BALL_RIGHT_TOES ].radius = 0.025; + _bodyBall[ BODY_BALL_LEFT_KNEE ].radius = 0.025; + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius = 0.025; + _bodyBall[ BODY_BALL_LEFT_TOES ].radius = 0.025; + _bodyBall[ BODY_BALL_RIGHT_HIP ].radius = 0.04; + _bodyBall[ BODY_BALL_RIGHT_KNEE ].radius = 0.025; + _bodyBall[ BODY_BALL_RIGHT_HEEL ].radius = 0.025; + _bodyBall[ BODY_BALL_RIGHT_TOES ].radius = 0.025; // specify the parent joint for each ball - _bodyBall[ BODY_BALL_PELVIS ].parentJoint = AVATAR_JOINT_PELVIS; + _bodyBall[ BODY_BALL_PELVIS ].parentJoint = AVATAR_JOINT_PELVIS; _bodyBall[ BODY_BALL_TORSO ].parentJoint = AVATAR_JOINT_TORSO; - _bodyBall[ BODY_BALL_CHEST ].parentJoint = AVATAR_JOINT_CHEST; - _bodyBall[ BODY_BALL_NECK_BASE ].parentJoint = AVATAR_JOINT_NECK_BASE; + _bodyBall[ BODY_BALL_CHEST ].parentJoint = AVATAR_JOINT_CHEST; + _bodyBall[ BODY_BALL_NECK_BASE ].parentJoint = AVATAR_JOINT_NECK_BASE; _bodyBall[ BODY_BALL_HEAD_BASE ].parentJoint = AVATAR_JOINT_HEAD_BASE; _bodyBall[ BODY_BALL_HEAD_TOP ].parentJoint = AVATAR_JOINT_HEAD_TOP; _bodyBall[ BODY_BALL_LEFT_COLLAR ].parentJoint = AVATAR_JOINT_LEFT_COLLAR; _bodyBall[ BODY_BALL_LEFT_SHOULDER ].parentJoint = AVATAR_JOINT_LEFT_SHOULDER; - _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentJoint = AVATAR_JOINT_LEFT_ELBOW; - _bodyBall[ BODY_BALL_LEFT_WRIST ].parentJoint = AVATAR_JOINT_LEFT_WRIST; + _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentJoint = AVATAR_JOINT_LEFT_ELBOW; + _bodyBall[ BODY_BALL_LEFT_WRIST ].parentJoint = AVATAR_JOINT_LEFT_WRIST; _bodyBall[ BODY_BALL_LEFT_FINGERTIPS ].parentJoint = AVATAR_JOINT_LEFT_FINGERTIPS; _bodyBall[ BODY_BALL_RIGHT_COLLAR ].parentJoint = AVATAR_JOINT_RIGHT_COLLAR; - _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentJoint = AVATAR_JOINT_RIGHT_SHOULDER; - _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentJoint = AVATAR_JOINT_RIGHT_ELBOW; + _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentJoint = AVATAR_JOINT_RIGHT_SHOULDER; + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentJoint = AVATAR_JOINT_RIGHT_ELBOW; _bodyBall[ BODY_BALL_RIGHT_WRIST ].parentJoint = AVATAR_JOINT_RIGHT_WRIST; _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS ].parentJoint = AVATAR_JOINT_RIGHT_FINGERTIPS; - _bodyBall[ BODY_BALL_LEFT_HIP ].parentJoint = AVATAR_JOINT_LEFT_HIP; - _bodyBall[ BODY_BALL_LEFT_KNEE ].parentJoint = AVATAR_JOINT_LEFT_KNEE; - _bodyBall[ BODY_BALL_LEFT_HEEL ].parentJoint = AVATAR_JOINT_LEFT_HEEL; - _bodyBall[ BODY_BALL_LEFT_TOES ].parentJoint = AVATAR_JOINT_LEFT_TOES; - _bodyBall[ BODY_BALL_RIGHT_HIP ].parentJoint = AVATAR_JOINT_RIGHT_HIP; - _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentJoint = AVATAR_JOINT_RIGHT_KNEE; - _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentJoint = AVATAR_JOINT_RIGHT_HEEL; - _bodyBall[ BODY_BALL_RIGHT_TOES ].parentJoint = AVATAR_JOINT_RIGHT_TOES; + _bodyBall[ BODY_BALL_LEFT_HIP ].parentJoint = AVATAR_JOINT_LEFT_HIP; + _bodyBall[ BODY_BALL_LEFT_KNEE ].parentJoint = AVATAR_JOINT_LEFT_KNEE; + _bodyBall[ BODY_BALL_LEFT_HEEL ].parentJoint = AVATAR_JOINT_LEFT_HEEL; + _bodyBall[ BODY_BALL_LEFT_TOES ].parentJoint = AVATAR_JOINT_LEFT_TOES; + _bodyBall[ BODY_BALL_RIGHT_HIP ].parentJoint = AVATAR_JOINT_RIGHT_HIP; + _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentJoint = AVATAR_JOINT_RIGHT_KNEE; + _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentJoint = AVATAR_JOINT_RIGHT_HEEL; + _bodyBall[ BODY_BALL_RIGHT_TOES ].parentJoint = AVATAR_JOINT_RIGHT_TOES; //_bodyBall[ BODY_BALL_LEFT_MID_THIGH].parentJoint = AVATAR_JOINT_LEFT_HIP; // specify the parent offset for each ball - _bodyBall[ BODY_BALL_PELVIS ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_PELVIS ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_TORSO ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_CHEST ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_NECK_BASE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_CHEST ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_NECK_BASE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_HEAD_BASE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_HEAD_TOP ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_LEFT_COLLAR ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_LEFT_SHOULDER ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_WRIST ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_WRIST ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_LEFT_FINGERTIPS ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_RIGHT_COLLAR ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_RIGHT_WRIST ].parentOffset = glm::vec3(0.0, 0.0, 0.0); _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_HIP ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_KNEE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_HEEL ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_LEFT_TOES ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_HIP ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentOffset = glm::vec3(0.0, 0.0, 0.0); - _bodyBall[ BODY_BALL_RIGHT_TOES ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_HIP ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_KNEE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_HEEL ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_LEFT_TOES ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_HIP ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentOffset = glm::vec3(0.0, 0.0, 0.0); + _bodyBall[ BODY_BALL_RIGHT_TOES ].parentOffset = glm::vec3(0.0, 0.0, 0.0); //_bodyBall[ BODY_BALL_LEFT_MID_THIGH].parentOffset = glm::vec3(-0.1, -0.1, 0.0); // specify the parent BALL for each ball - _bodyBall[ BODY_BALL_PELVIS ].parentBall = BODY_BALL_NULL; + _bodyBall[ BODY_BALL_PELVIS ].parentBall = BODY_BALL_NULL; _bodyBall[ BODY_BALL_TORSO ].parentBall = BODY_BALL_PELVIS; - _bodyBall[ BODY_BALL_CHEST ].parentBall = BODY_BALL_TORSO; - _bodyBall[ BODY_BALL_NECK_BASE ].parentBall = BODY_BALL_CHEST; + _bodyBall[ BODY_BALL_CHEST ].parentBall = BODY_BALL_TORSO; + _bodyBall[ BODY_BALL_NECK_BASE ].parentBall = BODY_BALL_CHEST; _bodyBall[ BODY_BALL_HEAD_BASE ].parentBall = BODY_BALL_NECK_BASE; _bodyBall[ BODY_BALL_HEAD_TOP ].parentBall = BODY_BALL_HEAD_BASE; _bodyBall[ BODY_BALL_LEFT_COLLAR ].parentBall = BODY_BALL_CHEST; _bodyBall[ BODY_BALL_LEFT_SHOULDER ].parentBall = BODY_BALL_LEFT_COLLAR; - _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentBall = BODY_BALL_LEFT_SHOULDER; - _bodyBall[ BODY_BALL_LEFT_WRIST ].parentBall = BODY_BALL_LEFT_ELBOW; + _bodyBall[ BODY_BALL_LEFT_ELBOW ].parentBall = BODY_BALL_LEFT_SHOULDER; + _bodyBall[ BODY_BALL_LEFT_WRIST ].parentBall = BODY_BALL_LEFT_ELBOW; _bodyBall[ BODY_BALL_LEFT_FINGERTIPS ].parentBall = BODY_BALL_LEFT_WRIST; _bodyBall[ BODY_BALL_RIGHT_COLLAR ].parentBall = BODY_BALL_CHEST; - _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentBall = BODY_BALL_RIGHT_COLLAR; - _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentBall = BODY_BALL_RIGHT_SHOULDER; + _bodyBall[ BODY_BALL_RIGHT_SHOULDER ].parentBall = BODY_BALL_RIGHT_COLLAR; + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].parentBall = BODY_BALL_RIGHT_SHOULDER; _bodyBall[ BODY_BALL_RIGHT_WRIST ].parentBall = BODY_BALL_RIGHT_ELBOW; _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS ].parentBall = BODY_BALL_RIGHT_WRIST; - _bodyBall[ BODY_BALL_LEFT_HIP ].parentBall = BODY_BALL_PELVIS; + _bodyBall[ BODY_BALL_LEFT_HIP ].parentBall = BODY_BALL_PELVIS; //_bodyBall[ BODY_BALL_LEFT_MID_THIGH ].parentBall = BODY_BALL_LEFT_HIP; - // _bodyBall[ BODY_BALL_LEFT_KNEE ].parentBall = BODY_BALL_LEFT_MID_THIGH; - _bodyBall[ BODY_BALL_LEFT_KNEE ].parentBall = BODY_BALL_LEFT_HIP; + // _bodyBall[ BODY_BALL_LEFT_KNEE ].parentBall = BODY_BALL_LEFT_MID_THIGH; + _bodyBall[ BODY_BALL_LEFT_KNEE ].parentBall = BODY_BALL_LEFT_HIP; - _bodyBall[ BODY_BALL_LEFT_HEEL ].parentBall = BODY_BALL_LEFT_KNEE; - _bodyBall[ BODY_BALL_LEFT_TOES ].parentBall = BODY_BALL_LEFT_HEEL; - _bodyBall[ BODY_BALL_RIGHT_HIP ].parentBall = BODY_BALL_PELVIS; - _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentBall = BODY_BALL_RIGHT_HIP; - _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentBall = BODY_BALL_RIGHT_KNEE; - _bodyBall[ BODY_BALL_RIGHT_TOES ].parentBall = BODY_BALL_RIGHT_HEEL; + _bodyBall[ BODY_BALL_LEFT_HEEL ].parentBall = BODY_BALL_LEFT_KNEE; + _bodyBall[ BODY_BALL_LEFT_TOES ].parentBall = BODY_BALL_LEFT_HEEL; + _bodyBall[ BODY_BALL_RIGHT_HIP ].parentBall = BODY_BALL_PELVIS; + _bodyBall[ BODY_BALL_RIGHT_KNEE ].parentBall = BODY_BALL_RIGHT_HIP; + _bodyBall[ BODY_BALL_RIGHT_HEEL ].parentBall = BODY_BALL_RIGHT_KNEE; + _bodyBall[ BODY_BALL_RIGHT_TOES ].parentBall = BODY_BALL_RIGHT_HEEL; /* // to aid in hand-shaking and hand-holding, the right hand is not collidable - _bodyBall[ BODY_BALL_RIGHT_ELBOW ].isCollidable = false; - _bodyBall[ BODY_BALL_RIGHT_WRIST ].isCollidable = false; + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].isCollidable = false; + _bodyBall[ BODY_BALL_RIGHT_WRIST ].isCollidable = false; _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS].isCollidable = false; */ } @@ -308,7 +308,7 @@ void Avatar::updateFromGyrosAndOrWebcam() { _joints.clear(); for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { if (joints.size() > i && joints[i].isValid) { - JointData data = { i, joints[i].position, joints[i].orientation }; + JointData data = { i, joints[i].orientation }; _joints.push_back(data); } } @@ -496,72 +496,18 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { _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); - // apply joint data (if any) to skeleton + bool enableHandMovement = true; for (vector::iterator it = _joints.begin(); it != _joints.end(); it++) { - _skeleton.joint[it->jointID].absoluteRotation = orientation * it->rotation; - _skeleton.joint[it->jointID].position = _position + orientation * it->position; + _skeleton.joint[it->jointID].rotation = it->rotation; - AvatarJointID derivedJointID = AVATAR_JOINT_NULL; - AvatarJointID secondJointID = AVATAR_JOINT_NULL; - float proportion = 0.5f; - switch (it->jointID) { - case AVATAR_JOINT_NECK_BASE: - secondJointID = AVATAR_JOINT_TORSO; - derivedJointID = AVATAR_JOINT_CHEST; - break; - - case AVATAR_JOINT_HEAD_BASE: - derivedJointID = AVATAR_JOINT_HEAD_TOP; - break; - - case AVATAR_JOINT_LEFT_SHOULDER: - secondJointID = AVATAR_JOINT_CHEST; - derivedJointID = AVATAR_JOINT_LEFT_COLLAR; - break; - - case AVATAR_JOINT_LEFT_WRIST: - derivedJointID = AVATAR_JOINT_LEFT_FINGERTIPS; - break; - - case AVATAR_JOINT_RIGHT_SHOULDER: - secondJointID = AVATAR_JOINT_CHEST; - derivedJointID = AVATAR_JOINT_RIGHT_COLLAR; - break; - - case AVATAR_JOINT_RIGHT_WRIST: - derivedJointID = AVATAR_JOINT_RIGHT_FINGERTIPS; - break; - - case AVATAR_JOINT_LEFT_HEEL: - derivedJointID = AVATAR_JOINT_LEFT_TOES; - break; - - case AVATAR_JOINT_RIGHT_HIP: - secondJointID = AVATAR_JOINT_LEFT_HIP; - derivedJointID = AVATAR_JOINT_PELVIS; - break; - - case AVATAR_JOINT_RIGHT_HEEL: - derivedJointID = AVATAR_JOINT_RIGHT_TOES; - break; - } - if (derivedJointID != AVATAR_JOINT_NULL) { - if (secondJointID == AVATAR_JOINT_NULL) { - _skeleton.joint[derivedJointID].position = _skeleton.joint[it->jointID].position; - _skeleton.joint[derivedJointID].absoluteRotation = _skeleton.joint[it->jointID].absoluteRotation; - - } else { - _skeleton.joint[derivedJointID].position = glm::mix(_skeleton.joint[it->jointID].position, - _skeleton.joint[secondJointID].position, proportion); - _skeleton.joint[derivedJointID].absoluteRotation = safeMix(_skeleton.joint[it->jointID].absoluteRotation, - _skeleton.joint[secondJointID].absoluteRotation, proportion); - } - } + // disable hand movement if we have joint info for the right wrist + enableHandMovement &= (it->jointID != AVATAR_JOINT_RIGHT_WRIST); } + // update avatar skeleton + _skeleton.update(deltaTime, getOrientation(), _position); + //determine the lengths of the body springs now that we have updated the skeleton at least once if (!_ballSpringsInitialized) { for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) { @@ -592,7 +538,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { } //update the movement of the hand and process handshaking with other avatars... - updateHandMovementAndTouching(deltaTime); + updateHandMovementAndTouching(deltaTime, enableHandMovement); _avatarTouch.simulate(deltaTime); // apply gravity and collision with the ground/floor @@ -772,7 +718,7 @@ void Avatar::setOrientation(const glm::quat& orientation) { _bodyRoll = eulerAngles.z; } -void Avatar::updateHandMovementAndTouching(float deltaTime) { +void Avatar::updateHandMovementAndTouching(float deltaTime, bool enableHandMovement) { glm::quat orientation = getOrientation(); @@ -781,12 +727,14 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) { glm::vec3 up = orientation * IDENTITY_UP; glm::vec3 front = orientation * IDENTITY_FRONT; - glm::vec3 transformedHandMovement - = right * _movedHandOffset.x * 2.0f - + up * -_movedHandOffset.y * 2.0f - + front * -_movedHandOffset.y * 2.0f; + if (enableHandMovement) { + glm::vec3 transformedHandMovement = + right * _movedHandOffset.x * 2.0f + + up * -_movedHandOffset.y * 2.0f + + front * -_movedHandOffset.y * 2.0f; - _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position += transformedHandMovement; + _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position += transformedHandMovement; + } if (isMyAvatar()) { _avatarTouch.setMyBodyPosition(_position); @@ -877,7 +825,9 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) { //constrain right arm length and re-adjust elbow position as it bends // NOTE - the following must be called on all avatars - not just _isMine - updateArmIKAndConstraints(deltaTime); + if (enableHandMovement) { + updateArmIKAndConstraints(deltaTime); + } //Set right hand position and state to be transmitted, and also tell AvatarTouch about it if (isMyAvatar()) { diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index fe8f867f18..cb26500e71 100755 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -229,7 +229,7 @@ private: void updateBodyBalls( float deltaTime ); void calculateBoneLengths(); void readSensors(); - void updateHandMovementAndTouching(float deltaTime); + void updateHandMovementAndTouching(float deltaTime, bool enableHandMovement); void updateAvatarCollisions(float deltaTime); void updateArmIKAndConstraints( float deltaTime ); void updateCollisionWithSphere( glm::vec3 position, float radius, float deltaTime ); diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index 44eddeeb93..934cafae91 100644 --- a/interface/src/Head.cpp +++ b/interface/src/Head.cpp @@ -184,9 +184,10 @@ void Head::simulate(float deltaTime, bool isMine) { const float FULLY_CLOSED = 1.0f; if (_leftEyeBlinkVelocity == 0.0f && _rightEyeBlinkVelocity == 0.0f) { // no blinking when brows are raised; blink less with increasing loudness - const float ROOT_LOUDNESS_TO_BLINK_INTERVAL = 0.75f; - if (forceBlink || (_browAudioLift < EPSILON && shouldDo( - sqrtf(_averageLoudness) * ROOT_LOUDNESS_TO_BLINK_INTERVAL, deltaTime))) { + const float BASE_BLINK_RATE = 15.0f / 60.0f; + const float ROOT_LOUDNESS_TO_BLINK_INTERVAL = 0.25f; + if (forceBlink || (_browAudioLift < EPSILON && shouldDo(glm::max(1.0f, sqrt(_averageLoudness) * + ROOT_LOUDNESS_TO_BLINK_INTERVAL) / BASE_BLINK_RATE, deltaTime))) { _leftEyeBlinkVelocity = BLINK_SPEED; _rightEyeBlinkVelocity = BLINK_SPEED; } diff --git a/interface/src/Webcam.cpp b/interface/src/Webcam.cpp index 7079f0af4d..d4fde500bd 100644 --- a/interface/src/Webcam.cpp +++ b/interface/src/Webcam.cpp @@ -208,13 +208,20 @@ void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const Rota // see if we have joint data if (!_joints.isEmpty()) { _estimatedJoints.resize(NUM_AVATAR_JOINTS); + glm::vec3 origin; + if (_joints[AVATAR_JOINT_LEFT_HIP].isValid && _joints[AVATAR_JOINT_RIGHT_HIP].isValid) { + origin = glm::mix(_joints[AVATAR_JOINT_LEFT_HIP].position, _joints[AVATAR_JOINT_RIGHT_HIP].position, 0.5f); + + } else if (_joints[AVATAR_JOINT_TORSO].isValid) { + origin = _joints[AVATAR_JOINT_TORSO].position + glm::vec3(0.0f, -0.09f, -0.01f); + } for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { if (!_joints[i].isValid) { continue; } const float JOINT_SMOOTHING = 0.95f; _estimatedJoints[i].isValid = true; - _estimatedJoints[i].position = glm::mix(_joints[i].position - joints[AVATAR_JOINT_TORSO].position, + _estimatedJoints[i].position = glm::mix(_joints[i].position - origin, _estimatedJoints[i].position, JOINT_SMOOTHING); _estimatedJoints[i].orientation = safeMix(_joints[i].orientation, _estimatedJoints[i].orientation, JOINT_SMOOTHING); @@ -296,6 +303,27 @@ static AvatarJointID xnToAvatarJoint(XnSkeletonJoint joint) { } } +static int getParentJoint(XnSkeletonJoint joint) { + switch (joint) { + case XN_SKEL_HEAD: return XN_SKEL_NECK; + case XN_SKEL_TORSO: return -1; + + case XN_SKEL_LEFT_ELBOW: return XN_SKEL_LEFT_SHOULDER; + case XN_SKEL_LEFT_HAND: return XN_SKEL_LEFT_ELBOW; + + case XN_SKEL_RIGHT_ELBOW: return XN_SKEL_RIGHT_SHOULDER; + case XN_SKEL_RIGHT_HAND: return XN_SKEL_RIGHT_ELBOW; + + case XN_SKEL_LEFT_KNEE: return XN_SKEL_LEFT_HIP; + case XN_SKEL_LEFT_FOOT: return XN_SKEL_LEFT_KNEE; + + case XN_SKEL_RIGHT_KNEE: return XN_SKEL_RIGHT_HIP; + case XN_SKEL_RIGHT_FOOT: return XN_SKEL_RIGHT_KNEE; + + default: return XN_SKEL_TORSO; + } +} + static glm::vec3 xnToGLM(const XnVector3D& vector, bool flip = false) { return glm::vec3(vector.X * (flip ? -1 : 1), vector.Y, vector.Z); } @@ -305,7 +333,7 @@ static glm::quat xnToGLM(const XnMatrix3X3& matrix) { matrix.elements[0], matrix.elements[3], matrix.elements[6], matrix.elements[1], matrix.elements[4], matrix.elements[7], matrix.elements[2], matrix.elements[5], matrix.elements[8])); - return glm::quat(rotation.w, rotation.x, rotation.y, rotation.z); + return glm::quat(rotation.w, rotation.x, rotation.y, -rotation.z); } static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) { @@ -344,6 +372,8 @@ void FrameGrabber::reset() { #endif } + + void FrameGrabber::grabFrame() { if (!(_initialized || init())) { return; @@ -377,11 +407,27 @@ void FrameGrabber::grabFrame() { continue; } _userGenerator.GetSkeletonCap().GetSkeletonJoint(_userID, activeJoints[i], transform); + glm::quat rotation = xnToGLM(transform.orientation.orientation); + int parent = getParentJoint(activeJoints[i]); + if (parent != -1) { + XnSkeletonJointOrientation parentOrientation; + _userGenerator.GetSkeletonCap().GetSkeletonJointOrientation( + _userID, (XnSkeletonJoint)parent, parentOrientation); + if (i == XN_SKEL_TORSO) { + glm::vec3 eulers = safeEulerAngles(rotation); + printLog("a: %g %g %g\n", eulers.x, eulers.y, eulers.z); + } + rotation = glm::inverse(xnToGLM(parentOrientation.orientation)) * rotation; + if (i == XN_SKEL_TORSO) { + glm::vec3 eulers = safeEulerAngles(rotation); + printLog("r: %g %g %g\n", eulers.x, eulers.y, eulers.z); + } + } XnVector3D projected; _depthGenerator.ConvertRealWorldToProjective(1, &transform.position.position, &projected); const float METERS_PER_MM = 1.0f / 1000.0f; joints[avatarJoint] = Joint(xnToGLM(transform.position.position, true) * METERS_PER_MM, - xnToGLM(transform.orientation.orientation), xnToGLM(projected)); + rotation, xnToGLM(projected)); } } } @@ -463,7 +509,7 @@ bool FrameGrabber::init() { _userGenerator.GetSkeletonCap().RegisterToCalibrationStart(calibrationStarted, 0, calibrationStartCallback); _userGenerator.GetSkeletonCap().RegisterToCalibrationComplete(calibrationCompleted, 0, calibrationCompleteCallback); - _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL); + _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_UPPER); _xnContext.StartGeneratingAll(); return true; diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index bd3b6a357a..20fc780a9b 100755 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -152,6 +152,13 @@ int AvatarData::getBroadcastData(unsigned char* destinationBuffer) { } } + // skeleton joints + *destinationBuffer++ = (unsigned char)_joints.size(); + for (vector::iterator it = _joints.begin(); it != _joints.end(); it++) { + *destinationBuffer++ = (unsigned char)it->jointID; + destinationBuffer += packOrientationQuatToBytes(destinationBuffer, it->rotation); + } + return destinationBuffer - bufferStart; } @@ -263,6 +270,16 @@ int AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) { _handData->setFingerRoots(fingerRoots); } + // skeleton joints + if (sourceBuffer - startPosition < numBytes) // safety check + { + _joints.resize(*sourceBuffer++); + for (vector::iterator it = _joints.begin(); it != _joints.end(); it++) { + it->jointID = *sourceBuffer++; + sourceBuffer += unpackOrientationQuatFromBytes(sourceBuffer, it->rotation); + } + } + return sourceBuffer - startPosition; } diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 90d437f253..012f0c97a0 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -150,7 +150,6 @@ class JointData { public: int jointID; - glm::vec3 position; glm::quat rotation; };