Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-16 17:38:15 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

help avatar walk up steps

Browse source
This commit is contained in:
Andrew Meadows 2017-03-30 11:21:34 -07:00
parent a31a861e19
commit e21bd7a67a
17 changed files with 1501 additions and 204 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
interface/src/Application.cpp
View file

@ -4331,13 +4331,6 @@ void Application::update(float deltaTime) {
if (nearbyEntitiesAreReadyForPhysics()) {
_physicsEnabled = true;
getMyAvatar()->updateMotionBehaviorFromMenu();
} else {
auto characterController = getMyAvatar()->getCharacterController();
if (characterController) {
// if we have a character controller, disable it here so the avatar doesn't get stuck due to
// a non-loading collision hull.
characterController->setEnabled(false);
}
}
}
} else if (domainLoadingInProgress) {

4
interface/src/Menu.cpp
View file

@ -532,6 +532,10 @@ Menu::Menu() {
avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
UNSPECIFIED_POSITION, "Developer");
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableAvatarCollisions, 0, true,
avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
UNSPECIFIED_POSITION, "Developer");
// Developer > Hands >>>
MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false,

2
interface/src/Menu.h
View file

@ -96,7 +96,7 @@ namespace MenuOption {
const QString DontRenderEntitiesAsScene = "Don't Render Entities as Scene";
const QString EchoLocalAudio = "Echo Local Audio";
const QString EchoServerAudio = "Echo Server Audio";
const QString EnableCharacterController = "Collide with world";
const QString EnableAvatarCollisions = "Enable Avatar Collisions";
const QString EnableInverseKinematics = "Enable Inverse Kinematics";
const QString EntityScriptServerLog = "Entity Script Server Log";
const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation";

36
interface/src/avatar/MyAvatar.cpp Normal file → Executable file
View file

@ -150,8 +150,6 @@ MyAvatar::MyAvatar(QThread* thread, RigPointer rig) :
// when we leave a domain we lift whatever restrictions that domain may have placed on our scale
connect(&domainHandler, &DomainHandler::disconnectedFromDomain, this, &MyAvatar::clearScaleRestriction);
_characterController.setEnabled(true);
_bodySensorMatrix = deriveBodyFromHMDSensor();
using namespace recording;
@ -588,8 +586,8 @@ void MyAvatar::simulate(float deltaTime) {
}
});
_characterController.setFlyingAllowed(flyingAllowed);
if (!_characterController.isEnabled() && !ghostingAllowed) {
_characterController.setEnabled(true);
if (!ghostingAllowed && _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
_characterController.setCollisionGroup(BULLET_COLLISION_GROUP_MY_AVATAR);
}
}
@ -1449,7 +1447,8 @@ void MyAvatar::updateMotors() {
_characterController.clearMotors();
glm::quat motorRotation;
if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) {
if (_characterController.getState() == CharacterController::State::Hover) {
if (_characterController.getState() == CharacterController::State::Hover ||
_characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
motorRotation = getMyHead()->getCameraOrientation();
} else {
// non-hovering = walking: follow camera twist about vertical but not lift
@ -1495,6 +1494,7 @@ void MyAvatar::prepareForPhysicsSimulation() {
qDebug() << "Warning: getParentVelocity failed" << getID();
parentVelocity = glm::vec3();
}
_characterController.handleChangedCollisionGroup();
_characterController.setParentVelocity(parentVelocity);
_characterController.setPositionAndOrientation(getPosition(), getOrientation());
@ -1906,7 +1906,7 @@ void MyAvatar::updateActionMotor(float deltaTime) {
float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_MOTOR_SPEED;
float speedGrowthTimescale = 2.0f;
float speedIncreaseFactor = 1.8f;
motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale , 0.0f, 1.0f) * speedIncreaseFactor;
motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale, 0.0f, 1.0f) * speedIncreaseFactor;
const float maxBoostSpeed = getUniformScale() * MAX_BOOST_SPEED;
if (_isPushing) {
@ -1949,9 +1949,17 @@ void MyAvatar::updatePosition(float deltaTime) {
measureMotionDerivatives(deltaTime);
_moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
} else {
// physics physics simulation updated elsewhere
float speed2 = glm::length2(velocity);
_moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
if (_moving) {
// scan for walkability
glm::vec3 position = getPosition();
MyCharacterController::RayShotgunResult result;
glm::vec3 step = deltaTime * (getRotation() * _actionMotorVelocity);
_characterController.testRayShotgun(position, step, result);
_characterController.setStepUpEnabled(result.walkable);
}
}
// capture the head rotation, in sensor space, when the user first indicates they would like to move/fly.
@ -2188,14 +2196,13 @@ void MyAvatar::updateMotionBehaviorFromMenu() {
} else {
_motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED;
}
setCharacterControllerEnabled(menu->isOptionChecked(MenuOption::EnableCharacterController));
setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
}
void MyAvatar::setCharacterControllerEnabled(bool enabled) {
void MyAvatar::setAvatarCollisionsEnabled(bool enabled) {
if (QThread::currentThread() != thread()) {
QMetaObject::invokeMethod(this, "setCharacterControllerEnabled", Q_ARG(bool, enabled));
QMetaObject::invokeMethod(this, "setAvatarCollisionsEnabled", Q_ARG(bool, enabled));
return;
}
@ -2207,11 +2214,12 @@ void MyAvatar::setCharacterControllerEnabled(bool enabled) {
ghostingAllowed = zone->getGhostingAllowed();
}
}
_characterController.setEnabled(ghostingAllowed ? enabled : true);
int16_t group = enabled || !ghostingAllowed ? BULLET_COLLISION_GROUP_MY_AVATAR : BULLET_COLLISION_GROUP_COLLISIONLESS;
_characterController.setCollisionGroup(group);
}
bool MyAvatar::getCharacterControllerEnabled() {
return _characterController.isEnabled();
bool MyAvatar::getAvatarCollisionsEnabled() {
return _characterController.getCollisionGroup() != BULLET_COLLISION_GROUP_COLLISIONLESS;
}
void MyAvatar::clearDriveKeys() {

6
interface/src/avatar/MyAvatar.h
View file

@ -128,7 +128,7 @@ class MyAvatar : public Avatar {
Q_PROPERTY(float isAway READ getIsAway WRITE setAway)
Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled)
Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled)
Q_PROPERTY(bool avatarCollisionsEnabled READ getAvatarCollisionsEnabled WRITE setAvatarCollisionsEnabled)
Q_PROPERTY(bool useAdvancedMovementControls READ useAdvancedMovementControls WRITE setUseAdvancedMovementControls)
public:
@ -470,8 +470,8 @@ public:
bool hasDriveInput() const;
Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
Q_INVOKABLE bool getCharacterControllerEnabled();
Q_INVOKABLE void setAvatarCollisionsEnabled(bool enabled);
Q_INVOKABLE bool getAvatarCollisionsEnabled();
virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;

438
interface/src/avatar/MyCharacterController.cpp Normal file → Executable file
View file

@ -15,11 +15,15 @@
#include "MyAvatar.h"
// TODO: improve walking up steps
// TODO: make avatars able to walk up and down steps/slopes
// TODO: make avatars stand on steep slope
// TODO: make avatars not snag on low ceilings
void MyCharacterController::RayShotgunResult::reset() {
hitFraction = 1.0f;
walkable = true;
}
MyCharacterController::MyCharacterController(MyAvatar* avatar) {
assert(avatar);
@ -30,37 +34,33 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) {
MyCharacterController::~MyCharacterController() {
}
void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
CharacterController::setDynamicsWorld(world);
if (world) {
initRayShotgun(world);
}
}
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;
_radius = 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) {
btCollisionShape* shape = computeShape();
// HACK: use some simple mass property defaults for now
const float DEFAULT_AVATAR_MASS = 100.0f;
const btScalar DEFAULT_AVATAR_MASS = 100.0f;
const btVector3 DEFAULT_AVATAR_INERTIA_TENSOR(30.0f, 8.0f, 30.0f);
btCollisionShape* shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
_rigidBody = new btRigidBody(DEFAULT_AVATAR_MASS, nullptr, shape, DEFAULT_AVATAR_INERTIA_TENSOR);
} else {
btCollisionShape* shape = _rigidBody->getCollisionShape();
if (shape) {
delete shape;
}
shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
shape = computeShape();
_rigidBody->setCollisionShape(shape);
}
@ -72,12 +72,414 @@ void MyCharacterController::updateShapeIfNecessary() {
if (_state == State::Hover) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
_rigidBody->setGravity(_gravity * _currentUp);
}
//_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
_rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() &
~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT));
} else {
// TODO: handle this failure case
}
}
}
bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result) {
btVector3 rayDirection = glmToBullet(step);
btScalar stepLength = rayDirection.length();
if (stepLength < FLT_EPSILON) {
return false;
}
rayDirection /= stepLength;
// get _ghost ready for ray traces
btTransform transform = _rigidBody->getWorldTransform();
btVector3 newPosition = glmToBullet(position);
transform.setOrigin(newPosition);
_ghost.setWorldTransform(transform);
btMatrix3x3 rotation = transform.getBasis();
_ghost.refreshOverlappingPairCache();
CharacterRayResult rayResult(&_ghost);
CharacterRayResult closestRayResult(&_ghost);
btVector3 rayStart;
btVector3 rayEnd;
// compute rotation that will orient local ray start points to face step direction
btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f);
btVector3 adjustedDirection = rayDirection - rayDirection.dot(_currentUp) * _currentUp;
btVector3 axis = forward.cross(adjustedDirection);
btScalar lengthAxis = axis.length();
if (lengthAxis > FLT_EPSILON) {
// we're walking sideways
btScalar angle = acosf(lengthAxis / adjustedDirection.length());
if (rayDirection.dot(forward) < 0.0f) {
angle = PI - angle;
}
axis /= lengthAxis;
rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation;
} else if (rayDirection.dot(forward) < 0.0f) {
// we're walking backwards
rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation;
}
// scan the top
// NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on.
// The approximate extra distance can be derived with trigonometry.
//
// minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ]
//
// where: theta = max angle between floor normal and vertical
//
// if stepLength is not long enough we can add the difference.
//
btScalar cosTheta = _minFloorNormalDotUp;
btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope
btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP;
if (forwardSlop < 0.0f) {
// BIG step, no slop necessary
forwardSlop = 0.0f;
}
const btScalar backSlop = 0.04f;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
if (result.walkable) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
result.walkable = false;
// the top scan wasn't walkable so don't bother scanning the bottom
// remove both forwardSlop and backSlop
result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);
return result.hitFraction < 1.0f;
}
}
}
}
if (_state == State::Hover) {
// scan the bottom just like the top
for (int32_t i = 0; i < _bottomPoints.size(); ++i) {
rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
if (result.walkable) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
result.walkable = false;
// the bottom scan wasn't walkable
// remove both forwardSlop and backSlop
result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);
return result.hitFraction < 1.0f;
}
}
}
}
} else {
// scan the bottom looking for nearest step point
// remove forwardSlop
result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength);
for (int32_t i = 0; i < _bottomPoints.size(); ++i) {
rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
}
}
// remove backSlop
// NOTE: backSlop removal can produce a NEGATIVE hitFraction!
// which means the shape is actually in interpenetration
result.hitFraction = ((closestRayResult.m_closestHitFraction * (backSlop + stepLength)) - backSlop) / stepLength;
}
return result.hitFraction < 1.0f;
}
glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const glm::vec3& step) {
btVector3 stepDirection = glmToBullet(step);
btScalar stepLength = stepDirection.length();
if (stepLength < FLT_EPSILON) {
return glm::vec3(0.0f);
}
stepDirection /= stepLength;
// get _ghost ready for ray traces
btTransform transform = _rigidBody->getWorldTransform();
btVector3 newPosition = glmToBullet(position);
transform.setOrigin(newPosition);
btMatrix3x3 rotation = transform.getBasis();
_ghost.setWorldTransform(transform);
_ghost.refreshOverlappingPairCache();
// compute rotation that will orient local ray start points to face stepDirection
btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f);
btVector3 horizontalDirection = stepDirection - stepDirection.dot(_currentUp) * _currentUp;
btVector3 axis = forward.cross(horizontalDirection);
btScalar lengthAxis = axis.length();
if (lengthAxis > FLT_EPSILON) {
// non-zero sideways component
btScalar angle = asinf(lengthAxis / horizontalDirection.length());
if (stepDirection.dot(forward) < 0.0f) {
angle = PI - angle;
}
axis /= lengthAxis;
rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation;
} else if (stepDirection.dot(forward) < 0.0f) {
// backwards
rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation;
}
CharacterRayResult rayResult(&_ghost);
btVector3 rayStart;
btVector3 rayEnd;
btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f);
int32_t numPenetrations = 0;
{ // first we scan straight out from capsule center to see if we're stuck on anything
btScalar forwardRatio = 0.5f;
btScalar backRatio = 0.25f;
btVector3 radial;
bool stuck = false;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = rotation * _topPoints[i];
radial = rayStart - rayStart.dot(_currentUp) * _currentUp;
rayEnd = newPosition + rayStart + forwardRatio * radial;
rayStart += newPosition - backRatio * radial;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
btScalar totalRatio = backRatio + forwardRatio;
btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * totalRatio - backRatio) / forwardRatio;
if (adjustedHitFraction < 0.0f) {
penetration += adjustedHitFraction * radial;
++numPenetrations;
} else {
stuck = true;
}
}
}
if (numPenetrations > 0) {
if (numPenetrations > 1) {
penetration /= (btScalar)numPenetrations;
}
return bulletToGLM(penetration);
} else if (stuck) {
return glm::vec3(0.0f);
}
}
// if we get here then we're not stuck pushing into any surface
// so now we scan to see if the way before us is "walkable"
// scan the top
// NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on.
// The approximate extra distance can be derived with trigonometry.
//
// minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ]
//
// where: theta = max angle between floor normal and vertical
//
// if stepLength is not long enough we can add the difference.
//
btScalar cosTheta = _minFloorNormalDotUp;
btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
const btScalar MIN_FORWARD_SLOP = 0.10f; // HACK: not sure why this is necessary to detect steepest walkable slope
btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP;
if (forwardSlop < 0.0f) {
// BIG step, no slop necessary
forwardSlop = 0.0f;
}
// we push the step forward by stepMargin to help reduce accidental penetration
const btScalar MIN_STEP_MARGIN = 0.04f;
btScalar stepMargin = glm::max(_radius, MIN_STEP_MARGIN);
btScalar expandedStepLength = stepLength + forwardSlop + stepMargin;
// loop over topPoints
bool walkable = true;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = newPosition + rotation * _topPoints[i];
rayEnd = rayStart + expandedStepLength * stepDirection;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
walkable = false;
break;
}
}
}
// scan the bottom
// TODO: implement sliding along sloped floors
bool steppingUp = false;
expandedStepLength = stepLength + MIN_FORWARD_SLOP + MIN_STEP_MARGIN;
for (int32_t i = _bottomPoints.size() - 1; i > -1; --i) {
rayStart = newPosition + rotation * _bottomPoints[i] - MIN_STEP_MARGIN * stepDirection;
rayEnd = rayStart + expandedStepLength * stepDirection;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - MIN_STEP_MARGIN) / (stepLength + MIN_FORWARD_SLOP);
if (adjustedHitFraction < 1.0f) {
steppingUp = true;
break;
}
}
}
if (!walkable && steppingUp ) {
return glm::vec3(0.0f);
}
// else it might not be walkable, but we aren't steppingUp yet which means we can still move forward
// TODO: slide up ramps and fall off edges (then we can remove the vertical follow of Avatar's RigidBody)
return step;
}
btConvexHullShape* MyCharacterController::computeShape() const {
// HACK: the avatar collides using convex hull with a collision margin equal to
// the old capsule radius. Two points define a capsule and additional points are
// spread out at chest level to produce a slight taper toward the feet. This
// makes the avatar more likely to collide with vertical walls at a higher point
// and thus less likely to produce a single-point collision manifold below the
// _maxStepHeight when walking into against vertical surfaces --> fixes a bug
// where the "walk up steps" feature would allow the avatar to walk up vertical
// walls.
const int32_t NUM_POINTS = 6;
btVector3 points[NUM_POINTS];
btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f);
btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f);
btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f);
points[0] = _halfHeight * yAxis;
points[1] = -_halfHeight * yAxis;
points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis;
points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis;
points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis;
points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis;
btConvexHullShape* shape = new btConvexHullShape(reinterpret_cast<btScalar*>(points), NUM_POINTS);
shape->setMargin(_radius);
return shape;
}
void MyCharacterController::initRayShotgun(const btCollisionWorld* world) {
// In order to trace rays out from the avatar's shape surface we need to know where the start points are in
// the local-frame. Since the avatar shape is somewhat irregular computing these points by hand is a hassle
// so instead we ray-trace backwards to the avatar to find them.
//
// We trace back a regular grid (see below) of points against the shape and keep any that hit.
// ___
// + / + \ +
// |+ +|
// +| + | +
// |+ +|
// +| + | +
// |+ +|
// + \ + / +
// ---
// The shotgun will send rays out from these same points to see if the avatar's shape can proceed through space.
// helper class for simple ray-traces against character
class MeOnlyResultCallback : public btCollisionWorld::ClosestRayResultCallback {
public:
MeOnlyResultCallback (btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {
_me = me;
m_collisionFilterGroup = BULLET_COLLISION_GROUP_DYNAMIC;
m_collisionFilterMask = BULLET_COLLISION_MASK_DYNAMIC;
}
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override {
if (rayResult.m_collisionObject != _me) {
return 1.0f;
}
return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
}
btRigidBody* _me;
};
const btScalar fullHalfHeight = _radius + _halfHeight;
const btScalar divisionLine = -fullHalfHeight + _maxStepHeight; // line between top and bottom
const btScalar topHeight = fullHalfHeight - divisionLine;
const btScalar slop = 0.02f;
const int32_t NUM_ROWS = 5; // must be odd number > 1
const int32_t NUM_COLUMNS = 5; // must be odd number > 1
btVector3 reach = (2.0f * _radius) * btVector3(0.0f, 0.0f, 1.0f);
{ // top points
_topPoints.clear();
_topPoints.reserve(NUM_ROWS * NUM_COLUMNS);
btScalar stepY = (topHeight - slop) / (btScalar)(NUM_ROWS - 1);
btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);
btTransform transform = _rigidBody->getWorldTransform();
btVector3 position = transform.getOrigin();
btMatrix3x3 rotation = transform.getBasis();
for (int32_t i = 0; i < NUM_ROWS; ++i) {
int32_t maxJ = NUM_COLUMNS;
btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;
if (i % 2 == 1) {
// odd rows have one less point and start a halfStep closer
maxJ -= 1;
offsetX += 0.5f * stepX;
}
for (int32_t j = 0; j < maxJ; ++j) {
btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, divisionLine + (btScalar)(i) * stepY, 0.0f);
btVector3 localRayStart = localRayEnd - reach;
MeOnlyResultCallback result(_rigidBody);
world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);
if (result.m_closestHitFraction < 1.0f) {
_topPoints.push_back(localRayStart + result.m_closestHitFraction * reach);
}
}
}
}
{ // bottom points
_bottomPoints.clear();
_bottomPoints.reserve(NUM_ROWS * NUM_COLUMNS);
btScalar steepestStepHitHeight = (_radius + 0.04f) * (1.0f - DEFAULT_MIN_FLOOR_NORMAL_DOT_UP);
btScalar stepY = (_maxStepHeight - slop - steepestStepHitHeight) / (btScalar)(NUM_ROWS - 1);
btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);
btTransform transform = _rigidBody->getWorldTransform();
btVector3 position = transform.getOrigin();
btMatrix3x3 rotation = transform.getBasis();
for (int32_t i = 0; i < NUM_ROWS; ++i) {
int32_t maxJ = NUM_COLUMNS;
btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;
if (i % 2 == 1) {
// odd rows have one less point and start a halfStep closer
maxJ -= 1;
offsetX += 0.5f * stepX;
}
for (int32_t j = 0; j < maxJ; ++j) {
btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, (divisionLine - slop) - (btScalar)(i) * stepY, 0.0f);
btVector3 localRayStart = localRayEnd - reach;
MeOnlyResultCallback result(_rigidBody);
world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);
if (result.m_closestHitFraction < 1.0f) {
_bottomPoints.push_back(localRayStart + result.m_closestHitFraction * reach);
}
}
}
}
}

