diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 6d29a261dd..2c9568cb50 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -248,6 +248,7 @@ void KinectPlugin::init() { auto preference = new CheckPreference(KINECT_PLUGIN, "Extra Debugging", debugGetter, debugSetter); preferences->addPreference(preference); } + } bool KinectPlugin::isSupported() const { @@ -493,11 +494,15 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { //_joints[j].orientation = jointOrientation; if (joints[j].JointType == JointType_HandRight) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); - _joints[j].orientation = jointOrientation * kinectToHandRight; + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandRight; + _joints[j].orientation = QMAR(jointTmp); } else if (joints[j].JointType == JointType_HandLeft) { // To transform from Kinect to our LEFT Hand.... Postive 90 deg around Y static const quat kinectToHandLeft = glm::angleAxis(PI / 2.0f, Vectors::UNIT_Y); - _joints[j].orientation = jointOrientation * kinectToHandLeft; + // add moving average of orientation quaternion + glm::quat jointTmp = jointOrientation * kinectToHandLeft; + _joints[j].orientation = QMAL(jointTmp); } else { _joints[j].orientation = jointOrientation; } @@ -644,3 +649,44 @@ void KinectPlugin::InputDevice::clearState() { _poseStateMap[poseIndex] = controller::Pose(); } } + +glm::quat KinectPlugin::QMAL(glm::quat q) +{ + if (glm::dot(_q_barL, q) < 0) + _q_barL = (1 - _deltaL)*_q_barL + -_deltaL*q; + else + _q_barL = (1 - _deltaL)*_q_barL + _deltaL*q; + + _q_barL = glm::normalize(_q_barL); + + + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barL.x << "," << _q_barL.y << "," << _q_barL.z << "," << _q_barL.w; + } + return _q_barL; +} + + +glm::quat KinectPlugin::QMAR(glm::quat q) +{ + if (glm::dot(_q_barR, q) < 0) + _q_barR = (1 - _deltaR)*_q_barR + -_deltaR*q; + else + _q_barR = (1 - _deltaR)*_q_barR + _deltaR*q; + + _q_barR = glm::normalize(_q_barR); + + + _q_barR = -_q_barR; + + if (_debug) + { + qDebug() << __FUNCTION__ << "q = " << q.x << "," << q.y << "," << q.z << "," << q.w; + qDebug() << __FUNCTION__ << "_q_barL = " << _q_barR.x << "," << _q_barR.y << "," << _q_barR.z << "," << _q_barR.w; + } + + return _q_barR; +} + diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 90794fa6b0..16e41cce8d 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -16,6 +16,12 @@ #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers #endif +// glm files + +#include +#include + + // Windows Header Files #include @@ -58,6 +64,14 @@ public: virtual void saveSettings() const override; virtual void loadSettings() override; +private: + // add variables for moving average + + glm::quat _q_barL; + glm::quat _q_barR; + float _deltaL = (float)0.5; + float _deltaR = (float)0.5; + protected: struct KinectJoint { @@ -113,6 +127,9 @@ protected: // Body reader mutable IBodyFrameReader* _bodyFrameReader { nullptr }; + // moving average for a quaternion + glm::quat QMAL(glm::quat q); + glm::quat QMAR(glm::quat q); #endif };