From 4959a885815413b01058289d3e7e3614f160813e Mon Sep 17 00:00:00 2001 From: Brad Hefta-Gaub Date: Thu, 8 Dec 2016 22:13:56 -0800 Subject: [PATCH] first cut at kinect input plugin --- cmake/macros/TargetKinect.cmake | 17 + cmake/modules/FindKinect.cmake | 59 ++ interface/resources/controllers/kinect.json | 7 + .../hifi/dialogs/GeneralPreferencesDialog.qml | 2 +- plugins/CMakeLists.txt | 3 + plugins/hifiKinect/CMakeLists.txt | 17 + plugins/hifiKinect/src/KinectPlugin.cpp | 626 ++++++++++++++++++ plugins/hifiKinect/src/KinectPlugin.h | 119 ++++ plugins/hifiKinect/src/KinectProvider.cpp | 49 ++ plugins/hifiKinect/src/plugin.json | 1 + 10 files changed, 899 insertions(+), 1 deletion(-) create mode 100644 cmake/macros/TargetKinect.cmake create mode 100644 cmake/modules/FindKinect.cmake create mode 100644 interface/resources/controllers/kinect.json create mode 100644 plugins/hifiKinect/CMakeLists.txt create mode 100644 plugins/hifiKinect/src/KinectPlugin.cpp create mode 100644 plugins/hifiKinect/src/KinectPlugin.h create mode 100644 plugins/hifiKinect/src/KinectProvider.cpp create mode 100644 plugins/hifiKinect/src/plugin.json diff --git a/cmake/macros/TargetKinect.cmake b/cmake/macros/TargetKinect.cmake new file mode 100644 index 0000000000..ca9c853d8d --- /dev/null +++ b/cmake/macros/TargetKinect.cmake @@ -0,0 +1,17 @@ +# +# Created by Brad Hefta-Gaub on 2016/12/7 +# Copyright 2016 High Fidelity, Inc. +# +# Distributed under the Apache License, Version 2.0. +# See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +# +macro(TARGET_KINECT) + # Kinect SDK data reader is only available on these platforms + if (WIN32) + #add_dependency_external_projects(kinect) + find_package(Kinect REQUIRED) + target_include_directories(${TARGET_NAME} PRIVATE ${KINECT_INCLUDE_DIRS}) + target_link_libraries(${TARGET_NAME} ${KINECT_LIBRARIES}) + add_definitions(-DHAVE_KINECT) + endif(WIN32) +endmacro() diff --git a/cmake/modules/FindKinect.cmake b/cmake/modules/FindKinect.cmake new file mode 100644 index 0000000000..4d1b4fac62 --- /dev/null +++ b/cmake/modules/FindKinect.cmake @@ -0,0 +1,59 @@ +# +# FindKinect.cmake +# +# Try to find the Perception Kinect SDK +# +# You must provide a KINECT_ROOT_DIR which contains lib and include directories +# +# Once done this will define +# +# KINECT_FOUND - system found Kinect SDK +# KINECT_INCLUDE_DIRS - the Kinect SDK include directory +# KINECT_LIBRARIES - Link this to use Kinect +# +# Created by Brad Hefta-Gaub on 2016/12/7 +# Copyright 2016 High Fidelity, Inc. +# +# Distributed under the Apache License, Version 2.0. +# See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +# + +include("${MACRO_DIR}/HifiLibrarySearchHints.cmake") +hifi_library_search_hints("kinect") + +find_path(KINECT_INCLUDE_DIRS Kinect.h PATH_SUFFIXES inc HINTS $ENV{KINECT_ROOT_DIR}) + +if (WIN32) + + if ("${CMAKE_SIZEOF_VOID_P}" EQUAL "8") + set(ARCH_DIR "x64") + else() + set(ARCH_DIR "x86") + endif() + + find_library( + KINECT_LIBRARY_RELEASE Kinect20 + PATH_SUFFIXES "Libs/${ARCH_DIR}" "lib" + HINTS ${KINECT_SEARCH_DIRS} + PATH "C:/Program Files/Microsoft SDKs/Kinect/v2.0_1409") + + set(KINECT_LIBRARIES ${KINECT_LIBRARY}) + + # DLL not needed yet?? + #find_path(KINECT_DLL_PATH Kinect20.Face.dll PATH_SUFFIXES "bin" HINTS ${KINECT_SEARCH_DIRS}) + + +endif () + +include(SelectLibraryConfigurations) +select_library_configurations(KINECT) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(KINECT DEFAULT_MSG KINECT_INCLUDE_DIRS KINECT_LIBRARY) + +# DLLs not needed yet +#if (WIN32) +# add_paths_to_fixup_libs(${KINECT_DLL_PATH}) +#endif () + +mark_as_advanced(KINECT_INCLUDE_DIRS KINECT_LIBRARIES KINECT_SEARCH_DIRS) diff --git a/interface/resources/controllers/kinect.json b/interface/resources/controllers/kinect.json new file mode 100644 index 0000000000..96a504030c --- /dev/null +++ b/interface/resources/controllers/kinect.json @@ -0,0 +1,7 @@ +{ + "name": "Kinect to Standard", + "channels": [ + { "from": "Kinect.LeftHand", "to": "Standard.LeftHand" }, + { "from": "Kinect.RightHand", "to": "Standard.RightHand" } + ] +} diff --git a/interface/resources/qml/hifi/dialogs/GeneralPreferencesDialog.qml b/interface/resources/qml/hifi/dialogs/GeneralPreferencesDialog.qml index fec836626f..9be1c30e55 100644 --- a/interface/resources/qml/hifi/dialogs/GeneralPreferencesDialog.qml +++ b/interface/resources/qml/hifi/dialogs/GeneralPreferencesDialog.qml @@ -17,7 +17,7 @@ PreferencesDialog { id: root objectName: "GeneralPreferencesDialog" title: "General Settings" - showCategories: ["UI", "Snapshots", "Scripts", "Privacy", "Octree", "HMD", "Sixense Controllers", "Perception Neuron"] + showCategories: ["UI", "Snapshots", "Scripts", "Privacy", "Octree", "HMD", "Sixense Controllers", "Perception Neuron", "Kinect"] property var settings: Settings { category: root.objectName property alias x: root.x diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 6dbc57431e..4553b80793 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -26,6 +26,9 @@ if (NOT SERVER_ONLY AND NOT ANDROID) add_subdirectory(${DIR}) set(DIR "hifiNeuron") add_subdirectory(${DIR}) + + set(DIR "hifiKinect") + add_subdirectory(${DIR}) endif() # server-side plugins diff --git a/plugins/hifiKinect/CMakeLists.txt b/plugins/hifiKinect/CMakeLists.txt new file mode 100644 index 0000000000..c2c131106b --- /dev/null +++ b/plugins/hifiKinect/CMakeLists.txt @@ -0,0 +1,17 @@ +# +# Created by Brad Hefta-Gaub on 2016/12/7 +# Copyright 2016 High Fidelity, Inc. +# +# Distributed under the Apache License, Version 2.0. +# See the accompanying file LICENSE or http:#www.apache.org/licenses/LICENSE-2.0.html +# + +if (WIN32) + set(TARGET_NAME hifiKinect) + setup_hifi_plugin(Script Qml Widgets) + link_hifi_libraries(shared controllers ui plugins input-plugins) + + # need to setup appropriate externals... + target_kinect() + +endif() diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp new file mode 100644 index 0000000000..a2e4b4762f --- /dev/null +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -0,0 +1,626 @@ +// +// KinectPlugin.cpp +// +// Created by Brad Hefta-Gaub on 2016/12/7 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + + +#include "KinectPlugin.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(inputplugins) +Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") + + +const char* KinectPlugin::NAME = "Kinect"; +const char* KinectPlugin::KINECT_ID_STRING = "Kinect"; + +QStringList jointNames = { + "SpineBase", + "SpineMid", + "Neck", + "Head", + "ShoulderLeft", + "ElbowLeft", + "WristLeft", + "HandLeft", + "ShoulderRight", + "ElbowRight", + "WristRight", + "HandRight", + "HipLeft", + "KneeLeft", + "AnkleLeft", + "FootLeft", + "HipRight", + "KneeRight", + "AnkleRight", + "FootRight", + "SpineShoulder", + "HandTipLeft", + "ThumbLeft", + "HandTipRight", + "ThumbRight" +}; + +const bool DEFAULT_ENABLED = false; + +// indices of joints of the Kinect standard skeleton. +// This is 'almost' the same as the High Fidelity standard skeleton. +// It is missing a thumb joint. +enum KinectJointIndex { + SpineBase = 0, + SpineMid, + Neck, + Head, + + ShoulderLeft, + ElbowLeft, + WristLeft, + HandLeft, + + ShoulderRight, + ElbowRight, + WristRight, + HandRight, + + HipLeft, + KneeLeft, + AnkleLeft, + FootLeft, + + HipRight, + KneeRight, + AnkleRight, + FootRight, + + SpineShoulder, + + HandTipLeft, + ThumbLeft, + + HandTipRight, + ThumbRight, + + Size +}; + +// -1 or 0??? +#define UNKNOWN_JOINT (controller::StandardPoseChannel)0 + +// Almost a direct mapping except for LEFT_HAND_THUMB1 and RIGHT_HAND_THUMB1, +// which are not present in the Kinect standard skeleton. +static controller::StandardPoseChannel KinectJointIndexToPoseIndexMap[KinectJointIndex::Size] = { + controller::HIPS, + controller::SPINE, + controller::NECK, + controller::HEAD, + + controller::LEFT_SHOULDER, + controller::LEFT_ARM, + controller::LEFT_FORE_ARM, + controller::LEFT_HAND, + + controller::RIGHT_SHOULDER, + controller::RIGHT_ARM, + controller::RIGHT_FORE_ARM, + controller::RIGHT_HAND, + + controller::RIGHT_UP_LEG, // hip socket + controller::RIGHT_LEG, // knee? + controller::RIGHT_FOOT, // ankle? + UNKNOWN_JOINT, // ???? + + controller::LEFT_UP_LEG, // hip socket + controller::LEFT_LEG, // knee? + controller::LEFT_FOOT, // ankle? + UNKNOWN_JOINT, // ???? + + UNKNOWN_JOINT, /* SpineShoulder */ + + controller::LEFT_HAND_INDEX4, + controller::LEFT_HAND_THUMB4, + + controller::RIGHT_HAND_INDEX4, + controller::RIGHT_HAND_THUMB4, + + + /** + controller::SPINE1, + controller::SPINE2, + controller::SPINE3, + + controller::RIGHT_HAND_THUMB2, + controller::RIGHT_HAND_THUMB3, + controller::RIGHT_HAND_THUMB4, + controller::RIGHT_HAND_INDEX1, + controller::RIGHT_HAND_INDEX2, + controller::RIGHT_HAND_INDEX3, + controller::RIGHT_HAND_INDEX4, + controller::RIGHT_HAND_MIDDLE1, + controller::RIGHT_HAND_MIDDLE2, + controller::RIGHT_HAND_MIDDLE3, + controller::RIGHT_HAND_MIDDLE4, + controller::RIGHT_HAND_RING1, + controller::RIGHT_HAND_RING2, + controller::RIGHT_HAND_RING3, + controller::RIGHT_HAND_RING4, + controller::RIGHT_HAND_PINKY1, + controller::RIGHT_HAND_PINKY2, + controller::RIGHT_HAND_PINKY3, + controller::RIGHT_HAND_PINKY4, + + controller::LEFT_HAND_THUMB2, + controller::LEFT_HAND_THUMB3, + controller::LEFT_HAND_THUMB4, + controller::LEFT_HAND_INDEX1, + controller::LEFT_HAND_INDEX2, + controller::LEFT_HAND_INDEX3, + controller::LEFT_HAND_INDEX4, + controller::LEFT_HAND_MIDDLE1, + controller::LEFT_HAND_MIDDLE2, + controller::LEFT_HAND_MIDDLE3, + controller::LEFT_HAND_MIDDLE4, + controller::LEFT_HAND_RING1, + controller::LEFT_HAND_RING2, + controller::LEFT_HAND_RING3, + controller::LEFT_HAND_RING4, + controller::LEFT_HAND_PINKY1, + controller::LEFT_HAND_PINKY2, + controller::LEFT_HAND_PINKY3, + controller::LEFT_HAND_PINKY4 + **/ +}; + +// in rig frame +static glm::vec3 rightHandThumb1DefaultAbsTranslation(-2.155500650405884, -0.7610001564025879, 2.685631036758423); +static glm::vec3 leftHandThumb1DefaultAbsTranslation(2.1555817127227783, -0.7603635787963867, 2.6856393814086914); + +static controller::StandardPoseChannel KinectJointIndexToPoseIndex(KinectJointIndex i) { + assert(i >= 0 && i < KinectJointIndex::Size); + if (i >= 0 && i < KinectJointIndex::Size) { + return KinectJointIndexToPoseIndexMap[i]; + } else { + return UNKNOWN_JOINT; // not sure what to do here, but don't crash! + } +} + +static const char* controllerJointName(controller::StandardPoseChannel i) { + switch (i) { + case controller::HIPS: return "Hips"; + case controller::RIGHT_UP_LEG: return "RightUpLeg"; + case controller::RIGHT_LEG: return "RightLeg"; + case controller::RIGHT_FOOT: return "RightFoot"; + case controller::LEFT_UP_LEG: return "LeftUpLeg"; + case controller::LEFT_LEG: return "LeftLeg"; + case controller::LEFT_FOOT: return "LeftFoot"; + case controller::SPINE: return "Spine"; + case controller::SPINE1: return "Spine1"; + case controller::SPINE2: return "Spine2"; + case controller::SPINE3: return "Spine3"; + case controller::NECK: return "Neck"; + case controller::HEAD: return "Head"; + case controller::RIGHT_SHOULDER: return "RightShoulder"; + case controller::RIGHT_ARM: return "RightArm"; + case controller::RIGHT_FORE_ARM: return "RightForeArm"; + case controller::RIGHT_HAND: return "RightHand"; + case controller::RIGHT_HAND_THUMB1: return "RightHandThumb1"; + case controller::RIGHT_HAND_THUMB2: return "RightHandThumb2"; + case controller::RIGHT_HAND_THUMB3: return "RightHandThumb3"; + case controller::RIGHT_HAND_THUMB4: return "RightHandThumb4"; + case controller::RIGHT_HAND_INDEX1: return "RightHandIndex1"; + case controller::RIGHT_HAND_INDEX2: return "RightHandIndex2"; + case controller::RIGHT_HAND_INDEX3: return "RightHandIndex3"; + case controller::RIGHT_HAND_INDEX4: return "RightHandIndex4"; + case controller::RIGHT_HAND_MIDDLE1: return "RightHandMiddle1"; + case controller::RIGHT_HAND_MIDDLE2: return "RightHandMiddle2"; + case controller::RIGHT_HAND_MIDDLE3: return "RightHandMiddle3"; + case controller::RIGHT_HAND_MIDDLE4: return "RightHandMiddle4"; + case controller::RIGHT_HAND_RING1: return "RightHandRing1"; + case controller::RIGHT_HAND_RING2: return "RightHandRing2"; + case controller::RIGHT_HAND_RING3: return "RightHandRing3"; + case controller::RIGHT_HAND_RING4: return "RightHandRing4"; + case controller::RIGHT_HAND_PINKY1: return "RightHandPinky1"; + case controller::RIGHT_HAND_PINKY2: return "RightHandPinky2"; + case controller::RIGHT_HAND_PINKY3: return "RightHandPinky3"; + case controller::RIGHT_HAND_PINKY4: return "RightHandPinky4"; + case controller::LEFT_SHOULDER: return "LeftShoulder"; + case controller::LEFT_ARM: return "LeftArm"; + case controller::LEFT_FORE_ARM: return "LeftForeArm"; + case controller::LEFT_HAND: return "LeftHand"; + case controller::LEFT_HAND_THUMB1: return "LeftHandThumb1"; + case controller::LEFT_HAND_THUMB2: return "LeftHandThumb2"; + case controller::LEFT_HAND_THUMB3: return "LeftHandThumb3"; + case controller::LEFT_HAND_THUMB4: return "LeftHandThumb4"; + case controller::LEFT_HAND_INDEX1: return "LeftHandIndex1"; + case controller::LEFT_HAND_INDEX2: return "LeftHandIndex2"; + case controller::LEFT_HAND_INDEX3: return "LeftHandIndex3"; + case controller::LEFT_HAND_INDEX4: return "LeftHandIndex4"; + case controller::LEFT_HAND_MIDDLE1: return "LeftHandMiddle1"; + case controller::LEFT_HAND_MIDDLE2: return "LeftHandMiddle2"; + case controller::LEFT_HAND_MIDDLE3: return "LeftHandMiddle3"; + case controller::LEFT_HAND_MIDDLE4: return "LeftHandMiddle4"; + case controller::LEFT_HAND_RING1: return "LeftHandRing1"; + case controller::LEFT_HAND_RING2: return "LeftHandRing2"; + case controller::LEFT_HAND_RING3: return "LeftHandRing3"; + case controller::LEFT_HAND_RING4: return "LeftHandRing4"; + case controller::LEFT_HAND_PINKY1: return "LeftHandPinky1"; + case controller::LEFT_HAND_PINKY2: return "LeftHandPinky2"; + case controller::LEFT_HAND_PINKY3: return "LeftHandPinky3"; + case controller::LEFT_HAND_PINKY4: return "LeftHandPinky4"; + default: return "???"; + } +} + +// convert between YXZ Kinect euler angles in degrees to quaternion +// this is the default setting in the Axis Kinect server. +static quat eulerToQuat(const vec3& e) { + // euler.x and euler.y are swaped, WTF. + return (glm::angleAxis(e.x * RADIANS_PER_DEGREE, Vectors::UNIT_Y) * + glm::angleAxis(e.y * RADIANS_PER_DEGREE, Vectors::UNIT_X) * + glm::angleAxis(e.z * RADIANS_PER_DEGREE, Vectors::UNIT_Z)); +} + +// +// KinectPlugin +// + +void KinectPlugin::init() { + loadSettings(); + + auto preferences = DependencyManager::get(); + static const QString KINECT_PLUGIN { "Kinect" }; + { + auto getter = [this]()->bool { return _enabled; }; + auto setter = [this](bool value) { _enabled = value; saveSettings(); }; + auto preference = new CheckPreference(KINECT_PLUGIN, "Enabled", getter, setter); + preferences->addPreference(preference); + } +} + +bool KinectPlugin::isSupported() const { + // FIXME -- check to see if there's a camera or not... + return true; +} + +bool KinectPlugin::activate() { + InputPlugin::activate(); + + loadSettings(); + + if (_enabled) { + + // register with userInputMapper + auto userInputMapper = DependencyManager::get(); + userInputMapper->registerDevice(_inputDevice); + + return initializeDefaultSensor(); + } else { + return false; + } +} + +bool KinectPlugin::initializeDefaultSensor() { +#ifdef HAVE_KINECT + HRESULT hr; + + hr = GetDefaultKinectSensor(&_kinectSensor); + if (FAILED(hr)) { + return false; + } + + if (_kinectSensor) { + // Initialize the Kinect and get coordinate mapper and the body reader + IBodyFrameSource* bodyFrameSource = NULL; + + hr = _kinectSensor->Open(); + + if (SUCCEEDED(hr)) { + hr = _kinectSensor->get_CoordinateMapper(&_coordinateMapper); + } + + if (SUCCEEDED(hr)) { + hr = _kinectSensor->get_BodyFrameSource(&bodyFrameSource); + } + + if (SUCCEEDED(hr)) { + hr = bodyFrameSource->OpenReader(&_bodyFrameReader); + } + + SafeRelease(bodyFrameSource); + } + + if (!_kinectSensor || FAILED(hr)) { + qDebug() << "No ready Kinect found!"; + return false; + } + + qDebug() << "Kinect found WOOT!"; + + return true; +#else + return false; +#endif +} + +/// +/// Converts a body point to screen space +/// +/// body point to tranform +/// width (in pixels) of output buffer +/// height (in pixels) of output buffer +/// point in screen-space +#if 0 +D2D1_POINT_2F KinectPlugin::BodyToScreen(const CameraSpacePoint& bodyPoint, int width, int height) { + // Calculate the body's position on the screen + DepthSpacePoint depthPoint = { 0 }; + _coordinateMapper->MapCameraPointToDepthSpace(bodyPoint, &depthPoint); + + float screenPointX = static_cast(depthPoint.X * width) / cDepthWidth; + float screenPointY = static_cast(depthPoint.Y * height) / cDepthHeight; + + return D2D1::Point2F(screenPointX, screenPointY); +} +#endif + +void KinectPlugin::updateBody() { +#ifndef HAVE_KINECT + return; +#else + if (!_bodyFrameReader) { + return; + } + + IBodyFrame* pBodyFrame = NULL; + + HRESULT hr = _bodyFrameReader->AcquireLatestFrame(&pBodyFrame); + + if (SUCCEEDED(hr)) { + + //qDebug() << __FUNCTION__ << "line:" << __LINE__; + + INT64 nTime = 0; + + hr = pBodyFrame->get_RelativeTime(&nTime); + + IBody* ppBodies[BODY_COUNT] = {0}; + + if (SUCCEEDED(hr)) { + hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies); + } + + if (SUCCEEDED(hr)) { + ProcessBody(nTime, BODY_COUNT, ppBodies); + } + + for (int i = 0; i < _countof(ppBodies); ++i) { + SafeRelease(ppBodies[i]); + } + } + + SafeRelease(pBodyFrame); +#endif +} + +#ifdef HAVE_KINECT +/// +/// Handle new body data +/// timestamp of frame +/// body data count +/// body data in frame +/// +void KinectPlugin::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies) { + bool foundOneBody = false; + if (_coordinateMapper) { + for (int i = 0; i < nBodyCount; ++i) { + if (foundOneBody) { + break; + } + IBody* pBody = ppBodies[i]; + if (pBody) { + BOOLEAN tracked = false; + HRESULT hr = pBody->get_IsTracked(&tracked); + + if (SUCCEEDED(hr) && tracked) { + foundOneBody = true; + + if (_joints.size() != JointType_Count) { + _joints.resize(JointType_Count, { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }); + } + + + Joint joints[JointType_Count]; + JointOrientation jointOrientations[JointType_Count]; + HandState leftHandState = HandState_Unknown; + HandState rightHandState = HandState_Unknown; + + pBody->get_HandLeftState(&leftHandState); + pBody->get_HandRightState(&rightHandState); + + hr = pBody->GetJoints(_countof(joints), joints); + hr = pBody->GetJointOrientations(_countof(jointOrientations), jointOrientations); + + if (SUCCEEDED(hr)) { + auto jointCount = _countof(joints); + //qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; + QString state; + for (int j = 0; j < jointCount; ++j) { + QString jointName = jointNames[joints[j].JointType]; + + switch(joints[j].TrackingState) { + case TrackingState_NotTracked: + state = "Not Tracked"; + break; + case TrackingState_Inferred: + state = "Inferred"; + break; + case TrackingState_Tracked: + state = "Tracked"; + break; + } + glm::vec3 jointPosition { joints[j].Position.X, + joints[j].Position.Y, + joints[j].Position.Z }; + + if (joints[j].TrackingState != TrackingState_NotTracked) { + _joints[j].pos = jointPosition; + + if (joints[j].JointType == JointType_HandLeft || + joints[j].JointType == JointType_HandRight) { + //qDebug() << __FUNCTION__ << jointName << ":" << state << "pos:" << jointPosition; + } + + } + + + // FIXME - do something interesting with the joints... + //jointPoints[j] = BodyToScreen(joints[j].Position, width, height); + } + } + } + } + } + } +} +#endif + + +void KinectPlugin::deactivate() { + // unregister from userInputMapper + if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) { + auto userInputMapper = DependencyManager::get(); + userInputMapper->removeDevice(_inputDevice->_deviceID); + } + + InputPlugin::deactivate(); + saveSettings(); + +#ifdef HAVE_KINECT + // done with body frame reader + SafeRelease(_bodyFrameReader); + + // done with coordinate mapper + SafeRelease(_coordinateMapper); + + // close the Kinect Sensor + if (_kinectSensor) + { + _kinectSensor->Close(); + } + + SafeRelease(_kinectSensor); +#endif // HAVE_KINECT + +} + +void KinectPlugin::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { + + if (!_enabled) { + return; + } + + //qDebug() << __FUNCTION__ << "deltaTime:" << deltaTime; + + updateBody(); // updates _joints + + std::vector joints = _joints; + + auto userInputMapper = DependencyManager::get(); + userInputMapper->withLock([&, this]() { + _inputDevice->update(deltaTime, inputCalibrationData, joints, _prevJoints); + }); + + _prevJoints = joints; +} + +void KinectPlugin::saveSettings() const { + Settings settings; + QString idString = getID(); + settings.beginGroup(idString); + { + settings.setValue(QString("enabled"), _enabled); + } + settings.endGroup(); + qDebug() << __FUNCTION__ << "_enabled:" << _enabled; +} + +void KinectPlugin::loadSettings() { + Settings settings; + QString idString = getID(); + settings.beginGroup(idString); + { + // enabled + _enabled = settings.value("enabled", QVariant(DEFAULT_ENABLED)).toBool(); + } + settings.endGroup(); + qDebug() << __FUNCTION__ << "_enabled:" << _enabled; +} + +// +// InputDevice +// + +// FIXME - we probably should only return the inputs we ACTUALLY have +controller::Input::NamedVector KinectPlugin::InputDevice::getAvailableInputs() const { + static controller::Input::NamedVector availableInputs; + if (availableInputs.size() == 0) { + for (int i = 0; i < controller::NUM_STANDARD_POSES; i++) { + auto channel = static_cast(i); + availableInputs.push_back(makePair(channel, controllerJointName(channel))); + } + }; + return availableInputs; +} + +QString KinectPlugin::InputDevice::getDefaultMappingConfig() const { + static const QString MAPPING_JSON = PathUtils::resourcesPath() + "/controllers/kinect.json"; + return MAPPING_JSON; +} + +void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, + const std::vector& joints, const std::vector& prevJoints) { + for (size_t i = 0; i < joints.size(); i++) { + int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); + glm::vec3 linearVel, angularVel; + const glm::vec3& pos = joints[i].pos; + const glm::vec3& rotEuler = joints[i].euler; + + if (Vectors::ZERO == pos && Vectors::ZERO == rotEuler) { + _poseStateMap[poseIndex] = controller::Pose(); + continue; + } + + + glm::quat rot = eulerToQuat(rotEuler); + if (i < prevJoints.size()) { + linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s + // quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation. + glm::quat d = glm::log(rot * glm::inverse(eulerToQuat(prevJoints[i].euler))); + angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s + } + _poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel); + + if (poseIndex == controller::LEFT_HAND || + poseIndex == controller::RIGHT_HAND) { + //qDebug() << __FUNCTION__ << "_poseStateMap[]=" << _poseStateMap[poseIndex].translation; + } + + } + + _poseStateMap[controller::RIGHT_HAND_THUMB1] = controller::Pose(rightHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); + _poseStateMap[controller::LEFT_HAND_THUMB1] = controller::Pose(leftHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); +} + diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h new file mode 100644 index 0000000000..a761b063d2 --- /dev/null +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -0,0 +1,119 @@ +// +// KinectPlugin.h +// +// Created by Brad Hefta-Gaub on 2016/12/7 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_KinectPlugin_h +#define hifi_KinectPlugin_h + +#ifdef HAVE_KINECT +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers +#endif + +// Windows Header Files +#include + +#include + +// Kinect Header files +#include + +// Safe release for interfaces +template inline void SafeRelease(Interface *& pInterfaceToRelease) { + if (pInterfaceToRelease != NULL) { + pInterfaceToRelease->Release(); + pInterfaceToRelease = NULL; + } +} +#endif + +#include +#include +#include + +// Handles interaction with the Kinect SDK +class KinectPlugin : public InputPlugin { + Q_OBJECT +public: + //friend void FrameDataReceivedCallback(void* context, void* sender, _BvhDataHeaderEx* header, float* data); + + bool isHandController() const override { return false; } + + // Plugin functions + virtual void init() override; + virtual bool isSupported() const override; + virtual const QString getName() const override { return NAME; } + const QString getID() const override { return KINECT_ID_STRING; } + + virtual bool activate() override; + virtual void deactivate() override; + + virtual void pluginFocusOutEvent() override { _inputDevice->focusOutEvent(); } + virtual void pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) override; + + virtual void saveSettings() const override; + virtual void loadSettings() override; + +protected: + + struct KinectJoint { + glm::vec3 pos; + glm::vec3 euler; // FIXME -- we don't get this... + }; + + class InputDevice : public controller::InputDevice { + public: + friend class KinectPlugin; + + InputDevice() : controller::InputDevice("Kinect") {} + + // Device functions + virtual controller::Input::NamedVector getAvailableInputs() const override; + virtual QString getDefaultMappingConfig() const override; + virtual void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) override {}; + virtual void focusOutEvent() override {}; + + void update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, + const std::vector& joints, const std::vector& prevJoints); + }; + + std::shared_ptr _inputDevice { std::make_shared() }; + + static const char* NAME; + static const char* KINECT_ID_STRING; + + bool _enabled; + + // copy of data directly from the KinectDataReader SDK + std::vector _joints; + + // one frame old copy of _joints, used to caluclate angular and linear velocity. + std::vector _prevJoints; + + + // Kinect SDK related items... + + bool KinectPlugin::initializeDefaultSensor(); + void updateBody(); + +#ifdef HAVE_KINECT + void ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies); + + // Current Kinect + IKinectSensor* _kinectSensor { nullptr }; + ICoordinateMapper* _coordinateMapper { nullptr }; + + // Body reader + IBodyFrameReader* _bodyFrameReader { nullptr }; +#endif + +}; + +#endif // hifi_KinectPlugin_h + diff --git a/plugins/hifiKinect/src/KinectProvider.cpp b/plugins/hifiKinect/src/KinectProvider.cpp new file mode 100644 index 0000000000..269ec50b60 --- /dev/null +++ b/plugins/hifiKinect/src/KinectProvider.cpp @@ -0,0 +1,49 @@ +// +// Created by Brad Hefta-Gaub on 2016/12/7 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include + +#include +#include +#include + +#include +#include + +#include "KinectPlugin.h" + +class KinectProvider : public QObject, public InputProvider +{ + Q_OBJECT + Q_PLUGIN_METADATA(IID InputProvider_iid FILE "plugin.json") + Q_INTERFACES(InputProvider) + +public: + KinectProvider(QObject* parent = nullptr) : QObject(parent) {} + virtual ~KinectProvider() {} + + virtual InputPluginList getInputPlugins() override { + static std::once_flag once; + std::call_once(once, [&] { + InputPluginPointer plugin(new KinectPlugin()); + if (plugin->isSupported()) { + _inputPlugins.push_back(plugin); + } + }); + return _inputPlugins; + } + + virtual void destroyInputPlugins() override { + _inputPlugins.clear(); + } + +private: + InputPluginList _inputPlugins; +}; + +#include "KinectProvider.moc" diff --git a/plugins/hifiKinect/src/plugin.json b/plugins/hifiKinect/src/plugin.json new file mode 100644 index 0000000000..daa3a668dd --- /dev/null +++ b/plugins/hifiKinect/src/plugin.json @@ -0,0 +1 @@ +{"name":"Kinect"}