Merge pull request #8613 from AndrewMeadows/oobe2

clamp IK hips offset based on nearby physical objects

interface/src/avatar/MyAvatar.cpp

@@ -1354,7 +1354,18 @@ void MyAvatar::prepareForPhysicsSimulation() {
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
-    // ANDREW TODO -- measure maxHipOffsetRadius here and transmit that to Rig
+    // figure 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)) {
+        // OUTOFBODY_HACK -- flip PI about yAxis
+        hipsPosition.x *= -1.0f;
+        hipsPosition.z *= -1.0f;
+        maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius);
+    }
+    _rig->setMaxHipsOffsetLength(maxHipsOffsetRadius);
 
     glm::vec3 position = getPosition();
     glm::quat orientation = getOrientation();

libraries/animation/src/AnimInverseKinematics.cpp

@@ -505,6 +505,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 // measure new _hipsOffset for next frame
                 // by looking for discrepancies between where a targeted endEffector is
                 // and where it wants to be (after IK solutions are done)
+
+                // OUTOFBODY_HACK:use weighted average between HMD and other targets
+                float HMD_WEIGHT = 10.0f;
+                float OTHER_WEIGHT = 1.0f;
+                float totalWeight = 0.0f;
+
                 glm::vec3 newHipsOffset = Vectors::ZERO;
                 for (auto& target: targets) {
                     int targetIndex = target.getIndex();
@@ -516,25 +522,42 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                             glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
                             glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
                             const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
-                            newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
+                            newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under);
+                            totalWeight += OTHER_WEIGHT;
                         } else if (target.getType() == IKTarget::Type::HmdHead) {
-                            // we want to shift the hips to bring the head to its designated position
                             glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
-                            _hipsOffset += target.getTranslation() - actual;
-                            // and ignore all other targets
-                            newHipsOffset = _hipsOffset;
-                            break;
+                            glm::vec3 thisOffset = target.getTranslation() - actual;
+                            glm::vec3 futureHipsOffset = _hipsOffset + thisOffset;
+                            if (glm::length(futureHipsOffset) < _maxHipsOffsetLength) {
+                                // it is imperative to shift the hips and bring the head to its designated position
+                                // so we slam newHipsOffset here and ignore all other targets
+                                newHipsOffset = futureHipsOffset;
+                                totalWeight = 0.0f;
+                                break;
+                            } else {
+                                newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
+                                totalWeight += HMD_WEIGHT;
+                            }
                         }
                     } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
                         glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
                         glm::vec3 targetPosition = target.getTranslation();
-                        newHipsOffset += targetPosition - actualPosition;
+                        newHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition);
+                        totalWeight += OTHER_WEIGHT;
                     }
                 }
+                if (totalWeight > 1.0f) {
+                    newHipsOffset /= totalWeight;
+                }
 
                 // smooth transitions by relaxing _hipsOffset toward the new value
-                const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
+                const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
                 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 +571,12 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
     }
 }
 
+void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
+    // OUTOFBODY_HACK: manually adjust scale here
+    const float METERS_TO_CENTIMETERS = 100.0f;
+    _maxHipsOffsetLength = METERS_TO_CENTIMETERS * 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

@@ -300,7 +300,6 @@ void Rig::clearJointAnimationPriority(int index) {
 void Rig::clearIKJointLimitHistory() {
     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->clearIKJointLimitHistory();
@@ -310,6 +309,18 @@ void Rig::clearIKJointLimitHistory() {
     }
 }
 
+void Rig::setMaxHipsOffsetLength(float maxLength) {
+    if (_animNode) {
+        _animNode->traverse([&](AnimNode::Pointer node) {
+            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,35 @@ 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);
+        btTransform startTransform = transform;
+        startTransform.setOrigin(startPos);
+        btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset);
+
+        // sweep test a sphere
+        btSphereShape sphere(_radius);
+        CharacterSweepResult result(&_ghost);
+        btTransform endTransform = startTransform;
+        endTransform.setOrigin(endPos);
+        _ghost.sweepTest(&sphere, startTransform, 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

@@ -17,8 +17,6 @@
 #include "CharacterGhostShape.h"
 #include "CharacterRayResult.h"
 
-const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f;
-
 
 CharacterGhostObject::~CharacterGhostObject() {
     removeFromWorld();
@@ -199,6 +197,20 @@ 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);
+        return result.hasHit();
+    }
+    return false;
+}
+
 void CharacterGhostObject::removeFromWorld() {
     if (_world && _inWorld) {
         _world->removeCollisionObject(this);
@@ -218,24 +230,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;