// virtual
void AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(getObjectRotation()));
if (_body) {
_body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
}
}
void MyCharacterController::setAvatarPositionAndOrientation(
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);
_avatarBodyTransform = btTransform(bodyOrientation, bodyPosition);
}
void DynamicCharacterController::preSimulation(btScalar timeStep) {
if (_enabled && _dynamicsWorld) {
glm::quat rotation = _avatarData->getOrientation();
// TODO: update gravity if up has changed
updateUpAxis(rotation);
glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset;
_rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
// the rotation is dictated by AvatarData
btTransform xform = _rigidBody->getWorldTransform();
xform.setRotation(glmToBullet(rotation));
_rigidBody->setWorldTransform(xform);
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp;
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
if (rayCallback.hasHit()) {
_floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
const btScalar MIN_HOVER_HEIGHT = 3.0f;
if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) {
setHovering(false);
}
// TODO: use collision events rather than ray-trace test to disable jumping
const btScalar JUMP_PROXIMITY_THRESHOLD = 0.1f * _radius;
if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) {
_isJumping = false;
}
} else {
_floorDistance = FLT_MAX;
setHovering(true);
}
_walkVelocity = glmToBullet(_avatarData->getTargetVelocity());
if (_pendingFlags & PENDING_FLAG_JUMP) {
_pendingFlags &= ~ PENDING_FLAG_JUMP;
if (onGround()) {
_isJumping = true;
btVector3 velocity = _rigidBody->getLinearVelocity();
velocity += _jumpSpeed * _currentUp;
_rigidBody->setLinearVelocity(velocity);
}
}
}
}
btTypedConstraint* ObjectConstraintBallSocket::getConstraint() {
btPoint2PointConstraint* constraint { nullptr };
QUuid otherEntityID;
glm::vec3 pivotInA;
glm::vec3 pivotInB;
withReadLock([&]{
constraint = static_cast<btPoint2PointConstraint*>(_constraint);
pivotInA = _pivotInA;
otherEntityID = _otherID;
pivotInB = _pivotInB;
});
if (constraint) {
return constraint;
}
static QString repeatedBallSocketNoRigidBody = LogHandler::getInstance().addRepeatedMessageRegex(
"ObjectConstraintBallSocket::getConstraint -- no rigidBody.*");
btRigidBody* rigidBodyA = getRigidBody();
if (!rigidBodyA) {
qCDebug(physics) << "ObjectConstraintBallSocket::getConstraint -- no rigidBodyA";
return nullptr;
}
if (!otherEntityID.isNull()) {
// This constraint is between two entities... find the other rigid body.
btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
if (!rigidBodyB) {
qCDebug(physics) << "ObjectConstraintBallSocket::getConstraint -- no rigidBodyB";
return nullptr;
}
constraint = new btPoint2PointConstraint(*rigidBodyA, *rigidBodyB, glmToBullet(pivotInA), glmToBullet(pivotInB));
} else {
// This constraint is between an entity and the world-frame.
constraint = new btPoint2PointConstraint(*rigidBodyA, glmToBullet(pivotInA));
}
withWriteLock([&]{
_constraint = constraint;
});
// if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
forceBodyNonStatic();
activateBody();
updateBallSocket();
return constraint;
}
void ObjectConstraintBallSocket::updateBallSocket() {
btPoint2PointConstraint* constraint { nullptr };
withReadLock([&]{
constraint = static_cast<btPoint2PointConstraint*>(_constraint);
});
if (!constraint) {
return;
}
constraint->setPivotA(glmToBullet(_pivotInA));
constraint->setPivotB(glmToBullet(_pivotInB));
}
CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const glm::quat& rot, float horizTimescale, float vertTimescale) {
velocity = glmToBullet(vel);
rotation = glmToBullet(rot);
hTimescale = horizTimescale;
if (hTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) {
hTimescale = MIN_CHARACTER_MOTOR_TIMESCALE;
}
vTimescale = vertTimescale;
if (vTimescale < 0.0f) {
vTimescale = hTimescale;
} else if (vTimescale < MIN_CHARACTER_MOTOR_TIMESCALE) {
vTimescale = MIN_CHARACTER_MOTOR_TIMESCALE;
}
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation frame for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
if (_isKinematic) {
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation.
uint32_t substep = PhysicsEngine::getNumSubsteps();
float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateKinematicMotion(dt);
_entity->setLastSimulated(usecTimestampNow());
// bypass const-ness so we can remember the substep
const_cast<EntityMotionState*>(this)->_lastKinematicSubstep = substep;
}
worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}
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) {
// HACK: use some simple mass property defaults for now
float mass = 100.0f;
btVector3 inertia(30.0f, 8.0f, 30.0f);
// create RigidBody if it doesn't exist
if (!_rigidBody) {
btCollisionShape* shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
_rigidBody = new btRigidBody(mass, nullptr, shape, inertia);
} else {
btCollisionShape* shape = _rigidBody->getCollisionShape();
if (shape) {
delete shape;
}
shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
_rigidBody->setCollisionShape(shape);
}
_rigidBody->setSleepingThresholds(0.0f, 0.0f);
_rigidBody->setAngularFactor(0.0f);
_rigidBody->setWorldTransform(btTransform(glmToBullet(_avatar->getOrientation()),
glmToBullet(_avatar->getPosition())));
if (_isHovering) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp);
}
//_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
} else {
// TODO: handle this failure case
}
}
}
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 = nullptr;
delete _shape;
_shape = nullptr;
// 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);
// HACK: use some simple mass property defaults for now
float mass = 100.0f;
btVector3 inertia(30.0f, 8.0f, 30.0f);
// create new body
_rigidBody = new btRigidBody(mass, nullptr, _shape, inertia);
_rigidBody->setSleepingThresholds(0.0f, 0.0f);
_rigidBody->setAngularFactor(0.0f);
_rigidBody->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()),
glmToBullet(_avatarData->getPosition())));
if (_isHovering) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp);
}
//_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
} else {
// TODO: handle this failure case
}
}
}
btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
btCollisionShape* shape = NULL;
switch(info.getType()) {
case SHAPE_TYPE_BOX: {
shape = new btBoxShape(glmToBullet(info.getHalfExtents()));
}
break;
case SHAPE_TYPE_SPHERE: {
float radius = info.getHalfExtents().x;
shape = new btSphereShape(radius);
}
break;
case SHAPE_TYPE_CAPSULE_Y: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = halfExtents.x;
float height = 2.0f * halfExtents.y;
shape = new btCapsuleShape(radius, height);
}
break;
case SHAPE_TYPE_COMPOUND: {
const QVector<QVector<glm::vec3>>& points = info.getPoints();
uint32_t numSubShapes = info.getNumSubShapes();
if (numSubShapes == 1) {
auto hull = new btConvexHullShape();
const QVector<QVector<glm::vec3>>& points = info.getPoints();
foreach (glm::vec3 point, points[0]) {
btVector3 btPoint(point[0], point[1], point[2]);
hull->addPoint(btPoint, false);
}
hull->recalcLocalAabb();
shape = hull;
} else {
void ObjectAction::setAngularVelocity(glm::vec3 angularVelocity) {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return;
}
rigidBody->setAngularVelocity(glmToBullet(angularVelocity));
rigidBody->activate();
}
void ObjectAction::setLinearVelocity(glm::vec3 linearVelocity) {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return;
}
rigidBody->setLinearVelocity(glmToBullet(glm::vec3(0.0f)));
rigidBody->activate();
}
// virtual
void AvatarMotionState::setWorldTransform(const btTransform& worldTrans) {
const float SPRING_TIMESCALE = 0.5f;
float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE;
btVector3 currentPosition = worldTrans.getOrigin();
btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition;
float distance = offsetToTarget.length();
if ((1.0f - tau) * distance > _diameter) {
// the avatar body is far from its target --> slam position
btTransform newTransform;
newTransform.setOrigin(currentPosition + offsetToTarget);
newTransform.setRotation(glmToBullet(getObjectRotation()));
_body->setWorldTransform(newTransform);
_body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
} else {
// the avatar body is near its target --> slam velocity
btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget;
_body->setLinearVelocity(velocity);
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
// slam its rotation
btTransform newTransform = worldTrans;
newTransform.setRotation(glmToBullet(getObjectRotation()));
_body->setWorldTransform(newTransform);
}
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
if (_motionType == MOTION_TYPE_KINEMATIC) {
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation.
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateKinematicMotion(dt);
// bypass const-ness so we can remember the step
const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}
void MyCharacterController::updateUpAxis(const glm::quat& rotation) {
btVector3 oldUp = _currentUp;
_currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
if (!_isHovering) {
const btScalar MIN_UP_ERROR = 0.01f;
if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
_rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp);
}
}
}
void EntityMotionState::saveKinematicState(btScalar timeStep) {
_body->saveKinematicState(timeStep);
// This is a WORKAROUND for a quirk in Bullet: due to floating point error slow spinning kinematic objects will
// have a measured angular velocity of zero. This probably isn't a bug that the Bullet team is interested in
// fixing since there is one very simple workaround: use double-precision math for the physics simulation.
// We're not ready migrate to double-precision yet so we explicitly work around it by slamming the RigidBody's
// angular velocity with the value in the entity.
_body->setAngularVelocity(glmToBullet(_entity->getWorldAngularVelocity()));
}
void ObjectMotionState::handleEasyChanges(uint32_t flags, PhysicsEngine* engine) {
if (flags & EntityItem::DIRTY_POSITION) {
btTransform worldTrans;
if (flags & EntityItem::DIRTY_ROTATION) {
worldTrans.setRotation(glmToBullet(getObjectRotation()));
} else {
worldTrans = _body->getWorldTransform();
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
_body->setWorldTransform(worldTrans);
} else if (flags & EntityItem::DIRTY_ROTATION) {
btTransform worldTrans = _body->getWorldTransform();
worldTrans.setRotation(glmToBullet(getObjectRotation()));
_body->setWorldTransform(worldTrans);
}
if (flags & EntityItem::DIRTY_LINEAR_VELOCITY) {
_body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
_body->setGravity(glmToBullet(getObjectGravity()));
}
if (flags & EntityItem::DIRTY_ANGULAR_VELOCITY) {
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
}
if (flags & EntityItem::DIRTY_MATERIAL) {
updateBodyMaterialProperties();
}
if (flags & EntityItem::DIRTY_MASS) {
updateBodyMassProperties();
}
}
void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning entity";
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning physics info";
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
if (!rigidBody) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no rigidBody";
return;
}
withWriteLock([&] {
if (_kinematicSetVelocity) {
if (_previousSet) {
glm::vec3 positionalVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;
rigidBody->setLinearVelocity(glmToBullet(positionalVelocity));
}
}
btTransform worldTrans = rigidBody->getWorldTransform();
worldTrans.setOrigin(glmToBullet(_positionalTarget));
worldTrans.setRotation(glmToBullet(_rotationalTarget));
rigidBody->setWorldTransform(worldTrans);
motionState->dirtyInternalKinematicChanges();
_previousPositionalTarget = _positionalTarget;
_previousRotationalTarget = _rotationalTarget;
_previousSet = true;
});
activateBody();
}
void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) {
if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) {
if (flags & EntityItem::DIRTY_POSITION) {
_sentPosition = _entity->getPositionInMeters() - ObjectMotionState::getWorldOffset();
btTransform worldTrans;
worldTrans.setOrigin(glmToBullet(_sentPosition));
_sentRotation = _entity->getRotation();
worldTrans.setRotation(glmToBullet(_sentRotation));
_body->setWorldTransform(worldTrans);
}
if (flags & EntityItem::DIRTY_VELOCITY) {
updateObjectVelocities();
}
_sentFrame = frame;
}
// TODO: entity support for friction and restitution
//_restitution = _entity->getRestitution();
_body->setRestitution(_restitution);
//_friction = _entity->getFriction();
_body->setFriction(_friction);
_linearDamping = _entity->getDamping();
_angularDamping = _entity->getAngularDamping();
_body->setDamping(_linearDamping, _angularDamping);
if (flags & EntityItem::DIRTY_MASS) {
float mass = _entity->computeMass();
btVector3 inertia(0.0f, 0.0f, 0.0f);
_body->getCollisionShape()->calculateLocalInertia(mass, inertia);
_body->setMassProps(mass, inertia);
_body->updateInertiaTensor();
}
_body->activate();
};
void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
withTryReadLock([&] {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState->getRigidBody();
if (!rigidBody) {
qDebug() << "ObjectActionOffset::updateActionWorker no rigidBody";
return;
}
const float MAX_LINEAR_TIMESCALE = 600.0f; // 10 minutes is a long time
if (_positionalTargetSet && _linearTimeScale < MAX_LINEAR_TIMESCALE) {
glm::vec3 objectPosition = bulletToGLM(rigidBody->getCenterOfMassPosition());
glm::vec3 springAxis = objectPosition - _pointToOffsetFrom; // from anchor to object
float distance = glm::length(springAxis);
if (distance > FLT_EPSILON) {
springAxis /= distance; // normalize springAxis
// compute (critically damped) target velocity of spring relaxation
glm::vec3 offset = (distance - _linearDistance) * springAxis;
glm::vec3 targetVelocity = (-1.0f / _linearTimeScale) * offset;
// compute current velocity and its parallel component
glm::vec3 currentVelocity = bulletToGLM(rigidBody->getLinearVelocity());
glm::vec3 parallelVelocity = glm::dot(currentVelocity, springAxis) * springAxis;
// we blend the parallel component with the spring's target velocity to get the new velocity
float blend = deltaTimeStep / _linearTimeScale;
if (blend > 1.0f) {
blend = 1.0f;
}
rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity)));
}
}
});
}
void BulletUtilTests::fromGLMToBullet() {
glm::vec3 gV(1.23f, 4.56f, 7.89f);
btVector3 bV = glmToBullet(gV);
QCOMPARE(gV.x, bV.getX());
QCOMPARE(gV.y, bV.getY());
QCOMPARE(gV.z, bV.getZ());
float angle = 0.317f * PI;
btVector3 axis(1.23f, 2.34f, 3.45f);
axis.normalize();
btQuaternion bQ(axis, angle);
glm::quat gQ = bulletToGLM(bQ);
QCOMPARE(gQ.x, bQ.getX());
QCOMPARE(gQ.y, bQ.getY());
QCOMPARE(gQ.z, bQ.getZ());
QCOMPARE(gQ.w, bQ.getW());
}
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().
const float MINIMUM_TIME_REMAINING = 0.005f;
const float MAX_DISPLACEMENT = 0.5f * _radius;
_followTimeRemaining -= dt;
if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) {
btTransform bodyTransform = _rigidBody->getWorldTransform();
btVector3 startPos = bodyTransform.getOrigin();
btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
btVector3 vel = deltaPos / _followTimeRemaining;
btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling.
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 = deltaAngle / _followTimeRemaining;
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;
}
void ObjectMotionState::handleEasyChanges(uint32_t flags) {
if (flags & EntityItem::DIRTY_POSITION) {
btTransform worldTrans;
if (flags & EntityItem::DIRTY_ROTATION) {
worldTrans.setRotation(glmToBullet(getObjectRotation()));
} else {
worldTrans = _body->getWorldTransform();
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
_body->setWorldTransform(worldTrans);
} else if (flags & EntityItem::DIRTY_ROTATION) {
btTransform worldTrans = _body->getWorldTransform();
worldTrans.setRotation(glmToBullet(getObjectRotation()));
_body->setWorldTransform(worldTrans);
}
if (flags & EntityItem::DIRTY_LINEAR_VELOCITY) {
_body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
_body->setGravity(glmToBullet(getObjectGravity()));
}
if (flags & EntityItem::DIRTY_ANGULAR_VELOCITY) {
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
}
if (flags & EntityItem::DIRTY_MATERIAL) {
updateBodyMaterialProperties();
}
if (flags & EntityItem::DIRTY_MASS) {
float mass = getMass();
btVector3 inertia(0.0f, 0.0f, 0.0f);
_body->getCollisionShape()->calculateLocalInertia(mass, inertia);
_body->setMassProps(mass, inertia);
_body->updateInertiaTensor();
}
}
void MyCharacterController::setHMDVelocity(const glm::vec3& velocity) {
_hmdVelocity = glmToBullet(velocity);
}
void MyCharacterController::setTargetVelocity(const glm::vec3& velocity) {
//_walkVelocity = glmToBullet(_avatarData->getTargetVelocity());
_walkVelocity = glmToBullet(velocity);
}
void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
if (!tryLockForRead()) {
// don't risk hanging the thread running the physics simulation
qDebug() << "ObjectActionSpring::updateActionWorker lock failed";
return;
}
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
unlock();
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState->getRigidBody();
if (!rigidBody) {
unlock();
qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";
return;
}
const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
if (_linearTimeScale < MAX_TIMESCALE) {
btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
float offsetLength = offset.length();
float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setLinearVelocity((- speed / offsetLength) * offset);
}
if (_angularTimeScale < MAX_TIMESCALE) {
btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
btQuaternion bodyRotation = rigidBody->getOrientation();
auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
const float ALMOST_ONE = 0.99999f;
if (glm::abs(alignmentDot) < ALMOST_ONE) {
btQuaternion target = glmToBullet(_rotationalTarget);
if (alignmentDot < 0.0f) {
target = -target;
}
// if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
//
// Q1 = dQ * Q0
//
// solving for dQ gives:
//
// dQ = Q1 * Q0^
btQuaternion deltaQ = target * bodyRotation.inverse();
float angle = deltaQ.getAngle();
const float MIN_ANGLE = 1.0e-4f;
if (angle > MIN_ANGLE) {
targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
}
}
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setAngularVelocity(targetVelocity);
}
unlock();
}
void CharacterController::setParentVelocity(const glm::vec3& velocity) {
_parentVelocity = glmToBullet(velocity);
}
void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
btVector3 btVelocity = glmToBullet(velocity);
computeNewVelocity(dt, btVelocity);
velocity = bulletToGLM(btVelocity);
}
void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
const btScalar MIN_SPEED = 0.001f;
btVector3 actualVelocity = _rigidBody->getLinearVelocity();
if (actualVelocity.length() < MIN_SPEED) {
actualVelocity = btVector3(0.0f, 0.0f, 0.0f);
}
btVector3 desiredVelocity = _walkVelocity;
if (desiredVelocity.length() < MIN_SPEED) {
desiredVelocity = btVector3(0.0f, 0.0f, 0.0f);
}
// decompose into horizontal and vertical components.
btVector3 actualVertVelocity = actualVelocity.dot(_currentUp) * _currentUp;
btVector3 actualHorizVelocity = actualVelocity - actualVertVelocity;
btVector3 desiredVertVelocity = desiredVelocity.dot(_currentUp) * _currentUp;
btVector3 desiredHorizVelocity = desiredVelocity - desiredVertVelocity;
btVector3 finalVelocity;
switch (_state) {
case State::Ground:
case State::Takeoff:
{
// horizontal ground control
const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f;
btScalar tau = dt / WALK_ACCELERATION_TIMESCALE;
finalVelocity = tau * desiredHorizVelocity + (1.0f - tau) * actualHorizVelocity + actualVertVelocity;
}
break;
case State::InAir:
{
// horizontal air control
const btScalar IN_AIR_ACCELERATION_TIMESCALE = 2.0f;
btScalar tau = dt / IN_AIR_ACCELERATION_TIMESCALE;
finalVelocity = tau * desiredHorizVelocity + (1.0f - tau) * actualHorizVelocity + actualVertVelocity;
}
break;
case State::Hover:
{
// vertical and horizontal air control
const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f;
btScalar tau = dt / FLY_ACCELERATION_TIMESCALE;
finalVelocity = tau * desiredVelocity + (1.0f - tau) * actualVelocity;
}
break;
}
_rigidBody->setLinearVelocity(finalVelocity);
// Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
// Rather then 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;
_followTimeRemaining -= dt;
if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) {
btTransform bodyTransform = _rigidBody->getWorldTransform();
btVector3 startPos = bodyTransform.getOrigin();
btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
btVector3 vel = deltaPos / _followTimeRemaining;
btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling.
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 = deltaAngle / _followTimeRemaining;
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;
}
void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) {
_followTimeRemaining = timeRemaining;
_followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset));
}
