diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs
index 06ba33d..111a637 100644
--- a/LinearAlgebra/Spherical.cs
+++ b/LinearAlgebra/Spherical.cs
@@ -1,14 +1,13 @@
-namespace Passer.LinearAlgebra
-{
- public class Spherical
- {
+using Vector3 = UnityEngine.Vector3;
+
+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;
+
+ }
}
}
\ No newline at end of file
diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs
new file mode 100644
index 0000000..0845d21
--- /dev/null
+++ b/Sensors/DistanceSensor.cs
@@ -0,0 +1,27 @@
+using System;
+
+namespace Passer.Control.Core {
+
+ public class DistanceSensor : Thing {
+ public float distance = 0;
+
+ 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);
+ }
+
+ }
+
+}
\ No newline at end of file
diff --git a/SiteServer.cs b/SiteServer.cs
index 4172a8e..09e3a5f 100644
--- a/SiteServer.cs
+++ b/SiteServer.cs
@@ -22,6 +22,10 @@ namespace Passer.Control.Core {
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
}
+ public void Close() {
+ this.udpClient.Close();
+ }
+
public override void Publish() {
}
diff --git a/Thing.cs b/Thing.cs
index 93bc59a..a4f2958 100644
--- a/Thing.cs
+++ b/Thing.cs
@@ -32,6 +32,7 @@ namespace Passer.Control.Core {
#region Properties
+ public RemoteParticipant participant;
public RemoteParticipant participant;
public delegate void ChangeHandler();
@@ -125,27 +126,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) {
- //Thing.Add(this, invokeEvent);
- }
+ // public virtual void Init(bool invokeEvent = false) {
+ // if (invokeEvent)
+ // InvokeNewThing(this);
+ // }
- public Thing(bool initialize = true) {
- if (initialize) {
- //this.Init();
- //Thing.Add(this);
- }
- //OnNewThing?.Invoke(this);
+ public Thing(bool invokeEvent = false) {
+ if (invokeEvent)
+ InvokeNewThing(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 +202,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);
- // }
- // }
-
}
}
diff --git a/Unity/DebugConsole.cs b/Unity/DebugConsole.cs
new file mode 100644
index 0000000..cb4de46
--- /dev/null
+++ b/Unity/DebugConsole.cs
@@ -0,0 +1,25 @@
+#if UNITY_5_3_OR_NEWER
+using System.IO;
+using System.Text;
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+ public class UnityLogWriter : TextWriter {
+ public override void Write(char value) {
+ Debug.Log(value);
+ }
+
+ public override void Write(string value) {
+ Debug.Log(value);
+ }
+
+ public override void WriteLine(string value) {
+ Debug.Log(value);
+ }
+
+ public override Encoding Encoding => Encoding.UTF8;
+ }
+
+}
+#endif
\ No newline at end of file
diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs
new file mode 100644
index 0000000..1b7c9c4
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs
new file mode 100644
index 0000000..628ab09
--- /dev/null
+++ b/Unity/SiteServer.cs
@@ -0,0 +1,38 @@
+#if UNITY_5_3_OR_NEWER
+using System;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+ public class SiteServer : MonoBehaviour {
+ public Core.SiteServer site;
+
+ public Queue<Core.Thing> thingQueue = new();
+
+ protected virtual void Awake() {
+ Console.SetOut(new UnityLogWriter());
+
+ site = new(7681);
+ Core.Thing.OnNewThing += HandleNewThing;
+ }
+
+ void OnApplicationQuit() {
+ site.Close();
+ }
+
+ public void HandleNewThing(Core.Thing thing) {
+ thingQueue.Enqueue(thing);
+ }
+
+ protected virtual void Update() {
+ site.Update((ulong)(Time.time * 1000));
+ if (thingQueue.TryDequeue(out Core.Thing thing)) {
+ thing.CreateComponent();
+ }
+ }
+
+ }
+
+}
+#endif
\ No newline at end of file
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
new file mode 100644
index 0000000..2687f52
--- /dev/null
+++ b/Unity/Thing.cs
@@ -0,0 +1,38 @@
+#if UNITY_5_3_OR_NEWER
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+ public class Thing : MonoBehaviour {
+
+ public Core.Thing core {get; set; }
+
+ protected void SetCoreThing(Core.Thing thing) {
+ core = thing;
+ core.component = this;
+
+ SiteServer siteServer = FindAnyObjectByType<SiteServer>();
+ if (siteServer == null) {
+ Debug.LogWarning("No site server found");
+ return;
+ }
+ siteServer.site.Add(thing);
+ }
+
+ protected virtual void Update() {
+ if (core == null)
+ return;
+
+ if (core.linearVelocity != null) {
+ Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
+ this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction);
+ }
+ if (core.angularVelocity != null) {
+ Vector3 angularVelocity = core.angularVelocity.ToVector3();
+ this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
+ }
+ }
+ }
+
+}
+#endif
\ No newline at end of file
diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs
index adeb0c0..00fd906 100644
--- a/test/UnitTest1.cs
+++ b/test/UnitTest1.cs
@@ -1,4 +1,5 @@
-using System;
+#if !UNITY_5_3_OR_NEWER
+using System;
using System.Threading;
using NUnit.Framework;
@@ -83,3 +84,4 @@ namespace ControlCore.test {
}
}
}
+#endif
\ No newline at end of file