From 9c977450e36e0f2b1e9e0a478aff48f4fc13a1dd Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Fri, 5 Jul 2013 22:14:32 -0700 Subject: [PATCH] Finally got the rotations figured out; I had forgotten the default pose/bind pose distinction. --- interface/src/Avatar.cpp | 7 +---- interface/src/Skeleton.cpp | 49 ++++++++++-------------------- interface/src/Skeleton.h | 1 - interface/src/Webcam.cpp | 61 +++++++++++++++++++++++++++----------- 4 files changed, 60 insertions(+), 58 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 6323820f65..27ccf0db4b 100755 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -499,12 +499,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { // 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; - - AvatarJointID parent = _skeleton.joint[it->jointID].parent; - const glm::quat& parentRotation = (parent == AVATAR_JOINT_NULL) ? - orientation : _skeleton.joint[parent].absoluteRotation; - _skeleton.joint[it->jointID].rotation = glm::inverse(parentRotation) * _skeleton.joint[it->jointID].absoluteRotation; + _skeleton.joint[it->jointID].rotation = it->rotation; // disable hand movement if we have joint info for the right wrist enableHandMovement &= (it->jointID != AVATAR_JOINT_RIGHT_WRIST); diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp index 5bf8538109..0d166abfa1 100644 --- a/interface/src/Skeleton.cpp +++ b/interface/src/Skeleton.cpp @@ -6,6 +6,7 @@ #include "Skeleton.h" #include "Util.h" +#include "world.h" const float BODY_SPRING_DEFAULT_TIGHTNESS = 1000.0f; const float FLOATING_HEIGHT = 0.13f; @@ -18,12 +19,21 @@ void Skeleton::initialize() { for (int b = 0; b < NUM_AVATAR_JOINTS; b++) { joint[b].parent = AVATAR_JOINT_NULL; joint[b].position = glm::vec3(0.0, 0.0, 0.0); - joint[b].defaultPosePosition = glm::vec3(0.0, 0.0, 0.0); joint[b].rotation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f); joint[b].length = 0.0; joint[b].bindRadius = 1.0f / 8; } + // put the arms at the side + joint[AVATAR_JOINT_LEFT_ELBOW].rotation = glm::quat(glm::vec3(0.0f, 0.0f, PIf * 0.5f)); + joint[AVATAR_JOINT_RIGHT_ELBOW].rotation = glm::quat(glm::vec3(0.0f, 0.0f, -PIf * 0.5f)); + + // bend the knees + joint[AVATAR_JOINT_LEFT_KNEE].rotation = joint[AVATAR_JOINT_RIGHT_KNEE].rotation = + glm::quat(glm::vec3(PIf / 8.0f, 0.0f, 0.0f)); + joint[AVATAR_JOINT_LEFT_HEEL].rotation = joint[AVATAR_JOINT_RIGHT_HEEL].rotation = + glm::quat(glm::vec3(-PIf / 4.0f, 0.0f, 0.0f)); + // specify the parental hierarchy joint[ AVATAR_JOINT_PELVIS ].parent = AVATAR_JOINT_NULL; joint[ AVATAR_JOINT_TORSO ].parent = AVATAR_JOINT_PELVIS; @@ -80,39 +90,9 @@ void Skeleton::initialize() { joint[ AVATAR_JOINT_RIGHT_HEEL ].bindPosePosition = glm::vec3( 0.00, -0.23, 0.00 ); joint[ AVATAR_JOINT_RIGHT_TOES ].bindPosePosition = glm::vec3( 0.00, 0.00, -0.06 ); - // specify the default pose position - joint[ AVATAR_JOINT_PELVIS ].defaultPosePosition = glm::vec3( 0.0, 0.0, 0.0 ); - joint[ AVATAR_JOINT_TORSO ].defaultPosePosition = glm::vec3( 0.0, 0.09, -0.01 ); - 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 ); - joint[ AVATAR_JOINT_LEFT_ELBOW ].defaultPosePosition = glm::vec3( 0.0, -0.16, 0.0 ); - joint[ AVATAR_JOINT_LEFT_WRIST ].defaultPosePosition = glm::vec3( 0.0, -0.117, 0.0 ); - joint[ AVATAR_JOINT_LEFT_FINGERTIPS ].defaultPosePosition = glm::vec3( 0.0, -0.1, 0.0 ); - - joint[ AVATAR_JOINT_RIGHT_COLLAR ].defaultPosePosition = glm::vec3( 0.06, 0.04, 0.01 ); - joint[ AVATAR_JOINT_RIGHT_SHOULDER ].defaultPosePosition = glm::vec3( 0.05, 0.0, 0.01 ); - joint[ AVATAR_JOINT_RIGHT_ELBOW ].defaultPosePosition = glm::vec3( 0.0, -0.16, 0.0 ); - joint[ AVATAR_JOINT_RIGHT_WRIST ].defaultPosePosition = glm::vec3( 0.0, -0.117, 0.0 ); - joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].defaultPosePosition = glm::vec3( 0.0, -0.1, 0.0 ); - - joint[ AVATAR_JOINT_LEFT_HIP ].defaultPosePosition = glm::vec3( -0.05, 0.0, 0.02 ); - joint[ AVATAR_JOINT_LEFT_KNEE ].defaultPosePosition = glm::vec3( 0.01, -0.25, -0.03 ); - joint[ AVATAR_JOINT_LEFT_HEEL ].defaultPosePosition = glm::vec3( 0.01, -0.22, 0.08 ); - joint[ AVATAR_JOINT_LEFT_TOES ].defaultPosePosition = glm::vec3( 0.00, -0.03, -0.05 ); - - joint[ AVATAR_JOINT_RIGHT_HIP ].defaultPosePosition = glm::vec3( 0.05, 0.0, 0.02 ); - joint[ AVATAR_JOINT_RIGHT_KNEE ].defaultPosePosition = glm::vec3( -0.01, -0.25, -0.03 ); - joint[ AVATAR_JOINT_RIGHT_HEEL ].defaultPosePosition = glm::vec3( -0.01, -0.22, 0.08 ); - joint[ AVATAR_JOINT_RIGHT_TOES ].defaultPosePosition = glm::vec3( 0.00, -0.03, -0.05 ); - // calculate bone length, absolute bind positions/rotations for (int b = 0; b < NUM_AVATAR_JOINTS; b++) { - joint[b].length = glm::length(joint[b].defaultPosePosition); + joint[b].length = glm::length(joint[b].bindPosePosition); if (joint[b].parent == AVATAR_JOINT_NULL) { joint[b].absoluteBindPosePosition = joint[b].bindPosePosition; @@ -121,7 +101,8 @@ void Skeleton::initialize() { joint[b].absoluteBindPosePosition = joint[ joint[b].parent ].absoluteBindPosePosition + joint[b].bindPosePosition; glm::vec3 parentDirection = joint[ joint[b].parent ].absoluteBindPoseRotation * JOINT_DIRECTION; - joint[b].absoluteBindPoseRotation = joint[ joint[b].parent ].absoluteBindPoseRotation; + joint[b].absoluteBindPoseRotation = rotationBetween(parentDirection, joint[b].bindPosePosition) * + joint[ joint[b].parent ].absoluteBindPoseRotation; } } } @@ -139,7 +120,7 @@ void Skeleton::update(float deltaTime, const glm::quat& orientation, glm::vec3 p joint[b].position = joint[ joint[b].parent ].position; } - glm::vec3 rotatedJointVector = joint[b].absoluteRotation * joint[b].defaultPosePosition; + glm::vec3 rotatedJointVector = joint[b].absoluteRotation * joint[b].bindPosePosition; joint[b].position += rotatedJointVector; } } diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h index bb953fe947..8d99a80fa6 100644 --- a/interface/src/Skeleton.h +++ b/interface/src/Skeleton.h @@ -63,7 +63,6 @@ public: { AvatarJointID parent; // which joint is this joint connected to? glm::vec3 position; // the position at the "end" of the joint - in global space - glm::vec3 defaultPosePosition; // the parent relative position when the avatar is in the default pose glm::vec3 bindPosePosition; // the parent relative position when the avatar is in the "T-pose" glm::vec3 absoluteBindPosePosition; // the absolute position when the avatar is in the "T-pose" glm::quat absoluteBindPoseRotation; // the absolute rotation when the avatar is in the "T-pose" diff --git a/interface/src/Webcam.cpp b/interface/src/Webcam.cpp index fd44b54c4a..a81bec8ddd 100644 --- a/interface/src/Webcam.cpp +++ b/interface/src/Webcam.cpp @@ -219,7 +219,7 @@ void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const Rota if (!_joints[i].isValid) { continue; } - const float JOINT_SMOOTHING = 0.95f; + const float JOINT_SMOOTHING = 0.5f; _estimatedJoints[i].isValid = true; _estimatedJoints[i].position = glm::mix(_joints[i].position - origin, _estimatedJoints[i].position, JOINT_SMOOTHING); @@ -279,30 +279,49 @@ FrameGrabber::~FrameGrabber() { #ifdef HAVE_OPENNI static AvatarJointID xnToAvatarJoint(XnSkeletonJoint joint) { switch (joint) { - case XN_SKEL_HEAD: return AVATAR_JOINT_HEAD_BASE; - case XN_SKEL_NECK: return AVATAR_JOINT_NECK_BASE; - case XN_SKEL_TORSO: return AVATAR_JOINT_TORSO; + case XN_SKEL_HEAD: return AVATAR_JOINT_HEAD_TOP; + case XN_SKEL_NECK: return AVATAR_JOINT_HEAD_BASE; + case XN_SKEL_TORSO: return AVATAR_JOINT_CHEST; - case XN_SKEL_LEFT_SHOULDER: return AVATAR_JOINT_RIGHT_COLLAR; - case XN_SKEL_LEFT_ELBOW: return AVATAR_JOINT_RIGHT_SHOULDER; - case XN_SKEL_LEFT_HAND: return AVATAR_JOINT_RIGHT_ELBOW; + case XN_SKEL_LEFT_SHOULDER: return AVATAR_JOINT_RIGHT_ELBOW; + case XN_SKEL_LEFT_ELBOW: return AVATAR_JOINT_RIGHT_WRIST; - case XN_SKEL_RIGHT_SHOULDER: return AVATAR_JOINT_LEFT_COLLAR; - case XN_SKEL_RIGHT_ELBOW: return AVATAR_JOINT_LEFT_SHOULDER; - case XN_SKEL_RIGHT_HAND: return AVATAR_JOINT_LEFT_ELBOW; + case XN_SKEL_RIGHT_SHOULDER: return AVATAR_JOINT_LEFT_ELBOW; + case XN_SKEL_RIGHT_ELBOW: return AVATAR_JOINT_LEFT_WRIST; - case XN_SKEL_LEFT_HIP: return AVATAR_JOINT_RIGHT_HIP; - case XN_SKEL_LEFT_KNEE: return AVATAR_JOINT_RIGHT_KNEE; - case XN_SKEL_LEFT_FOOT: return AVATAR_JOINT_RIGHT_HEEL; + case XN_SKEL_LEFT_HIP: return AVATAR_JOINT_RIGHT_KNEE; + case XN_SKEL_LEFT_KNEE: return AVATAR_JOINT_RIGHT_HEEL; + case XN_SKEL_LEFT_FOOT: return AVATAR_JOINT_RIGHT_TOES; - case XN_SKEL_RIGHT_HIP: return AVATAR_JOINT_LEFT_HIP; - case XN_SKEL_RIGHT_KNEE: return AVATAR_JOINT_LEFT_KNEE; - case XN_SKEL_RIGHT_FOOT: return AVATAR_JOINT_LEFT_HEEL; + case XN_SKEL_RIGHT_HIP: return AVATAR_JOINT_LEFT_KNEE; + case XN_SKEL_RIGHT_KNEE: return AVATAR_JOINT_LEFT_HEEL; + case XN_SKEL_RIGHT_FOOT: return AVATAR_JOINT_LEFT_TOES; default: return AVATAR_JOINT_NULL; } } +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); } @@ -386,9 +405,17 @@ void FrameGrabber::grabFrame() { _userGenerator.GetSkeletonCap().GetSkeletonJoint(_userID, activeJoints[i], transform); XnVector3D projected; _depthGenerator.ConvertRealWorldToProjective(1, &transform.position.position, &projected); + glm::quat rotation = xnToGLM(transform.orientation.orientation); + int parentJoint = getParentJoint(activeJoints[i]); + if (parentJoint != -1) { + XnSkeletonJointOrientation parentOrientation; + _userGenerator.GetSkeletonCap().GetSkeletonJointOrientation( + _userID, (XnSkeletonJoint)parentJoint, parentOrientation); + rotation = glm::inverse(xnToGLM(parentOrientation.orientation)) * rotation; + } 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)); } } }