diff --git a/assignment-client/src/AssignmentDynamic.h b/assignment-client/src/AssignmentDynamic.h index 35db8b1524..604c418c13 100644 --- a/assignment-client/src/AssignmentDynamic.h +++ b/assignment-client/src/AssignmentDynamic.h @@ -24,6 +24,8 @@ public: AssignmentDynamic(EntityDynamicType type, const QUuid& id, EntityItemPointer ownerEntity); virtual ~AssignmentDynamic(); + virtual void remapIDs(QHash& map) override {}; + virtual void removeFromSimulation(EntitySimulationPointer simulation) const override; virtual EntityItemWeakPointer getOwnerEntity() const override { return _ownerEntity; } virtual void setOwnerEntity(const EntityItemPointer ownerEntity) override { _ownerEntity = ownerEntity; } diff --git a/interface/src/InterfaceDynamicFactory.cpp b/interface/src/InterfaceDynamicFactory.cpp index 5acc0700c0..49edaae90f 100644 --- a/interface/src/InterfaceDynamicFactory.cpp +++ b/interface/src/InterfaceDynamicFactory.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,8 @@ EntityDynamicPointer interfaceDynamicFactory(EntityDynamicType type, const QUuid return std::make_shared(id, ownerEntity); case DYNAMIC_TYPE_SPRING: return std::make_shared(id, ownerEntity); + case DYNAMIC_TYPE_TRACTOR: + return std::make_shared(id, ownerEntity); case DYNAMIC_TYPE_HOLD: return std::make_shared(id, ownerEntity); case DYNAMIC_TYPE_TRAVEL_ORIENTED: diff --git a/interface/src/avatar/AvatarActionFarGrab.cpp b/interface/src/avatar/AvatarActionFarGrab.cpp index afa21e58d7..1144591d09 100644 --- a/interface/src/avatar/AvatarActionFarGrab.cpp +++ b/interface/src/avatar/AvatarActionFarGrab.cpp @@ -12,7 +12,7 @@ #include "AvatarActionFarGrab.h" AvatarActionFarGrab::AvatarActionFarGrab(const QUuid& id, EntityItemPointer ownerEntity) : - ObjectActionSpring(id, ownerEntity) { + ObjectActionTractor(id, ownerEntity) { _type = DYNAMIC_TYPE_FAR_GRAB; #if WANT_DEBUG qDebug() << "AvatarActionFarGrab::AvatarActionFarGrab"; @@ -32,7 +32,7 @@ QByteArray AvatarActionFarGrab::serialize() const { dataStream << DYNAMIC_TYPE_FAR_GRAB; dataStream << getID(); - dataStream << ObjectActionSpring::springVersion; + dataStream << ObjectActionTractor::tractorVersion; serializeParameters(dataStream); @@ -55,7 +55,7 @@ void AvatarActionFarGrab::deserialize(QByteArray serializedArguments) { uint16_t serializationVersion; dataStream >> serializationVersion; - if (serializationVersion != ObjectActionSpring::springVersion) { + if (serializationVersion != ObjectActionTractor::tractorVersion) { assert(false); return; } diff --git a/interface/src/avatar/AvatarActionFarGrab.h b/interface/src/avatar/AvatarActionFarGrab.h index 46c9f65dcf..bcaf7f2f3c 100644 --- a/interface/src/avatar/AvatarActionFarGrab.h +++ b/interface/src/avatar/AvatarActionFarGrab.h @@ -13,9 +13,9 @@ #define hifi_AvatarActionFarGrab_h #include -#include +#include -class AvatarActionFarGrab : public ObjectActionSpring { +class AvatarActionFarGrab : public ObjectActionTractor { public: AvatarActionFarGrab(const QUuid& id, EntityItemPointer ownerEntity); virtual ~AvatarActionFarGrab(); diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index 627dc2ba02..c1d2f903f3 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/interface/src/avatar/AvatarActionHold.cpp @@ -21,7 +21,7 @@ const int AvatarActionHold::velocitySmoothFrames = 6; AvatarActionHold::AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntity) : - ObjectActionSpring(id, ownerEntity) + ObjectActionTractor(id, ownerEntity) { _type = DYNAMIC_TYPE_HOLD; _measuredLinearVelocities.resize(AvatarActionHold::velocitySmoothFrames); @@ -224,12 +224,12 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm:: void AvatarActionHold::updateActionWorker(float deltaTimeStep) { if (_kinematic) { - if (prepareForSpringUpdate(deltaTimeStep)) { + if (prepareForTractorUpdate(deltaTimeStep)) { doKinematicUpdate(deltaTimeStep); } } else { forceBodyNonStatic(); - ObjectActionSpring::updateActionWorker(deltaTimeStep); + ObjectActionTractor::updateActionWorker(deltaTimeStep); } } diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h index 7eeda53e06..6acc71b45c 100644 --- a/interface/src/avatar/AvatarActionHold.h +++ b/interface/src/avatar/AvatarActionHold.h @@ -16,12 +16,12 @@ #include #include -#include +#include #include "avatar/MyAvatar.h" -class AvatarActionHold : public ObjectActionSpring { +class AvatarActionHold : public ObjectActionTractor { public: AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntity); virtual ~AvatarActionHold(); diff --git a/libraries/entities/src/EntityDynamicInterface.cpp b/libraries/entities/src/EntityDynamicInterface.cpp index 57f86105b2..822ac1c9d6 100644 --- a/libraries/entities/src/EntityDynamicInterface.cpp +++ b/libraries/entities/src/EntityDynamicInterface.cpp @@ -105,6 +105,9 @@ EntityDynamicType EntityDynamicInterface::dynamicTypeFromString(QString dynamicT if (normalizedDynamicTypeString == "spring") { return DYNAMIC_TYPE_SPRING; } + if (normalizedDynamicTypeString == "tractor") { + return DYNAMIC_TYPE_TRACTOR; + } if (normalizedDynamicTypeString == "hold") { return DYNAMIC_TYPE_HOLD; } @@ -140,6 +143,8 @@ QString EntityDynamicInterface::dynamicTypeToString(EntityDynamicType dynamicTyp return "offset"; case DYNAMIC_TYPE_SPRING: return "spring"; + case DYNAMIC_TYPE_TRACTOR: + return "tractor"; case DYNAMIC_TYPE_HOLD: return "hold"; case DYNAMIC_TYPE_TRAVEL_ORIENTED: diff --git a/libraries/entities/src/EntityDynamicInterface.h b/libraries/entities/src/EntityDynamicInterface.h index c04aaf67b2..b27e600660 100644 --- a/libraries/entities/src/EntityDynamicInterface.h +++ b/libraries/entities/src/EntityDynamicInterface.h @@ -17,6 +17,7 @@ #include class EntityItem; +class EntityItemID; class EntitySimulation; using EntityItemPointer = std::shared_ptr; using EntityItemWeakPointer = std::weak_ptr; @@ -28,6 +29,7 @@ enum EntityDynamicType { DYNAMIC_TYPE_NONE = 0, DYNAMIC_TYPE_OFFSET = 1000, DYNAMIC_TYPE_SPRING = 2000, + DYNAMIC_TYPE_TRACTOR = 2100, DYNAMIC_TYPE_HOLD = 3000, DYNAMIC_TYPE_TRAVEL_ORIENTED = 4000, DYNAMIC_TYPE_HINGE = 5000, @@ -44,6 +46,9 @@ public: virtual ~EntityDynamicInterface() { } const QUuid& getID() const { return _id; } EntityDynamicType getType() const { return _type; } + + virtual void remapIDs(QHash& map) = 0; + virtual bool isAction() const { return false; } virtual bool isConstraint() const { return false; } virtual bool isReadyForAdd() const { return true; } diff --git a/libraries/entities/src/EntityTree.cpp b/libraries/entities/src/EntityTree.cpp index 76483d0786..457b7c21f6 100644 --- a/libraries/entities/src/EntityTree.cpp +++ b/libraries/entities/src/EntityTree.cpp @@ -25,6 +25,8 @@ #include "RecurseOctreeToMapOperator.h" #include "LogHandler.h" #include "EntityEditFilters.h" +#include "EntityDynamicFactoryInterface.h" + static const quint64 DELETED_ENTITIES_EXTRA_USECS_TO_CONSIDER = USECS_PER_MSEC * 50; const float EntityTree::DEFAULT_MAX_TMP_ENTITY_LIFETIME = 60 * 60; // 1 hour @@ -1527,6 +1529,48 @@ void EntityTree::pruneTree() { recurseTreeWithOperator(&theOperator); } + +QByteArray EntityTree::remapActionDataIDs(QByteArray actionData, QHash& map) { + if (actionData.isEmpty()) { + return actionData; + } + + QDataStream serializedActionsStream(actionData); + QVector serializedActions; + serializedActionsStream >> serializedActions; + + auto actionFactory = DependencyManager::get(); + + QHash remappedActions; + foreach(QByteArray serializedAction, serializedActions) { + QDataStream serializedActionStream(serializedAction); + EntityDynamicType actionType; + QUuid oldActionID; + serializedActionStream >> actionType; + serializedActionStream >> oldActionID; + EntityDynamicPointer action = actionFactory->factoryBA(nullptr, serializedAction); + if (action) { + action->remapIDs(map); + remappedActions[action->getID()] = action; + } + } + + QVector remappedSerializedActions; + + QHash::const_iterator i = remappedActions.begin(); + while (i != remappedActions.end()) { + EntityDynamicPointer action = i.value(); + QByteArray bytesForAction = action->serialize(); + remappedSerializedActions << bytesForAction; + i++; + } + + QByteArray result; + QDataStream remappedSerializedActionsStream(&result, QIODevice::WriteOnly); + remappedSerializedActionsStream << remappedSerializedActions; + return result; +} + QVector EntityTree::sendEntities(EntityEditPacketSender* packetSender, EntityTreePointer localTree, float x, float y, float z) { SendEntitiesOperationArgs args; @@ -1543,71 +1587,67 @@ QVector EntityTree::sendEntities(EntityEditPacketSender* packetSen }); packetSender->releaseQueuedMessages(); + // the values from map are used as the list of successfully "sent" entities. If some didn't actually make it, + // pull them out. Bogus entries could happen if part of the imported data makes some reference to an entity + // that isn't in the data being imported. + QHash::iterator i = map.begin(); + while (i != map.end()) { + EntityItemID newID = i.value(); + if (localTree->findEntityByEntityItemID(newID)) { + i++; + } else { + i = map.erase(i); + } + } + return map.values().toVector(); } bool EntityTree::sendEntitiesOperation(OctreeElementPointer element, void* extraData) { SendEntitiesOperationArgs* args = static_cast(extraData); EntityTreeElementPointer entityTreeElement = std::static_pointer_cast(element); - std::function getMapped = [&](EntityItemPointer& item) -> const EntityItemID { - EntityItemID oldID = item->getEntityItemID(); - if (args->map->contains(oldID)) { // Already been handled (e.g., as a parent of somebody that we've processed). - return args->map->value(oldID); - } - EntityItemID newID = QUuid::createUuid(); - args->map->insert(oldID, newID); + auto getMapped = [&args](EntityItemID oldID) { + if (oldID.isNull()) { + return EntityItemID(); + } + + QHash::iterator iter = args->map->find(oldID); + if (iter == args->map->end()) { + EntityItemID newID = QUuid::createUuid(); + args->map->insert(oldID, newID); + return newID; + } + return iter.value(); + }; + + entityTreeElement->forEachEntity([&args, &getMapped, &element](EntityItemPointer item) { + EntityItemID oldID = item->getEntityItemID(); + EntityItemID newID = getMapped(oldID); EntityItemProperties properties = item->getProperties(); + EntityItemID oldParentID = properties.getParentID(); if (oldParentID.isInvalidID()) { // no parent properties.setPosition(properties.getPosition() + args->root); } else { EntityItemPointer parentEntity = args->ourTree->findEntityByEntityItemID(oldParentID); if (parentEntity) { // map the parent - // Warning: (non-tail) recursion of getMapped could blow the call stack if the parent hierarchy is VERY deep. - properties.setParentID(getMapped(parentEntity)); + properties.setParentID(getMapped(parentEntity->getID())); // But do not add root offset in this case. } else { // Should not happen, but let's try to be helpful... item->globalizeProperties(properties, "Cannot find %3 parent of %2 %1", args->root); } } - if (!properties.getXNNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getXNNeighborID()); - if (neighborEntity) { - properties.setXNNeighborID(getMapped(neighborEntity)); - } - } - if (!properties.getXPNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getXPNeighborID()); - if (neighborEntity) { - properties.setXPNeighborID(getMapped(neighborEntity)); - } - } - if (!properties.getYNNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getYNNeighborID()); - if (neighborEntity) { - properties.setYNNeighborID(getMapped(neighborEntity)); - } - } - if (!properties.getYPNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getYPNeighborID()); - if (neighborEntity) { - properties.setYPNeighborID(getMapped(neighborEntity)); - } - } - if (!properties.getZNNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getZNNeighborID()); - if (neighborEntity) { - properties.setZNNeighborID(getMapped(neighborEntity)); - } - } - if (!properties.getZPNeighborID().isInvalidID()) { - auto neighborEntity = args->ourTree->findEntityByEntityItemID(properties.getZPNeighborID()); - if (neighborEntity) { - properties.setZPNeighborID(getMapped(neighborEntity)); - } - } + properties.setXNNeighborID(getMapped(properties.getXNNeighborID())); + properties.setXPNeighborID(getMapped(properties.getXPNeighborID())); + properties.setYNNeighborID(getMapped(properties.getYNNeighborID())); + properties.setYPNeighborID(getMapped(properties.getYPNeighborID())); + properties.setZNNeighborID(getMapped(properties.getZNNeighborID())); + properties.setZPNeighborID(getMapped(properties.getZPNeighborID())); + + QByteArray actionData = properties.getActionData(); + properties.setActionData(remapActionDataIDs(actionData, *args->map)); // set creation time to "now" for imported entities properties.setCreated(usecTimestampNow()); @@ -1623,13 +1663,13 @@ bool EntityTree::sendEntitiesOperation(OctreeElementPointer element, void* extra // also update the local tree instantly (note: this is not our tree, but an alternate tree) if (args->otherTree) { args->otherTree->withWriteLock([&] { - args->otherTree->addEntity(newID, properties); + EntityItemPointer entity = args->otherTree->addEntity(newID, properties); + entity->deserializeActions(); }); } return newID; - }; + }); - entityTreeElement->forEachEntity(getMapped); return true; } diff --git a/libraries/entities/src/EntityTree.h b/libraries/entities/src/EntityTree.h index 63f7bbfd66..d7e069b005 100644 --- a/libraries/entities/src/EntityTree.h +++ b/libraries/entities/src/EntityTree.h @@ -205,6 +205,8 @@ public: virtual void dumpTree() override; virtual void pruneTree() override; + static QByteArray remapActionDataIDs(QByteArray actionData, QHash& map); + QVector sendEntities(EntityEditPacketSender* packetSender, EntityTreePointer localTree, float x, float y, float z); diff --git a/libraries/physics/src/ObjectActionSpring.cpp b/libraries/physics/src/ObjectActionSpring.cpp index 8c73f43d42..0014d4e914 100644 --- a/libraries/physics/src/ObjectActionSpring.cpp +++ b/libraries/physics/src/ObjectActionSpring.cpp @@ -42,41 +42,6 @@ ObjectActionSpring::~ObjectActionSpring() { #endif } -SpatiallyNestablePointer ObjectActionSpring::getOther() { - SpatiallyNestablePointer other; - withWriteLock([&]{ - if (_otherID == QUuid()) { - // no other - return; - } - other = _other.lock(); - if (other && other->getID() == _otherID) { - // other is already up-to-date - return; - } - if (other) { - // we have a pointer to other, but it's wrong - other.reset(); - _other.reset(); - } - // we have an other-id but no pointer to other cached - QSharedPointer parentFinder = DependencyManager::get(); - if (!parentFinder) { - return; - } - EntityItemPointer ownerEntity = _ownerEntity.lock(); - if (!ownerEntity) { - return; - } - bool success; - _other = parentFinder->find(_otherID, success, ownerEntity->getParentTree()); - if (success) { - other = _other.lock(); - } - }); - return other; -} - bool ObjectActionSpring::getTarget(float deltaTimeStep, glm::quat& rotation, glm::vec3& position, glm::vec3& linearVelocity, glm::vec3& angularVelocity, float& linearTimeScale, float& angularTimeScale) { diff --git a/libraries/physics/src/ObjectActionSpring.h b/libraries/physics/src/ObjectActionSpring.h index 8f810d7956..83a65b36b4 100644 --- a/libraries/physics/src/ObjectActionSpring.h +++ b/libraries/physics/src/ObjectActionSpring.h @@ -47,10 +47,6 @@ protected: glm::vec3 _linearVelocityTarget; glm::vec3 _angularVelocityTarget; - EntityItemID _otherID; - SpatiallyNestableWeakPointer _other; - SpatiallyNestablePointer getOther(); - virtual bool prepareForSpringUpdate(btScalar deltaTimeStep); void serializeParameters(QDataStream& dataStream) const; diff --git a/libraries/physics/src/ObjectActionTractor.cpp b/libraries/physics/src/ObjectActionTractor.cpp new file mode 100644 index 0000000000..4bb5d850a9 --- /dev/null +++ b/libraries/physics/src/ObjectActionTractor.cpp @@ -0,0 +1,378 @@ +// +// ObjectActionTractor.cpp +// libraries/physics/src +// +// Created by Seth Alves 2015-5-8 +// Copyright 2017 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 "QVariantGLM.h" + +#include "ObjectActionTractor.h" + +#include "PhysicsLogging.h" + +const float TRACTOR_MAX_SPEED = 10.0f; +const float MAX_TRACTOR_TIMESCALE = 600.0f; // 10 min is a long time + +const uint16_t ObjectActionTractor::tractorVersion = 1; + + +ObjectActionTractor::ObjectActionTractor(const QUuid& id, EntityItemPointer ownerEntity) : + ObjectAction(DYNAMIC_TYPE_TRACTOR, id, ownerEntity), + _positionalTarget(glm::vec3(0.0f)), + _desiredPositionalTarget(glm::vec3(0.0f)), + _linearTimeScale(FLT_MAX), + _positionalTargetSet(true), + _rotationalTarget(glm::quat()), + _desiredRotationalTarget(glm::quat()), + _angularTimeScale(FLT_MAX), + _rotationalTargetSet(true) { + #if WANT_DEBUG + qCDebug(physics) << "ObjectActionTractor::ObjectActionTractor"; + #endif +} + +ObjectActionTractor::~ObjectActionTractor() { + #if WANT_DEBUG + qCDebug(physics) << "ObjectActionTractor::~ObjectActionTractor"; + #endif +} + +bool ObjectActionTractor::getTarget(float deltaTimeStep, glm::quat& rotation, glm::vec3& position, + glm::vec3& linearVelocity, glm::vec3& angularVelocity, + float& linearTimeScale, float& angularTimeScale) { + SpatiallyNestablePointer other = getOther(); + withReadLock([&]{ + linearTimeScale = _linearTimeScale; + angularTimeScale = _angularTimeScale; + + if (!_otherID.isNull()) { + if (other) { + rotation = _desiredRotationalTarget * other->getRotation(); + position = other->getRotation() * _desiredPositionalTarget + other->getPosition(); + } else { + // we should have an "other" but can't find it, so disable the tractor. + linearTimeScale = FLT_MAX; + angularTimeScale = FLT_MAX; + } + } else { + rotation = _desiredRotationalTarget; + position = _desiredPositionalTarget; + } + linearVelocity = glm::vec3(); + angularVelocity = glm::vec3(); + }); + return true; +} + +bool ObjectActionTractor::prepareForTractorUpdate(btScalar deltaTimeStep) { + auto ownerEntity = _ownerEntity.lock(); + if (!ownerEntity) { + return false; + } + + glm::quat rotation; + glm::vec3 position; + glm::vec3 linearVelocity; + glm::vec3 angularVelocity; + + bool linearValid = false; + int linearTractorCount = 0; + bool angularValid = false; + int angularTractorCount = 0; + + QList tractorDerivedActions; + tractorDerivedActions.append(ownerEntity->getActionsOfType(DYNAMIC_TYPE_TRACTOR)); + tractorDerivedActions.append(ownerEntity->getActionsOfType(DYNAMIC_TYPE_FAR_GRAB)); + tractorDerivedActions.append(ownerEntity->getActionsOfType(DYNAMIC_TYPE_HOLD)); + + foreach (EntityDynamicPointer action, tractorDerivedActions) { + std::shared_ptr tractorAction = std::static_pointer_cast(action); + glm::quat rotationForAction; + glm::vec3 positionForAction; + glm::vec3 linearVelocityForAction; + glm::vec3 angularVelocityForAction; + float linearTimeScale; + float angularTimeScale; + bool success = tractorAction->getTarget(deltaTimeStep, + rotationForAction, positionForAction, + linearVelocityForAction, angularVelocityForAction, + linearTimeScale, angularTimeScale); + if (success) { + if (angularTimeScale < MAX_TRACTOR_TIMESCALE) { + angularValid = true; + angularTractorCount++; + angularVelocity += angularVelocityForAction; + if (tractorAction.get() == this) { + // only use the rotation for this action + rotation = rotationForAction; + } + } + + if (linearTimeScale < MAX_TRACTOR_TIMESCALE) { + linearValid = true; + linearTractorCount++; + position += positionForAction; + linearVelocity += linearVelocityForAction; + } + } + } + + if ((angularValid && angularTractorCount > 0) || (linearValid && linearTractorCount > 0)) { + withWriteLock([&]{ + if (linearValid && linearTractorCount > 0) { + position /= linearTractorCount; + linearVelocity /= linearTractorCount; + _positionalTarget = position; + _linearVelocityTarget = linearVelocity; + _positionalTargetSet = true; + _active = true; + } + if (angularValid && angularTractorCount > 0) { + angularVelocity /= angularTractorCount; + _rotationalTarget = rotation; + _angularVelocityTarget = angularVelocity; + _rotationalTargetSet = true; + _active = true; + } + }); + } + + return linearValid || angularValid; +} + + +void ObjectActionTractor::updateActionWorker(btScalar deltaTimeStep) { + if (!prepareForTractorUpdate(deltaTimeStep)) { + return; + } + + withReadLock([&]{ + auto ownerEntity = _ownerEntity.lock(); + if (!ownerEntity) { + return; + } + + void* physicsInfo = ownerEntity->getPhysicsInfo(); + if (!physicsInfo) { + return; + } + ObjectMotionState* motionState = static_cast(physicsInfo); + btRigidBody* rigidBody = motionState->getRigidBody(); + if (!rigidBody) { + qCDebug(physics) << "ObjectActionTractor::updateActionWorker no rigidBody"; + return; + } + + if (_linearTimeScale < MAX_TRACTOR_TIMESCALE) { + btVector3 targetVelocity(0.0f, 0.0f, 0.0f); + btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget); + float offsetLength = offset.length(); + if (offsetLength > FLT_EPSILON) { + float speed = glm::min(offsetLength / _linearTimeScale, TRACTOR_MAX_SPEED); + targetVelocity = (-speed / offsetLength) * offset; + if (speed > rigidBody->getLinearSleepingThreshold()) { + forceBodyNonStatic(); + rigidBody->activate(); + } + } + // this action is aggresively critically damped and defeats the current velocity + rigidBody->setLinearVelocity(targetVelocity); + } + + if (_angularTimeScale < MAX_TRACTOR_TIMESCALE) { + btVector3 targetVelocity(0.0f, 0.0f, 0.0f); + + btQuaternion bodyRotation = rigidBody->getOrientation(); + auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget)); + const float ALMOST_ONE = 0.99999f; + if (glm::abs(alignmentDot) < ALMOST_ONE) { + btQuaternion target = glmToBullet(_rotationalTarget); + if (alignmentDot < 0.0f) { + target = -target; + } + // if dQ is the incremental rotation that gets an object from Q0 to Q1 then: + // + // Q1 = dQ * Q0 + // + // solving for dQ gives: + // + // dQ = Q1 * Q0^ + btQuaternion deltaQ = target * bodyRotation.inverse(); + float speed = deltaQ.getAngle() / _angularTimeScale; + targetVelocity = speed * deltaQ.getAxis(); + if (speed > rigidBody->getAngularSleepingThreshold()) { + rigidBody->activate(); + } + } + // this action is aggresively critically damped and defeats the current velocity + rigidBody->setAngularVelocity(targetVelocity); + } + }); +} + +const float MIN_TIMESCALE = 0.1f; + + +bool ObjectActionTractor::updateArguments(QVariantMap arguments) { + glm::vec3 positionalTarget; + float linearTimeScale; + glm::quat rotationalTarget; + float angularTimeScale; + QUuid otherID; + + + bool needUpdate = false; + bool somethingChanged = ObjectDynamic::updateArguments(arguments); + withReadLock([&]{ + // targets are required, tractor-constants are optional + bool ok = true; + positionalTarget = EntityDynamicInterface::extractVec3Argument("tractor action", arguments, "targetPosition", ok, false); + if (!ok) { + positionalTarget = _desiredPositionalTarget; + } + ok = true; + linearTimeScale = EntityDynamicInterface::extractFloatArgument("tractor action", arguments, "linearTimeScale", ok, false); + if (!ok || linearTimeScale <= 0.0f) { + linearTimeScale = _linearTimeScale; + } + + ok = true; + rotationalTarget = EntityDynamicInterface::extractQuatArgument("tractor action", arguments, "targetRotation", ok, false); + if (!ok) { + rotationalTarget = _desiredRotationalTarget; + } + + ok = true; + angularTimeScale = + EntityDynamicInterface::extractFloatArgument("tractor action", arguments, "angularTimeScale", ok, false); + if (!ok) { + angularTimeScale = _angularTimeScale; + } + + ok = true; + otherID = QUuid(EntityDynamicInterface::extractStringArgument("tractor action", + arguments, "otherID", ok, false)); + if (!ok) { + otherID = _otherID; + } + + if (somethingChanged || + positionalTarget != _desiredPositionalTarget || + linearTimeScale != _linearTimeScale || + rotationalTarget != _desiredRotationalTarget || + angularTimeScale != _angularTimeScale || + otherID != _otherID) { + // something changed + needUpdate = true; + } + }); + + if (needUpdate) { + withWriteLock([&] { + _desiredPositionalTarget = positionalTarget; + _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale)); + _desiredRotationalTarget = rotationalTarget; + _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale)); + _otherID = otherID; + _active = true; + + auto ownerEntity = _ownerEntity.lock(); + if (ownerEntity) { + ownerEntity->setDynamicDataDirty(true); + ownerEntity->setDynamicDataNeedsTransmit(true); + } + }); + activateBody(); + } + + return true; +} + +QVariantMap ObjectActionTractor::getArguments() { + QVariantMap arguments = ObjectDynamic::getArguments(); + withReadLock([&] { + arguments["linearTimeScale"] = _linearTimeScale; + arguments["targetPosition"] = glmToQMap(_desiredPositionalTarget); + + arguments["targetRotation"] = glmToQMap(_desiredRotationalTarget); + arguments["angularTimeScale"] = _angularTimeScale; + + arguments["otherID"] = _otherID; + }); + return arguments; +} + +void ObjectActionTractor::serializeParameters(QDataStream& dataStream) const { + withReadLock([&] { + dataStream << _desiredPositionalTarget; + dataStream << _linearTimeScale; + dataStream << _positionalTargetSet; + dataStream << _desiredRotationalTarget; + dataStream << _angularTimeScale; + dataStream << _rotationalTargetSet; + dataStream << localTimeToServerTime(_expires); + dataStream << _tag; + dataStream << _otherID; + }); +} + +QByteArray ObjectActionTractor::serialize() const { + QByteArray serializedActionArguments; + QDataStream dataStream(&serializedActionArguments, QIODevice::WriteOnly); + + dataStream << DYNAMIC_TYPE_TRACTOR; + dataStream << getID(); + dataStream << ObjectActionTractor::tractorVersion; + + serializeParameters(dataStream); + + return serializedActionArguments; +} + +void ObjectActionTractor::deserializeParameters(QByteArray serializedArguments, QDataStream& dataStream) { + withWriteLock([&] { + dataStream >> _desiredPositionalTarget; + dataStream >> _linearTimeScale; + dataStream >> _positionalTargetSet; + + dataStream >> _desiredRotationalTarget; + dataStream >> _angularTimeScale; + dataStream >> _rotationalTargetSet; + + quint64 serverExpires; + dataStream >> serverExpires; + _expires = serverTimeToLocalTime(serverExpires); + + dataStream >> _tag; + + dataStream >> _otherID; + + _active = true; + }); +} + +void ObjectActionTractor::deserialize(QByteArray serializedArguments) { + QDataStream dataStream(serializedArguments); + + EntityDynamicType type; + dataStream >> type; + assert(type == getType()); + + QUuid id; + dataStream >> id; + assert(id == getID()); + + uint16_t serializationVersion; + dataStream >> serializationVersion; + if (serializationVersion != ObjectActionTractor::tractorVersion) { + assert(false); + return; + } + + deserializeParameters(serializedArguments, dataStream); +} diff --git a/libraries/physics/src/ObjectActionTractor.h b/libraries/physics/src/ObjectActionTractor.h new file mode 100644 index 0000000000..c629d84998 --- /dev/null +++ b/libraries/physics/src/ObjectActionTractor.h @@ -0,0 +1,56 @@ +// +// ObjectActionTractor.h +// libraries/physics/src +// +// Created by Seth Alves 2017-5-8 +// Copyright 2017 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 +// + +#ifndef hifi_ObjectActionTractor_h +#define hifi_ObjectActionTractor_h + +#include "ObjectAction.h" + +class ObjectActionTractor : public ObjectAction { +public: + ObjectActionTractor(const QUuid& id, EntityItemPointer ownerEntity); + virtual ~ObjectActionTractor(); + + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; + + virtual void updateActionWorker(float deltaTimeStep) override; + + virtual QByteArray serialize() const override; + virtual void deserialize(QByteArray serializedArguments) override; + + virtual bool getTarget(float deltaTimeStep, glm::quat& rotation, glm::vec3& position, + glm::vec3& linearVelocity, glm::vec3& angularVelocity, + float& linearTimeScale, float& angularTimeScale); + +protected: + static const uint16_t tractorVersion; + + glm::vec3 _positionalTarget; + glm::vec3 _desiredPositionalTarget; + float _linearTimeScale; + bool _positionalTargetSet; + + glm::quat _rotationalTarget; + glm::quat _desiredRotationalTarget; + float _angularTimeScale; + bool _rotationalTargetSet; + + glm::vec3 _linearVelocityTarget; + glm::vec3 _angularVelocityTarget; + + virtual bool prepareForTractorUpdate(btScalar deltaTimeStep); + + void serializeParameters(QDataStream& dataStream) const; + void deserializeParameters(QByteArray serializedArguments, QDataStream& dataStream); +}; + +#endif // hifi_ObjectActionTractor_h diff --git a/libraries/physics/src/ObjectConstraintBallSocket.cpp b/libraries/physics/src/ObjectConstraintBallSocket.cpp index 35f138e840..fb36a49e92 100644 --- a/libraries/physics/src/ObjectConstraintBallSocket.cpp +++ b/libraries/physics/src/ObjectConstraintBallSocket.cpp @@ -40,7 +40,7 @@ QList ObjectConstraintBallSocket::getRigidBodies() { result += getRigidBody(); QUuid otherEntityID; withReadLock([&]{ - otherEntityID = _otherEntityID; + otherEntityID = _otherID; }); if (!otherEntityID.isNull()) { result += getOtherRigidBody(otherEntityID); @@ -76,7 +76,7 @@ btTypedConstraint* ObjectConstraintBallSocket::getConstraint() { withReadLock([&]{ constraint = static_cast(_constraint); pivotInA = _pivotInA; - otherEntityID = _otherEntityID; + otherEntityID = _otherID; pivotInB = _pivotInB; }); if (constraint) { @@ -136,7 +136,7 @@ bool ObjectConstraintBallSocket::updateArguments(QVariantMap arguments) { otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("ball-socket constraint", arguments, "otherEntityID", ok, false)); if (!ok) { - otherEntityID = _otherEntityID; + otherEntityID = _otherID; } ok = true; @@ -147,7 +147,7 @@ bool ObjectConstraintBallSocket::updateArguments(QVariantMap arguments) { if (somethingChanged || pivotInA != _pivotInA || - otherEntityID != _otherEntityID || + otherEntityID != _otherID || pivotInB != _pivotInB) { // something changed needUpdate = true; @@ -157,7 +157,7 @@ bool ObjectConstraintBallSocket::updateArguments(QVariantMap arguments) { if (needUpdate) { withWriteLock([&] { _pivotInA = pivotInA; - _otherEntityID = otherEntityID; + _otherID = otherEntityID; _pivotInB = pivotInB; _active = true; @@ -178,11 +178,9 @@ bool ObjectConstraintBallSocket::updateArguments(QVariantMap arguments) { QVariantMap ObjectConstraintBallSocket::getArguments() { QVariantMap arguments = ObjectDynamic::getArguments(); withReadLock([&] { - if (_constraint) { - arguments["pivot"] = glmToQMap(_pivotInA); - arguments["otherEntityID"] = _otherEntityID; - arguments["otherPivot"] = glmToQMap(_pivotInB); - } + arguments["pivot"] = glmToQMap(_pivotInA); + arguments["otherEntityID"] = _otherID; + arguments["otherPivot"] = glmToQMap(_pivotInB); }); return arguments; } @@ -200,7 +198,7 @@ QByteArray ObjectConstraintBallSocket::serialize() const { dataStream << _tag; dataStream << _pivotInA; - dataStream << _otherEntityID; + dataStream << _otherID; dataStream << _pivotInB; }); @@ -232,7 +230,7 @@ void ObjectConstraintBallSocket::deserialize(QByteArray serializedArguments) { dataStream >> _tag; dataStream >> _pivotInA; - dataStream >> _otherEntityID; + dataStream >> _otherID; dataStream >> _pivotInB; _active = true; diff --git a/libraries/physics/src/ObjectConstraintBallSocket.h b/libraries/physics/src/ObjectConstraintBallSocket.h index 9e0b942a6f..1c02fa736a 100644 --- a/libraries/physics/src/ObjectConstraintBallSocket.h +++ b/libraries/physics/src/ObjectConstraintBallSocket.h @@ -38,8 +38,6 @@ protected: void updateBallSocket(); glm::vec3 _pivotInA; - - EntityItemID _otherEntityID; glm::vec3 _pivotInB; }; diff --git a/libraries/physics/src/ObjectConstraintConeTwist.cpp b/libraries/physics/src/ObjectConstraintConeTwist.cpp index a0a9a5fe0c..15d9378443 100644 --- a/libraries/physics/src/ObjectConstraintConeTwist.cpp +++ b/libraries/physics/src/ObjectConstraintConeTwist.cpp @@ -15,14 +15,14 @@ #include "ObjectConstraintConeTwist.h" #include "PhysicsLogging.h" - -const uint16_t ObjectConstraintConeTwist::constraintVersion = 1; - +const uint16_t CONE_TWIST_VERSION_WITH_UNUSED_PAREMETERS = 1; +const uint16_t ObjectConstraintConeTwist::constraintVersion = 2; +const glm::vec3 DEFAULT_CONE_TWIST_AXIS(1.0f, 0.0f, 0.0f); ObjectConstraintConeTwist::ObjectConstraintConeTwist(const QUuid& id, EntityItemPointer ownerEntity) : ObjectConstraint(DYNAMIC_TYPE_CONE_TWIST, id, ownerEntity), - _pivotInA(glm::vec3(0.0f)), - _axisInA(glm::vec3(0.0f)) + _axisInA(DEFAULT_CONE_TWIST_AXIS), + _axisInB(DEFAULT_CONE_TWIST_AXIS) { #if WANT_DEBUG qCDebug(physics) << "ObjectConstraintConeTwist::ObjectConstraintConeTwist"; @@ -40,7 +40,7 @@ QList ObjectConstraintConeTwist::getRigidBodies() { result += getRigidBody(); QUuid otherEntityID; withReadLock([&]{ - otherEntityID = _otherEntityID; + otherEntityID = _otherID; }); if (!otherEntityID.isNull()) { result += getOtherRigidBody(otherEntityID); @@ -56,18 +56,12 @@ void ObjectConstraintConeTwist::updateConeTwist() { float swingSpan1; float swingSpan2; float twistSpan; - float softness; - float biasFactor; - float relaxationFactor; withReadLock([&]{ constraint = static_cast(_constraint); swingSpan1 = _swingSpan1; swingSpan2 = _swingSpan2; twistSpan = _twistSpan; - softness = _softness; - biasFactor = _biasFactor; - relaxationFactor = _relaxationFactor; }); if (!constraint) { @@ -76,10 +70,7 @@ void ObjectConstraintConeTwist::updateConeTwist() { constraint->setLimit(swingSpan1, swingSpan2, - twistSpan, - softness, - biasFactor, - relaxationFactor); + twistSpan); } @@ -95,7 +86,7 @@ btTypedConstraint* ObjectConstraintConeTwist::getConstraint() { constraint = static_cast(_constraint); pivotInA = _pivotInA; axisInA = _axisInA; - otherEntityID = _otherEntityID; + otherEntityID = _otherID; pivotInB = _pivotInB; axisInB = _axisInB; }); @@ -109,11 +100,25 @@ btTypedConstraint* ObjectConstraintConeTwist::getConstraint() { return nullptr; } + if (glm::length(axisInA) < FLT_EPSILON) { + qCWarning(physics) << "cone-twist axis cannot be a zero vector"; + axisInA = DEFAULT_CONE_TWIST_AXIS; + } else { + axisInA = glm::normalize(axisInA); + } + if (!otherEntityID.isNull()) { // This coneTwist is between two entities... find the other rigid body. - glm::quat rotA = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); - glm::quat rotB = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInB)); + if (glm::length(axisInB) < FLT_EPSILON) { + qCWarning(physics) << "cone-twist axis cannot be a zero vector"; + axisInB = DEFAULT_CONE_TWIST_AXIS; + } else { + axisInB = glm::normalize(axisInB); + } + + glm::quat rotA = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInA); + glm::quat rotB = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInB); btTransform frameInA(glmToBullet(rotA), glmToBullet(pivotInA)); btTransform frameInB(glmToBullet(rotB), glmToBullet(pivotInB)); @@ -127,7 +132,7 @@ btTypedConstraint* ObjectConstraintConeTwist::getConstraint() { } else { // This coneTwist is between an entity and the world-frame. - glm::quat rot = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); + glm::quat rot = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInA); btTransform frameInA(glmToBullet(rot), glmToBullet(pivotInA)); @@ -157,9 +162,6 @@ bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { float swingSpan1; float swingSpan2; float twistSpan; - float softness; - float biasFactor; - float relaxationFactor; bool needUpdate = false; bool somethingChanged = ObjectDynamic::updateArguments(arguments); @@ -180,7 +182,7 @@ bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("coneTwist constraint", arguments, "otherEntityID", ok, false)); if (!ok) { - otherEntityID = _otherEntityID; + otherEntityID = _otherID; } ok = true; @@ -213,37 +215,15 @@ bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { twistSpan = _twistSpan; } - ok = true; - softness = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "softness", ok, false); - if (!ok) { - softness = _softness; - } - - ok = true; - biasFactor = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "biasFactor", ok, false); - if (!ok) { - biasFactor = _biasFactor; - } - - ok = true; - relaxationFactor = - EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "relaxationFactor", ok, false); - if (!ok) { - relaxationFactor = _relaxationFactor; - } - if (somethingChanged || pivotInA != _pivotInA || axisInA != _axisInA || - otherEntityID != _otherEntityID || + otherEntityID != _otherID || pivotInB != _pivotInB || axisInB != _axisInB || swingSpan1 != _swingSpan1 || swingSpan2 != _swingSpan2 || - twistSpan != _twistSpan || - softness != _softness || - biasFactor != _biasFactor || - relaxationFactor != _relaxationFactor) { + twistSpan != _twistSpan) { // something changed needUpdate = true; } @@ -253,15 +233,12 @@ bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { withWriteLock([&] { _pivotInA = pivotInA; _axisInA = axisInA; - _otherEntityID = otherEntityID; + _otherID = otherEntityID; _pivotInB = pivotInB; _axisInB = axisInB; _swingSpan1 = swingSpan1; _swingSpan2 = swingSpan2; _twistSpan = twistSpan; - _softness = softness; - _biasFactor = biasFactor; - _relaxationFactor = relaxationFactor; _active = true; @@ -281,19 +258,14 @@ bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) { QVariantMap ObjectConstraintConeTwist::getArguments() { QVariantMap arguments = ObjectDynamic::getArguments(); withReadLock([&] { - if (_constraint) { - arguments["pivot"] = glmToQMap(_pivotInA); - arguments["axis"] = glmToQMap(_axisInA); - arguments["otherEntityID"] = _otherEntityID; - arguments["otherPivot"] = glmToQMap(_pivotInB); - arguments["otherAxis"] = glmToQMap(_axisInB); - arguments["swingSpan1"] = _swingSpan1; - arguments["swingSpan2"] = _swingSpan2; - arguments["twistSpan"] = _twistSpan; - arguments["softness"] = _softness; - arguments["biasFactor"] = _biasFactor; - arguments["relaxationFactor"] = _relaxationFactor; - } + arguments["pivot"] = glmToQMap(_pivotInA); + arguments["axis"] = glmToQMap(_axisInA); + arguments["otherEntityID"] = _otherID; + arguments["otherPivot"] = glmToQMap(_pivotInB); + arguments["otherAxis"] = glmToQMap(_axisInB); + arguments["swingSpan1"] = _swingSpan1; + arguments["swingSpan2"] = _swingSpan2; + arguments["twistSpan"] = _twistSpan; }); return arguments; } @@ -312,15 +284,12 @@ QByteArray ObjectConstraintConeTwist::serialize() const { dataStream << _pivotInA; dataStream << _axisInA; - dataStream << _otherEntityID; + dataStream << _otherID; dataStream << _pivotInB; dataStream << _axisInB; dataStream << _swingSpan1; dataStream << _swingSpan2; dataStream << _twistSpan; - dataStream << _softness; - dataStream << _biasFactor; - dataStream << _relaxationFactor; }); return serializedConstraintArguments; @@ -339,7 +308,7 @@ void ObjectConstraintConeTwist::deserialize(QByteArray serializedArguments) { uint16_t serializationVersion; dataStream >> serializationVersion; - if (serializationVersion != ObjectConstraintConeTwist::constraintVersion) { + if (serializationVersion > ObjectConstraintConeTwist::constraintVersion) { assert(false); return; } @@ -352,15 +321,18 @@ void ObjectConstraintConeTwist::deserialize(QByteArray serializedArguments) { dataStream >> _pivotInA; dataStream >> _axisInA; - dataStream >> _otherEntityID; + dataStream >> _otherID; dataStream >> _pivotInB; dataStream >> _axisInB; dataStream >> _swingSpan1; dataStream >> _swingSpan2; dataStream >> _twistSpan; - dataStream >> _softness; - dataStream >> _biasFactor; - dataStream >> _relaxationFactor; + if (serializationVersion == CONE_TWIST_VERSION_WITH_UNUSED_PAREMETERS) { + float softness, biasFactor, relaxationFactor; + dataStream >> softness; + dataStream >> biasFactor; + dataStream >> relaxationFactor; + } _active = true; }); diff --git a/libraries/physics/src/ObjectConstraintConeTwist.h b/libraries/physics/src/ObjectConstraintConeTwist.h index 02297e2b91..ea8b2aadb6 100644 --- a/libraries/physics/src/ObjectConstraintConeTwist.h +++ b/libraries/physics/src/ObjectConstraintConeTwist.h @@ -40,16 +40,12 @@ protected: glm::vec3 _pivotInA; glm::vec3 _axisInA; - EntityItemID _otherEntityID; glm::vec3 _pivotInB; glm::vec3 _axisInB; float _swingSpan1 { TWO_PI }; float _swingSpan2 { TWO_PI };; float _twistSpan { TWO_PI };; - float _softness { 1.0f }; - float _biasFactor {0.3f }; - float _relaxationFactor { 1.0f }; }; #endif // hifi_ObjectConstraintConeTwist_h diff --git a/libraries/physics/src/ObjectConstraintHinge.cpp b/libraries/physics/src/ObjectConstraintHinge.cpp index cf91ca904b..dbadf433c3 100644 --- a/libraries/physics/src/ObjectConstraintHinge.cpp +++ b/libraries/physics/src/ObjectConstraintHinge.cpp @@ -16,7 +16,8 @@ #include "PhysicsLogging.h" -const uint16_t ObjectConstraintHinge::constraintVersion = 1; +const uint16_t HINGE_VERSION_WITH_UNUSED_PAREMETERS = 1; +const uint16_t ObjectConstraintHinge::constraintVersion = 2; const glm::vec3 DEFAULT_HINGE_AXIS(1.0f, 0.0f, 0.0f); ObjectConstraintHinge::ObjectConstraintHinge(const QUuid& id, EntityItemPointer ownerEntity) : @@ -40,7 +41,7 @@ QList ObjectConstraintHinge::getRigidBodies() { result += getRigidBody(); QUuid otherEntityID; withReadLock([&]{ - otherEntityID = _otherEntityID; + otherEntityID = _otherID; }); if (!otherEntityID.isNull()) { result += getOtherRigidBody(otherEntityID); @@ -56,25 +57,19 @@ void ObjectConstraintHinge::updateHinge() { glm::vec3 axisInA; float low; float high; - float softness; - float biasFactor; - float relaxationFactor; withReadLock([&]{ axisInA = _axisInA; constraint = static_cast(_constraint); low = _low; high = _high; - biasFactor = _biasFactor; - relaxationFactor = _relaxationFactor; - softness = _softness; }); if (!constraint) { return; } - constraint->setLimit(low, high, softness, biasFactor, relaxationFactor); + constraint->setLimit(low, high); } @@ -90,7 +85,7 @@ btTypedConstraint* ObjectConstraintHinge::getConstraint() { constraint = static_cast(_constraint); pivotInA = _pivotInA; axisInA = _axisInA; - otherEntityID = _otherEntityID; + otherEntityID = _otherID; pivotInB = _pivotInB; axisInB = _axisInB; }); @@ -159,9 +154,6 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) { glm::vec3 axisInB; float low; float high; - float softness; - float biasFactor; - float relaxationFactor; bool needUpdate = false; bool somethingChanged = ObjectDynamic::updateArguments(arguments); @@ -182,7 +174,7 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) { otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("hinge constraint", arguments, "otherEntityID", ok, false)); if (!ok) { - otherEntityID = _otherEntityID; + otherEntityID = _otherID; } ok = true; @@ -209,36 +201,14 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) { high = _high; } - ok = true; - softness = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "softness", ok, false); - if (!ok) { - softness = _softness; - } - - ok = true; - biasFactor = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "biasFactor", ok, false); - if (!ok) { - biasFactor = _biasFactor; - } - - ok = true; - relaxationFactor = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, - "relaxationFactor", ok, false); - if (!ok) { - relaxationFactor = _relaxationFactor; - } - if (somethingChanged || pivotInA != _pivotInA || axisInA != _axisInA || - otherEntityID != _otherEntityID || + otherEntityID != _otherID || pivotInB != _pivotInB || axisInB != _axisInB || low != _low || - high != _high || - softness != _softness || - biasFactor != _biasFactor || - relaxationFactor != _relaxationFactor) { + high != _high) { // something changed needUpdate = true; } @@ -248,14 +218,11 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) { withWriteLock([&] { _pivotInA = pivotInA; _axisInA = axisInA; - _otherEntityID = otherEntityID; + _otherID = otherEntityID; _pivotInB = pivotInB; _axisInB = axisInB; _low = low; _high = high; - _softness = softness; - _biasFactor = biasFactor; - _relaxationFactor = relaxationFactor; _active = true; @@ -275,18 +242,17 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) { QVariantMap ObjectConstraintHinge::getArguments() { QVariantMap arguments = ObjectDynamic::getArguments(); withReadLock([&] { + arguments["pivot"] = glmToQMap(_pivotInA); + arguments["axis"] = glmToQMap(_axisInA); + arguments["otherEntityID"] = _otherID; + arguments["otherPivot"] = glmToQMap(_pivotInB); + arguments["otherAxis"] = glmToQMap(_axisInB); + arguments["low"] = _low; + arguments["high"] = _high; if (_constraint) { - arguments["pivot"] = glmToQMap(_pivotInA); - arguments["axis"] = glmToQMap(_axisInA); - arguments["otherEntityID"] = _otherEntityID; - arguments["otherPivot"] = glmToQMap(_pivotInB); - arguments["otherAxis"] = glmToQMap(_axisInB); - arguments["low"] = _low; - arguments["high"] = _high; - arguments["softness"] = _softness; - arguments["biasFactor"] = _biasFactor; - arguments["relaxationFactor"] = _relaxationFactor; arguments["angle"] = static_cast(_constraint)->getHingeAngle(); // [-PI,PI] + } else { + arguments["angle"] = 0.0f; } }); return arguments; @@ -303,14 +269,11 @@ QByteArray ObjectConstraintHinge::serialize() const { withReadLock([&] { dataStream << _pivotInA; dataStream << _axisInA; - dataStream << _otherEntityID; + dataStream << _otherID; dataStream << _pivotInB; dataStream << _axisInB; dataStream << _low; dataStream << _high; - dataStream << _softness; - dataStream << _biasFactor; - dataStream << _relaxationFactor; dataStream << localTimeToServerTime(_expires); dataStream << _tag; @@ -332,7 +295,7 @@ void ObjectConstraintHinge::deserialize(QByteArray serializedArguments) { uint16_t serializationVersion; dataStream >> serializationVersion; - if (serializationVersion != ObjectConstraintHinge::constraintVersion) { + if (serializationVersion > ObjectConstraintHinge::constraintVersion) { assert(false); return; } @@ -340,14 +303,17 @@ void ObjectConstraintHinge::deserialize(QByteArray serializedArguments) { withWriteLock([&] { dataStream >> _pivotInA; dataStream >> _axisInA; - dataStream >> _otherEntityID; + dataStream >> _otherID; dataStream >> _pivotInB; dataStream >> _axisInB; dataStream >> _low; dataStream >> _high; - dataStream >> _softness; - dataStream >> _biasFactor; - dataStream >> _relaxationFactor; + if (serializationVersion == HINGE_VERSION_WITH_UNUSED_PAREMETERS) { + float softness, biasFactor, relaxationFactor; + dataStream >> softness; + dataStream >> biasFactor; + dataStream >> relaxationFactor; + } quint64 serverExpires; dataStream >> serverExpires; diff --git a/libraries/physics/src/ObjectConstraintHinge.h b/libraries/physics/src/ObjectConstraintHinge.h index 07ce8eb8a3..bb9505fbae 100644 --- a/libraries/physics/src/ObjectConstraintHinge.h +++ b/libraries/physics/src/ObjectConstraintHinge.h @@ -40,7 +40,6 @@ protected: glm::vec3 _pivotInA; glm::vec3 _axisInA; - EntityItemID _otherEntityID; glm::vec3 _pivotInB; glm::vec3 _axisInB; @@ -49,27 +48,9 @@ protected: // https://gamedev.stackexchange.com/questions/71436/what-are-the-parameters-for-bthingeconstraintsetlimit // - // softness: a negative measure of the friction that determines how much the hinge rotates for a given force. A high - // softness would make the hinge rotate easily like it's oiled then. - // biasFactor: an offset for the relaxed rotation of the hinge. It won't be right in the middle of the low and high angles - // anymore. 1.0f is the neural value. - // relaxationFactor: a measure of how much force is applied internally to bring the hinge in its central rotation. - // This is right in the middle of the low and high angles. For example, consider a western swing door. After - // walking through it will swing in both directions but at the end it stays right in the middle. - - // http://javadoc.jmonkeyengine.org/com/jme3/bullet/joints/HingeJoint.html - // - // _softness - the factor at which the velocity error correction starts operating, i.e. a softness of 0.9 means that - // the vel. corr starts at 90% of the limit range. - // _biasFactor - the magnitude of the position correction. It tells you how strictly the position error (drift) is - // corrected. - // _relaxationFactor - the rate at which velocity errors are corrected. This can be seen as the strength of the - // limits. A low value will make the the limits more spongy. - - - float _softness { 0.9f }; - float _biasFactor { 0.3f }; - float _relaxationFactor { 1.0f }; + // softness: unused + // biasFactor: unused + // relaxationFactor: unused }; #endif // hifi_ObjectConstraintHinge_h diff --git a/libraries/physics/src/ObjectConstraintSlider.cpp b/libraries/physics/src/ObjectConstraintSlider.cpp index d7d4df78af..3716960a6e 100644 --- a/libraries/physics/src/ObjectConstraintSlider.cpp +++ b/libraries/physics/src/ObjectConstraintSlider.cpp @@ -17,12 +17,12 @@ const uint16_t ObjectConstraintSlider::constraintVersion = 1; - +const glm::vec3 DEFAULT_SLIDER_AXIS(1.0f, 0.0f, 0.0f); ObjectConstraintSlider::ObjectConstraintSlider(const QUuid& id, EntityItemPointer ownerEntity) : ObjectConstraint(DYNAMIC_TYPE_SLIDER, id, ownerEntity), - _pointInA(glm::vec3(0.0f)), - _axisInA(glm::vec3(0.0f)) + _axisInA(DEFAULT_SLIDER_AXIS), + _axisInB(DEFAULT_SLIDER_AXIS) { } @@ -34,7 +34,7 @@ QList ObjectConstraintSlider::getRigidBodies() { result += getRigidBody(); QUuid otherEntityID; withReadLock([&]{ - otherEntityID = _otherEntityID; + otherEntityID = _otherID; }); if (!otherEntityID.isNull()) { result += getOtherRigidBody(otherEntityID); @@ -77,7 +77,7 @@ btTypedConstraint* ObjectConstraintSlider::getConstraint() { constraint = static_cast(_constraint); pointInA = _pointInA; axisInA = _axisInA; - otherEntityID = _otherEntityID; + otherEntityID = _otherID; pointInB = _pointInB; axisInB = _axisInB; }); @@ -91,11 +91,25 @@ btTypedConstraint* ObjectConstraintSlider::getConstraint() { return nullptr; } + if (glm::length(axisInA) < FLT_EPSILON) { + qCWarning(physics) << "slider axis cannot be a zero vector"; + axisInA = DEFAULT_SLIDER_AXIS; + } else { + axisInA = glm::normalize(axisInA); + } + if (!otherEntityID.isNull()) { // This slider is between two entities... find the other rigid body. - glm::quat rotA = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); - glm::quat rotB = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInB)); + if (glm::length(axisInB) < FLT_EPSILON) { + qCWarning(physics) << "slider axis cannot be a zero vector"; + axisInB = DEFAULT_SLIDER_AXIS; + } else { + axisInB = glm::normalize(axisInB); + } + + glm::quat rotA = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA); + glm::quat rotB = glm::rotation(DEFAULT_SLIDER_AXIS, axisInB); btTransform frameInA(glmToBullet(rotA), glmToBullet(pointInA)); btTransform frameInB(glmToBullet(rotB), glmToBullet(pointInB)); @@ -109,7 +123,7 @@ btTypedConstraint* ObjectConstraintSlider::getConstraint() { } else { // This slider is between an entity and the world-frame. - glm::quat rot = glm::rotation(glm::vec3(1.0f, 0.0f, 0.0f), glm::normalize(axisInA)); + glm::quat rot = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA); btTransform frameInA(glmToBullet(rot), glmToBullet(pointInA)); @@ -160,7 +174,7 @@ bool ObjectConstraintSlider::updateArguments(QVariantMap arguments) { otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("slider constraint", arguments, "otherEntityID", ok, false)); if (!ok) { - otherEntityID = _otherEntityID; + otherEntityID = _otherID; } ok = true; @@ -202,7 +216,7 @@ bool ObjectConstraintSlider::updateArguments(QVariantMap arguments) { if (somethingChanged || pointInA != _pointInA || axisInA != _axisInA || - otherEntityID != _otherEntityID || + otherEntityID != _otherID || pointInB != _pointInB || axisInB != _axisInB || linearLow != _linearLow || @@ -218,7 +232,7 @@ bool ObjectConstraintSlider::updateArguments(QVariantMap arguments) { withWriteLock([&] { _pointInA = pointInA; _axisInA = axisInA; - _otherEntityID = otherEntityID; + _otherID = otherEntityID; _pointInB = pointInB; _axisInB = axisInB; _linearLow = linearLow; @@ -244,18 +258,21 @@ bool ObjectConstraintSlider::updateArguments(QVariantMap arguments) { QVariantMap ObjectConstraintSlider::getArguments() { QVariantMap arguments = ObjectDynamic::getArguments(); withReadLock([&] { + arguments["point"] = glmToQMap(_pointInA); + arguments["axis"] = glmToQMap(_axisInA); + arguments["otherEntityID"] = _otherID; + arguments["otherPoint"] = glmToQMap(_pointInB); + arguments["otherAxis"] = glmToQMap(_axisInB); + arguments["linearLow"] = _linearLow; + arguments["linearHigh"] = _linearHigh; + arguments["angularLow"] = _angularLow; + arguments["angularHigh"] = _angularHigh; if (_constraint) { - arguments["point"] = glmToQMap(_pointInA); - arguments["axis"] = glmToQMap(_axisInA); - arguments["otherEntityID"] = _otherEntityID; - arguments["otherPoint"] = glmToQMap(_pointInB); - arguments["otherAxis"] = glmToQMap(_axisInB); - arguments["linearLow"] = _linearLow; - arguments["linearHigh"] = _linearHigh; - arguments["angularLow"] = _angularLow; - arguments["angularHigh"] = _angularHigh; arguments["linearPosition"] = static_cast(_constraint)->getLinearPos(); arguments["angularPosition"] = static_cast(_constraint)->getAngularPos(); + } else { + arguments["linearPosition"] = 0.0f; + arguments["angularPosition"] = 0.0f; } }); return arguments; @@ -275,7 +292,7 @@ QByteArray ObjectConstraintSlider::serialize() const { dataStream << _pointInA; dataStream << _axisInA; - dataStream << _otherEntityID; + dataStream << _otherID; dataStream << _pointInB; dataStream << _axisInB; dataStream << _linearLow; @@ -313,7 +330,7 @@ void ObjectConstraintSlider::deserialize(QByteArray serializedArguments) { dataStream >> _pointInA; dataStream >> _axisInA; - dataStream >> _otherEntityID; + dataStream >> _otherID; dataStream >> _pointInB; dataStream >> _axisInB; dataStream >> _linearLow; diff --git a/libraries/physics/src/ObjectConstraintSlider.h b/libraries/physics/src/ObjectConstraintSlider.h index d616b9954c..36ecca0a2c 100644 --- a/libraries/physics/src/ObjectConstraintSlider.h +++ b/libraries/physics/src/ObjectConstraintSlider.h @@ -40,7 +40,6 @@ protected: glm::vec3 _pointInA; glm::vec3 _axisInA; - EntityItemID _otherEntityID; glm::vec3 _pointInB; glm::vec3 _axisInB; diff --git a/libraries/physics/src/ObjectDynamic.cpp b/libraries/physics/src/ObjectDynamic.cpp index 3cb9f5b405..253739375a 100644 --- a/libraries/physics/src/ObjectDynamic.cpp +++ b/libraries/physics/src/ObjectDynamic.cpp @@ -24,6 +24,27 @@ ObjectDynamic::ObjectDynamic(EntityDynamicType type, const QUuid& id, EntityItem ObjectDynamic::~ObjectDynamic() { } +void ObjectDynamic::remapIDs(QHash& map) { + withWriteLock([&]{ + if (!_id.isNull()) { + // just force our ID to something new -- action IDs don't go into the map + _id = QUuid::createUuid(); + } + + if (!_otherID.isNull()) { + QHash::iterator iter = map.find(_otherID); + if (iter == map.end()) { + // not found, add it + QUuid oldOtherID = _otherID; + _otherID = QUuid::createUuid(); + map.insert(oldOtherID, _otherID); + } else { + _otherID = iter.value(); + } + } + }); +} + qint64 ObjectDynamic::getEntityServerClockSkew() const { auto nodeList = DependencyManager::get(); @@ -274,3 +295,38 @@ QList ObjectDynamic::getRigidBodies() { result += getRigidBody(); return result; } + +SpatiallyNestablePointer ObjectDynamic::getOther() { + SpatiallyNestablePointer other; + withWriteLock([&]{ + if (_otherID == QUuid()) { + // no other + return; + } + other = _other.lock(); + if (other && other->getID() == _otherID) { + // other is already up-to-date + return; + } + if (other) { + // we have a pointer to other, but it's wrong + other.reset(); + _other.reset(); + } + // we have an other-id but no pointer to other cached + QSharedPointer parentFinder = DependencyManager::get(); + if (!parentFinder) { + return; + } + EntityItemPointer ownerEntity = _ownerEntity.lock(); + if (!ownerEntity) { + return; + } + bool success; + _other = parentFinder->find(_otherID, success, ownerEntity->getParentTree()); + if (success) { + other = _other.lock(); + } + }); + return other; +} diff --git a/libraries/physics/src/ObjectDynamic.h b/libraries/physics/src/ObjectDynamic.h index dcd0103a55..582f47db77 100644 --- a/libraries/physics/src/ObjectDynamic.h +++ b/libraries/physics/src/ObjectDynamic.h @@ -29,6 +29,8 @@ public: ObjectDynamic(EntityDynamicType type, const QUuid& id, EntityItemPointer ownerEntity); virtual ~ObjectDynamic(); + virtual void remapIDs(QHash& map) override; + virtual void removeFromSimulation(EntitySimulationPointer simulation) const override; virtual EntityItemWeakPointer getOwnerEntity() const override { return _ownerEntity; } virtual void setOwnerEntity(const EntityItemPointer ownerEntity) override { _ownerEntity = ownerEntity; } @@ -67,6 +69,10 @@ protected: QString _tag; quint64 _expires { 0 }; // in seconds since epoch + EntityItemID _otherID; + SpatiallyNestableWeakPointer _other; + SpatiallyNestablePointer getOther(); + private: qint64 getEntityServerClockSkew() const; };