From d6541e9ee77ce1fdca7ea003b08f4030b3854b4e Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 18 Dec 2015 11:06:58 -0800 Subject: [PATCH] angular velocity is a vector, not a quaternion --- interface/src/avatar/MyAvatar.cpp | 14 ++++++------ libraries/avatars/src/HandData.cpp | 8 +++---- libraries/avatars/src/HandData.h | 19 ++++++++-------- .../controllers/src/controllers/Pose.cpp | 6 ++--- libraries/controllers/src/controllers/Pose.h | 6 ++--- plugins/hifiSixense/src/SixenseManager.cpp | 22 ++++++++----------- plugins/hifiSixense/src/SixenseManager.h | 8 +++---- 7 files changed, 39 insertions(+), 44 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 5c8230bd88..c989f1dc71 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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()->getHashCopy(); - + foreach (const AvatarSharedPointer& avatarPointer, hash) { auto avatar = static_pointer_cast(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 diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp index 5d783a671e..7ba23b01ad 100644 --- a/libraries/avatars/src/HandData.cpp +++ b/libraries/avatars/src/HandData.cpp @@ -14,7 +14,7 @@ #include #include -#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); diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index d782f240ee..34ed610f80 100644 --- a/libraries/avatars/src/HandData.h +++ b/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 _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; diff --git a/libraries/controllers/src/controllers/Pose.cpp b/libraries/controllers/src/controllers/Pose.cpp index 2281fc98ff..f07472e9e0 100644 --- a/libraries/controllers/src/controllers/Pose.cpp +++ b/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; diff --git a/libraries/controllers/src/controllers/Pose.h b/libraries/controllers/src/controllers/Pose.h index b8d27824f3..a8a4452758 100644 --- a/libraries/controllers/src/controllers/Pose.h +++ b/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); diff --git a/plugins/hifiSixense/src/SixenseManager.cpp b/plugins/hifiSixense/src/SixenseManager.cpp index 93329d768e..fb47697166 100644 --- a/plugins/hifiSixense/src/SixenseManager.cpp +++ b/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::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(); diff --git a/plugins/hifiSixense/src/SixenseManager.h b/plugins/hifiSixense/src/SixenseManager.h index 5cd4dc2451..0de5fe93aa 100644 --- a/plugins/hifiSixense/src/SixenseManager.h +++ b/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 using SampleAverage = MovingAverage; - using Samples = std::pair, SampleAverage>; + using Samples = std::pair, SampleAverage>; using MovingAverageMap = std::map; 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 };