From 06ff984f905413cfa380e8b3d8fbef83a750d3bb Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 21 Sep 2016 15:21:56 -0700 Subject: [PATCH] sky-hook for walking up steps --- .../src/avatar/MyCharacterController.cpp | 11 --- libraries/physics/src/CharacterController.cpp | 97 ++++++++++++------- libraries/physics/src/CharacterController.h | 14 ++- 3 files changed, 71 insertions(+), 51 deletions(-) diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index ef0c2d1cac..ad7879d2cc 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -34,17 +34,6 @@ void MyCharacterController::updateShapeIfNecessary() { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE; - // compute new dimensions from avatar's bounding box - float x = _boxScale.x; - float z = _boxScale.z; - setCapsuleRadius(0.5f * sqrtf(0.5f * (x * x + z * z))); - _halfHeight = 0.5f * _boxScale.y - _radius; - float MIN_HALF_HEIGHT = 0.1f; - if (_halfHeight < MIN_HALF_HEIGHT) { - _halfHeight = MIN_HALF_HEIGHT; - } - // NOTE: _shapeLocalOffset is already computed - if (_radius > 0.0f) { // create RigidBody if it doesn't exist if (!_rigidBody) { diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 514e8a4f8f..b1047c8e1e 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -63,7 +63,6 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const } CharacterController::CharacterController() { - _halfHeight = 1.0f; _floorDistance = MAX_FALL_HEIGHT; _targetVelocity.setValue(0.0f, 0.0f, 0.0f); @@ -146,30 +145,40 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { } } -static const float COS_PI_OVER_THREE = cosf(PI / 3.0f); +bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) { + _stepHeight = _minStepHeight; // clears last step obstacle + btDispatcher* dispatcher = collisionWorld->getDispatcher(); + int numManifolds = dispatcher->getNumManifolds(); + bool hasFloor = false; + const float COS_PI_OVER_THREE = cosf(PI / 3.0f); + + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); -bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) const { - int numManifolds = collisionWorld->getDispatcher()->getNumManifolds(); for (int i = 0; i < numManifolds; i++) { - btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i); - const btCollisionObject* obA = static_cast(contactManifold->getBody0()); - const btCollisionObject* obB = static_cast(contactManifold->getBody1()); - if (obA == _rigidBody || obB == _rigidBody) { + btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i); + if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) { + bool characterIsFirst = _rigidBody == contactManifold->getBody0(); int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; j++) { - btManifoldPoint& pt = contactManifold->getContactPoint(j); - - // check to see if contact point is touching the bottom sphere of the capsule. - // and the contact normal is not slanted too much. - float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY(); - btVector3 normal = (obA == _rigidBody) ? pt.m_normalWorldOnB : -pt.m_normalWorldOnB; - if (contactPointY < -_halfHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) { - return true; + // check for "floor" + btManifoldPoint& contact = contactManifold->getContactPoint(j); + btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame + btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character + btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp); + if (hitHeight < _radius && normal.dot(_currentUp) > COS_PI_OVER_THREE) { + hasFloor = true; + } + // remember highest step obstacle + if (hitHeight > _stepHeight && hitHeight < _maxStepHeight && normal.dot(_targetVelocity) < 0.0f ) { + _stepHeight = hitHeight; + _stepPoint = transform * pointOnCharacter; // rotate into world-frame + _stepNormal = normal; } } } } - return false; + return hasFloor; } void CharacterController::preStep(btCollisionWorld* collisionWorld) { @@ -332,23 +341,22 @@ void CharacterController::setState(State desiredState) { } void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) { - _boxScale = scale; - - float x = _boxScale.x; - float z = _boxScale.z; + float x = scale.x; + float z = scale.z; float radius = 0.5f * sqrtf(0.5f * (x * x + z * z)); - float halfHeight = 0.5f * _boxScale.y - radius; + float halfHeight = 0.5f * scale.y - radius; float MIN_HALF_HEIGHT = 0.1f; if (halfHeight < MIN_HALF_HEIGHT) { halfHeight = MIN_HALF_HEIGHT; } // compare dimensions - float radiusDelta = glm::abs(radius - _radius); - float heightDelta = glm::abs(halfHeight - _halfHeight); - if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) { - // shape hasn't changed --> nothing to do - } else { + if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) { + _radius = radius; + _halfHeight = halfHeight; + _minStepHeight = 0.02f; // HACK: hardcoded now but should be shape margin + _maxStepHeight = 0.75f * (_halfHeight + _radius); + if (_dynamicsWorld) { // must REMOVE from world prior to shape update _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION; @@ -358,7 +366,7 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const } // it's ok to change offset immediately -- there are no thread safety issues here - _shapeLocalOffset = minCorner + 0.5f * _boxScale; + _shapeLocalOffset = minCorner + 0.5f * scale; } void CharacterController::setCollisionGroup(int16_t group) { @@ -435,10 +443,6 @@ glm::vec3 CharacterController::getLinearVelocity() const { return velocity; } -void CharacterController::setCapsuleRadius(float radius) { - _radius = radius; -} - glm::vec3 CharacterController::getVelocityChange() const { if (_rigidBody) { return bulletToGLM(_velocityChange); @@ -487,11 +491,32 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // compute local UP btVector3 up = _currentUp.rotate(axis, -angle); + // add sky hook when encountering a step obstacle + btVector3 motorVelocity = motor.velocity; + btScalar vTimescale = motor.vTimescale; + if (_stepHeight > _minStepHeight) { + // there is a step + btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle); + if (motorVelocityWF.dot(_stepNormal) < 0.0f) { + // the motor pushes against step + btVector3 leverArm = _stepPoint; + motorVelocityWF = _stepNormal.cross(leverArm.cross(motorVelocityWF)); + btScalar distortedLength = motorVelocityWF.length(); + if (distortedLength > FLT_EPSILON) { + // scale the motor in the correct direction and rotate back to motor-frame + motorVelocityWF *= (motorVelocity.length() / distortedLength); + motorVelocity += motorVelocityWF.rotate(axis, -angle); + // make vTimescale as small as possible + vTimescale = glm::min(vTimescale, motor.hTimescale); + } + } + } + // split velocity into horizontal and vertical components btVector3 vVelocity = velocity.dot(up) * up; btVector3 hVelocity = velocity - vVelocity; - btVector3 vTargetVelocity = motor.velocity.dot(up) * up; - btVector3 hTargetVelocity = motor.velocity - vTargetVelocity; + btVector3 vTargetVelocity = motorVelocity.dot(up) * up; + btVector3 hTargetVelocity = motorVelocity - vTargetVelocity; // modify each component separately btScalar maxTau = 0.0f; @@ -503,8 +528,8 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel maxTau = tau; hVelocity += (hTargetVelocity - hVelocity) * tau; } - if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { - btScalar tau = dt / motor.vTimescale; + if (vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) { + btScalar tau = dt / vTimescale; if (tau > 1.0f) { tau = 1.0f; } diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 7d739bfae3..9591579d63 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -92,7 +92,6 @@ public: glm::vec3 getLinearVelocity() const; glm::vec3 getVelocityChange() const; - virtual void setCapsuleRadius(float radius); float getCapsuleRadius() const { return _radius; } float getCapsuleHalfHeight() const { return _halfHeight; } glm::vec3 getCapsuleLocalOffset() const { return _shapeLocalOffset; } @@ -130,7 +129,7 @@ protected: #endif void updateUpAxis(const glm::quat& rotation); - bool checkForSupport(btCollisionWorld* collisionWorld) const; + bool checkForSupport(btCollisionWorld* collisionWorld); protected: struct CharacterMotor { @@ -163,8 +162,15 @@ protected: quint32 _jumpButtonDownCount; quint32 _takeoffJumpButtonID; - btScalar _halfHeight; - btScalar _radius; + // data for walking up steps + btVector3 _stepPoint; + btVector3 _stepNormal; + btScalar _stepHeight { 0.0f }; + btScalar _minStepHeight { 0.0f }; + btScalar _maxStepHeight { 0.0f }; + + btScalar _halfHeight { 0.0f }; + btScalar _radius { 0.0f }; btScalar _floorDistance; bool _hasSupport;