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
@@ -536,7 +536,7 @@ void MyAvatar::render(RenderArgs* renderArgs, const glm::vec3& cameraPosition) {
     if (!_shouldRender) {
         return; // exit early
     }
-    
+
     Avatar::render(renderArgs, cameraPosition);
 }
 
@@ -799,7 +799,7 @@ void MyAvatar::updateLookAtTargetAvatar() {
     const float GREATEST_LOOKING_AT_DISTANCE = 10.0f;
 
     AvatarHash hash = DependencyManager::get<AvatarManager>()->getHashCopy();
-    
+
     foreach (const AvatarSharedPointer& avatarPointer, hash) {
         auto avatar = static_pointer_cast<Avatar>(avatarPointer);
         bool isCurrentTarget = avatar->getIsLookAtTarget();
@@ -1175,7 +1175,7 @@ void MyAvatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, fl
     if (!_skeletonModel.isRenderable()) {
         return; // wait until all models are loaded
     }
-    
+
     fixupModelsInScene();
 
     //  Render head so long as the camera isn't inside it

libraries/avatars/src/HandData.cpp

@@ -14,7 +14,7 @@
 #include <GeometryUtil.h>
 #include <SharedUtil.h>
 
-#include "AvatarData.h" 
+#include "AvatarData.h"
 #include "HandData.h"
 
 
@@ -38,7 +38,7 @@ PalmData& HandData::addNewPalm(Hand whichHand)  {
 PalmData HandData::getCopyOfPalmData(Hand hand) const {
     QReadLocker locker(&_palmsLock);
 
-    // the palms are not necessarily added in left-right order, 
+    // the palms are not necessarily added in left-right order,
     // so we have to search for the correct hand
     for (const auto& palm : _palms) {
         if (palm.whichHand() == hand && palm.isActive()) {
@@ -64,7 +64,7 @@ void PalmData::addToPosition(const glm::vec3& delta) {
     _rawPosition += _owningHandData->worldToLocalVector(delta);
 }
 
-bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
+bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration,
                                         const PalmData*& collidingPalm) const {
     QReadLocker locker(&_palmsLock);
 
@@ -93,7 +93,7 @@ glm::vec3 HandData::getBasePosition() const {
 float HandData::getBaseScale() const {
     return _owningAvatarData->getTargetScale();
 }
-        
+
 glm::vec3 PalmData::getFingerDirection() const {
     // finger points along yAxis in hand-frame
     const glm::vec3 LOCAL_FINGER_DIRECTION(0.0f, 1.0f, 0.0f);

libraries/avatars/src/HandData.h

@@ -38,7 +38,7 @@ public:
 
     HandData(AvatarData* owningAvatar);
     virtual ~HandData() {}
-    
+
     // position conversion
     glm::vec3 localToWorldPosition(const glm::vec3& localPosition) {
         return getBasePosition() + getBaseOrientation() * localPosition * getBaseScale();
@@ -60,7 +60,7 @@ public:
     /// \param penetration[out] the vector in which to store the penetration
     /// \param collidingPalm[out] a const PalmData* to the palm that was collided with
     /// \return whether or not the sphere penetrated
-    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration,
         const PalmData*& collidingPalm) const;
 
     glm::quat getBaseOrientation() const;
@@ -74,7 +74,7 @@ protected:
     AvatarData* _owningAvatarData;
     std::vector<PalmData> _palms;
     mutable QReadWriteLock _palmsLock{ QReadWriteLock::Recursive };
-    
+
     glm::vec3 getBasePosition() const;
     float getBaseScale() const;
 
@@ -112,13 +112,12 @@ 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);
 
     void addToPenetration(const glm::vec3& penetration) { _totalPenetration += penetration; }
     void resolvePenetrations() { addToPosition(-_totalPenetration); _totalPenetration = glm::vec3(0.0f); }
-    
+
     void setTipPosition(const glm::vec3& position) { _tipPosition = position; }
     const glm::vec3 getTipPosition() const { return _owningHandData->localToWorldPosition(_tipPosition); }
     const glm::vec3& getTipRawPosition() const { return _tipPosition; }
@@ -126,16 +125,16 @@ public:
     void setTipVelocity(const glm::vec3& velocity) { _tipVelocity = velocity; }
     const glm::vec3 getTipVelocity() const { return _owningHandData->localToWorldDirection(_tipVelocity); }
     const glm::vec3& getTipRawVelocity() const { return _tipVelocity; }
-    
+
     void incrementFramesWithoutData() { _numFramesWithoutData++; }
     void resetFramesWithoutData() { _numFramesWithoutData = 0; }
     int  getFramesWithoutData() const { return _numFramesWithoutData; }
-    
+
     // FIXME - these are used in SkeletonModel::updateRig() the skeleton/rig should probably get this information
     // from an action and/or the UserInputMapper instead of piping it through here.
     void setTrigger(float trigger) { _trigger = trigger; }
     float getTrigger() const { return _trigger; }
-    
+
     // return world-frame:
     glm::vec3 getFingerDirection() const;
     glm::vec3 getNormal() const;
@@ -148,13 +147,13 @@ private:
     glm::vec3 _rawAngularVelocity;
     glm::quat _rawDeltaRotation;
     glm::quat _lastRotation;
-    
+
     glm::vec3 _tipPosition;
     glm::vec3 _tipVelocity;
     glm::vec3 _totalPenetration; /// accumulator for per-frame penetrations
 
     float _trigger;
-    
+
     bool _isActive; /// This has current valid data
     int _numFramesWithoutData; /// after too many frames without data, this tracked object assumed lost.
     HandData* _owningHandData;

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 {
@@ -26,7 +26,7 @@ namespace controller {
         }
 
         // FIXME add margin of error?  Or add an additional withinEpsilon function?
-        return translation == right.getTranslation() && rotation == right.getRotation() && 
+        return translation == right.getTranslation() && rotation == right.getRotation() &&
             velocity == right.getVelocity() && angularVelocity == right.getAngularVelocity();
     }
 
@@ -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;
+        samples.first.addSample(velocity);
+        velocity = samples.first.average;
 
         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
-        //samples.second.addSample(glm::vec4(angularVelocity.x, angularVelocity.y, angularVelocity.z, angularVelocity.w));
-        //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

@@ -49,12 +49,12 @@ private:
     static const int CALIBRATION_STATE_IDLE = 0;
     static const int CALIBRATION_STATE_IN_PROGRESS = 1;
     static const int CALIBRATION_STATE_COMPLETE = 2;
-    static const glm::vec3 DEFAULT_AVATAR_POSITION; 
+    static const glm::vec3 DEFAULT_AVATAR_POSITION;
     static const float CONTROLLER_THRESHOLD;
-    
+
     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 {
@@ -81,7 +81,7 @@ private:
         // these are calibration results
         glm::vec3 _avatarPosition { DEFAULT_AVATAR_POSITION }; // in hydra-frame
         glm::quat _avatarRotation; // in hydra-frame
-    
+
         float _lastDistance;
         bool _requestReset { false };
         bool _debugDrawRaw { false };