RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Merge commit '623d3d6156bc2cba12d5846f9e341f157d8ddd79'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-12 09:41:28 +01:00
commit 5a9d92002d
9 changed files with 213 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
LinearAlgebra/Spherical.cs
View File

@ -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;
}
}
}

27
Sensors/DistanceSensor.cs Normal file
View File

@ -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);
}
}
}

4
SiteServer.cs
View File

@ -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() {
}

57
Thing.cs
View File

@ -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);
// }
// }
}
}

25
Unity/DebugConsole.cs Normal file
View File

@ -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

43
Unity/DistanceSensor.cs Normal file
View File

@ -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

38
Unity/SiteServer.cs Normal file
View File

@ -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

38
Unity/Thing.cs Normal file
View File

@ -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

4
test/UnitTest1.cs
View File

@ -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