From 8dd5c9b92b66c0e0917b72dce2daf679dc60dbcd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 19 Sep 2016 12:40:16 -0700 Subject: [PATCH] fix kinematic motion for ground and hover --- interface/src/avatar/MyAvatar.cpp | 3 +- libraries/physics/src/CharacterController.cpp | 26 +++-- libraries/physics/src/CharacterController.h | 4 +- .../physics/src/CharacterGhostObject.cpp | 105 +++++++++++------- libraries/physics/src/CharacterGhostObject.h | 14 ++- 5 files changed, 93 insertions(+), 59 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 245eba0098..cd4ee4784b 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1378,7 +1378,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { glm::vec3 position = getPosition(); glm::quat orientation = getOrientation(); if (_characterController.isEnabledAndReady()) { - _characterController.getPositionAndOrientation(position, orientation); + glm::quat bogusOrientation; + _characterController.getPositionAndOrientation(position, bogusOrientation); } nextAttitude(position, orientation); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 7bb8d98ebe..acd82d73ea 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -130,7 +130,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { // KINEMATIC_CONTROLLER_HACK _ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup)); _ghost.setCollisionWorld(_dynamicsWorld); - _ghost.setDistanceToFeet(_radius + _halfHeight); + _ghost.setRadiusAndHalfHeight(_radius, _halfHeight); _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK _ghost.setMinWallAngle(PI / 4.0f); // HACK _ghost.setUpDirection(_currentUp); @@ -177,10 +177,10 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) cons void CharacterController::preStep(btCollisionWorld* collisionWorld) { // trace a ray straight down to see if we're standing on the ground - const btTransform& xform = _rigidBody->getWorldTransform(); + const btTransform& transform = _rigidBody->getWorldTransform(); // rayStart is at center of bottom sphere - btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp; + btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp; // rayEnd is some short distance outside bottom sphere const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius; @@ -213,7 +213,8 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { _ghost.setMotorVelocity(_targetVelocity); float overshoot = 1.0f * _radius; _ghost.move(dt, overshoot); - _rigidBody->setWorldTransform(_ghost.getWorldTransform()); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _rigidBody->setWorldTransform(transform); _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); } else { // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. @@ -400,9 +401,8 @@ void CharacterController::setPositionAndOrientation( // TODO: update gravity if up has changed updateUpAxis(orientation); - btQuaternion bodyOrientation = glmToBullet(orientation); - btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset); - _characterBodyTransform = btTransform(bodyOrientation, bodyPosition); + _rotation = glmToBullet(orientation); + _position = glmToBullet(position + orientation * _shapeLocalOffset); } void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const { @@ -485,10 +485,11 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel if (tau > 1.0f) { tau = 1.0f; } - velocity += (motor.velocity - velocity) * tau; + velocity += tau * (motor.velocity - velocity); // rotate back into world-frame velocity = velocity.rotate(axis, angle); + _targetVelocity += (tau * motor.velocity).rotate(axis, angle); // store the velocity and weight velocities.push_back(velocity); @@ -584,14 +585,14 @@ void CharacterController::preSimulation() { if (_dynamicsWorld) { quint64 now = usecTimestampNow(); - // slam body to where it is supposed to be - _rigidBody->setWorldTransform(_characterBodyTransform); + // slam body transform + _rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position))); btVector3 velocity = _rigidBody->getLinearVelocity(); _preSimulationVelocity = velocity; // scan for distant floor // rayStart is at center of bottom sphere - btVector3 rayStart = _characterBodyTransform.getOrigin(); + btVector3 rayStart = _position; // rayEnd is straight down MAX_FALL_HEIGHT btScalar rayLength = _radius + MAX_FALL_HEIGHT; @@ -679,6 +680,9 @@ void CharacterController::preSimulation() { } break; } + if (_moveKinematically && _ghost.isHovering()) { + SET_STATE(State::Hover, "kinematic motion"); // HACK + } } else { // OUTOFBODY_HACK -- in collisionless state switch between Ground and Hover states if (rayHasHit) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 85a02cd56a..74649d1572 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -153,7 +153,9 @@ protected: btVector3 _preSimulationVelocity; btVector3 _velocityChange; btTransform _followDesiredBodyTransform; - btTransform _characterBodyTransform; + btVector3 _position; + btQuaternion _rotation; + //btTransform _characterBodyTransform; glm::vec3 _shapeLocalOffset; diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp index 6529f2c944..a4b6e90266 100755 --- a/libraries/physics/src/CharacterGhostObject.cpp +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include "CharacterGhostShape.h" #include "CharacterRayResult.h" @@ -38,6 +40,10 @@ void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mas mask = _collisionFilterMask; } +void CharacterGhostObject::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) { + _radius = radius; + _halfHeight = halfHeight; +} void CharacterGhostObject::setUpDirection(const btVector3& up) { btScalar length = up.length(); @@ -99,10 +105,12 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // TODO: figure out how to untrap character } + btTransform startTransform = getWorldTransform(); + btVector3 startPosition = startTransform.getOrigin(); if (_onFloor) { - // a floor was identified during resolvePenetration() - _hovering = false; - updateTraction(); + // resolvePenetration() pushed the avatar out of a floor so + // we must updateTraction() before using _linearVelocity + updateTraction(startPosition); } btVector3 forwardSweep = dt * _linearVelocity; @@ -110,7 +118,7 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { btScalar MIN_SWEEP_DISTANCE = 0.0001f; if (stepDistance < MIN_SWEEP_DISTANCE) { // not moving, no need to sweep - updateHoverState(getWorldTransform()); + updateTraction(startPosition); return; } @@ -128,22 +136,19 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // step forward CharacterSweepResult result(this); - btTransform startTransform = getWorldTransform(); - btTransform transform = startTransform; - btTransform nextTransform = transform; - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); // forward + btTransform nextTransform = startTransform; + nextTransform.setOrigin(startPosition + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); // forward if (!result.hasHit()) { - nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep); setWorldTransform(nextTransform); - updateHoverState(nextTransform); - updateTraction(); + updateTraction(nextTransform.getOrigin()); return; } // check if this hit is obviously unsteppable - btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _upDirection)); + btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection)); btScalar hitHeight = hitFromBase.dot(_upDirection); if (hitHeight > _maxStepHeight) { // capsule can't step over the obstacle so move forward as much as possible before we bail @@ -152,8 +157,8 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { if (forwardDistance > stepDistance) { forwardTranslation *= stepDistance / forwardDistance; } - transform.setOrigin(transform.getOrigin() + forwardTranslation); - setWorldTransform(transform); + nextTransform.setOrigin(startPosition + forwardTranslation); + setWorldTransform(nextTransform); return; } // if we get here then we hit something that might be steppable @@ -166,35 +171,37 @@ void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { // raise by availableStepHeight before sweeping forward result.resetHitHistory(); - transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection); - nextTransform.setOrigin(transform.getOrigin() + forwardSweep); - sweepTest(convexShape, transform, nextTransform, result); + startTransform.setOrigin(startPosition + availableStepHeight * _upDirection); + nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit()) { - transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep); + startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep); } else { - transform = nextTransform; + startTransform = nextTransform; } // sweep down in search of future landing spot result.resetHitHistory(); - btVector3 downSweep = (dt * _linearVelocity.dot(_upDirection) - availableStepHeight) * _upDirection; - nextTransform.setOrigin(transform.getOrigin() + downSweep); - sweepTest(convexShape, transform, nextTransform, result); + btVector3 downSweep = (- availableStepHeight) * _upDirection; + nextTransform.setOrigin(startTransform.getOrigin() + downSweep); + sweepTest(convexShape, startTransform, nextTransform, result); if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { // can stand on future landing spot, so we interpolate toward it _floorNormal = result.m_hitNormalWorld; + _floorContact = result.m_hitPointWorld; _onFloor = true; _hovering = false; - nextTransform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * downSweep); - btVector3 totalStep = nextTransform.getOrigin() - startTransform.getOrigin(); - transform.setOrigin(startTransform.getOrigin() + (stepDistance / totalStep.length()) * totalStep); + nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep); + btVector3 totalStep = nextTransform.getOrigin() - startPosition; + nextTransform.setOrigin(startPosition + (stepDistance / totalStep.length()) * totalStep); + updateTraction(nextTransform.getOrigin()); } else { // either there is no future landing spot, or there is but we can't stand on it // in any case: we go forward as much as possible - transform.setOrigin(startTransform.getOrigin() + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep); + updateTraction(nextTransform.getOrigin()); } - setWorldTransform(transform); - updateTraction(); + setWorldTransform(nextTransform); } bool CharacterGhostObject::sweepTest( @@ -297,6 +304,11 @@ bool CharacterGhostObject::resolvePenetration(int numTries) { if (normalDotUp > _maxWallNormalUpComponent) { mostFloorPenetration = penetrationDepth; _floorNormal = normal; + if (directionSign > 0.0f) { + _floorContact = pt.m_positionWorldOnA; + } else { + _floorContact = pt.m_positionWorldOnB; + } _onFloor = true; } } @@ -327,17 +339,36 @@ void CharacterGhostObject::refreshOverlappingPairCache() { void CharacterGhostObject::updateVelocity(btScalar dt) { if (_hovering) { - _linearVelocity *= 0.99f; // HACK damping + _linearVelocity *= 0.999f; // HACK damping } else { _linearVelocity += (dt * _gravity) * _upDirection; } } -void CharacterGhostObject::updateTraction() { +void CharacterGhostObject::updateHoverState(const btVector3& position) { + if (_onFloor) { + _hovering = false; + } else { + // cast a ray down looking for floor support + CharacterRayResult rayResult(this); + btScalar distanceToFeet = _radius + _halfHeight; + btScalar slop = 2.0f * getCollisionShape()->getMargin(); // slop to help ray start OUTSIDE the floor object + btVector3 startPos = position - ((distanceToFeet - slop) * _upDirection); + btVector3 endPos = startPos - (2.0f * distanceToFeet) * _upDirection; + rayTest(startPos, endPos, rayResult); + // we're hovering if the ray didn't hit anything or hit unstandable slope + _hovering = !rayResult.hasHit() || rayResult.m_hitNormalWorld.dot(_upDirection) < _maxWallNormalUpComponent; + } +} + +void CharacterGhostObject::updateTraction(const btVector3& position) { + updateHoverState(position); if (_hovering) { _linearVelocity = _motorVelocity; } else if (_onFloor) { - btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal); + // compute a velocity that swings the capsule around the _floorContact + btVector3 leverArm = _floorContact - position; + btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm)); btScalar pathLength = pathDirection.length(); if (pathLength > FLT_EPSILON) { _linearVelocity = (_motorSpeed / pathLength) * pathDirection; @@ -360,13 +391,3 @@ btScalar CharacterGhostObject::measureAvailableStepHeight() const { return result.m_closestHitFraction * _maxStepHeight; } -void CharacterGhostObject::updateHoverState(const btTransform& transform) { - // cast a ray down looking for floor support - CharacterRayResult rayResult(this); - btVector3 startPos = transform.getOrigin() - ((_distanceToFeet - 0.1f) * _upDirection); // 0.1 HACK to make ray hit - btVector3 endPos = startPos - (2.0f * _distanceToFeet) * _upDirection; - rayTest(startPos, endPos, rayResult); - // we're hovering if the ray didn't hit an object we can stand on - _hovering = !(rayResult.hasHit() && rayResult.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent); -} - diff --git a/libraries/physics/src/CharacterGhostObject.h b/libraries/physics/src/CharacterGhostObject.h index dd2f694a59..274057a907 100755 --- a/libraries/physics/src/CharacterGhostObject.h +++ b/libraries/physics/src/CharacterGhostObject.h @@ -31,7 +31,7 @@ public: void setCollisionGroupAndMask(int16_t group, int16_t mask); void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const; - void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; } + void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight); void setUpDirection(const btVector3& up); void setMotorVelocity(const btVector3& velocity); void setGravity(btScalar gravity) { _gravity = gravity; } // NOTE: we expect _gravity to be negative (in _upDirection) @@ -50,6 +50,9 @@ public: const btTransform& start, const btTransform& end, CharacterSweepResult& result) const; + + bool isHovering() const { return _hovering; } + protected: void removeFromWorld(); void addToWorld(); @@ -61,17 +64,20 @@ protected: bool resolvePenetration(int numTries); void refreshOverlappingPairCache(); void updateVelocity(btScalar dt); - void updateTraction(); + void updateTraction(const btVector3& position); btScalar measureAvailableStepHeight() const; - void updateHoverState(const btTransform& transform); + void updateHoverState(const btVector3& position); protected: btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal + btVector3 _floorContact { 0.0f, 0.0f, 0.0f }; // internal, last floor contact point btCollisionWorld* _world { nullptr }; // input, pointer to world - btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + //btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + btScalar _halfHeight { 0.0f }; + btScalar _radius { 0.0f }; btScalar _motorSpeed { 0.0f }; // internal, cached for speed btScalar _gravity { 0.0f }; // input, amplitude of gravity along _upDirection (should be negative) btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal