measure max hips offset and clamp it in IK

interface/src/avatar/MyAvatar.cpp

@@ -1354,7 +1354,15 @@ void MyAvatar::prepareForPhysicsSimulation() {
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
-    // ANDREW TODO -- measure maxHipOffsetRadius here and transmit that to Rig
+    // figoure out how far the hips can move before they hit something
+    int hipsJoint = getJointIndex("Hips");
+    glm::vec3 hipsPosition; // rig-frame
+    // OUTOFBODY_HACK -- hardcoded maxHipsOffsetRadius (ultimately must exceed FollowHelper lateral/forward/back walk thresholds)
+    float maxHipsOffsetRadius = 3.0f * _characterController.getCapsuleRadius();
+    if (_rig->getJointPosition(hipsJoint, hipsPosition)) {
+        maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius);
+    }
+    _rig->setMaxHipsOffsetLength(maxHipsOffsetRadius);
 
     glm::vec3 position = getPosition();
     glm::quat orientation = getOrientation();

libraries/animation/src/AnimInverseKinematics.cpp

@@ -535,6 +535,11 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 // smooth transitions by relaxing _hipsOffset toward the new value
                 const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
                 float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ?  dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
+                float newOffsetLength = glm::length(newHipsOffset);
+                if (newOffsetLength > _maxHipsOffsetLength) {
+                    // clamp the hips offset
+                    newHipsOffset *= _maxHipsOffsetLength / newOffsetLength;
+                }
                 _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
             }
         }
@@ -548,6 +553,11 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
     }
 }
 
+void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
+    assert(maxLength > 0.0f);
+    _maxHipsOffsetLength = maxLength;
+}
+
 RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
     RotationConstraint* constraint = nullptr;
     std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);

libraries/animation/src/AnimInverseKinematics.h

@@ -39,6 +39,8 @@ public:
 
     void clearIKJointLimitHistory();
 
+    void setMaxHipsOffsetLength(float maxLength);
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
@@ -83,6 +85,7 @@ protected:
 
     // experimental data for moving hips during IK
     glm::vec3 _hipsOffset { Vectors::ZERO };
+    float _maxHipsOffsetLength { 1.0f };
     int _headIndex { -1 };
     int _hipsIndex { -1 };
     int _hipsParentIndex { -1 };

libraries/animation/src/Rig.cpp

@@ -310,6 +310,19 @@ void Rig::clearIKJointLimitHistory() {
     }
 }
 
+void Rig::setMaxHipsOffsetLength(float maxLength) {
+    if (_animNode) {
+        _animNode->traverse([&](AnimNode::Pointer node) {
+            // only report clip nodes as valid roles.
+            auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
+            if (ikNode) {
+                ikNode->setMaxHipsOffsetLength(maxLength);
+            }
+            return true;
+        });
+    }
+}
+
 void Rig::setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority) {
     if (isIndexValid(index)) {
         if (valid) {

libraries/animation/src/Rig.h

@@ -104,6 +104,7 @@ public:
     void clearJointAnimationPriority(int index);
 
     void clearIKJointLimitHistory();
+    void setMaxHipsOffsetLength(float maxLength);
 
     // geometry space
     void setJointState(int index, bool valid, const glm::quat& rotation, const glm::vec3& translation, float priority);

libraries/physics/src/CharacterController.cpp

@@ -323,7 +323,7 @@ void CharacterController::setState(State desiredState) {
     }
 }
 
-void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) {
+void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) {
     _boxScale = scale;
 
     float x = _boxScale.x;
@@ -350,7 +350,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm
     }
 
     // it's ok to change offset immediately -- there are no thread safety issues here
-    _shapeLocalOffset = corner + 0.5f * _boxScale;
+    _shapeLocalOffset = minCorner + 0.5f * _boxScale;
 }
 
 void CharacterController::setCollisionGroup(int16_t group) {
@@ -724,6 +724,33 @@ void CharacterController::setFlyingAllowed(bool value) {
     }
 }
 
