angular velocity is a vector, not a quaternion

interface/src/avatar/MyAvatar.cpp

@@ -509,25 +509,25 @@ glm::vec3 MyAvatar::getRightHandTipPosition() const {
 controller::Pose MyAvatar::getLeftHandPose() const {
     auto palmData = getHandData()->getCopyOfPalmData(HandData::LeftHand);
     return palmData.isValid() ? controller::Pose(palmData.getPosition(), palmData.getRotation(),
-        palmData.getVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose();
+        palmData.getVelocity(), palmData.getRawAngularVelocity()) : controller::Pose();
 }
 
 controller::Pose MyAvatar::getRightHandPose() const {
     auto palmData = getHandData()->getCopyOfPalmData(HandData::RightHand);
     return palmData.isValid() ? controller::Pose(palmData.getPosition(), palmData.getRotation(),
-        palmData.getVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose();
+        palmData.getVelocity(), palmData.getRawAngularVelocity()) : controller::Pose();
 }
 
 controller::Pose MyAvatar::getLeftHandTipPose() const {
     auto palmData = getHandData()->getCopyOfPalmData(HandData::LeftHand);
     return palmData.isValid() ? controller::Pose(palmData.getTipPosition(), palmData.getRotation(),
-        palmData.getTipVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose();
+        palmData.getTipVelocity(), palmData.getRawAngularVelocity()) : controller::Pose();
 }
 
 controller::Pose MyAvatar::getRightHandTipPose() const {
     auto palmData = getHandData()->getCopyOfPalmData(HandData::RightHand);
     return palmData.isValid() ? controller::Pose(palmData.getTipPosition(), palmData.getRotation(),
-        palmData.getTipVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose();
+        palmData.getTipVelocity(), palmData.getRawAngularVelocity()) : controller::Pose();
 }
 
 // virtual

libraries/avatars/src/HandData.h

@@ -112,7 +112,6 @@ public:
 
     void setRawAngularVelocity(const glm::vec3& angularVelocity) { _rawAngularVelocity = angularVelocity; }
     const glm::vec3& getRawAngularVelocity() const { return _rawAngularVelocity; }
-    glm::quat getRawAngularVelocityAsQuat() const { return glm::quat(_rawAngularVelocity); }
 
     void addToPosition(const glm::vec3& delta);
 

libraries/controllers/src/controllers/Pose.cpp

@@ -16,7 +16,7 @@
 namespace controller {
 
     Pose::Pose(const vec3& translation, const quat& rotation,
-            const vec3& velocity, const quat& angularVelocity) :
+            const vec3& velocity, const vec3& angularVelocity) :
             translation(translation), rotation(rotation), velocity(velocity), angularVelocity(angularVelocity), valid (true) { }
 
     bool Pose::operator==(const Pose& right) const {
@@ -35,7 +35,7 @@ namespace controller {
         obj.setProperty("translation", vec3toScriptValue(engine, pose.translation));
         obj.setProperty("rotation", quatToScriptValue(engine, pose.rotation));
         obj.setProperty("velocity", vec3toScriptValue(engine, pose.velocity));
-        obj.setProperty("angularVelocity", quatToScriptValue(engine, pose.angularVelocity));
+        obj.setProperty("angularVelocity", vec3toScriptValue(engine, pose.angularVelocity));
         obj.setProperty("valid", pose.valid);
 
         return obj;

libraries/controllers/src/controllers/Pose.h

@@ -23,12 +23,12 @@ namespace controller {
         vec3 translation;
         quat rotation;
         vec3 velocity;
-        quat angularVelocity;
+        vec3 angularVelocity;
         bool valid{ false };
 
         Pose() {}
         Pose(const vec3& translation, const quat& rotation,
-             const vec3& velocity = vec3(), const quat& angularVelocity = quat());
+             const vec3& velocity = vec3(), const vec3& angularVelocity = vec3());
 
         Pose(const Pose&) = default;
         Pose& operator = (const Pose&) = default;
@@ -38,7 +38,7 @@ namespace controller {
         vec3 getTranslation() const { return translation; }
         quat getRotation() const { return rotation; }
         vec3 getVelocity() const { return velocity; }
-        quat getAngularVelocity() const { return angularVelocity; }
+        vec3 getAngularVelocity() const { return angularVelocity; }
 
         static QScriptValue toScriptValue(QScriptEngine* engine, const Pose& event);
         static void fromScriptValue(const QScriptValue& object, Pose& event);

plugins/hifiSixense/src/SixenseManager.cpp

@@ -191,7 +191,7 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) {
                 //  Rotation of Palm
                 glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]);
                 handlePoseEvent(deltaTime, position, rotation, left);
-                rawPoses[i] = controller::Pose(position, rotation, glm::vec3(0), glm::quat());
+                rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO);
             } else {
                 _poseStateMap.clear();
                 _collectedSamples.clear();
@@ -457,25 +457,21 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos
     rotation = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand;
 
     glm::vec3 velocity(0.0f);
-    glm::quat angularVelocity;
+    glm::vec3 angularVelocity(0.0f);
 
     if (prevPose.isValid() && deltaTime > std::numeric_limits<float>::epsilon()) {
+        auto& samples = _collectedSamples[hand];
 
         velocity = (position - prevPose.getTranslation()) / deltaTime;
-
-        auto deltaRot = rotation * glm::conjugate(prevPose.getRotation());
-        auto axis = glm::axis(deltaRot);
-        auto angle = glm::angle(deltaRot);
-        angularVelocity = glm::angleAxis(angle / deltaTime, axis);
-
-        // Average
-        auto& samples = _collectedSamples[hand];
         samples.first.addSample(velocity);
         velocity = samples.first.average;
 
-        // FIXME: // Not using quaternion average yet for angular velocity because it s probably wrong but keep the MovingAverage in place
+        auto deltaRot = rotation * glm::conjugate(prevPose.getRotation());
-        //samples.second.addSample(glm::vec4(angularVelocity.x, angularVelocity.y, angularVelocity.z, angularVelocity.w));
+        auto axis = glm::axis(deltaRot);
-        //angularVelocity = glm::quat(samples.second.average.w, samples.second.average.x, samples.second.average.y, samples.second.average.z);
+        auto speed = glm::angle(deltaRot) / deltaTime;
+        angularVelocity = speed * axis;
+        samples.second.addSample(angularVelocity);
+        angularVelocity = samples.second.average;
     } else if (!prevPose.isValid()) {
         _collectedSamples[hand].first.clear();
         _collectedSamples[hand].second.clear();

plugins/hifiSixense/src/SixenseManager.h

@@ -54,7 +54,7 @@ private:
 
     template<typename T>
     using SampleAverage = MovingAverage<T, MAX_NUM_AVERAGING_SAMPLES>;
-    using Samples = std::pair<SampleAverage<glm::vec3>, SampleAverage<glm::vec4>>;
+    using Samples = std::pair<SampleAverage<glm::vec3>, SampleAverage<glm::vec3>>;
     using MovingAverageMap = std::map<int, Samples>;
 
     class InputDevice : public controller::InputDevice {