mirror of
https://github.com/HifiExperiments/overte.git
synced 2025-08-04 18:10:14 +02:00
309 lines
11 KiB
C++
309 lines
11 KiB
C++
#include <BulletCollision/CollisionShapes/btMultiSphereShape.h>
|
|
#include <BulletDynamics/Dynamics/btRigidBody.h>
|
|
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
|
|
#include <LinearMath/btDefaultMotionState.h>
|
|
|
|
#include "BulletUtil.h"
|
|
#include "DynamicCharacterController.h"
|
|
|
|
const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
|
|
const float DEFAULT_GRAVITY = 5.0f;
|
|
const float TERMINAL_VELOCITY = 55.0f;
|
|
const float JUMP_SPEED = 3.5f;
|
|
|
|
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;
|
|
|
|
DynamicCharacterController::DynamicCharacterController(AvatarData* avatarData) {
|
|
_rayLambda[0] = 1.0f;
|
|
_rayLambda[1] = 1.0f;
|
|
_halfHeight = 1.0f;
|
|
_shape = NULL;
|
|
_rigidBody = NULL;
|
|
|
|
assert(avatarData);
|
|
_avatarData = avatarData;
|
|
|
|
_enabled = false;
|
|
|
|
_walkVelocity.setValue(0.0f,0.0f,0.0f);
|
|
_jumpSpeed = JUMP_SPEED;
|
|
_isOnGround = false;
|
|
_isJumping = false;
|
|
_isHovering = true;
|
|
|
|
_pendingFlags = PENDING_FLAG_UPDATE_SHAPE;
|
|
updateShapeIfNecessary();
|
|
}
|
|
|
|
DynamicCharacterController::~DynamicCharacterController() {
|
|
}
|
|
|
|
// virtual
|
|
void DynamicCharacterController::setWalkDirection(const btVector3& walkDirection) {
|
|
_walkVelocity = walkDirection;
|
|
}
|
|
|
|
void DynamicCharacterController::preStep(btCollisionWorld* collisionWorld) {
|
|
const btTransform& xform = _rigidBody->getCenterOfMassTransform();
|
|
|
|
btVector3 down = -xform.getBasis()[1];
|
|
btVector3 forward = xform.getBasis()[2];
|
|
down.normalize();
|
|
forward.normalize();
|
|
|
|
_raySource[0] = xform.getOrigin();
|
|
_raySource[1] = xform.getOrigin();
|
|
|
|
_rayTarget[0] = _raySource[0] + down * _halfHeight * btScalar(1.1f);
|
|
_rayTarget[1] = _raySource[1] + forward * _halfHeight * btScalar(1.1f);
|
|
|
|
class ClosestNotMe : public btCollisionWorld::ClosestRayResultCallback {
|
|
public:
|
|
ClosestNotMe(btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {
|
|
_me = me;
|
|
}
|
|
|
|
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) {
|
|
if (rayResult.m_collisionObject == _me)
|
|
return 1.0f;
|
|
|
|
return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace
|
|
);
|
|
}
|
|
protected:
|
|
btRigidBody* _me;
|
|
};
|
|
|
|
ClosestNotMe rayCallback(_rigidBody);
|
|
|
|
int i = 0;
|
|
for (i = 0; i < 2; i++) {
|
|
rayCallback.m_closestHitFraction = 1.0f;
|
|
collisionWorld->rayTest(_raySource[i], _rayTarget[i], rayCallback);
|
|
if (rayCallback.hasHit()) {
|
|
_rayLambda[i] = rayCallback.m_closestHitFraction;
|
|
} else {
|
|
_rayLambda[i] = 1.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld,btScalar dt) {
|
|
btVector3 currentVelocity = _rigidBody->getLinearVelocity();
|
|
btScalar currentSpeed = currentVelocity.length();
|
|
|
|
btVector3 desiredVelocity = _walkVelocity;
|
|
btScalar desiredSpeed = desiredVelocity.length();
|
|
const btScalar MIN_SPEED = 0.001f;
|
|
if (desiredSpeed < MIN_SPEED) {
|
|
if (currentSpeed < MIN_SPEED) {
|
|
_rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
|
|
} else {
|
|
const btScalar BRAKING_TIMESCALE = 0.2f;
|
|
btScalar tau = dt / BRAKING_TIMESCALE;
|
|
_rigidBody->setLinearVelocity((1.0f - tau) * currentVelocity);
|
|
}
|
|
} else {
|
|
const btScalar WALKING_TIMESCALE = 0.5f;
|
|
btScalar tau = dt / WALKING_TIMESCALE;
|
|
if (onGround()) {
|
|
// subtract vertical component
|
|
desiredVelocity = desiredVelocity - desiredVelocity.dot(_currentUp) * _currentUp;
|
|
}
|
|
_rigidBody->setLinearVelocity(currentVelocity - tau * (currentVelocity - desiredVelocity));
|
|
}
|
|
}
|
|
|
|
bool DynamicCharacterController::canJump() const {
|
|
return onGround();
|
|
}
|
|
|
|
void DynamicCharacterController::jump() {
|
|
/*
|
|
if (!canJump()) {
|
|
return;
|
|
}
|
|
|
|
btTransform xform = _rigidBody->getCenterOfMassTransform();
|
|
btVector3 up = xform.getBasis()[1];
|
|
up.normalize();
|
|
btScalar magnitude = (btScalar(1.0)/_rigidBody->getInvMass()) * btScalar(8.0);
|
|
_rigidBody->applyCentralImpulse(up * magnitude);
|
|
*/
|
|
}
|
|
|
|
bool DynamicCharacterController::onGround() const {
|
|
return _rayLambda[0] < btScalar(1.0);
|
|
}
|
|
|
|
void DynamicCharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) {
|
|
_boxScale = scale;
|
|
|
|
float x = _boxScale.x;
|
|
float z = _boxScale.z;
|
|
float radius = 0.5f * sqrtf(0.5f * (x * x + z * z));
|
|
float halfHeight = 0.5f * _boxScale.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 (_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;
|
|
}
|
|
}
|
|
|
|
// it's ok to change offset immediately -- there are no thread safety issues here
|
|
_shapeLocalOffset = corner + 0.5f * _boxScale;
|
|
}
|
|
|
|
bool DynamicCharacterController::needsRemoval() const {
|
|
return (bool)(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION);
|
|
}
|
|
|
|
bool DynamicCharacterController::needsAddition() const {
|
|
return (bool)(_pendingFlags & PENDING_FLAG_ADD_TO_SIMULATION);
|
|
}
|
|
|
|
void DynamicCharacterController::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;
|
|
_isHovering = true;
|
|
} else {
|
|
if (_dynamicsWorld) {
|
|
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
|
|
}
|
|
_pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION;
|
|
_isOnGround = false;
|
|
}
|
|
_enabled = enabled;
|
|
}
|
|
}
|
|
|
|
void DynamicCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
|
|
if (_dynamicsWorld != world) {
|
|
if (_dynamicsWorld) {
|
|
if (_rigidBody) {
|
|
_dynamicsWorld->removeRigidBody(_rigidBody);
|
|
_dynamicsWorld->removeAction(this);
|
|
}
|
|
_dynamicsWorld = NULL;
|
|
}
|
|
if (world && _rigidBody) {
|
|
_dynamicsWorld = world;
|
|
_pendingFlags &= ~ PENDING_FLAG_JUMP;
|
|
_dynamicsWorld->addRigidBody(_rigidBody);
|
|
_dynamicsWorld->addAction(this);
|
|
//reset(_dynamicsWorld);
|
|
}
|
|
}
|
|
if (_dynamicsWorld) {
|
|
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
|
|
// shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
|
|
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION;
|
|
} else {
|
|
_pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
|
|
}
|
|
} else {
|
|
_pendingFlags &= ~ PENDING_FLAG_REMOVE_FROM_SIMULATION;
|
|
}
|
|
}
|
|
|
|
void DynamicCharacterController::updateShapeIfNecessary() {
|
|
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
|
|
// make sure there is NO pending removal from simulation at this point
|
|
// (don't want to delete _rigidBody out from under the simulation)
|
|
assert(!(_pendingFlags & PENDING_FLAG_REMOVE_FROM_SIMULATION));
|
|
_pendingFlags &= ~ PENDING_FLAG_UPDATE_SHAPE;
|
|
// delete shape and RigidBody
|
|
delete _rigidBody;
|
|
_rigidBody = NULL;
|
|
delete _shape;
|
|
_shape = NULL;
|
|
|
|
// 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 new shape
|
|
_shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
|
|
|
|
// create new body
|
|
float mass = 1.0f;
|
|
btVector3 inertia(1.0f, 1.0f, 1.0f);
|
|
_rigidBody = new btRigidBody(mass, NULL, _shape, inertia);
|
|
_rigidBody->setSleepingThresholds (0.0f, 0.0f);
|
|
_rigidBody->setAngularFactor (0.0f);
|
|
_rigidBody->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()),
|
|
glmToBullet(_avatarData->getPosition())));
|
|
//_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
|
|
} else {
|
|
// TODO: handle this failure case
|
|
}
|
|
}
|
|
}
|
|
|
|
void DynamicCharacterController::preSimulation(btScalar timeStep) {
|
|
if (_enabled && _dynamicsWorld) {
|
|
glm::quat rotation = _avatarData->getOrientation();
|
|
_currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
|
|
glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset;
|
|
|
|
// TODO: get intended WALK velocity from _avatarData, not its actual velocity
|
|
btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity());
|
|
|
|
_rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
|
|
_rigidBody->setLinearVelocity(walkVelocity);
|
|
if (_pendingFlags & PENDING_FLAG_JUMP) {
|
|
_pendingFlags &= ~ PENDING_FLAG_JUMP;
|
|
if (canJump()) {
|
|
// TODO: make jump work
|
|
//_verticalVelocity = _jumpSpeed;
|
|
_isJumping = true;
|
|
}
|
|
}
|
|
|
|
// the rotation is determined by AvatarData
|
|
btTransform xform = _rigidBody->getCenterOfMassTransform();
|
|
xform.setRotation(glmToBullet(rotation));
|
|
_rigidBody->setCenterOfMassTransform(xform);
|
|
}
|
|
}
|
|
|
|
void DynamicCharacterController::postSimulation() {
|
|
if (_enabled && _rigidBody) {
|
|
const btTransform& avatarTransform = _rigidBody->getCenterOfMassTransform();
|
|
glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
|
|
glm::vec3 position = bulletToGLM(avatarTransform.getOrigin());
|
|
|
|
_avatarData->setOrientation(rotation);
|
|
_avatarData->setPosition(position - rotation * _shapeLocalOffset);
|
|
}
|
|
}
|
|
|