diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c6150dcc53..3d4cb5b266 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3977,11 +3977,17 @@ void Application::update(float deltaTime) { controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND); controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND); + + qDebug() << __FUNCTION__ << "...............leftHandPose:" << leftHandPose.translation; + auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix()); auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix; myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix)); + qDebug() << __FUNCTION__ << "POST transform leftHandPose:" << leftHandPose.translation; + + updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index a2e4b4762f..a9f4d8abf2 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -28,7 +28,7 @@ Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") const char* KinectPlugin::NAME = "Kinect"; const char* KinectPlugin::KINECT_ID_STRING = "Kinect"; -QStringList jointNames = { +QStringList kinectJointNames = { "SpineBase", "SpineMid", "Neck", @@ -198,71 +198,75 @@ static controller::StandardPoseChannel KinectJointIndexToPoseIndex(KinectJointIn } } -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 "???"; +QStringList controllerJointNames = { + "Hips", + "RightUpLeg", + "RightLeg", + "RightFoot", + "LeftUpLeg", + "LeftLeg", + "LeftFoot", + "Spine", + "Spine1", + "Spine2", + "Spine3", + "Neck", + "Head", + "RightShoulder", + "RightArm", + "RightForeArm", + "RightHand", + "RightHandThumb1", + "RightHandThumb2", + "RightHandThumb3", + "RightHandThumb4", + "RightHandIndex1", + "RightHandIndex2", + "RightHandIndex3", + "RightHandIndex4", + "RightHandMiddle1", + "RightHandMiddle2", + "RightHandMiddle3", + "RightHandMiddle4", + "RightHandRing1", + "RightHandRing2", + "RightHandRing3", + "RightHandRing4", + "RightHandPinky1", + "RightHandPinky2", + "RightHandPinky3", + "RightHandPinky4", + "LeftShoulder", + "LeftArm", + "LeftForeArm", + "LeftHand", + "LeftHandThumb1", + "LeftHandThumb2", + "LeftHandThumb3", + "LeftHandThumb4", + "LeftHandIndex1", + "LeftHandIndex2", + "LeftHandIndex3", + "LeftHandIndex4", + "LeftHandMiddle1", + "LeftHandMiddle2", + "LeftHandMiddle3", + "LeftHandMiddle4", + "LeftHandRing1", + "LeftHandRing2", + "LeftHandRing3", + "LeftHandRing4", + "LeftHandPinky1", + "LeftHandPinky2", + "LeftHandPinky3", + "LeftHandPinky4" +}; + +static const char* getControllerJointName(controller::StandardPoseChannel i) { + if (i >= 0 && i < controller::NUM_STANDARD_POSES) { + return qPrintable(controllerJointNames[i]); } + return "unknown"; } // convert between YXZ Kinect euler angles in degrees to quaternion @@ -356,26 +360,6 @@ bool KinectPlugin::initializeDefaultSensor() { #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; @@ -389,15 +373,9 @@ void KinectPlugin::updateBody() { 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); } @@ -441,7 +419,6 @@ void KinectPlugin::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies) { _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; @@ -456,38 +433,17 @@ void KinectPlugin::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies) { 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]; + //QString jointName = kinectJointNames[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 }; + // filling in the _joints data... 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); } } } @@ -516,8 +472,7 @@ void KinectPlugin::deactivate() { SafeRelease(_coordinateMapper); // close the Kinect Sensor - if (_kinectSensor) - { + if (_kinectSensor) { _kinectSensor->Close(); } @@ -532,8 +487,6 @@ void KinectPlugin::pluginUpdate(float deltaTime, const controller::InputCalibrat return; } - //qDebug() << __FUNCTION__ << "deltaTime:" << deltaTime; - updateBody(); // updates _joints std::vector joints = _joints; @@ -554,7 +507,6 @@ void KinectPlugin::saveSettings() const { settings.setValue(QString("enabled"), _enabled); } settings.endGroup(); - qDebug() << __FUNCTION__ << "_enabled:" << _enabled; } void KinectPlugin::loadSettings() { @@ -566,7 +518,6 @@ void KinectPlugin::loadSettings() { _enabled = settings.value("enabled", QVariant(DEFAULT_ENABLED)).toBool(); } settings.endGroup(); - qDebug() << __FUNCTION__ << "_enabled:" << _enabled; } // @@ -577,9 +528,9 @@ void KinectPlugin::loadSettings() { 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))); + for (int i = 0; i < KinectJointIndex::Size; i++) { + auto channel = KinectJointIndexToPoseIndex(static_cast(i)); + availableInputs.push_back(makePair(channel, getControllerJointName(channel))); } }; return availableInputs; @@ -596,6 +547,8 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); glm::vec3 linearVel, angularVel; const glm::vec3& pos = joints[i].pos; + + // FIXME - left over from neuron, needs to be turned into orientations from kinect SDK const glm::vec3& rotEuler = joints[i].euler; if (Vectors::ZERO == pos && Vectors::ZERO == rotEuler) { @@ -604,6 +557,7 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC } + // FIXME - left over from neuron, needs to be turned into orientations from kinect SDK glm::quat rot = eulerToQuat(rotEuler); if (i < prevJoints.size()) { linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s @@ -612,14 +566,9 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC 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; - } - } + // FIXME - left over from neuron _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()); }