From 85a19faeb4ec342e68dc4824042ac7f78baf29c4 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 11 Feb 2025 16:02:28 +0100
Subject: [PATCH 1/2] First step to ControlCore
---
Sensors/DistanceSensor.cs | 20 ++++++++++++++++++
SiteServer.cs | 6 +++++-
Unity/DebugConsole.cs | 25 +++++++++++++++++++++++
Unity/SiteServer.cs | 43 +++++++++++++++++++++++++++++++++++++++
Unity/Thing.cs | 37 +++++++++++++++++++++++++++++++++
test/UnitTest1.cs | 4 +++-
6 files changed, 133 insertions(+), 2 deletions(-)
create mode 100644 Sensors/DistanceSensor.cs
create mode 100644 Unity/DebugConsole.cs
create mode 100644 Unity/SiteServer.cs
create mode 100644 Unity/Thing.cs
diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs
new file mode 100644
index 0000000..f347a4f
--- /dev/null
+++ b/Sensors/DistanceSensor.cs
@@ -0,0 +1,20 @@
+using System;
+
+namespace Passer.Control.Core {
+
+ public class DistanceSensor : Thing {
+ public float distance = 0;
+
+ public DistanceSensor() : base() { }
+
+ public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
+ }
+
+ 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..f44916b 100644
--- a/SiteServer.cs
+++ b/SiteServer.cs
@@ -13,7 +13,7 @@ namespace Passer.Control.Core {
this.ipAddress = "0.0.0.0";
this.port = port;
- this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
+ //this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
Console.Write($"Prepare receive on port {port}");
this.udpClient = new UdpClient(port); // for receiving
@@ -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/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/SiteServer.cs b/Unity/SiteServer.cs
new file mode 100644
index 0000000..cbc987e
--- /dev/null
+++ b/Unity/SiteServer.cs
@@ -0,0 +1,43 @@
+#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) {
+ // if (thing.participant == null) {
+ // Console.WriteLine($"new thing without participant [{thing.networkId}/{thing.id}] {thing.name}");
+ // this.site.Add(thing, false);
+ // }
+
+ 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..1fd9b0e
--- /dev/null
+++ b/Unity/Thing.cs
@@ -0,0 +1,37 @@
+#if UNITY_5_3_OR_NEWER
+using UnityEngine;
+
+namespace Passer.Control.Unity {
+
+ public class Thing : MonoBehaviour {
+
+ protected Core.Thing core;
+
+ 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);
+ }
+
+ 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(direction * core.linearVelocity.distance * Time.deltaTime);
+ }
+ if (core.angularVelocity != null) {
+ Vector3 axis = Quaternion.AngleAxis(core.angularVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
+ this.transform.Rotate(axis, core.angularVelocity.distance * 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
From 623d3d6156bc2cba12d5846f9e341f157d8ddd79 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 11 Feb 2025 17:56:01 +0100
Subject: [PATCH 2/2] DistanceSensor working locally
---
LinearAlgebra/Spherical.cs | 28 +++++++++++++++----
Sensors/DistanceSensor.cs | 9 +++++-
Thing.cs | 56 ++++++++------------------------------
Unity/DistanceSensor.cs | 43 +++++++++++++++++++++++++++++
Unity/SiteServer.cs | 5 ----
Unity/Thing.cs | 15 +++++-----
6 files changed, 93 insertions(+), 63 deletions(-)
create mode 100644 Unity/DistanceSensor.cs
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
index f347a4f..0845d21 100644
--- a/Sensors/DistanceSensor.cs
+++ b/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);
diff --git a/Thing.cs b/Thing.cs
index 93bc59a..74fd51c 100644
--- a/Thing.cs
+++ b/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) {
- //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 +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);
- // }
- // }
-
}
}
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
index cbc987e..628ab09 100644
--- a/Unity/SiteServer.cs
+++ b/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);
}
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
index 1fd9b0e..2687f52 100644
--- a/Unity/Thing.cs
+++ b/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;
- this.transform.Rotate(axis, core.angularVelocity.distance * Time.deltaTime);
+ Vector3 angularVelocity = core.angularVelocity.ToVector3();
+ this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
}
}
}