////  SpatiallyNestable.cpp//  libraries/shared/src/////  Created by Seth Alves on 2015-10-18//  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 <QQueue> #include "DependencyManager.h"#include "SpatiallyNestable.h" const float defaultAACubeSize = 1.0f;const int maxParentingChain = 30; SpatiallyNestable::SpatiallyNestable(NestableType nestableType, QUuid id) :    _nestableType(nestableType),    _id(id),    _transform() {    // set flags in _transform    _transform.setTranslation(glm::vec3(0.0f));    _transform.setRotation(glm::quat());} const QUuid SpatiallyNestable::getID() const {    QUuid result;    _idLock.withReadLock([&] {        result = _id;    });    return result;} void SpatiallyNestable::setID(const QUuid& id) {    _idLock.withWriteLock([&] {        _id = id;    });} const QUuid SpatiallyNestable::getParentID() const {    QUuid result;    _idLock.withReadLock([&] {        result = _parentID;    });    return result;} void SpatiallyNestable::setParentID(const QUuid& parentID) {    _idLock.withWriteLock([&] {        if (_parentID != parentID) {            _parentID = parentID;            _parentKnowsMe = false;        }    });} Transform SpatiallyNestable::getParentTransform(bool& success, int depth) const {    Transform result;    SpatiallyNestablePointer parent = getParentPointer(success);    if (!success) {        return result;    }    if (parent) {        Transform parentTransform = parent->getTransform(_parentJointIndex, success, depth + 1);        result = parentTransform.setScale(1.0f); // TODO: scaling    }    return result;} SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) const {    SpatiallyNestablePointer parent = _parent.lock();    QUuid parentID = getParentID(); // used for its locking     if (!parent && parentID.isNull()) {        // no parent        success = true;        return nullptr;    }     if (parent && parent->getID() == parentID) {        // parent pointer is up-to-date        if (!_parentKnowsMe) {            parent->beParentOfChild(getThisPointer());            _parentKnowsMe = true;        }        success = true;        return parent;    }     if (parent) {        // we have a parent pointer but our _parentID doesn't indicate this parent.        parent->forgetChild(getThisPointer());        _parentKnowsMe = false;        _parent.reset();    }     // we have a _parentID but no parent pointer, or our parent pointer was to the wrong thing    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return nullptr;    }    _parent = parentFinder->find(parentID, success, getParentTree());    if (!success) {        return nullptr;    }     parent = _parent.lock();    if (parent) {        parent->beParentOfChild(getThisPointer());        _parentKnowsMe = true;    }     success = (parent || parentID.isNull());    return parent;} void SpatiallyNestable::beParentOfChild(SpatiallyNestablePointer newChild) const {    _childrenLock.withWriteLock([&] {        _children[newChild->getID()] = newChild;    });} void SpatiallyNestable::forgetChild(SpatiallyNestablePointer newChild) const {    _childrenLock.withWriteLock([&] {        _children.remove(newChild->getID());    });} void SpatiallyNestable::setParentJointIndex(quint16 parentJointIndex) {    _parentJointIndex = parentJointIndex;} glm::vec3 SpatiallyNestable::worldToLocal(const glm::vec3& position,                                          const QUuid& parentID, int parentJointIndex,                                          bool& success) {    Transform result;    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return glm::vec3(0.0f);    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return glm::vec3(0.0f);    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return glm::vec3(0.0f);    }     if (parent) {        parentTransform = parent->getTransform(parentJointIndex, success);        if (!success) {            return glm::vec3(0.0f);        }        parentTransform.setScale(1.0f); // TODO: scale    }    success = true;     Transform positionTransform;    positionTransform.setTranslation(position);    Transform myWorldTransform;    Transform::mult(myWorldTransform, parentTransform, positionTransform);    myWorldTransform.setTranslation(position);    Transform::inverseMult(result, parentTransform, myWorldTransform);    return result.getTranslation();} glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation,                                          const QUuid& parentID, int parentJointIndex,                                          bool& success) {    Transform result;    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return glm::quat();    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return glm::quat();    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return glm::quat();    }     if (parent) {        parentTransform = parent->getTransform(parentJointIndex, success);        if (!success) {            return glm::quat();        }        parentTransform.setScale(1.0f); // TODO: scale    }    success = true;     Transform orientationTransform;    orientationTransform.setRotation(orientation);    Transform myWorldTransform;    Transform::mult(myWorldTransform, parentTransform, orientationTransform);    myWorldTransform.setRotation(orientation);    Transform::inverseMult(result, parentTransform, myWorldTransform);    return result.getRotation();} glm::vec3 SpatiallyNestable::localToWorld(const glm::vec3& position,                                          const QUuid& parentID, int parentJointIndex,                                          bool& success) {    Transform result;    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return glm::vec3(0.0f);    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return glm::vec3(0.0f);    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return glm::vec3(0.0f);    }     if (parent) {        parentTransform = parent->getTransform(parentJointIndex, success);        if (!success) {            return glm::vec3(0.0f);        }        parentTransform.setScale(1.0f); // TODO: scale    }    success = true;     Transform positionTransform;    positionTransform.setTranslation(position);    Transform::mult(result, parentTransform, positionTransform);    return result.getTranslation();} glm::quat SpatiallyNestable::localToWorld(const glm::quat& orientation,                                          const QUuid& parentID, int parentJointIndex,                                          bool& success) {    Transform result;    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return glm::quat();    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return glm::quat();    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return glm::quat();    }     if (parent) {        parentTransform = parent->getTransform(parentJointIndex, success);        if (!success) {            return glm::quat();        }        parentTransform.setScale(1.0f);    }    success = true;     Transform orientationTransform;    orientationTransform.setRotation(orientation);    Transform::mult(result, parentTransform, orientationTransform);    return result.getRotation();} glm::vec3 SpatiallyNestable::getPosition(bool& success) const {    return getTransform(success).getTranslation();} glm::vec3 SpatiallyNestable::getPosition() const {    bool success;    auto result = getPosition(success);    #ifdef WANT_DEBUG    if (!success) {        qDebug() << "Warning -- getPosition failed" << getID();    }    #endif    return result;} glm::vec3 SpatiallyNestable::getPosition(int jointIndex, bool& success) const {    return getTransform(jointIndex, success).getTranslation();} void SpatiallyNestable::setPosition(const glm::vec3& position, bool& success, bool tellPhysics) {    // guard against introducing NaN into the transform    if (isNaN(position)) {        success = false;        return;    }    Transform parentTransform = getParentTransform(success);    Transform myWorldTransform;    _transformLock.withWriteLock([&] {        Transform::mult(myWorldTransform, parentTransform, _transform);        myWorldTransform.setTranslation(position);        Transform::inverseMult(_transform, parentTransform, myWorldTransform);    });    if (success) {        locationChanged(tellPhysics);    } else {        qDebug() << "setPosition failed for" << getID();    }} void SpatiallyNestable::setPosition(const glm::vec3& position) {    bool success;    setPosition(position, success);    #ifdef WANT_DEBUG    if (!success) {        qDebug() << "Warning -- setPosition failed" << getID();    }    #endif} glm::quat SpatiallyNestable::getOrientation(bool& success) const {    return getTransform(success).getRotation();} glm::quat SpatiallyNestable::getOrientation() const {    bool success;    auto result = getOrientation(success);    #ifdef WANT_DEBUG    if (!success) {        qDebug() << "Warning -- getOrientation failed" << getID();    }    #endif    return result;} glm::quat SpatiallyNestable::getOrientation(int jointIndex, bool& success) const {    return getTransform(jointIndex, success).getRotation();} void SpatiallyNestable::setOrientation(const glm::quat& orientation, bool& success, bool tellPhysics) {    // guard against introducing NaN into the transform    if (isNaN(orientation)) {        success = false;        return;    }     Transform parentTransform = getParentTransform(success);    Transform myWorldTransform;    _transformLock.withWriteLock([&] {        Transform::mult(myWorldTransform, parentTransform, _transform);        myWorldTransform.setRotation(orientation);        Transform::inverseMult(_transform, parentTransform, myWorldTransform);    });    if (success) {        locationChanged(tellPhysics);    }} void SpatiallyNestable::setOrientation(const glm::quat& orientation) {    bool success;    setOrientation(orientation, success);    #ifdef WANT_DEBUG    if (!success) {        qDebug() << "Warning -- setOrientation failed" << getID();    }    #endif} glm::vec3 SpatiallyNestable::getVelocity(bool& success) const {    glm::vec3 result;    Transform parentTransform = getParentTransform(success);    if (!success) {        return result;    }    glm::vec3 parentVelocity = getParentVelocity(success);    if (!success) {        return result;    }    _velocityLock.withReadLock([&] {        // TODO: take parent angularVelocity into account.        result = parentVelocity + parentTransform.getRotation() * _velocity;    });    return result;} glm::vec3 SpatiallyNestable::getVelocity() const {    bool success;    glm::vec3 result = getVelocity(success);    if (!success) {        qDebug() << "Warning -- setVelocity failed" << getID();    }    return result;} void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {    glm::vec3 parentVelocity = getParentVelocity(success);    Transform parentTransform = getParentTransform(success);    _velocityLock.withWriteLock([&] {        // HACK: until we are treating _velocity the same way we treat _position (meaning,        // _velocity is a vs parent value and any request for a world-frame velocity must        // be computed), do this to avoid equipped (parenting-grabbed) things from drifting.        // turning a zero velocity into a non-zero _velocity (because the avatar is moving)        // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity,        // which causes it to drift from the hand.        if (hasAncestorOfType(NestableType::Avatar)) {            _velocity = velocity;        } else {            // TODO: take parent angularVelocity into account.            _velocity = glm::inverse(parentTransform.getRotation()) * (velocity - parentVelocity);        }    });} void SpatiallyNestable::setVelocity(const glm::vec3& velocity) {    bool success;    setVelocity(velocity, success);    if (!success) {        qDebug() << "Warning -- setVelocity failed" << getID();    }} glm::vec3 SpatiallyNestable::getParentVelocity(bool& success) const {    glm::vec3 result;    SpatiallyNestablePointer parent = getParentPointer(success);    if (!success) {        return result;    }    if (parent) {        result = parent->getVelocity(success);    }    return result;} glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const {    glm::vec3 result;    Transform parentTransform = getParentTransform(success);    if (!success) {        return result;    }    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);    if (!success) {        return result;    }    _angularVelocityLock.withReadLock([&] {        result = parentAngularVelocity + parentTransform.getRotation() * _angularVelocity;    });    return result;} glm::vec3 SpatiallyNestable::getAngularVelocity() const {    bool success;    glm::vec3 result = getAngularVelocity(success);    if (!success) {        qDebug() << "Warning -- getAngularVelocity failed" << getID();    }    return result;} void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, bool& success) {    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);    Transform parentTransform = getParentTransform(success);    _angularVelocityLock.withWriteLock([&] {        _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity);    });} void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity) {    bool success;    setAngularVelocity(angularVelocity, success);    if (!success) {        qDebug() << "Warning -- setAngularVelocity failed" << getID();    }} glm::vec3 SpatiallyNestable::getParentAngularVelocity(bool& success) const {    glm::vec3 result;    SpatiallyNestablePointer parent = getParentPointer(success);    if (!success) {        return result;    }    if (parent) {

Lines 1–500 of 905. Load more