From 69e463bbfdcfbacc2e185e6059d8e980be9c42a5 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 4 Sep 2015 16:01:05 -0700
Subject: [PATCH 01/12] AnimDebugDraw Improvements

* Now always works, regardless of whether or not Rig or AnimGraph animations
  are enabled.
* Changed joint radius to 1 cm.
* Changed xyz axis length to 4 cm.
---
 interface/src/avatar/MyAvatar.cpp            | 31 ++++----
 interface/src/avatar/MyAvatar.h              |  1 +
 libraries/animation/src/AnimSkeleton.cpp     | 74 ++++++++++++--------
 libraries/animation/src/AnimSkeleton.h       |  3 +
 libraries/animation/src/JointState.h         |  2 +
 libraries/animation/src/Rig.cpp              | 19 +++--
 libraries/animation/src/Rig.h                |  3 +
 libraries/render-utils/src/AnimDebugDraw.cpp | 74 +++++++++++++++++---
 libraries/render-utils/src/AnimDebugDraw.h   | 13 ++--
 9 files changed, 157 insertions(+), 63 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index de7e2ba8fb..89ebe650d0 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -735,7 +735,7 @@ void MyAvatar::setEnableDebugDrawAnimPose(bool isEnabled) {
     _enableDebugDrawAnimPose = isEnabled;
 
     if (!isEnabled) {
-        AnimDebugDraw::getInstance().removeAnimNode("myAvatar");
+        AnimDebugDraw::getInstance().removePoses("myAvatar");
     }
 }
 
@@ -1248,8 +1248,6 @@ void MyAvatar::initAnimGraph() {
 
 void MyAvatar::destroyAnimGraph() {
     _rig->destroyAnimGraph();
-    AnimDebugDraw::getInstance().removeSkeleton("myAvatar");
-    AnimDebugDraw::getInstance().removeAnimNode("myAvatar");
 }
 
 void MyAvatar::preRender(RenderArgs* renderArgs) {
@@ -1261,26 +1259,35 @@ void MyAvatar::preRender(RenderArgs* renderArgs) {
         initHeadBones();
         _skeletonModel.setCauterizeBoneSet(_headBoneSet);
         initAnimGraph();
+        _debugDrawSkeleton = std::make_shared<AnimSkeleton>(_skeletonModel.getGeometry()->getFBXGeometry());
     }
 
     if (_enableDebugDrawBindPose || _enableDebugDrawAnimPose) {
 
-        AnimSkeleton::ConstPointer animSkeleton = _rig->getAnimSkeleton();
-        AnimNode::ConstPointer animNode = _rig->getAnimNode();
-
-        // bones space is rotated
+        // bones are aligned such that z is forward, not -z.
         glm::quat rotY180 = glm::angleAxis((float)M_PI, glm::vec3(0.0f, 1.0f, 0.0f));
         AnimPose xform(glm::vec3(1), rotY180 * getOrientation(), getPosition());
 
-        if (animSkeleton && _enableDebugDrawBindPose) {
+        if (_enableDebugDrawBindPose && _debugDrawSkeleton) {
             glm::vec4 gray(0.2f, 0.2f, 0.2f, 0.2f);
-            AnimDebugDraw::getInstance().addSkeleton("myAvatar", animSkeleton, xform, gray);
+            AnimDebugDraw::getInstance().addSkeleton("myAvatar", _debugDrawSkeleton, xform, gray);
         }
 
-        // This only works for when new anim system is enabled, at the moment.
-        if (animNode && animSkeleton && _enableDebugDrawAnimPose && _rig->getEnableAnimGraph()) {
+        if (_enableDebugDrawAnimPose && _debugDrawSkeleton) {
             glm::vec4 cyan(0.1f, 0.6f, 0.6f, 1.0f);
-            AnimDebugDraw::getInstance().addAnimNode("myAvatar", animNode, xform, cyan);
+
+            // build AnimPoseVec from JointStates.
+            AnimPoseVec poses;
+            poses.reserve(_debugDrawSkeleton->getNumJoints());
+            for (int i = 0; i < _debugDrawSkeleton->getNumJoints(); i++) {
+                AnimPose pose = _debugDrawSkeleton->getRelativeBindPose(i);
+                glm::quat jointRot;
+                _rig->getJointRotationInConstrainedFrame(i, jointRot);
+                pose.rot = pose.rot * jointRot;
+                poses.push_back(pose);
+            }
+
+            AnimDebugDraw::getInstance().addPoses("myAvatar", _debugDrawSkeleton, poses, xform, cyan);
         }
     }
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index fadb5d669c..e743dea860 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -321,6 +321,7 @@ private:
 
     bool _enableDebugDrawBindPose = false;
     bool _enableDebugDrawAnimPose = false;
+    AnimSkeleton::ConstPointer _debugDrawSkeleton = nullptr;
 };
 
 #endif // hifi_MyAvatar_h
diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp
index 3f11607f26..fdc20db63c 100644
--- a/libraries/animation/src/AnimSkeleton.cpp
+++ b/libraries/animation/src/AnimSkeleton.cpp
@@ -58,7 +58,52 @@ AnimPose::operator glm::mat4() const {
                      glm::vec4(zAxis, 0.0f), glm::vec4(trans, 1.0f));
 }
 
+AnimSkeleton::AnimSkeleton(const FBXGeometry& fbxGeometry) {
+    // convert to std::vector of joints
+    std::vector<FBXJoint> joints;
+    joints.reserve(fbxGeometry.joints.size());
+    for (auto& joint : fbxGeometry.joints) {
+        joints.push_back(joint);
+    }
+
+    AnimPose geometryOffset(fbxGeometry.offset);
+    buildSkeletonFromJoints(joints, geometryOffset);
+}
+
 AnimSkeleton::AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset) {
+    buildSkeletonFromJoints(joints, geometryOffset);
+}
+
+int AnimSkeleton::nameToJointIndex(const QString& jointName) const {
+    for (size_t i = 0; i < _joints.size(); i++) {
+        if (_joints[i].name == jointName) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+int AnimSkeleton::getNumJoints() const {
+    return _joints.size();
+}
+
+AnimPose AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
+    return _absoluteBindPoses[jointIndex];
+}
+
+AnimPose AnimSkeleton::getRelativeBindPose(int jointIndex) const {
+    return _relativeBindPoses[jointIndex];
+}
+
+int AnimSkeleton::getParentIndex(int jointIndex) const {
+    return _joints[jointIndex].parentIndex;
+}
+
+const QString& AnimSkeleton::getJointName(int jointIndex) const {
+    return _joints[jointIndex].name;
+}
+
+void AnimSkeleton::buildSkeletonFromJoints(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset) {
     _joints = joints;
 
     // build a cache of bind poses
@@ -113,35 +158,6 @@ AnimSkeleton::AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose&
     }
 }
 
-int AnimSkeleton::nameToJointIndex(const QString& jointName) const {
-    for (size_t i = 0; i < _joints.size(); i++) {
-        if (_joints[i].name == jointName) {
-            return i;
-        }
-    }
-    return -1;
-}
-
-int AnimSkeleton::getNumJoints() const {
-    return _joints.size();
-}
-
-AnimPose AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
-    return _absoluteBindPoses[jointIndex];
-}
-
-AnimPose AnimSkeleton::getRelativeBindPose(int jointIndex) const {
-    return _relativeBindPoses[jointIndex];
-}
-
-int AnimSkeleton::getParentIndex(int jointIndex) const {
-    return _joints[jointIndex].parentIndex;
-}
-
-const QString& AnimSkeleton::getJointName(int jointIndex) const {
-    return _joints[jointIndex].name;
-}
-
 #ifndef NDEBUG
 void AnimSkeleton::dump() const {
     qCDebug(animation) << "[";
diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h
index c0c5036cc7..973e32232d 100644
--- a/libraries/animation/src/AnimSkeleton.h
+++ b/libraries/animation/src/AnimSkeleton.h
@@ -47,6 +47,7 @@ public:
     using Pointer = std::shared_ptr<AnimSkeleton>;
     using ConstPointer = std::shared_ptr<const AnimSkeleton>;
 
+    AnimSkeleton(const FBXGeometry& fbxGeometry);
     AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset);
     int nameToJointIndex(const QString& jointName) const;
     const QString& getJointName(int jointIndex) const;
