Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-24 06:53:59 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #5786 from AndrewMeadows/ik-with-constraints

Browse source 
inverse kinematics works with rotation constraints
This commit is contained in:
Howard Stearns 2015-09-11 17:34:50 -07:00
commit 4ffe2d0747
12 changed files with 197 additions and 130 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
interface/src/Menu.cpp
View file

@ -467,7 +467,6 @@ Menu::Menu() {
SLOT(toggleConnexion(bool)));
MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHands, 0, true);
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false);
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandMouseInput, 0, true);
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::LowVelocityFilter, 0, true,

1
interface/src/Menu.h
View file

@ -175,7 +175,6 @@ namespace MenuOption {
const QString DisableNackPackets = "Disable Entity NACK Packets";
const QString DiskCacheEditor = "Disk Cache Editor";
const QString DisplayCrashOptions = "Display Crash Options";
const QString DisplayHands = "Show Hand Info";
const QString DisplayHandTargets = "Show Hand Targets";
const QString DisplayModelBounds = "Display Model Bounds";
const QString DisplayModelTriangles = "Display Model Triangles";

5
interface/src/avatar/Avatar.cpp
View file

@ -602,7 +602,10 @@ void Avatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, floa
getHead()->render(renderArgs, 1.0f, renderFrustum);
}
getHand()->render(renderArgs, false);
if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE &&
Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
getHand()->renderHandTargets(renderArgs, false);
}
}
getHead()->renderLookAts(renderArgs);
}

100
interface/src/avatar/Hand.cpp
View file

