// This callback is invoked by the physics simulation at the end of each simulation step...
// iff the corresponding RigidBody is DYNAMIC and has moved.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
measureBodyAcceleration();
_entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset());
_entity->setRotation(bulletToGLM(worldTrans.getRotation()));
_entity->setVelocity(getBodyLinearVelocity());
_entity->setAngularVelocity(getBodyAngularVelocity());
_entity->setLastSimulated(usecTimestampNow());
if (_entity->getSimulatorID().isNull()) {
_loopsWithoutOwner++;
if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextOwnershipBid) {
//qDebug() << "Warning -- claiming something I saw moving." << getName();
setOutgoingPriority(VOLUNTEER_SIMULATION_PRIORITY);
}
}
#ifdef WANT_DEBUG
quint64 now = usecTimestampNow();
qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID();
qCDebug(physics) << " last edited:" << _entity->getLastEdited()
<< formatUsecTime(now - _entity->getLastEdited()) << "ago";
qCDebug(physics) << " last simulated:" << _entity->getLastSimulated()
<< formatUsecTime(now - _entity->getLastSimulated()) << "ago";
qCDebug(physics) << " last updated:" << _entity->getLastUpdated()
<< formatUsecTime(now - _entity->getLastUpdated()) << "ago";
#endif
}
void MyCharacterController::getAvatarPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const {
if (_enabled && _rigidBody) {
const btTransform& avatarTransform = _rigidBody->getWorldTransform();
rotation = bulletToGLM(avatarTransform.getRotation());
position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset;
}
}
// This callback is invoked by the physics simulation at the end of each simulation step...
// iff the corresponding RigidBody is DYNAMIC and has moved.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
if (!_entity) {
return;
}
measureBodyAcceleration();
_entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset());
_entity->setRotation(bulletToGLM(worldTrans.getRotation()));
_entity->setVelocity(getBodyLinearVelocity());
_entity->setAngularVelocity(getBodyAngularVelocity());
_entity->setLastSimulated(usecTimestampNow());
if (_entity->getSimulatorID().isNull()) {
_loopsWithoutOwner++;
const uint32_t OWNERSHIP_BID_DELAY = 50;
if (_loopsWithoutOwner > OWNERSHIP_BID_DELAY) {
//qDebug() << "Warning -- claiming something I saw moving." << getName();
_candidateForOwnership = true;
}
} else {
_loopsWithoutOwner = 0;
}
#ifdef WANT_DEBUG
quint64 now = usecTimestampNow();
qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID();
qCDebug(physics) << " last edited:" << _entity->getLastEdited() << formatUsecTime(now - _entity->getLastEdited()) << "ago";
qCDebug(physics) << " last simulated:" << _entity->getLastSimulated() << formatUsecTime(now - _entity->getLastSimulated()) << "ago";
qCDebug(physics) << " last updated:" << _entity->getLastUpdated() << formatUsecTime(now - _entity->getLastUpdated()) << "ago";
#endif
}
bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) {
if (!_rigidBody) {
return false;
}
const btTransform& worldTrans = _rigidBody->getCenterOfMassTransform();
avatarRigidBodyPosition = bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset();
avatarRigidBodyRotation = bulletToGLM(worldTrans.getRotation());
return true;
}
void DynamicCharacterController::postSimulation() {
if (_enabled && _rigidBody) {
const btTransform& avatarTransform = _rigidBody->getWorldTransform();
glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
glm::vec3 position = bulletToGLM(avatarTransform.getOrigin());
_avatarData->nextAttitude(position - rotation * _shapeLocalOffset, rotation);
_avatarData->setVelocity(bulletToGLM(_rigidBody->getLinearVelocity()));
}
}
// This callback is invoked by the physics simulation at the end of each simulation frame...
// iff the corresponding RigidBody is DYNAMIC and has moved.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
_entity->setPositionInMeters(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset());
_entity->setRotation(bulletToGLM(worldTrans.getRotation()));
glm::vec3 v;
getVelocity(v);
_entity->setVelocityInMeters(v);
getAngularVelocity(v);
// DANGER! EntityItem stores angularVelocity in degrees/sec!!!
_entity->setAngularVelocity(glm::degrees(v));
_outgoingPacketFlags = DIRTY_PHYSICS_FLAGS;
EntityMotionState::enqueueOutgoingEntity(_entity);
}
glm::vec3 ObjectAction::getAngularVelocity() {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return glm::vec3(0.0f);
}
return bulletToGLM(rigidBody->getAngularVelocity());
}
glm::quat ObjectAction::getRotation() {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return glm::quat(0.0f, 0.0f, 0.0f, 1.0f);
}
return bulletToGLM(rigidBody->getOrientation());
}
glm::vec3 ObjectAction::getPosition() {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return glm::vec3(0.0f);
}
return bulletToGLM(rigidBody->getCenterOfMassPosition());
}
glm::vec3 CharacterController::getFollowVelocity() const {
if (_followTime > 0.0f) {
return bulletToGLM(_followLinearDisplacement) / _followTime;
} else {
return glm::vec3();
}
}
glm::vec3 MyCharacterController::getLinearVelocity() const {
glm::vec3 velocity(0.0f);
if (_rigidBody) {
velocity = bulletToGLM(_rigidBody->getLinearVelocity());
}
return velocity;
}
CollisionEvents& PhysicsEngine::getCollisionEvents() {
const uint32_t CONTINUE_EVENT_FILTER_FREQUENCY = 10;
_collisionEvents.clear();
// scan known contacts and trigger events
ContactMap::iterator contactItr = _contactMap.begin();
while (contactItr != _contactMap.end()) {
ContactInfo& contact = contactItr->second;
ContactEventType type = contact.computeType(_numContactFrames);
if(type != CONTACT_EVENT_TYPE_CONTINUE || _numSubsteps % CONTINUE_EVENT_FILTER_FREQUENCY == 0) {
ObjectMotionState* motionStateA = static_cast<ObjectMotionState*>(contactItr->first._a);
ObjectMotionState* motionStateB = static_cast<ObjectMotionState*>(contactItr->first._b);
glm::vec3 velocityChange = (motionStateA ? motionStateA->getObjectLinearVelocityChange() : glm::vec3(0.0f)) +
(motionStateB ? motionStateB->getObjectLinearVelocityChange() : glm::vec3(0.0f));
if (motionStateA && motionStateA->getType() == MOTIONSTATE_TYPE_ENTITY) {
QUuid idA = motionStateA->getObjectID();
QUuid idB;
if (motionStateB && motionStateB->getType() == MOTIONSTATE_TYPE_ENTITY) {
idB = motionStateB->getObjectID();
}
glm::vec3 position = bulletToGLM(contact.getPositionWorldOnB()) + _originOffset;
glm::vec3 penetration = bulletToGLM(contact.distance * contact.normalWorldOnB);
_collisionEvents.push_back(Collision(type, idA, idB, position, penetration, velocityChange));
} else if (motionStateB && motionStateB->getType() == MOTIONSTATE_TYPE_ENTITY) {
QUuid idB = motionStateB->getObjectID();
glm::vec3 position = bulletToGLM(contact.getPositionWorldOnA()) + _originOffset;
// NOTE: we're flipping the order of A and B (so that the first objectID is never NULL)
// hence we must negate the penetration.
glm::vec3 penetration = - bulletToGLM(contact.distance * contact.normalWorldOnB);
_collisionEvents.push_back(Collision(type, idB, QUuid(), position, penetration, velocityChange));
}
}
if (type == CONTACT_EVENT_TYPE_END) {
ContactMap::iterator iterToDelete = contactItr;
++contactItr;
_contactMap.erase(iterToDelete);
} else {
++contactItr;
}
}
return _collisionEvents;
}
// This callback is invoked by the physics simulation at the end of each simulation step...
// iff the corresponding RigidBody is DYNAMIC and has moved.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
measureBodyAcceleration();
bool positionSuccess;
_entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false);
if (!positionSuccess) {
static QString repeatedMessage =
LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform "
"setPosition failed.*");
qCDebug(physics) << "EntityMotionState::setWorldTransform setPosition failed" << _entity->getID();
}
bool orientationSuccess;
_entity->setOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false);
if (!orientationSuccess) {
static QString repeatedMessage =
LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform "
"setOrientation failed.*");
qCDebug(physics) << "EntityMotionState::setWorldTransform setOrientation failed" << _entity->getID();
}
_entity->setVelocity(getBodyLinearVelocity());
_entity->setAngularVelocity(getBodyAngularVelocity());
_entity->setLastSimulated(usecTimestampNow());
if (_entity->getSimulatorID().isNull()) {
_loopsWithoutOwner++;
if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextOwnershipBid) {
upgradeOutgoingPriority(VOLUNTEER_SIMULATION_PRIORITY);
}
}
#ifdef WANT_DEBUG
quint64 now = usecTimestampNow();
qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID();
qCDebug(physics) << " last edited:" << _entity->getLastEdited()
<< formatUsecTime(now - _entity->getLastEdited()) << "ago";
qCDebug(physics) << " last simulated:" << _entity->getLastSimulated()
<< formatUsecTime(now - _entity->getLastSimulated()) << "ago";
qCDebug(physics) << " last updated:" << _entity->getLastUpdated()
<< formatUsecTime(now - _entity->getLastUpdated()) << "ago";
#endif
}
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 EntityMotionState::resetMeasuredBodyAcceleration() {
_lastMeasureStep = ObjectMotionState::getWorldSimulationStep();
if (_body) {
_lastVelocity = bulletToGLM(_body->getLinearVelocity());
} else {
_lastVelocity = glm::vec3(0.0f);
}
_measuredAcceleration = glm::vec3(0.0f);
}
void BulletUtilTests::fromBulletToGLM() {
btVector3 bV(1.23f, 4.56f, 7.89f);
glm::vec3 gV = bulletToGLM(bV);
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;
}
glm::vec3 ObjectMotionState::getBodyLinearVelocity() const {
// returns the body's velocity unless it is moving too slow in which case returns zero
btVector3 velocity = _body->getLinearVelocity();
// NOTE: the threshold to use here relates to the linear displacement threshold (dX) for sending updates
// to objects that are tracked server-side (e.g. entities which use dX = 2mm). Hence an object moving
// just under this velocity threshold would trigger an update about V/dX times per second.
const float MIN_LINEAR_SPEED_SQUARED = 0.0036f; // 6 mm/sec
if (velocity.length2() < MIN_LINEAR_SPEED_SQUARED) {
velocity *= 0.0f;
}
return bulletToGLM(velocity);
}
// This callback is invoked by the physics simulation at the end of each simulation step...
// iff the corresponding RigidBody is DYNAMIC and ACTIVE.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
assert(entityTreeIsLocked());
measureBodyAcceleration();
// If transform or velocities are flagged as dirty it means a network or scripted change
// occured between the beginning and end of the stepSimulation() and we DON'T want to apply
// these physics simulation results.
uint32_t flags = _entity->getDirtyFlags() & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES);
if (!flags) {
// flags are clear
_entity->setWorldTransform(bulletToGLM(worldTrans.getOrigin()), bulletToGLM(worldTrans.getRotation()));
_entity->setWorldVelocity(getBodyLinearVelocity());
_entity->setWorldAngularVelocity(getBodyAngularVelocity());
_entity->setLastSimulated(usecTimestampNow());
} else {
// only set properties NOT flagged
if (!(flags & Simulation::DIRTY_TRANSFORM)) {
_entity->setWorldTransform(bulletToGLM(worldTrans.getOrigin()), bulletToGLM(worldTrans.getRotation()));
}
if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) {
_entity->setWorldVelocity(getBodyLinearVelocity());
}
if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) {
_entity->setWorldAngularVelocity(getBodyAngularVelocity());
}
if (flags != (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES)) {
_entity->setLastSimulated(usecTimestampNow());
}
}
if (_entity->getSimulatorID().isNull()) {
_loopsWithoutOwner++;
if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextBidExpiry) {
_bumpedPriority = glm::max(_bumpedPriority, VOLUNTEER_SIMULATION_PRIORITY);
}
}
}
void EntityMotionState::measureBodyAcceleration() {
// try to manually measure the true acceleration of the object
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
uint32_t numSubsteps = thisStep - _lastMeasureStep;
if (numSubsteps > 0) {
float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP);
float invDt = 1.0f / dt;
_lastMeasureStep = thisStep;
// Note: the integration equation for velocity uses damping: v1 = (v0 + a * dt) * (1 - D)^dt
// hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt
glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity());
_measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt;
_lastVelocity = velocity;
}
}
bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
assert(_body);
// if we've never checked before, our _lastStep will be 0, and we need to initialize our state
if (_lastStep == 0) {
btTransform xform = _body->getWorldTransform();
_serverPosition = bulletToGLM(xform.getOrigin());
_serverRotation = bulletToGLM(xform.getRotation());
_serverVelocity = getBodyLinearVelocityGTSigma();
_serverAngularVelocity = bulletToGLM(_body->getAngularVelocity());
_lastStep = simulationStep;
_serverActionData = _entity->getActionData();
_sentInactive = true;
return false;
}
#ifdef WANT_DEBUG
glm::vec3 wasPosition = _serverPosition;
glm::quat wasRotation = _serverRotation;
glm::vec3 wasAngularVelocity = _serverAngularVelocity;
#endif
int numSteps = simulationStep - _lastStep;
float dt = (float)(numSteps) * PHYSICS_ENGINE_FIXED_SUBSTEP;
const float INACTIVE_UPDATE_PERIOD = 0.5f;
if (_sentInactive) {
// we resend the inactive update every INACTIVE_UPDATE_PERIOD
// until it is removed from the outgoing updates
// (which happens when we don't own the simulation and it isn't touching our simulation)
return (dt > INACTIVE_UPDATE_PERIOD);
}
bool isActive = _body->isActive();
if (!isActive) {
// object has gone inactive but our last send was moving --> send non-moving update immediately
return true;
}
_lastStep = simulationStep;
if (glm::length2(_serverVelocity) > 0.0f) {
_serverVelocity += _serverAcceleration * dt;
_serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt);
_serverPosition += dt * _serverVelocity;
}
if (_entity->actionDataNeedsTransmit()) {
setOutgoingPriority(SCRIPT_EDIT_SIMULATION_PRIORITY);
return true;
}
if (_entity->shouldSuppressLocationEdits()) {
return false;
}
// Else we measure the error between current and extrapolated transform (according to expected behavior
// of remote EntitySimulation) and return true if the error is significant.
// NOTE: math is done in the simulation-frame, which is NOT necessarily the same as the world-frame
// due to _worldOffset.
// TODO: compensate for _worldOffset offset here
// compute position error
btTransform worldTrans = _body->getWorldTransform();
glm::vec3 position = bulletToGLM(worldTrans.getOrigin());
float dx2 = glm::distance2(position, _serverPosition);
const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // Sqrt() - corresponds to 2 millimeters
if (dx2 > MAX_POSITION_ERROR_SQUARED) {
#ifdef WANT_DEBUG
qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ....";
qCDebug(physics) << "wasPosition:" << wasPosition;
qCDebug(physics) << "bullet position:" << position;
qCDebug(physics) << "_serverPosition:" << _serverPosition;
qCDebug(physics) << "dx2:" << dx2;
#endif
return true;
}
if (glm::length2(_serverAngularVelocity) > 0.0f) {
// compute rotation error
float attenuation = powf(1.0f - _body->getAngularDamping(), dt);
_serverAngularVelocity *= attenuation;
// Bullet caps the effective rotation velocity inside its rotation integration step, therefore
// we must integrate with the same algorithm and timestep in order achieve similar results.
for (int i = 0; i < numSteps; ++i) {
_serverRotation = glm::normalize(computeBulletRotationStep(_serverAngularVelocity,
PHYSICS_ENGINE_FIXED_SUBSTEP) * _serverRotation);
}
}
const float MIN_ROTATION_DOT = 0.99999f; // This corresponds to about 0.5 degrees of rotation
glm::quat actualRotation = bulletToGLM(worldTrans.getRotation());
#ifdef WANT_DEBUG
if ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) {
qCDebug(physics) << ".... ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) ....";
qCDebug(physics) << "wasAngularVelocity:" << wasAngularVelocity;
qCDebug(physics) << "_serverAngularVelocity:" << _serverAngularVelocity;
qCDebug(physics) << "length wasAngularVelocity:" << glm::length(wasAngularVelocity);
qCDebug(physics) << "length _serverAngularVelocity:" << glm::length(_serverAngularVelocity);
qCDebug(physics) << "wasRotation:" << wasRotation;
qCDebug(physics) << "bullet actualRotation:" << actualRotation;
qCDebug(physics) << "_serverRotation:" << _serverRotation;
}
#endif
return (fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT);
}
glm::quat CharacterController::getFollowAngularDisplacement() const {
return bulletToGLM(_followAngularDisplacement);
}
void ObjectMotionState::getVelocity(glm::vec3& velocityOut) const {
velocityOut = bulletToGLM(_body->getLinearVelocity());
}
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::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
btVector3 btVelocity = glmToBullet(velocity);
computeNewVelocity(dt, btVelocity);
velocity = bulletToGLM(btVelocity);
}
glm::vec3 CharacterController::getVelocityChange() const {
if (_rigidBody) {
return bulletToGLM(_velocityChange);
}
return glm::vec3(0.0f);
}
const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info) {
btCollisionShape* shape = NULL;
int type = info.getType();
switch(type) {
case SHAPE_TYPE_BOX: {
shape = new btBoxShape(glmToBullet(info.getHalfExtents()));
}
break;
case SHAPE_TYPE_SPHERE: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = glm::max(halfExtents.x, glm::max(halfExtents.y, halfExtents.z));
shape = new btSphereShape(radius);
}
break;
case SHAPE_TYPE_ELLIPSOID: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = halfExtents.x;
const float MIN_RADIUS = 0.001f;
const float MIN_RELATIVE_SPHERICAL_ERROR = 0.001f;
if (radius > MIN_RADIUS
&& fabsf(radius - halfExtents.y) / radius < MIN_RELATIVE_SPHERICAL_ERROR
&& fabsf(radius - halfExtents.z) / radius < MIN_RELATIVE_SPHERICAL_ERROR) {
// close enough to true sphere
shape = new btSphereShape(radius);
} else {
ShapeInfo::PointList points;
points.reserve(NUM_UNIT_SPHERE_DIRECTIONS);
for (uint32_t i = 0; i < NUM_UNIT_SPHERE_DIRECTIONS; ++i) {
points.push_back(bulletToGLM(_unitSphereDirections[i]) * halfExtents);
}
shape = createConvexHull(points);
}
}
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:
case SHAPE_TYPE_SIMPLE_HULL: {
const ShapeInfo::PointCollection& pointCollection = info.getPointCollection();
uint32_t numSubShapes = info.getNumSubShapes();
if (numSubShapes == 1) {
shape = createConvexHull(pointCollection[0]);
} else {
auto compound = new btCompoundShape();
btTransform trans;
trans.setIdentity();
foreach (const ShapeInfo::PointList& hullPoints, pointCollection) {
btConvexHullShape* hull = createConvexHull(hullPoints);
compound->addChildShape(trans, hull);
}
shape = compound;
}
}
break;
case SHAPE_TYPE_SIMPLE_COMPOUND: {
const ShapeInfo::PointCollection& pointCollection = info.getPointCollection();
const ShapeInfo::TriangleIndices& triangleIndices = info.getTriangleIndices();
uint32_t numIndices = triangleIndices.size();
uint32_t numMeshes = info.getNumSubShapes();
const uint32_t MIN_NUM_SIMPLE_COMPOUND_INDICES = 2; // END_OF_MESH_PART + END_OF_MESH
if (numMeshes > 0 && numIndices > MIN_NUM_SIMPLE_COMPOUND_INDICES) {
uint32_t i = 0;
std::vector<btConvexHullShape*> hulls;
for (auto& points : pointCollection) {
// build a hull around each part
while (i < numIndices) {
ShapeInfo::PointList hullPoints;
hullPoints.reserve(points.size());
while (i < numIndices) {
int32_t j = triangleIndices[i];
++i;
if (j == END_OF_MESH_PART) {
// end of part
break;
}
hullPoints.push_back(points[j]);
}
if (hullPoints.size() > 0) {
btConvexHullShape* hull = createConvexHull(hullPoints);
hulls.push_back(hull);
}
assert(i < numIndices);
if (triangleIndices[i] == END_OF_MESH) {
// end of mesh
++i;
break;
}
}
}
uint32_t numHulls = (uint32_t)hulls.size();
if (numHulls == 1) {
shape = hulls[0];
} else {
auto compound = new btCompoundShape();
btTransform trans;
trans.setIdentity();
for (auto hull : hulls) {
compound->addChildShape(trans, hull);
}
shape = compound;
}
}
}
break;
case SHAPE_TYPE_STATIC_MESH: {
btTriangleIndexVertexArray* dataArray = createStaticMeshArray(info);
if (dataArray) {
shape = new StaticMeshShape(dataArray);
}
}
break;
}
glm::vec3 CharacterController::getFollowLinearDisplacement() const {
return bulletToGLM(_followLinearDisplacement);
}
void ObjectMotionState::getAngularVelocity(glm::vec3& angularVelocityOut) const {
angularVelocityOut = bulletToGLM(_body->getAngularVelocity());
}
bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
assert(_body);
// if we've never checked before, our _sentFrame will be 0, and we need to initialize our state
if (_sentFrame == 0) {
_sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin());
_sentVelocity = bulletToGLM(_body->getLinearVelocity());
_sentAngularVelocity = bulletToGLM(_body->getAngularVelocity());
_sentFrame = simulationFrame;
return false;
}
float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_sentFrame = simulationFrame;
bool isActive = _body->isActive();
if (!isActive) {
if (_sentMoving) {
// this object just went inactive so send an update immediately
return true;
} else {
const float NON_MOVING_UPDATE_PERIOD = 1.0f;
if (dt > NON_MOVING_UPDATE_PERIOD && _numNonMovingUpdates < MAX_NUM_NON_MOVING_UPDATES) {
// RELIABLE_SEND_HACK: since we're not yet using a reliable method for non-moving update packets we repeat these
// at a faster rate than the MAX period above, and only send a limited number of them.
return true;
}
}
}
// Else we measure the error between current and extrapolated transform (according to expected behavior
// of remote EntitySimulation) and return true if the error is significant.
// NOTE: math in done the simulation-frame, which is NOT necessarily the same as the world-frame
// due to _worldOffset.
// compute position error
if (glm::length2(_sentVelocity) > 0.0f) {
_sentVelocity += _sentAcceleration * dt;
_sentVelocity *= powf(1.0f - _linearDamping, dt);
_sentPosition += dt * _sentVelocity;
}
btTransform worldTrans = _body->getWorldTransform();
glm::vec3 position = bulletToGLM(worldTrans.getOrigin());
float dx2 = glm::distance2(position, _sentPosition);
const float MAX_POSITION_ERROR_SQUARED = 0.001f; // 0.001 m^2 ~~> 0.03 m
if (dx2 > MAX_POSITION_ERROR_SQUARED) {
return true;
}
if (glm::length2(_sentAngularVelocity) > 0.0f) {
// compute rotation error
_sentAngularVelocity *= powf(1.0f - _angularDamping, dt);
float spin = glm::length(_sentAngularVelocity);
const float MIN_SPIN = 1.0e-4f;
if (spin > MIN_SPIN) {
glm::vec3 axis = _sentAngularVelocity / spin;
_sentRotation = glm::normalize(glm::angleAxis(dt * spin, axis) * _sentRotation);
}
}
const float MIN_ROTATION_DOT = 0.98f;
glm::quat actualRotation = bulletToGLM(worldTrans.getRotation());
return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
}
