Commit fd9f1303 authored by Jérémy Lacoche's avatar Jérémy Lacoche
Browse files

Add a method to compute world pose of a node if it has no target

parent aa919a17

Editor/Scripts/Windows/GraphEditorWindow.cs

+58 −17
Original line number Diff line number Diff line
@@ -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

                }
@@ -1366,8 +1366,16 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows 
            // 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(
@@ -1379,6 +1387,39 @@ namespace ETSI.ARF.WorldStorage.Editor.Windows 
                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)

        {

            int maxDepth = 0 ;
@@ -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();