From 3e5da90f4790d2398ab9bd8b2c1fcc3bcc1e3443 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Thu, 13 Feb 2025 12:57:05 +0100
Subject: [PATCH] Basic ant behaviour

---
 LinearAlgebra/Spherical.cs | 15 +++++++++++++--
 Thing.cs                   |  2 ++
 Unity/DistanceSensor.cs    | 16 ++++++++++------
 Unity/Thing.cs             |  1 +
 4 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs
index 111a637..e9c2b5b 100644
--- a/LinearAlgebra/Spherical.cs
+++ b/LinearAlgebra/Spherical.cs
@@ -1,4 +1,4 @@
-using Vector3 = UnityEngine.Vector3;
+using UnityEngine;
 
 namespace Passer.LinearAlgebra {
     public class Spherical {
@@ -6,6 +6,7 @@ namespace Passer.LinearAlgebra {
         public Direction direction;
 
         public static Spherical zero = new(0, 0, 0);
+        public static Spherical forward = new(1, 0, 0);
 
         public Spherical(float distance, float horizontal, float vertical) {
             this.distance = distance;
@@ -16,6 +17,17 @@ namespace Passer.LinearAlgebra {
             this.direction = direction;
         }
 
+        public static Spherical FromVector3(Vector3 v) {
+            float distance = v.magnitude;
+            if (distance == 0.0f)
+                return new Spherical(distance, 0, 0);
+            else {
+                float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg;
+                float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg;
+                return new Spherical(distance, horizontalAngle, verticalAngle);
+            }
+        }
+
         public Vector3 ToVector3() {
             float verticalRad = (UnityEngine.Mathf.PI / 2) - this.direction.vertical * UnityEngine.Mathf.Deg2Rad;
             float horizontalRad = this.direction.horizontal * UnityEngine.Mathf.Deg2Rad;
@@ -30,7 +42,6 @@ namespace Passer.LinearAlgebra {
 
             Vector3 v = new Vector3(x, y, z);
             return v;
-
         }
     }
 }
\ No newline at end of file
diff --git a/Thing.cs b/Thing.cs
index 74fd51c..c78851a 100644
--- a/Thing.cs
+++ b/Thing.cs
@@ -7,6 +7,7 @@ namespace Passer.Control.Core {
     /// <summary>
     /// A thing is the basic building block
     /// </summary>
+    [Serializable]
     public class Thing {
 
         #region Types
@@ -126,6 +127,7 @@ namespace Passer.Control.Core {
         public Spherical angularVelocity;
 
 #if UNITY_5_3_OR_NEWER
+        [NonSerialized]
         public Unity.Thing component;
 #endif
 
diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs
index 1b7c9c4..c623c15 100644
--- a/Unity/DistanceSensor.cs
+++ b/Unity/DistanceSensor.cs
@@ -7,7 +7,7 @@ namespace Passer.Control.Unity {
     public class DistanceSensor : Thing {
 
         public new Core.DistanceSensor core {
-            get => (Core.DistanceSensor)base.core; 
+            get => (Core.DistanceSensor)base.core;
             set => base.core = value;
         }
 
@@ -29,12 +29,16 @@ namespace Passer.Control.Unity {
 
         IEnumerator MeasureDistance() {
             while (Application.isPlaying) {
-                if (Physics.Raycast(this.transform.position, this.transform.forward, out RaycastHit hitInfo, 10.0f)) {
-                    core.distance = hitInfo.distance;
-
-                    // send distance to...
-                    yield return new WaitForSeconds(1);
+                if (Physics.Raycast(this.transform.position, this.transform.forward, out RaycastHit hitInfo, 2.0f)) {
+                    Thing thing = hitInfo.transform.GetComponentInParent<Thing>();
+                    if (thing == null) {
+                        Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}");
+                        core.distance = hitInfo.distance;
+                    }
+                    else
+                        core.distance = 0;
                 }
+                yield return new WaitForSeconds(0.1f);
             }
         }
 
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
index 2687f52..a280ca2 100644
--- a/Unity/Thing.cs
+++ b/Unity/Thing.cs
@@ -5,6 +5,7 @@ namespace Passer.Control.Unity {
 
     public class Thing : MonoBehaviour {
 
+        [field: SerializeField]
         public Core.Thing core {get; set; }
 
         protected void SetCoreThing(Core.Thing thing) {