diff --git a/CMakeLists.txt b/CMakeLists.txt index b7fa55d4a2..2451ab240a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ add_definitions(-DGLM_FORCE_RADIANS) if (WIN32) add_definitions(-DNOMINMAX -D_CRT_SECURE_NO_WARNINGS) set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} "C:\\Program Files\\Microsoft SDKs\\Windows\\v7.1 ") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") elseif (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-long-long -pedantic") #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-unknown-pragmas") diff --git a/assignment-client/src/audio/AudioMixer.cpp b/assignment-client/src/audio/AudioMixer.cpp index 1c3046b9b1..a38f3b6e4b 100644 --- a/assignment-client/src/audio/AudioMixer.cpp +++ b/assignment-client/src/audio/AudioMixer.cpp @@ -99,6 +99,7 @@ void AudioMixer::addBufferToMixForListeningNodeWithBuffer(PositionalAudioRingBuf bool shouldAttenuate = (bufferToAdd != listeningNodeBuffer); if (shouldAttenuate) { + // if the two buffer pointers do not match then these are different buffers glm::vec3 relativePosition = bufferToAdd->getPosition() - listeningNodeBuffer->getPosition(); diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index 8e4aecdf51..429084480d 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -11,6 +11,8 @@ #include +#include +//#include #include #include "JointState.h" @@ -18,7 +20,48 @@ JointState::JointState() : _animationPriority(0.0f), _fbxJoint(NULL), - _isConstrained(false) { + _constraint(NULL) { +} + +JointState::JointState(const JointState& other) : _constraint(NULL) { + _transform = other._transform; + _rotation = other._rotation; + _rotationInParentFrame = other._rotationInParentFrame; + _animationPriority = other._animationPriority; + _fbxJoint = other._fbxJoint; + // DO NOT copy _constraint +} + +JointState::~JointState() { + delete _constraint; + _constraint = NULL; + if (_constraint) { + delete _constraint; + _constraint = NULL; + } +} + +void JointState::setFBXJoint(const FBXJoint* joint) { + assert(joint != NULL); + _rotationInParentFrame = joint->rotation; + // NOTE: JointState does not own the FBXJoint to which it points. + _fbxJoint = joint; + if (_constraint) { + delete _constraint; + _constraint = NULL; + } +} + +void JointState::updateConstraint() { + if (_constraint) { + delete _constraint; + _constraint = NULL; + } + if (glm::distance2(glm::vec3(-PI), _fbxJoint->rotationMin) > EPSILON || + glm::distance2(glm::vec3(PI), _fbxJoint->rotationMax) > EPSILON ) { + // this joint has rotation constraints + _constraint = AngularConstraint::newAngularConstraint(_fbxJoint->rotationMin, _fbxJoint->rotationMax); + } } void JointState::copyState(const JointState& state) { @@ -30,18 +73,7 @@ void JointState::copyState(const JointState& state) { _visibleTransform = state._visibleTransform; _visibleRotation = extractRotation(_visibleTransform); _visibleRotationInParentFrame = state._visibleRotationInParentFrame; - // DO NOT copy _fbxJoint -} - -void JointState::setFBXJoint(const FBXJoint* joint) { - assert(joint != NULL); - _rotationInParentFrame = joint->rotation; - // NOTE: JointState does not own the FBXJoint to which it points. - _fbxJoint = joint; - // precompute whether there are any constraints or not - float distanceMin = glm::distance(_fbxJoint->rotationMin, glm::vec3(-PI)); - float distanceMax = glm::distance(_fbxJoint->rotationMax, glm::vec3(PI)); - _isConstrained = distanceMin > EPSILON || distanceMax > EPSILON; + // DO NOT copy _fbxJoint or _constraint } void JointState::computeTransform(const glm::mat4& parentTransform) { @@ -70,11 +102,15 @@ void JointState::restoreRotation(float fraction, float priority) { } } -void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority) { +void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority, bool constrain) { // rotation is from bind- to model-frame assert(_fbxJoint != NULL); if (priority >= _animationPriority) { - setRotationInParentFrame(_rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation)); + glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation); + if (constrain && _constraint) { + _constraint->softClamp(targetRotation, _rotationInParentFrame, 0.5f); + } + setRotationInParentFrame(targetRotation); _animationPriority = priority; } } @@ -99,7 +135,7 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa return; } _animationPriority = priority; - if (!constrain || !_isConstrained) { + if (!constrain || _constraint == NULL) { // no constraints _rotationInParentFrame = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation; _rotation = delta * _rotation; @@ -122,10 +158,12 @@ void JointState::mixRotationDelta(const glm::quat& delta, float mixFactor, float if (mixFactor > 0.0f && mixFactor <= 1.0f) { targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor); } + if (_constraint) { + _constraint->softClamp(targetRotation, _rotationInParentFrame, 0.5f); + } setRotationInParentFrame(targetRotation); } - glm::quat JointState::computeParentRotation() const { // R = Rp * Rpre * r * Rpost // Rp = R * (Rpre * r * Rpost)^ diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index 8412cfd0cb..3bd752cdff 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -18,15 +18,19 @@ #include +class AngularConstraint; + class JointState { public: JointState(); - - void copyState(const JointState& state); + JointState(const JointState& other); + ~JointState(); void setFBXJoint(const FBXJoint* joint); const FBXJoint& getFBXJoint() const { return *_fbxJoint; } + void updateConstraint(); + void copyState(const JointState& state); void computeTransform(const glm::mat4& parentTransform); @@ -64,7 +68,7 @@ public: /// \param rotation is from bind- to model-frame /// computes and sets new _rotationInParentFrame /// NOTE: the JointState's model-frame transform/rotation are NOT updated! - void setRotationFromBindFrame(const glm::quat& rotation, float priority); + void setRotationFromBindFrame(const glm::quat& rotation, float priority, bool constrain = false); void setRotationInParentFrame(const glm::quat& targetRotation); const glm::quat& getRotationInParentFrame() const { return _rotationInParentFrame; } @@ -95,7 +99,7 @@ private: glm::quat _visibleRotationInParentFrame; const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint - bool _isConstrained; + AngularConstraint* _constraint; // JointState owns its AngularConstraint }; #endif // hifi_JointState_h diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index ffca2c8ea6..86d742c949 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -561,8 +561,6 @@ bool Model::updateGeometry() { void Model::setJointStates(QVector states) { _jointStates = states; - // compute an approximate bounding radius for broadphase collision queries - // against PhysicsSimulation boundaries int numJoints = _jointStates.size(); float radius = 0.0f; for (int i = 0; i < numJoints; ++i) { @@ -570,6 +568,7 @@ void Model::setJointStates(QVector states) { if (distance > radius) { radius = distance; } + _jointStates[i].updateConstraint(); } for (int i = 0; i < _jointStates.size(); i++) { _jointStates[i].slaveVisibleTransform(); @@ -1159,14 +1158,9 @@ void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm: } glm::quat deltaRotation = rotationBetween(leverArm, targetPosition - pivot); - /* DON'T REMOVE! This code provides the gravitational effect on the IK solution. - * It is commented out for the moment because we're blending the IK solution with - * the default pose which provides similar stability, but we might want to use - * gravity again later. - - // We want to mix the shortest rotation with one that will pull the system down with gravity. - // So we compute a simplified center of mass, where each joint has a mass of 1.0 and we don't - // bother averaging it because we only need direction. + // We want to mix the shortest rotation with one that will pull the system down with gravity + // so that limbs don't float unrealistically. To do this we compute a simplified center of mass + // where each joint has unit mass and we don't bother averaging it because we only need direction. if (j > 1) { glm::vec3 centerOfMass(0.0f); @@ -1188,11 +1182,9 @@ void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm: } deltaRotation = safeMix(deltaRotation, gravityDelta, mixFactor); } - */ // Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose - // at in the process. This provides stability to the IK solution and removes the necessity - // for the gravity effect. + // at in the process. This provides stability to the IK solution for most models. glm::quat oldNextRotation = nextState.getRotation(); float mixFactor = 0.03f; nextState.mixRotationDelta(deltaRotation, mixFactor, priority); @@ -1217,7 +1209,7 @@ void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm: } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR); // set final rotation of the end joint - endState.setRotationFromBindFrame(targetRotation, priority); + endState.setRotationFromBindFrame(targetRotation, priority, true); _shapesAreDirty = !_shapes.isEmpty(); } diff --git a/interface/src/ui/ModelsBrowser.cpp b/interface/src/ui/ModelsBrowser.cpp index ec583a14f7..a803625206 100644 --- a/interface/src/ui/ModelsBrowser.cpp +++ b/interface/src/ui/ModelsBrowser.cpp @@ -214,7 +214,7 @@ void ModelHandler::update() { NetworkAccessManager& networkAccessManager = NetworkAccessManager::getInstance(); QNetworkRequest request(url); QNetworkReply* reply = networkAccessManager.head(request); - connect(reply, SIGNAL(finished()), SLOT(processCheck())); + connect(reply, SIGNAL(finished()), SLOT(downloadFinished())); } _lock.unlock(); } @@ -266,7 +266,7 @@ void ModelHandler::queryNewFiles(QString marker) { NetworkAccessManager& networkAccessManager = NetworkAccessManager::getInstance(); QNetworkRequest request(url); QNetworkReply* reply = networkAccessManager.get(request); - connect(reply, SIGNAL(finished()), SLOT(processCheck())); + connect(reply, SIGNAL(finished()), SLOT(downloadFinished())); } diff --git a/libraries/script-engine/src/ScriptEngine.cpp b/libraries/script-engine/src/ScriptEngine.cpp index 63ae58b924..172233845a 100644 --- a/libraries/script-engine/src/ScriptEngine.cpp +++ b/libraries/script-engine/src/ScriptEngine.cpp @@ -471,7 +471,10 @@ void ScriptEngine::run() { // pack a placeholder value for sequence number for now, will be packed when destination node is known int numPreSequenceNumberBytes = audioPacket.size(); - packetStream << (quint16)0; + packetStream << (quint16) 0; + + // assume scripted avatar audio is mono and set channel flag to zero + packetStream << (quint8) 0; // use the orientation and position of this avatar for the source of this audio packetStream.writeRawData(reinterpret_cast(&_avatarData->getPosition()), sizeof(glm::vec3)); diff --git a/libraries/shared/src/AngularConstraint.cpp b/libraries/shared/src/AngularConstraint.cpp new file mode 100644 index 0000000000..4689568ac8 --- /dev/null +++ b/libraries/shared/src/AngularConstraint.cpp @@ -0,0 +1,201 @@ +// +// 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 + +#include "AngularConstraint.h" +#include "SharedUtil.h" + +// 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 the average range of the two non-roll rotations + glm::vec3 range = maxAngles - minAngles; + float coneAngle = 0.25f * (range[0] + range[1]); + return new ConeRollerConstraint(coneAngle, coneAxis, minAngles.z, maxAngles.z); + } + return NULL; +} + +bool AngularConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) { + glm::quat clampedTarget = targetRotation; + bool clamped = clamp(clampedTarget); + if (clamped) { + // check if oldRotation is also clamped + glm::quat clampedOld = oldRotation; + bool clamped2 = clamp(clampedOld); + if (clamped2) { + // oldRotation is already beyond the constraint + // we clamp again midway between targetRotation and clamped oldPosition + clampedTarget = glm::shortMix(clampedOld, targetRotation, mixFraction); + // and then clamp that + clamp(clampedTarget); + } + // finally we mix targetRotation with the clampedTarget + targetRotation = glm::shortMix(clampedTarget, targetRotation, mixFraction); + } + return clamped; +} + +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::clamp(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)); + 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; +} + +bool HingeConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) { + // the hinge works best without a soft clamp + return clamp(targetRotation); +} + +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::clamp(glm::quat& rotation) const { + bool applied = false; + glm::vec3 rotatedAxis = rotation * _coneAxis; + glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis); + float perpAxisLength = glm::length(perpAxis); + if (perpAxisLength > EPSILON) { + perpAxis /= perpAxisLength; + // enforce the cone + float angle = acosf(glm::dot(rotatedAxis, _coneAxis)); + if (angle > _coneAngle) { + rotation = glm::angleAxis(angle - _coneAngle, perpAxis) * rotation; + rotatedAxis = rotation * _coneAxis; + applied = true; + } + } else { + // the rotation is 100% roll + // there is no obvious perp axis so we must pick one + perpAxis = rotatedAxis; + // find the first non-zero element: + float iValue = 0.0f; + int i = 0; + for (i = 0; i < 3; ++i) { + if (fabsf(perpAxis[i]) > EPSILON) { + iValue = perpAxis[i]; + break; + } + } + assert(i != 3); + // swap or negate the next element + int j = (i + 1) % 3; + float jValue = perpAxis[j]; + if (fabsf(jValue - iValue) > EPSILON) { + perpAxis[i] = jValue; + perpAxis[j] = iValue; + } else { + perpAxis[i] = -iValue; + } + perpAxis = glm::cross(perpAxis, rotatedAxis); + perpAxisLength = glm::length(perpAxis); + assert(perpAxisLength > EPSILON); + perpAxis /= perpAxisLength; + } + // measure the roll + // NOTE: perpAxis is perpendicular to both _coneAxis and rotatedConeAxis, so we can + // rotate it again and we'll end up with an something that has only been rolled. + glm::vec3 rolledPerpAxis = rotation * perpAxis; + float sign = glm::dot(rotatedAxis, glm::cross(perpAxis, rolledPerpAxis)) > 0.0f ? 1.0f : -1.0f; + float roll = sign * angleBetween(rolledPerpAxis, perpAxis); + if (roll < _minRoll || roll > _maxRoll) { + float clampedRoll = clampAngle(roll, _minRoll, _maxRoll); + rotation = glm::normalize(glm::angleAxis(clampedRoll - 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..929a58959b --- /dev/null +++ b/libraries/shared/src/AngularConstraint.h @@ -0,0 +1,55 @@ +// +// 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 clamp(glm::quat& rotation) const = 0; + virtual bool softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction); +protected: +}; + +class HingeConstraint : public AngularConstraint { +public: + HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle); + virtual bool clamp(glm::quat& rotation) const; + virtual bool softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction); +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 clamp(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..00916a7267 --- /dev/null +++ b/tests/shared/src/AngularConstraintTests.cpp @@ -0,0 +1,476 @@ +// +// 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->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not clamp()" << 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->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not clamp()" << 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->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should not clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: HingeConstraint should clamp()" << 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; + } + } + delete c; +} + +void AngularConstraintTests::testConeRollerConstraint() { + float minAngleX = -PI / 5.0f; + float minAngleY = -PI / 5.0f; + float minAngleZ = -PI / 8.0f; + + float maxAngleX = PI / 4.0f; + float maxAngleY = PI / 3.0f; + float maxAngleZ = PI / 4.0f; + + glm::vec3 minAngles(minAngleX, minAngleY, minAngleZ); + glm::vec3 maxAngles(maxAngleX, maxAngleY, maxAngleZ); + AngularConstraint* c = AngularConstraint::newAngularConstraint(minAngles, maxAngles); + + float expectedConeAngle = 0.25 * (maxAngleX - minAngleX + maxAngleY - minAngleY); + 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 expectedConeAxis = pitch * yaw * glm::vec3(0.0f, 0.0f, 1.0f); + + glm::vec3 xAxis(1.0f, 0.0f, 0.0f); + glm::vec3 perpAxis = glm::normalize(xAxis - glm::dot(xAxis, expectedConeAxis) * expectedConeAxis); + + if (!c) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: newAngularConstraint() should make a constraint" << std::endl; + } + { // test in middle of constraint + glm::vec3 angles(PI/20.0f, 0.0f, PI/10.0f); + glm::quat rotation(angles); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not change rotation" << std::endl; + } + } + float deltaAngle = 0.001f; + { // test just inside edge of cone + glm::quat rotation = glm::angleAxis(expectedConeAngle - deltaAngle, perpAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not change rotation" << std::endl; + } + } + { // test just outside edge of cone + glm::quat rotation = glm::angleAxis(expectedConeAngle + deltaAngle, perpAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should change rotation" << std::endl; + } + } + { // test just inside min edge of roll + glm::quat rotation = glm::angleAxis(minAngleZ + deltaAngle, expectedConeAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not change rotation" << std::endl; + } + } + { // test just inside max edge of roll + glm::quat rotation = glm::angleAxis(maxAngleZ - deltaAngle, expectedConeAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; + } + if (rotation != newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should not change rotation" << std::endl; + } + } + { // test just outside min edge of roll + glm::quat rotation = glm::angleAxis(minAngleZ - deltaAngle, expectedConeAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(minAngleZ, expectedConeAxis); + if (fabsf(1.0f - glm::dot(newRotation, expectedRotation)) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test just outside max edge of roll + glm::quat rotation = glm::angleAxis(maxAngleZ + deltaAngle, expectedConeAxis); + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should change rotation" << std::endl; + } + glm::quat expectedRotation = glm::angleAxis(maxAngleZ, expectedConeAxis); + if (fabsf(1.0f - glm::dot(newRotation, expectedRotation)) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + deltaAngle = 0.25f * expectedConeAngle; + { // test far outside cone and min roll + glm::quat roll = glm::angleAxis(minAngleZ - deltaAngle, expectedConeAxis); + glm::quat pitchYaw = glm::angleAxis(expectedConeAngle + deltaAngle, perpAxis); + glm::quat rotation = pitchYaw * roll; + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should change rotation" << std::endl; + } + glm::quat expectedRoll = glm::angleAxis(minAngleZ, expectedConeAxis); + glm::quat expectedPitchYaw = glm::angleAxis(expectedConeAngle, perpAxis); + glm::quat expectedRotation = expectedPitchYaw * expectedRoll; + if (fabsf(1.0f - glm::dot(newRotation, expectedRotation)) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + { // test far outside cone and max roll + glm::quat roll = glm::angleAxis(maxAngleZ + deltaAngle, expectedConeAxis); + glm::quat pitchYaw = glm::angleAxis(- expectedConeAngle - deltaAngle, perpAxis); + glm::quat rotation = pitchYaw * roll; + + glm::quat newRotation = rotation; + bool constrained = c->clamp(newRotation); + if (!constrained) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; + } + if (rotation == newRotation) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: ConeRollerConstraint should change rotation" << std::endl; + } + glm::quat expectedRoll = glm::angleAxis(maxAngleZ, expectedConeAxis); + glm::quat expectedPitchYaw = glm::angleAxis(- expectedConeAngle, perpAxis); + glm::quat expectedRotation = expectedPitchYaw * expectedRoll; + if (fabsf(1.0f - glm::dot(newRotation, expectedRotation)) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: rotation = " << newRotation << " but expected " << expectedRotation << std::endl; + } + } + delete c; +} + +void AngularConstraintTests::runAllTests() { + testHingeConstraint(); + testConeRollerConstraint(); +} 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 diff --git a/tests/shared/src/main.cpp b/tests/shared/src/main.cpp index 3ae1b7b34d..6215d394a8 100644 --- a/tests/shared/src/main.cpp +++ b/tests/shared/src/main.cpp @@ -8,9 +8,11 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include "AngularConstraintTests.h" #include "MovingPercentileTests.h" int main(int argc, char** argv) { MovingPercentileTests::runAllTests(); + AngularConstraintTests::runAllTests(); return 0; }