@@ -66,6 +67,8 @@ public:
 #endif
 
 protected:
+    void buildSkeletonFromJoints(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset);
+
     std::vector<FBXJoint> _joints;
     AnimPoseVec _absoluteBindPoses;
     AnimPoseVec _relativeBindPoses;
diff --git a/libraries/animation/src/JointState.h b/libraries/animation/src/JointState.h
index 4f45661eb2..1fa7576b74 100644
--- a/libraries/animation/src/JointState.h
+++ b/libraries/animation/src/JointState.h
@@ -26,6 +26,8 @@ class AngularConstraint;
 
 class JointState {
 public:
+    friend class MyAvatar;
+
     JointState() {}
     JointState(const JointState& other) { copyState(other); }
     JointState(const FBXJoint& joint);
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index f2ea922ab7..18e6eff095 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -901,6 +901,14 @@ glm::quat Rig::setJointRotationInConstrainedFrame(int jointIndex, glm::quat targ
     return endRotation;
 }
 
+bool Rig::getJointRotationInConstrainedFrame(int jointIndex, glm::quat& quatOut) const {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+    quatOut = _jointStates[jointIndex].getRotationInConstrainedFrame();
+    return true;
+}
+
 void Rig::updateVisibleJointStates() {
     for (int i = 0; i < _jointStates.size(); i++) {
         _jointStates[i].slaveVisibleTransform();
@@ -1010,16 +1018,7 @@ void Rig::initAnimGraph(const QUrl& url, const FBXGeometry& fbxGeometry) {
         return;
     }
 
-    // convert to std::vector of joints
-    std::vector<FBXJoint> joints;
-    joints.reserve(fbxGeometry.joints.size());
-    for (auto& joint : fbxGeometry.joints) {
-        joints.push_back(joint);
-    }
-
-    // create skeleton
-    AnimPose geometryOffset(fbxGeometry.offset);
-    _animSkeleton = std::make_shared<AnimSkeleton>(joints, geometryOffset);
+    _animSkeleton = std::make_shared<AnimSkeleton>(fbxGeometry);
 
     // load the anim graph
     _animLoader.reset(new AnimNodeLoader(url));
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 0bf0645b4d..4d37dc92e6 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -51,6 +51,8 @@ typedef std::shared_ptr<Rig> RigPointer;
 
 class Rig : public QObject, public std::enable_shared_from_this<Rig> {
 
+    friend MyAvatar;
+
 public:
 
     struct HeadParameters {
@@ -153,6 +155,7 @@ public:
     glm::vec3 getJointDefaultTranslationInConstrainedFrame(int jointIndex);
     glm::quat setJointRotationInConstrainedFrame(int jointIndex, glm::quat targetRotation,
                                                  float priority, bool constrain = false, float mix = 1.0f);
+    bool getJointRotationInConstrainedFrame(int jointIndex, glm::quat& rotOut) const;
     glm::quat getJointDefaultRotationInParentFrame(int jointIndex);
     void updateVisibleJointStates();
 
diff --git a/libraries/render-utils/src/AnimDebugDraw.cpp b/libraries/render-utils/src/AnimDebugDraw.cpp
index 1c0f7e0054..44a58b3d67 100644
--- a/libraries/render-utils/src/AnimDebugDraw.cpp
+++ b/libraries/render-utils/src/AnimDebugDraw.cpp
@@ -152,22 +152,30 @@ AnimDebugDraw::~AnimDebugDraw() {
     }
 }
 
-void AnimDebugDraw::addSkeleton(std::string key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color) {
+void AnimDebugDraw::addSkeleton(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color) {
     _skeletons[key] = SkeletonInfo(skeleton, rootPose, color);
 }
 
-void AnimDebugDraw::removeSkeleton(std::string key) {
+void AnimDebugDraw::removeSkeleton(const std::string& key) {
     _skeletons.erase(key);
 }
 
-void AnimDebugDraw::addAnimNode(std::string key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color) {
+void AnimDebugDraw::addAnimNode(const std::string& key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color) {
     _animNodes[key] = AnimNodeInfo(animNode, rootPose, color);
 }
 
-void AnimDebugDraw::removeAnimNode(std::string key) {
+void AnimDebugDraw::removeAnimNode(const std::string& key) {
     _animNodes.erase(key);
 }
 
+void AnimDebugDraw::addPoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color) {
+    _poses[key] = PosesInfo(skeleton, poses, rootPose, color);
+}
+
+void AnimDebugDraw::removePoses(const std::string& key) {
+    _poses.erase(key);
+}
+
 static const uint32_t red = toRGBA(255, 0, 0, 255);
 static const uint32_t green = toRGBA(0, 255, 0, 255);
 static const uint32_t blue = toRGBA(0, 0, 255, 255);
@@ -176,6 +184,8 @@ const int NUM_CIRCLE_SLICES = 24;
 
 static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius, Vertex*& v) {
 
+    const float XYZ_AXIS_LENGTH = radius * 4.0f;
+
     AnimPose finalPose = rootPose * pose;
     glm::vec3 base = rootPose * pose.trans;
 
@@ -195,7 +205,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = red;
     v++;
-    v->pos = finalPose * glm::vec3(radius * 2.0f, 0.0f, 0.0f);
+    v->pos = finalPose * glm::vec3(XYZ_AXIS_LENGTH, 0.0f, 0.0f);
     v->rgba = red;
     v++;
 
@@ -213,7 +223,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = green;
     v++;
-    v->pos = finalPose * glm::vec3(0.0f, radius * 2.0f, 0.0f);
+    v->pos = finalPose * glm::vec3(0.0f, XYZ_AXIS_LENGTH, 0.0f);
     v->rgba = green;
     v++;
 
@@ -231,7 +241,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = blue;
     v++;
-    v->pos = finalPose * glm::vec3(0.0f, 0.0f, radius * 2.0f);
+    v->pos = finalPose * glm::vec3(0.0f, 0.0f, XYZ_AXIS_LENGTH);
     v->rgba = blue;
     v++;
 
@@ -322,7 +332,7 @@ void AnimDebugDraw::update() {
         const size_t VERTICES_PER_BONE = (6 + (NUM_CIRCLE_SLICES * 2) * 3);
         const size_t VERTICES_PER_LINK = 8 * 2;
 
-        const float BONE_RADIUS = 0.0075f;
+        const float BONE_RADIUS = 0.01f; // 1 cm
 
         // figure out how many verts we will need.
         int numVerts = 0;
@@ -350,6 +360,17 @@ void AnimDebugDraw::update() {
             }
         }
 
+        for (auto&& iter : _poses) {
+            AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
+            numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
+            for (int i = 0; i < skeleton->getNumJoints(); i++) {
+                auto parentIndex = skeleton->getParentIndex(i);
+                if (parentIndex >= 0) {
+                    numVerts += VERTICES_PER_LINK;
+                }
+            }
+        }
+
         data._vertexBuffer->resize(sizeof(Vertex) * numVerts);
         Vertex* verts = (Vertex*)data._vertexBuffer->editData();
         Vertex* v = verts;
@@ -418,6 +439,43 @@ void AnimDebugDraw::update() {
             }
         }
 
+        for (auto&& iter : _poses) {
+            AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
+            AnimPoseVec& poses = std::get<1>(iter.second);
+            AnimPose rootPose = std::get<2>(iter.second);
+            int hipsIndex = skeleton->nameToJointIndex("Hips");
+            if (hipsIndex >= 0) {
+                rootPose.trans -= skeleton->getRelativeBindPose(hipsIndex).trans;
+            }
+            glm::vec4 color = std::get<3>(iter.second);
+
+            std::vector<AnimPose> absAnimPose;
+            absAnimPose.resize(skeleton->getNumJoints());
+
+            for (int i = 0; i < skeleton->getNumJoints(); i++) {
+                AnimPose pose = poses[i];
+
+                const float radius = BONE_RADIUS / (pose.scale.x * rootPose.scale.x);
+
+                auto parentIndex = skeleton->getParentIndex(i);
+                if (parentIndex >= 0) {
+                    absAnimPose[i] = absAnimPose[parentIndex] * poses[i];
+                } else {
+                    absAnimPose[i] = poses[i];
+                }
+
+                // draw bone
+                addBone(rootPose, absAnimPose[i], radius, v);
+
+                // draw link to parent
+                if (parentIndex >= 0) {
+                    assert(parentIndex < skeleton->getNumJoints());
+                    AnimPose parentPose = poses[parentIndex];
+                    addLink(rootPose, absAnimPose[i], absAnimPose[parentIndex], radius, color, v);
+                }
+            }
+        }
+
         assert(numVerts == (v - verts));
 
         data._indexBuffer->resize(sizeof(uint16_t) * numVerts);
diff --git a/libraries/render-utils/src/AnimDebugDraw.h b/libraries/render-utils/src/AnimDebugDraw.h
index 489213b80b..cd17f62590 100644
--- a/libraries/render-utils/src/AnimDebugDraw.h
+++ b/libraries/render-utils/src/AnimDebugDraw.h
@@ -27,11 +27,14 @@ public:
     AnimDebugDraw();
     ~AnimDebugDraw();
 
-    void addSkeleton(std::string key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color);
-    void removeSkeleton(std::string key);
+    void addSkeleton(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color);
+    void removeSkeleton(const std::string& key);
 
-    void addAnimNode(std::string key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color);
-    void removeAnimNode(std::string key);
+    void addAnimNode(const std::string& key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color);
+    void removeAnimNode(const std::string& key);
+
+    void addPoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color);
+    void removePoses(const std::string& key);
 
     void update();
 
@@ -44,9 +47,11 @@ protected:
 
     typedef std::tuple<AnimSkeleton::ConstPointer, AnimPose, glm::vec4> SkeletonInfo;
     typedef std::tuple<AnimNode::ConstPointer, AnimPose, glm::vec4> AnimNodeInfo;
+    typedef std::tuple<AnimSkeleton::ConstPointer, AnimPoseVec, AnimPose, glm::vec4> PosesInfo;
 
     std::unordered_map<std::string, SkeletonInfo> _skeletons;
     std::unordered_map<std::string, AnimNodeInfo> _animNodes;
+    std::unordered_map<std::string, PosesInfo> _poses;
 
     // no copies
     AnimDebugDraw(const AnimDebugDraw&) = delete;

From bb5d061d782fbfc8db9c541f9a0176ad0b39927e Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 4 Sep 2015 16:17:35 -0700
Subject: [PATCH 02/12] linux and mac compile fixes.

Removed "friend MyAvatar" from Rig and JointState.
This was only there for debugging, but I accidentally committed it.
---
 libraries/animation/src/JointState.h | 2 --
 libraries/animation/src/Rig.h        | 3 ---
 2 files changed, 5 deletions(-)

diff --git a/libraries/animation/src/JointState.h b/libraries/animation/src/JointState.h
index 1fa7576b74..4f45661eb2 100644
--- a/libraries/animation/src/JointState.h
+++ b/libraries/animation/src/JointState.h
@@ -26,8 +26,6 @@ class AngularConstraint;
 
 class JointState {
 public:
-    friend class MyAvatar;
-
     JointState() {}
     JointState(const JointState& other) { copyState(other); }
     JointState(const FBXJoint& joint);
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 4d37dc92e6..254d708eaa 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -50,9 +50,6 @@ class Rig;
 typedef std::shared_ptr<Rig> RigPointer;
 
 class Rig : public QObject, public std::enable_shared_from_this<Rig> {
-
-    friend MyAvatar;
-
 public:
 
     struct HeadParameters {

From 6d5927c0c9ddbc69780c49bf358a94d2877dcf1a Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 8 Sep 2015 10:51:23 -0700
Subject: [PATCH 03/12] Now with less copies.

* AnimSkeleton now returns AnimPoses by const ref.
* AnimDebugDraw: uses references to Poses on the stack instead of copies
  within inner loops.
* AnimDebugDraw: Removed unnecessary universal refs in range based for loops.
---
 libraries/animation/src/AnimSkeleton.cpp     |  4 ++--
 libraries/animation/src/AnimSkeleton.h       |  4 ++--
 libraries/render-utils/src/AnimDebugDraw.cpp | 19 +++++++++----------
 3 files changed, 13 insertions(+), 14 deletions(-)

diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp
index fdc20db63c..5dfed265a2 100644
--- a/libraries/animation/src/AnimSkeleton.cpp
+++ b/libraries/animation/src/AnimSkeleton.cpp
@@ -87,11 +87,11 @@ int AnimSkeleton::getNumJoints() const {
     return _joints.size();
 }
 
-AnimPose AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
+const AnimPose& AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
     return _absoluteBindPoses[jointIndex];
 }
 
-AnimPose AnimSkeleton::getRelativeBindPose(int jointIndex) const {
+const AnimPose& AnimSkeleton::getRelativeBindPose(int jointIndex) const {
     return _relativeBindPoses[jointIndex];
 }
 
diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h
index 973e32232d..4a6c3c0087 100644
--- a/libraries/animation/src/AnimSkeleton.h
+++ b/libraries/animation/src/AnimSkeleton.h
@@ -54,10 +54,10 @@ public:
     int getNumJoints() const;
 
     // absolute pose, not relative to parent
-    AnimPose getAbsoluteBindPose(int jointIndex) const;
+    const AnimPose& getAbsoluteBindPose(int jointIndex) const;
 
     // relative to parent pose
-    AnimPose getRelativeBindPose(int jointIndex) const;
+    const AnimPose& getRelativeBindPose(int jointIndex) const;
 
     int getParentIndex(int jointIndex) const;
 
diff --git a/libraries/render-utils/src/AnimDebugDraw.cpp b/libraries/render-utils/src/AnimDebugDraw.cpp
index 44a58b3d67..b91db94f74 100644
--- a/libraries/render-utils/src/AnimDebugDraw.cpp
+++ b/libraries/render-utils/src/AnimDebugDraw.cpp
@@ -336,7 +336,7 @@ void AnimDebugDraw::update() {
 
         // figure out how many verts we will need.
         int numVerts = 0;
-        for (auto&& iter : _skeletons) {
+        for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
             for (int i = 0; i < skeleton->getNumJoints(); i++) {
@@ -347,7 +347,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _animNodes) {
+        for (auto& iter : _animNodes) {
             AnimNode::ConstPointer& animNode = std::get<0>(iter.second);
             auto poses = animNode->getPosesInternal();
             numVerts += poses.size() * VERTICES_PER_BONE;
@@ -360,7 +360,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _poses) {
+        for (auto& iter : _poses) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
             for (int i = 0; i < skeleton->getNumJoints(); i++) {
@@ -374,7 +374,7 @@ void AnimDebugDraw::update() {
         data._vertexBuffer->resize(sizeof(Vertex) * numVerts);
         Vertex* verts = (Vertex*)data._vertexBuffer->editData();
         Vertex* v = verts;
-        for (auto&& iter : _skeletons) {
+        for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
             int hipsIndex = skeleton->nameToJointIndex("Hips");
@@ -401,7 +401,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _animNodes) {
+        for (auto& iter : _animNodes) {
             AnimNode::ConstPointer& animNode = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
             if (animNode->_skeleton) {
@@ -439,7 +439,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _poses) {
+        for (auto& iter : _poses) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             AnimPoseVec& poses = std::get<1>(iter.second);
             AnimPose rootPose = std::get<2>(iter.second);
@@ -453,15 +453,15 @@ void AnimDebugDraw::update() {
             absAnimPose.resize(skeleton->getNumJoints());
 
             for (int i = 0; i < skeleton->getNumJoints(); i++) {
-                AnimPose pose = poses[i];
+                const AnimPose& pose = poses[i];
 
                 const float radius = BONE_RADIUS / (pose.scale.x * rootPose.scale.x);
 
                 auto parentIndex = skeleton->getParentIndex(i);
                 if (parentIndex >= 0) {
-                    absAnimPose[i] = absAnimPose[parentIndex] * poses[i];
+                    absAnimPose[i] = absAnimPose[parentIndex] * pose;
                 } else {
-                    absAnimPose[i] = poses[i];
+                    absAnimPose[i] = pose;
                 }
 
                 // draw bone
@@ -470,7 +470,6 @@ void AnimDebugDraw::update() {
                 // draw link to parent
                 if (parentIndex >= 0) {
                     assert(parentIndex < skeleton->getNumJoints());
-                    AnimPose parentPose = poses[parentIndex];
                     addLink(rootPose, absAnimPose[i], absAnimPose[parentIndex], radius, color, v);
                 }
             }

From 90fe54e6d69af236b7eee13e0e5a14d2018e100c Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 8 Sep 2015 15:01:33 -0700
Subject: [PATCH 04/12] remove cruft and minor cleanup

---
 libraries/animation/src/Rig.cpp | 11 ++---------
 libraries/animation/src/Rig.h   |  1 -
 2 files changed, 2 insertions(+), 10 deletions(-)

diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index f2ea922ab7..69334f9310 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -477,7 +477,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos
         }
 
         if (glm::length(localVel) > moveThresh) {
-            if (fabs(forwardSpeed) > 0.5f * fabs(lateralSpeed)) {
+            if (fabsf(forwardSpeed) > 0.5f * fabsf(lateralSpeed)) {
                 if (forwardSpeed > 0.0f) {
                     // forward
                     _animVars.set("isMovingForward", true);
@@ -501,7 +501,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos
             }
             _state = RigRole::Move;
         } else {
-            if (fabs(turningSpeed) > turnThresh) {
+            if (fabsf(turningSpeed) > turnThresh) {
                 if (turningSpeed > 0.0f) {
                     // turning right
                     _animVars.set("isTurningRight", true);
@@ -907,13 +907,6 @@ void Rig::updateVisibleJointStates() {
     }
 }
 
-void Rig::setJointTransform(int jointIndex, glm::mat4 newTransform) {
-    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
-        return;
-    }
-    _jointStates[jointIndex].setTransform(newTransform);
-}
-
 void Rig::setJointVisibleTransform(int jointIndex, glm::mat4 newTransform) {
     if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 0bf0645b4d..387a31c626 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -133,7 +133,6 @@ public:
                                              glm::vec3 translation, glm::quat rotation) const;
     bool getVisibleJointRotationInWorldFrame(int jointIndex, glm::quat& result, glm::quat rotation) const;
     glm::mat4 getJointTransform(int jointIndex) const;
-    void setJointTransform(int jointIndex, glm::mat4 newTransform);
     glm::mat4 getJointVisibleTransform(int jointIndex) const;
     void setJointVisibleTransform(int jointIndex, glm::mat4 newTransform);
     // Start or stop animations as needed.

From 281e4f21fc33a67e784bf5b2fccb0706c5296b33 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 8 Sep 2015 15:01:51 -0700
Subject: [PATCH 05/12] fix avatar bounding capsule calculations

---
 interface/src/avatar/SkeletonModel.cpp |  18 ++-
 libraries/animation/src/JointState.cpp |   4 +-
 libraries/animation/src/JointState.h   |   4 +-
 libraries/fbx/src/FBXReader.cpp        | 168 +++++++++++--------------
 libraries/fbx/src/FBXReader.h          |  10 +-
 libraries/fbx/src/OBJReader.cpp        |   2 -
 6 files changed, 98 insertions(+), 108 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 5ffd0f8dec..6590b5936a 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -544,14 +544,20 @@ void SkeletonModel::computeBoundingShape() {
     totalExtents.addPoint(glm::vec3(0.0f));
     int numStates = _rig->getJointStateCount();
     for (int i = 0; i < numStates; i++) {
-        // compute the default transform of this joint
         const JointState& state = _rig->getJointState(i);
 
-        // Each joint contributes a sphere at its position
-        glm::vec3 axis(state.getBoneRadius());
-        glm::vec3 jointPosition = state.getPosition();
-        totalExtents.addPoint(jointPosition + axis);
-        totalExtents.addPoint(jointPosition - axis);
+        const glm::mat4& jointTransform = state.getTransform();
+        float scale = extractUniformScale(jointTransform);
+
+        // Each joint contributes a capsule defined by FBXJoint.shapeInfo.
+        // For totalExtents we use the capsule endpoints expanded by the radius.
+        const FBXJointShapeInfo& shapeInfo = geometry.joints.at(i).shapeInfo;
+        for (int j = 0; j < shapeInfo.points.size(); ++j) {
+            glm::vec3 transformedPoint = extractTranslation(jointTransform * glm::translate(shapeInfo.points[j]));
+            vec3 radius(scale * shapeInfo.radius);
+            totalExtents.addPoint(transformedPoint + radius);
+            totalExtents.addPoint(transformedPoint - radius);
+        }
     }
 
     // compute bounding shape parameters
diff --git a/libraries/animation/src/JointState.cpp b/libraries/animation/src/JointState.cpp
index edea3b462d..7493e95084 100644
--- a/libraries/animation/src/JointState.cpp
+++ b/libraries/animation/src/JointState.cpp
@@ -41,7 +41,7 @@ void JointState::copyState(const JointState& other) {
     // DO NOT copy _constraint
     _name = other._name;
     _isFree = other._isFree;
-    _boneRadius = other._boneRadius;
+//    _boneRadius = other._boneRadius;
     _parentIndex = other._parentIndex;
     _defaultRotation = other._defaultRotation;
     _inverseDefaultRotation = other._inverseDefaultRotation;
@@ -58,7 +58,7 @@ JointState::JointState(const FBXJoint& joint) {
     _rotationInConstrainedFrame = joint.rotation;
     _name = joint.name;
     _isFree = joint.isFree;
-    _boneRadius = joint.boneRadius;
+//    _boneRadius = joint.boneRadius;
     _parentIndex = joint.parentIndex;
     _translation = joint.translation;
     _defaultRotation = joint.rotation;
diff --git a/libraries/animation/src/JointState.h b/libraries/animation/src/JointState.h
index 4f45661eb2..426603bde1 100644
--- a/libraries/animation/src/JointState.h
+++ b/libraries/animation/src/JointState.h
@@ -118,7 +118,7 @@ public:
     const glm::quat& getDefaultRotation() const { return _defaultRotation; }
     const glm::quat& getInverseDefaultRotation() const { return _inverseDefaultRotation; }
     const QString& getName() const { return _name; }
-    float getBoneRadius() const { return _boneRadius; }
+//    float getBoneRadius() const { return _boneRadius; }
     bool getIsFree() const { return _isFree; }
     float getAnimationPriority() const { return _animationPriority; }
     void setAnimationPriority(float priority) { _animationPriority = priority; }
@@ -149,7 +149,7 @@ private:
     QString _name;
     int _parentIndex;
     bool _isFree;
-    float _boneRadius;
+//    float _boneRadius;
     glm::vec3 _rotationMin;
     glm::vec3 _rotationMax;
     glm::quat _preRotation;
diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp
index 35390a8e44..5410859340 100644
--- a/libraries/fbx/src/FBXReader.cpp
+++ b/libraries/fbx/src/FBXReader.cpp
@@ -1257,21 +1257,7 @@ QString getString(const QVariant& value) {
     return list.isEmpty() ? value.toString() : list.at(0).toString();
 }
 
-class JointShapeInfo {
-public:
-    JointShapeInfo() : numVertices(0), 
-            sumVertexWeights(0.0f), sumWeightedRadii(0.0f), numVertexWeights(0), 
-            boneBegin(0.0f), averageRadius(0.0f) {
-    }
-
-    // NOTE: the points here are in the "joint frame" which has the "jointEnd" at the origin
-    int numVertices;        // num vertices from contributing meshes
-    float sumVertexWeights; // sum of all vertex weights
-    float sumWeightedRadii; // sum of weighted vertices
-    int numVertexWeights;   // num vertices that contributed to sums
-    glm::vec3 boneBegin;    // parent joint location (in joint frame)
-    float averageRadius;
-};
+typedef std::vector<glm::vec3> ShapeVertices;
 
 class AnimationCurve {
 public:
@@ -2282,22 +2268,21 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
         joint.postTransform = model.postTransform;
         joint.rotationMin = model.rotationMin;
         joint.rotationMax = model.rotationMax;
-        glm::quat combinedRotation = model.preRotation * model.rotation * model.postRotation;
+        glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation;
         if (joint.parentIndex == -1) {
-            joint.transform = geometry.offset * glm::translate(model.translation) * model.preTransform * 
-                glm::mat4_cast(combinedRotation) * model.postTransform;
+            joint.transform = geometry.offset * glm::translate(joint.translation) * joint.preTransform * 
+                glm::mat4_cast(combinedRotation) * joint.postTransform;
             joint.inverseDefaultRotation = glm::inverse(combinedRotation);
-            joint.distanceToParent = 0.0f;
+           joint.distanceToParent = 0.0f;
 
         } else {
             const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex);
-            joint.transform = parentJoint.transform * glm::translate(model.translation) *
-                model.preTransform * glm::mat4_cast(combinedRotation) * model.postTransform;
+            joint.transform = parentJoint.transform * glm::translate(joint.translation) *
+                joint.preTransform * glm::mat4_cast(combinedRotation) * joint.postTransform;
             joint.inverseDefaultRotation = glm::inverse(combinedRotation) * parentJoint.inverseDefaultRotation;
             joint.distanceToParent = glm::distance(extractTranslation(parentJoint.transform),
                 extractTranslation(joint.transform));
         }
-        joint.boneRadius = 0.0f;
         joint.inverseBindRotation = joint.inverseDefaultRotation;
         joint.name = model.name;
         
@@ -2326,9 +2311,10 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
                 zCurve.values.isEmpty() ? defaultValues.z : zCurve.values.at(i % zCurve.values.size()))));
         }
     }
-    // for each joint we allocate a JointShapeInfo in which we'll store collision shape info
-    QVector<JointShapeInfo> jointShapeInfos;
-    jointShapeInfos.resize(geometry.joints.size());
+
+    // NOTE: shapeVertices are in joint-frame
+    QVector<ShapeVertices> shapeVertices;
+    shapeVertices.resize(geometry.joints.size());
 
     // find our special joints
     geometry.leftEyeJointIndex = modelIDs.indexOf(jointEyeLeftID);
@@ -2585,8 +2571,10 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
                         boneDirection /= boneLength;
                     }
                 }
-                float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
-                JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
+
+                float clusterScale = extractUniformScale(fbxCluster.inverseBindMatrix);
+                glm::mat4 meshToJoint = glm::inverse(joint.bindTransform) * modelTransform;
+                ShapeVertices& points = shapeVertices[jointIndex];
 
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
@@ -2595,18 +2583,13 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
                     totalWeight += weight;
                     for (QMultiHash<int, int>::const_iterator it = extracted.newIndices.constFind(oldIndex);
                             it != extracted.newIndices.end() && it.key() == oldIndex; it++) {
-                        // expand the bone radius for vertices with at least 1/4 weight
+
+                        // remember vertices with at least 1/4 weight
                         const float EXPANSION_WEIGHT_THRESHOLD = 0.25f;
                         if (weight > EXPANSION_WEIGHT_THRESHOLD) {
-                            const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
-                            float proj = glm::dot(boneDirection, boneEnd - vertex);
-                            float radiusWeight = (proj < 0.0f || proj > boneLength) ? 0.5f * weight : weight;
-
-                            jointShapeInfo.sumVertexWeights += radiusWeight;
-                            jointShapeInfo.sumWeightedRadii += radiusWeight * radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj);
-                            ++jointShapeInfo.numVertexWeights;
-
-                            ++jointShapeInfo.numVertices;
+                            // transform to joint-frame and save for later
+                            const glm::mat4 vertexTransform = meshToJoint * glm::translate(extracted.mesh.vertices.at(it.value()));
+                            points.push_back(extractTranslation(vertexTransform) * clusterScale);
                         }
 
                         // look for an unused slot in the weights vector
@@ -2649,54 +2632,16 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
             // this is a single-mesh joint
             int jointIndex = maxJointIndex;
             FBXJoint& joint = geometry.joints[jointIndex];
-            JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
 
-            glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform;
-            glm::vec3 boneEnd = extractTranslation(transformJointToMesh);
-            glm::vec3 boneBegin = boneEnd;
-
-            glm::vec3 boneDirection;
-            float boneLength = 0.0f;
-            if (joint.parentIndex != -1) {
-                boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
-                boneDirection = boneEnd - boneBegin;
-                boneLength = glm::length(boneDirection);
-                if (boneLength > EPSILON) {
-                    boneDirection /= boneLength;
-                }
-            }
-            float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
-
-            // compute average vertex
-            glm::vec3 averageVertex(0.0f);
+            // transform cluster vertices to joint-frame and save for later
+            float clusterScale = extractUniformScale(firstFBXCluster.inverseBindMatrix);
+            glm::mat4 meshToJoint = glm::inverse(joint.bindTransform) * modelTransform;
+            ShapeVertices& points = shapeVertices[jointIndex];
             foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
-                float proj = glm::dot(boneDirection, boneEnd - vertex);
-                float radiusWeight = (proj < 0.0f || proj > boneLength) ? 0.5f : 1.0f;
-                jointShapeInfo.sumVertexWeights += radiusWeight;
-                jointShapeInfo.sumWeightedRadii += radiusWeight * radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj);
-                ++jointShapeInfo.numVertexWeights;
-                averageVertex += vertex;
+                const glm::mat4 vertexTransform = meshToJoint * glm::translate(vertex);
+                points.push_back(extractTranslation(vertexTransform) * clusterScale);
             }
 
-            // compute joint's radius
-            int numVertices = extracted.mesh.vertices.size();
-            jointShapeInfo.numVertices = numVertices;
-            if (numVertices > 0) {
-                // compute average radius
-                averageVertex /= (float)jointShapeInfo.numVertices;
-                float averageRadius = 0.0f;
-                foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
-                    averageRadius += glm::distance(vertex, averageVertex);
-                }
-                averageRadius *= radiusScale / (float)jointShapeInfo.numVertices;
-
-                // final radius is minimum of average and weighted
-                float weightedRadius = jointShapeInfo.sumWeightedRadii / jointShapeInfo.sumVertexWeights;
-                jointShapeInfo.averageRadius = glm::min(weightedRadius, averageRadius);
-            }
-
-            // clear sumVertexWeights (this flags it as a single-mesh joint for later)
-            jointShapeInfo.sumVertexWeights = 0.0f;
         }
         extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);
 
@@ -2721,24 +2666,59 @@ FBXGeometry* extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping
     glm::vec3 defaultCapsuleAxis(0.0f, 1.0f, 0.0f);
     for (int i = 0; i < geometry.joints.size(); ++i) {
         FBXJoint& joint = geometry.joints[i];
-        JointShapeInfo& jointShapeInfo = jointShapeInfos[i];
 
-        if (joint.parentIndex == -1) {
-            jointShapeInfo.boneBegin = glm::vec3(0.0f);
+        // NOTE: points are in joint-frame
+        // compute average point
+        ShapeVertices& points = shapeVertices[i];
+        glm::vec3 avgPoint = glm::vec3(0.0f);
+        for (uint32_t j = 0; j < points.size(); ++j) {
+            avgPoint += points[j];
+        }
+        avgPoint /= (float)points.size();
+
+        // compute axis from begin to avgPoint
+        glm::vec3 begin(0.0f);
+        glm::vec3 end = avgPoint;
+        glm::vec3 axis = end - begin;
+        float axisLength = glm::length(axis);
+        if (axisLength > EPSILON) {
+            axis /= axisLength;
         } else {
-            const FBXJoint& parentJoint = geometry.joints[joint.parentIndex];
-            glm::quat inverseRotation = glm::inverse(extractRotation(joint.transform));
-            jointShapeInfo.boneBegin = inverseRotation * (extractTranslation(parentJoint.transform) - extractTranslation(joint.transform));
+            axis = glm::vec3(0.0f);
         }
 
-        if (jointShapeInfo.sumVertexWeights > 0.0f) {
-            // mutiple meshes contributed to the bone radius and now that all 
-            // contributing meshes are done we can finally compute the boneRadius
-            joint.boneRadius = jointShapeInfo.sumWeightedRadii / jointShapeInfo.sumVertexWeights;
-        } else {
-            // single-mesh joint
-            joint.boneRadius = jointShapeInfo.averageRadius;
+        // measure average cylindrical radius
+        float avgRadius = 0.0f;
+        if (points.size() > 0) {
+            float minProjection = FLT_MAX;
+            float maxProjection = -FLT_MIN;
+            for (uint32_t j = 0; j < points.size(); ++j) {
+                glm::vec3 offset = points[j] - avgPoint;
+                float projection = glm::dot(offset, axis);
+                maxProjection = glm::max(maxProjection, projection);
+                minProjection = glm::min(minProjection, projection);
+                avgRadius += glm::length(offset - projection * axis);
+            }
+            avgRadius /= (float)points.size();
+        
+            // compute endpoints of capsule in joint-frame
+            glm::vec3 capsuleBegin = avgPoint;
+            glm::vec3 capsuleEnd = avgPoint;
+            if (maxProjection - minProjection < 2.0f * avgRadius) {
+                // the mesh-as-cylinder approximation is too short to collide as a capsule
+                // so we'll collapse it to a sphere (although that isn't a very good approximation)
+                capsuleBegin = avgPoint + 0.5f * (maxProjection + minProjection) * axis;
+                capsuleEnd = capsuleBegin;
+            } else {
+                capsuleBegin = avgPoint + (minProjection + avgRadius) * axis;
+                capsuleEnd = avgPoint + (maxProjection - avgRadius) * axis;
+            }
+
+            // save points for later
+            joint.shapeInfo.points.push_back(capsuleBegin);
+            joint.shapeInfo.points.push_back(capsuleEnd);
         }
+        joint.shapeInfo.radius = avgRadius;
     }
     geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString());
     
diff --git a/libraries/fbx/src/FBXReader.h b/libraries/fbx/src/FBXReader.h
index 158b5581c6..cff22676c8 100644
--- a/libraries/fbx/src/FBXReader.h
+++ b/libraries/fbx/src/FBXReader.h
@@ -55,15 +55,21 @@ public:
     QVector<glm::vec3> normals;
 };
 
+struct FBXJointShapeInfo {
+    // same units and frame as FBXJoint.translation
+    QVector<glm::vec3> points;
+    float radius;
+};
+
 /// A single joint (transformation node) extracted from an FBX document.
 class FBXJoint {
 public:
 
-    bool isFree;
+    FBXJointShapeInfo shapeInfo;
     QVector<int> freeLineage;
+    bool isFree;
     int parentIndex;
     float distanceToParent;
-    float boneRadius;
 
     // http://download.autodesk.com/us/fbx/20112/FBX_SDK_HELP/SDKRef/a00209.html
 
diff --git a/libraries/fbx/src/OBJReader.cpp b/libraries/fbx/src/OBJReader.cpp
index 3eff3bdec5..b4fa000d47 100644
--- a/libraries/fbx/src/OBJReader.cpp
+++ b/libraries/fbx/src/OBJReader.cpp
@@ -444,7 +444,6 @@ FBXGeometry* OBJReader::readOBJ(QIODevice* device, const QVariantHash& mapping,
         geometry.joints[0].isFree = false;
         geometry.joints[0].parentIndex = -1;
         geometry.joints[0].distanceToParent = 0;
-        geometry.joints[0].boneRadius = 0;
         geometry.joints[0].translation = glm::vec3(0, 0, 0);
         geometry.joints[0].rotationMin = glm::vec3(0, 0, 0);
         geometry.joints[0].rotationMax = glm::vec3(0, 0, 0);
@@ -620,7 +619,6 @@ void fbxDebugDump(const FBXGeometry& fbxgeo) {
         qCDebug(modelformat) << "    freeLineage" << joint.freeLineage;
         qCDebug(modelformat) << "    parentIndex" << joint.parentIndex;
         qCDebug(modelformat) << "    distanceToParent" << joint.distanceToParent;
-        qCDebug(modelformat) << "    boneRadius" << joint.boneRadius;
         qCDebug(modelformat) << "    translation" << joint.translation;
         qCDebug(modelformat) << "    preTransform" << joint.preTransform;
         qCDebug(modelformat) << "    preRotation" << joint.preRotation;

From 4a40781e2f961666d8f1b3b80deea552d29c59ae Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 8 Sep 2015 15:11:19 -0700
Subject: [PATCH 06/12] remove some commented out cruft

---
 libraries/animation/src/JointState.cpp | 2 --
 libraries/animation/src/JointState.h   | 2 --
 2 files changed, 4 deletions(-)

diff --git a/libraries/animation/src/JointState.cpp b/libraries/animation/src/JointState.cpp
index 7493e95084..9597a46726 100644
--- a/libraries/animation/src/JointState.cpp
+++ b/libraries/animation/src/JointState.cpp
@@ -41,7 +41,6 @@ void JointState::copyState(const JointState& other) {
     // DO NOT copy _constraint
     _name = other._name;
     _isFree = other._isFree;
-//    _boneRadius = other._boneRadius;
     _parentIndex = other._parentIndex;
     _defaultRotation = other._defaultRotation;
     _inverseDefaultRotation = other._inverseDefaultRotation;
@@ -58,7 +57,6 @@ JointState::JointState(const FBXJoint& joint) {
     _rotationInConstrainedFrame = joint.rotation;
     _name = joint.name;
     _isFree = joint.isFree;
-//    _boneRadius = joint.boneRadius;
     _parentIndex = joint.parentIndex;
     _translation = joint.translation;
     _defaultRotation = joint.rotation;
diff --git a/libraries/animation/src/JointState.h b/libraries/animation/src/JointState.h
index 426603bde1..07ed010104 100644
--- a/libraries/animation/src/JointState.h
+++ b/libraries/animation/src/JointState.h
@@ -118,7 +118,6 @@ public:
     const glm::quat& getDefaultRotation() const { return _defaultRotation; }
     const glm::quat& getInverseDefaultRotation() const { return _inverseDefaultRotation; }
     const QString& getName() const { return _name; }
-//    float getBoneRadius() const { return _boneRadius; }
     bool getIsFree() const { return _isFree; }
     float getAnimationPriority() const { return _animationPriority; }
     void setAnimationPriority(float priority) { _animationPriority = priority; }
@@ -149,7 +148,6 @@ private:
     QString _name;
     int _parentIndex;
     bool _isFree;
-//    float _boneRadius;
     glm::vec3 _rotationMin;
     glm::vec3 _rotationMax;
     glm::quat _preRotation;

From 387d50c21744e8c846e70aa19d65fd4b757c3090 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 8 Sep 2015 16:35:06 -0700
Subject: [PATCH 07/12] Added a flag to enable and disable lean.

This should help improve our idle and walk animations, because
animation on the "lean" joint was being lost, even when we did
not require procedural leaning.
---
 interface/src/avatar/SkeletonModel.cpp | 5 +++--
 libraries/animation/src/Rig.cpp        | 4 +++-
 libraries/animation/src/Rig.h          | 1 +
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 5ffd0f8dec..7b0c1e8cbb 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -103,13 +103,15 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         _rig->computeMotionAnimationState(deltaTime, _owningAvatar->getPosition(), _owningAvatar->getVelocity(), _owningAvatar->getOrientation());
     }
     Model::updateRig(deltaTime, parentTransform);
+    Head* head = _owningAvatar->getHead();
     if (_owningAvatar->isMyAvatar()) {
+        MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
-        Head* head = _owningAvatar->getHead();
 
         Rig::HeadParameters params;
         params.modelRotation = getRotation();
         params.modelTranslation = getTranslation();
+        params.enableLean = qApp->isHMDMode() && !myAvatar->getStandingHMDSensorMode();
         params.leanSideways = head->getFinalLeanSideways();
         params.leanForward = head->getFinalLeanForward();
         params.torsoTwist = head->getTorsoTwist();
@@ -133,7 +135,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         // However, in the !isLookingAtMe case, the eyes aren't rotating the way they should right now.
         // We will revisit that as priorities allow, and particularly after the new rig/animation/joints.
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
-        Head* head = _owningAvatar->getHead();
         // If the head is not positioned, updateEyeJoints won't get the math right
         glm::quat headOrientation;
         _rig->getJointRotation(geometry.headJointIndex, headOrientation);
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index f2ea922ab7..ad24ed0c30 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -936,7 +936,9 @@ glm::quat Rig::getJointDefaultRotationInParentFrame(int jointIndex) {
 }
 
 void Rig::updateFromHeadParameters(const HeadParameters& params) {
-    updateLeanJoint(params.leanJointIndex, params.leanSideways, params.leanForward, params.torsoTwist);
+    if (params.enableLean) {
+        updateLeanJoint(params.leanJointIndex, params.leanSideways, params.leanForward, params.torsoTwist);
+    }
     updateNeckJoint(params.neckJointIndex, params.localHeadOrientation, params.leanSideways, params.leanForward, params.torsoTwist);
     updateEyeJoints(params.leftEyeJointIndex, params.rightEyeJointIndex, params.modelTranslation, params.modelRotation,
                     params.worldHeadOrientation, params.eyeLookAt, params.eyeSaccade);
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 0bf0645b4d..39c2540ee3 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -57,6 +57,7 @@ public:
         float leanSideways = 0.0f; // degrees
         float leanForward = 0.0f; // degrees
         float torsoTwist = 0.0f; // degrees
+        bool enableLean = false;
         glm::quat modelRotation = glm::quat();
         glm::quat localHeadOrientation = glm::quat();
         glm::quat worldHeadOrientation = glm::quat();

From 614c0be59a0078d50cebcc5f04b241c3c80db0d1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 8 Sep 2015 17:09:37 -0700
Subject: [PATCH 08/12] HACK so legless avatars don't drag their knuckles

---
 interface/src/avatar/SkeletonModel.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 6590b5936a..df2d9818d1 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -558,6 +558,10 @@ void SkeletonModel::computeBoundingShape() {
             totalExtents.addPoint(transformedPoint + radius);
             totalExtents.addPoint(transformedPoint - radius);
         }
+        // HACK so that default legless robot doesn't knuckle-drag
+        if (shapeInfo.points.size() == 0 && (state.getName() == "LeftFoot" || state.getName() == "RightFoot")) {
+            totalExtents.addPoint(extractTranslation(jointTransform));
+        }
     }
 
     // compute bounding shape parameters

From 66b6d5178fc7ac31dfc98159c486392b7987d15d Mon Sep 17 00:00:00 2001
From: James Pollack <james@highfidelity.io>
Date: Tue, 8 Sep 2015 17:25:39 -0700
Subject: [PATCH 09/12] Cleanup wand when createWand.js script ends

---
 examples/toys/bubblewand/bubble.js     | 2 +-
 examples/toys/bubblewand/createWand.js | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/toys/bubblewand/bubble.js b/examples/toys/bubblewand/bubble.js
index 05b625c124..3cc68fecfa 100644
--- a/examples/toys/bubblewand/bubble.js
+++ b/examples/toys/bubblewand/bubble.js
@@ -26,7 +26,7 @@
 
     this.collisionWithEntity = function(myID, otherID, collision) {
         //if(Entites.getEntityProperties(otherID).userData.objectType==='') { merge bubbles?}
-        Entities.deleteEntity(myID);
+      // Entities.deleteEntity(myID);
       //  this.burstBubbleSound(collision.contactPoint)
 
     };
diff --git a/examples/toys/bubblewand/createWand.js b/examples/toys/bubblewand/createWand.js
index d5204c8075..15c347d62a 100644
--- a/examples/toys/bubblewand/createWand.js
+++ b/examples/toys/bubblewand/createWand.js
@@ -35,7 +35,7 @@ var wand = Entities.addEntity({
 });
 
 function cleanup() {
-	//Entities.deleteEntity(wand);
+	Entities.deleteEntity(wand);
 }
 
 

From ece8addf81a00e68a4e21de6fb2fb098c4944061 Mon Sep 17 00:00:00 2001
From: Brad Hefta-Gaub <brad@highfidelity.io>
Date: Tue, 8 Sep 2015 18:08:30 -0700
Subject: [PATCH 10/12] fix issue with default scripts

---
 examples/defaultScripts.js | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/defaultScripts.js b/examples/defaultScripts.js
index 4cb98a5df5..53e7e43aa9 100644
--- a/examples/defaultScripts.js
+++ b/examples/defaultScripts.js
@@ -14,7 +14,7 @@ Script.load("selectAudioDevice.js");
 Script.load("inspect.js");
 Script.load("notifications.js");
 Script.load("users.js");
-Script.load("handControllerGrab.js");
+Script.load("controllers/handControllerGrab.js");
 Script.load("grab.js");
 Script.load("directory.js");
 Script.load("dialTone.js");

From 75a05ff15dca45ba3ebea7ca635d135a13530846 Mon Sep 17 00:00:00 2001
From: Brad Davis <bdavis@saintandreas.org>
Date: Tue, 8 Sep 2015 20:52:18 -0700
Subject: [PATCH 11/12] Reduce logging spam in omniTools/magBalls

---
 examples/libraries/utils.js | 32 ++++++++++++++++++++++++++++----
 1 file changed, 28 insertions(+), 4 deletions(-)

diff --git a/examples/libraries/utils.js b/examples/libraries/utils.js
index 10694b11f5..1275975fd8 100644
--- a/examples/libraries/utils.js
+++ b/examples/libraries/utils.js
@@ -96,20 +96,44 @@ mergeObjects = function(proto, custom) {
     return result;
 }
 
+LOG_WARN = 1;
+
 logWarn = function(str) {
-    print(str);
+    if (LOG_WARN) {
+        print(str);
+    }
 }
 
+LOG_ERROR = 1;
+
 logError = function(str) {
-    print(str);
+    if (LOG_ERROR) {
+        print(str);
+    }
 }
 
+LOG_INFO = 1;
+
 logInfo = function(str) {
-    print(str);
+    if (LOG_INFO) {
+        print(str);
+    }
 }
 
+LOG_DEBUG = 0;
+
 logDebug = function(str) {
-    print(str);
+    if (LOG_DEBUG) {
+        print(str);
+    }
+}
+
+LOG_TRACE = 0;
+
+logTrace = function(str) {
+    if (LOG_TRACE) {
+        print(str);
+    }
 }
 
 // Computes the penetration between a point and a sphere (centered at the origin)

From b0329a58a650c032bf416edd96c91f9eb205f214 Mon Sep 17 00:00:00 2001
From: Brad Hefta-Gaub <brad@highfidelity.io>
Date: Wed, 9 Sep 2015 09:29:31 -0700
Subject: [PATCH 12/12] lower volume

---
 examples/entityScripts/boombox.js | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/entityScripts/boombox.js b/examples/entityScripts/boombox.js
index d1d18ec615..cc0dc23a23 100644
--- a/examples/entityScripts/boombox.js
+++ b/examples/entityScripts/boombox.js
@@ -71,7 +71,7 @@
                             if (_this.injector == null) {
                                 _this.injector = Audio.playSound(_this.song,  {
                                     position: props.position, // position of boombox entity
-                                    volume: 0.5,
+                                    volume: 0.1,
                                     loop: true
                                 });
                             } else {