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

DistanceSensor working locally

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-11 17:56:01 +01:00
parent 85a19faeb4
commit 623d3d6156
6 changed files with 93 additions and 63 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;
}
}
}

9
Sensors/DistanceSensor.cs
View File

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

56
Thing.cs
View File

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

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

5
Unity/SiteServer.cs
View File

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

15
Unity/Thing.cs
View File

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