DistanceSensor working locally

LinearAlgebra/Spherical.cs

@@ -1,14 +1,13 @@
-namespace Passer.LinearAlgebra
+using Vector3 = UnityEngine.Vector3;
-{
+
-    public class Spherical
+namespace Passer.LinearAlgebra {
-    {
+    public class Spherical {
         public float distance;
         public Direction direction;
 
         public static Spherical zero = new(0, 0, 0);
 
-        public Spherical(float distance, float horizontal, float vertical)
+        public Spherical(float distance, float horizontal, float vertical) {
-        {
             this.distance = distance;
             this.direction = new Direction(horizontal, vertical);
         }
@@ -16,5 +15,22 @@ namespace Passer.LinearAlgebra
             this.distance = distance;
             this.direction = direction;
         }
+
+        public Vector3 ToVector3() {
+            float verticalRad = (UnityEngine.Mathf.PI / 2) - this.direction.vertical * UnityEngine.Mathf.Deg2Rad;
+            float horizontalRad = this.direction.horizontal * UnityEngine.Mathf.Deg2Rad;
+            float cosVertical = UnityEngine.Mathf.Cos(verticalRad);
+            float sinVertical = UnityEngine.Mathf.Sin(verticalRad);
+            float cosHorizontal = UnityEngine.Mathf.Cos(horizontalRad);
+            float sinHorizontal = UnityEngine.Mathf.Sin(horizontalRad);
+
+            float x = this.distance * sinVertical * sinHorizontal;
+            float y = this.distance * cosVertical;
+            float z = this.distance * sinVertical * cosHorizontal;
+
+            Vector3 v = new Vector3(x, y, z);
+            return v;
+
+        }
     }
 }

Sensors/DistanceSensor.cs

@@ -5,11 +5,18 @@ namespace Passer.Control.Core {
     public class DistanceSensor : Thing {
         public float distance = 0;
 
-        public DistanceSensor() : base() { }
+        public DistanceSensor() : base(true) { }
 
         public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
         }
 
+#if UNITY_5_3_OR_NEWER
+        public override void CreateComponent() {
+            this.component = Unity.DistanceSensor.Create(this.parent);
+            this.component.core = this;
+        }
+#endif        
+
         public override void ProcessBinary(byte[] bytes) {
             byte ix = 0;
             this.distance = LowLevelMessages.ReceiveFloat16(bytes, ref ix);

Thing.cs

@@ -125,27 +125,29 @@ namespace Passer.Control.Core {
         }
         public Spherical angularVelocity;
 
+#if UNITY_5_3_OR_NEWER
+        public Unity.Thing component;
+#endif
+
         #endregion Properties
 
         #region Init
 
-        public virtual void Init(bool invokeEvent = true) {
+        // public virtual void Init(bool invokeEvent = false) {
-            //Thing.Add(this, invokeEvent);
+        //     if (invokeEvent)
-        }
+        //         InvokeNewThing(this);
+        // }
 
-        public Thing(bool initialize = true) {
+        public Thing(bool invokeEvent = false) {
-            if (initialize) {
+            if (invokeEvent)
-                //this.Init();
+                InvokeNewThing(this);
-                //Thing.Add(this);
-            }
-            //OnNewThing?.Invoke(this);
         }
         public Thing(RemoteParticipant sender, byte networkId, byte thingId, byte thingType = 0) {
             this.participant = sender;
             this.id = thingId;
             this.type = thingType;
             this.networkId = networkId;
-            this.Init();
+            //this.Init();
             //OnNewThing?.Invoke(this);
             //Thing.Add(this);
         }
@@ -199,45 +201,11 @@ namespace Passer.Control.Core {
              OnNewThing?.Invoke(thing);
         }
 
-        // public static void Add(Thing thing, bool invokeEvent = true) {
-        //     Console.WriteLine("added thing");
-        //     Thing foundThing = Get(thing.networkId, thing.id);
-
-        //     if (foundThing == null) {
-        //         if (thing.id == 0)
-        //             thing.id = (byte)(allThings.Count + 1);
-        //         allThings.Add(thing);
-        //         if (invokeEvent)
-        //             OnNewThing?.Invoke(thing);
-        //         Console.Write($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
-        //     }
-        // }
-
-        // public static Thing Get(byte networkId, byte thingId) {
-        //     Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
-        //     return thing;
-        // }
-
         public static bool IsThing(Thing thing, byte networkId, byte thingId) {
             if (thing == null)
                 return false;
             return (thing.networkId == networkId) && (thing.id == thingId);
         }
 
-        // public static void Remove(byte networkId, byte thingId) {
-        //     allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
-        // }
-
-        // public static Thing[] GetAllThings() {
-        //     return allThings.ToArray();
-        // }
-
-        // public static void UpdateAll(ulong currentTimeMS) {
-        //     foreach (Thing thing in allThings) {
-        //         if (thing.parent == null) // update only root things
-        //             thing.Update(currentTimeMS);
-        //     }
-        // }
-
     }
 }

Unity/DistanceSensor.cs

@@ -0,0 +1,43 @@
+#if UNITY_5_3_OR_NEWER
+using System.Collections;
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+    public class DistanceSensor : Thing {
+
+        public new Core.DistanceSensor core {
+            get => (Core.DistanceSensor)base.core; 
+            set => base.core = value;
+        }
+
+        protected virtual void Start() {
+            if (core == null)
+                SetCoreThing(new Core.DistanceSensor());
+
+            StartCoroutine(MeasureDistance());
+        }
+
+        public static DistanceSensor Create(Core.Thing parent) {
+            GameObject distanceObj = new("Distance sensor");
+            DistanceSensor component = distanceObj.AddComponent<DistanceSensor>();
+            if (parent != null && parent.component != null)
+                distanceObj.transform.SetParent(parent.component.transform);
+
+            return component;
+        }
+
+        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);
+                }
+            }
+        }
+
+    }
+}
+#endif

Unity/SiteServer.cs

@@ -22,11 +22,6 @@ namespace Passer.Control.Unity {
         }
 
         public void HandleNewThing(Core.Thing thing) {
-            // if (thing.participant == null) {
-            //     Console.WriteLine($"new thing without participant [{thing.networkId}/{thing.id}] {thing.name}");
-            //     this.site.Add(thing, false);
-            // }
-
             thingQueue.Enqueue(thing);
         }
 

Unity/Thing.cs

@@ -5,16 +5,17 @@ namespace Passer.Control.Unity {
 
     public class Thing : MonoBehaviour {
 
-        protected Core.Thing core;
+        public Core.Thing core {get; set; }
+
+        protected void SetCoreThing(Core.Thing thing) {
+            core = thing;
+            core.component = this;
 
-        protected void CreateThing(Core.Thing thing) {
             SiteServer siteServer = FindAnyObjectByType<SiteServer>();
             if (siteServer == null) {
                 Debug.LogWarning("No site server found");
                 return;
             }
-
-            core = thing;
             siteServer.site.Add(thing);
         }
 
@@ -24,11 +25,11 @@ namespace Passer.Control.Unity {
 
             if (core.linearVelocity != null) {
                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
-                this.transform.Translate(direction * core.linearVelocity.distance * Time.deltaTime);
+                this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction);
             }
             if (core.angularVelocity != null) {
-                Vector3 axis = Quaternion.AngleAxis(core.angularVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
+                Vector3 angularVelocity = core.angularVelocity.ToVector3();
-                this.transform.Rotate(axis, core.angularVelocity.distance * Time.deltaTime);
+                this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
             }
         }
     }