////  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 uint16_t ObjectActionSpring::springVersion = 1; ObjectActionSpring::ObjectActionSpring(EntityActionType type, QUuid id, EntityItemPointer ownerEntity) :    ObjectAction(type, id, ownerEntity) {    #if WANT_DEBUG    qDebug() << "ObjectActionSpring::ObjectActionSpring";    #endif} ObjectActionSpring::~ObjectActionSpring() {    #if WANT_DEBUG    qDebug() << "ObjectActionSpring::~ObjectActionSpring";    #endif} void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {    if (!tryLockForRead()) {        // don't risk hanging the thread running the physics simulation        qDebug() << "ObjectActionSpring::updateActionWorker lock failed";        return;    }     auto ownerEntity = _ownerEntity.lock();    if (!ownerEntity) {        return;    }     void* physicsInfo = ownerEntity->getPhysicsInfo();    if (!physicsInfo) {        unlock();        return;    }    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);    btRigidBody* rigidBody = motionState->getRigidBody();    if (!rigidBody) {        unlock();        qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";        return;    }     // handle the linear part    if (_positionalTargetSet) {        // check for NaN        if (_positionalTarget.x != _positionalTarget.x ||            _positionalTarget.y != _positionalTarget.y ||            _positionalTarget.z != _positionalTarget.z) {            qDebug() << "ObjectActionSpring::updateActionWorker -- target position includes NaN";            unlock();            lockForWrite();            _active = false;            unlock();            return;        }        glm::vec3 offset = _positionalTarget - bulletToGLM(rigidBody->getCenterOfMassPosition());        float offsetLength = glm::length(offset);        float speed = offsetLength / _linearTimeScale;         if (offsetLength > IGNORE_POSITION_DELTA) {            glm::vec3 newVelocity = glm::normalize(offset) * speed;            rigidBody->setLinearVelocity(glmToBullet(newVelocity));            rigidBody->activate();        } else {            rigidBody->setLinearVelocity(glmToBullet(glm::vec3(0.0f)));        }    }     // handle rotation    if (_rotationalTargetSet) {        if (_rotationalTarget.x != _rotationalTarget.x ||            _rotationalTarget.y != _rotationalTarget.y ||            _rotationalTarget.z != _rotationalTarget.z ||            _rotationalTarget.w != _rotationalTarget.w) {            qDebug() << "AvatarActionHold::updateActionWorker -- target rotation includes NaN";            unlock();            lockForWrite();            _active = false;            unlock();            return;        }         glm::quat bodyRotation = bulletToGLM(rigidBody->getOrientation());        // if qZero and qOne are too close to each other, we can get NaN for angle.        auto alignmentDot = glm::dot(bodyRotation, _rotationalTarget);        const float almostOne = 0.99999f;        if (glm::abs(alignmentDot) < almostOne) {            glm::quat target = _rotationalTarget;            if (alignmentDot < 0) {                target = -target;            }            glm::quat qZeroInverse = glm::inverse(bodyRotation);            glm::quat deltaQ = target * qZeroInverse;            glm::vec3 axis = glm::axis(deltaQ);            float angle = glm::angle(deltaQ);            assert(!isNaN(angle));            glm::vec3 newAngularVelocity = (angle / _angularTimeScale) * glm::normalize(axis);            rigidBody->setAngularVelocity(glmToBullet(newAngularVelocity));            rigidBody->activate();        } else {            rigidBody->setAngularVelocity(glmToBullet(glm::vec3(0.0f)));        }    }     unlock();}  bool ObjectActionSpring::updateArguments(QVariantMap arguments) {    // targets are required, spring-constants are optional    bool ptOk = true;    glm::vec3 positionalTarget =        EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ptOk, false);    bool pscOk = true;    float linearTimeScale =        EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", pscOk, false);    if (ptOk && pscOk && linearTimeScale <= 0.0f) {        qDebug() << "spring action -- linearTimeScale must be greater than zero.";        return false;    }     bool rtOk = true;    glm::quat rotationalTarget =        EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", rtOk, false);    bool rscOk = true;    float angularTimeScale =        EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", rscOk, false);     if (!ptOk && !rtOk) {        qDebug() << "spring action requires at least one of targetPosition or targetRotation argument";        return false;    }     lockForWrite();     _positionalTargetSet = _rotationalTargetSet = false;     if (ptOk) {        _positionalTarget = positionalTarget;        _positionalTargetSet = true;         if (pscOk) {            _linearTimeScale = linearTimeScale;        } else {            _linearTimeScale = 0.1f;        }    }     if (rtOk) {        _rotationalTarget = rotationalTarget;        _rotationalTargetSet = true;         if (rscOk) {            _angularTimeScale = angularTimeScale;        } else {            _angularTimeScale = 0.1f;        }    }     _active = true;    unlock();    return true;} QVariantMap ObjectActionSpring::getArguments() {    QVariantMap arguments;    lockForRead();     if (_positionalTargetSet) {        arguments["linearTimeScale"] = _linearTimeScale;        arguments["targetPosition"] = glmToQMap(_positionalTarget);    }     if (_rotationalTargetSet) {        arguments["targetRotation"] = glmToQMap(_rotationalTarget);        arguments["angularTimeScale"] = _angularTimeScale;    }     unlock();    return arguments;} QByteArray ObjectActionSpring::serialize() {    QByteArray ba;    QDataStream dataStream(&ba, QIODevice::WriteOnly);     dataStream << getType();    dataStream << getID();    dataStream << ObjectActionSpring::springVersion;     dataStream << _positionalTarget;    dataStream << _linearTimeScale;    dataStream << _positionalTargetSet;     dataStream << _rotationalTarget;    dataStream << _angularTimeScale;    dataStream << _rotationalTargetSet;     return ba;} void ObjectActionSpring::deserialize(QByteArray serializedArguments) {    QDataStream dataStream(serializedArguments);     EntityActionType type;    QUuid id;    uint16_t serializationVersion;     dataStream >> type;    assert(type == getType());    dataStream >> id;    assert(id == getID());    dataStream >> serializationVersion;    if (serializationVersion != ObjectActionSpring::springVersion) {        return;    }     dataStream >> _positionalTarget;    dataStream >> _linearTimeScale;    dataStream >> _positionalTargetSet;     dataStream >> _rotationalTarget;    dataStream >> _angularTimeScale;    dataStream >> _rotationalTargetSet;     _active = true;}