diff --git a/cmake/modules/FindOpenNI.cmake b/cmake/modules/FindOpenNI.cmake new file mode 100644 index 0000000000..111a33dfbe --- /dev/null +++ b/cmake/modules/FindOpenNI.cmake @@ -0,0 +1,44 @@ +# Find the OpenNI library +# +# You must provide an OPENNI_ROOT_DIR which contains lib and include directories +# +# Once done this will define +# +# OPENNI_FOUND - system found OpenNI +# OPENNI_INCLUDE_DIRS - the OpenNI include directory +# OPENNI_LIBRARIES - Link this to use OpenNI +# +# Created on 6/28/2013 by Andrzej Kapolka +# Copyright (c) 2013 High Fidelity +# + +if (OPENNI_LIBRARIES AND OPENNI_INCLUDE_DIRS) + # in cache already + set(OPENNI_FOUND TRUE) +else (OPENNI_LIBRARIES AND OPENNI_INCLUDE_DIRS) + find_path(OPENNI_INCLUDE_DIRS XnOpenNI.h /usr/include/ni) + + if (APPLE) + find_library(OPENNI_LIBRARIES libOpenNI.dylib /usr/lib) + elseif (UNIX) + find_library(OPENNI_LIBRARIES libOpenNI.so /usr/lib) + endif () + + if (OPENNI_INCLUDE_DIRS AND OPENNI_LIBRARIES) + set(OPENNI_FOUND TRUE) + endif (OPENNI_INCLUDE_DIRS AND OPENNI_LIBRARIES) + + if (OPENNI_FOUND) + if (NOT OPENNI_FIND_QUIETLY) + message(STATUS "Found OpenNI: ${OPENNI_LIBRARIES}") + endif (NOT OPENNI_FIND_QUIETLY) + else (OPENNI_FOUND) + if (OPENNI_FIND_REQUIRED) + message(FATAL_ERROR "Could not find OpenNI") + endif (OPENNI_FIND_REQUIRED) + endif (OPENNI_FOUND) + + # show the OPENNI_INCLUDE_DIRS and OPENNI_LIBRARIES variables only in the advanced view + mark_as_advanced(OPENNI_INCLUDE_DIRS OPENNI_LIBRARIES) + +endif (OPENNI_LIBRARIES AND OPENNI_INCLUDE_DIRS) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index 5de01a50bc..cccb9b96b0 100755 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -95,6 +95,15 @@ find_package(Leap) find_package(OpenCV) find_package(ZLIB) find_package(UVCCameraControl) +find_package(OpenNI) + +# let the source know that we have OpenNI/NITE for Kinect +if (OPENNI_FOUND) + add_definitions(-DHAVE_OPENNI) + include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${OPENNI_INCLUDE_DIRS}") + target_link_libraries(${TARGET_NAME} ${OPENNI_LIBRARIES}) +endif (OPENNI_FOUND) # include headers for interface and InterfaceConfig. include_directories( diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index e27b6c3465..7ee0488f42 100755 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -955,14 +955,17 @@ void Application::idle() { // Only run simulation code if more than IDLE_SIMULATE_MSECS have passed since last time we ran if (diffclock(&_lastTimeIdle, &check) > IDLE_SIMULATE_MSECS) { - // We call processEvents() here because the idle timer takes priority over - // event handling in Qt, so when the framerate gets low events will pile up - // unless we handle them here. - // NOTE - this is commented out for now - causing event processing issues reported by Philip and Ryan - // birarda - July 3rd - - // processEvents(); + // If we're using multi-touch look, immediately process any + // touch events, and no other events. + // This is necessary because id the idle() call takes longer than the + // interval between idle() calls, the event loop never gets to run, + // and touch events get delayed. + if (_touchLook->isChecked()) { + sendPostedEvents(NULL, QEvent::TouchBegin); + sendPostedEvents(NULL, QEvent::TouchUpdate); + sendPostedEvents(NULL, QEvent::TouchEnd); + } update(1.0f / _fps); @@ -1930,11 +1933,11 @@ void Application::update(float deltaTime) { void Application::updateAvatar(float deltaTime) { - // Update my avatar's head position from gyros and/or webcam - _myAvatar.updateHeadFromGyrosAndOrWebcam(_gyroLook->isChecked(), - glm::vec3(_headCameraPitchYawScale, - _headCameraPitchYawScale, - _headCameraPitchYawScale)); + // Update my avatar's state from gyros and/or webcam + _myAvatar.updateFromGyrosAndOrWebcam(_gyroLook->isChecked(), + glm::vec3(_headCameraPitchYawScale, + _headCameraPitchYawScale, + _headCameraPitchYawScale)); if (_serialHeadSensor.isActive()) { diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 2c94b134f3..c2c850c923 100755 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -148,121 +148,121 @@ 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; + //_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); + //_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_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_FINGERTIPS].isCollidable = false; + _bodyBall[ BODY_BALL_RIGHT_ELBOW ].isCollidable = false; + _bodyBall[ BODY_BALL_RIGHT_WRIST ].isCollidable = false; + _bodyBall[ BODY_BALL_RIGHT_FINGERTIPS ].isCollidable = false; */ } @@ -285,24 +285,37 @@ void Avatar::reset() { } // Update avatar head rotation with sensor data -void Avatar::updateHeadFromGyrosAndOrWebcam(bool gyroLook, const glm::vec3& amplifyAngle) { - +void Avatar::updateFromGyrosAndOrWebcam(bool gyroLook, const glm::vec3& amplifyAngle) { SerialInterface* gyros = Application::getInstance()->getSerialHeadSensor(); Webcam* webcam = Application::getInstance()->getWebcam(); glm::vec3 estimatedPosition, estimatedRotation; if (gyros->isActive()) { - if (webcam->isActive()) { - estimatedPosition = webcam->getEstimatedPosition(); - } estimatedRotation = gyros->getEstimatedRotation(); - + } else if (webcam->isActive()) { - estimatedPosition = webcam->getEstimatedPosition(); estimatedRotation = webcam->getEstimatedRotation(); } else { return; } + if (webcam->isActive()) { + estimatedPosition = webcam->getEstimatedPosition(); + + // compute and store the joint rotations + const JointVector& joints = webcam->getEstimatedJoints(); + _joints.clear(); + for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { + if (joints.size() > i && joints[i].isValid) { + JointData data = { i, joints[i].rotation }; + _joints.push_back(data); + + if (i == AVATAR_JOINT_CHEST) { + // if we have a chest rotation, don't apply lean based on head + estimatedPosition = glm::vec3(); + } + } + } + } _head.setPitch(estimatedRotation.x * amplifyAngle.x); _head.setYaw(estimatedRotation.y * amplifyAngle.y); _head.setRoll(estimatedRotation.z * amplifyAngle.z); @@ -485,9 +498,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].rotation = it->rotation; + + // 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++) { @@ -518,7 +540,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 @@ -698,7 +720,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(); @@ -707,12 +729,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); @@ -803,7 +827,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 8b6c79bd7c..855d49e51c 100755 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -87,7 +87,7 @@ public: void reset(); void simulate(float deltaTime, Transmitter* transmitter); void updateThrust(float deltaTime, Transmitter * transmitter); - void updateHeadFromGyrosAndOrWebcam(bool gyroLook, const glm::vec3& amplifyAngles); + void updateFromGyrosAndOrWebcam(bool gyroLook, const glm::vec3& amplifyAngles); void updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight); void updateFromTouch(float touchAvgDistX, float touchAvgDistY); void addBodyYaw(float y) {_bodyYaw += y;}; @@ -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/Skeleton.cpp b/interface/src/Skeleton.cpp index 1167194534..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; @@ -122,7 +102,7 @@ void Skeleton::initialize() { joint[b].bindPosePosition; glm::vec3 parentDirection = joint[ joint[b].parent ].absoluteBindPoseRotation * JOINT_DIRECTION; joint[b].absoluteBindPoseRotation = rotationBetween(parentDirection, joint[b].bindPosePosition) * - joint[ joint[b].parent ].absoluteBindPoseRotation; + joint[ joint[b].parent ].absoluteBindPoseRotation; } } } @@ -140,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 1b370f6c4b..6c7006c5f4 100644 --- a/interface/src/Webcam.cpp +++ b/interface/src/Webcam.cpp @@ -21,11 +21,16 @@ using namespace cv; using namespace std; -// register OpenCV matrix type with Qt metatype system +#ifdef HAVE_OPENNI +using namespace xn; +#endif + +// register types with Qt metatype system +int jointVectorMetaType = qRegisterMetaType("JointVector"); int matMetaType = qRegisterMetaType("cv::Mat"); int rotatedRectMetaType = qRegisterMetaType("cv::RotatedRect"); -Webcam::Webcam() : _enabled(false), _active(false), _frameTextureID(0) { +Webcam::Webcam() : _enabled(false), _active(false), _frameTextureID(0), _depthTextureID(0) { // the grabber simply runs as fast as possible _grabber = new FrameGrabber(); _grabber->moveToThread(&_grabberThread); @@ -79,9 +84,45 @@ void Webcam::renderPreview(int screenWidth, int screenHeight) { glTexCoord2f(0, 1); glVertex2f(left, top + PREVIEW_HEIGHT); glEnd(); - glBindTexture(GL_TEXTURE_2D, 0); - glDisable(GL_TEXTURE_2D); + if (_depthTextureID != 0) { + glBindTexture(GL_TEXTURE_2D, _depthTextureID); + glBegin(GL_QUADS); + int depthPreviewWidth = _depthWidth * PREVIEW_HEIGHT / _depthHeight; + int depthLeft = screenWidth - depthPreviewWidth - 10; + glTexCoord2f(0, 0); + glVertex2f(depthLeft, top - PREVIEW_HEIGHT); + glTexCoord2f(1, 0); + glVertex2f(depthLeft + depthPreviewWidth, top - PREVIEW_HEIGHT); + glTexCoord2f(1, 1); + glVertex2f(depthLeft + depthPreviewWidth, top); + glTexCoord2f(0, 1); + glVertex2f(depthLeft, top); + glEnd(); + + glBindTexture(GL_TEXTURE_2D, 0); + glDisable(GL_TEXTURE_2D); + + if (!_joints.isEmpty()) { + glColor3f(1.0f, 0.0f, 0.0f); + glPointSize(4.0f); + glBegin(GL_POINTS); + float projectedScale = PREVIEW_HEIGHT / (float)_depthHeight; + foreach (const Joint& joint, _joints) { + if (joint.isValid) { + glVertex2f(depthLeft + joint.projected.x * projectedScale, + top - PREVIEW_HEIGHT + joint.projected.y * projectedScale); + } + } + glEnd(); + glPointSize(1.0f); + } + } else { + glBindTexture(GL_TEXTURE_2D, 0); + glDisable(GL_TEXTURE_2D); + } + + glColor3f(1.0f, 1.0f, 1.0f); glBegin(GL_LINE_LOOP); Point2f facePoints[4]; _faceRect.points(facePoints); @@ -107,26 +148,45 @@ Webcam::~Webcam() { delete _grabber; } -void Webcam::setFrame(const Mat& frame, const RotatedRect& faceRect) { +void Webcam::setFrame(const Mat& frame, int format, const Mat& depth, const RotatedRect& faceRect, const JointVector& joints) { IplImage image = frame; glPixelStorei(GL_UNPACK_ROW_LENGTH, image.widthStep / 3); if (_frameTextureID == 0) { glGenTextures(1, &_frameTextureID); glBindTexture(GL_TEXTURE_2D, _frameTextureID); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, _frameWidth = image.width, _frameHeight = image.height, 0, GL_BGR, + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, _frameWidth = image.width, _frameHeight = image.height, 0, format, GL_UNSIGNED_BYTE, image.imageData); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - printLog("Capturing webcam at %dx%d.\n", _frameWidth, _frameHeight); + printLog("Capturing video at %dx%d.\n", _frameWidth, _frameHeight); } else { glBindTexture(GL_TEXTURE_2D, _frameTextureID); - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _frameWidth, _frameHeight, GL_BGR, GL_UNSIGNED_BYTE, image.imageData); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _frameWidth, _frameHeight, format, GL_UNSIGNED_BYTE, image.imageData); + } + + if (!depth.empty()) { + IplImage depthImage = depth; + glPixelStorei(GL_UNPACK_ROW_LENGTH, depthImage.widthStep); + if (_depthTextureID == 0) { + glGenTextures(1, &_depthTextureID); + glBindTexture(GL_TEXTURE_2D, _depthTextureID); + glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, _depthWidth = depthImage.width, _depthHeight = depthImage.height, 0, + GL_LUMINANCE, GL_UNSIGNED_BYTE, depthImage.imageData); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + printLog("Capturing depth at %dx%d.\n", _depthWidth, _depthHeight); + + } else { + glBindTexture(GL_TEXTURE_2D, _depthTextureID); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _depthWidth, _depthHeight, GL_LUMINANCE, + GL_UNSIGNED_BYTE, depthImage.imageData); + } } glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); glBindTexture(GL_TEXTURE_2D, 0); - // store our face rect, update our frame count for fps computation + // store our face rect and joints, update our frame count for fps computation _faceRect = faceRect; + _joints = joints; _frameCount++; const int MAX_FPS = 60; @@ -140,33 +200,60 @@ void Webcam::setFrame(const Mat& frame, const RotatedRect& faceRect) { } _lastFrameTimestamp = now; - // roll is just the angle of the face rect (correcting for 180 degree rotations) - float roll = faceRect.angle; - if (roll < -90.0f) { - roll += 180.0f; + // 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) { + const glm::vec3 TORSO_TO_PELVIS = glm::vec3(0.0f, -0.09f, -0.01f); + origin = _joints[AVATAR_JOINT_TORSO].position + TORSO_TO_PELVIS; + } + for (int i = 0; i < NUM_AVATAR_JOINTS; i++) { + if (!_joints[i].isValid) { + continue; + } + const float JOINT_SMOOTHING = 0.9f; + _estimatedJoints[i].isValid = true; + _estimatedJoints[i].position = glm::mix(_joints[i].position - origin, + _estimatedJoints[i].position, JOINT_SMOOTHING); + _estimatedJoints[i].rotation = safeMix(_joints[i].rotation, + _estimatedJoints[i].rotation, JOINT_SMOOTHING); + } + _estimatedRotation = safeEulerAngles(_estimatedJoints[AVATAR_JOINT_HEAD_BASE].rotation); + _estimatedPosition = _estimatedJoints[AVATAR_JOINT_HEAD_BASE].position; - } else if (roll > 90.0f) { - roll -= 180.0f; - } - const float ROTATION_SMOOTHING = 0.95f; - _estimatedRotation.z = glm::mix(roll, _estimatedRotation.z, ROTATION_SMOOTHING); - - // determine position based on translation and scaling of the face rect - if (_initialFaceRect.size.area() == 0) { - _initialFaceRect = faceRect; - _estimatedPosition = glm::vec3(); - } else { - float proportion = sqrtf(_initialFaceRect.size.area() / (float)faceRect.size.area()); - const float DISTANCE_TO_CAMERA = 0.333f; - const float POSITION_SCALE = 0.5f; - float z = DISTANCE_TO_CAMERA * proportion - DISTANCE_TO_CAMERA; - glm::vec3 position = glm::vec3( - (faceRect.center.x - _initialFaceRect.center.x) * proportion * POSITION_SCALE / _frameWidth, - (faceRect.center.y - _initialFaceRect.center.y) * proportion * POSITION_SCALE / _frameWidth, - z); - const float POSITION_SMOOTHING = 0.95f; - _estimatedPosition = glm::mix(position, _estimatedPosition, POSITION_SMOOTHING); + // roll is just the angle of the face rect (correcting for 180 degree rotations) + float roll = faceRect.angle; + if (roll < -90.0f) { + roll += 180.0f; + + } else if (roll > 90.0f) { + roll -= 180.0f; + } + const float ROTATION_SMOOTHING = 0.95f; + _estimatedRotation.z = glm::mix(roll, _estimatedRotation.z, ROTATION_SMOOTHING); + + // determine position based on translation and scaling of the face rect + if (_initialFaceRect.size.area() == 0) { + _initialFaceRect = faceRect; + _estimatedPosition = glm::vec3(); + + } else { + float proportion = sqrtf(_initialFaceRect.size.area() / (float)faceRect.size.area()); + const float DISTANCE_TO_CAMERA = 0.333f; + const float POSITION_SCALE = 0.5f; + float z = DISTANCE_TO_CAMERA * proportion - DISTANCE_TO_CAMERA; + glm::vec3 position = glm::vec3( + (faceRect.center.x - _initialFaceRect.center.x) * proportion * POSITION_SCALE / _frameWidth, + (faceRect.center.y - _initialFaceRect.center.y) * proportion * POSITION_SCALE / _frameWidth, + z); + const float POSITION_SMOOTHING = 0.95f; + _estimatedPosition = glm::mix(position, _estimatedPosition, POSITION_SMOOTHING); + } } // note that we have data @@ -176,7 +263,7 @@ void Webcam::setFrame(const Mat& frame, const RotatedRect& faceRect) { QTimer::singleShot(qMax((int)remaining / 1000, 0), _grabber, SLOT(grabFrame())); } -FrameGrabber::FrameGrabber() : _capture(0), _searchWindow(0, 0, 0, 0) { +FrameGrabber::FrameGrabber() : _initialized(false), _capture(0), _searchWindow(0, 0, 0, 0) { } FrameGrabber::~FrameGrabber() { @@ -185,52 +272,170 @@ FrameGrabber::~FrameGrabber() { } } +#ifdef HAVE_OPENNI +static AvatarJointID xnToAvatarJoint(XnSkeletonJoint joint) { + switch (joint) { + 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_ELBOW; + case XN_SKEL_LEFT_ELBOW: return AVATAR_JOINT_RIGHT_WRIST; + + 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_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_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); +} + +static glm::quat xnToGLM(const XnMatrix3X3& matrix) { + glm::quat rotation = glm::quat_cast(glm::mat3( + matrix.elements[0], matrix.elements[1], matrix.elements[2], + matrix.elements[3], matrix.elements[4], matrix.elements[5], + matrix.elements[6], matrix.elements[7], matrix.elements[8])); + return glm::quat(rotation.w, -rotation.x, rotation.y, rotation.z); +} + +static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) { + printLog("Found user %d.\n", id); + generator.GetSkeletonCap().RequestCalibration(id, false); +} + +static void XN_CALLBACK_TYPE lostUser(UserGenerator& generator, XnUserID id, void* cookie) { + printLog("Lost user %d.\n", id); +} + +static void XN_CALLBACK_TYPE calibrationStarted(SkeletonCapability& capability, XnUserID id, void* cookie) { + printLog("Calibration started for user %d.\n", id); +} + +static void XN_CALLBACK_TYPE calibrationCompleted(SkeletonCapability& capability, + XnUserID id, XnCalibrationStatus status, void* cookie) { + if (status == XN_CALIBRATION_STATUS_OK) { + printLog("Calibration completed for user %d.\n", id); + capability.StartTracking(id); + + } else { + printLog("Calibration failed to user %d.\n", id); + capability.RequestCalibration(id, true); + } +} +#endif + void FrameGrabber::reset() { _searchWindow = cv::Rect(0, 0, 0, 0); + +#ifdef HAVE_OPENNI + if (_userGenerator.IsValid() && _userGenerator.GetSkeletonCap().IsTracking(_userID)) { + _userGenerator.GetSkeletonCap().RequestCalibration(_userID, true); + } +#endif } void FrameGrabber::grabFrame() { - if (_capture == 0) { - if ((_capture = cvCaptureFromCAM(-1)) == 0) { - printLog("Failed to open webcam.\n"); - return; - } - const int IDEAL_FRAME_WIDTH = 320; - const int IDEAL_FRAME_HEIGHT = 240; - cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_WIDTH, IDEAL_FRAME_WIDTH); - cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, IDEAL_FRAME_HEIGHT); + if (!(_initialized || init())) { + return; + } + int format = GL_BGR; + Mat frame; + JointVector joints; + +#ifdef HAVE_OPENNI + if (_depthGenerator.IsValid()) { + _xnContext.WaitAnyUpdateAll(); + frame = Mat(_imageMetaData.YRes(), _imageMetaData.XRes(), CV_8UC3, (void*)_imageGenerator.GetImageMap()); + format = GL_RGB; -#ifdef __APPLE__ - configureCamera(0x5ac, 0x8510, false, 0.975, 0.5, 1.0, 0.5, true, 0.5); -#else - cvSetCaptureProperty(_capture, CV_CAP_PROP_EXPOSURE, 0.5); - cvSetCaptureProperty(_capture, CV_CAP_PROP_CONTRAST, 0.5); - cvSetCaptureProperty(_capture, CV_CAP_PROP_SATURATION, 0.5); - cvSetCaptureProperty(_capture, CV_CAP_PROP_BRIGHTNESS, 0.5); - cvSetCaptureProperty(_capture, CV_CAP_PROP_HUE, 0.5); - cvSetCaptureProperty(_capture, CV_CAP_PROP_GAIN, 0.5); -#endif - - switchToResourcesParentIfRequired(); - if (!_faceCascade.load("resources/haarcascades/haarcascade_frontalface_alt.xml")) { - printLog("Failed to load Haar cascade for face tracking.\n"); + Mat depth = Mat(_depthMetaData.YRes(), _depthMetaData.XRes(), CV_16UC1, (void*)_depthGenerator.GetDepthMap()); + const double EIGHT_BIT_MAX = 255; + const double ELEVEN_BIT_MAX = 2047; + depth.convertTo(_grayDepthFrame, CV_8UC1, EIGHT_BIT_MAX / ELEVEN_BIT_MAX); + + _userID = 0; + XnUInt16 userCount = 1; + _userGenerator.GetUsers(&_userID, userCount); + if (userCount > 0 && _userGenerator.GetSkeletonCap().IsTracking(_userID)) { + joints.resize(NUM_AVATAR_JOINTS); + const int MAX_ACTIVE_JOINTS = 16; + XnSkeletonJoint activeJoints[MAX_ACTIVE_JOINTS]; + XnUInt16 activeJointCount = MAX_ACTIVE_JOINTS; + _userGenerator.GetSkeletonCap().EnumerateActiveJoints(activeJoints, activeJointCount); + XnSkeletonJointTransformation transform; + for (int i = 0; i < activeJointCount; i++) { + AvatarJointID avatarJoint = xnToAvatarJoint(activeJoints[i]); + if (avatarJoint == AVATAR_JOINT_NULL) { + continue; + } + _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, + rotation, xnToGLM(projected)); + } } } - IplImage* image = cvQueryFrame(_capture); - if (image == 0) { - // try again later - QMetaObject::invokeMethod(this, "grabFrame", Qt::QueuedConnection); - return; - } - // make sure it's in the format we expect - if (image->nChannels != 3 || image->depth != IPL_DEPTH_8U || image->dataOrder != IPL_DATA_ORDER_PIXEL || - image->origin != 0) { - printLog("Invalid webcam image format.\n"); - return; +#endif + + if (frame.empty()) { + IplImage* image = cvQueryFrame(_capture); + if (image == 0) { + // try again later + QMetaObject::invokeMethod(this, "grabFrame", Qt::QueuedConnection); + return; + } + // make sure it's in the format we expect + if (image->nChannels != 3 || image->depth != IPL_DEPTH_8U || image->dataOrder != IPL_DATA_ORDER_PIXEL || + image->origin != 0) { + printLog("Invalid webcam image format.\n"); + return; + } + frame = image; } // if we don't have a search window (yet), try using the face cascade - Mat frame = image; int channels = 0; float ranges[] = { 0, 180 }; const float* range = ranges; @@ -239,7 +444,7 @@ void FrameGrabber::grabFrame() { _faceCascade.detectMultiScale(frame, faces, 1.1, 6); if (!faces.empty()) { _searchWindow = faces.front(); - updateHSVFrame(frame); + updateHSVFrame(frame, format); Mat faceHsv(_hsvFrame, _searchWindow); Mat faceMask(_mask, _searchWindow); @@ -252,7 +457,7 @@ void FrameGrabber::grabFrame() { } RotatedRect faceRect; if (_searchWindow.area() > 0) { - updateHSVFrame(frame); + updateHSVFrame(frame, format); calcBackProject(&_hsvFrame, 1, &channels, _histogram, _backProject, &range); bitwise_and(_backProject, _mask, _backProject); @@ -261,10 +466,74 @@ void FrameGrabber::grabFrame() { _searchWindow = faceRect.boundingRect(); } QMetaObject::invokeMethod(Application::getInstance()->getWebcam(), "setFrame", - Q_ARG(cv::Mat, frame), Q_ARG(cv::RotatedRect, faceRect)); + Q_ARG(cv::Mat, frame), Q_ARG(int, format), Q_ARG(cv::Mat, _grayDepthFrame), + Q_ARG(cv::RotatedRect, faceRect), Q_ARG(JointVector, joints)); } -void FrameGrabber::updateHSVFrame(const Mat& frame) { - cvtColor(frame, _hsvFrame, CV_BGR2HSV); +bool FrameGrabber::init() { + _initialized = true; + + // load our face cascade + switchToResourcesParentIfRequired(); + if (!_faceCascade.load("resources/haarcascades/haarcascade_frontalface_alt.xml")) { + printLog("Failed to load Haar cascade for face tracking.\n"); + return false; + } + + // first try for a Kinect +#ifdef HAVE_OPENNI + _xnContext.Init(); + if (_depthGenerator.Create(_xnContext) == XN_STATUS_OK && _imageGenerator.Create(_xnContext) == XN_STATUS_OK && + _userGenerator.Create(_xnContext) == XN_STATUS_OK && + _userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) { + _depthGenerator.GetMetaData(_depthMetaData); + _imageGenerator.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); + _imageGenerator.GetMetaData(_imageMetaData); + + XnCallbackHandle userCallbacks, calibrationStartCallback, calibrationCompleteCallback; + _userGenerator.RegisterUserCallbacks(newUser, lostUser, 0, userCallbacks); + _userGenerator.GetSkeletonCap().RegisterToCalibrationStart(calibrationStarted, 0, calibrationStartCallback); + _userGenerator.GetSkeletonCap().RegisterToCalibrationComplete(calibrationCompleted, 0, calibrationCompleteCallback); + + _userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_UPPER); + + _xnContext.StartGeneratingAll(); + return true; + } +#endif + + // next, an ordinary webcam + if ((_capture = cvCaptureFromCAM(-1)) == 0) { + printLog("Failed to open webcam.\n"); + return false; + } + const int IDEAL_FRAME_WIDTH = 320; + const int IDEAL_FRAME_HEIGHT = 240; + cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_WIDTH, IDEAL_FRAME_WIDTH); + cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, IDEAL_FRAME_HEIGHT); + +#ifdef __APPLE__ + configureCamera(0x5ac, 0x8510, false, 0.975, 0.5, 1.0, 0.5, true, 0.5); +#else + cvSetCaptureProperty(_capture, CV_CAP_PROP_EXPOSURE, 0.5); + cvSetCaptureProperty(_capture, CV_CAP_PROP_CONTRAST, 0.5); + cvSetCaptureProperty(_capture, CV_CAP_PROP_SATURATION, 0.5); + cvSetCaptureProperty(_capture, CV_CAP_PROP_BRIGHTNESS, 0.5); + cvSetCaptureProperty(_capture, CV_CAP_PROP_HUE, 0.5); + cvSetCaptureProperty(_capture, CV_CAP_PROP_GAIN, 0.5); +#endif + + return true; +} + +void FrameGrabber::updateHSVFrame(const Mat& frame, int format) { + cvtColor(frame, _hsvFrame, format == GL_RGB ? CV_RGB2HSV : CV_BGR2HSV); inRange(_hsvFrame, Scalar(0, 55, 65), Scalar(180, 256, 256), _mask); } + +Joint::Joint(const glm::vec3& position, const glm::quat& rotation, const glm::vec3& projected) : + isValid(true), position(position), rotation(rotation), projected(projected) { +} + +Joint::Joint() : isValid(false) { +} diff --git a/interface/src/Webcam.h b/interface/src/Webcam.h index 4bf6dea53e..f0910c7bce 100644 --- a/interface/src/Webcam.h +++ b/interface/src/Webcam.h @@ -12,11 +12,17 @@ #include #include #include +#include #include +#include #include +#ifdef HAVE_OPENNI + #include +#endif + #include "InterfaceConfig.h" class QImage; @@ -24,6 +30,9 @@ class QImage; struct CvCapture; class FrameGrabber; +class Joint; + +typedef QVector JointVector; class Webcam : public QObject { Q_OBJECT @@ -36,6 +45,7 @@ public: const bool isActive() const { return _active; } const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; } const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; } + const JointVector& getEstimatedJoints() const { return _estimatedJoints; } void reset(); void renderPreview(int screenWidth, int screenHeight); @@ -43,7 +53,8 @@ public: public slots: void setEnabled(bool enabled); - void setFrame(const cv::Mat& image, const cv::RotatedRect& faceRect); + void setFrame(const cv::Mat& video, int format, const cv::Mat& depth, + const cv::RotatedRect& faceRect, const JointVector& joints); private: @@ -54,9 +65,13 @@ private: bool _active; int _frameWidth; int _frameHeight; + int _depthWidth; + int _depthHeight; GLuint _frameTextureID; + GLuint _depthTextureID; cv::RotatedRect _faceRect; cv::RotatedRect _initialFaceRect; + JointVector _joints; long long _startTimestamp; int _frameCount; @@ -65,6 +80,7 @@ private: glm::vec3 _estimatedPosition; glm::vec3 _estimatedRotation; + JointVector _estimatedJoints; }; class FrameGrabber : public QObject { @@ -82,8 +98,10 @@ public slots: private: - void updateHSVFrame(const cv::Mat& frame); + bool init(); + void updateHSVFrame(const cv::Mat& frame, int format); + bool _initialized; CvCapture* _capture; cv::CascadeClassifier _faceCascade; cv::Mat _hsvFrame; @@ -91,8 +109,32 @@ private: cv::SparseMat _histogram; cv::Mat _backProject; cv::Rect _searchWindow; + cv::Mat _grayDepthFrame; + +#ifdef HAVE_OPENNI + xn::Context _xnContext; + xn::DepthGenerator _depthGenerator; + xn::ImageGenerator _imageGenerator; + xn::UserGenerator _userGenerator; + xn::DepthMetaData _depthMetaData; + xn::ImageMetaData _imageMetaData; + XnUserID _userID; +#endif }; +class Joint { +public: + + Joint(const glm::vec3& position, const glm::quat& rotation, const glm::vec3& projected); + Joint(); + + bool isValid; + glm::vec3 position; + glm::quat rotation; + glm::vec3 projected; +}; + +Q_DECLARE_METATYPE(JointVector) Q_DECLARE_METATYPE(cv::Mat) Q_DECLARE_METATYPE(cv::RotatedRect) diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 9779d0a1f5..1bef9026e7 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 5d24199b9b..f65cf7bcb5 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -36,6 +37,8 @@ enum KeyState DELETE_KEY_DOWN }; +class JointData; + class AvatarData : public NodeData { public: AvatarData(Node* owningNode = NULL); @@ -132,14 +135,23 @@ protected: bool _wantDelta; bool _wantOcclusionCulling; + std::vector _joints; + HeadData* _headData; HandData* _handData; + private: // privatize the copy constructor and assignment operator so they cannot be called AvatarData(const AvatarData&); AvatarData& operator= (const AvatarData&); }; +class JointData { +public: + + int jointID; + glm::quat rotation; +}; // These pack/unpack functions are designed to start specific known types in as efficient a manner // as possible. Taking advantage of the known characteristics of the semantic types.