From 204c3d839e487775542ffa3adf1d1b4aa6fb9cf9 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Sun, 29 Nov 2015 10:17:21 -0800
Subject: [PATCH] added localToWorld calls, put locks around access to
 _children hashtable

---
 libraries/shared/src/SpatiallyNestable.cpp | 53 +++++++++++++++++++---
 libraries/shared/src/SpatiallyNestable.h   |  6 +++
 2 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 87ef7565fb..f5d05a77ad 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -76,11 +76,15 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer() const {
 }
 
 void SpatiallyNestable::beParentOfChild(SpatiallyNestablePointer newChild) const {
-    _children[newChild->getID()] = newChild;
+    _childrenLock.withWriteLock([&] {
+        _children[newChild->getID()] = newChild;
+    });
 }
 
 void SpatiallyNestable::forgetChild(SpatiallyNestablePointer newChild) const {
-    _children.remove(newChild->getID());
+    _childrenLock.withWriteLock([&] {
+        _children.remove(newChild->getID());
+    });
 }
 
 void SpatiallyNestable::setParentID(const QUuid& parentID) {
@@ -110,6 +114,39 @@ glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation) {
     return result.getRotation();
 }
 
+glm::vec3 SpatiallyNestable::localToWorld(const glm::vec3& position, QUuid parentID, int parentJointIndex) {
+    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();
+    auto parentWP = parentFinder->find(parentID);
+    auto parent = parentWP.lock();
+    Transform parentTransform;
+    if (parent) {
+        parentTransform = parent->getTransform(parentJointIndex);
+        parentTransform.setScale(1.0f);
+    }
+    Transform positionTransform;
+    positionTransform.setTranslation(position);
+    Transform result;
+    Transform::mult(result, parentTransform, positionTransform);
+    return result.getTranslation();
+}
+
+glm::quat SpatiallyNestable::localToWorld(const glm::quat& orientation, QUuid parentID, int parentJointIndex) {
+    QSharedPointer<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();
+    auto parentWP = parentFinder->find(parentID);
+    auto parent = parentWP.lock();
+    Transform parentTransform;
+    if (parent) {
+        parentTransform = parent->getTransform(parentJointIndex);
+        parentTransform.setScale(1.0f);
+    }
+    Transform orientationTransform;
+    orientationTransform.setRotation(orientation);
+    Transform result;
+    Transform::mult(result, parentTransform, orientationTransform);
+    return result.getRotation();
+}
+
+
 const glm::vec3& SpatiallyNestable::getPosition() const {
     Transform parentTransformDescaled = getParentTransform();
     glm::mat4 parentMat;
@@ -223,12 +260,14 @@ void SpatiallyNestable::setLocalScale(const glm::vec3& scale) {
 
 QList<SpatiallyNestablePointer> SpatiallyNestable::getChildren() const {
     QList<SpatiallyNestablePointer> children;
-    foreach (SpatiallyNestableWeakPointer childWP, _children.values()) {
-        SpatiallyNestablePointer child = childWP.lock();
-        if (child) {
-            children << child;
+    _childrenLock.withReadLock([&] {
+        foreach (SpatiallyNestableWeakPointer childWP, _children.values()) {
+            SpatiallyNestablePointer child = childWP.lock();
+            if (child) {
+                children << child;
+            }
         }
-    }
+    });
     return children;
 }
 
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index 30602d26b4..74d9cc9a95 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -16,6 +16,7 @@
 
 #include "Transform.h"
 #include "SpatialParentFinder.h"
+#include "shared/ReadWriteLockable.h"
 
 
 class SpatiallyNestable;
@@ -49,6 +50,9 @@ public:
     glm::vec3 worldToLocal(const glm::vec3& position);
     glm::quat worldToLocal(const glm::quat& orientation);
 
+    static glm::vec3 localToWorld(const glm::vec3& position, QUuid parentID, int parentJointIndex);
+    static glm::quat localToWorld(const glm::quat& orientation, QUuid parentID, int parentJointIndex);
+
     // world frame
     virtual const Transform& getTransform() const;
     virtual void setTransform(const Transform& transform);
@@ -101,6 +105,8 @@ protected:
 
     virtual void beParentOfChild(SpatiallyNestablePointer newChild) const;
     virtual void forgetChild(SpatiallyNestablePointer newChild) const;
+
+    mutable ReadWriteLockable _childrenLock;
     mutable QHash<QUuid, SpatiallyNestableWeakPointer> _children;
 
     virtual void parentChanged() {} // called when parent pointer is updated