@ -8,22 +8,21 @@
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include <QImage>
#include "Hand.h"
#include <glm/glm.hpp>
#include <GeometryUtil.h>
#include <NodeList.h>
#include <RenderArgs.h>
#include "Avatar.h"
#include "AvatarManager.h"
#include "Hand.h"
#include "Menu.h"
#include "MyAvatar.h"
#include "Util.h"
#include "world.h"
using namespace std;
const float PALM_COLLISION_RADIUS = 0.03f;
Hand::Hand(Avatar* owningAvatar) :
HandData((AvatarData*)owningAvatar),
_owningAvatar(owningAvatar)
@ -40,76 +39,67 @@ void Hand::simulate(float deltaTime, bool isMine) {
}
}
void Hand::render(RenderArgs* renderArgs, bool isMine) {
void Hand::renderHandTargets(RenderArgs* renderArgs, bool isMine) {
float avatarScale = 1.0f;
if (_owningAvatar) {
avatarScale = _owningAvatar->getScale();
}
const float alpha = 1.0f;
const glm::vec3 redColor(1.0f, 0.0f, 0.0f); // Color the hand targets red to be different than skin
const glm::vec3 greenColor(0.0f, 1.0f, 0.0f); // Color the hand targets red to be different than skin
const glm::vec3 blueColor(0.0f, 0.0f, 1.0f); // Color the hand targets red to be different than skin
const glm::vec3 grayColor(0.5f);
const int NUM_FACETS = 8;
const float SPHERE_RADIUS = 0.03f * avatarScale;
gpu::Batch& batch = *renderArgs->_batch;
if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE &&
Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes)) {
// draw a green sphere at hand joint location, which is actually near the wrist)
if (isMine) {
for (size_t i = 0; i < getNumPalms(); i++) {
PalmData& palm = getPalms()[i];
if (!palm.isActive()) {
continue;
}
// draw a gray sphere at the target position of the "Hand" joint
glm::vec3 position = palm.getPosition();
Transform transform = Transform();
transform.setTranslation(position);
transform.setRotation(palm.getRotation());
batch.setModelTransform(transform);
DependencyManager::get<GeometryCache>()->renderSphere(batch, PALM_COLLISION_RADIUS * _owningAvatar->getScale(), 10, 10, glm::vec3(0.0f, 1.0f, 0.0f));
DependencyManager::get<GeometryCache>()->renderSphere(batch, SPHERE_RADIUS,
NUM_FACETS, NUM_FACETS, grayColor);
// draw a green sphere at the old "finger tip"
position = palm.getTipPosition();
transform.setTranslation(position);
batch.setModelTransform(transform);
DependencyManager::get<GeometryCache>()->renderSphere(batch, SPHERE_RADIUS,
NUM_FACETS, NUM_FACETS, greenColor, false);
}
}
if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE && Menu::getInstance()->isOptionChecked(MenuOption::DisplayHands)) {
renderHandTargets(renderArgs, isMine);
}
const float AXIS_RADIUS = 0.1f * SPHERE_RADIUS;
const float AXIS_LENGTH = 10.0f * SPHERE_RADIUS;
}
void Hand::renderHandTargets(RenderArgs* renderArgs, bool isMine) {
gpu::Batch& batch = *renderArgs->_batch;
const float avatarScale = DependencyManager::get<AvatarManager>()->getMyAvatar()->getScale();
const float alpha = 1.0f;
const glm::vec3 handColor(1.0, 0.0, 0.0); // Color the hand targets red to be different than skin
if (isMine && Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
for (size_t i = 0; i < getNumPalms(); ++i) {
PalmData& palm = getPalms()[i];
if (!palm.isActive()) {
continue;
}
glm::vec3 targetPosition = palm.getTipPosition();
Transform transform = Transform();
transform.setTranslation(targetPosition);
batch.setModelTransform(transform);
const float collisionRadius = 0.05f;
DependencyManager::get<GeometryCache>()->renderSphere(batch, collisionRadius, 10, 10, glm::vec4(0.5f,0.5f,0.5f, alpha), false);
}
}
const float PALM_BALL_RADIUS = 0.03f * avatarScale;
const float PALM_DISK_RADIUS = 0.06f * avatarScale;
const float PALM_DISK_THICKNESS = 0.01f * avatarScale;
const float PALM_FINGER_ROD_RADIUS = 0.003f * avatarScale;
// Draw the palm ball and disk
// Draw the coordinate frames of the hand targets
for (size_t i = 0; i < getNumPalms(); ++i) {
PalmData& palm = getPalms()[i];
if (palm.isActive()) {
glm::vec3 tip = palm.getTipPosition();
glm::vec3 root = palm.getPosition();
const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
glm::quat palmRotation = palm.getRotation();
Transform transform = Transform();
transform.setTranslation(glm::vec3());
batch.setModelTransform(transform);
Avatar::renderJointConnectingCone(batch, root, tip, PALM_FINGER_ROD_RADIUS, PALM_FINGER_ROD_RADIUS, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
glm::vec3 tip = root + palmRotation * glm::vec3(AXIS_LENGTH, 0.0f, 0.0f);
Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(redColor.r, redColor.g, redColor.b, alpha));
// Render sphere at palm/finger root
glm::vec3 offsetFromPalm = root + palm.getNormal() * PALM_DISK_THICKNESS;
Avatar::renderJointConnectingCone(batch, root, offsetFromPalm, PALM_DISK_RADIUS, 0.0f, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
transform = Transform();
transform.setTranslation(root);
batch.setModelTransform(transform);
DependencyManager::get<GeometryCache>()->renderSphere(batch, PALM_BALL_RADIUS, 20.0f, 20.0f, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
tip = root + palmRotation * glm::vec3(0.0f, AXIS_LENGTH, 0.0f);
Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(greenColor.r, greenColor.g, greenColor.b, alpha));
tip = root + palmRotation * glm::vec3(0.0f, 0.0f, AXIS_LENGTH);
Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(blueColor.r, blueColor.g, blueColor.b, alpha));
}
}
}

23
interface/src/avatar/Hand.h
View file

@ -11,41 +11,26 @@
#ifndef hifi_Hand_h
#define hifi_Hand_h
#include <vector>
#include <QAction>
#include <glm/glm.hpp>
#include <SharedUtil.h>
#include <AvatarData.h>
#include <AudioScriptingInterface.h>
#include <HandData.h>
#include <Model.h>
#include "world.h"
class Avatar;
class RenderArgs;
class Hand : public HandData {
public:
Hand(Avatar* owningAvatar);
void simulate(float deltaTime, bool isMine);
void render(RenderArgs* renderArgs, bool isMine);
void renderHandTargets(RenderArgs* renderArgs, bool isMine);
private:
// disallow copies of the Hand, copy of owning Avatar is disallowed too
Hand(const Hand&);
Hand& operator= (const Hand&);
int _controllerButtons; /// Button states read from hand-held controllers
int _controllerButtons; /// Button states read from hand-held controllers
Avatar* _owningAvatar;
void renderHandTargets(RenderArgs* renderArgs, bool isMine);
Avatar* _owningAvatar;
};
#endif // hifi_Hand_h

