diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index e66a2f44e9..3b3c135eb5 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -11,6 +11,7 @@ #include +#include //#include #include @@ -18,7 +19,13 @@ JointState::JointState() : _animationPriority(0.0f), - _fbxJoint(NULL) { + _fbxJoint(NULL), + _constraint(NULL) { +} + +JointState::~JointState() { + delete _constraint; + _constraint = NULL; } void JointState::setFBXJoint(const FBXJoint* joint) { diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index b1a584d4ec..4eadc51f7c 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -18,9 +18,12 @@ #include +class AngularConstraint; + class JointState { public: JointState(); + ~JointState(); void setFBXJoint(const FBXJoint* joint); const FBXJoint& getFBXJoint() const { return *_fbxJoint; } @@ -61,6 +64,7 @@ private: glm::quat _rotation; // joint- to model-frame const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint + AngularConstraint* _constraint; }; #endif // hifi_JointState_h diff --git a/libraries/shared/src/AngularConstraint.cpp b/libraries/shared/src/AngularConstraint.cpp new file mode 100644 index 0000000000..bf565d586e --- /dev/null +++ b/libraries/shared/src/AngularConstraint.cpp @@ -0,0 +1,176 @@ +// +// AngularConstraint.cpp +// interface/src/renderer +// +// Created by Andrew Meadows on 2014.05.30 +// Copyright 2014 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 // adebug +#include + +#include "AngularConstraint.h" +#include "SharedUtil.h" +#include "StreamUtils.h" // adebug + +// helper function +/// \param angle radian angle to be clamped within angleMin and angleMax +/// \param angleMin minimum value +/// \param angleMax maximum value +/// \return value between minAngle and maxAngle closest to angle +float clampAngle(float angle, float angleMin, float angleMax) { + float minDistance = angle - angleMin; + float maxDistance = angle - angleMax; + if (maxDistance > 0.0f) { + minDistance = glm::min(minDistance, angleMin + TWO_PI - angle); + angle = (minDistance < maxDistance) ? angleMin : angleMax; + } else if (minDistance < 0.0f) { + maxDistance = glm::max(maxDistance, angleMax - TWO_PI - angle); + angle = (minDistance > maxDistance) ? angleMin : angleMax; + } + return angle; +} + +// static +AngularConstraint* AngularConstraint::newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles) { + float minDistance2 = glm::distance2(minAngles, glm::vec3(-PI, -PI, -PI)); + float maxDistance2 = glm::distance2(maxAngles, glm::vec3(PI, PI, PI)); + if (minDistance2 < EPSILON && maxDistance2 < EPSILON) { + // no constraint + return NULL; + } + // count the zero length elements + glm::vec3 rangeAngles = maxAngles - minAngles; + int pivotIndex = -1; + int numZeroes = 0; + for (int i = 0; i < 3; ++i) { + if (rangeAngles[i] < EPSILON) { + ++numZeroes; + } else { + pivotIndex = i; + } + } + if (numZeroes == 2) { + // this is a hinge + int forwardIndex = (pivotIndex + 1) % 3; + glm::vec3 forwardAxis(0.0f); + forwardAxis[forwardIndex] = 1.0f; + glm::vec3 rotationAxis(0.0f); + rotationAxis[pivotIndex] = 1.0f; + return new HingeConstraint(forwardAxis, rotationAxis, minAngles[pivotIndex], maxAngles[pivotIndex]); + } else if (numZeroes == 0) { + // approximate the angular limits with a cone roller + // we assume the roll is about z + glm::vec3 middleAngles = 0.5f * (maxAngles - minAngles); + glm::quat yaw = glm::angleAxis(middleAngles[1], glm::vec3(0.0f, 1.0f, 0.0f)); + glm::quat pitch = glm::angleAxis(middleAngles[0], glm::vec3(1.0f, 0.0f, 0.0f)); + glm::vec3 coneAxis = pitch * yaw * glm::vec3(0.0f, 0.0f, 1.0f); + // the coneAngle is half of the average range of the two non-roll rotations + float coneAngle = 0.25f * (middleAngles[0] + middleAngles[1]); + + return new ConeRollerConstraint(coneAngle, coneAxis, minAngles.z, maxAngles.z); + } + return NULL; +} + +HingeConstraint::HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle) + : _minAngle(minAngle), _maxAngle(maxAngle) { + assert(_minAngle < _maxAngle); + // we accept the rotationAxis direction + assert(glm::length(rotationAxis) > EPSILON); + _rotationAxis = glm::normalize(rotationAxis); + // but we compute the final _forwardAxis + glm::vec3 otherAxis = glm::cross(_rotationAxis, forwardAxis); + assert(glm::length(otherAxis) > EPSILON); + _forwardAxis = glm::normalize(glm::cross(otherAxis, _rotationAxis)); +} + +// virtual +bool HingeConstraint::applyTo(glm::quat& rotation) const { + glm::vec3 forward = rotation * _forwardAxis; + forward -= glm::dot(forward, _rotationAxis) * _rotationAxis; + float length = glm::length(forward); + if (length < EPSILON) { + // infinite number of solutions ==> choose the middle of the contrained range + rotation = glm::angleAxis(0.5f * (_minAngle + _maxAngle), _rotationAxis); + return true; + } + forward /= length; + float sign = (glm::dot(glm::cross(_forwardAxis, forward), _rotationAxis) > 0.0f ? 1.0f : -1.0f); + //float angle = sign * acos(glm::dot(forward, _forwardAxis) / length); + float angle = sign * acos(glm::dot(forward, _forwardAxis)); +// std::cout << "adebug forward = " << forward << " _forwardAxis = " << _forwardAxis << " angle = " << angle << std::endl; // adebug + glm::quat newRotation = glm::angleAxis(clampAngle(angle, _minAngle, _maxAngle), _rotationAxis); + if (fabsf(1.0f - glm::dot(newRotation, rotation)) > EPSILON * EPSILON) { + rotation = newRotation; + return true; + } + return false; +} + +ConeRollerConstraint::ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll) + : _coneAngle(coneAngle), _minRoll(minRoll), _maxRoll(maxRoll) { + assert(_maxRoll >= _minRoll); + float axisLength = glm::length(coneAxis); + assert(axisLength > EPSILON); + _coneAxis = coneAxis / axisLength; +} + +// virtual +bool ConeRollerConstraint::applyTo(glm::quat& rotation) const { + bool applied = false; + glm::vec3 rotatedAxis = rotation * _coneAxis; + glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis); + float perpAxisLength = glm::length(perpAxis); + + // enforce the cone + float angle = acosf(glm::dot(rotatedAxis, _coneAxis)); + if (angle > _coneAngle && perpAxisLength > EPSILON) { + perpAxis /= perpAxisLength; + rotation = glm::angleAxis(angle - _coneAngle, perpAxis) * rotation; + rotatedAxis = rotation * _coneAxis; + applied = true; + } + + // enforce the roll + if (perpAxisLength < EPSILON) { + // there is no obvious perp axis so we must pick one + perpAxis = rotatedAxis; + // find the first non-zero element: + float value = 0.0f; + int i = 0; + for (i = 0; i < 3; ++i) { + if (fabsf(perpAxis[i]) > EPSILON) { + value = perpAxis[i]; + break; + } + } + assert(i != 3); + // swap or negate the next element + int j = (i + 1) % 3; + float value2 = perpAxis[j]; + if (fabsf(value2 - value) > EPSILON) { + perpAxis[i] = value2; + perpAxis[j] = value; + } else { + perpAxis[i] = -value; + } + perpAxis = glm::cross(perpAxis, rotatedAxis); + perpAxisLength = glm::length(perpAxis); + assert(perpAxisLength > EPSILON); + } + perpAxis /= perpAxisLength; + glm::vec3 rotatedPerpAxis = rotation * perpAxis; + float roll = angleBetween(rotatedPerpAxis, perpAxis); + if (roll < _minRoll || roll > _maxRoll) { + float newRoll = clampAngle(roll, _minRoll, _maxRoll); + rotation = glm::angleAxis(newRoll - roll, rotatedAxis) * rotation; + applied = true; + } + return applied; +} + + diff --git a/libraries/shared/src/AngularConstraint.h b/libraries/shared/src/AngularConstraint.h new file mode 100644 index 0000000000..8003e4c9a3 --- /dev/null +++ b/libraries/shared/src/AngularConstraint.h @@ -0,0 +1,53 @@ +// +// AngularConstraint.h +// interface/src/renderer +// +// Created by Andrew Meadows on 2014.05.30 +// Copyright 2013 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 +// + +#ifndef hifi_AngularConstraint_h +#define hifi_AngularConstraint_h + +#include + + +class AngularConstraint { +public: + /// \param minAngles minumum euler angles for the constraint + /// \param maxAngles minumum euler angles for the constraint + /// \return pointer to new AngularConstraint of the right type or NULL if none could be made + static AngularConstraint* newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles); + + AngularConstraint() {} + virtual ~AngularConstraint() {} + virtual bool applyTo(glm::quat& rotation) const = 0; +protected: +}; + +class HingeConstraint : public AngularConstraint { +public: + HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle); + virtual bool applyTo(glm::quat& rotation) const; +protected: + glm::vec3 _forwardAxis; + glm::vec3 _rotationAxis; + float _minAngle; + float _maxAngle; +}; + +class ConeRollerConstraint : public AngularConstraint { +public: + ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll); + virtual bool applyTo(glm::quat& rotation) const; +private: + float _coneAngle; + glm::vec3 _coneAxis; + float _minRoll; + float _maxRoll; +}; + +#endif // hifi_AngularConstraint_h diff --git a/tests/shared/src/AngularConstraintTests.cpp b/tests/shared/src/AngularConstraintTests.cpp new file mode 100644 index 0000000000..4c087821c5 --- /dev/null +++ b/tests/shared/src/AngularConstraintTests.cpp @@ -0,0 +1,291 @@ +// +// AngularConstraintTests.cpp +// tests/physics/src +// +// Created by Andrew Meadows on 2014.05.30 +// Copyright 2014 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 + +#include +#include +#include + +#include "AngularConstraintTests.h" + + +void AngularConstraintTests::testHingeConstraint() { + float minAngle = -PI; + float maxAngle = 0.0f; + glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + glm::vec3 minAngles(0.0f, -PI, 0.0f); + glm::vec3 maxAngles(0.0f, 0.0f, 0.0f); + + AngularConstraint* c = AngularConstraint::newAngularConstraint(minAngles, maxAngles); + if (!c) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: newAngularConstraint() should make a constraint" << std::endl; + } + + { // test in middle of constraint + float angle = 0.5f * (minAngle + maxAngle); + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not applyTo()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not change rotation" << std::endl; + } + } + { // test just inside min edge of constraint + float angle = minAngle + 10.f * EPSILON; + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not applyTo()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not change rotation" << std::endl; + } + } + { // test just inside max edge of constraint + float angle = maxAngle - 10.f * EPSILON; + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not applyTo()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not change rotation" << std::endl; + } + } + { // test just outside min edge of constraint + float angle = minAngle - 0.001f; + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test just outside max edge of constraint + float angle = maxAngle + 0.001f; + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test far outside min edge of constraint (wraps around to max) + float angle = minAngle - 0.75f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test far outside max edge of constraint (wraps around to min) + float angle = maxAngle + 0.75f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + + float ACCEPTABLE_ERROR = 1.0e-4f; + { // test nearby but off-axis rotation + float offAngle = 0.1f; + glm::quat offRotation(offAngle, glm::vec3(1.0f, 0.0f, 0.0f)); + float angle = 0.5f * (maxAngle + minAngle); + glm::quat rotation = offRotation * glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(angle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test way off rotation > maxAngle + float offAngle = 0.5f; + glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f)); + float angle = maxAngle + 0.2f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + rotation = offRotation * glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test way off rotation < minAngle + float offAngle = 0.5f; + glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f)); + float angle = minAngle - 0.2f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + rotation = offRotation * glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test way off rotation > maxAngle with wrap over to minAngle + float offAngle = -0.5f; + glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f)); + float angle = maxAngle + 0.6f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + rotation = offRotation * glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test way off rotation < minAngle with wrap over to maxAngle + float offAngle = -0.6f; + glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f)); + float angle = minAngle - 0.7f * (TWO_PI - (maxAngle - minAngle)); + glm::quat rotation = glm::angleAxis(angle, yAxis); + rotation = offRotation * glm::angleAxis(angle, yAxis); + + glm::quat newRotation = rotation; + bool constrained = c->applyTo(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should applyTo()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis); + float qDot = glm::dot(expectedRotation, newRotation); + if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } +} + +void AngularConstraintTests::testConeRollerConstraint() { +} + +void AngularConstraintTests::runAllTests() { + testHingeConstraint(); +} diff --git a/tests/shared/src/AngularConstraintTests.h b/tests/shared/src/AngularConstraintTests.h new file mode 100644 index 0000000000..f0994f08c9 --- /dev/null +++ b/tests/shared/src/AngularConstraintTests.h @@ -0,0 +1,21 @@ +// +// AngularConstraintTests.h +// tests/physics/src +// +// Created by Andrew Meadows on 2014.05.30 +// Copyright 2014 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 +// + +#ifndef hifi_AngularConstraintTests_h +#define hifi_AngularConstraintTests_h + +namespace AngularConstraintTests { + void testHingeConstraint(); + void testConeRollerConstraint(); + void runAllTests(); +} + +#endif // hifi_AngularConstraintTests_h