28
interface/src/avatar/MyCharacterController.h
View file

@ -24,10 +24,36 @@ public:
explicit MyCharacterController(MyAvatar* avatar);
~MyCharacterController ();
virtual void updateShapeIfNecessary() override;
void setDynamicsWorld(btDynamicsWorld* world) override;
void updateShapeIfNecessary() override;
// Sweeping a convex shape through the physics simulation can be expensive when the obstacles are too
// complex (e.g. small 20k triangle static mesh) so instead we cast several rays forward and if they
// don't hit anything we consider it a clean sweep. Hence this "Shotgun" code.
class RayShotgunResult {
public:
void reset();
float hitFraction { 1.0f };
bool walkable { true };
};
/// return true if RayShotgun hits anything
bool testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result);
glm::vec3 computeHMDStep(const glm::vec3& position, const glm::vec3& step);
protected:
void initRayShotgun(const btCollisionWorld* world);
private:
btConvexHullShape* computeShape() const;
protected:
MyAvatar* _avatar { nullptr };
// shotgun scan data
btAlignedObjectArray<btVector3> _topPoints;
btAlignedObjectArray<btVector3> _bottomPoints;
};
#endif // hifi_MyCharacterController_h

394
libraries/physics/src/CharacterController.cpp Normal file → Executable file
View file

@ -13,8 +13,8 @@
#include <NumericalConstants.h>
#include "PhysicsCollisionGroups.h"
#include "ObjectMotionState.h"
#include "PhysicsHelpers.h"
#include "PhysicsLogging.h"
const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
@ -62,10 +62,6 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const
}
CharacterController::CharacterController() {
_halfHeight = 1.0f;
_enabled = false;
_floorDistance = MAX_FALL_HEIGHT;
_targetVelocity.setValue(0.0f, 0.0f, 0.0f);
@ -107,6 +103,7 @@ bool CharacterController::needsAddition() const {
void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
if (_dynamicsWorld != world) {
// remove from old world
if (_dynamicsWorld) {
if (_rigidBody) {
_dynamicsWorld->removeRigidBody(_rigidBody);
@ -115,16 +112,25 @@ 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
// because adding an object to the world will overwrite it with the default gravity.
btVector3 oldGravity = _rigidBody->getGravity();
_dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, BULLET_COLLISION_MASK_MY_AVATAR);
_dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
_dynamicsWorld->addAction(this);
// restore gravity settings
_rigidBody->setGravity(oldGravity);
// restore gravity settings because adding an object to the world overwrites its gravity setting
_rigidBody->setGravity(_gravity * _currentUp);
btCollisionShape* shape = _rigidBody->getCollisionShape();
assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);
_ghost.setCharacterShape(static_cast<btConvexHullShape*>(shape));
}
_ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
_ghost.setCollisionWorld(_dynamicsWorld);
_ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
_ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight));
_ghost.setMinWallAngle(PI / 4.0f);
_ghost.setUpDirection(_currentUp);
_ghost.setMotorOnly(true);
_ghost.setWorldTransform(_rigidBody->getWorldTransform());
}
if (_dynamicsWorld) {
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
@ -138,38 +144,78 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
}
}
static const float COS_PI_OVER_THREE = cosf(PI / 3.0f);
bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
bool pushing = _targetVelocity.length2() > FLT_EPSILON;
btDispatcher* dispatcher = collisionWorld->getDispatcher();
int numManifolds = dispatcher->getNumManifolds();
bool hasFloor = false;
btTransform rotation = _rigidBody->getWorldTransform();
rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part
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<const btCollisionObject*>(contactManifold->getBody0());
const btCollisionObject* obB = static_cast<const btCollisionObject*>(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();
int stepContactIndex = -1;
float highestStep = _minStepHeight;
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 < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
hasFloor = true;
if (!pushing) {
// we're not pushing against anything so we can early exit
// (all we need to know is that there is a floor)
break;
}
}
if (pushing && _targetVelocity.dot(normal) < 0.0f) {
// remember highest step obstacle
if (!_stepUpEnabled || hitHeight > _maxStepHeight) {
// this manifold is invalidated by point that is too high
stepContactIndex = -1;
break;
} else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {
highestStep = hitHeight;
stepContactIndex = j;
hasFloor = true;
}
}
}
if (stepContactIndex > -1 && highestStep > _stepHeight) {
// remember step info for later
btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex);
btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
_stepNormal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
_stepHeight = highestStep;
_stepPoint = rotation * pointOnCharacter; // rotate into world-frame
}
if (hasFloor && !(pushing && _stepUpEnabled)) {
// early exit since all we need to know is that we're on a floor
break;
}
}
}
return false;
return hasFloor;
}
void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) {
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
}
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;
@ -183,21 +229,16 @@ void CharacterController::preStep(btCollisionWorld* collisionWorld) {
if (rayCallback.hasHit()) {
_floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
}
_hasSupport = checkForSupport(collisionWorld);
}
const btScalar MIN_TARGET_SPEED = 0.001f;
const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED;
void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) {
_stepHeight = _minStepHeight; // clears memory of last step obstacle
_hasSupport = checkForSupport(collisionWorld);
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().
const float MINIMUM_TIME_REMAINING = 0.005f;
const float MAX_DISPLACEMENT = 0.5f * _radius;
@ -231,6 +272,28 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
_rigidBody->setWorldTransform(btTransform(endRot, endPos));
}
_followTime += dt;
float stepUpSpeed2 = _stepUpVelocity.length2();
if (stepUpSpeed2 > FLT_EPSILON) {
// we step up with micro-teleports rather than applying velocity
// use a speed that would ballistically reach _stepHeight under gravity
_stepUpVelocity /= sqrtf(stepUpSpeed2);
btScalar minStepUpSpeed = sqrtf(fabsf(2.0f * _gravity * _stepHeight));
btTransform transform = _rigidBody->getWorldTransform();
transform.setOrigin(transform.getOrigin() + (dt * minStepUpSpeed) * _stepUpVelocity);
_rigidBody->setWorldTransform(transform);
// make sure the upward velocity is large enough to clear the very top of the step
const btScalar MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT = 0.5f;
minStepUpSpeed = MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT * sqrtf(fabsf(2.0f * _gravity * _minStepHeight));
btScalar vDotUp = velocity.dot(_currentUp);
if (vDotUp < minStepUpSpeed) {
velocity += (minStepUpSpeed - vDotUp) * _stepUpVelocity;
}
}
_rigidBody->setLinearVelocity(velocity + _parentVelocity);
_ghost.setWorldTransform(_rigidBody->getWorldTransform());
}
void CharacterController::jump() {
@ -272,95 +335,96 @@ void CharacterController::setState(State desiredState) {
#ifdef DEBUG_STATE_CHANGE
qCDebug(physics) << "CharacterController::setState" << stateToStr(desiredState) << "from" << stateToStr(_state) << "," << reason;
#endif
if (desiredState == State::Hover && _state != State::Hover) {
// hover enter
if (_rigidBody) {
if (_rigidBody) {
if (desiredState == State::Hover && _state != State::Hover) {
// hover enter
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
}
} else if (_state == State::Hover && desiredState != State::Hover) {
// hover exit
if (_rigidBody) {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
} else if (_state == State::Hover && desiredState != State::Hover) {
// hover exit
if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(_gravity * _currentUp);
}
}
}
_state = desiredState;
}
}
void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) {
_boxScale = scale;
float x = _boxScale.x;
float z = _boxScale.z;
void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) {
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;
const btScalar DEFAULT_MIN_STEP_HEIGHT = 0.041f; // HACK: hardcoded now but should just larger than shape margin
const btScalar MAX_STEP_FRACTION_OF_HALF_HEIGHT = 0.56f;
_minStepHeight = DEFAULT_MIN_STEP_HEIGHT;
_maxStepHeight = MAX_STEP_FRACTION_OF_HALF_HEIGHT * (_halfHeight + _radius);
if (_dynamicsWorld) {
// must REMOVE from world prior to shape update
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
}
_pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
// only need to ADD back when we happen to be enabled
if (_enabled) {
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
}
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
}
// it's ok to change offset immediately -- there are no thread safety issues here
_shapeLocalOffset = corner + 0.5f * _boxScale;
_shapeLocalOffset = minCorner + 0.5f * scale;
}
void CharacterController::setEnabled(bool enabled) {
if (enabled != _enabled) {
if (enabled) {
// Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit.
// Setting the ADD bit here works for all cases so we don't even bother checking other bits.
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
} else {
if (_dynamicsWorld) {
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
}
_pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION;
void CharacterController::setCollisionGroup(int16_t group) {
if (_collisionGroup != group) {
_collisionGroup = group;
_pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP;
_ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
}
}
void CharacterController::handleChangedCollisionGroup() {
if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_GROUP) {
// ATM the easiest way to update collision groups is to remove/re-add the RigidBody
if (_dynamicsWorld) {
_dynamicsWorld->removeRigidBody(_rigidBody);
_dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
}
_pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP;
if (_state != State::Hover && _rigidBody) {
_gravity = _collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ? 0.0f : DEFAULT_CHARACTER_GRAVITY;
_rigidBody->setGravity(_gravity * _currentUp);
}
SET_STATE(State::Hover, "setEnabled");
_enabled = enabled;
}
}
void CharacterController::updateUpAxis(const glm::quat& rotation) {
btVector3 oldUp = _currentUp;
_currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
if (_state != State::Hover) {
const btScalar MIN_UP_ERROR = 0.01f;
if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
}
_ghost.setUpDirection(_currentUp);
if (_state != State::Hover && _rigidBody) {
_rigidBody->setGravity(_gravity * _currentUp);
}
}
void CharacterController::setPositionAndOrientation(
const glm::vec3& position,
const glm::quat& orientation) {
// 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 {
if (_enabled && _rigidBody) {
if (_rigidBody) {
const btTransform& avatarTransform = _rigidBody->getWorldTransform();
rotation = bulletToGLM(avatarTransform.getRotation());
position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset;
@ -428,16 +492,18 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
btScalar angle = motor.rotation.getAngle();
btVector3 velocity = worldVelocity.rotate(axis, -angle);
if (_state == State::Hover || motor.hTimescale == motor.vTimescale) {
if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ||
_state == State::Hover || motor.hTimescale == motor.vTimescale) {
// modify velocity
btScalar tau = dt / motor.hTimescale;
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);
@ -445,12 +511,32 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
} else {
// compute local UP
btVector3 up = _currentUp.rotate(axis, -angle);
btVector3 motorVelocity = motor.velocity;
// save these non-adjusted components for later
btVector3 vTargetVelocity = motorVelocity.dot(up) * up;
btVector3 hTargetVelocity = motorVelocity - vTargetVelocity;
if (_stepHeight > _minStepHeight) {
// there is a step --> compute velocity direction to go over step
btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle);
if (motorVelocityWF.dot(_stepNormal) < 0.0f) {
// the motor pushes against step
motorVelocityWF = _stepNormal.cross(_stepPoint.cross(motorVelocityWF));
btScalar doubleCrossLength2 = motorVelocityWF.length2();
if (doubleCrossLength2 > FLT_EPSILON) {
// scale the motor in the correct direction and rotate back to motor-frame
motorVelocityWF *= (motorVelocity.length() / sqrtf(doubleCrossLength2));
_stepUpVelocity += motorVelocityWF.rotate(axis, -angle);
}
}
}
// 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 vMotorVelocity = motorVelocity.dot(up) * up;
btVector3 hMotorVelocity = motorVelocity - vMotorVelocity;
// modify each component separately
btScalar maxTau = 0.0f;
@ -460,7 +546,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
tau = 1.0f;
}
maxTau = tau;
hVelocity += (hTargetVelocity - hVelocity) * tau;
hVelocity += (hMotorVelocity - hVelocity) * tau;
}
if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) {
btScalar tau = dt / motor.vTimescale;
@ -470,11 +556,12 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
if (tau > maxTau) {
maxTau = tau;
}
vVelocity += (vTargetVelocity - vVelocity) * tau;
vVelocity += (vMotorVelocity - vVelocity) * tau;
}
// add components back together and rotate into world-frame
velocity = (hVelocity + vVelocity).rotate(axis, angle);
_targetVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle);
// store velocity and weights
velocities.push_back(velocity);
@ -492,6 +579,8 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
velocities.reserve(_motors.size());
std::vector<btScalar> weights;
weights.reserve(_motors.size());
_targetVelocity = btVector3(0.0f, 0.0f, 0.0f);
_stepUpVelocity = btVector3(0.0f, 0.0f, 0.0f);
for (int i = 0; i < (int)_motors.size(); ++i) {
applyMotor(i, dt, velocity, velocities, weights);
}
@ -507,14 +596,18 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
for (size_t i = 0; i < velocities.size(); ++i) {
velocity += (weights[i] / totalWeight) * velocities[i];
}
_targetVelocity /= totalWeight;
}
if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
velocity = btVector3(0.0f, 0.0f, 0.0f);
}
// 'thrust' is applied at the very end
_targetVelocity += dt * _linearAcceleration;
velocity += dt * _linearAcceleration;
_targetVelocity = velocity;
// Note the differences between these two variables:
// _targetVelocity = ideal final velocity according to input
// velocity = real final velocity after motors are applied to current velocity
}
void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
@ -523,57 +616,54 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
velocity = bulletToGLM(btVelocity);
}
void CharacterController::preSimulation() {
if (_enabled && _dynamicsWorld && _rigidBody) {
quint64 now = usecTimestampNow();
void CharacterController::updateState() {
if (!_dynamicsWorld) {
return;
}
const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius;
const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight;
const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND;
const btScalar MIN_HOVER_HEIGHT = 2.5f;
const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND;
// slam body to where it is supposed to be
_rigidBody->setWorldTransform(_characterBodyTransform);
btVector3 velocity = _rigidBody->getLinearVelocity();
_preSimulationVelocity = velocity;
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = _position;
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = _characterBodyTransform.getOrigin();
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius;
const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight;
const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND;
const btScalar MIN_HOVER_HEIGHT = 2.5f;
const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND;
const btScalar MAX_WALKING_SPEED = 2.5f;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
bool rayHasHit = rayCallback.hasHit();
quint64 now = usecTimestampNow();
if (rayHasHit) {
_rayHitStartTime = now;
_floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight);
} else {
const quint64 RAY_HIT_START_PERIOD = 500 * MSECS_PER_SECOND;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
bool rayHasHit = rayCallback.hasHit();
if (rayHasHit) {
_rayHitStartTime = now;
_floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight);
} else if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) {
if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) {
rayHasHit = true;
} else {
_floorDistance = FLT_MAX;
}
}
// record a time stamp when the jump button was first pressed.
if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
if (_pendingFlags & PENDING_FLAG_JUMP) {
_jumpButtonDownStartTime = now;
_jumpButtonDownCount++;
}
// record a time stamp when the jump button was first pressed.
bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP;
if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
if (_pendingFlags & PENDING_FLAG_JUMP) {
_jumpButtonDownStartTime = now;
_jumpButtonDownCount++;
}
}
bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP;
btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp;
bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f);
btVector3 velocity = _preSimulationVelocity;
// disable normal state transitions while collisionless
if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) {
switch (_state) {
case State::Ground:
if (!rayHasHit && !_hasSupport) {
@ -613,32 +703,47 @@ void CharacterController::preSimulation() {
break;
}
case State::Hover:
if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) {
btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp;
const btScalar MAX_WALKING_SPEED = 2.5f;
bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f);
if ((_floorDistance < MIN_HOVER_HEIGHT) &&
!(jumpButtonHeld || flyingFast || (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD)) {
SET_STATE(State::InAir, "near ground");
} else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) {
SET_STATE(State::Ground, "touching ground");
}
break;
}
} else {
// in collisionless state switch only between Ground and Hover states
if (rayHasHit) {
SET_STATE(State::Ground, "collisionless above ground");
} else {
SET_STATE(State::Hover, "collisionless in air");
}
}
}
void CharacterController::preSimulation() {
if (_rigidBody) {
// slam body transform and remember velocity
_rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position)));
_preSimulationVelocity = _rigidBody->getLinearVelocity();
updateState();
}
_previousFlags = _pendingFlags;
_pendingFlags &= ~PENDING_FLAG_JUMP;
_followTime = 0.0f;
_followLinearDisplacement = btVector3(0, 0, 0);
_followAngularDisplacement = btQuaternion::getIdentity();
}
void CharacterController::postSimulation() {
// postSimulation() exists for symmetry and just in case we need to do something here later
if (_enabled && _dynamicsWorld && _rigidBody) {
btVector3 velocity = _rigidBody->getLinearVelocity();
_velocityChange = velocity - _preSimulationVelocity;
if (_rigidBody) {
_velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity;
}
}
bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) {
if (!_rigidBody) {
return false;
@ -655,7 +760,10 @@ void CharacterController::setFlyingAllowed(bool value) {
_flyingAllowed = value;
if (!_flyingAllowed && _state == State::Hover) {
SET_STATE(State::InAir, "flying not allowed");
// disable normal state transitions while collisionless
if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) {
SET_STATE(State::InAir, "flying not allowed");
}
}
}
}

54
libraries/physics/src/CharacterController.h
View file

@ -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 <assert.h>
#include <stdint.h>
@ -19,12 +19,18 @@
#include <BulletDynamics/Character/btCharacterControllerInterface.h>
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include <PhysicsCollisionGroups.h>
#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;
const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2;
const uint32_t PENDING_FLAG_JUMP = 1U << 3;
const uint32_t PENDING_FLAG_UPDATE_COLLISION_GROUP = 1U << 4;
const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f);
const float DEFAULT_CHARACTER_GRAVITY = -5.0f;
@ -44,7 +50,7 @@ public:
bool needsRemoval() const;
bool needsAddition() const;
void setDynamicsWorld(btDynamicsWorld* world);
virtual void setDynamicsWorld(btDynamicsWorld* world);
btCollisionObject* getCollisionObject() { return _rigidBody; }
virtual void updateShapeIfNecessary() = 0;
@ -56,10 +62,7 @@ public:
virtual void warp(const btVector3& origin) override { }
virtual void debugDraw(btIDebugDraw* debugDrawer) override { }
virtual void setUpInterpolate(bool value) override { }
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override {
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
}
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override;
virtual void preStep(btCollisionWorld *collisionWorld) override;
virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) override;
virtual bool canJump() const override { assert(false); return false; } // never call this
@ -69,6 +72,7 @@ public:
void clearMotors();
void addMotor(const glm::vec3& velocity, const glm::quat& rotation, float horizTimescale, float vertTimescale = -1.0f);
void applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector<btVector3>& velocities, std::vector<btScalar>& weights);
void setStepUpEnabled(bool enabled) { _stepUpEnabled = enabled; }
void computeNewVelocity(btScalar dt, btVector3& velocity);
void computeNewVelocity(btScalar dt, glm::vec3& velocity);
@ -103,12 +107,15 @@ public:
};
State getState() const { return _state; }
void updateState();
void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale);
void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale);
bool isEnabled() const { return _enabled; } // thread-safe
void setEnabled(bool enabled);
bool isEnabledAndReady() const { return _enabled && _dynamicsWorld; }
bool isEnabledAndReady() const { return _dynamicsWorld; }
void setCollisionGroup(int16_t group);
int16_t getCollisionGroup() const { return _collisionGroup; }
void handleChangedCollisionGroup();
bool getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation);
@ -123,7 +130,7 @@ protected:
#endif
void updateUpAxis(const glm::quat& rotation);
bool checkForSupport(btCollisionWorld* collisionWorld) const;
bool checkForSupport(btCollisionWorld* collisionWorld);
protected:
struct CharacterMotor {
@ -136,6 +143,7 @@ protected:
};
std::vector<CharacterMotor> _motors;
CharacterGhostObject _ghost;
btVector3 _currentUp;
btVector3 _targetVelocity;
btVector3 _parentVelocity;
@ -144,6 +152,8 @@ protected:
btTransform _followDesiredBodyTransform;
btScalar _followTimeRemaining;
btTransform _characterBodyTransform;
btVector3 _position;
btQuaternion _rotation;
glm::vec3 _shapeLocalOffset;
@ -155,13 +165,23 @@ protected:
quint32 _jumpButtonDownCount;
quint32 _takeoffJumpButtonID;
btScalar _halfHeight;
btScalar _radius;
// data for walking up steps
btVector3 _stepPoint { 0.0f, 0.0f, 0.0f };
btVector3 _stepNormal { 0.0f, 0.0f, 0.0f };
btVector3 _stepUpVelocity { 0.0f, 0.0f, 0.0f };
btScalar _stepHeight { 0.0f };
btScalar _minStepHeight { 0.0f };
btScalar _maxStepHeight { 0.0f };
btScalar _minFloorNormalDotUp { DEFAULT_MIN_FLOOR_NORMAL_DOT_UP };
btScalar _halfHeight { 0.0f };
btScalar _radius { 0.0f };
btScalar _floorDistance;
bool _stepUpEnabled { true };
bool _hasSupport;
btScalar _gravity;
btScalar _gravity { DEFAULT_CHARACTER_GRAVITY };
btScalar _jumpSpeed;
btScalar _followTime;
@ -169,7 +189,6 @@ protected:
btQuaternion _followAngularDisplacement;
btVector3 _linearAcceleration;
std::atomic_bool _enabled;
State _state;
bool _isPushingUp;
@ -179,6 +198,7 @@ protected:
uint32_t _previousFlags { 0 };
bool _flyingAllowed { true };
int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR };
};
#endif // hifi_CharacterControllerInterface_h
#endif // hifi_CharacterController_h

415
libraries/physics/src/CharacterGhostObject.cpp Executable file
View file

@ -0,0 +1,415 @@
//
// CharacterGhostObject.cpp
// libraries/physics/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 <stdint.h>
#include <assert.h>
#include <PhysicsHelpers.h>
#include "CharacterRayResult.h"
#include "CharacterGhostShape.h"
CharacterGhostObject::~CharacterGhostObject() {
removeFromWorld();
if (_ghostShape) {
delete _ghostShape;
_ghostShape = nullptr;
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::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) {
_radius = radius;
_halfHeight = halfHeight;
}
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;
if (_motorOnly) {
_linearVelocity = _motorVelocity;
}
}
// override of btCollisionObject::setCollisionShape()
void CharacterGhostObject::setCharacterShape(btConvexHullShape* shape) {
assert(shape);
// we create our own shape with an expanded Aabb for more reliable sweep tests
if (_ghostShape) {
delete _ghostShape;
}
_ghostShape = new CharacterGhostShape(static_cast<const btConvexHullShape*>(shape));
setCollisionShape(_ghostShape);
}
void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) {
if (world != _world) {
removeFromWorld();
_world = world;
addToWorld();
}
}
void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) {
bool oldOnFloor = _onFloor;
_onFloor = false;
_steppingUp = false;
assert(_world && _inWorld);
updateVelocity(dt, gravity);
// 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
}
btTransform startTransform = getWorldTransform();
btVector3 startPosition = startTransform.getOrigin();
if (_onFloor) {
// resolvePenetration() pushed the avatar out of a floor so
// we must updateTraction() before using _linearVelocity
updateTraction(startPosition);
}
btScalar speed = _linearVelocity.length();
btVector3 forwardSweep = dt * _linearVelocity;
btScalar stepDistance = dt * speed;
btScalar MIN_SWEEP_DISTANCE = 0.0001f;
if (stepDistance < MIN_SWEEP_DISTANCE) {
// not moving, no need to sweep
updateTraction(startPosition);
return;
}
// augment forwardSweep to help slow moving sweeps get over steppable ledges
const btScalar MIN_OVERSHOOT = 0.04f; // default margin
if (overshoot < MIN_OVERSHOOT) {
overshoot = MIN_OVERSHOOT;
}
btScalar longSweepDistance = stepDistance + overshoot;
forwardSweep *= longSweepDistance / stepDistance;
// step forward
CharacterSweepResult result(this);
btTransform nextTransform = startTransform;
nextTransform.setOrigin(startPosition + forwardSweep);
sweepTest(_characterShape, startTransform, nextTransform, result); // forward
if (!result.hasHit()) {
nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep);
setWorldTransform(nextTransform);
updateTraction(nextTransform.getOrigin());
return;
}
bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < MIN_OVERSHOOT;
if (verticalOnly) {
// no need to step
nextTransform.setOrigin(startPosition + (result.m_closestHitFraction * stepDistance / longSweepDistance) * forwardSweep);
setWorldTransform(nextTransform);
if (result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
_floorNormal = result.m_hitNormalWorld;
_floorContact = result.m_hitPointWorld;
_steppingUp = false;
_onFloor = true;
_hovering = false;
}
updateTraction(nextTransform.getOrigin());
return;
}
// check if this hit is obviously unsteppable
btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection));
btScalar hitHeight = hitFromBase.dot(_upDirection);
if (hitHeight > _maxStepHeight) {
// shape 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;
}
nextTransform.setOrigin(startPosition + forwardTranslation);
setWorldTransform(nextTransform);
_onFloor = _onFloor || oldOnFloor;
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();
startTransform.setOrigin(startPosition + availableStepHeight * _upDirection);
nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep);
sweepTest(_characterShape, startTransform, nextTransform, result);
if (result.hasHit()) {
startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep);
} else {
startTransform = nextTransform;
}
// sweep down in search of future landing spot
result.resetHitHistory();
btVector3 downSweep = (- availableStepHeight) * _upDirection;
nextTransform.setOrigin(startTransform.getOrigin() + downSweep);
sweepTest(_characterShape, 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;
_steppingUp = true;
_onFloor = true;
_hovering = false;
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
nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep);
_onFloor = _onFloor || oldOnFloor;
updateTraction(nextTransform.getOrigin());
}
setWorldTransform(nextTransform);
}
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;
}
bool CharacterGhostObject::rayTest(const btVector3& start,
const btVector3& end,
CharacterRayResult& result) const {
if (_world && _inWorld) {
_world->rayTest(start, end, result);
}
return result.hasHit();
}
void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut) {
// minBoxOut and maxBoxOut will be updated with penetration envelope.
// If one of the corner points is <0,0,0> then the penetration is resolvable in a single step,
// but if the space spanned by the two corners extends in both directions along at least one
// component then we the object is sandwiched between two opposing objects.
// We assume this object has just been moved to its current location, or else objects have been
// moved around it since the last step so we must update the overlapping pairs.
refreshOverlappingPairCache();
// compute collision details
btHashedOverlappingPairCache* pairCache = getOverlappingPairCache();
_world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher());
// loop over contact manifolds to compute the penetration box
minBoxOut = btVector3(0.0f, 0.0f, 0.0f);
maxBoxOut = btVector3(0.0f, 0.0f, 0.0f);
btManifoldArray manifoldArray;
int numPairs = pairCache->getNumOverlappingPairs();
for (int i = 0; i < numPairs; i++) {
manifoldArray.resize(0);
btBroadphasePair* collisionPair = &(pairCache->getOverlappingPairArray()[i]);
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(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;
if (directionSign > 0.0f) {
_floorContact = pt.m_positionWorldOnA;
} else {
_floorContact = pt.m_positionWorldOnB;
}
_onFloor = true;
}
}
btVector3 penetration = (-penetrationDepth) * normal;
minBoxOut.setMin(penetration);
maxBoxOut.setMax(penetration);
}
}
}
}
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::removeFromWorld() {
if (_world && _inWorld) {
_world->removeCollisionObject(this);
_inWorld = false;
}
}
void CharacterGhostObject::addToWorld() {
if (_world && !_inWorld) {
assert(getCollisionShape());
setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
_world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask);
_inWorld = true;
}
}
bool CharacterGhostObject::resolvePenetration(int numTries) {
btVector3 minBox, maxBox;
measurePenetration(minBox, maxBox);
btVector3 restore = maxBox + minBox;
if (restore.length2() > 0.0f) {
btTransform transform = getWorldTransform();
transform.setOrigin(transform.getOrigin() + restore);
setWorldTransform(transform);
return false;
}
return true;
}
void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) {
if (!_motorOnly) {
if (_hovering) {
_linearVelocity *= 0.999f; // HACK damping
} else {
_linearVelocity += (dt * gravity) * _upDirection;
}
}
}
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 || _motorOnly) {
_linearVelocity = _motorVelocity;
} else if (_onFloor) {
// compute a velocity that swings the shape around the _floorContact
btVector3 leverArm = _floorContact - position;
btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm));
btScalar pathLength = pathDirection.length();
if (pathLength > FLT_EPSILON) {
_linearVelocity = (_motorVelocity.length() / pathLength) * pathDirection;
} else {
_linearVelocity = btVector3(0.0f, 0.0f, 0.0f);
}
}
}
btScalar CharacterGhostObject::measureAvailableStepHeight() const {
CharacterSweepResult result(this);
btTransform transform = getWorldTransform();
btTransform nextTransform = transform;
nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection);
sweepTest(_characterShape, transform, nextTransform, result);
return result.m_closestHitFraction * _maxStepHeight;
}

103
libraries/physics/src/CharacterGhostObject.h Executable file
View file

@ -0,0 +1,103 @@
//
// CharacterGhostObject.h
// libraries/physics/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 <stdint.h>
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#include <stdint.h>
#include "CharacterSweepResult.h"
#include "CharacterRayResult.h"
class CharacterGhostShape;
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 setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight);
void setUpDirection(const btVector3& up);
void setMotorVelocity(const btVector3& velocity);
void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); }
void setMaxStepHeight(btScalar height) { _maxStepHeight = height; }
void setLinearVelocity(const btVector3& velocity) { _linearVelocity = velocity; }
const btVector3& getLinearVelocity() const { return _linearVelocity; }
void setCharacterShape(btConvexHullShape* shape);
void setCollisionWorld(btCollisionWorld* world);
void move(btScalar dt, btScalar overshoot, btScalar gravity);
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 isHovering() const { return _hovering; }
void setHovering(bool hovering) { _hovering = hovering; }
void setMotorOnly(bool motorOnly) { _motorOnly = motorOnly; }
bool hasSupport() const { return _onFloor; }
bool isSteppingUp() const { return _steppingUp; }
const btVector3& getFloorNormal() const { return _floorNormal; }
void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut);
void refreshOverlappingPairCache();
protected:
void removeFromWorld();
void addToWorld();
bool resolvePenetration(int numTries);
void updateVelocity(btScalar dt, btScalar gravity);
void updateTraction(const btVector3& position);
btScalar measureAvailableStepHeight() const;
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 _halfHeight { 0.0f };
btScalar _radius { 0.0f };
btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal
btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb
btConvexHullShape* _characterShape { nullptr }; // input, shape of character
CharacterGhostShape* _ghostShape { nullptr }; // internal, shape whose Aabb is used for overlap cache
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 _steppingUp { false }; // output, future sweep hit a steppable ledge
bool _hasFloor { false }; // output, has floor underneath to fall on
bool _motorOnly { false }; // input, _linearVelocity slaves to _motorVelocity
};
#endif // hifi_CharacterGhostObject_h

31
libraries/physics/src/CharacterGhostShape.cpp Normal file
View file

@ -0,0 +1,31 @@
//
// CharacterGhostShape.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.14
// 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 "CharacterGhostShape.h"
#include <assert.h>
CharacterGhostShape::CharacterGhostShape(const btConvexHullShape* shape) :
btConvexHullShape(reinterpret_cast<const btScalar*>(shape->getUnscaledPoints()), shape->getNumPoints(), sizeof(btVector3)) {
assert(shape);
assert(shape->getUnscaledPoints());
assert(shape->getNumPoints() > 0);
setMargin(shape->getMargin());
}
void CharacterGhostShape::getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const {
btConvexHullShape::getAabb(t, aabbMin, aabbMax);
// double the size of the Aabb by expanding both corners by half the extent
btVector3 expansion = 0.5f * (aabbMax - aabbMin);
aabbMin -= expansion;
aabbMax += expansion;
}

25
libraries/physics/src/CharacterGhostShape.h Normal file
View file

@ -0,0 +1,25 @@
//
// CharacterGhostShape.h
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.14
// 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_CharacterGhostShape_h
#define hifi_CharacterGhostShape_h
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
class CharacterGhostShape : public btConvexHullShape {
// Same as btConvexHullShape but reports an expanded Aabb for larger ghost overlap cache
public:
CharacterGhostShape(const btConvexHullShape* shape);
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override;
};
#endif // hifi_CharacterGhostShape_h

31
libraries/physics/src/CharacterRayResult.cpp Executable file
View file

@ -0,0 +1,31 @@
//
// CharaterRayResult.cpp
// libraries/physics/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 <assert.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);
}

44
libraries/physics/src/CharacterRayResult.h Normal file
View file

@ -0,0 +1,44 @@
//
// CharaterRayResult.h
// libraries/physics/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 <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
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

42
libraries/physics/src/CharacterSweepResult.cpp Executable file
View file

@ -0,0 +1,42 @@
//
// CharaterSweepResult.cpp
// libraries/physics/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 <assert.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);
}

45
libraries/physics/src/CharacterSweepResult.h Normal file
View file

@ -0,0 +1,45 @@
//
// CharaterSweepResult.h
// libraries/physics/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 <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
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