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

@@ -15,6 +15,7 @@
 #include <avatar/AvatarActionFarGrab.h>
 #include <ObjectActionOffset.h>
 #include <ObjectActionSpring.h>
+#include <ObjectActionTractor.h>
 #include <ObjectActionTravelOriented.h>
 #include <ObjectConstraintHinge.h>
 #include <ObjectConstraintSlider.h>
@@ -34,6 +35,8 @@ EntityDynamicPointer interfaceDynamicFactory(EntityDynamicType type, const QUuid
             return std::make_shared<ObjectActionOffset>(id, ownerEntity);
         case DYNAMIC_TYPE_SPRING:
             return std::make_shared<ObjectActionSpring>(id, ownerEntity);
+        case DYNAMIC_TYPE_TRACTOR:
+            return std::make_shared<ObjectActionTractor>(id, ownerEntity);
         case DYNAMIC_TYPE_HOLD:
             return std::make_shared<AvatarActionHold>(id, ownerEntity);
         case DYNAMIC_TYPE_TRAVEL_ORIENTED:

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;
     }

interface/src/avatar/AvatarActionFarGrab.h

@@ -13,9 +13,9 @@
 #define hifi_AvatarActionFarGrab_h
 
 #include <EntityItem.h>
-#include <ObjectActionSpring.h>
+#include <ObjectActionTractor.h>
 
-class AvatarActionFarGrab : public ObjectActionSpring {
+class AvatarActionFarGrab : public ObjectActionTractor {
 public:
     AvatarActionFarGrab(const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~AvatarActionFarGrab();

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);
     }
 }
 

interface/src/avatar/AvatarActionHold.h

@@ -16,12 +16,12 @@
 
 #include <EntityItem.h>
 #include <AnimPose.h>
-#include <ObjectActionSpring.h>
+#include <ObjectActionTractor.h>
 
 #include "avatar/MyAvatar.h"
 
 
-class AvatarActionHold : public ObjectActionSpring {
+class AvatarActionHold : public ObjectActionTractor {
 public:
     AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~AvatarActionHold();

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:

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,
 };
 
 

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<EntityDynamicPointer> 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<ObjectActionTractor> tractorAction = std::static_pointer_cast<ObjectActionTractor>(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<ObjectMotionState*>(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);
+}

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