////  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 "SpatiallyNestable.h" #include <QtCore/QSharedPointer> #include <queue> #include "DependencyManager.h"#include "SharedUtil.h"#include "StreamUtils.h"#include "SharedLogging.h" const float defaultAACubeSize = 1.0f;const int MAX_PARENTING_CHAIN_SIZE = 30; SpatiallyNestable::SpatiallyNestable(NestableType nestableType, QUuid id) :    _id(id),    _nestableType(nestableType),    _transform() {    // set flags in _transform    _transform.setTranslation(glm::vec3(0.0f));    _transform.setRotation(glm::quat());    _transform.setScale(1.0f);    _scaleChanged = usecTimestampNow();    _translationChanged = usecTimestampNow();    _rotationChanged = usecTimestampNow();} SpatiallyNestable::~SpatiallyNestable() {    forEachChild([&](SpatiallyNestablePointer object) {        object->parentDeleted();    });} const QUuid SpatiallyNestable::getID() const {    QUuid result;    _idLock.withReadLock([&] {        result = _id;    });    return result;} void SpatiallyNestable::setID(const QUuid& id) {    // adjust the parentID of any children    forEachChild([&](SpatiallyNestablePointer object) {        object->setParentID(id);    });    _idLock.withWriteLock([&] {        _id = id;    });} const QUuid SpatiallyNestable::getParentID() const {    QUuid result;    _idLock.withReadLock([&] {        result = _parentID;    });    return result;} void SpatiallyNestable::setParentID(const QUuid& parentID) {    bool success = false;    auto parent = getParentPointer(success);    bool parentChanged = false;    _idLock.withWriteLock([&] {        if (_parentID != parentID) {            parentChanged = true;            _parentID = parentID;            _parentKnowsMe = false;        }    });     if (parentChanged && success && parent) {        parent->recalculateChildCauterization();    }     if (!_parentKnowsMe) {        success = false;        parent = getParentPointer(success);        if (success && parent) {            parent->updateQueryAACube();            parent->recalculateChildCauterization();        }    }} Transform SpatiallyNestable::getParentTransform(bool& success, int depth) const {    Transform result;    SpatiallyNestablePointer parent = getParentPointer(success);    if (!success) {        return result;    }    if (parent) {        result = parent->getJointTransform(_parentJointIndex, success, depth + 1);        if (getScalesWithParent()) {            result.setScale(parent->scaleForChildren());        }    }    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 ||                   (parentID == AVATAR_SELF_ID && parent->isMyAvatar()))) {        // parent pointer is up-to-date        if (!_parentKnowsMe) {            SpatialParentTree* parentTree = parent->getParentTree();            if (!parentTree || parentTree == getParentTree()) {                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) {         // it's possible for an entity with a parent of AVATAR_SELF_ID can be imported into a side-tree        // such as the clipboard's.  if this is the case, we don't want the parent to consider this a        // child.        SpatialParentTree* parentTree = parent->getParentTree();        if (!parentTree || parentTree == getParentTree()) {            parent->beParentOfChild(getThisPointer());            _parentKnowsMe = true;        }    }     success = (parent || parentID.isNull());    return parent;} void SpatiallyNestable::beParentOfChild(SpatiallyNestablePointer newChild) const {    _childrenLock.withWriteLock([&] {        _children[newChild->getID()] = newChild;    });    // Our QueryAACube will automatically be updated to include our new child} void SpatiallyNestable::forgetChild(SpatiallyNestablePointer newChild) const {    _childrenLock.withWriteLock([&] {        _children.remove(newChild->getID());    });    _queryAACubeSet = false; // We need to reset our queryAACube when we lose a child} void SpatiallyNestable::setParentJointIndex(quint16 parentJointIndex) {    _parentJointIndex = parentJointIndex;    bool success = false;    auto parent = getParentPointer(success);    if (success && parent) {        parent->recalculateChildCauterization();    }} glm::vec3 SpatiallyNestable::worldToLocal(const glm::vec3& position,                                          const QUuid& parentID, int parentJointIndex,                                          bool scalesWithParent, bool& success) {    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->getJointTransform(parentJointIndex, success);        if (!success) {            return glm::vec3(0.0f);        }        if (scalesWithParent) {            parentTransform.setScale(parent->scaleForChildren());        }    }    success = true;     Transform invParentTransform;    parentTransform.evalInverse(invParentTransform);    return invParentTransform.transform(position);} glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation,                                          const QUuid& parentID, int parentJointIndex,                                          bool scalesWithParent, bool& success) {    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->getJointTransform(parentJointIndex, success);        if (!success) {            return glm::quat();        }        if (scalesWithParent) {            parentTransform.setScale(parent->scaleForChildren());        }    }    success = true;     glm::quat invParentOrientation = glm::inverse(parentTransform.getRotation());    return invParentOrientation * orientation;} glm::vec3 SpatiallyNestable::worldToLocalVelocity(const glm::vec3& velocity, const QUuid& parentID,                                                  int parentJointIndex, bool scalesWithParent, bool& success) {    SpatiallyNestablePointer parent = SpatiallyNestable::findByID(parentID, success);    if (!success || !parent) {        return velocity;    }    Transform parentTransform = parent->getTransform(success);    if (!success) {        return velocity;    }    if (scalesWithParent) {        parentTransform.setScale(parent->scaleForChildren());    }    glm::vec3 parentVelocity = parent->getWorldVelocity(success);    if (!success) {        return velocity;    }     return glm::inverse(parentTransform.getRotation()) * (velocity - parentVelocity);} glm::vec3 SpatiallyNestable::worldToLocalAngularVelocity(const glm::vec3& angularVelocity, const QUuid& parentID,                                                         int parentJointIndex, bool scalesWithParent, bool& success) {    SpatiallyNestablePointer parent = SpatiallyNestable::findByID(parentID, success);    if (!success || !parent) {        return angularVelocity;    }    Transform parentTransform = parent->getTransform(success);    if (!success) {        return angularVelocity;    }    if (scalesWithParent) {        parentTransform.setScale(parent->scaleForChildren());    }     return glm::inverse(parentTransform.getRotation()) * angularVelocity;}  glm::vec3 SpatiallyNestable::worldToLocalDimensions(const glm::vec3& dimensions,                                                    const QUuid& parentID, int parentJointIndex,                                                    bool scalesWithParent, bool& success) {    if (!scalesWithParent) {        success = true;        return dimensions;    }     QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return dimensions;    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return dimensions;    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return dimensions;    }     success = true;    if (parent) {        return dimensions / parent->scaleForChildren();    }    return dimensions;} glm::vec3 SpatiallyNestable::localToWorld(const glm::vec3& position,                                          const QUuid& parentID, int parentJointIndex,                                          bool scalesWithParent,                                          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->getJointTransform(parentJointIndex, success);        if (!success) {            return glm::vec3(0.0f);        }        if (scalesWithParent) {            parentTransform.setScale(parent->scaleForChildren());        }    }    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 scalesWithParent,                                          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->getJointTransform(parentJointIndex, success);        if (!success) {            return glm::quat();        }        if (scalesWithParent) {            parentTransform.setScale(parent->scaleForChildren());        }    }    success = true;     Transform orientationTransform;    orientationTransform.setRotation(orientation);    Transform::mult(result, parentTransform, orientationTransform);    return result.getRotation();} glm::vec3 SpatiallyNestable::localToWorldVelocity(const glm::vec3& velocity, const QUuid& parentID,                                                  int parentJointIndex, bool scalesWithParent, bool& success) {    SpatiallyNestablePointer parent = SpatiallyNestable::findByID(parentID, success);    if (!success || !parent) {        return velocity;    }    Transform parentTransform = parent->getTransform(success);    if (!success) {        return velocity;    }    if (scalesWithParent) {        parentTransform.setScale(parent->scaleForChildren());    }    glm::vec3 parentVelocity = parent->getWorldVelocity(success);    if (!success) {        return velocity;    }     return parentVelocity + parentTransform.getRotation() * velocity;} glm::vec3 SpatiallyNestable::localToWorldAngularVelocity(const glm::vec3& angularVelocity, const QUuid& parentID,                                                         int parentJointIndex, bool scalesWithParent, bool& success) {    SpatiallyNestablePointer parent = SpatiallyNestable::findByID(parentID, success);    if (!success || !parent) {        return angularVelocity;    }    Transform parentTransform = parent->getTransform(success);    if (!success) {        return angularVelocity;    }    if (scalesWithParent) {        parentTransform.setScale(parent->scaleForChildren());    }    return parentTransform.getRotation() * angularVelocity;}  glm::vec3 SpatiallyNestable::localToWorldDimensions(const glm::vec3& dimensions,                                                    const QUuid& parentID, int parentJointIndex, bool scalesWithParent,                                                    bool& success) {    if (!scalesWithParent) {        success = true;        return dimensions;    }     Transform result;    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();    if (!parentFinder) {        success = false;        return dimensions;    }     Transform parentTransform;    auto parentWP = parentFinder->find(parentID, success);    if (!success) {        return dimensions;    }     auto parent = parentWP.lock();    if (!parentID.isNull() && !parent) {        success = false;        return dimensions;    }     success = true;    if (parent) {        return dimensions * parent->scaleForChildren();    }    return dimensions;} void SpatiallyNestable::setWorldTransform(const glm::vec3& position, const glm::quat& orientation) {    // guard against introducing NaN into the transform    if (isNaN(orientation) || isNaN(position)) {        return;    }     bool success = true;    Transform parentTransform = getParentTransform(success);    if (success) {        bool changed = false;

Lines 1–500 of 1509. Load more