Adding trajectory output to parse it with schemasim.

Assets/DLU/Scripts/SceneController.cs

@@ -207,9 +207,49 @@ public class SceneController : MonoBehaviour
     {
         JSONObject response = new JSONObject();
         response.Add("video_file", videoPath);
+        response.Add("trajectories.log", GetSchemasimLog());
+        response.Add("context", JSONHandler.instance.GenerateContextJSON());
         response.Add("result", "Success!");
         return response;
     }
+
+    private JSONArray GetSchemasimLog()
+    {
+        Dictionary<string, List<JSONObject>> logs = new Dictionary<string, List<JSONObject>>();
+
+        int count = 0;
+        foreach (SemanticBehaviour sb in JSONHandler.instance.semanticBehaviours)
+        {
+            (List<Vector3> positions, List<Quaternion> rotations) = sb.GetLog();
+            count = positions.Count;
+            List<JSONObject> logData = new List<JSONObject>();
+            for (int i = 0; i < positions.Count; ++i)
+            {
+                JSONObject jsonObject = new JSONObject();
+                jsonObject.Add("tx", positions[i].x);
+                jsonObject.Add("ty", positions[i].y);
+                jsonObject.Add("tz", positions[i].z);
+                jsonObject.Add("rx", rotations[i].x);
+                jsonObject.Add("ry", rotations[i].y);
+                jsonObject.Add("rz", rotations[i].z);
+                jsonObject.Add("rw", rotations[i].w);
+                logData.Add(jsonObject);
+            }
+            logs.Add(sb.GetComponent<InstanceIdentifier>().name, logData);
+        }
+
+        JSONArray logRows = new JSONArray();
+        for (int t = 0; t < count; ++t)
+        {
+            JSONObject row = new JSONObject();
+            foreach (var log in logs)
+            {
+                row.Add(log.Key, log.Value[t]);
+            }
+            logRows.Add(row);
+        }
+        return logRows;
+    }
 }
 
 }
\ No newline at end of file

Assets/DLU/Scripts/SemanticBehaviour.cs

 using UnityEngine;
 using SimpleJSON;
+using System.Collections.Generic;
 
 namespace dlu
 {
@@ -10,6 +11,21 @@ public abstract class SemanticBehaviour : MonoBehaviour
     public string is_a;
 
     public abstract void WriteTo(JSONObject jsonObject);
+
+    protected List<Vector3> positionHistory;
+    protected List<Quaternion> rotationHistory;
+
+    public (List<Vector3>, List<Quaternion>) GetLog()
+    {
+        return (positionHistory, rotationHistory);
+    }
+
+    protected void Update()
+    {
+        positionHistory.Add(transform.position);
+        rotationHistory.Add(transform.rotation);
+    }
+
 }
 
 }
\ No newline at end of file