diff --git a/interface/src/devices/DdeFaceTracker.cpp b/interface/src/devices/DdeFaceTracker.cpp index e596342473..4cbbe28ec2 100644 --- a/interface/src/devices/DdeFaceTracker.cpp +++ b/interface/src/devices/DdeFaceTracker.cpp @@ -17,6 +17,8 @@ #include #include +#include + #include "DdeFaceTracker.h" #include "FaceshiftConstants.h" #include "InterfaceLogging.h" @@ -267,6 +269,8 @@ float DdeFaceTracker::getBlendshapeCoefficient(int index) const { void DdeFaceTracker::decodePacket(const QByteArray& buffer) { if(buffer.size() > MIN_PACKET_SIZE) { + bool isFitering = Menu::getInstance()->isOptionChecked(MenuOption::DDEFiltering); + Packet packet; int bytesToCopy = glm::min((int)sizeof(packet), buffer.size()); memset(&packet.name, '\n', MAX_NAME_SIZE + 1); @@ -287,11 +291,36 @@ void DdeFaceTracker::decodePacket(const QByteArray& buffer) { translation -= _referenceTranslation; translation /= LEAN_DAMPING_FACTOR; translation.x *= -1; - _headTranslation = translation; + if (isFitering) { + glm::vec3 linearVelocity = (translation - _lastHeadTranslation) / _averageMessageTime; + const float LINEAR_VELOCITY_FILTER_STRENGTH = 0.3f; + float velocityFilter = glm::clamp(1.0f - glm::length(linearVelocity) * + LINEAR_VELOCITY_FILTER_STRENGTH, 0.0f, 1.0f); + _filteredHeadTranslation = velocityFilter * _filteredHeadTranslation + (1.0f - velocityFilter) * translation; + _lastHeadTranslation = translation; + _headTranslation = _filteredHeadTranslation; + } else { + _headTranslation = translation; + } // Compute relative rotation rotation = glm::inverse(_referenceRotation) * rotation; - _headRotation = rotation; + if (isFitering) { + glm::quat r = rotation * glm::inverse(_headRotation); + float theta = 2 * acos(r.w); + glm::vec3 angularVelocity; + if (theta > EPSILON) { + float rMag = glm::length(glm::vec3(r.x, r.y, r.z)); + angularVelocity = theta / _averageMessageTime * glm::vec3(r.x, r.y, r.z) / rMag; + } else { + angularVelocity = glm::vec3(0, 0, 0); + } + const float ANGULAR_VELOCITY_FILTER_STRENGTH = 0.3f; + _headRotation = safeMix(_headRotation, rotation, glm::clamp(glm::length(angularVelocity) * + ANGULAR_VELOCITY_FILTER_STRENGTH, 0.0f, 1.0f)); + } else { + _headRotation = rotation; + } // Translate DDE coefficients to Faceshift compatible coefficients for (int i = 0; i < NUM_EXPRESSIONS; i += 1) { @@ -339,7 +368,16 @@ void DdeFaceTracker::decodePacket(const QByteArray& buffer) { = glm::clamp(DDE_COEFFICIENT_SCALES[i] * _coefficients[i], 0.0f, 1.0f); } - } else { + // Calculate average frame time + const float FRAME_AVERAGING_FACTOR = 0.99f; + quint64 usecsNow = usecTimestampNow(); + if (_lastMessageReceived != 0) { + _averageMessageTime = FRAME_AVERAGING_FACTOR * _averageMessageTime + + (1.0f - FRAME_AVERAGING_FACTOR) * (float)(usecsNow - _lastMessageReceived) / 1000000.0f; + } + _lastMessageReceived = usecsNow; + +} else { qCDebug(interfaceapp) << "[Error] DDE Face Tracker Decode Error"; } _lastReceiveTimestamp = usecTimestampNow(); diff --git a/interface/src/devices/DdeFaceTracker.h b/interface/src/devices/DdeFaceTracker.h index 3cea667327..261b538d83 100644 --- a/interface/src/devices/DdeFaceTracker.h +++ b/interface/src/devices/DdeFaceTracker.h @@ -23,6 +23,8 @@ #include "FaceTracker.h" +const float STARTING_DDE_MESSAGE_TIME = 0.033f; + class DdeFaceTracker : public FaceTracker, public Dependency { Q_OBJECT SINGLETON_DEPENDENCY @@ -100,6 +102,11 @@ private: int _jawOpenIndex; QVector _coefficients; + + quint64 _lastMessageReceived = 0; + float _averageMessageTime = STARTING_DDE_MESSAGE_TIME; + glm::vec3 _lastHeadTranslation = glm::vec3(0.0f); + glm::vec3 _filteredHeadTranslation = glm::vec3(0.0f); }; #endif // hifi_DdeFaceTracker_h \ No newline at end of file