Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-13 12:12:36 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #7711 from AndrewMeadows/faster-kinematics

Browse source 
faster hifi kinematic motion calculations take-4
This commit is contained in:
Brad Hefta-Gaub 2016-04-22 13:41:02 -07:00
commit 410bf3c95a
12 changed files with 247 additions and 173 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

173
libraries/entities/src/EntityItem.cpp
View file

@ -731,7 +731,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
// we want to extrapolate the motion forward to compensate for packet travel time, but
// we don't want the side effect of flag setting.
simulateKinematicMotion(skipTimeForward, false);
stepKinematicMotion(skipTimeForward);
}
if (overwriteLocalData) {
@ -872,130 +872,113 @@ void EntityItem::simulate(const quint64& now) {
qCDebug(entities) << " ********** EntityItem::simulate() .... SETTING _lastSimulated=" << _lastSimulated;
#endif
simulateKinematicMotion(timeElapsed);
if (!hasActions()) {
if (!stepKinematicMotion(timeElapsed)) {
// this entity is no longer moving
// flag it to transition from KINEMATIC to STATIC
_dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
setAcceleration(Vectors::ZERO);
}
}
_lastSimulated = now;
}
void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) {
#ifdef WANT_DEBUG
qCDebug(entities) << "EntityItem::simulateKinematicMotion timeElapsed" << timeElapsed;
#endif
const float MIN_TIME_SKIP = 0.0f;
const float MAX_TIME_SKIP = 1.0f; // in seconds
timeElapsed = glm::clamp(timeElapsed, MIN_TIME_SKIP, MAX_TIME_SKIP);
if (hasActions()) {
return;
bool EntityItem::stepKinematicMotion(float timeElapsed) {
if (timeElapsed <= 0.0f) {
return true;
}
if (hasLocalAngularVelocity()) {
glm::vec3 localAngularVelocity = getLocalAngularVelocity();
const float MAX_TIME_ELAPSED = 1.0f; // seconds
if (timeElapsed > MAX_TIME_ELAPSED) {
qCWarning(entities) << "kinematic timestep = " << timeElapsed << " truncated to " << MAX_TIME_ELAPSED;
}
timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED);
Transform transform;
glm::vec3 linearVelocity;
glm::vec3 angularVelocity;
getLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
bool moving = false;
if (glm::length2(angularVelocity) > 0.0f) {
// angular damping
if (_angularDamping > 0.0f) {
localAngularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
#ifdef WANT_DEBUG
qCDebug(entities) << " angularDamping :" << _angularDamping;
qCDebug(entities) << " newAngularVelocity:" << localAngularVelocity;
#endif
angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
}
float angularSpeed = glm::length(localAngularVelocity);
const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
if (setFlags && angularSpeed > 0.0f) {
_dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
}
localAngularVelocity = ENTITY_ITEM_ZERO_VEC3;
const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED =
KINEMATIC_ANGULAR_SPEED_THRESHOLD * KINEMATIC_ANGULAR_SPEED_THRESHOLD;
if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) {
angularVelocity = Vectors::ZERO;
} else {
// for improved agreement with the way Bullet integrates rotations we use an approximation
// and break the integration into bullet-sized substeps
glm::quat rotation = getRotation();
glm::quat rotation = transform.getRotation();
float dt = timeElapsed;
while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) {
glm::quat dQ = computeBulletRotationStep(localAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
while (dt > 0.0f) {
glm::quat dQ = computeBulletRotationStep(angularVelocity, glm::min(dt, PHYSICS_ENGINE_FIXED_SUBSTEP));
rotation = glm::normalize(dQ * rotation);
dt -= PHYSICS_ENGINE_FIXED_SUBSTEP;
}
// NOTE: this final partial substep can drift away from a real Bullet simulation however
// it only becomes significant for rapidly rotating objects
// (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec).
glm::quat dQ = computeBulletRotationStep(localAngularVelocity, dt);
rotation = glm::normalize(dQ * rotation);
bool success;
setOrientation(rotation, success, false);
transform.setRotation(rotation);
}
setLocalAngularVelocity(localAngularVelocity);
moving = true;
}
if (hasLocalVelocity()) {
// acceleration is in the global frame, so transform it into the local frame.
// TODO: Move this into SpatiallyNestable.
bool success;
Transform transform = getParentTransform(success);
glm::vec3 localAcceleration(glm::vec3::_null);
if (success) {
localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration();
} else {
localAcceleration = getAcceleration();
}
glm::vec3 position = transform.getTranslation();
float linearSpeedSquared = glm::length2(linearVelocity);
const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED =
KINEMATIC_LINEAR_SPEED_THRESHOLD * KINEMATIC_LINEAR_SPEED_THRESHOLD;
if (linearSpeedSquared > 0.0f) {
glm::vec3 deltaVelocity = Vectors::ZERO;
// linear damping
glm::vec3 localVelocity = getLocalVelocity();
if (_damping > 0.0f) {
localVelocity *= powf(1.0f - _damping, timeElapsed);
#ifdef WANT_DEBUG
qCDebug(entities) << " damping:" << _damping;
qCDebug(entities) << " velocity AFTER dampingResistance:" << localVelocity;
qCDebug(entities) << " glm::length(velocity):" << glm::length(localVelocity);
#endif
deltaVelocity = (powf(1.0f - _damping, timeElapsed) - 1.0f) * linearVelocity;
}
// integrate position forward
glm::vec3 localPosition = getLocalPosition();
glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed;
const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
if (glm::length2(_acceleration) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
// yes acceleration
// acceleration is in world-frame but we need it in local-frame
glm::vec3 linearAcceleration = _acceleration;
bool success;
Transform parentTransform = getParentTransform(success);
if (success) {
linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration;
}
deltaVelocity += linearAcceleration * timeElapsed;
#ifdef WANT_DEBUG
qCDebug(entities) << " EntityItem::simulate()....";
qCDebug(entities) << " timeElapsed:" << timeElapsed;
qCDebug(entities) << " old AACube:" << getMaximumAACube();
qCDebug(entities) << " old position:" << localPosition;
qCDebug(entities) << " old velocity:" << localVelocity;
qCDebug(entities) << " old getAABox:" << getAABox();
qCDebug(entities) << " newPosition:" << newPosition;
qCDebug(entities) << " glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition);
#endif
localPosition = newLocalPosition;
// apply effective acceleration, which will be the same as gravity if the Entity isn't at rest.
localVelocity += localAcceleration * timeElapsed;
float speed = glm::length(localVelocity);
const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec
if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) {
setVelocity(ENTITY_ITEM_ZERO_VEC3);
if (setFlags && speed > 0.0f) {
_dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
&& glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
&& glm::length2(linearVelocity + deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
linearVelocity = Vectors::ZERO;
} else {
// NOTE: we do NOT include the second-order acceleration term (0.5 * a * dt^2)
// when computing the displacement because Bullet also ignores that term. Yes,
// this is an approximation and it works best when dt is small.
position += timeElapsed * linearVelocity;
linearVelocity += deltaVelocity;
}
} else {
setLocalPosition(localPosition);
setLocalVelocity(localVelocity);
// no acceleration
if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
linearVelocity = Vectors::ZERO;
} else {
// NOTE: we don't use second-order acceleration term for linear displacement
// because Bullet doesn't use it.
position += timeElapsed * linearVelocity;
linearVelocity += deltaVelocity;
}
}
#ifdef WANT_DEBUG
qCDebug(entities) << " new position:" << position;
qCDebug(entities) << " new velocity:" << velocity;
qCDebug(entities) << " new AACube:" << getMaximumAACube();
qCDebug(entities) << " old getAABox:" << getAABox();
#endif
moving = true;
}
if (moving) {
transform.setTranslation(position);
setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
}
return moving;
}
bool EntityItem::isMoving() const {

2
libraries/entities/src/EntityItem.h
View file

@ -152,7 +152,7 @@ public:
// perform linear extrapolation for SimpleEntitySimulation
void simulate(const quint64& now);
void simulateKinematicMotion(float timeElapsed, bool setFlags=true);
bool stepKinematicMotion(float timeElapsed); // return 'true' if moving
virtual bool needsToCallUpdate() const { return false; }

129
libraries/physics/src/EntityMotionState.cpp
View file

@ -56,8 +56,8 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer
_serverGravity(0.0f),
_serverAcceleration(0.0f),
_serverActionData(QByteArray()),
_lastVelocity(glm::vec3(0.0f)),
_measuredAcceleration(glm::vec3(0.0f)),
_lastVelocity(0.0f),
_measuredAcceleration(0.0f),
_nextOwnershipBid(0),
_measuredDeltaTime(0.0f),
_lastMeasureStep(0),
@ -178,19 +178,26 @@ bool EntityMotionState::isMoving() const {
// (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 {
BT_PROFILE("getWorldTransform");
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
if (_motionType == MOTION_TYPE_KINEMATIC) {
BT_PROFILE("kinematicIntegration");
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation.
// of the physics simulation and uses full gravity for acceleration.
_entity->setAcceleration(_entity->getGravity());
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateKinematicMotion(dt);
_entity->stepKinematicMotion(dt);
// bypass const-ness so we can remember the step
const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
// and set the acceleration-matches-gravity count high so that if we send an update
// it will use the correct acceleration for remote simulations
_accelerationNearlyGravityCount = (uint8_t)(-1);
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
@ -270,6 +277,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
_serverPosition = bulletToGLM(xform.getOrigin());
_serverRotation = bulletToGLM(xform.getRotation());
_serverVelocity = getBodyLinearVelocityGTSigma();
_serverAcceleration = Vectors::ZERO;
_serverAngularVelocity = bulletToGLM(_body->getAngularVelocity());
_lastStep = simulationStep;
_serverActionData = _entity->getActionData();
@ -309,6 +317,8 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
if (glm::length2(_serverVelocity) > 0.0f) {
_serverVelocity += _serverAcceleration * dt;
_serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt);
// NOTE: we ignore the second-order acceleration term when integrating
// the position forward because Bullet also does this.
_serverPosition += dt * _serverVelocity;
}
@ -334,19 +344,23 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
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
const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm
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;
// we don't mind larger position error when the object has high speed
// so we divide by speed and check again
float speed2 = glm::length2(_serverVelocity);
const float MIN_ERROR_RATIO_SQUARED = 0.0025f; // corresponds to 5% error in 1 second
const float MIN_SPEED_SQUARED = 1.0e-6f; // corresponds to 1mm/sec
if (speed2 < MIN_SPEED_SQUARED || dx2 / speed2 > MIN_ERROR_RATIO_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) {
@ -400,7 +414,11 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) {
if (_entity->getSimulatorID() != Physics::getSessionUUID()) {
// we don't own the simulation
bool shouldBid = _outgoingPriority > 0 && // but we would like to own it and
// NOTE: we do not volunteer to own kinematic or static objects
uint8_t insufficientPriority = _body->isStaticOrKinematicObject() ? VOLUNTEER_SIMULATION_PRIORITY : 0;
bool shouldBid = _outgoingPriority > insufficientPriority && // but we would like to own it AND
usecTimestampNow() > _nextOwnershipBid; // it is time to bid again
if (shouldBid && _outgoingPriority < _entity->getSimulationPriority()) {
// we are insufficiently interested so clear our interest
@ -420,51 +438,39 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
if (!_body->isActive()) {
// make sure all derivatives are zero
glm::vec3 zero(0.0f);
_entity->setVelocity(zero);
_entity->setAngularVelocity(zero);
_entity->setAcceleration(zero);
_entity->setVelocity(Vectors::ZERO);
_entity->setAngularVelocity(Vectors::ZERO);
_entity->setAcceleration(Vectors::ZERO);
_numInactiveUpdates++;
} else {
const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
float gravityLength = glm::length(_entity->getGravity());
float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength);
const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
// acceleration measured during the most recent simulation step was close to gravity.
if (getAccelerationNearlyGravityCount() < STEPS_TO_DECIDE_BALLISTIC) {
// only increment this if we haven't reached the threshold yet. this is to avoid
// overflowing the counter.
incrementAccelerationNearlyGravityCount();
}
} else {
// acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
resetAccelerationNearlyGravityCount();
}
glm::vec3 gravity = _entity->getGravity();
// if this entity has been accelerated at close to gravity for a certain number of simulation-steps, let
// the entity server's estimates include gravity.
if (getAccelerationNearlyGravityCount() >= STEPS_TO_DECIDE_BALLISTIC) {
_entity->setAcceleration(_entity->getGravity());
const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {
_entity->setAcceleration(gravity);
} else {
_entity->setAcceleration(glm::vec3(0.0f));
_entity->setAcceleration(Vectors::ZERO);
}
const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec
const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec
if (!_body->isStaticOrKinematicObject()) {
const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec
const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec
bool movingSlowlyLinear =
glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
(DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == glm::vec3(0.0f);
bool movingSlowlyLinear =
glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
(DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == Vectors::ZERO;
if (movingSlowly) {
// velocities might not be zero, but we'll fake them as such, which will hopefully help convince
// other simulating observers to deactivate their own copies
glm::vec3 zero(0.0f);
_entity->setVelocity(zero);
_entity->setAngularVelocity(zero);
if (movingSlowly) {
// velocities might not be zero, but we'll fake them as such, which will hopefully help convince
// other simulating observers to deactivate their own copies
glm::vec3 zero(0.0f);
_entity->setVelocity(zero);
_entity->setAngularVelocity(zero);
}
}
_numInactiveUpdates = 0;
}
@ -612,9 +618,9 @@ void EntityMotionState::resetMeasuredBodyAcceleration() {
if (_body) {
_lastVelocity = getBodyLinearVelocityGTSigma();
} else {
_lastVelocity = glm::vec3(0.0f);
_lastVelocity = Vectors::ZERO;
}
_measuredAcceleration = glm::vec3(0.0f);
_measuredAcceleration = Vectors::ZERO;
}
void EntityMotionState::measureBodyAcceleration() {
@ -638,8 +644,23 @@ void EntityMotionState::measureBodyAcceleration() {
_lastStep = ObjectMotionState::getWorldSimulationStep();
_numInactiveUpdates = 0;
}
glm::vec3 gravity = _entity->getGravity();
float gravityLength = glm::length(gravity);
float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength);
const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
// acceleration measured during the most recent simulation step was close to gravity.
if (_accelerationNearlyGravityCount < (uint8_t)(-2)) {
++_accelerationNearlyGravityCount;
}
} else {
// acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
_accelerationNearlyGravityCount = 0;
}
}
}
glm::vec3 EntityMotionState::getObjectLinearVelocityChange() const {
// This is the dampened change in linear velocity, as calculated in measureBodyAcceleration: dv = a * dt
// It is generally only meaningful during the lifespan of collision. In particular, it is not meaningful

6
libraries/physics/src/EntityMotionState.h
View file

@ -51,10 +51,6 @@ public:
virtual uint32_t getIncomingDirtyFlags() override;
virtual void clearIncomingDirtyFlags() override;
void incrementAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount++; }
void resetAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount = 0; }
uint8_t getAccelerationNearlyGravityCount() { return _accelerationNearlyGravityCount; }
virtual float getObjectRestitution() const override { return _entity->getRestitution(); }
virtual float getObjectFriction() const override { return _entity->getFriction(); }
virtual float getObjectLinearDamping() const override { return _entity->getDamping(); }
@ -123,7 +119,7 @@ protected:
uint32_t _lastStep; // last step of server extrapolation
uint8_t _loopsWithoutOwner;
uint8_t _accelerationNearlyGravityCount;
mutable uint8_t _accelerationNearlyGravityCount;
uint8_t _numInactiveUpdates { 1 };
uint8_t _outgoingPriority { 0 };
};

7
libraries/physics/src/ObjectMotionState.cpp
View file

@ -45,6 +45,7 @@ void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
worldSimulationStep = step;
}
// static
uint32_t ObjectMotionState::getWorldSimulationStep() {
return worldSimulationStep;
}
@ -298,6 +299,12 @@ void ObjectMotionState::updateBodyVelocities() {
_body->setActivationState(ACTIVE_TAG);
}
void ObjectMotionState::updateLastKinematicStep() {
// NOTE: we init to worldSimulationStep - 1 so that: when any object transitions to kinematic
// it will compute a non-zero dt on its first step.
_lastKinematicStep = ObjectMotionState::getWorldSimulationStep() - 1;
}
void ObjectMotionState::updateBodyMassProperties() {
float mass = getMass();
btVector3 inertia(0.0f, 0.0f, 0.0f);

2
libraries/physics/src/ObjectMotionState.h
View file

@ -86,6 +86,8 @@ public:
void updateBodyMaterialProperties();
void updateBodyVelocities();
void updateLastKinematicStep();
virtual void updateBodyMassProperties();
MotionStateType getType() const { return _type; }

10
libraries/physics/src/PhysicsEngine.cpp
View file

@ -86,9 +86,8 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
body->updateInertiaTensor();
motionState->updateBodyVelocities();
const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f; // 1 cm/sec
const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f; // ~1 deg/sec
body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);
motionState->updateLastKinematicStep();
body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD);
break;
}
case MOTION_TYPE_DYNAMIC: {
@ -109,9 +108,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
// NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds.
// (the 2 seconds is determined by: static btRigidBody::gDeactivationTime
const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec
const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec
body->setSleepingThresholds(DYNAMIC_LINEAR_VELOCITY_THRESHOLD, DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD);
if (!motionState->isMoving()) {
// try to initialize this object as inactive
body->forceActivationState(ISLAND_SLEEPING);
@ -252,6 +249,7 @@ void PhysicsEngine::stepSimulation() {
float timeStep = btMin(dt, MAX_TIMESTEP);
if (_myAvatarController) {
BT_PROFILE("avatarController");
// TODO: move this stuff outside and in front of stepSimulation, because
// the updateShapeIfNecessary() call needs info from MyAvatar and should
// be done on the main thread during the pre-simulation stuff

26
libraries/physics/src/ThreadSafeDynamicsWorld.cpp
View file

@ -67,7 +67,10 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
saveKinematicState(fixedTimeStep*clampedSimulationSteps);
applyGravity();
{
BT_PROFILE("applyGravity");
applyGravity();
}
for (int i=0;i<clampedSimulationSteps;i++) {
internalSingleStepSimulation(fixedTimeStep);
@ -143,3 +146,24 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
}
}
void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
///to switch status _after_ adding kinematic objects to the world
///fix it for Bullet 3.x release
BT_PROFILE("saveKinematicState");
for (int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body && body->getActivationState() != ISLAND_SLEEPING)
{
if (body->isKinematicObject())
{
//to calculate velocities next frame
body->saveKinematicState(timeStep);
}
}
}
}

1
libraries/physics/src/ThreadSafeDynamicsWorld.h
View file

@ -41,6 +41,7 @@ public:
btScalar fixedTimeStep = btScalar(1.)/btScalar(60.),
SubStepCallback onSubStep = []() { });
virtual void synchronizeMotionStates() override;
virtual void saveKinematicState(btScalar timeStep) override;
// btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated
// but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide

5
libraries/shared/src/PhysicsHelpers.h
View file

@ -22,6 +22,11 @@
const int PHYSICS_ENGINE_MAX_NUM_SUBSTEPS = 6; // Bullet will start to "lose time" at 10 FPS.
const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 90.0f;
const float DYNAMIC_LINEAR_SPEED_THRESHOLD = 0.05f; // 5 cm/sec
const float DYNAMIC_ANGULAR_SPEED_THRESHOLD = 0.087266f; // ~5 deg/sec
const float KINEMATIC_LINEAR_SPEED_THRESHOLD = 0.001f; // 1 mm/sec
const float KINEMATIC_ANGULAR_SPEED_THRESHOLD = 0.008f; // ~0.5 deg/sec
// return incremental rotation (Bullet-style) caused by angularVelocity over timeStep
glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep);

52
libraries/shared/src/SpatiallyNestable.cpp
View file

@ -90,11 +90,9 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
return parent;
}
SpatiallyNestablePointer thisPointer = getThisPointer();
if (parent) {
// we have a parent pointer but our _parentID doesn't indicate this parent.
parent->forgetChild(thisPointer);
parent->forgetChild(getThisPointer());
_parentKnowsMe = false;
_parent.reset();
}
@ -112,16 +110,11 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
parent = _parent.lock();
if (parent) {
parent->beParentOfChild(thisPointer);
parent->beParentOfChild(getThisPointer());
_parentKnowsMe = true;
}
if (parent || parentID.isNull()) {
success = true;
} else {
success = false;
}
success = (parent || parentID.isNull());
return parent;
}
@ -426,7 +419,7 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
// _velocity is a vs parent value and any request for a world-frame velocity must
// be computed), do this to avoid equipped (parenting-grabbed) things from drifting.
// turning a zero velocity into a non-zero _velocity (because the avatar is moving)
// causes EntityItem::simulateKinematicMotion to have an effect on the equipped entity,
// causes EntityItem::stepKinematicMotion to have an effect on the equipped entity,
// which causes it to drift from the hand.
if (hasAncestorOfType(NestableType::Avatar)) {
_velocity = velocity;
@ -873,3 +866,40 @@ bool SpatiallyNestable::hasAncestorOfType(NestableType nestableType) {
return parent->hasAncestorOfType(nestableType);
}
void SpatiallyNestable::getLocalTransformAndVelocities(
Transform& transform,
glm::vec3& velocity,
glm::vec3& angularVelocity) const {
// transform
_transformLock.withReadLock([&] {
transform = _transform;
});
// linear velocity
_velocityLock.withReadLock([&] {
velocity = _velocity;
});
// angular velocity
_angularVelocityLock.withReadLock([&] {
angularVelocity = _angularVelocity;
});
}
void SpatiallyNestable::setLocalTransformAndVelocities(
const Transform& localTransform,
const glm::vec3& localVelocity,
const glm::vec3& localAngularVelocity) {
// transform
_transformLock.withWriteLock([&] {
_transform = localTransform;
});
// linear velocity
_velocityLock.withWriteLock([&] {
_velocity = localVelocity;
});
// angular velocity
_angularVelocityLock.withWriteLock([&] {
_angularVelocity = localAngularVelocity;
});
locationChanged(false);
}

7
libraries/shared/src/SpatiallyNestable.h
View file

@ -151,6 +151,13 @@ protected:
quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to?
SpatiallyNestablePointer getParentPointer(bool& success) const;
void getLocalTransformAndVelocities(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const;
void setLocalTransformAndVelocities(
const Transform& localTransform,
const glm::vec3& localVelocity,
const glm::vec3& localAngularVelocity);
mutable SpatiallyNestableWeakPointer _parent;
virtual void beParentOfChild(SpatiallyNestablePointer newChild) const;