diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 858ee21371..0404ab9646 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -71,7 +71,7 @@ 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; } diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp index 6d29a261dd..3a36be0982 100644 --- a/plugins/hifiKinect/src/KinectPlugin.cpp +++ b/plugins/hifiKinect/src/KinectPlugin.cpp @@ -24,7 +24,6 @@ Q_DECLARE_LOGGING_CATEGORY(inputplugins) Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") - const char* KinectPlugin::NAME = "Kinect"; const char* KinectPlugin::KINECT_ID_STRING = "Kinect"; @@ -493,11 +492,23 @@ 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 jointSample = jointOrientation * kinectToHandRight; + if (glm::dot(jointSample, _RightHandOrientationAverage.getAverage()) < 0) { + jointSample = -jointSample; + } + _RightHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_RightHandOrientationAverage.getAverage()); } 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 jointSample = jointOrientation * kinectToHandLeft; + if (glm::dot(jointSample, _LeftHandOrientationAverage.getAverage()) < 0) { + jointSample = -jointSample; + } + _LeftHandOrientationAverage.addSample(jointSample); + _joints[j].orientation = glm::normalize(_LeftHandOrientationAverage.getAverage()); } else { _joints[j].orientation = jointOrientation; } @@ -643,4 +654,4 @@ void KinectPlugin::InputDevice::clearState() { int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); _poseStateMap[poseIndex] = controller::Pose(); } -} +} \ No newline at end of file diff --git a/plugins/hifiKinect/src/KinectPlugin.h b/plugins/hifiKinect/src/KinectPlugin.h index 90794fa6b0..158e66ee62 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) { @@ -58,6 +59,11 @@ public: virtual void saveSettings() const override; virtual void loadSettings() override; +private: + // add variables for moving average + ThreadSafeMovingAverage _LeftHandOrientationAverage; + ThreadSafeMovingAverage _RightHandOrientationAverage; + protected: struct KinectJoint {