Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-19 15:43:50 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Adding a amoving Average for the velocity of the hydra

Browse source
This commit is contained in:
samcake 2015-10-23 15:03:04 -07:00
parent 30b44d1e33
commit f14a321d03
2 changed files with 49 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
libraries/input-plugins/src/input-plugins/SixenseManager.cpp
View file

@ -178,6 +178,7 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) {
if (sixenseGetNumActiveControllers() == 0) {
_poseStateMap.clear();
_collectedSamples.clear();
return;
}
@ -234,9 +235,12 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) {
} else {
_poseStateMap.clear();
_collectedSamples.clear();
}
} else {
_poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose();
auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND;
_poseStateMap[hand] = controller::Pose();
_collectedSamples[hand].clear();
}
}
@ -387,6 +391,8 @@ void SixenseManager::handleButtonEvent(unsigned int buttons, bool left) {
void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left) {
#ifdef HAVE_SIXENSE
auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND;
// From ABOVE the sixense coordinate frame looks like this:
//
// |
@ -401,7 +407,7 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
// |
// |
// z
auto prevPose = _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND];
auto prevPose = _poseStateMap[hand];
// Transform the measured position into body frame.
position = _avatarRotation * (position + _avatarPosition);
@ -448,7 +454,10 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
glm::vec3 velocity(0.0f);
glm::quat angularVelocity;
if (prevPose.isValid() && deltaTime > std::numeric_limits<float>::epsilon()) {
velocity = (position - prevPose.getTranslation()) / deltaTime;
auto deltaRot = rotation * glm::conjugate(prevPose.getRotation());
@ -456,9 +465,15 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
auto angle = glm::angle(deltaRot);
angularVelocity = glm::angleAxis(angle / deltaTime, axis);
// Average
auto& samples = _collectedSamples[hand];
samples.addSample(velocity);
velocity = samples.average;
} else if (!prevPose.isValid()) {
_collectedSamples[hand].clear();
}
_poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(position, rotation, velocity, angularVelocity);
_poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity);
#endif // HAVE_SIXENSE
}

31
libraries/input-plugins/src/input-plugins/SixenseManager.h
View file

@ -94,6 +94,36 @@ private:
glm::vec3 _reachRight;
float _lastDistance;
bool _useSixenseFilter = true;
template <class T, int MAX_NUM_SAMPLES> class MovingAverage {
public:
using Samples = std::list< T >;
Samples samples;
T average;
void clear() {
samples.clear();
}
bool isAverageValid() const { return !samples.empty(); }
void addSample(T sample) {
samples.push_front(sample);
int numSamples = samples.size();
if (numSamples < MAX_NUM_SAMPLES) {
average = (sample + average * float(numSamples - 1)) / float(numSamples);
} else {
T tail = samples.back();
samples.pop_back();
average = average + (sample - tail) / float(numSamples);
}
}
};
static const int MAX_NUM_AVERAGING_SAMPLES = 10; // At ~100 updates per seconds this means averaging over ~.1s
using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >;
MovingAverageMap _collectedSamples;
#ifdef __APPLE__
QLibrary* _sixenseLibrary;
@ -109,3 +139,4 @@ private:
};
#endif // hifi_SixenseManager_h