5
interface/src/avatar/MyAvatar.cpp
View file

@ -1251,7 +1251,10 @@ void MyAvatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, fl
getHead()->renderLookAts(renderArgs);
}
getHand()->render(renderArgs, true);
if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE &&
Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
getHand()->renderHandTargets(renderArgs, true);
}
}
void MyAvatar::setVisibleInSceneIfReady(Model* model, render::ScenePointer scene, bool visible) {

139
libraries/animation/src/AnimInverseKinematics.cpp
View file

@ -178,15 +178,15 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
// compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[index].trans;
glm::vec3 leverArm = tip - jointPosition;
glm::vec3 pivotLine = targetPose.trans - jointPosition;
glm::vec3 targetLine = targetPose.trans - jointPosition;
// compute the axis of the rotation that would align them
glm::vec3 axis = glm::cross(leverArm, pivotLine);
glm::vec3 axis = glm::cross(leverArm, targetLine);
float axisLength = glm::length(axis);
if (axisLength > EPSILON) {
// compute deltaRotation for alignment (brings tip closer to target)
axis /= axisLength;
float angle = acosf(glm::dot(leverArm, pivotLine) / (glm::length(leverArm) * glm::length(pivotLine)));
float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
// NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
// still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
@ -203,7 +203,13 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
glm::quat newRot = glm::normalize(glm::inverse(absolutePoses[parentIndex].rot) * deltaRotation * absolutePoses[index].rot);
RotationConstraint* constraint = getConstraint(index);
if (constraint) {
constraint->apply(newRot);
bool constrained = constraint->apply(newRot);
if (constrained) {
// the constraint will modify the movement of the tip so we have to compute the modified
// model-frame deltaRotation
// Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^
deltaRotation = absolutePoses[parentIndex].rot * newRot * glm::inverse(absolutePoses[index].rot);
}
}
_relativePoses[index].rot = newRot;
}
@ -258,7 +264,18 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
//virtual
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
loadPoses(underPoses);
if (_relativePoses.size() != underPoses.size()) {
loadPoses(underPoses);
} else {
// relax toward underpose
const float RELAXATION_TIMESCALE = 0.25f;
const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f);
int numJoints = (int)_relativePoses.size();
for (int i = 0; i < numJoints; ++i) {
float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot));
_relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, alpha));
}
}
return evaluate(animVars, dt, triggersOut);
}
@ -291,34 +308,34 @@ void AnimInverseKinematics::initConstraints() {
// We create constraints for the joints shown here
// (and their Left counterparts if applicable).
//
// O RightHand
// /
// O /
// | O RightForeArm
// Neck O /
// | /
// O-------O-------O----O----O RightArm
// Spine2|
// |
// O Spine1
//
// O RightHand
// Head /
// O /
// Neck| O RightForeArm
// O /
// O | O / RightShoulder
// O-------O-------O' \|/ 'O
// Spine2 O RightArm
// |
// |
// O Spine
// Spine1 O
// |
// |
// O---O---O RightUpLeg
// Spine O
// y |
// | |
// | O---O---O RightUpLeg
// z | | |
// \ | | |
// \| | |
// x -----+ O O RightLeg
// | |
// | |
// | |
// O O RightLeg
// | |
// y | |
// | | |
// | O O RightFoot
// z | / /
// \ | O--O O--O
// \|
// x -----+
// O O RightFoot
// / /
// O--O O--O
loadDefaultPoses(_skeleton->getRelativeBindPoses());
@ -337,21 +354,23 @@ void AnimInverseKinematics::initConstraints() {
_constraints.clear();
for (int i = 0; i < numJoints; ++i) {
QString name = _skeleton->getJointName(i);
bool isLeft = name.startsWith("Left", Qt::CaseInsensitive);
// compute the joint's baseName and remember if it was Left or not
QString baseName = _skeleton->getJointName(i);
bool isLeft = baseName.startsWith("Left", Qt::CaseInsensitive);
float mirror = isLeft ? -1.0f : 1.0f;
if (isLeft) {
//name.remove(0, 4);
} else if (name.startsWith("Right", Qt::CaseInsensitive)) {
//name.remove(0, 5);
baseName.remove(0, 4);
} else if (baseName.startsWith("Right", Qt::CaseInsensitive)) {
baseName.remove(0, 5);
}
RotationConstraint* constraint = nullptr;
if (0 == name.compare("Arm", Qt::CaseInsensitive)) {
if (0 == baseName.compare("Arm", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
/* KEEP THIS CODE for future experimentation
// these directions are approximate swing limits in root-frame
// NOTE: they don't need to be normalized
std::vector<glm::vec3> swungDirections;
@ -371,9 +390,16 @@ void AnimInverseKinematics::initConstraints() {
swungDirections[j] = invAbsoluteRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
*/
// simple cone
std::vector<float> minDots;
const float MAX_HAND_SWING = PI / 2.0f;
minDots.push_back(cosf(MAX_HAND_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("UpLeg", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("UpLegXXX", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
@ -399,12 +425,19 @@ void AnimInverseKinematics::initConstraints() {
stConstraint->setSwingLimits(swungDirections);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("RightHand", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("Hand", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_HAND_TWIST = PI / 2.0f;
stConstraint->setTwistLimits(-MAX_HAND_TWIST, MAX_HAND_TWIST);
const float MAX_HAND_TWIST = PI;
const float MIN_HAND_TWIST = -PI / 2.0f;
if (isLeft) {
stConstraint->setTwistLimits(-MAX_HAND_TWIST, -MIN_HAND_TWIST);
} else {
stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST);
}
/* KEEP THIS CODE for future experimentation
* a more complicated wrist with asymmetric cone
// these directions are approximate swing limits in parent-frame
// NOTE: they don't need to be normalized
std::vector<glm::vec3> swungDirections;
@ -422,15 +455,28 @@ void AnimInverseKinematics::initConstraints() {
swungDirections[j] = invRelativeRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
*/
/*
// simple cone
std::vector<float> minDots;
const float MAX_HAND_SWING = PI / 3.0f;
const float MAX_HAND_SWING = PI / 2.0f;
minDots.push_back(cosf(MAX_HAND_SWING));
stConstraint->setSwingLimits(minDots);
*/
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (name.startsWith("SpineXXX", Qt::CaseInsensitive)) {
} else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_SHOULDER_TWIST = PI / 8.0f;
stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
std::vector<float> minDots;
const float MAX_SHOULDER_SWING = PI / 14.0f;
minDots.push_back(cosf(MAX_SHOULDER_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (baseName.startsWith("Spine", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_SPINE_TWIST = PI / 8.0f;
@ -442,7 +488,7 @@ void AnimInverseKinematics::initConstraints() {
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("NeckXXX", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_NECK_TWIST = PI / 2.0f;
@ -454,7 +500,7 @@ void AnimInverseKinematics::initConstraints() {
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("ForeArm", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("ForeArm", Qt::CaseInsensitive)) {
// The elbow joint rotates about the parent-frame's zAxis (-zAxis) for the Right (Left) arm.
ElbowConstraint* eConstraint = new ElbowConstraint();
glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@ -464,7 +510,7 @@ void AnimInverseKinematics::initConstraints() {
// then measure the angles to swing the yAxis into alignment
glm::vec3 hingeAxis = - mirror * zAxis;
const float MIN_ELBOW_ANGLE = 0.0f;
const float MAX_ELBOW_ANGLE = 7.0f * PI / 8.0f;
const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f;
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * yAxis;
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * yAxis;
@ -485,7 +531,7 @@ void AnimInverseKinematics::initConstraints() {
eConstraint->setAngleLimits(minAngle, maxAngle);
constraint = static_cast<RotationConstraint*>(eConstraint);
} else if (0 == name.compare("Leg", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("LegXXX", Qt::CaseInsensitive)) {
// The knee joint rotates about the parent-frame's -xAxis.
ElbowConstraint* eConstraint = new ElbowConstraint();
glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@ -516,7 +562,7 @@ void AnimInverseKinematics::initConstraints() {
eConstraint->setAngleLimits(minAngle, maxAngle);
constraint = static_cast<RotationConstraint*>(eConstraint);
} else if (0 == name.compare("Foot", Qt::CaseInsensitive)) {
} else if (0 == baseName.compare("FootXXX", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
@ -558,14 +604,11 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
_maxTargetIndex = 0;
// No constraints!
/*
if (skeleton) {
initConstraints();
} else {
clearConstraints();
}
*/
}
void AnimInverseKinematics::relaxTowardDefaults(float dt) {

2
libraries/animation/src/AnimInverseKinematics.h
View file

@ -46,9 +46,9 @@ protected:
struct IKTargetVar {
IKTargetVar(const QString& jointNameIn, const std::string& positionVarIn, const std::string& rotationVarIn) :
jointName(jointNameIn),
positionVar(positionVarIn),
rotationVar(rotationVarIn),
jointName(jointNameIn),
jointIndex(-1),
hasPerformedJointLookup(false) {}

45
libraries/animation/src/SwingTwistConstraint.cpp
View file

@ -19,6 +19,10 @@
const float MIN_MINDOT = -0.999f;
const float MAX_MINDOT = 1.0f;
const int LAST_CLAMP_LOW_BOUNDARY = -1;
const int LAST_CLAMP_NO_BOUNDARY = 0;
const int LAST_CLAMP_HIGH_BOUNDARY = 1;
SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {
setCone(PI);
}
@ -67,7 +71,8 @@ SwingTwistConstraint::SwingTwistConstraint() :
RotationConstraint(),
_swingLimitFunction(),
_minTwist(-PI),
_maxTwist(PI)
_maxTwist(PI),
_lastTwistBoundary(0)
{
}
@ -158,10 +163,11 @@ void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
// NOTE: min/maxTwist angles should be in the range [-PI, PI]
_minTwist = glm::min(minTwist, maxTwist);
_maxTwist = glm::max(minTwist, maxTwist);
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}
bool SwingTwistConstraint::apply(glm::quat& rotation) const {
// decompose the rotation into first twist about yAxis, then swing about something perp
const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
// NOTE: rotation = postRotation * referenceRotation
@ -176,9 +182,43 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
glm::vec3 twistedX = twistRotation * xAxis;
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis));
// adjust measured twistAngle according to clamping history
switch (_lastTwistBoundary) {
case LAST_CLAMP_LOW_BOUNDARY:
// clamp to min
if (twistAngle > _maxTwist) {
twistAngle -= TWO_PI;
}
break;
case LAST_CLAMP_HIGH_BOUNDARY:
// clamp to max
if (twistAngle < _minTwist) {
twistAngle += TWO_PI;
}
break;
default: // LAST_CLAMP_NO_BOUNDARY
// clamp to nearest boundary
float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
if (twistAngle > midBoundary) {
// lower boundary is closer --> phase down one cycle
twistAngle -= TWO_PI;
} else if (twistAngle < midBoundary - TWO_PI) {
// higher boundary is closer --> phase up one cycle
twistAngle += TWO_PI;
}
break;
}
// clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
bool twistWasClamped = (twistAngle != clampedTwistAngle);
// remember twist's clamp boundary history
if (twistWasClamped) {
_lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
} else {
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}
// clamp the swing
// The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
@ -201,6 +241,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
}
if (swingWasClamped || twistWasClamped) {
// update the rotation
twistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
rotation = swingRotation * twistRotation * _referenceRotation;
return true;

4
libraries/animation/src/SwingTwistConstraint.h
View file

@ -77,6 +77,10 @@ protected:
SwingLimitFunction _swingLimitFunction;
float _minTwist;
float _maxTwist;
// We want to remember the LAST clamped boundary, so we an use it even when the far boundary is closer.
// This reduces "pops" when the input twist angle goes far beyond and wraps around toward the far boundary.
mutable int _lastTwistBoundary;
};
#endif // hifi_SwingTwistConstraint_h

1
libraries/avatars/src/HandData.h
View file

@ -19,6 +19,7 @@
#include <glm/gtc/quaternion.hpp>
#include <NumericalConstants.h>
#include <SharedUtil.h>
class AvatarData;
class PalmData;

1
libraries/physics/src/DynamicCharacterController.h
View file

@ -12,7 +12,6 @@ class btCollisionWorld;
const int NUM_CHARACTER_CONTROLLER_RAYS = 2;
///DynamicCharacterController is obsolete/unsupported at the moment
class DynamicCharacterController : public btCharacterControllerInterface
{
protected: