Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-20 14:03:55 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

adding kinematic character controller demo

Browse source
This commit is contained in:
Andrew Meadows 2016-09-14 11:43:52 -07:00
parent 0fde3a2d76
commit f5c5c68937
13 changed files with 741 additions and 50 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
interface/src/Menu.cpp
View file

@ -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,

1
interface/src/Menu.h
View file

@ -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";

4
interface/src/avatar/MyAvatar.cpp
View file

@ -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));
}

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

@ -492,6 +492,7 @@ private:
ThreadSafeValueCache<controller::Pose> _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 };

10
interface/src/avatar/MyCharacterController.cpp
View file

@ -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
}

123
libraries/physics/src/CharacterController.cpp
View file

@ -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<btScalar> 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;
}
}

16
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>
@ -22,6 +22,7 @@
#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;
@ -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<CharacterMotor> _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

390
libraries/physics/src/CharacterGhostObject.cpp Normal file
View file

@ -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 <assert.h>
#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<const btConvexShape*>(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<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;
_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<const btConvexShape*>(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);
}

83
libraries/physics/src/CharacterGhostObject.h Normal file
View file

@ -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 <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#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

29
libraries/physics/src/CharacterRayResult.cpp Normal file
View file

@ -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);
}

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

@ -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 <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

40
libraries/physics/src/CharacterSweepResult.cpp Normal file
View file

@ -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);
}

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

@ -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 <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