diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 84d1c2cac6..52a812f538 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -506,6 +506,11 @@ Menu::Menu() { avatar, SLOT(updateMotionBehaviorFromMenu()), UNSPECIFIED_POSITION, "Developer"); + // KINEMATIC_CONTROLLER_HACK + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::MoveKinematically, 0, false, + avatar, SLOT(updateMotionBehaviorFromMenu()), + UNSPECIFIED_POSITION, "Developer"); + // Developer > Hands >>> MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands"); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false, diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 5e213f3974..c706d50adf 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -189,6 +189,7 @@ namespace MenuOption { const QString UseAudioForMouth = "Use Audio for Mouth"; const QString UseCamera = "Use Camera"; const QString UseAnimPreAndPostRotations = "Use Anim Pre and Post Rotations"; + const QString MoveKinematically = "Move Kinematically"; // KINEMATIC_CONTROLLER_HACK const QString VelocityFilter = "Velocity Filter"; const QString VisibleToEveryone = "Everyone"; const QString VisibleToFriends = "Friends"; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 38d84fdd53..8d5ec30424 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1933,6 +1933,10 @@ void MyAvatar::updateMotionBehaviorFromMenu() { _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED; } + // KINEMATIC_CONTROLLER_HACK + bool moveKinematically = menu->isOptionChecked(MenuOption::MoveKinematically); + _characterController.setMoveKinematically(moveKinematically); + setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions)); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 59a363c483..0a3b217a66 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -492,6 +492,7 @@ private: ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; bool _hmdLeanRecenterEnabled = true; + bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 6e52f4a949..ef0c2d1cac 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -37,7 +37,7 @@ void MyCharacterController::updateShapeIfNecessary() { // compute new dimensions from avatar's bounding box float x = _boxScale.x; float z = _boxScale.z; - _radius = 0.5f * sqrtf(0.5f * (x * x + z * 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) { @@ -74,7 +74,13 @@ void MyCharacterController::updateShapeIfNecessary() { } else { _rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp); } - //_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); + // KINEMATIC_CONTROLLER_HACK + if (_moveKinematically) { + _rigidBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); + } else { + _rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() & + ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)); + } } else { // TODO: handle this failure case } diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 331be5c9da..81772c59b3 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -102,6 +102,7 @@ bool CharacterController::needsAddition() const { void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { if (_dynamicsWorld != world) { + // remove from old world if (_dynamicsWorld) { if (_rigidBody) { _dynamicsWorld->removeRigidBody(_rigidBody); @@ -110,6 +111,7 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld = nullptr; } if (world && _rigidBody) { + // add to new world _dynamicsWorld = world; _pendingFlags &= ~PENDING_FLAG_JUMP; // Before adding the RigidBody to the world we must save its oldGravity to the side @@ -119,7 +121,18 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) { _dynamicsWorld->addAction(this); // restore gravity settings _rigidBody->setGravity(oldGravity); + _ghost.setCollisionShape(_rigidBody->getCollisionShape()); // KINEMATIC_CONTROLLER_HACK } + // KINEMATIC_CONTROLLER_HACK + int16_t group = BULLET_COLLISION_GROUP_MY_AVATAR; + int16_t mask = BULLET_COLLISION_MASK_MY_AVATAR & (~ group); + _ghost.setCollisionGroupAndMask(group, mask); + _ghost.setCollisionWorld(_dynamicsWorld); + _ghost.setDistanceToFeet(_radius + _halfHeight); + _ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK + _ghost.setMinWallAngle(PI / 4.0f); // HACK + _ghost.setUpDirection(_currentUp); + _ghost.setGravity(DEFAULT_CHARACTER_GRAVITY); } if (_dynamicsWorld) { if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) { @@ -188,54 +201,67 @@ const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED; void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity; computeNewVelocity(dt, velocity); - _rigidBody->setLinearVelocity(velocity + _parentVelocity); - // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. - // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal. - // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). + if (_moveKinematically) { + // KINEMATIC_CONTROLLER_HACK + btTransform transform = _rigidBody->getWorldTransform(); + transform.setOrigin(_ghost.getWorldTransform().getOrigin()); + _ghost.setWorldTransform(transform); + _ghost.setMotorVelocity(_simpleMotorVelocity); + float overshoot = 1.0f * _radius; + _ghost.move(dt, overshoot); + _rigidBody->setWorldTransform(_ghost.getWorldTransform()); + _rigidBody->setLinearVelocity(_ghost.getLinearVelocity()); + } else { + // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. + // Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal. + // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate(). - if (_following) { - // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned - const float NORMAL_WALKING_SPEED = 0.5f; - const float FOLLOW_TIME = 0.8f; - const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); + _rigidBody->setLinearVelocity(velocity + _parentVelocity); + if (_following) { + // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned + const float NORMAL_WALKING_SPEED = 0.5f; + const float FOLLOW_TIME = 0.8f; + const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); - const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; + const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME; - btTransform bodyTransform = _rigidBody->getWorldTransform(); + btTransform bodyTransform = _rigidBody->getWorldTransform(); - btVector3 startPos = bodyTransform.getOrigin(); - btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; - btVector3 vel = deltaPos * (0.5f / dt); - btScalar speed = vel.length(); - if (speed > NORMAL_WALKING_SPEED) { - vel *= NORMAL_WALKING_SPEED / speed; + btVector3 startPos = bodyTransform.getOrigin(); + btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos; + btVector3 vel = deltaPos * (0.5f / dt); + btScalar speed = vel.length(); + if (speed > NORMAL_WALKING_SPEED) { + vel *= NORMAL_WALKING_SPEED / speed; + } + btVector3 linearDisplacement = vel * dt; + btVector3 endPos = startPos + linearDisplacement; + + btQuaternion startRot = bodyTransform.getRotation(); + glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); + glm::vec2 currentRight(currentFacing.y, -currentFacing.x); + glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); + float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); + float angularSpeed = 0.5f * deltaAngle / dt; + if (angularSpeed > MAX_ANGULAR_SPEED) { + angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; + } + float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); + btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); + btQuaternion endRot = angularDisplacement * startRot; + + // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. + btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); + btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); + + _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; + _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; + + _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } - btVector3 linearDisplacement = vel * dt; - btVector3 endPos = startPos + linearDisplacement; - - btQuaternion startRot = bodyTransform.getRotation(); - glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot)); - glm::vec2 currentRight(currentFacing.y, -currentFacing.x); - glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation())); - float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f)); - float angularSpeed = 0.5f * deltaAngle / dt; - if (angularSpeed > MAX_ANGULAR_SPEED) { - angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed; - } - float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight)); - btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt); - btQuaternion endRot = angularDisplacement * startRot; - - // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. - btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); - btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); - - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); _followTime += dt; + _ghost.setWorldTransform(_rigidBody->getWorldTransform()); } } @@ -353,6 +379,7 @@ void CharacterController::handleChangedCollisionGroup() { void CharacterController::updateUpAxis(const glm::quat& rotation) { _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS); + _ghost.setUpDirection(_currentUp); if (_state != State::Hover && _rigidBody) { if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); @@ -414,6 +441,10 @@ glm::vec3 CharacterController::getLinearVelocity() const { return velocity; } +void CharacterController::setCapsuleRadius(float radius) { + _radius = radius; +} + glm::vec3 CharacterController::getVelocityChange() const { if (_rigidBody) { return bulletToGLM(_velocityChange); @@ -490,6 +521,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel // add components back together and rotate into world-frame velocity = (hVelocity + vVelocity).rotate(axis, angle); + _simpleMotorVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle); // store velocity and weights velocities.push_back(velocity); @@ -507,6 +539,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { velocities.reserve(_motors.size()); std::vector weights; weights.reserve(_motors.size()); + _simpleMotorVelocity = btVector3(0.0f, 0.0f, 0.0f); for (int i = 0; i < (int)_motors.size(); ++i) { applyMotor(i, dt, velocity, velocities, weights); } @@ -522,6 +555,7 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) { for (size_t i = 0; i < velocities.size(); ++i) { velocity += (weights[i] / totalWeight) * velocities[i]; } + _simpleMotorVelocity /= totalWeight; } if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) { velocity = btVector3(0.0f, 0.0f, 0.0f); @@ -686,3 +720,10 @@ void CharacterController::setFlyingAllowed(bool value) { } } } + +void CharacterController::setMoveKinematically(bool kinematic) { + if (kinematic != _moveKinematically) { + _moveKinematically = kinematic; + _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE; + } +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 2d8cf489de..3b1ec6e945 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -9,8 +9,8 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#ifndef hifi_CharacterControllerInterface_h -#define hifi_CharacterControllerInterface_h +#ifndef hifi_CharacterController_h +#define hifi_CharacterController_h #include #include @@ -22,6 +22,7 @@ #include #include "BulletUtil.h" +#include "CharacterGhostObject.h" const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0; const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1; @@ -95,6 +96,7 @@ 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; } @@ -110,10 +112,6 @@ public: void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale); - /* - bool isEnabled() const { return _enabled; } // thread-safe - void setEnabled(bool enabled); - */ bool isEnabledAndReady() const { return _dynamicsWorld; } void setCollisionGroup(int16_t group); @@ -124,6 +122,7 @@ public: void setFlyingAllowed(bool value); + void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK protected: #ifdef DEBUG_STATE_CHANGE @@ -146,11 +145,13 @@ protected: }; std::vector _motors; + CharacterGhostObject _ghost; // KINEMATIC_CONTROLLER_HACK btVector3 _currentUp; btVector3 _targetVelocity; btVector3 _parentVelocity; btVector3 _preSimulationVelocity; btVector3 _velocityChange; + btVector3 _simpleMotorVelocity; // KINEMATIC_CONTROLLER_HACK btTransform _followDesiredBodyTransform; btTransform _characterBodyTransform; @@ -188,7 +189,8 @@ protected: uint32_t _previousFlags { 0 }; bool _flyingAllowed { true }; + bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR }; }; -#endif // hifi_CharacterControllerInterface_h +#endif // hifi_CharacterController_h diff --git a/libraries/physics/src/CharacterGhostObject.cpp b/libraries/physics/src/CharacterGhostObject.cpp new file mode 100644 index 0000000000..f3e1da3585 --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.cpp @@ -0,0 +1,390 @@ +// +// CharacterGhostObject.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.08.26 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterGhostObject.h" + +#include + +#include "CharacterRayResult.h" + +const btScalar DEFAULT_STEP_UP_HEIGHT = 0.5f; + + +CharacterGhostObject::~CharacterGhostObject() { + removeFromWorld(); + setCollisionShape(nullptr); +} + +void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) { + _collisionFilterGroup = group; + _collisionFilterMask = mask; + // TODO: if this probe is in the world reset ghostObject overlap cache +} + +void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mask) const { + group = _collisionFilterGroup; + mask = _collisionFilterMask; +} + + +void CharacterGhostObject::setUpDirection(const btVector3& up) { + btScalar length = up.length(); + if (length > FLT_EPSILON) { + _upDirection /= length; + } else { + _upDirection = btVector3(0.0f, 1.0f, 0.0f); + } +} + +void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) { + _motorVelocity = velocity; + _motorSpeed = _motorVelocity.length(); +} + +// override of btCollisionObject::setCollisionShape() +void CharacterGhostObject::setCollisionShape(btCollisionShape* shape) { + assert(!shape || shape->isConvex()); // if shape is valid then please make it convex + if (shape != getCollisionShape()) { + bool wasInWorld = _inWorld; + removeFromWorld(); + btCollisionObject::setCollisionShape(shape); + if (wasInWorld) { + assert(shape); // please remove from world before setting null shape + addToWorld(); + } + } +} + +void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) { + if (world != _world) { + removeFromWorld(); + _world = world; + addToWorld(); + } +} + +void CharacterGhostObject::move(btScalar dt, btScalar overshoot) { + _onFloor = false; + assert(_world && _inWorld); + updateVelocity(dt); + + // resolve any penetrations before sweeping + int32_t MAX_LOOPS = 4; + int32_t numExtractions = 0; + btVector3 totalPosition(0.0f, 0.0f, 0.0f); + while (numExtractions < MAX_LOOPS) { + if (resolvePenetration(numExtractions)) { + numExtractions = 0; + break; + } + totalPosition += getWorldTransform().getOrigin(); + ++numExtractions; + } + if (numExtractions > 1) { + // penetration resolution was probably oscillating between opposing objects + // so we use the average of the solutions + totalPosition /= btScalar(numExtractions); + btTransform transform = getWorldTransform(); + transform.setOrigin(totalPosition); + setWorldTransform(transform); + + // TODO: figure out how to untrap character + } + if (_onFloor) { + // a floor was identified during resolvePenetration() + _hovering = false; + updateTraction(); + } + + btVector3 forwardSweep = dt * _linearVelocity; + btScalar stepDistance = forwardSweep.length(); + btScalar MIN_SWEEP_DISTANCE = 0.0001f; + if (stepDistance < MIN_SWEEP_DISTANCE) { + // not moving, no need to sweep + updateHoverState(getWorldTransform()); + return; + } + + const btCollisionShape* shape = getCollisionShape(); + assert(shape->isConvex()); + const btConvexShape* convexShape= static_cast(shape); + + // augment forwardSweep to help slow moving sweeps get over steppable ledges + btScalar margin = shape->getMargin(); + if (overshoot < margin) { + overshoot = margin; + } + btScalar longSweepDistance = stepDistance + overshoot; + forwardSweep *= longSweepDistance / stepDistance; + + // expand this object's Aabb in the broadphase and + // update the pairCache for the sweepTests we intend to do + btVector3 minAabb, maxAabb; + getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); + minAabb.setMin(minAabb - btVector3(margin, margin, margin)); + maxAabb.setMax(maxAabb + btVector3(margin, margin, margin)); + minAabb.setMin(minAabb + forwardSweep); + maxAabb.setMax(maxAabb + forwardSweep); + minAabb.setMin(minAabb + _maxStepHeight * _upDirection); + maxAabb.setMax(maxAabb + _maxStepHeight * _upDirection); + + // this updates both pairCaches: world broadphase and ghostobject + _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); + + // 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 + + if (!result.hasHit()) { + nextTransform.setOrigin(transform.getOrigin() + (stepDistance / longSweepDistance) * forwardSweep); + setWorldTransform(nextTransform); + updateHoverState(nextTransform); + updateTraction(); + return; + } + + // check if this hit is obviously unsteppable + btVector3 hitFromBase = result.m_hitPointWorld - (transform.getOrigin() - (_distanceToFeet * _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 + btVector3 forwardTranslation = result.m_closestHitFraction * forwardSweep; + btScalar forwardDistance = forwardTranslation.length(); + if (forwardDistance > stepDistance) { + forwardTranslation *= stepDistance / forwardDistance; + } + transform.setOrigin(transform.getOrigin() + forwardTranslation); + setWorldTransform(transform); + return; + } + // if we get here then we hit something that might be steppable + + // remember the forward sweep hit fraction for later + btScalar forwardSweepHitFraction = result.m_closestHitFraction; + + // figure out how high we can step up + btScalar availableStepHeight = measureAvailableStepHeight(); + + // raise by availableStepHeight before sweeping forward + result.resetHitHistory(); + transform.setOrigin(startTransform.getOrigin() + availableStepHeight * _upDirection); + nextTransform.setOrigin(transform.getOrigin() + forwardSweep); + sweepTest(convexShape, transform, nextTransform, result); + if (result.hasHit()) { + transform.setOrigin(transform.getOrigin() + result.m_closestHitFraction * forwardSweep); + } else { + transform = 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); + if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) { + // can stand on future landing spot, so we interpolate toward it + _floorNormal = result.m_hitNormalWorld; + _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); + } 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); + } + setWorldTransform(transform); + updateTraction(); +} + +void CharacterGhostObject::removeFromWorld() { + if (_world && _inWorld) { + _world->removeCollisionObject(this); + _inWorld = false; + } +} + +void CharacterGhostObject::addToWorld() { + if (_world && !_inWorld) { + assert(getCollisionShape()); + setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); + //assert(getBroadphaseHandle()); + //int16_t group = getBroadphaseHandle()->m_collisionFilterGroup; + //int16_t mask = getBroadphaseHandle()->m_collisionFilterMask; + _world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask); + _inWorld = true; + } +} + +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 { + if (_world && _inWorld) { + _world->rayTest(start, end, result); + } + return result.hasHit(); +} + +bool CharacterGhostObject::resolvePenetration(int numTries) { + assert(_world); + // We refresh the overlapping pairCache because any previous movement may have pushed us + // into an overlap that was not in the cache. + refreshOverlappingPairCache(); + + // compute collision details + btHashedOverlappingPairCache* pairCache = getOverlappingPairCache(); + _world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher()); + + // loop over contact manifolds + btTransform transform = getWorldTransform(); + btVector3 position = transform.getOrigin(); + btVector3 minBox =btVector3(0.0f, 0.0f, 0.0f); + btVector3 maxBox = btVector3(0.0f, 0.0f, 0.0f); + btManifoldArray manifoldArray; + const btScalar PENETRATION_RESOLUTION_FUDGE_FACTOR = 0.0001f; // speeds up resolvation + + int numPairs = pairCache->getNumOverlappingPairs(); + for (int i = 0; i < numPairs; i++) { + manifoldArray.resize(0); + btBroadphasePair* collisionPair = &(pairCache->getOverlappingPairArray()[i]); + + btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + + if ((obj0 && !obj0->hasContactResponse()) && (obj1 && !obj1->hasContactResponse())) { + // we know this probe has no contact response + // but neither does the other object so skip this manifold + continue; + } + + if (!collisionPair->m_algorithm) { + // null m_algorithm means the two shape types don't know how to collide! + // shouldn't fall in here but just in case + continue; + } + + btScalar mostFloorPenetration = 0.0f; + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j = 0;j < manifoldArray.size(); j++) { + btPersistentManifold* manifold = manifoldArray[j]; + btScalar directionSign = (manifold->getBody0() == this) ? btScalar(1.0) : btScalar(-1.0); + for (int p = 0; p < manifold->getNumContacts(); p++) { + const btManifoldPoint& pt = manifold->getContactPoint(p); + if (pt.getDistance() > 0.0f) { + continue; + } + + // normal always points from object to character + btVector3 normal = directionSign * pt.m_normalWorldOnB; + + btScalar penetrationDepth = pt.getDistance(); + if (penetrationDepth < mostFloorPenetration) { // remember penetrationDepth is negative + btScalar normalDotUp = normal.dot(_upDirection); + if (normalDotUp > _maxWallNormalUpComponent) { + mostFloorPenetration = penetrationDepth; + _floorNormal = normal; + _onFloor = true; + } + } + + btVector3 penetration = (-penetrationDepth + PENETRATION_RESOLUTION_FUDGE_FACTOR) * normal; + minBox.setMin(penetration); + maxBox.setMax(penetration); + } + } + } + + btVector3 restore = maxBox + minBox; + if (restore.length2() > 0.0f) { + transform.setOrigin(position + restore); + setWorldTransform(transform); + return false; + } + return true; +} + +void CharacterGhostObject::refreshOverlappingPairCache() { + assert(_world && _inWorld); + btVector3 minAabb, maxAabb; + getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb); + + // this updates both pairCaches: world broadphase and ghostobject + _world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher()); +} + +void CharacterGhostObject::updateVelocity(btScalar dt) { + if (_hovering) { + _linearVelocity *= 0.99f; // HACK damping + } else { + _linearVelocity += (dt * _gravity) * _upDirection; + } +} + +void CharacterGhostObject::updateTraction() { + if (_hovering) { + _linearVelocity = _motorVelocity; + } else if (_onFloor) { + btVector3 pathDirection = _floorNormal.cross(_motorVelocity).cross(_floorNormal); + btScalar pathLength = pathDirection.length(); + if (pathLength > FLT_EPSILON) { + _linearVelocity = (_motorSpeed / pathLength) * pathDirection; + } else { + _linearVelocity = btVector3(0.0f, 0.0f, 0.0f); + } + } +} + +btScalar CharacterGhostObject::measureAvailableStepHeight() const { + const btCollisionShape* shape = getCollisionShape(); + assert(shape->isConvex()); + const btConvexShape* convexShape= static_cast(shape); + + CharacterSweepResult result(this); + btTransform transform = getWorldTransform(); + btTransform nextTransform = transform; + nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection); + sweepTest(convexShape, transform, nextTransform, result); + 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 new file mode 100644 index 0000000000..df569181b9 --- /dev/null +++ b/libraries/physics/src/CharacterGhostObject.h @@ -0,0 +1,83 @@ +// +// CharacterGhostObject.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.08.26 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterGhostObject_h +#define hifi_CharacterGhostObject_h + +#include +#include + +#include "CharacterSweepResult.h" +#include "CharacterRayResult.h" + + +class CharacterGhostObject : public btPairCachingGhostObject { +public: + CharacterGhostObject() { } + ~CharacterGhostObject(); + + void setCollisionGroupAndMask(int16_t group, int16_t mask); + void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const; + + void setDistanceToFeet(btScalar distance) { _distanceToFeet = distance; } + 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) + void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); } + void setMaxStepHeight(btScalar height) { _maxStepHeight = height; } + + const btVector3& getLinearVelocity() const { return _linearVelocity; } + + void setCollisionShape(btCollisionShape* shape) override; + + void setCollisionWorld(btCollisionWorld* world); + + 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; + bool rayTest(const btVector3& start, + const btVector3& end, + CharacterRayResult& result) const; + + bool resolvePenetration(int numTries); + void refreshOverlappingPairCache(); + void updateVelocity(btScalar dt); + void updateTraction(); + btScalar measureAvailableStepHeight() const; + void updateHoverState(const btTransform& transform); + +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 + btCollisionWorld* _world { nullptr }; // input, pointer to world + btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape + 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 + btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb + int16_t _collisionFilterGroup { 0 }; + int16_t _collisionFilterMask { 0 }; + bool _inWorld { false }; // internal, was added to world + bool _hovering { false }; // internal, + bool _onFloor { false }; // output, is actually standing on floor + bool _hasFloor { false }; // output, has floor underneath to fall on +}; + +#endif // hifi_CharacterGhostObject_h diff --git a/libraries/physics/src/CharacterRayResult.cpp b/libraries/physics/src/CharacterRayResult.cpp new file mode 100644 index 0000000000..45a84a8274 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.cpp @@ -0,0 +1,29 @@ +// +// CharaterRayResult.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.05 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterRayResult.h" + +#include "CharacterGhostObject.h" + +CharacterRayResult::CharacterRayResult (const CharacterGhostObject* character) : + btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)), + _character(character) +{ + assert(_character); + _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask); +} + +btScalar CharacterRayResult::addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { + if (rayResult.m_collisionObject == _character) { + return 1.0f; + } + return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); +} diff --git a/libraries/physics/src/CharacterRayResult.h b/libraries/physics/src/CharacterRayResult.h new file mode 100644 index 0000000000..b74959aa34 --- /dev/null +++ b/libraries/physics/src/CharacterRayResult.h @@ -0,0 +1,44 @@ +// +// CharaterRayResult.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.05 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterRayResult_h +#define hifi_CharacterRayResult_h + +#include +#include + +class CharacterGhostObject; + +class CharacterRayResult : public btCollisionWorld::ClosestRayResultCallback { +public: + CharacterRayResult (const CharacterGhostObject* character); + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override; + +protected: + const CharacterGhostObject* _character; + + // Note: Public data members inherited from ClosestRayResultCallback + // + // btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + // btVector3 m_rayToWorld; + // btVector3 m_hitNormalWorld; + // btVector3 m_hitPointWorld; + // + // Note: Public data members inherited from RayResultCallback + // + // btScalar m_closestHitFraction; + // const btCollisionObject* m_collisionObject; + // short int m_collisionFilterGroup; + // short int m_collisionFilterMask; +}; + +#endif // hifi_CharacterRayResult_h diff --git a/libraries/physics/src/CharacterSweepResult.cpp b/libraries/physics/src/CharacterSweepResult.cpp new file mode 100644 index 0000000000..86c9871e87 --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.cpp @@ -0,0 +1,40 @@ +// +// CharaterSweepResult.cpp +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.01 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CharacterSweepResult.h" + +#include "CharacterGhostObject.h" + +CharacterSweepResult::CharacterSweepResult(const CharacterGhostObject* character) + : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)), + _character(character) +{ + // set collision group and mask to match _character + assert(_character); + _character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask); +} + +btScalar CharacterSweepResult::addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) { + // skip objects that we shouldn't collide with + if (!convexResult.m_hitCollisionObject->hasContactResponse()) { + return btScalar(1.0); + } + if (convexResult.m_hitCollisionObject == _character) { + return btScalar(1.0); + } + + return ClosestConvexResultCallback::addSingleResult(convexResult, useWorldFrame); +} + +void CharacterSweepResult::resetHitHistory() { + m_hitCollisionObject = nullptr; + m_closestHitFraction = btScalar(1.0f); +} diff --git a/libraries/physics/src/CharacterSweepResult.h b/libraries/physics/src/CharacterSweepResult.h new file mode 100644 index 0000000000..f80bd0bb47 --- /dev/null +++ b/libraries/physics/src/CharacterSweepResult.h @@ -0,0 +1,45 @@ +// +// CharaterSweepResult.h +// libraries/physcis/src +// +// Created by Andrew Meadows 2016.09.01 +// Copyright 2016 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_CharacterSweepResult_h +#define hifi_CharacterSweepResult_h + +#include +#include + + +class CharacterGhostObject; + +class CharacterSweepResult : public btCollisionWorld::ClosestConvexResultCallback { +public: + CharacterSweepResult(const CharacterGhostObject* character); + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) override; + void resetHitHistory(); +protected: + const CharacterGhostObject* _character; + + // NOTE: Public data members inherited from ClosestConvexResultCallback: + // + // btVector3 m_convexFromWorld; // unused except by btClosestNotMeConvexResultCallback + // btVector3 m_convexToWorld; // unused except by btClosestNotMeConvexResultCallback + // btVector3 m_hitNormalWorld; + // btVector3 m_hitPointWorld; + // const btCollisionObject* m_hitCollisionObject; + // + // NOTE: Public data members inherited from ConvexResultCallback: + // + // btScalar m_closestHitFraction; + // short int m_collisionFilterGroup; + // short int m_collisionFilterMask; + +}; + +#endif // hifi_CharacterSweepResult_h