diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp old mode 100644 new mode 100755 index 9978d915fd..1ace282023 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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(); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 27300699c4..48e3fdb978 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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::iterator constraintItr = _constraints.find(index); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index c9560c7383..7e4a7e5473 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -39,6 +39,8 @@ public: void clearIKJointLimitHistory(); + void setMaxHipsOffsetLength(float maxLength); + protected: void computeTargets(const AnimVariantMap& animVars, std::vector& targets, const AnimPoseVec& underPoses); void solveWithCyclicCoordinateDescent(const std::vector& 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 }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2d11a1e17f..240190ae2c 100644 --- a/libraries/animation/src/Rig.cpp +++ b/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(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) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 7eb0316889..f0cc68a828 100644 --- a/libraries/animation/src/Rig.h +++ b/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); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp old mode 100644 new mode 100755 index d3a38ed533..256b64421b --- a/libraries/physics/src/CharacterController.cpp +++ b/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; diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 3b1ec6e945..2a3a81b416 100644 --- a/libraries/physics/src/CharacterController.h +++ b/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: diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp old mode 100644 new mode 100755 index c503f8a927..903d84c646 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/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 { diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h old mode 100644 new mode 100755 index 8faf429542..dd2f694a59 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -16,6 +16,7 @@ #include #include +#include #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;