From de589a32f35fe2784bc96a577322c5e93dc7633e Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 8 May 2017 16:36:51 -0700 Subject: [PATCH] clone spring action into one called tractor and use tractor is most places. this frees us to fix the math in spring so that it's actually a spring --- interface/src/InterfaceDynamicFactory.cpp | 3 + interface/src/avatar/AvatarActionFarGrab.cpp | 6 +- interface/src/avatar/AvatarActionFarGrab.h | 4 +- interface/src/avatar/AvatarActionHold.cpp | 6 +- interface/src/avatar/AvatarActionHold.h | 4 +- .../entities/src/EntityDynamicInterface.cpp | 5 + .../entities/src/EntityDynamicInterface.h | 3 +- libraries/physics/src/ObjectActionTractor.cpp | 378 ++++++++++++++++++ libraries/physics/src/ObjectActionTractor.h | 56 +++ 9 files changed, 454 insertions(+), 11 deletions(-) create mode 100644 libraries/physics/src/ObjectActionTractor.cpp create mode 100644 libraries/physics/src/ObjectActionTractor.h diff --git a/interface/src/InterfaceDynamicFactory.cpp b/interface/src/InterfaceDynamicFactory.cpp index 4b9e3b96ed..e4b3c0b500 100644 --- a/interface/src/InterfaceDynamicFactory.cpp +++ b/interface/src/InterfaceDynamicFactory.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -34,6 +35,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 71b3bda534..c44cf17b6c 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; } @@ -143,6 +146,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 ef20f84da4..89147936bf 100644 --- a/libraries/entities/src/EntityDynamicInterface.h +++ b/libraries/entities/src/EntityDynamicInterface.h @@ -29,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, @@ -36,7 +37,7 @@ enum EntityDynamicType { DYNAMIC_TYPE_SLIDER = 7000, DYNAMIC_TYPE_BALL_SOCKET = 8000, DYNAMIC_TYPE_CONE_TWIST = 9000, - DYNAMIC_TYPE_MOTOR = 10000 + DYNAMIC_TYPE_MOTOR = 10000, }; 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