////  ObjectActionSpring.cpp//  libraries/physics/src////  Created by Seth Alves 2015-6-5//  Copyright 2015 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 "ObjectActionSpring.h" const float SPRING_MAX_SPEED = 10.0f; const uint16_t ObjectActionSpring::springVersion = 1;  ObjectActionSpring::ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity) :    ObjectAction(ACTION_TYPE_SPRING, id, ownerEntity),    _positionalTarget(glm::vec3(0.0f)),    _linearTimeScale(FLT_MAX),    _positionalTargetSet(true),    _rotationalTarget(glm::quat()),    _angularTimeScale(FLT_MAX),    _rotationalTargetSet(true) {    #if WANT_DEBUG    qDebug() << "ObjectActionSpring::ObjectActionSpring";    #endif} ObjectActionSpring::~ObjectActionSpring() {    #if WANT_DEBUG    qDebug() << "ObjectActionSpring::~ObjectActionSpring";    #endif} void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {    // don't risk hanging the thread running the physics simulation    auto lockResult = withTryReadLock([&]{        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) {            qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";            return;        }         const float MAX_TIMESCALE = 600.0f; // 10 min is a long time        if (_linearTimeScale < MAX_TIMESCALE) {            btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);            float offsetLength = offset.length();            btVector3 targetVelocity(0.0f, 0.0f, 0.0f);             if (offsetLength > 0) {                float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;                targetVelocity = (-speed / offsetLength) * offset;            }             // this action is aggresively critically damped and defeats the current velocity            rigidBody->setLinearVelocity(targetVelocity);        }         if (_angularTimeScale < MAX_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 angle = deltaQ.getAngle();                const float MIN_ANGLE = 1.0e-4f;                if (angle > MIN_ANGLE) {                    targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();                }            }            // this action is aggresively critically damped and defeats the current velocity            rigidBody->setAngularVelocity(targetVelocity);        }    });    if (!lockResult) {        qDebug() << "ObjectActionSpring::updateActionWorker lock failed";    }} const float MIN_TIMESCALE = 0.1f; bool ObjectActionSpring::updateArguments(QVariantMap arguments) {    if (!ObjectAction::updateArguments(arguments)) {        return false;    }    // targets are required, spring-constants are optional    bool ok = true;    glm::vec3 positionalTarget =        EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);    if (!ok) {        positionalTarget = _positionalTarget;    }    ok = true;    float linearTimeScale =        EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);    if (!ok || linearTimeScale <= 0.0f) {        linearTimeScale = _linearTimeScale;    }     ok = true;    glm::quat rotationalTarget =        EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);    if (!ok) {        rotationalTarget = _rotationalTarget;    }     ok = true;    float angularTimeScale =        EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);    if (!ok) {        angularTimeScale = _angularTimeScale;    }     if (positionalTarget != _positionalTarget            || linearTimeScale != _linearTimeScale            || rotationalTarget != _rotationalTarget            || angularTimeScale != _angularTimeScale) {        // something changed        withWriteLock([&] {            _positionalTarget = positionalTarget;            _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale));            _rotationalTarget = rotationalTarget;            _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));            _active = true;            activateBody();        });    }    return true;} QVariantMap ObjectActionSpring::getArguments() {    QVariantMap arguments = ObjectAction::getArguments();    withReadLock([&] {        arguments["linearTimeScale"] = _linearTimeScale;        arguments["targetPosition"] = glmToQMap(_positionalTarget);         arguments["targetRotation"] = glmToQMap(_rotationalTarget);        arguments["angularTimeScale"] = _angularTimeScale;    });    return arguments;} QByteArray ObjectActionSpring::serialize() const {    QByteArray serializedActionArguments;    QDataStream dataStream(&serializedActionArguments, QIODevice::WriteOnly);     dataStream << ACTION_TYPE_SPRING;    dataStream << getID();    dataStream << ObjectActionSpring::springVersion;     dataStream << _positionalTarget;    dataStream << _linearTimeScale;    dataStream << _positionalTargetSet;     dataStream << _rotationalTarget;    dataStream << _angularTimeScale;    dataStream << _rotationalTargetSet;     dataStream << _expires;    dataStream << _tag;     return serializedActionArguments;} void ObjectActionSpring::deserialize(QByteArray serializedArguments) {    QDataStream dataStream(serializedArguments);     EntityActionType type;    dataStream >> type;    assert(type == getType());     QUuid id;    dataStream >> id;    assert(id == getID());     uint16_t serializationVersion;    dataStream >> serializationVersion;    if (serializationVersion != ObjectActionSpring::springVersion) {        return;    }     dataStream >> _positionalTarget;    dataStream >> _linearTimeScale;    dataStream >> _positionalTargetSet;     dataStream >> _rotationalTarget;    dataStream >> _angularTimeScale;    dataStream >> _rotationalTargetSet;     dataStream >> _expires;    dataStream >> _tag;     _active = true;}