diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 858ee21371..2559bb36ec 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -61,7 +61,7 @@ public: const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES; const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING; std::atomic numSamples{ 0 }; - std::atomic average; + T average; void clear() { numSamples = 0; @@ -71,12 +71,15 @@ public: void addSample(T sample) { if (numSamples > 0) { - average = ((float)sample * WEIGHTING) + ((float)average * ONE_MINUS_WEIGHTING); + average = (sample * WEIGHTING) + (average * ONE_MINUS_WEIGHTING); } else { average = sample; } numSamples++; } + + + }; template class ThreadSafeMovingAverage { diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index d9888ca8d6..8f10efed86 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -21,6 +21,7 @@ #include #include + Q_DECLARE_LOGGING_CATEGORY(inputplugins) Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") @@ -494,14 +495,22 @@ void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { if (joints[j].JointType == JointType_HandRight) { static const quat kinectToHandRight = glm::angleAxis(-PI / 2.0f, Vectors::UNIT_Y); // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandRight; - _joints[j].orientation = QMAR(jointTmp); + glm::quat jointSample = jointOrientation * kinectToHandRight; + if (glm::dot(jointSample, _RightHandOrientationAverage.average) < 0) { + jointSample = -jointSample; + } + _RightHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_RightHandOrientationAverage.average); } 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); // add moving average of orientation quaternion - glm::quat jointTmp = jointOrientation * kinectToHandLeft; - _joints[j].orientation = QMAL(jointTmp); + glm::quat jointSample = jointOrientation * kinectToHandLeft; + if (glm::dot(jointSample, _LeftHandOrientationAverage.average) < 0) { + jointSample = -jointSample; + } + _LeftHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_LeftHandOrientationAverage.average); } else { _joints[j].orientation = jointOrientation; } @@ -647,43 +656,4 @@ void KinectPlugin::InputDevice::clearState() { int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); _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; -} - +} \ No newline at end of file diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 114d4b76aa..73cc9f4103 100644 --- a/plugins/hifiKinect/src/KinectPlugin.h +++ b/plugins/hifiKinect/src/KinectPlugin.h @@ -23,6 +23,7 @@ // Kinect Header files #include +#include // Safe release for interfaces template inline void SafeRelease(Interface *& pInterfaceToRelease) { @@ -61,10 +62,8 @@ public: private: // add variables for moving average - glm::quat _q_barL; - glm::quat _q_barR; - float _deltaL = 0.5f; - float _deltaR = 0.5f; + MovingAverage _LeftHandOrientationAverage; + MovingAverage _RightHandOrientationAverage; protected: @@ -121,9 +120,6 @@ 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 };