////  EntityMotionState.cpp//  libraries/entities/src////  Created by Andrew Meadows on 2014.11.06//  Copyright 2013 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "EntityMotionState.h" #include <glm/gtx/norm.hpp> #include <EntityItem.h>#include <EntityItemProperties.h>#include <EntityEditPacketSender.h>#include <LogHandler.h>#include <PhysicsCollisionGroups.h>#include <Profile.h> #include "BulletUtil.h"#include "PhysicsEngine.h"#include "PhysicsHelpers.h"#include "PhysicsLogging.h" const uint8_t LOOPS_FOR_SIMULATION_ORPHAN = 50;const quint64 USECS_BETWEEN_OWNERSHIP_BIDS = USECS_PER_SECOND / 5;  EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer entity) :    ObjectMotionState(nullptr),    _entity(entity),    _serverPosition(0.0f),    _serverRotation(),    _serverVelocity(0.0f),    _serverAngularVelocity(0.0f),    _serverGravity(0.0f),    _serverAcceleration(0.0f),    _serverActionData(QByteArray()),    _lastVelocity(0.0f),    _measuredAcceleration(0.0f),    _nextBidExpiry(0),    _measuredDeltaTime(0.0f),    _lastMeasureStep(0),    _lastStep(0),    _loopsWithoutOwner(0),    _accelerationNearlyGravityCount(0),    _numInactiveUpdates(1){    // Why is _numInactiveUpdates initialied to 1?    // Because: when an entity is first created by a LOCAL operatioin its local simulation ownership is assumed,    // which causes it to be immediately placed on the 'owned' list, but in this case an "update" already just    // went out for the object's creation and there is no need to send another.  By initializing _numInactiveUpdates    // to 1 here we trick remoteSimulationOutOfSync() to return "false" the first time through for this case.     _type = MOTIONSTATE_TYPE_ENTITY;    assert(_entity);    // we need the side-effects of EntityMotionState::setShape() so we call it explicitly here    // rather than pass the legit shape pointer to the ObjectMotionState ctor above.    setShape(shape);    setMass(_entity->computeMass());     if (_entity->isAvatarEntity() && !_entity->isMyAvatarEntity()) {        // avatar entities are always thus, so we cache this fact in _ownershipState        _ownershipState = EntityMotionState::OwnershipState::Unownable;    }     Transform localTransform;    _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity);    _serverPosition = localTransform.getTranslation();    _serverRotation = localTransform.getRotation();    _serverAcceleration = _entity->getAcceleration();    _serverActionData = _entity->getDynamicData();} EntityMotionState::~EntityMotionState() {    if (_entity) {        assert(_entity->getPhysicsInfo() == this);        _entity->setPhysicsInfo(nullptr);        _entity.reset();    }} void EntityMotionState::updateServerPhysicsVariables() {    // only slam these values if we are NOT the simulation owner    Transform localTransform;    _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity);    _serverPosition = localTransform.getTranslation();    _serverRotation = localTransform.getRotation();    _serverAcceleration = _entity->getAcceleration();    _serverActionData = _entity->getDynamicData();    _lastStep = ObjectMotionState::getWorldSimulationStep();} void EntityMotionState::handleDeactivation() {    if (_entity->getDirtyFlags() & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES)) {        // Some non-physical event (script-call or network-packet) has modified the entity's transform and/or        // velocities at the last minute before deactivation --> the values stored in _server* and _body are stale.        // We assume the EntityMotionState is the last to know, so we copy from EntityItem to _server* variables        // here but don't clear the flags --> the will body be set straight before next simulation step.        updateServerPhysicsVariables();    } else if (_body->isStaticOrKinematicObject() && _ownershipState != EntityMotionState::OwnershipState::LocallyOwned) {            // To allow the ESS to move entities around in a kinematic way we had to remove the requirement that            // every moving+simulated entity has an authoritative simulation owner.  As a result, we cannot rely            // on a final authoritative update of kinmatic objects prior to deactivation in the local simulation.            // For this case (unowned kinematic objects) we update the _server* variables for good measure but            // leave the entity and body alone. They should have been updated correctly in the last call to            // EntityMotionState::getWorldTransform().            updateServerPhysicsVariables();    } else {        // copy _server data to entity        Transform localTransform = _entity->getLocalTransform();        localTransform.setTranslation(_serverPosition);        localTransform.setRotation(_serverRotation);        _entity->setLocalTransformAndVelocities(localTransform, ENTITY_ITEM_ZERO_VEC3, ENTITY_ITEM_ZERO_VEC3);        // and also to RigidBody        btTransform worldTrans;        worldTrans.setOrigin(glmToBullet(_entity->getWorldPosition()));        worldTrans.setRotation(glmToBullet(_entity->getWorldOrientation()));        _body->setWorldTransform(worldTrans);        // no need to update velocities... should already be zero    }    if (!isLocallyOwned()) {        // HACK: To allow the ESS to move entities around in a kinematic way we had to remove the requirement that        // every moving+simulated entity has an authoritative simulation owner.  As a result, we cannot rely        // on a simulation owner to update the QueryAACube on the entity-server.        _entity->updateQueryAACube();    }} // virtualvoid EntityMotionState::handleEasyChanges(uint32_t& flags) {    if (_ownershipState != EntityMotionState::OwnershipState::LocallyOwned) {        updateServerPhysicsVariables();    }    ObjectMotionState::handleEasyChanges(flags);     if (flags & Simulation::DIRTY_SIMULATOR_ID) {        if (_entity->getSimulatorID().isNull()) {            // simulation ownership has been removed            if (glm::length2(_entity->getWorldVelocity()) == 0.0f) {            // TODO: also check angularVelocity                // this object is coming to rest                flags &= ~Simulation::DIRTY_PHYSICS_ACTIVATION;                _body->setActivationState(WANTS_DEACTIVATION);                const float ACTIVATION_EXPIRY = 3.0f; // something larger than the 2.0 hard coded in Bullet                _body->setDeactivationTime(ACTIVATION_EXPIRY);            } else {                // disowned object is still moving --> start timer for ownership bid                // TODO? put a delay in here proportional to distance from object?                _bumpedPriority = glm::max(_bumpedPriority, VOLUNTEER_SIMULATION_PRIORITY);                _nextBidExpiry = usecTimestampNow() + USECS_BETWEEN_OWNERSHIP_BIDS;            }            _loopsWithoutOwner = 0;            _numInactiveUpdates = 0;        } else if (!isLocallyOwned()) {            // the entity is owned by someone else            _nextBidExpiry = usecTimestampNow() + USECS_BETWEEN_OWNERSHIP_BIDS;            _numInactiveUpdates = 0;        }    }    if (flags & Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY) {        // The DIRTY_SIMULATOR_OWNERSHIP_PRIORITY bit means one of the following:        // (1) we own it but may need to change the priority OR...        // (2) we don't own it but should bid (because a local script has been changing physics properties)        // reset bid expiry so that we bid ASAP        _nextBidExpiry = 0;    }    if ((flags & Simulation::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) {        if (_body->isKinematicObject()) {            // only force activate kinematic bodies (dynamic shouldn't need force and            // active static bodies are special (see PhysicsEngine::_activeStaticBodies))            _body->activate(true);            _lastKinematicStep = ObjectMotionState::getWorldSimulationStep();        } else {            _body->activate();        }    }     if (flags & Simulation::DIRTY_MASS) {        setMass(_entity->computeMass());        updateBodyMassProperties();    }}  PhysicsMotionType EntityMotionState::computePhysicsMotionType() const {    if (!_entity) {        return MOTION_TYPE_STATIC;    }     if (_entity->getLocked()) {        if (_entity->isMoving()) {            return MOTION_TYPE_KINEMATIC;        }        return MOTION_TYPE_STATIC;    }     if (_entity->getDynamic()) {        if (!_entity->getParentID().isNull()) {            // if something would have been dynamic but is a child of something else, force it to be kinematic, instead.            return MOTION_TYPE_KINEMATIC;        }        return MOTION_TYPE_DYNAMIC;    }    if (_entity->hasActions() ||        _entity->isMovingRelativeToParent() ||        _entity->hasAncestorOfType(NestableType::Avatar)) {        return MOTION_TYPE_KINEMATIC;    }    return MOTION_TYPE_STATIC;} bool EntityMotionState::isMoving() const {    return _entity && _entity->isMovingRelativeToParent();} // This callback is invoked by the physics simulation in two cases:// (1) when the RigidBody is first added to the world//     (irregardless of PhysicsMotionType: 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 positionvoid EntityMotionState::getWorldTransform(btTransform& worldTrans) const {    BT_PROFILE("getWorldTransform");    if (!_entity) {        return;    }    if (_motionType == MOTION_TYPE_KINEMATIC) {        BT_PROFILE("kinematicIntegration");        uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();        if (hasInternalKinematicChanges()) {            // ACTION_CAN_CONTROL_KINEMATIC_OBJECT_HACK: This kinematic body was moved by an Action            // and doesn't require transform update because the body is authoritative and its transform            // has already been copied out --> do no kinematic integration.            clearInternalKinematicChanges();            _lastKinematicStep = thisStep;            return;        }        // This is physical kinematic motion which steps strictly by the subframe count        // of the physics simulation and uses full gravity for acceleration.        _entity->setAcceleration(_entity->getGravity());         float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;        _lastKinematicStep = thisStep;        _entity->stepKinematicMotion(dt);         // 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->getWorldOrientation()));} // 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) {    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);        }    }}  const uint8_t MAX_NUM_INACTIVE_UPDATES = 20; bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {    // NOTE: this method is only ever called when the entity simulation is locally owned    DETAILED_PROFILE_RANGE(simulation_physics, "CheckOutOfSync");     // Since we own the simulation: make sure _bidPriority is not less than current owned priority    // because: a _bidPriority of zero indicates that we should drop ownership in the send.    // TODO: need to be able to detect when logic dictates we *decrease* priority    // WIP: print info whenever _bidPriority mismatches what is known to the entity     int numSteps = simulationStep - _lastStep;    float dt = (float)(numSteps) * PHYSICS_ENGINE_FIXED_SUBSTEP;     if (_numInactiveUpdates > 0) {        if (_numInactiveUpdates > MAX_NUM_INACTIVE_UPDATES) {            // clear local ownership (stop sending updates) and let the server clear itself            _entity->clearSimulationOwnership();            return false;        }        // we resend the inactive update with a growing delay: every INACTIVE_UPDATE_PERIOD * _numInactiveUpdates        // until it is removed from the owned list        // (which happens when we no longer own the simulation)        const float INACTIVE_UPDATE_PERIOD = 0.5f;        return (dt > INACTIVE_UPDATE_PERIOD * (float)_numInactiveUpdates);    }     if (!_body->isActive()) {        // object has gone inactive but our last send was moving --> send non-moving update immediately        return true;    }     if (usecTimestampNow() > _entity->getSimulationOwnershipExpiry()) {        // send update every so often else server will revoke our ownership        return true;    }     if (_body->isStaticOrKinematicObject()) {        return false;    }     _lastStep = simulationStep;    if (glm::length2(_serverVelocity) > 0.0f) {        // the entity-server doesn't know where avatars are, so it doesn't do simple extrapolation for children of        // avatars.  We are trying to guess what values the entity server has, so we don't do it here, either.  See        // related code in EntitySimulation::moveSimpleKinematics.        bool ancestryIsKnown;        _entity->getMaximumAACube(ancestryIsKnown);        bool hasAvatarAncestor = _entity->hasAncestorOfType(NestableType::Avatar);         if (ancestryIsKnown && !hasAvatarAncestor) {            _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;        }    }     // 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    bool parentTransformSuccess;    Transform localToWorld = _entity->getParentTransform(parentTransformSuccess);    Transform worldToLocal;    if (parentTransformSuccess) {        localToWorld.evalInverse(worldToLocal);    }     btTransform worldTrans = _body->getWorldTransform();    glm::vec3 position = worldToLocal.transform(bulletToGLM(worldTrans.getOrigin()));     float dx2 = glm::distance2(position, _serverPosition);    const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm    if (dx2 > MAX_POSITION_ERROR_SQUARED) {        // 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) {            return true;        }    }     if (glm::length2(_serverAngularVelocity) > 0.0f) {        // compute rotation error        //         // 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.        float attenuation = powf(1.0f - _body->getAngularDamping(), PHYSICS_ENGINE_FIXED_SUBSTEP);        _serverAngularVelocity *= attenuation;        glm::quat rotation = computeBulletRotationStep(_serverAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);        for (int i = 1; i < numSteps; ++i) {            _serverAngularVelocity *= attenuation;            rotation = computeBulletRotationStep(_serverAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * rotation;        }        _serverRotation = glm::normalize(rotation * _serverRotation);        const float MIN_ROTATION_DOT = 0.99999f; // This corresponds to about 0.5 degrees of rotation        glm::quat actualRotation = worldToLocal.getRotation() * bulletToGLM(worldTrans.getRotation());        return (fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT);    }    return false;} bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) {    // NOTE: this method is only ever called when the entity simulation is locally owned    DETAILED_PROFILE_RANGE(simulation_physics, "ShouldSend");    // NOTE: we expect _entity and _body to be valid in this context, since shouldSendUpdate() is only called    // after doesNotNeedToSendUpdate() returns false and that call should return 'true' if _entity or _body are NULL.     // this case is prevented by setting _ownershipState to OwnershipState::Unownable in EntityMotionState::ctor    assert(!(_entity->isAvatarEntity() && !_entity->isMyAvatarEntity()));     if (_entity->getTransitingWithAvatar()) {        return false;    }    if (_entity->dynamicDataNeedsTransmit()) {        return true;    }    if (_entity->shouldSuppressLocationEdits()) {        // "shouldSuppressLocationEdits" really means: "the entity has a 'Hold' action therefore        // we don't need send an update unless the entity is not contained by its queryAACube"        return _entity->queryAACubeNeedsUpdate();    }     return remoteSimulationOutOfSync(simulationStep);} void EntityMotionState::updateSendVelocities() {    if (!_body->isActive()) {        if (!_body->isKinematicObject()) {            clearObjectVelocities();        }        if (_entity->getEntityHostType() == entity::HostType::AVATAR) {            // AvatarEntities only ever need to send one update (their updates are sent over a lossless protocol)            // so we set the count to the max to prevent resends            _numInactiveUpdates = MAX_NUM_INACTIVE_UPDATES;        } else {            ++_numInactiveUpdates;        }    } else {        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.        const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;        if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {            _entity->setAcceleration(gravity);        } else {            _entity->setAcceleration(Vectors::ZERO);        }         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->getWorldVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);            bool movingSlowlyAngular = glm::length2(_entity->getWorldAngularVelocity()) <                    (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                clearObjectVelocities();            }        }        _numInactiveUpdates = 0;    }} void EntityMotionState::sendBid(OctreeEditPacketSender* packetSender, uint32_t step) {    DETAILED_PROFILE_RANGE(simulation_physics, "Bid");     updateSendVelocities();     EntityItemProperties properties;    Transform localTransform;    glm::vec3 linearVelocity;    glm::vec3 angularVelocity;    _entity->getLocalTransformAndVelocities(localTransform, linearVelocity, angularVelocity);    properties.setPosition(localTransform.getTranslation());    properties.setRotation(localTransform.getRotation());    properties.setVelocity(linearVelocity);    properties.setAcceleration(_entity->getAcceleration());    properties.setAngularVelocity(angularVelocity);     // we don't own the simulation for this entity yet, but we're sending a bid for it    quint64 now = usecTimestampNow();    uint8_t finalBidPriority = computeFinalBidPriority();    _entity->prepareForSimulationOwnershipBid(properties, now, finalBidPriority);     EntityTreeElementPointer element = _entity->getElement();    EntityTreePointer tree = element ? element->getTree() : nullptr; 

Lines 1–500 of 839. Load more