From 8ddcae2a157ed4e74720b47d9570905bb4eb8f01 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 9 Mar 2017 17:25:22 -0800 Subject: [PATCH] restore transform of deactivated entities --- interface/src/Application.cpp | 5 ++- libraries/entities/src/EntityItem.cpp | 2 +- libraries/physics/src/EntityMotionState.cpp | 39 +++++++++++++++---- libraries/physics/src/EntityMotionState.h | 1 + .../physics/src/PhysicalEntitySimulation.cpp | 13 +++++++ .../physics/src/PhysicalEntitySimulation.h | 1 + libraries/physics/src/PhysicsEngine.cpp | 2 +- libraries/physics/src/PhysicsEngine.h | 3 +- .../physics/src/ThreadSafeDynamicsWorld.cpp | 27 +++++++++---- .../physics/src/ThreadSafeDynamicsWorld.h | 4 ++ 10 files changed, 77 insertions(+), 20 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 4894ae55ec..368541490a 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4439,7 +4439,10 @@ void Application::update(float deltaTime) { getEntities()->getTree()->withWriteLock([&] { PerformanceTimer perfTimer("handleOutgoingChanges"); - const VectorOfMotionStates& outgoingChanges = _physicsEngine->getOutgoingChanges(); + const VectorOfMotionStates& deactivations = _physicsEngine->getDeactivatedMotionStates(); + _entitySimulation->handleDeactivatedMotionStates(deactivations); + + const VectorOfMotionStates& outgoingChanges = _physicsEngine->getChangedMotionStates(); _entitySimulation->handleChangedMotionStates(outgoingChanges); avatarManager->handleChangedMotionStates(outgoingChanges); }); diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index db253b639f..888de44505 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -676,7 +676,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef filterRejection = newSimOwner.getID().isNull(); bool verbose = getName() == "fubar"; // adebug if (verbose && _simulationOwner != newSimOwner) { - std::cout << (void*)(this) << " adebug ownership changed " + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug ownership changed " << _simulationOwner.getID().toString().toStdString() << "." << (int)_simulationOwner.getPriority() << "-->" << newSimOwner.getID().toString().toStdString() << "." << (int)newSimOwner.getPriority() << std::endl; // adebug diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 77c5a4f697..5da9d52169 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -10,6 +10,7 @@ // #include +#include // adebug #include #include @@ -28,7 +29,8 @@ #endif // adebug TODO BOOKMARK: -// The problem is that userB may deactivate and disown and object before userA deactivates +// Consider an object near deactivation owned by userB and in view of userA... +// The problem is that userB may deactivate and disown the object before userA deactivates // userA will sometimes insert non-zero velocities (and position errors) into the Entity before it is deactivated locally // // It would be nice to prevent data export from Bullet to Entity for unowned objects except in cases where it is really needed (?) @@ -109,6 +111,24 @@ void EntityMotionState::updateServerPhysicsVariables() { _serverActionData = _entity->getActionData(); } +void EntityMotionState::handleDeactivation() { + // adebug + glm::vec3 pos = _entity->getPosition(); + float dx = glm::distance(pos, _serverPosition); + glm::vec3 v = _entity->getVelocity(); + float dv = glm::distance(v, _serverVelocity); + std::cout << "adebug deactivate '" << _entity->getName().toStdString() + << "' dx = " << dx << " dv = " << dv << std::endl; // adebug + // adebug + + // copy _server data to entity + bool success; + _entity->setPosition(_serverPosition, success, false); + _entity->setOrientation(_serverRotation, success, false); + _entity->setVelocity(ENTITY_ITEM_ZERO_VEC3); + _entity->setAngularVelocity(ENTITY_ITEM_ZERO_VEC3); +} + // virtual void EntityMotionState::handleEasyChanges(uint32_t& flags) { assert(entityTreeIsLocked()); @@ -125,7 +145,7 @@ void EntityMotionState::handleEasyChanges(uint32_t& flags) { _outgoingPriority = 0; bool verbose = _entity->getName() == "fubar"; // adebug if (verbose) { - std::cout << (void*)(this) << " adebug flag for deactivation" << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug flag for deactivation" << std::endl; // adebug } } else { // disowned object is still moving --> start timer for ownership bid @@ -244,7 +264,7 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { } // This callback is invoked by the physics simulation at the end of each simulation step... -// iff the corresponding RigidBody is DYNAMIC and has moved. +// iff the corresponding RigidBody is DYNAMIC and ACTIVE. void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { assert(_entity); assert(entityTreeIsLocked()); @@ -256,7 +276,10 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { // it goes inactive (at which point we should slam bullet to agree with entity) if (_body->getActivationState() == WANTS_DEACTIVATION && !_entity->isMoving()) { if (verbose) { - std::cout << (void*)(this) << " adebug v = " << _body->getLinearVelocity().length() << " w = " << _body->getAngularVelocity().length() << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug entity at rest but physics is not?" + << " v = " << _body->getLinearVelocity().length() + << " w = " << _body->getAngularVelocity().length() + << std::endl; // adebug } } measureBodyAcceleration(); @@ -279,7 +302,7 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { if (verbose && (glm::length(getBodyLinearVelocity()) > 0.0f || glm::length(getBodyAngularVelocity()) > 0.0f) && _entity->getSimulationOwner().getID().isNull()) { - std::cout << (void*)(this) << " adebug set non-zero v on unowned object AS = " << _body->getActivationState() << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug set non-zero v on unowned object AS = " << _body->getActivationState() << std::endl; // adebug } _entity->setVelocity(getBodyLinearVelocity()); @@ -627,7 +650,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ _outgoingPriority = 0; _entity->setPendingOwnershipPriority(_outgoingPriority, now); if (verbose) { - std::cout << (void*)(this) << " adebug sendUpdate() clearOwnership numInactiveUpdates = " << (int)_numInactiveUpdates << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug sendUpdate() clearOwnership numInactiveUpdates = " << (int)_numInactiveUpdates << std::endl; // adebug } } else if (Physics::getSessionUUID() != _entity->getSimulatorID()) { // we don't own the simulation for this entity yet, but we're sending a bid for it @@ -640,7 +663,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ _entity->rememberHasSimulationOwnershipBid(); // ...then reset _outgoingPriority in preparation for the next frame if (verbose) { - std::cout << (void*)(this) << " adebug sendUpdate() bidOwnership at " << (int)_outgoingPriority << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug sendUpdate() bidOwnership at " << (int)_outgoingPriority << std::endl; // adebug } _outgoingPriority = 0; } else if (_outgoingPriority != _entity->getSimulationPriority()) { @@ -654,7 +677,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ } _entity->setPendingOwnershipPriority(_outgoingPriority, now); if (verbose) { - std::cout << (void*)(this) << " adebug sendUpdate() changePriority to " << (int)_outgoingPriority << std::endl; // adebug + std::cout << (void*)(this) << " " << secTimestampNow() << " adebug sendUpdate() changePriority to " << (int)_outgoingPriority << std::endl; // adebug } } diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index feac47d8ec..380edf3927 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -29,6 +29,7 @@ public: virtual ~EntityMotionState(); void updateServerPhysicsVariables(); + void handleDeactivation(); virtual void handleEasyChanges(uint32_t& flags) override; virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override; diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp index 0b0bbcb6fc..bd76b2d70f 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.cpp +++ b/libraries/physics/src/PhysicalEntitySimulation.cpp @@ -259,6 +259,19 @@ void PhysicalEntitySimulation::getObjectsToChange(VectorOfMotionStates& result) _pendingChanges.clear(); } +void PhysicalEntitySimulation::handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates) { + for (auto stateItr : motionStates) { + ObjectMotionState* state = &(*stateItr); + assert(state); + if (state->getType() == MOTIONSTATE_TYPE_ENTITY) { + EntityMotionState* entityState = static_cast(state); + entityState->handleDeactivation(); + EntityItemPointer entity = entityState->getEntity(); + _entitiesToSort.insert(entity); + } + } +} + void PhysicalEntitySimulation::handleChangedMotionStates(const VectorOfMotionStates& motionStates) { QMutexLocker lock(&_mutex); diff --git a/libraries/physics/src/PhysicalEntitySimulation.h b/libraries/physics/src/PhysicalEntitySimulation.h index 9035308741..5f6185add3 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.h +++ b/libraries/physics/src/PhysicalEntitySimulation.h @@ -56,6 +56,7 @@ public: void setObjectsToChange(const VectorOfMotionStates& objectsToChange); void getObjectsToChange(VectorOfMotionStates& result); + void handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates); void handleChangedMotionStates(const VectorOfMotionStates& motionStates); void handleCollisionEvents(const CollisionEvents& collisionEvents); diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index f57be4eab3..7d97d25135 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -449,7 +449,7 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() { return _collisionEvents; } -const VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() { +const VectorOfMotionStates& PhysicsEngine::getChangedMotionStates() { BT_PROFILE("copyOutgoingChanges"); // Bullet will not deactivate static objects (it doesn't expect them to be active) // so we must deactivate them ourselves diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index bbafbb06b6..b2ebe58f08 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -65,7 +65,8 @@ public: bool hasOutgoingChanges() const { return _hasOutgoingChanges; } /// \return reference to list of changed MotionStates. The list is only valid until beginning of next simulation loop. - const VectorOfMotionStates& getOutgoingChanges(); + const VectorOfMotionStates& getChangedMotionStates(); + const VectorOfMotionStates& getDeactivatedMotionStates() const { return _dynamicsWorld->getDeactivatedMotionStates(); } /// \return reference to list of Collision events. The list is only valid until beginning of next simulation loop. const CollisionEvents& getCollisionEvents(); diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index c9cbc6a2be..3c05257fc5 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -128,25 +128,36 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() { for (int i=0;igetMotionState()) { - synchronizeMotionState(body); - _changedMotionStates.push_back(static_cast(body->getMotionState())); - } + if (body && body->getMotionState()) { + synchronizeMotionState(body); + _changedMotionStates.push_back(static_cast(body->getMotionState())); } } } else { //iterate over all active rigid bodies + // TODO? if this becomes a performance bottleneck we could derive our own SimulationIslandManager + // that remembers a list of objects it recently deactivated + _activeStates.clear(); + _deactivatedStates.clear(); for (int i=0;iisActive()) { - if (body->getMotionState()) { + ObjectMotionState* motionState = static_cast(body->getMotionState()); + if (motionState) { + if (body->isActive()) { synchronizeMotionState(body); - _changedMotionStates.push_back(static_cast(body->getMotionState())); + _changedMotionStates.push_back(motionState); + _activeStates.insert(motionState); + } else if (_lastActiveStates.find(motionState) != _lastActiveStates.end()) { + // this object was active last frame but is no longer + _deactivatedStates.push_back(motionState); } } } } + if (_deactivatedStates.size() > 0) { + std::cout << secTimestampNow() << " adebug num deactivated = " << _deactivatedStates.size() << std::endl; // adebug + } + _activeStates.swap(_lastActiveStates); } void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) { diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h index 68062d8d29..b4fcca8cdb 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h @@ -49,12 +49,16 @@ public: float getLocalTimeAccumulation() const { return m_localTime; } const VectorOfMotionStates& getChangedMotionStates() const { return _changedMotionStates; } + const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; } private: // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState() void synchronizeMotionState(btRigidBody* body); VectorOfMotionStates _changedMotionStates; + VectorOfMotionStates _deactivatedStates; + SetOfMotionStates _activeStates; + SetOfMotionStates _lastActiveStates; }; #endif // hifi_ThreadSafeDynamicsWorld_h