+float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance) {
+    btVector3 hipsOffset = glmToBullet(currentHipsOffset); // rig-frame
+    btScalar hipsOffsetLength = hipsOffset.length();
+    if (hipsOffsetLength > FLT_EPSILON) {
+        const btTransform& transform = _rigidBody->getWorldTransform();
+
+        // rotate into world-frame
+        btTransform rotation = transform;
+        rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
+        btVector3 startPos = transform.getOrigin() - rotation * glmToBullet(_shapeLocalOffset);
+        btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset);
+
+        // sweep test a sphere
+        btSphereShape sphere(_radius);
+        CharacterSweepResult result(&_ghost);
+        btTransform endTransform = transform;
+        endTransform.setOrigin(endPos);
+        _ghost.sweepTest(&sphere, transform, endTransform, result);
+
+        // measure sweep success
+        if (result.hasHit()) {
+            maxSweepDistance *= result.m_closestHitFraction;
+        }
+    }
+    return maxSweepDistance;
+}
+
 void CharacterController::setMoveKinematically(bool kinematic) {
     if (kinematic != _moveKinematically) {
         _moveKinematically = kinematic;

libraries/physics/src/CharacterController.h

@@ -110,7 +110,7 @@ public:
 
     State getState() const { return _state; }
 
-    void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale);
+    void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale);
 
     bool isEnabledAndReady() const { return _dynamicsWorld; }
 
@@ -122,6 +122,7 @@ public:
 
     void setFlyingAllowed(bool value);
 
+    float measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance);
     void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK
 
 protected:

libraries/physics/src/CharacterGhostObject.cpp

@@ -29,6 +29,8 @@ CharacterGhostObject::~CharacterGhostObject() {
     }
 }
 
+const int16_t wtf = 9; // adebug wtf?
+
 void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) {
     _collisionFilterGroup = group;
     _collisionFilterMask = mask;
@@ -199,6 +201,24 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) {
     updateTraction();
 }
 
+bool CharacterGhostObject::sweepTest(
+        const btConvexShape* shape,
+        const btTransform& start,
+        const btTransform& end,
+        CharacterSweepResult& result) const {
+    if (_world && _inWorld) {
+        assert(shape);
+
+        btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration;
+        convexSweepTest(shape, start, end, result, allowedPenetration);
+
+        if (result.hasHit()) {
+            return true;
+        }
+    }
+    return false;
+}
+
 void CharacterGhostObject::removeFromWorld() {
     if (_world && _inWorld) {
         _world->removeCollisionObject(this);
@@ -218,24 +238,6 @@ void CharacterGhostObject::addToWorld() {
     }
 }
 
-bool CharacterGhostObject::sweepTest(
-        const btConvexShape* shape,
-        const btTransform& start,
-        const btTransform& end,
-        CharacterSweepResult& result) const {
-    if (_world && _inWorld) {
-        assert(shape);
-
-        btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration;
-        convexSweepTest(shape, start, end, result, allowedPenetration);
-
-        if (result.hasHit()) {
-            return true;
-        }
-    }
-    return false;
-}
-
 bool CharacterGhostObject::rayTest(const btVector3& start,
         const btVector3& end,
         CharacterRayResult& result) const {

libraries/physics/src/CharacterGhostObject.h

@@ -16,6 +16,7 @@
 
 #include <btBulletDynamicsCommon.h>
 #include <BulletCollision/CollisionDispatch/btGhostObject.h>
+#include <stdint.h>
 
 #include "CharacterSweepResult.h"
 #include "CharacterRayResult.h"
@@ -45,14 +46,14 @@ public:
 
     void move(btScalar dt, btScalar overshoot);
 
-protected:
-    void removeFromWorld();
-    void addToWorld();
-
     bool sweepTest(const btConvexShape* shape,
             const btTransform& start,
             const btTransform& end,
             CharacterSweepResult& result) const;
+protected:
+    void removeFromWorld();
+    void addToWorld();
+
     bool rayTest(const btVector3& start,
             const btVector3& end,
             CharacterRayResult& result) const;