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(); + 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(); 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); } } }