Merge branch 'master' of https://github.com/highfidelity/hifi

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")

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();
         

interface/src/renderer/JointState.cpp

@@ -11,6 +11,8 @@
 
 #include <glm/gtx/norm.hpp>
 
+#include <AngularConstraint.h>
+//#include <GeometryUtil.h>
 #include <SharedUtil.h>
 
 #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
+    // DO NOT copy _fbxJoint or _constraint
-}
-
-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;
 }
 
 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)^

interface/src/renderer/JointState.h

@@ -18,15 +18,19 @@
 
 #include <FBXReader.h>
 
+class AngularConstraint;
+
 class JointState {
 public:
     JointState();
-
+    JointState(const JointState& other);
-    void copyState(const JointState& state);
+    ~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

interface/src/renderer/Model.cpp

@@ -561,8 +561,6 @@ bool Model::updateGeometry() {
 void Model::setJointStates(QVector<JointState> 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<JointState> 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.
+            // We want to mix the shortest rotation with one that will pull the system down with gravity
-            * It is commented out for the moment because we're blending the IK solution with 
+            // so that limbs don't float unrealistically.  To do this we compute a simplified center of mass
-            * the default pose which provides similar stability, but we might want to use
+            // where each joint has unit mass and we don't bother averaging it because we only need direction.
-            * 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.
             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
+            // at in the process.  This provides stability to the IK solution for most models.
-            // for the gravity effect.
             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();
 }

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()));
             
 }
 

libraries/script-engine/src/ScriptEngine.cpp

@@ -473,6 +473,9 @@ void ScriptEngine::run() {
                 int numPreSequenceNumberBytes = audioPacket.size();
                 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<const char*>(&_avatarData->getPosition()), sizeof(glm::vec3));
                 glm::quat headOrientation = _avatarData->getHeadOrientation();

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 <glm/gtx/norm.hpp>
+
+#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;
+}
+
+

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 <glm/glm.hpp>
+
+
+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

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 <iostream>
+
+#include <AngularConstraint.h>
+#include <SharedUtil.h>
+#include <StreamUtils.h>
+
+#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();
+}

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

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;
 }