diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index a0f2b4cb05..34825ee725 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2227,6 +2227,7 @@ void Application::update(float deltaTime) { PerformanceTimer perfTimer("physics"); _myAvatar->relayDriveKeysToCharacterController(); _physicsEngine.stepSimulation(); + _physicsEngine.dumpStatsIfNecessary(); } if (!_aboutToQuit) { diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 2fb943c0b6..12fb1ba99d 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -75,6 +75,8 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) { _dirtyFlags = 0; _changedOnServer = 0; _element = NULL; + _simulatorIDChangedTime = 0; + _shouldClaimSimulationOwnership = false; initFromEntityItemID(entityItemID); } @@ -90,6 +92,7 @@ EntityItem::EntityItem(const EntityItemID& entityItemID, const EntityItemPropert _dirtyFlags = 0; _changedOnServer = 0; _element = NULL; + _simulatorIDChangedTime = 0; initFromEntityItemID(entityItemID); setProperties(properties); } @@ -321,6 +324,14 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef return 0; } + // if this bitstream indicates that this node is the simulation owner, ignore any physics-related updates. + glm::vec3 savePosition = _position; + glm::quat saveRotation = _rotation; + glm::vec3 saveVelocity = _velocity; + glm::vec3 saveGravity = _gravity; + glm::vec3 saveAcceleration = _acceleration; + + // Header bytes // object ID [16 bytes] // ByteCountCoded(type code) [~1 byte] @@ -420,7 +431,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef #endif bool ignoreServerPacket = false; // assume we'll use this server packet - + // If this packet is from the same server edit as the last packet we accepted from the server // we probably want to use it. if (fromSameServerEdit) { @@ -567,7 +578,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef READ_ENTITY_PROPERTY_STRING(PROP_USER_DATA, setUserData); if (args.bitstreamVersion >= VERSION_ENTITIES_HAVE_ACCELERATION) { - READ_ENTITY_PROPERTY_STRING(PROP_SIMULATOR_ID, setSimulatorID); + READ_ENTITY_PROPERTY_UUID(PROP_SIMULATOR_ID, setSimulatorID); } if (args.bitstreamVersion >= VERSION_ENTITIES_HAS_MARKETPLACE_ID) { @@ -606,6 +617,20 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef _lastSimulated = now; } } + + + auto nodeList = DependencyManager::get(); + const QUuid& myNodeID = nodeList->getSessionUUID(); + if (_simulatorID == myNodeID) { + // the packet that produced this bitstream originally came from physics simulations performed by + // this node, so our version has to be newer than what the packet contained. + _position = savePosition; + _rotation = saveRotation; + _velocity = saveVelocity; + _gravity = saveGravity; + _acceleration = saveAcceleration; + } + return bytesRead; } @@ -680,6 +705,7 @@ void EntityItem::simulate(const quint64& now) { #ifdef WANT_DEBUG qCDebug(entities) << "********** EntityItem::simulate()"; qCDebug(entities) << " entity ID=" << getEntityItemID(); + qCDebug(entities) << " simulator ID=" << getSimulatorID(); qCDebug(entities) << " now=" << now; qCDebug(entities) << " _lastSimulated=" << _lastSimulated; qCDebug(entities) << " timeElapsed=" << timeElapsed; @@ -697,6 +723,7 @@ void EntityItem::simulate(const quint64& now) { qCDebug(entities) << " MOVING...="; qCDebug(entities) << " hasVelocity=" << hasVelocity(); qCDebug(entities) << " hasGravity=" << hasGravity(); + qCDebug(entities) << " hasAcceleration=" << hasAcceleration(); qCDebug(entities) << " hasAngularVelocity=" << hasAngularVelocity(); qCDebug(entities) << " getAngularVelocity=" << getAngularVelocity(); } @@ -766,7 +793,6 @@ void EntityItem::simulateKinematicMotion(float timeElapsed) { qCDebug(entities) << " damping:" << _damping; qCDebug(entities) << " velocity AFTER dampingResistance:" << velocity; qCDebug(entities) << " glm::length(velocity):" << glm::length(velocity); - qCDebug(entities) << " velocityEspilon :" << ENTITY_ITEM_EPSILON_VELOCITY_LENGTH; #endif } @@ -787,13 +813,7 @@ void EntityItem::simulateKinematicMotion(float timeElapsed) { position = newPosition; - // apply gravity - if (hasGravity()) { - // handle resting on surface case, this is definitely a bit of a hack, and it only works on the - // "ground" plane of the domain, but for now it's what we've got - velocity += getGravity() * timeElapsed; - } - + // apply effective acceleration, which will be the same as gravity if the Entity isn't at rest. if (hasAcceleration()) { velocity += getAcceleration() * timeElapsed; } @@ -1150,14 +1170,8 @@ void EntityItem::updateGravity(const glm::vec3& value) { } void EntityItem::updateAcceleration(const glm::vec3& value) { - if (glm::distance(_acceleration, value) > MIN_ACCELERATION_DELTA) { - if (glm::length(value) < MIN_ACCELERATION_DELTA) { - _acceleration = ENTITY_ITEM_ZERO_VEC3; - } else { - _acceleration = value; - } - _dirtyFlags |= EntityItem::DIRTY_VELOCITY; - } + _acceleration = value; + _dirtyFlags |= EntityItem::DIRTY_VELOCITY; } void EntityItem::updateAngularVelocity(const glm::vec3& value) { @@ -1195,3 +1209,9 @@ void EntityItem::updateLifetime(float value) { } } +void EntityItem::setSimulatorID(const QUuid& value) { + if (_simulatorID != value) { + _simulatorID = value; + _simulatorIDChangedTime = usecTimestampNow(); + } +} diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index 8f7feb2ed8..ad856698a2 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -194,8 +194,8 @@ public: void setGravity(const glm::vec3& value) { _gravity = value; } /// gravity in meters bool hasGravity() const { return _gravity != ENTITY_ITEM_ZERO_VEC3; } - const glm::vec3 getAcceleration() const { return _acceleration; } /// get acceleration in meters/second - void setAcceleration(const glm::vec3& value) { _acceleration = value; } /// acceleration in meters/second + const glm::vec3 getAcceleration() const { return _acceleration; } /// get acceleration in meters/second/second + void setAcceleration(const glm::vec3& value) { _acceleration = value; } /// acceleration in meters/second/second bool hasAcceleration() const { return _acceleration != ENTITY_ITEM_ZERO_VEC3; } float getDamping() const { return _damping; } @@ -255,8 +255,11 @@ public: const QString& getUserData() const { return _userData; } void setUserData(const QString& value) { _userData = value; } - QString getSimulatorID() const { return _simulatorID; } - void setSimulatorID(const QString& id) { _simulatorID = id; } + QUuid getSimulatorID() const { return _simulatorID; } + void setSimulatorID(const QUuid& value); + quint64 getSimulatorIDChangedTime() const { return _simulatorIDChangedTime; } + void setShouldClaimSimulationOwnership(bool value) { _shouldClaimSimulationOwnership = value; } + bool getShouldClaimSimulationOwnership() { return _shouldClaimSimulationOwnership; } const QString& getMarketplaceID() const { return _marketplaceID; } void setMarketplaceID(const QString& value) { _marketplaceID = value; } @@ -323,7 +326,7 @@ protected: quint64 _lastEdited; // last official local or remote edit time quint64 _lastEditedFromRemote; // last time we received and edit from the server - quint64 _lastEditedFromRemoteInRemoteTime; // last time we received and edit from the server (in server-time-frame) + quint64 _lastEditedFromRemoteInRemoteTime; // last time we received an edit from the server (in server-time-frame) quint64 _created; quint64 _changedOnServer; @@ -351,7 +354,9 @@ protected: bool _collisionsWillMove; bool _locked; QString _userData; - QString _simulatorID; // id of Node which is currently responsible for simulating this Entity + QUuid _simulatorID; // id of Node which is currently responsible for simulating this Entity + quint64 _simulatorIDChangedTime; // when was _simulatorID last updated? + bool _shouldClaimSimulationOwnership; QString _marketplaceID; // NOTE: Damping is applied like this: v *= pow(1 - damping, dt) diff --git a/libraries/entities/src/EntityItemProperties.cpp b/libraries/entities/src/EntityItemProperties.cpp index 8a25d59f26..e75fa8104f 100644 --- a/libraries/entities/src/EntityItemProperties.cpp +++ b/libraries/entities/src/EntityItemProperties.cpp @@ -307,7 +307,7 @@ QScriptValue EntityItemProperties::copyToScriptValue(QScriptEngine* engine) cons COPY_PROPERTY_TO_QSCRIPTVALUE(animationIsPlaying); COPY_PROPERTY_TO_QSCRIPTVALUE(animationFPS); COPY_PROPERTY_TO_QSCRIPTVALUE(animationFrameIndex); - COPY_PROPERTY_TO_QSCRIPTVALUE_GETTER(animationSettings,getAnimationSettings()); + COPY_PROPERTY_TO_QSCRIPTVALUE_GETTER(animationSettings, getAnimationSettings()); COPY_PROPERTY_TO_QSCRIPTVALUE(glowLevel); COPY_PROPERTY_TO_QSCRIPTVALUE(localRenderAlpha); COPY_PROPERTY_TO_QSCRIPTVALUE(ignoreForCollisions); @@ -319,7 +319,7 @@ QScriptValue EntityItemProperties::copyToScriptValue(QScriptEngine* engine) cons COPY_PROPERTY_TO_QSCRIPTVALUE(locked); COPY_PROPERTY_TO_QSCRIPTVALUE(textures); COPY_PROPERTY_TO_QSCRIPTVALUE(userData); - COPY_PROPERTY_TO_QSCRIPTVALUE(simulatorID); + COPY_PROPERTY_TO_QSCRIPTVALUE_GETTER(simulatorID, getSimulatorIDAsString()); COPY_PROPERTY_TO_QSCRIPTVALUE(text); COPY_PROPERTY_TO_QSCRIPTVALUE(lineHeight); COPY_PROPERTY_TO_QSCRIPTVALUE_COLOR_GETTER(textColor, getTextColor()); @@ -403,7 +403,7 @@ void EntityItemProperties::copyFromScriptValue(const QScriptValue& object) { COPY_PROPERTY_FROM_QSCRIPTVALUE_BOOL(locked, setLocked); COPY_PROPERTY_FROM_QSCRIPTVALUE_STRING(textures, setTextures); COPY_PROPERTY_FROM_QSCRIPTVALUE_STRING(userData, setUserData); - COPY_PROPERTY_FROM_QSCRIPTVALUE_STRING(simulatorID, setSimulatorID); + COPY_PROPERTY_FROM_QSCRIPTVALUE_UUID(simulatorID, setSimulatorID); COPY_PROPERTY_FROM_QSCRIPTVALUE_STRING(text, setText); COPY_PROPERTY_FROM_QSCRIPTVALUE_FLOAT(lineHeight, setLineHeight); COPY_PROPERTY_FROM_QSCRIPTVALUE_COLOR(textColor, setTextColor); @@ -798,7 +798,7 @@ bool EntityItemProperties::decodeEntityEditPacket(const unsigned char* data, int READ_ENTITY_PROPERTY_TO_PROPERTIES(PROP_COLLISIONS_WILL_MOVE, bool, setCollisionsWillMove); READ_ENTITY_PROPERTY_TO_PROPERTIES(PROP_LOCKED, bool, setLocked); READ_ENTITY_PROPERTY_STRING_TO_PROPERTIES(PROP_USER_DATA, setUserData); - READ_ENTITY_PROPERTY_STRING_TO_PROPERTIES(PROP_SIMULATOR_ID, setSimulatorID); + READ_ENTITY_PROPERTY_UUID_TO_PROPERTIES(PROP_SIMULATOR_ID, setSimulatorID); if (properties.getType() == EntityTypes::Text) { READ_ENTITY_PROPERTY_STRING_TO_PROPERTIES(PROP_TEXT, setText); diff --git a/libraries/entities/src/EntityItemProperties.h b/libraries/entities/src/EntityItemProperties.h index 7de0fc0e8b..3788916807 100644 --- a/libraries/entities/src/EntityItemProperties.h +++ b/libraries/entities/src/EntityItemProperties.h @@ -200,7 +200,7 @@ public: DEFINE_PROPERTY_REF(PROP_TEXTURES, Textures, textures, QString); DEFINE_PROPERTY_REF_WITH_SETTER_AND_GETTER(PROP_ANIMATION_SETTINGS, AnimationSettings, animationSettings, QString); DEFINE_PROPERTY_REF(PROP_USER_DATA, UserData, userData, QString); - DEFINE_PROPERTY_REF(PROP_SIMULATOR_ID, SimulatorID, simulatorID, QString); + DEFINE_PROPERTY_REF(PROP_SIMULATOR_ID, SimulatorID, simulatorID, QUuid); DEFINE_PROPERTY_REF(PROP_TEXT, Text, text, QString); DEFINE_PROPERTY(PROP_LINE_HEIGHT, LineHeight, lineHeight, float); DEFINE_PROPERTY_REF(PROP_TEXT_COLOR, TextColor, textColor, xColor); @@ -257,6 +257,7 @@ public: const QStringList& getTextureNames() const { return _textureNames; } void setTextureNames(const QStringList& value) { _textureNames = value; } + QString getSimulatorIDAsString() const { return _simulatorID.toString().mid(1,36).toUpper(); } private: QUuid _id; @@ -330,7 +331,7 @@ inline QDebug operator<<(QDebug debug, const EntityItemProperties& properties) { DEBUG_PROPERTY_IF_CHANGED(debug, properties, Locked, locked, ""); DEBUG_PROPERTY_IF_CHANGED(debug, properties, Textures, textures, ""); DEBUG_PROPERTY_IF_CHANGED(debug, properties, UserData, userData, ""); - DEBUG_PROPERTY_IF_CHANGED(debug, properties, SimulatorID, simulatorID, ""); + DEBUG_PROPERTY_IF_CHANGED(debug, properties, SimulatorID, simulatorID, QUuid()); DEBUG_PROPERTY_IF_CHANGED(debug, properties, Text, text, ""); DEBUG_PROPERTY_IF_CHANGED(debug, properties, LineHeight, lineHeight, ""); DEBUG_PROPERTY_IF_CHANGED(debug, properties, TextColor, textColor, ""); diff --git a/libraries/entities/src/EntityItemPropertiesDefaults.h b/libraries/entities/src/EntityItemPropertiesDefaults.h index 4b595ae0b8..001f963ecb 100644 --- a/libraries/entities/src/EntityItemPropertiesDefaults.h +++ b/libraries/entities/src/EntityItemPropertiesDefaults.h @@ -23,7 +23,7 @@ const glm::vec3 ENTITY_ITEM_ZERO_VEC3(0.0f); const bool ENTITY_ITEM_DEFAULT_LOCKED = false; const QString ENTITY_ITEM_DEFAULT_USER_DATA = QString(""); const QString ENTITY_ITEM_DEFAULT_MARKETPLACE_ID = QString(""); -const QString ENTITY_ITEM_DEFAULT_SIMULATOR_ID = QString(""); +const QUuid ENTITY_ITEM_DEFAULT_SIMULATOR_ID = QUuid(); const float ENTITY_ITEM_DEFAULT_LOCAL_RENDER_ALPHA = 1.0f; const float ENTITY_ITEM_DEFAULT_GLOW_LEVEL = 0.0f; diff --git a/libraries/entities/src/EntityItemPropertiesMacros.h b/libraries/entities/src/EntityItemPropertiesMacros.h index e3e54f5bc8..9cb3449d59 100644 --- a/libraries/entities/src/EntityItemPropertiesMacros.h +++ b/libraries/entities/src/EntityItemPropertiesMacros.h @@ -88,6 +88,26 @@ } \ } +#define READ_ENTITY_PROPERTY_UUID(P,O) \ + if (propertyFlags.getHasProperty(P)) { \ + uint16_t length; \ + memcpy(&length, dataAt, sizeof(length)); \ + dataAt += sizeof(length); \ + bytesRead += sizeof(length); \ + QUuid value; \ + if (length == 0) { \ + value = QUuid(); \ + } else { \ + QByteArray ba((const char*)dataAt, length); \ + value = QUuid::fromRfc4122(ba); \ + dataAt += length; \ + bytesRead += length; \ + } \ + if (overwriteLocalData) { \ + O(value); \ + } \ + } + #define READ_ENTITY_PROPERTY_COLOR(P,M) \ if (propertyFlags.getHasProperty(P)) { \ if (overwriteLocalData) { \ @@ -127,6 +147,25 @@ properties.O(value); \ } + +#define READ_ENTITY_PROPERTY_UUID_TO_PROPERTIES(P,O) \ + if (propertyFlags.getHasProperty(P)) { \ + uint16_t length; \ + memcpy(&length, dataAt, sizeof(length)); \ + dataAt += sizeof(length); \ + processedBytes += sizeof(length); \ + QUuid value; \ + if (length == 0) { \ + value = QUuid(); \ + } else { \ + QByteArray ba((const char*)dataAt, length); \ + value = QUuid::fromRfc4122(ba); \ + dataAt += length; \ + processedBytes += length; \ + } \ + properties.O(value); \ + } + #define READ_ENTITY_PROPERTY_COLOR_TO_PROPERTIES(P,O) \ if (propertyFlags.getHasProperty(P)) { \ xColor color; \ @@ -207,6 +246,15 @@ } \ } +#define COPY_PROPERTY_FROM_QSCRIPTVALUE_UUID(P, S) \ + QScriptValue P = object.property(#P); \ + if (P.isValid()) { \ + QUuid newValue = P.toVariant().toUuid(); \ + if (_defaultSettings || newValue != _##P) { \ + S(newValue); \ + } \ + } + #define COPY_PROPERTY_FROM_QSCRIPTVALUE_VEC3(P, S) \ QScriptValue P = object.property(#P); \ if (P.isValid()) { \ @@ -290,6 +338,7 @@ T get##N() const { return _##n; } \ void set##N(T value) { _##n = value; _##n##Changed = true; } \ bool n##Changed() const { return _##n##Changed; } \ + void set##N##Changed(bool value) { _##n##Changed = value; } \ private: \ T _##n; \ bool _##n##Changed; @@ -299,6 +348,7 @@ const T& get##N() const { return _##n; } \ void set##N(const T& value) { _##n = value; _##n##Changed = true; } \ bool n##Changed() const { return _##n##Changed; } \ + void set##N##Changed(bool value) { _##n##Changed = value; } \ private: \ T _##n; \ bool _##n##Changed; @@ -308,6 +358,7 @@ const T& get##N() const { return _##n; } \ void set##N(const T& value); \ bool n##Changed() const; \ + void set##N##Changed(bool value) { _##n##Changed = value; } \ private: \ T _##n; \ bool _##n##Changed; @@ -317,6 +368,7 @@ T get##N() const; \ void set##N(const T& value); \ bool n##Changed() const; \ + void set##N##Changed(bool value) { _##n##Changed = value; } \ private: \ T _##n; \ bool _##n##Changed; @@ -328,6 +380,7 @@ bool n##Changed() const { return _##n##Changed; } \ QString get##N##AsString() const; \ void set##N##FromString(const QString& name); \ + void set##N##Changed(bool value) { _##n##Changed = value; } \ private: \ T _##n; \ bool _##n##Changed; diff --git a/libraries/entities/src/EntityScriptingInterface.cpp b/libraries/entities/src/EntityScriptingInterface.cpp index c41b9a5c41..53335beda0 100644 --- a/libraries/entities/src/EntityScriptingInterface.cpp +++ b/libraries/entities/src/EntityScriptingInterface.cpp @@ -60,22 +60,45 @@ void EntityScriptingInterface::setEntityTree(EntityTree* modelTree) { } + +void setSimId(EntityItemProperties& propertiesWithSimID, EntityItem* entity) { + auto nodeList = DependencyManager::get(); + const QUuid myNodeID = nodeList->getSessionUUID(); + + // if this entity has non-zero physics/simulation related values, claim simulation ownership + if (propertiesWithSimID.velocityChanged() || + propertiesWithSimID.rotationChanged() || + propertiesWithSimID.containsPositionChange()) { + propertiesWithSimID.setSimulatorID(myNodeID); + entity->setSimulatorID(myNodeID); + } else if (entity->getSimulatorID() == myNodeID) { + propertiesWithSimID.setSimulatorID(QUuid()); // give up simulation ownership + entity->setSimulatorID(QUuid()); + } +} + + + EntityItemID EntityScriptingInterface::addEntity(const EntityItemProperties& properties) { // The application will keep track of creatorTokenID uint32_t creatorTokenID = EntityItemID::getNextCreatorTokenID(); + EntityItemProperties propertiesWithSimID = properties; + EntityItemID id(NEW_ENTITY, creatorTokenID, false ); // If we have a local entity tree set, then also update it. if (_entityTree) { _entityTree->lockForWrite(); - _entityTree->addEntity(id, properties); + EntityItem* entity = _entityTree->addEntity(id, propertiesWithSimID); + // This Node is creating a new object. If it's in motion, set this Node as the simulator. + setSimId(propertiesWithSimID, entity); _entityTree->unlock(); } // queue the packet - queueEntityMessage(PacketTypeEntityAddOrEdit, id, properties); + queueEntityMessage(PacketTypeEntityAddOrEdit, id, propertiesWithSimID); return id; } @@ -137,30 +160,29 @@ EntityItemID EntityScriptingInterface::editEntity(EntityItemID entityID, const E entityID.isKnownID = true; } } - + + EntityItemProperties propertiesWithSimID = properties; + // If we have a local entity tree set, then also update it. We can do this even if we don't know // the actual id, because we can edit out local entities just with creatorTokenID if (_entityTree) { _entityTree->lockForWrite(); - _entityTree->updateEntity(entityID, properties, canAdjustLocks()); + _entityTree->updateEntity(entityID, propertiesWithSimID, canAdjustLocks()); _entityTree->unlock(); } // if at this point, we know the id, send the update to the entity server if (entityID.isKnownID) { // make sure the properties has a type, so that the encode can know which properties to include - if (properties.getType() == EntityTypes::Unknown) { + if (propertiesWithSimID.getType() == EntityTypes::Unknown) { EntityItem* entity = _entityTree->findEntityByEntityItemID(entityID); if (entity) { - EntityItemProperties tempProperties = properties; - tempProperties.setType(entity->getType()); - queueEntityMessage(PacketTypeEntityAddOrEdit, entityID, tempProperties); - return entityID; + propertiesWithSimID.setType(entity->getType()); + setSimId(propertiesWithSimID, entity); } } - - // if the properties already includes the type, then use it as is - queueEntityMessage(PacketTypeEntityAddOrEdit, entityID, properties); + + queueEntityMessage(PacketTypeEntityAddOrEdit, entityID, propertiesWithSimID); } return entityID; diff --git a/libraries/entities/src/EntityTree.cpp b/libraries/entities/src/EntityTree.cpp index ea811389cb..5b15aa26b4 100644 --- a/libraries/entities/src/EntityTree.cpp +++ b/libraries/entities/src/EntityTree.cpp @@ -24,6 +24,10 @@ #include "EntitiesLogging.h" #include "RecurseOctreeToMapOperator.h" + +const quint64 SIMULATOR_CHANGE_LOCKOUT_PERIOD = (quint64)(0.2f * USECS_PER_SECOND); + + EntityTree::EntityTree(bool shouldReaverage) : Octree(shouldReaverage), _fbxService(NULL), @@ -69,25 +73,6 @@ bool EntityTree::handlesEditPacketType(PacketType packetType) const { } } -/// Give an EntityItemID and EntityItemProperties, this will either find the correct entity that already exists -/// in the tree or it will create a new entity of the type specified by the properties and return that item. -/// In the case that it creates a new item, the item will be properly added to the tree and all appropriate lookup hashes. -EntityItem* EntityTree::getOrCreateEntityItem(const EntityItemID& entityID, const EntityItemProperties& properties) { - EntityItem* result = NULL; - - // we need to first see if we already have the entity in our tree by finding the containing element of the entity - EntityTreeElement* containingElement = getContainingElement(entityID); - if (containingElement) { - result = containingElement->getEntityWithEntityItemID(entityID); - } - - // if the element does not exist, then create a new one of the specified type... - if (!result) { - result = addEntity(entityID, properties); - } - return result; -} - /// Adds a new entity item to the tree void EntityTree::postAddEntity(EntityItem* entity) { assert(entity); @@ -127,9 +112,9 @@ bool EntityTree::updateEntity(EntityItem* entity, const EntityItemProperties& pr return updateEntityWithElement(entity, properties, containingElement, allowLockChange); } -bool EntityTree::updateEntityWithElement(EntityItem* entity, const EntityItemProperties& properties, +bool EntityTree::updateEntityWithElement(EntityItem* entity, const EntityItemProperties& origProperties, EntityTreeElement* containingElement, bool allowLockChange) { - + EntityItemProperties properties = origProperties; if (!allowLockChange && (entity->getLocked() != properties.getLocked())) { qCDebug(entities) << "Refusing disallowed lock adjustment."; return false; @@ -149,6 +134,24 @@ bool EntityTree::updateEntityWithElement(EntityItem* entity, const EntityItemPro } } } else { + if (properties.simulatorIDChanged() && + !entity->getSimulatorID().isNull() && + properties.getSimulatorID() != entity->getSimulatorID()) { + // A Node is trying to take ownership of the simulation of this entity from another Node. Only allow this + // if ownership hasn't recently changed. + if (usecTimestampNow() - entity->getSimulatorIDChangedTime() < SIMULATOR_CHANGE_LOCKOUT_PERIOD) { + qCDebug(entities) << "simulator_change_lockout_period:" + << entity->getSimulatorID() << "to" << properties.getSimulatorID(); + // squash the physics-related changes. + properties.setSimulatorIDChanged(false); + properties.setPositionChanged(false); + properties.setVelocityChanged(false); + properties.setAccelerationChanged(false); + } else { + qCDebug(entities) << "allowing simulatorID change"; + } + } + QString entityScriptBefore = entity->getScript(); uint32_t preFlags = entity->getDirtyFlags(); UpdateEntityOperator theOperator(this, containingElement, entity, properties); diff --git a/libraries/entities/src/EntityTree.h b/libraries/entities/src/EntityTree.h index c49cfb2600..3f01dec408 100644 --- a/libraries/entities/src/EntityTree.h +++ b/libraries/entities/src/EntityTree.h @@ -82,7 +82,6 @@ public: virtual void update(); // The newer API... - EntityItem* getOrCreateEntityItem(const EntityItemID& entityID, const EntityItemProperties& properties); void postAddEntity(EntityItem* entityItem); EntityItem* addEntity(const EntityItemID& entityID, const EntityItemProperties& properties); diff --git a/libraries/entities/src/EntityTreeElement.cpp b/libraries/entities/src/EntityTreeElement.cpp index 51648c4fa6..349bd51372 100644 --- a/libraries/entities/src/EntityTreeElement.cpp +++ b/libraries/entities/src/EntityTreeElement.cpp @@ -359,7 +359,7 @@ OctreeElement::AppendState EntityTreeElement::appendElementData(OctreePacketData if (extraEncodeData && entityTreeElementExtraEncodeData) { // After processing, if we are PARTIAL or COMPLETED then we need to re-include our extra data. - // Only our patent can remove our extra data in these cases and only after it knows that all of it's + // Only our parent can remove our extra data in these cases and only after it knows that all of its // children have been encoded. // If we weren't able to encode ANY data about ourselves, then we go ahead and remove our element data // since that will signal that the entire element needs to be encoded on the next attempt @@ -696,7 +696,6 @@ bool EntityTreeElement::removeEntityItem(EntityItem* entity) { // and dirty path marking in one pass. int EntityTreeElement::readElementDataFromBuffer(const unsigned char* data, int bytesLeftToRead, ReadBitstreamToTreeParams& args) { - // If we're the root, but this bitstream doesn't support root elements with data, then // return without reading any bytes if (this == _myTree->getRoot() && args.bitstreamVersion < VERSION_ROOT_ELEMENT_HAS_DATA) { @@ -750,7 +749,7 @@ int EntityTreeElement::readElementDataFromBuffer(const unsigned char* data, int if (bestFitBefore != bestFitAfter) { // This is the case where the entity existed, and is in some element in our tree... if (!bestFitBefore && bestFitAfter) { - // This is the case where the entity existed, and is in some element in our tree... + // This is the case where the entity existed, and is in some element in our tree... if (currentContainingElement != this) { currentContainingElement->removeEntityItem(entityItem); addEntityItem(entityItem); diff --git a/libraries/entities/src/SimpleEntitySimulation.cpp b/libraries/entities/src/SimpleEntitySimulation.cpp index 6d45768c26..6343ed3e47 100644 --- a/libraries/entities/src/SimpleEntitySimulation.cpp +++ b/libraries/entities/src/SimpleEntitySimulation.cpp @@ -13,8 +13,12 @@ #include "EntityItem.h" #include "SimpleEntitySimulation.h" +#include "EntitiesLogging.h" + +const quint64 AUTO_REMOVE_SIMULATION_OWNER_USEC = 2 * USECS_PER_SECOND; void SimpleEntitySimulation::updateEntitiesInternal(const quint64& now) { + // now is usecTimestampNow() QSet::iterator itemItr = _movingEntities.begin(); while (itemItr != _movingEntities.end()) { EntityItem* entity = *itemItr; @@ -27,6 +31,23 @@ void SimpleEntitySimulation::updateEntitiesInternal(const quint64& now) { ++itemItr; } } + + // If an Entity has a simulation owner and we don't get an update for some amount of time, + // clear the owner. This guards against an interface failing to release the Entity when it + // has finished simulating it. + itemItr = _hasSimulationOwnerEntities.begin(); + while (itemItr != _hasSimulationOwnerEntities.end()) { + EntityItem* entity = *itemItr; + if (entity->getSimulatorID().isNull()) { + itemItr = _hasSimulationOwnerEntities.erase(itemItr); + } else if (usecTimestampNow() - entity->getLastChangedOnServer() >= AUTO_REMOVE_SIMULATION_OWNER_USEC) { + qCDebug(entities) << "auto-removing simulation owner" << entity->getSimulatorID(); + entity->setSimulatorID(QUuid()); + itemItr = _hasSimulationOwnerEntities.erase(itemItr); + } else { + ++itemItr; + } + } } void SimpleEntitySimulation::addEntityInternal(EntityItem* entity) { @@ -35,11 +56,15 @@ void SimpleEntitySimulation::addEntityInternal(EntityItem* entity) { } else if (entity->getCollisionsWillMove()) { _movableButStoppedEntities.insert(entity); } + if (!entity->getSimulatorID().isNull()) { + _hasSimulationOwnerEntities.insert(entity); + } } void SimpleEntitySimulation::removeEntityInternal(EntityItem* entity) { _movingEntities.remove(entity); _movableButStoppedEntities.remove(entity); + _hasSimulationOwnerEntities.remove(entity); } const int SIMPLE_SIMULATION_DIRTY_FLAGS = EntityItem::DIRTY_VELOCITY | EntityItem::DIRTY_MOTION_TYPE; @@ -55,6 +80,9 @@ void SimpleEntitySimulation::entityChangedInternal(EntityItem* entity) { _movingEntities.remove(entity); _movableButStoppedEntities.remove(entity); } + if (!entity->getSimulatorID().isNull()) { + _hasSimulationOwnerEntities.insert(entity); + } } entity->clearDirtyFlags(); } @@ -62,5 +90,6 @@ void SimpleEntitySimulation::entityChangedInternal(EntityItem* entity) { void SimpleEntitySimulation::clearEntitiesInternal() { _movingEntities.clear(); _movableButStoppedEntities.clear(); + _hasSimulationOwnerEntities.clear(); } diff --git a/libraries/entities/src/SimpleEntitySimulation.h b/libraries/entities/src/SimpleEntitySimulation.h index 92b6a28215..af79ec0131 100644 --- a/libraries/entities/src/SimpleEntitySimulation.h +++ b/libraries/entities/src/SimpleEntitySimulation.h @@ -30,6 +30,7 @@ protected: QSet _movingEntities; QSet _movableButStoppedEntities; + QSet _hasSimulationOwnerEntities; }; #endif // hifi_SimpleEntitySimulation_h diff --git a/libraries/networking/src/PacketHeaders.cpp b/libraries/networking/src/PacketHeaders.cpp index 52153f9e83..4d7979bddf 100644 --- a/libraries/networking/src/PacketHeaders.cpp +++ b/libraries/networking/src/PacketHeaders.cpp @@ -74,7 +74,7 @@ PacketVersion versionForPacketType(PacketType type) { return 1; case PacketTypeEntityAddOrEdit: case PacketTypeEntityData: - return VERSION_ENTITIES_HAVE_ACCELERATION; + return VERSION_ENTITIES_HAVE_UUIDS; case PacketTypeEntityErase: return 2; case PacketTypeAudioStreamStats: diff --git a/libraries/networking/src/PacketHeaders.h b/libraries/networking/src/PacketHeaders.h index 9fb14854aa..979eb4d451 100644 --- a/libraries/networking/src/PacketHeaders.h +++ b/libraries/networking/src/PacketHeaders.h @@ -135,6 +135,7 @@ const PacketVersion VERSION_ENTITIES_HAS_COLLISION_MODEL = 12; const PacketVersion VERSION_ENTITIES_HAS_MARKETPLACE_ID_DAMAGED = 13; const PacketVersion VERSION_ENTITIES_HAS_MARKETPLACE_ID = 14; const PacketVersion VERSION_ENTITIES_HAVE_ACCELERATION = 15; +const PacketVersion VERSION_ENTITIES_HAVE_UUIDS = 16; const PacketVersion VERSION_OCTREE_HAS_FILE_BREAKS = 1; #endif // hifi_PacketHeaders_h diff --git a/libraries/octree/src/OctreePacketData.cpp b/libraries/octree/src/OctreePacketData.cpp index 674faa11c3..64947010a0 100644 --- a/libraries/octree/src/OctreePacketData.cpp +++ b/libraries/octree/src/OctreePacketData.cpp @@ -411,6 +411,20 @@ bool OctreePacketData::appendValue(const QString& string) { return success; } +bool OctreePacketData::appendValue(const QUuid& uuid) { + QByteArray bytes = uuid.toRfc4122(); + if (uuid.isNull()) { + return appendValue((uint16_t)0); // zero length for null uuid + } else { + uint16_t length = bytes.size(); + bool success = appendValue(length); + if (success) { + success = appendRawData((const unsigned char*)bytes.constData(), bytes.size()); + } + return success; + } +} + bool OctreePacketData::appendValue(const QByteArray& bytes) { bool success = appendRawData((const unsigned char*)bytes.constData(), bytes.size()); return success; diff --git a/libraries/octree/src/OctreePacketData.h b/libraries/octree/src/OctreePacketData.h index 1c1576f509..992eca99ae 100644 --- a/libraries/octree/src/OctreePacketData.h +++ b/libraries/octree/src/OctreePacketData.h @@ -166,6 +166,9 @@ public: /// appends a string value to the end of the stream, may fail if new data stream is too long to fit in packet bool appendValue(const QString& string); + /// appends a uuid value to the end of the stream, may fail if new data stream is too long to fit in packet + bool appendValue(const QUuid& uuid); + /// appends a QByteArray value to the end of the stream, may fail if new data stream is too long to fit in packet bool appendValue(const QByteArray& bytes); diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index d84daa6b42..e5a12a2b66 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -18,6 +18,7 @@ #include "PhysicsHelpers.h" #include "PhysicsLogging.h" +static const float MEASURED_ACCELERATION_CLOSE_TO_ZERO = 0.05f; QSet* _outgoingEntityList; @@ -62,6 +63,9 @@ void EntityMotionState::stepKinematicSimulation(quint64 now) { // which is different from physical kinematic motion (inside getWorldTransform()) // which steps in physics simulation time. _entity->simulate(now); + // TODO: we can't use ObjectMotionState::measureAcceleration() here because the entity + // has no RigidBody and the timestep is a little bit out of sync with the physics simulation anyway. + // Hence we must manually measure kinematic velocity and acceleration. } bool EntityMotionState::isMoving() const { @@ -71,7 +75,7 @@ bool EntityMotionState::isMoving() const { // This callback is invoked by the physics simulation in two cases: // (1) when the RigidBody is first added to the world // (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC) -// (2) at the beginning of each simulation frame for KINEMATIC RigidBody's -- +// (2) at the beginning of each simulation step for KINEMATIC RigidBody's -- // it is an opportunity for outside code to update the object's simulation position void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { if (_isKinematic) { @@ -89,9 +93,10 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { worldTrans.setRotation(glmToBullet(_entity->getRotation())); } -// This callback is invoked by the physics simulation at the end of each simulation frame... +// 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) { + measureAcceleration(); _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset()); _entity->setRotation(bulletToGLM(worldTrans.getRotation())); @@ -116,7 +121,7 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { #endif } -void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) { +void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t step) { if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) { if (flags & EntityItem::DIRTY_POSITION) { _sentPosition = _entity->getPosition() - ObjectMotionState::getWorldOffset(); @@ -131,7 +136,7 @@ void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) { if (flags & EntityItem::DIRTY_VELOCITY) { updateObjectVelocities(); } - _sentFrame = frame; + _sentStep = step; } // TODO: entity support for friction and restitution @@ -162,8 +167,8 @@ void EntityMotionState::updateObjectVelocities() { _sentAngularVelocity = _entity->getAngularVelocity(); setAngularVelocity(_sentAngularVelocity); - _sentAcceleration = _entity->getGravity(); - setGravity(_sentAcceleration); + _sentGravity = _entity->getGravity(); + setGravity(_sentGravity); _body->setActivationState(ACTIVE_TAG); } @@ -179,13 +184,42 @@ float EntityMotionState::computeMass(const ShapeInfo& shapeInfo) const { return _entity->computeMass(); } -void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame) { +bool EntityMotionState::shouldSendUpdate(uint32_t simulationFrame) { + bool baseResult = this->ObjectMotionState::shouldSendUpdate(simulationFrame); + + if (!baseResult) { + return false; + } + + if (_entity->getShouldClaimSimulationOwnership()) { + return true; + } + + auto nodeList = DependencyManager::get(); + const QUuid& myNodeID = nodeList->getSessionUUID(); + const QUuid& simulatorID = _entity->getSimulatorID(); + + if (!simulatorID.isNull() && simulatorID != myNodeID) { + // some other Node owns the simulating of this, so don't broadcast the results of local simulation. + return false; + } + + return true; +} + +void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step) { if (!_entity->isKnownID()) { return; // never update entities that are unknown } if (_outgoingPacketFlags) { EntityItemProperties properties = _entity->getProperties(); + if (glm::length(_measuredAcceleration) < MEASURED_ACCELERATION_CLOSE_TO_ZERO) { + _entity->setAcceleration(glm::vec3(0.0f)); + } else { + _entity->setAcceleration(_entity->getGravity()); + } + if (_outgoingPacketFlags & EntityItem::DIRTY_POSITION) { btTransform worldTrans = _body->getWorldTransform(); _sentPosition = bulletToGLM(worldTrans.getOrigin()); @@ -194,7 +228,10 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ _sentRotation = bulletToGLM(worldTrans.getRotation()); properties.setRotation(_sentRotation); } - + + bool zeroSpeed = true; + bool zeroSpin = true; + if (_outgoingPacketFlags & EntityItem::DIRTY_VELOCITY) { if (_body->isActive()) { _sentVelocity = bulletToGLM(_body->getLinearVelocity()); @@ -202,12 +239,12 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ // if the speeds are very small we zero them out const float MINIMUM_EXTRAPOLATION_SPEED_SQUARED = 1.0e-4f; // 1cm/sec - bool zeroSpeed = (glm::length2(_sentVelocity) < MINIMUM_EXTRAPOLATION_SPEED_SQUARED); + zeroSpeed = (glm::length2(_sentVelocity) < MINIMUM_EXTRAPOLATION_SPEED_SQUARED); if (zeroSpeed) { _sentVelocity = glm::vec3(0.0f); } const float MINIMUM_EXTRAPOLATION_SPIN_SQUARED = 0.004f; // ~0.01 rotation/sec - bool zeroSpin = glm::length2(_sentAngularVelocity) < MINIMUM_EXTRAPOLATION_SPIN_SQUARED; + zeroSpin = glm::length2(_sentAngularVelocity) < MINIMUM_EXTRAPOLATION_SPIN_SQUARED; if (zeroSpin) { _sentAngularVelocity = glm::vec3(0.0f); } @@ -218,11 +255,32 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ _sentMoving = false; } properties.setVelocity(_sentVelocity); - _sentAcceleration = bulletToGLM(_body->getGravity()); - properties.setGravity(_sentAcceleration); + _sentGravity = _entity->getGravity(); + properties.setGravity(_entity->getGravity()); + _sentAcceleration = _entity->getAcceleration(); + properties.setAcceleration(_sentAcceleration); properties.setAngularVelocity(_sentAngularVelocity); } + auto nodeList = DependencyManager::get(); + QUuid myNodeID = nodeList->getSessionUUID(); + QUuid simulatorID = _entity->getSimulatorID(); + + if (_entity->getShouldClaimSimulationOwnership()) { + _entity->setSimulatorID(myNodeID); + properties.setSimulatorID(myNodeID); + _entity->setShouldClaimSimulationOwnership(false); + } + else if (simulatorID.isNull() && !(zeroSpeed && zeroSpin)) { + // The object is moving and nobody thinks they own the motion. set this Node as the simulator + _entity->setSimulatorID(myNodeID); + properties.setSimulatorID(myNodeID); + } else if (simulatorID == myNodeID && zeroSpeed && zeroSpin) { + // we are the simulator and the object has stopped. give up "simulator" status + _entity->setSimulatorID(QUuid()); + properties.setSimulatorID(QUuid()); + } + // RELIABLE_SEND_HACK: count number of updates for entities at rest so we can stop sending them after some limit. if (_sentMoving) { _numNonMovingUpdates = 0; @@ -231,7 +289,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ } if (_numNonMovingUpdates <= 1) { // we only update lastEdited when we're sending new physics data - // (i.e. NOT when we just simulate the positions forward, nore when we resend non-moving data) + // (i.e. NOT when we just simulate the positions forward, nor when we resend non-moving data) // NOTE: Andrew & Brad to discuss. Let's make sure we're using lastEdited, lastSimulated, and lastUpdated correctly quint64 lastSimulated = _entity->getLastSimulated(); _entity->setLastEdited(lastSimulated); @@ -240,7 +298,8 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ #ifdef WANT_DEBUG quint64 now = usecTimestampNow(); qCDebug(physics) << "EntityMotionState::sendUpdate()"; - qCDebug(physics) << " EntityItemId:" << _entity->getEntityItemID() << "---------------------------------------------"; + qCDebug(physics) << " EntityItemId:" << _entity->getEntityItemID() + << "---------------------------------------------"; qCDebug(physics) << " lastSimulated:" << debugTime(lastSimulated, now); #endif //def WANT_DEBUG @@ -254,6 +313,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ #ifdef WANT_DEBUG qCDebug(physics) << "EntityMotionState::sendUpdate()... calling queueEditEntityMessage()..."; #endif + entityPacketSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, id, properties); } else { #ifdef WANT_DEBUG @@ -264,7 +324,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ // The outgoing flags only itemized WHAT to send, not WHETHER to send, hence we always set them // to the full set. These flags may be momentarily cleared by incoming external changes. _outgoingPacketFlags = DIRTY_PHYSICS_FLAGS; - _sentFrame = frame; + _sentStep = step; } } diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 7214626fc4..2b965a39d3 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -50,13 +50,14 @@ public: virtual void setWorldTransform(const btTransform& worldTrans); // these relay incoming values to the RigidBody - virtual void updateObjectEasy(uint32_t flags, uint32_t frame); + virtual void updateObjectEasy(uint32_t flags, uint32_t step); virtual void updateObjectVelocities(); virtual void computeShapeInfo(ShapeInfo& shapeInfo); virtual float computeMass(const ShapeInfo& shapeInfo) const; - virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame); + virtual bool shouldSendUpdate(uint32_t simulationFrame); + virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step); virtual uint32_t getIncomingDirtyFlags() const; virtual void clearIncomingDirtyFlags(uint32_t flags) { _entity->clearDirtyFlags(flags); } diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index c9f416cc37..3d8e578056 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -22,7 +22,7 @@ const float MAX_FRICTION = 10.0f; const float DEFAULT_RESTITUTION = 0.5f; -// origin of physics simulation in world frame +// origin of physics simulation in world-frame glm::vec3 _worldOffset(0.0f); // static @@ -35,6 +35,12 @@ const glm::vec3& ObjectMotionState::getWorldOffset() { return _worldOffset; } +// static +uint32_t _simulationStep = 0; +void ObjectMotionState::setSimulationStep(uint32_t step) { + assert(step > _simulationStep); + _simulationStep = step; +} ObjectMotionState::ObjectMotionState() : _friction(DEFAULT_FRICTION), @@ -46,12 +52,16 @@ ObjectMotionState::ObjectMotionState() : _sentMoving(false), _numNonMovingUpdates(0), _outgoingPacketFlags(DIRTY_PHYSICS_FLAGS), - _sentFrame(0), + _sentStep(0), _sentPosition(0.0f), _sentRotation(), _sentVelocity(0.0f), _sentAngularVelocity(0.0f), - _sentAcceleration(0.0f) { + _sentGravity(0.0f), + _sentAcceleration(0.0f), + _lastSimulationStep(0), + _lastVelocity(0.0f), + _measuredAcceleration(0.0f) { } ObjectMotionState::~ObjectMotionState() { @@ -59,6 +69,27 @@ ObjectMotionState::~ObjectMotionState() { assert(_body == NULL); } +void ObjectMotionState::measureAcceleration() { + // try to manually measure the true acceleration of the object + uint32_t numSubsteps = _simulationStep - _lastSimulationStep; + if (numSubsteps > 0) { + float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP); + float invDt = 1.0f / dt; + _lastSimulationStep = _simulationStep; + + // 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 - _linearDamping, dt) - _lastVelocity) * invDt; + _lastVelocity = velocity; + } +} + +void ObjectMotionState::resetMeasuredAcceleration() { + _lastSimulationStep = _simulationStep; + _lastVelocity = bulletToGLM(_body->getLinearVelocity()); +} + void ObjectMotionState::setFriction(float friction) { _friction = btMax(btMin(fabsf(friction), MAX_FRICTION), 0.0f); } @@ -103,15 +134,16 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const { return !_body->isActive() && _numNonMovingUpdates > MAX_NUM_NON_MOVING_UPDATES; } -bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { +bool ObjectMotionState::shouldSendUpdate(uint32_t simulationStep) { 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()); + // if we've never checked before, our _sentStep will be 0, and we need to initialize our state + if (_sentStep == 0) { + btTransform xform = _body->getWorldTransform(); + _sentPosition = bulletToGLM(xform.getOrigin()); + _sentRotation = bulletToGLM(xform.getRotation()); _sentVelocity = bulletToGLM(_body->getLinearVelocity()); - _sentRotation = bulletToGLM(_body->getWorldTransform().getRotation()); _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity()); - _sentFrame = simulationFrame; + _sentStep = simulationStep; return false; } @@ -121,9 +153,9 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { glm::vec3 wasAngularVelocity = _sentAngularVelocity; #endif - int numFrames = simulationFrame - _sentFrame; - float dt = (float)(numFrames) * PHYSICS_ENGINE_FIXED_SUBSTEP; - _sentFrame = simulationFrame; + int numSteps = simulationStep - _sentStep; + float dt = (float)(numSteps) * PHYSICS_ENGINE_FIXED_SUBSTEP; + _sentStep = simulationStep; bool isActive = _body->isActive(); if (!isActive) { @@ -143,7 +175,7 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { // 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 + // NOTE: math is done in the simulation-frame, which is NOT necessarily the same as the world-frame // due to _worldOffset. // compute position error @@ -179,7 +211,7 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { // 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 < numFrames; ++i) { + for (int i = 0; i < numSteps; ++i) { _sentRotation = glm::normalize(computeBulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation); } } diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index fb402a178d..ad9ef861b9 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -47,6 +47,8 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit class OctreeEditPacketSender; +extern const int MAX_NUM_NON_MOVING_UPDATES; + class ObjectMotionState : public btMotionState { public: // The WorldOffset is used to keep the positions of objects in the simulation near the origin, to @@ -57,9 +59,14 @@ public: static void setWorldOffset(const glm::vec3& offset); static const glm::vec3& getWorldOffset(); + static void setSimulationStep(uint32_t step); + ObjectMotionState(); ~ObjectMotionState(); + void measureAcceleration(); + void resetMeasuredAcceleration(); + // An EASY update does not require the object to be removed and then reinserted into the PhysicsEngine virtual void updateObjectEasy(uint32_t flags, uint32_t frame) = 0; virtual void updateObjectVelocities() = 0; @@ -87,7 +94,7 @@ public: void clearOutgoingPacketFlags(uint32_t flags) { _outgoingPacketFlags &= ~flags; } bool doesNotNeedToSendUpdate() const; - virtual bool shouldSendUpdate(uint32_t simulationFrame); + virtual bool shouldSendUpdate(uint32_t simulationStep); virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame) = 0; virtual MotionType computeMotionType() const = 0; @@ -126,12 +133,17 @@ protected: int _numNonMovingUpdates; // RELIABLE_SEND_HACK for "not so reliable" resends of packets for non-moving objects uint32_t _outgoingPacketFlags; - uint32_t _sentFrame; + uint32_t _sentStep; glm::vec3 _sentPosition; // in simulation-frame (not world-frame) glm::quat _sentRotation;; glm::vec3 _sentVelocity; glm::vec3 _sentAngularVelocity; // radians per second + glm::vec3 _sentGravity; glm::vec3 _sentAcceleration; + + uint32_t _lastSimulationStep; + glm::vec3 _lastVelocity; + glm::vec3 _measuredAcceleration; }; #endif // hifi_ObjectMotionState_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 50f52a7efc..fbfb7914f0 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -23,8 +23,9 @@ uint32_t PhysicsEngine::getNumSubsteps() { return _numSubsteps; } -PhysicsEngine::PhysicsEngine(const glm::vec3& offset) - : _originOffset(offset) { +PhysicsEngine::PhysicsEngine(const glm::vec3& offset) : + _originOffset(offset), + _characterController(NULL) { } PhysicsEngine::~PhysicsEngine() { @@ -193,6 +194,9 @@ void PhysicsEngine::relayIncomingChangesToSimulation() { // hence the MotionState has all the knowledge and authority to perform the update. motionState->updateObjectEasy(flags, _numSubsteps); } + if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) { + motionState->resetMeasuredAcceleration(); + } } else { // the only way we should ever get here (motionState exists but no body) is when the object // is undergoing non-physical kinematic motion. @@ -288,77 +292,73 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) { } void PhysicsEngine::stepSimulation() { - { - lock(); - CProfileManager::Reset(); - BT_PROFILE("stepSimulation"); - // NOTE: the grand order of operations is: - // (1) pull incoming changes - // (2) step simulation - // (3) synchronize outgoing motion states - // (4) send outgoing packets + lock(); + CProfileManager::Reset(); + BT_PROFILE("stepSimulation"); + // NOTE: the grand order of operations is: + // (1) pull incoming changes + // (2) step simulation + // (3) synchronize outgoing motion states + // (4) send outgoing packets - // This is step (1) pull incoming changes - relayIncomingChangesToSimulation(); - - const int MAX_NUM_SUBSTEPS = 4; - const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * PHYSICS_ENGINE_FIXED_SUBSTEP; - float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds()); - _clock.reset(); - float timeStep = btMin(dt, MAX_TIMESTEP); - - // TODO: move character->preSimulation() into relayIncomingChanges - if (_characterController) { - if (_characterController->needsRemoval()) { - _characterController->setDynamicsWorld(NULL); - } - _characterController->updateShapeIfNecessary(); - if (_characterController->needsAddition()) { - _characterController->setDynamicsWorld(_dynamicsWorld); - } - _characterController->preSimulation(timeStep); + // This is step (1) pull incoming changes + relayIncomingChangesToSimulation(); + + const int MAX_NUM_SUBSTEPS = 4; + const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * PHYSICS_ENGINE_FIXED_SUBSTEP; + float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds()); + _clock.reset(); + float timeStep = btMin(dt, MAX_TIMESTEP); + + // TODO: move character->preSimulation() into relayIncomingChanges + if (_characterController) { + if (_characterController->needsRemoval()) { + _characterController->setDynamicsWorld(NULL); } - - // This is step (2) step simulation - int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP); - _numSubsteps += (uint32_t)numSubsteps; - stepNonPhysicalKinematics(usecTimestampNow()); - unlock(); - - // TODO: make all of this harvest stuff into one function: relayOutgoingChanges() - if (numSubsteps > 0) { - BT_PROFILE("postSimulation"); - // This is step (3) which is done outside of stepSimulation() so we can lock _entityTree. - // - // Unfortunately we have to unlock the simulation (above) before we try to lock the _entityTree - // to avoid deadlock -- the _entityTree may try to lock its EntitySimulation (from which this - // PhysicsEngine derives) when updating/adding/deleting entities so we need to wait for our own - // lock on the tree before we re-lock ourselves. - // - // TODO: untangle these lock sequences. - _entityTree->lockForWrite(); - lock(); - _dynamicsWorld->synchronizeMotionStates(); - - if (_characterController) { - _characterController->postSimulation(); - } - - unlock(); - _entityTree->unlock(); - - computeCollisionEvents(); + _characterController->updateShapeIfNecessary(); + if (_characterController->needsAddition()) { + _characterController->setDynamicsWorld(_dynamicsWorld); } + _characterController->preSimulation(timeStep); } - if (_dumpNextStats) { - _dumpNextStats = false; - CProfileManager::dumpAll(); + + // This is step (2) step simulation + int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP); + _numSubsteps += (uint32_t)numSubsteps; + stepNonPhysicalKinematics(usecTimestampNow()); + unlock(); + + // TODO: make all of this harvest stuff into one function: relayOutgoingChanges() + if (numSubsteps > 0) { + BT_PROFILE("postSimulation"); + // This is step (3) which is done outside of stepSimulation() so we can lock _entityTree. + // + // Unfortunately we have to unlock the simulation (above) before we try to lock the _entityTree + // to avoid deadlock -- the _entityTree may try to lock its EntitySimulation (from which this + // PhysicsEngine derives) when updating/adding/deleting entities so we need to wait for our own + // lock on the tree before we re-lock ourselves. + // + // TODO: untangle these lock sequences. + ObjectMotionState::setSimulationStep(_numSubsteps); + _entityTree->lockForWrite(); + lock(); + _dynamicsWorld->synchronizeMotionStates(); + + if (_characterController) { + _characterController->postSimulation(); + } + + unlock(); + _entityTree->unlock(); + + computeCollisionEvents(); } } void PhysicsEngine::stepNonPhysicalKinematics(const quint64& now) { BT_PROFILE("nonPhysicalKinematics"); QSet::iterator stateItr = _nonPhysicalKinematicObjects.begin(); + // TODO?: need to occasionally scan for stopped non-physical kinematics objects while (stateItr != _nonPhysicalKinematicObjects.end()) { ObjectMotionState* motionState = *stateItr; motionState->stepKinematicSimulation(now); @@ -366,10 +366,12 @@ void PhysicsEngine::stepNonPhysicalKinematics(const quint64& now) { } } -// TODO?: need to occasionally scan for stopped non-physical kinematics objects - void PhysicsEngine::computeCollisionEvents() { BT_PROFILE("computeCollisionEvents"); + + const btCollisionObject* characterCollisionObject = + _characterController ? _characterController->getCollisionObject() : NULL; + // update all contacts every frame int numManifolds = _collisionDispatcher->getNumManifolds(); for (int i = 0; i < numManifolds; ++i) { @@ -385,12 +387,23 @@ void PhysicsEngine::computeCollisionEvents() { // which will eventually trigger a CONTACT_EVENT_TYPE_END continue; } - + void* a = objectA->getUserPointer(); void* b = objectB->getUserPointer(); if (a || b) { // the manifold has up to 4 distinct points, but only extract info from the first _contactMap[ContactKey(a, b)].update(_numContactFrames, contactManifold->getContactPoint(0), _originOffset); + + // if our character capsule is colliding with something dynamic, claim simulation ownership. + // see EntityMotionState::sendUpdate + if (objectA == characterCollisionObject && !objectB->isStaticOrKinematicObject() && b) { + EntityItem* entityB = static_cast(b)->getEntity(); + entityB->setShouldClaimSimulationOwnership(true); + } + if (objectB == characterCollisionObject && !objectA->isStaticOrKinematicObject() && a) { + EntityItem* entityA = static_cast(a)->getEntity(); + entityA->setShouldClaimSimulationOwnership(true); + } } } } @@ -445,6 +458,13 @@ void PhysicsEngine::computeCollisionEvents() { } } +void PhysicsEngine::dumpStatsIfNecessary() { + if (_dumpNextStats) { + _dumpNextStats = false; + CProfileManager::dumpAll(); + } +} + // Bullet collision flags are as follows: // CF_STATIC_OBJECT= 1, // CF_KINEMATIC_OBJECT= 2, @@ -507,6 +527,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap body->setFriction(motionState->_friction); body->setDamping(motionState->_linearDamping, motionState->_angularDamping); _dynamicsWorld->addRigidBody(body); + motionState->resetMeasuredAcceleration(); } void PhysicsEngine::removeObjectFromBullet(ObjectMotionState* motionState) { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 01717be175..6e1f430237 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -68,9 +68,10 @@ public: void stepSimulation(); void stepNonPhysicalKinematics(const quint64& now); - void computeCollisionEvents(); + void dumpStatsIfNecessary(); + /// \param offset position of simulation origin in domain-frame void setOriginOffset(const glm::vec3& offset) { _originOffset = offset; }