mirror of
https://github.com/HifiExperiments/overte.git
synced 2025-07-05 06:51:11 +02:00
f6c4f6b9c4
When in HMD mode, holding your hand controller in front of your face, would often result in a 1-2 mm pop in wrist position. This was due to a problem in the IK elbow constraint. When the swing rotation out of the elbow's reference frame was close to but not exactly identity it would not be properly constrainted. But when the magnitude of that swing became big enough the constraint would engage. This small change in swing rotation was noticable as a 1-2 mm movement in the wrist.
83 lines
2.7 KiB
C++
83 lines
2.7 KiB
C++
//
|
|
// ElbowConstraint.cpp
|
|
//
|
|
// Copyright 2015 High Fidelity, Inc.
|
|
//
|
|
// Distributed under the Apache License, Version 2.0.
|
|
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
|
|
//
|
|
|
|
#include "ElbowConstraint.h"
|
|
|
|
#include <algorithm>
|
|
|
|
#include <GeometryUtil.h>
|
|
#include <NumericalConstants.h>
|
|
#include "AnimUtil.h"
|
|
|
|
ElbowConstraint::ElbowConstraint() :
|
|
_minAngle(-PI),
|
|
_maxAngle(PI)
|
|
{
|
|
}
|
|
|
|
void ElbowConstraint::setHingeAxis(const glm::vec3& axis) {
|
|
float axisLength = glm::length(axis);
|
|
assert(axisLength > EPSILON);
|
|
_axis = axis / axisLength;
|
|
|
|
// for later we need a second axis that is perpendicular to the first
|
|
for (int i = 0; i < 3; ++i) {
|
|
float component = _axis[i];
|
|
const float MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT = 0.57735f; // just under 1/sqrt(3)
|
|
if (fabsf(component) > MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT) {
|
|
int j = (i + 1) % 3;
|
|
int k = (j + 1) % 3;
|
|
_perpAxis[i] = - _axis[j];
|
|
_perpAxis[j] = component;
|
|
_perpAxis[k] = 0.0f;
|
|
_perpAxis = glm::normalize(_perpAxis);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ElbowConstraint::setAngleLimits(float minAngle, float maxAngle) {
|
|
// NOTE: min/maxAngle angles should be in the range [-PI, PI]
|
|
_minAngle = glm::min(minAngle, maxAngle);
|
|
_maxAngle = glm::max(minAngle, maxAngle);
|
|
}
|
|
|
|
bool ElbowConstraint::apply(glm::quat& rotation) const {
|
|
// decompose the rotation into swing and twist
|
|
// NOTE: rotation = postRotation * referenceRotation
|
|
glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
|
|
glm::quat swingRotation, twistRotation;
|
|
swingTwistDecomposition(postRotation, _axis, swingRotation, twistRotation);
|
|
// NOTE: postRotation = swingRotation * twistRotation
|
|
|
|
// compute twistAngle
|
|
float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
|
|
glm::vec3 twisted = twistRotation * _perpAxis;
|
|
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(_perpAxis, twisted), _axis));
|
|
|
|
// clamp twistAngle
|
|
float clampedTwistAngle = glm::clamp(twistAngle, _minAngle, _maxAngle);
|
|
bool twistWasClamped = (twistAngle != clampedTwistAngle);
|
|
|
|
// update rotation
|
|
if (twistWasClamped) {
|
|
twistRotation = glm::angleAxis(clampedTwistAngle, _axis);
|
|
}
|
|
// we discard all swing and only keep twist
|
|
rotation = twistRotation * _referenceRotation;
|
|
return true;
|
|
}
|
|
|
|
glm::quat ElbowConstraint::computeCenterRotation() const {
|
|
const size_t NUM_LIMITS = 2;
|
|
glm::quat limits[NUM_LIMITS];
|
|
limits[0] = glm::angleAxis(_minAngle, _axis) * _referenceRotation;
|
|
limits[1] = glm::angleAxis(_maxAngle, _axis) * _referenceRotation;
|
|
return averageQuats(NUM_LIMITS, limits);
|
|
}
|