diff --git a/LinearAlgebra/float16.cs b/LinearAlgebra/float16.cs index c1885c3..4195ee6 100644 --- a/LinearAlgebra/float16.cs +++ b/LinearAlgebra/float16.cs @@ -15,7 +15,8 @@ namespace Passer.LinearAlgebra { public float16() { _value = 0; } public float16(float f) { - _value = f32tof16(f); + //_value = f32tof16(f); + _value = F32ToF16__(f); } public float toFloat() { @@ -206,6 +207,59 @@ namespace Passer.LinearAlgebra { return BitConverter.ToUInt32(bytes, 0); } + public ushort F32ToF16__(float f) { + uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0); + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + int exp = (int)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + //This function is faulty!!!! ushort f32tof16(float f) { //uint t = *(uint*)&f; //uint t = (uint)BitConverter.SingleToInt32Bits(f); diff --git a/Messages/BinaryMsg.cs b/Messages/BinaryMsg.cs index 22a0b80..6953188 100644 --- a/Messages/BinaryMsg.cs +++ b/Messages/BinaryMsg.cs @@ -50,6 +50,8 @@ namespace RoboidControl { this.thingId = thing.id; this.thing = thing; this.bytes = this.thing.GenerateBinary(); + // if (this.bytes.Length > 0) + // System.Console.Write($"Binary message for [{networkId}/{thing.id}]"); } /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public BinaryMsg(byte[] buffer) { diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs index 01a579d..27835e6 100644 --- a/Messages/LowLevelMessages.cs +++ b/Messages/LowLevelMessages.cs @@ -9,7 +9,7 @@ namespace RoboidControl { SendFloat16(buffer, ref ix, new float16(v.distance)); SendAngle8(buffer, ref ix, v.direction.horizontal); - SendAngle8(buffer, ref ix, v.direction.horizontal); + SendAngle8(buffer, ref ix, v.direction.vertical); } public static Spherical ReceiveSpherical(byte[] data, ref byte ix) @@ -73,7 +73,8 @@ namespace RoboidControl angle -= 360; while (angle < -180) angle += 360; - buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); + sbyte value = (sbyte)(angle / 360.0f * 256.0f); + buffer[ix++] = (byte)value; } public static float ReceiveAngle8(byte[] data, ref byte ix) @@ -92,13 +93,15 @@ namespace RoboidControl public static void SendFloat16(byte[] data, ref byte ix, float16 f) { ushort binary = f.GetBinary(); - data[ix++] = (byte)(binary >> 8); - data[ix++] = (byte)(binary & 255); + data[ix++] = (byte)((binary >> 8) & 0xFF); + data[ix++] = (byte)(binary & 0xFF); } public static float ReceiveFloat16(byte[] data, ref byte ix) { - ushort value = (ushort)(data[ix++] << 8 | data[ix++]); + byte msb = data[ix++]; + byte lsb = data[ix++]; + ushort value = (ushort)(msb << 8 | lsb); float16 f16 = new float16(); f16.SetBinary(value); float f = f16.toFloat(); diff --git a/Participant.cs b/Participant.cs index 4defa13..175dbe5 100644 --- a/Participant.cs +++ b/Participant.cs @@ -66,11 +66,11 @@ namespace RoboidControl { this.port = port; } - public List senders = new List(); + public List owners = new List(); public RemoteParticipant GetParticipant(string ipAddress, int port) { //Console.WriteLine($"Get Participant {ipAddress}:{port}"); - foreach (RemoteParticipant sender in senders) { + foreach (RemoteParticipant sender in owners) { if (sender.ipAddress == ipAddress && sender.port == port) return sender; } @@ -79,9 +79,9 @@ namespace RoboidControl { public RemoteParticipant AddParticipant(string ipAddress, int port) { Console.WriteLine($"New Participant {ipAddress}:{port}"); RemoteParticipant participant = new(ipAddress, port) { - networkId = (byte)(this.senders.Count + 1) + networkId = (byte)(this.owners.Count + 1) }; - senders.Add(participant); + owners.Add(participant); return participant; } @@ -119,8 +119,7 @@ namespace RoboidControl { // It is hard to determine our source port string ipAddress = this.endPoint.Address.ToString(); RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); - if (remoteParticipant == null) - remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port); + remoteParticipant ??= AddParticipant(ipAddress, this.endPoint.Port); ReceiveData(data, remoteParticipant); @@ -146,10 +145,10 @@ namespace RoboidControl { Thing thing = this.things[ix]; if (thing != null) { thing.Update(currentTimeMS); - BinaryMsg binaryMsg = new(thing.owner.networkId, thing); - this.Send(thing.owner, binaryMsg); - //foreach (RemoteParticipant sender in this.senders) - // this.Send(sender, binaryMsg); + // if (thing.owner != this) { + // BinaryMsg binaryMsg = new(thing.owner.networkId, thing); + // this.Send(thing.owner, binaryMsg); + // } } } } @@ -170,16 +169,6 @@ namespace RoboidControl { this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing)); } - // public bool Send(IMessage msg) { - // int bufferSize = msg.Serialize(ref this.buffer); - // if (bufferSize <= 0) - // return true; - - // // Console.WriteLine($"msg to {endPoint.Address.ToString()} {endPoint.Port}"); - // this.udpClient?.Send(this.buffer, bufferSize, this.endPoint); - // return true; - // } - public bool Send(RemoteParticipant remoteParticipant, IMessage msg) { int bufferSize = msg.Serialize(ref this.buffer); if (bufferSize <= 0) @@ -209,23 +198,23 @@ namespace RoboidControl { return true; } - public bool SendBuffer(int bufferSize) { - //if (this.ipAddress == null) - // return false; + // public bool SendBuffer(int bufferSize) { + // //if (this.ipAddress == null) + // // return false; - // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); - //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port); - this.udpClient?.Send(this.buffer, bufferSize, this.endPoint); - return true; - } + // // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); + // //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port); + // this.udpClient?.Send(this.buffer, bufferSize, this.endPoint); + // return true; + // } - public bool PublishBuffer(int bufferSize) { - if (this.broadcastIpAddress == null) - return false; + // public bool PublishBuffer(int bufferSize) { + // if (this.broadcastIpAddress == null) + // return false; - this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port); - return true; - } + // this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port); + // return true; + // } #endregion @@ -290,14 +279,14 @@ namespace RoboidControl { } } - protected virtual void Process(InvestigateMsg msg) { } + protected virtual void Process(RemoteParticipant sender, InvestigateMsg msg) { } protected virtual void Process(RemoteParticipant sender, ThingMsg msg) { - Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]"); + //Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]"); } protected virtual void Process(RemoteParticipant sender, NameMsg msg) { - // Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); + //Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; @@ -327,9 +316,9 @@ namespace RoboidControl { thing.linearVelocity = msg.linearVelocity; if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) thing.angularVelocity = msg.angularVelocity; - + } - } + } protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); @@ -337,19 +326,19 @@ namespace RoboidControl { thing?.ProcessBinary(msg.bytes); } - protected virtual void Process(TextMsg temsgxt) { } + protected virtual void Process(RemoteParticipant sender, TextMsg temsgxt) { } - protected virtual void Process(DestroyMsg msg) { } + protected virtual void Process(RemoteParticipant sender, DestroyMsg msg) { } private void ForwardMessage(IMessage msg) { - foreach (Participant client in senders) { - if (client == this) - continue; - //UnityEngine.Debug.Log($"---> {client.ipAddress}"); - //IMessage.SendMsg(client, msg); - msg.Serialize(ref client.buffer); - client.SendBuffer(client.buffer.Length); - } + // foreach (Participant client in senders) { + // if (client == this) + // continue; + // //UnityEngine.Debug.Log($"---> {client.ipAddress}"); + // //IMessage.SendMsg(client, msg); + // msg.Serialize(ref client.buffer); + // client.SendBuffer(client.buffer.Length); + // } } #endregion diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index ebdc8de..1474e4a 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -18,9 +18,11 @@ namespace RoboidControl { get { return _touchedSomething; } set { _touchedSomething = value; - BinaryMsg msg = new(networkId, this); - foreach (RemoteParticipant remoteParticipant in thisParticipant.senders) - thisParticipant.Send(remoteParticipant, msg); + if (thisParticipant != null && this.owner != thisParticipant ) { + BinaryMsg msg = new(networkId, this); + foreach (RemoteParticipant remoteParticipant in thisParticipant.owners) + thisParticipant.Send(remoteParticipant, msg); + } } } diff --git a/SiteServer.cs b/SiteServer.cs index 63ca304..ab21e80 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -52,12 +52,12 @@ namespace RoboidControl { protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { } protected override void Process(RemoteParticipant sender, ThingMsg msg) { - Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); + //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing == null) { Thing newThing = null; if (thingMsgProcessors.TryGetValue(msg.thingType, out Func msgProcessor)) { - Console.WriteLine("Found thing message processor"); + //Console.WriteLine("Found thing message processor"); if (msgProcessor != null) newThing = msgProcessor(sender, msg.networkId, msg.thingId); } diff --git a/Thing.cs b/Thing.cs index 1643745..f87f81d 100644 --- a/Thing.cs +++ b/Thing.cs @@ -206,10 +206,11 @@ namespace RoboidControl { /// /// Create a new thing for the given participant /// - /// The participant for which this thing is created + /// The participant for which this thing is created /// True when a new thing event should be triggered - public Thing(RemoteParticipant participant, bool invokeEvent = false) { - this.owner = participant; + public Thing(RemoteParticipant owner, bool invokeEvent = false) { + this.owner = owner; + //owner.Add(this); if (invokeEvent) InvokeNewThing(this); } @@ -297,6 +298,7 @@ namespace RoboidControl { if (thing == null) return false; return (thing.networkId == networkId) && (thing.id == thingId); + //return (thing.id == thingId); } } diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index ee17799..54b0ea9 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -13,10 +13,8 @@ namespace RoboidControl.Unity { protected virtual void Awake() { Console.SetOut(new UnityLogWriter()); - site = new(7681); + site = new RoboidControl.SiteServer(7681); RoboidControl.Thing.OnNewThing += HandleNewThing; - - //site.Register(RoboidControl.Thing.Type.TouchSensor); } void OnApplicationQuit() { @@ -24,7 +22,7 @@ namespace RoboidControl.Unity { } public void HandleNewThing(RoboidControl.Thing thing) { - // Debug.Log("Handle New thing event"); + //Debug.Log("Handle New thing event"); site.Add(thing, false); thingQueue.Enqueue(thing); } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index d58c40c..6e94294 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -96,6 +96,8 @@ namespace RoboidControl.Unity { modelQuad.transform.SetParent(this.transform, false); modelQuad.transform.localEulerAngles = new(90, -90, 0); modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5; + if (this.name == "Ant") + modelQuad.transform.localScale *= 2; Material quadMaterial = new(Shader.Find("Unlit/Transparent")) { mainTexture = texture }; diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 847513d..5f8d8ec 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -24,6 +24,18 @@ namespace RoboidControl.Unity { participant = FindAnyObjectByType(); SetCoreThing(new RoboidControl.TouchSensor(participant.site)); } + // Somehow this does not work. + + // Rigidbody rb = GetComponentInParent(); + // if (rb == null) { + // RoboidControl.Thing thing = core; + // while (thing.parent != null) + // thing = thing.parent; + + // Thing unityThing = thing.component; + // rb = unityThing.gameObject.AddComponent(); + // rb.isKinematic = true; + // } } /// @@ -62,7 +74,6 @@ namespace RoboidControl.Unity { if (this.transform.root == other.transform.root) return; - // Debug.Log($"touched {other.gameObject.name}"); this.coreSensor.touchedSomething = true; } private void OnTriggerExit(Collider other) {