From c0c5084149f9724a6dbab7e587e28d99e03bee4a Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Fri, 26 Feb 2016 14:05:02 -0800
Subject: [PATCH] handle parenting loops

---
 libraries/shared/src/SpatiallyNestable.cpp | 31 ++++++++++++++--------
 libraries/shared/src/SpatiallyNestable.h   |  6 ++---
 2 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 557b25d9b2..1a897021f1 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -15,6 +15,7 @@
 #include "SpatiallyNestable.h"
 
 const float defaultAACubeSize = 1.0f;
+const int maxParentingChain = 30;
 
 SpatiallyNestable::SpatiallyNestable(NestableType nestableType, QUuid id) :
     _nestableType(nestableType),
@@ -56,14 +57,14 @@ void SpatiallyNestable::setParentID(const QUuid& parentID) {
     });
 }
 
-Transform SpatiallyNestable::getParentTransform(bool& success) const {
+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);
+        Transform parentTransform = parent->getTransform(_parentJointIndex, success, depth + 1);
         result = parentTransform.setScale(1.0f); // TODO: scaling
     }
     return result;
@@ -393,11 +394,11 @@ void SpatiallyNestable::setOrientation(const glm::quat& orientation) {
 
 glm::vec3 SpatiallyNestable::getVelocity(bool& success) const {
     glm::vec3 result;
-    glm::vec3 parentVelocity = getParentVelocity(success);
+    Transform parentTransform = getParentTransform(success);
     if (!success) {
         return result;
     }
-    Transform parentTransform = getParentTransform(success);
+    glm::vec3 parentVelocity = getParentVelocity(success);
     if (!success) {
         return result;
     }
@@ -448,11 +449,11 @@ glm::vec3 SpatiallyNestable::getParentVelocity(bool& success) const {
 
 glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const {
     glm::vec3 result;
-    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
+    Transform parentTransform = getParentTransform(success);
     if (!success) {
         return result;
     }
-    Transform parentTransform = getParentTransform(success);
+    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
     if (!success) {
         return result;
     }
@@ -499,22 +500,30 @@ glm::vec3 SpatiallyNestable::getParentAngularVelocity(bool& success) const {
     return result;
 }
 
-const Transform SpatiallyNestable::getTransform(bool& success) const {
-    // return a world-space transform for this object's location
-    Transform parentTransform = getParentTransform(success);
+const Transform SpatiallyNestable::getTransform(bool& success, int depth) const {
     Transform result;
+    // return a world-space transform for this object's location
+    Transform parentTransform = getParentTransform(success, depth);
     _transformLock.withReadLock([&] {
         Transform::mult(result, parentTransform, _transform);
     });
     return result;
 }
 
-const Transform SpatiallyNestable::getTransform(int jointIndex, bool& success) const {
+const Transform SpatiallyNestable::getTransform(int jointIndex, bool& success, int depth) const {
     // this returns the world-space transform for this object.  It finds its parent's transform (which may
     // cause this object's parent to query its parent, etc) and multiplies this object's local transform onto it.
     Transform jointInWorldFrame;
 
-    Transform worldTransform = getTransform(success);
+    if (depth > maxParentingChain) {
+        success = false;
+        // someone created a loop.  break it...
+        qDebug() << "Parenting loop detected.";
+        getThisPointer()->setParentID(QUuid());
+        return jointInWorldFrame;
+    }
+
+    Transform worldTransform = getTransform(success, depth);
     worldTransform.setScale(1.0f); // TODO -- scale;
     if (!success) {
         return jointInWorldFrame;
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index 2a8976b38f..ee6cccc7a8 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -52,10 +52,10 @@ public:
     static glm::quat localToWorld(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success);
 
     // world frame
-    virtual const Transform getTransform(bool& success) const;
+    virtual const Transform getTransform(bool& success, int depth = 0) const;
     virtual void setTransform(const Transform& transform, bool& success);
 
-    virtual Transform getParentTransform(bool& success) const;
+    virtual Transform getParentTransform(bool& success, int depth = 0) const;
 
     virtual glm::vec3 getPosition(bool& success) const;
     virtual glm::vec3 getPosition() const;
@@ -92,7 +92,7 @@ public:
     virtual void setScale(const glm::vec3& scale);
 
     // get world-frame values for a specific joint
-    virtual const Transform getTransform(int jointIndex, bool& success) const;
+    virtual const Transform getTransform(int jointIndex, bool& success, int depth = 0) const;
     virtual glm::vec3 getPosition(int jointIndex, bool& success) const;
     virtual glm::vec3 getScale(int jointIndex) const;