From fd9f1303e1eff5483777b4833c9bec9e567aa5cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Je=CC=81re=CC=81my=20Lacoche?= <jeremy.lacoche@orange.com>
Date: Fri, 7 Mar 2025 14:26:59 +0100
Subject: [PATCH] Add a method to compute world pose of a node if it has no
 target

---
 Editor/Scripts/Windows/GraphEditorWindow.cs | 75 ++++++++++++++++-----
 1 file changed, 58 insertions(+), 17 deletions(-)

diff --git a/Editor/Scripts/Windows/GraphEditorWindow.cs b/Editor/Scripts/Windows/GraphEditorWindow.cs
index e590a31..e1a8314 100644
--- a/Editor/Scripts/Windows/GraphEditorWindow.cs
+++ b/Editor/Scripts/Windows/GraphEditorWindow.cs
@@ -33,6 +33,7 @@ using UnityEngine.UIElements;
 using TMPro;
 using UnityEditor.Experimental.GraphView;
 using System.Drawing.Drawing2D;
+using System.Linq;
 
 namespace ETSI.ARF.WorldStorage.Editor.Windows
 {
@@ -609,7 +610,6 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows
                     Vector3 position ;
                     Vector3 rotation ;
                     EstimateWorldPose(trackableNode, out position, out rotation);
-                    Debug.Log("Position " +position);
                     GenerateAndUpdateVisual(trackable.UUID.ToString(), "Trackable: "+ trackable.TrackableType + "  "+ trackable.Name, position, rotation, true); // Todo : localCRS does not correspond to world pos
                 }
 
@@ -1356,27 +1356,68 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows
             visual.transform.Find("Canvas/Text").GetComponent<TextMeshProUGUI>().text = $"Name: { name }\nUUID: { UUID }";
             return visual;
         }
-        
+
         /// Here is an approach to estimate the world pose of a node
         /// It goes map depth and multiply the matrix of each link until it finds the last node considered as the origin (0, 0 ,0)
         /// This is not the optimal method as it can create inconsistencies
         /// A better approach would be to set the world poses of the nodes and then compute the matrix of the world links
         private void EstimateWorldPose(ARFNode node, out Vector3 pos, out Vector3 rot)
         {
-           // trackable
-           int newNDepth = 0 ;
-           Matrix4x4 mat = Matrix4x4.identity ;
-           ComputePose(node, ref mat , ref newNDepth);
-
+            // trackable
+            int newNDepth = 0;
+            Matrix4x4 mat = Matrix4x4.identity;
+            if (node.portIn.connections.Any())
+            {
+                ComputePose(node, ref mat, ref newNDepth); 
+            }
+            else
+            {
+                // In case of no in link : just find the first output, find its generated game object an compute pose by composing with link
+                // not very optimal but firs approach
+                ComputePoseNoPortIn(node, out mat);
+            }
             Vector3 translation = mat.GetColumn(3);
-            // Extract Rotation
-            Quaternion rotation = Quaternion.LookRotation(
-            mat.GetColumn(2),
-            mat.GetColumn(1)
-             );
-
-            pos = translation;
-            rot = rotation.eulerAngles;
+                // Extract Rotation
+                Quaternion rotation = Quaternion.LookRotation(
+                mat.GetColumn(2),
+                mat.GetColumn(1)
+                 );
+
+                pos = translation;
+                rot = rotation.eulerAngles;
+        }
+
+  
+        private void ComputePoseNoPortIn(ARFNode node , out Matrix4x4 mat)
+        {
+            mat = Matrix4x4.identity;
+            foreach(Edge edge in node.portOut.connections)
+            {
+                // not best way : try to find an output node allready present in scene. Find gameobject pose and transform with link
+                ARFEdgeLink arfEdge = (ARFEdgeLink) edge ;
+                if (arfEdge !=null)
+                {
+                    WorldLink link =  arfEdge.worldLink ;
+                    ARFNode newNode = (ARFNode) arfEdge.input.node ;
+                    string uuid  = "" ;
+                    ARFNodeTrackable trackNode =  newNode as ARFNodeTrackable;
+                    ARFNodeWorldAnchor worldNode = newNode as ARFNodeWorldAnchor;
+                    if (trackNode != null)
+                    {
+                        uuid= trackNode.trackable.UUID.ToString() ;
+                    }
+                    else 
+                    {
+                        uuid = worldNode.worldAnchor.UUID.ToString() ;
+                    }
+                    GameObject obj = GameObject.Find(uuid); // find associated gameobject (if it exists)
+                    if (obj != null)
+                    {
+                        // compose matrix with link
+                        mat = GetMatrix(link.Transform).inverse * obj.transform.localToWorldMatrix;; 
+                    }
+                }
+            }
         }
 
         private void ComputePose(ARFNode node, ref Matrix4x4 mat , ref int depth)
@@ -1396,8 +1437,6 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows
                     {
                         Matrix4x4 newMat = GetMatrix(link.Transform); 
                         mat = mat * newMat ;
-                        ARFNodeTrackable trackNode =  newNode as ARFNodeTrackable;
-                        ARFNodeWorldAnchor worldNode = newNode as ARFNodeWorldAnchor;
                         maxDepth = currentDepth ;
                     }
                 }
@@ -1405,6 +1444,8 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows
             depth += maxDepth ;
         }
 
+        
+
         private Matrix4x4 GetMatrix(Transform3D tr)
         {
              Matrix4x4 matrix = new Matrix4x4();
-- 
GitLab