diff --git a/.gitignore b/.gitignore index 86d94f4..2ef642b 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,6 @@ DoxyGen/DoxyWarnLogfile.txt +.vscode/settings.json +test/bin +test/obj +/bin +/obj diff --git a/ControlCore.csproj b/ControlCore.csproj new file mode 100644 index 0000000..b63b20c --- /dev/null +++ b/ControlCore.csproj @@ -0,0 +1,19 @@ + + + + false + false + net9.0 + enable + enable + + + + + + + + + + + diff --git a/LinearAlgebra/Angle.cs b/LinearAlgebra/Angle.cs new file mode 100644 index 0000000..6b65a0b --- /dev/null +++ b/LinearAlgebra/Angle.cs @@ -0,0 +1,5 @@ +class Angle +{ + public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); + public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; +} \ No newline at end of file diff --git a/LinearAlgebra/Quat32.cs b/LinearAlgebra/Quat32.cs index 1f41a99..9680eb0 100644 --- a/LinearAlgebra/Quat32.cs +++ b/LinearAlgebra/Quat32.cs @@ -7,6 +7,14 @@ namespace Passer.LinearAlgebra public float z; public float w; + public Quat32() + { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } + public Quat32(float x, float y, float z, float w) { this.x = x; @@ -14,5 +22,75 @@ namespace Passer.LinearAlgebra this.z = z; this.w = w; } + + public static Quat32 FromSwingTwist(SwingTwist s) + { + Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); + return q32; + } + + public static Quat32 Euler(float yaw, float pitch, float roll) + { + float rollOver2 = roll * Angle.Deg2Rad * 0.5f; + float sinRollOver2 = (float)Math.Sin((float)rollOver2); + float cosRollOver2 = (float)Math.Cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); + float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)Math.Sin((float)yawOver2); + float cosYawOver2 = (float)Math.Cos((float)yawOver2); + Quat32 result = new() + { + w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + + sinYawOver2 * sinPitchOver2 * sinRollOver2, + x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + + cosYawOver2 * sinPitchOver2 * sinRollOver2, + y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - + sinYawOver2 * cosPitchOver2 * sinRollOver2, + z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - + sinYawOver2 * sinPitchOver2 * cosRollOver2 + }; + return result; + } + + public void ToAngles(out float right, out float up, out float forward) + { + float test = this.x * this.y + this.z * this.w; + if (test > 0.499f) + { // singularity at north pole + right = 0; + up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = 90; + return; + //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); + } + else if (test < -0.499f) + { // singularity at south pole + right = 0; + up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = -90; + return; + //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); + } + else + { + float sqx = this.x * this.x; + float sqy = this.y * this.y; + float sqz = this.z * this.z; + + right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg; + up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg; + forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg; + return; + // return Vector3( + // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * + // Rad2Deg, + // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * + // Rad2Deg, + // asinf(2 * test) * Angle.Rad2Deg); + } + } + } } \ No newline at end of file diff --git a/LinearAlgebra/SwingTwist.cs b/LinearAlgebra/SwingTwist.cs index c73a28d..61774d1 100644 --- a/LinearAlgebra/SwingTwist.cs +++ b/LinearAlgebra/SwingTwist.cs @@ -1,20 +1,32 @@ -namespace Passer.LinearAlgebra { +namespace Passer.LinearAlgebra +{ - public class SwingTwist { + public class SwingTwist + { public Direction swing; public float twist; public static readonly SwingTwist zero = new(0, 0, 0); - public SwingTwist(Direction swing, float twist) { + public SwingTwist(Direction swing, float twist) + { this.swing = swing; this.twist = twist; } - public SwingTwist(float horizontalSwing, float verticalSwing, float twist) { + public SwingTwist(float horizontalSwing, float verticalSwing, float twist) + { this.swing = new Direction(horizontalSwing, verticalSwing); this.swing.Normalize(); this.twist = twist; } + public static SwingTwist FromQuat32(Quat32 q32) + { + // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + q32.ToAngles(out float right, out float up, out float forward); + SwingTwist r = new(up, right, forward); + return r; + } } } \ No newline at end of file diff --git a/LowLevelMessages.cs b/LowLevelMessages.cs index 051b5f5..055096b 100644 --- a/LowLevelMessages.cs +++ b/LowLevelMessages.cs @@ -1,91 +1,109 @@ using Passer.LinearAlgebra; -public class LowLevelMessages { +namespace Passer.Control.Core +{ + public class LowLevelMessages + { - public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) { - SendFloat16(buffer, ref ix, new float16(v.distance)); - SendAngle8(buffer, ref ix, v.direction.horizontal); - SendAngle8(buffer, ref ix, v.direction.horizontal); - } - - public static Spherical ReceiveSpherical(byte[] data, ref byte ix) { - float distance = ReceiveFloat16(data, ref ix); - float horizontal = ReceiveAngle8(data, ref ix); - float vertical = ReceiveAngle8(data, ref ix); - Spherical v = new(distance, horizontal, vertical); - return v; - } - - public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { - UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); - Quat32 q32 = new(q.x, q.y, q.z, q.w); - SendQuat32(buffer, ref ix, q32); - - } - public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) { - int qx = (int)(q.x * 127 + 128); - int qy = (int)(q.y * 127 + 128); - int qz = (int)(q.z * 127 + 128); - int qw = (int)(q.w * 255); - if (q.w < 0) { - qx = -qx; - qy = -qy; - qz = -qz; - qw = -qw; + public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) + { + SendFloat16(buffer, ref ix, new float16(v.distance)); + SendAngle8(buffer, ref ix, v.direction.horizontal); + SendAngle8(buffer, ref ix, v.direction.horizontal); } - buffer[ix++] = (byte)qx; - buffer[ix++] = (byte)qy; - buffer[ix++] = (byte)qz; - buffer[ix++] = (byte)qw; - } - public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { - Quat32 q = new( - (data[ix++] - 128.0F) / 127.0F, - (data[ix++] - 128.0F) / 127.0F, - (data[ix++] - 128.0F) / 127.0F, - data[ix++] / 255.0F); - return q; + public static Spherical ReceiveSpherical(byte[] data, ref byte ix) + { + float distance = ReceiveFloat16(data, ref ix); + float horizontal = ReceiveAngle8(data, ref ix); + float vertical = ReceiveAngle8(data, ref ix); + Spherical v = new(distance, horizontal, vertical); + return v; + } + + public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) + { + Quat32 q32 = Quat32.FromSwingTwist(s); + SendQuat32(buffer, ref ix, q32); + + } + public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) + { + int qx = (int)(q.x * 127 + 128); + int qy = (int)(q.y * 127 + 128); + int qz = (int)(q.z * 127 + 128); + int qw = (int)(q.w * 255); + if (q.w < 0) + { + qx = -qx; + qy = -qy; + qz = -qz; + qw = -qw; + } + + buffer[ix++] = (byte)qx; + buffer[ix++] = (byte)qy; + buffer[ix++] = (byte)qz; + buffer[ix++] = (byte)qw; + } + public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) + { + Quat32 q = new( + (data[ix++] - 128.0F) / 127.0F, + (data[ix++] - 128.0F) / 127.0F, + (data[ix++] - 128.0F) / 127.0F, + data[ix++] / 255.0F); + return q; + } + + public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) + { + Quat32 q32 = ReceiveQuat32(data, ref ix); + // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + + SwingTwist r = SwingTwist.FromQuat32(q32); + return r; + } + + public static void SendAngle8(byte[] buffer, ref byte ix, float angle) + { + // Normalize angle + while (angle >= 180) + angle -= 360; + while (angle < -180) + angle += 360; + buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); + } + + public static float ReceiveAngle8(byte[] data, ref byte ix) + { + float value = (data[ix++] * 180) / 128.0F; + return value; + } + + public static void SendFloat16(byte[] data, ref byte ix, float f) + { + float16 f16 = new(f); + ushort binary = f16.GetBinary(); + data[ix++] = (byte)(binary >> 8); + data[ix++] = (byte)(binary & 255); + } + 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); + } + + public static float ReceiveFloat16(byte[] data, ref byte ix) + { + ushort value = (ushort)(data[ix++] << 8 | data[ix++]); + float16 f16 = new(); + f16.SetBinary(value); + float f = f16.toFloat(); + return f; + } } - public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) { - Quat32 q32 = ReceiveQuat32(data, ref ix); - UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); - SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); - return r; - } - - public static void SendAngle8(byte[] buffer, ref byte ix, float angle) { - // Normalize angle - while (angle >= 180) - angle -= 360; - while (angle < -180) - angle += 360; - buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); - } - - public static float ReceiveAngle8(byte[] data, ref byte ix) { - float value = (data[ix++] * 180) / 128.0F; - return value; - } - - public static void SendFloat16(byte[] data, ref byte ix, float f) { - float16 f16 = new(f); - ushort binary = f16.GetBinary(); - data[ix++] = (byte)(binary >> 8); - data[ix++] = (byte)(binary & 255); - } - 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); - } - - public static float ReceiveFloat16(byte[] data, ref byte ix) { - ushort value = (ushort)(data[ix++] << 8 | data[ix++]); - float16 f16 = new(); - f16.SetBinary(value); - float f = f16.toFloat(); - return f; - } -} +} \ No newline at end of file diff --git a/Participant.cs b/Participant.cs index 147b05f..3e71974 100644 --- a/Participant.cs +++ b/Participant.cs @@ -37,7 +37,8 @@ namespace Passer.Control.Core #region Init - public Participant() { + public Participant() + { this.dataStream = new EchoStream(); others.Add(this); } @@ -59,14 +60,17 @@ namespace Passer.Control.Core private float nextPublishMe = 0; - public virtual void Update(float currentTime) { - if (currentTime > this.nextPublishMe) { + public virtual void Update(float currentTime) + { + if (currentTime > this.nextPublishMe) + { this.PublishBuffer(ClientMsg.Serialized(this.buffer, this.networkId)); ClientMsg.Publish(this, this.networkId); this.nextPublishMe = currentTime + Participant.publishInterval; } - for (int ix = 0; ix < this.others.Count; ix++) { + for (int ix = 0; ix < this.others.Count; ix++) + { Participant client = this.others[ix]; if (client == null) continue; @@ -85,7 +89,7 @@ namespace Passer.Control.Core //if (this.ipAddress == null) // return false; - UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); + // 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; @@ -104,8 +108,10 @@ namespace Passer.Control.Core #region Receive - public async Task ReceiveData() { - while (true) { + public async Task ReceiveData() + { + while (true) + { byte packetSize = (byte)this.dataStream.ReadByte(); if (packetSize != 0xFF) await ReceiveData(this.dataStream, this, packetSize); @@ -113,16 +119,19 @@ namespace Passer.Control.Core } } - public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) { + public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) + { byte msgId = (byte)dataStream.ReadByte(); - if (msgId == 0xFF) { + if (msgId == 0xFF) + { // Timeout return; } //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}"); bool result = false; - switch (msgId) { + switch (msgId) + { case ClientMsg.Id: // 0xA0 / 160 result = await ClientMsg.Receive(dataStream, client, packetSize); break; @@ -156,7 +165,8 @@ namespace Passer.Control.Core default: break; } - if (result == false) { + if (result == false) + { packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte await ReceiveData(dataStream, client, packetSize); } diff --git a/Thing.cs b/Thing.cs index 575f4e8..4e70207 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,7 +1,8 @@ using System.Collections.Generic; using Passer.LinearAlgebra; -namespace Passer.Control.Core { +namespace Passer.Control.Core +{ /// /// A thing is the basic building block @@ -18,29 +19,36 @@ namespace Passer.Control.Core { public event ChangeHandler OnParentChanged; private Thing _parent; - public Thing parent { + public Thing parent + { get => _parent; - set { + set + { if (_parent == value) return; - if (value == null) { + if (value == null) + { _parent.RemoveChild(this); _parent = null; - } else { + } + else + { value.AddChild(this); OnParentChanged?.Invoke(); } } } - public void AddChild(Thing child) { + public void AddChild(Thing child) + { if (children.Find(thing => thing == child) != null) return; child._parent = this; children.Add(child); } - public void RemoveChild(Thing child) { + public void RemoveChild(Thing child) + { children.Remove(child); } @@ -50,10 +58,13 @@ namespace Passer.Control.Core { public event ChangeHandler OnNameChanged; private string _name; - public string name { + public string name + { get => _name; - set { - if (_name != value) { + set + { + if (_name != value) + { _name = value; OnNameChanged?.Invoke(); } @@ -65,10 +76,13 @@ namespace Passer.Control.Core { public byte poseUpdated = 0x00; public event ChangeHandler OnPositionChanged; private Spherical _position; - public Spherical position { + public Spherical position + { get { return _position; } - set { - if (_position != value) { + set + { + if (_position != value) + { _position = value; OnPositionChanged?.Invoke(); } @@ -77,10 +91,13 @@ namespace Passer.Control.Core { public event ChangeHandler OnOrientationChanged; private SwingTwist _orientation; - public SwingTwist orientation { + public SwingTwist orientation + { get { return _orientation; } - set { - if (_orientation != value) { + set + { + if (_orientation != value) + { _orientation = value; OnOrientationChanged?.Invoke(); } @@ -89,10 +106,13 @@ namespace Passer.Control.Core { public event SphericalHandler OnLinearVelocityChanged; private Spherical _linearVelocity; - public Spherical linearVelocity { + public Spherical linearVelocity + { get => _linearVelocity; - set { - if (_linearVelocity != value) { + set + { + if (_linearVelocity != value) + { _linearVelocity = value; OnLinearVelocityChanged?.Invoke(_linearVelocity); } @@ -100,17 +120,21 @@ namespace Passer.Control.Core { } public Spherical angularVelocity; - public virtual void Init(bool invokeEvent = true) { + public virtual void Init(bool invokeEvent = true) + { Thing.Add(this, invokeEvent); } - public Thing(bool initialize = true) { - if (initialize) { + public Thing(bool initialize = true) + { + if (initialize) + { //this.Init(); Thing.Add(this); } } - public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) { + public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) + { this.participant = client; this.id = thingId; this.type = thingType; @@ -119,29 +143,31 @@ namespace Passer.Control.Core { Thing.Add(this); } - public virtual void Update(float currentTime) { + public virtual void Update(float currentTime) + { // should recurse over children... } - public virtual void ProcessBytes(byte[] bytes) { + public virtual void ProcessBytes(byte[] bytes) + { //if (sensor != null) // sensor.ProcessBytes(bytes); } // Experimental - public float stressLevel = 0; + // public float stressLevel = 0; - protected delegate void ReceptorFunc(Sensor sensor); - protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) { - sensor.Signaller += (sensor => Receptor(receptor, sensor)); - } - protected void Receptor(ReceptorFunc receptor, Sensor sensor) { - if (sensor.signalStrength <= stressLevel) - return; + // protected delegate void ReceptorFunc(Sensor sensor); + // protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) { + // sensor.Signaller += (sensor => Receptor(receptor, sensor)); + // } + // protected void Receptor(ReceptorFunc receptor, Sensor sensor) { + // if (sensor.signalStrength <= stressLevel) + // return; - receptor(sensor); - } + // receptor(sensor); + // } //---------- All Things @@ -150,40 +176,48 @@ namespace Passer.Control.Core { public delegate void ThingHandler(Thing t); public static event ThingHandler OnNewThing; - public static void Add(Thing thing, bool invokeEvent = true) { + public static void Add(Thing thing, bool invokeEvent = true) + { Thing foundThing = Get(thing.networkId, thing.id); - if (foundThing == null) { + if (foundThing == null) + { if (thing.id == 0) thing.id = (byte)(allThings.Count + 1); allThings.Add(thing); if (invokeEvent) OnNewThing?.Invoke(thing); - UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}"); + // UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}"); } } - public static Thing Get(byte networkId, byte thingId) { + public static Thing Get(byte networkId, byte thingId) + { Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId)); return thing; } - private static bool IsThing(Thing thing, byte networkId, byte thingId) { + private static bool IsThing(Thing thing, byte networkId, byte thingId) + { if (thing == null) return false; - return (thing.networkId == networkId ) && (thing.id == thingId); + return (thing.networkId == networkId) && (thing.id == thingId); } - public static void Remove(byte networkId, byte thingId) { + public static void Remove(byte networkId, byte thingId) + { allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId); } - public static Thing[] GetAllThings() { + public static Thing[] GetAllThings() + { return allThings.ToArray(); } - public static void UpdateAll(float currentTime) { - foreach (Thing thing in allThings) { + public static void UpdateAll(float currentTime) + { + foreach (Thing thing in allThings) + { if (thing.parent == null) // update only root things thing.Update(currentTime); } diff --git a/float16.cs b/float16.cs new file mode 100644 index 0000000..579cb97 --- /dev/null +++ b/float16.cs @@ -0,0 +1,272 @@ +namespace Passer.LinearAlgebra +{ + + public class float16 + { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) + { + _value = f32tof16(f); + } + + public float toFloat() + { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + + bool float16::operator !=(const float16 &f) { return (_value != f._value); } + + bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; + } + + bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; + } + + bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; + } + + bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; + } + + ////////////////////////////////////////////////////////// + // + // NEGATION + // + float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; + } + + ////////////////////////////////////////////////////////// + // + // MATH + // + float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); + } + + float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); + } + + float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); + } + + float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); + } + + float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; + } + + float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; + } + + float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; + } + + float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; + } + + ////////////////////////////////////////////////////////// + // + // MATH HELPER FUNCTIONS + // + int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; + } + + bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + + bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; + } + + bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) + { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) + { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) + { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) + { + f *= 2; + if ((man & (1 << i)) != 0) + f = f + 1; + } + //Debug.Log($"{f}"); + f = f * (float)Math.Pow(2.0f, exp - 25); + if (exp == 0) + { + f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + ushort f32tof16(float f) + { + //uint t = *(uint*)&f; + uint t = (uint)BitConverter.SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((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 = (short)(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); + } + + // -- END OF FILE -- + } + +} \ No newline at end of file diff --git a/test.sln b/test.sln new file mode 100644 index 0000000..5ab05c5 --- /dev/null +++ b/test.sln @@ -0,0 +1,36 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31903.59 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ControlCore", "ControlCore.csproj", "{F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "test", "test\test.csproj", "{2C350E2B-9DDA-4037-BAE5-E12AB7A52398}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Debug|Any CPU.Build.0 = Debug|Any CPU + {6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Release|Any CPU.ActiveCfg = Release|Any CPU + {6601071A-9E9C-42E1-95EA-0A36C5D718E4}.Release|Any CPU.Build.0 = Release|Any CPU + {DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Debug|Any CPU.Build.0 = Debug|Any CPU + {DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Release|Any CPU.ActiveCfg = Release|Any CPU + {DD7238DA-DBEF-4D6A-98DB-6FE28BBDB75D}.Release|Any CPU.Build.0 = Release|Any CPU + {F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {F2DEE6B0-AF41-454E-AAC8-9E1E3ACC769F}.Release|Any CPU.Build.0 = Release|Any CPU + {2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Debug|Any CPU.Build.0 = Debug|Any CPU + {2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Release|Any CPU.ActiveCfg = Release|Any CPU + {2C350E2B-9DDA-4037-BAE5-E12AB7A52398}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection +EndGlobal diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs new file mode 100644 index 0000000..c4b42fd --- /dev/null +++ b/test/UnitTest1.cs @@ -0,0 +1,17 @@ +namespace test; + +using Passer.Control.Core; + +public class Tests +{ + [SetUp] + public void Setup() + { + } + + [Test] + public void Test1() + { + Assert.Pass(); + } +} diff --git a/test/test.csproj b/test/test.csproj new file mode 100644 index 0000000..6da79b7 --- /dev/null +++ b/test/test.csproj @@ -0,0 +1,28 @@ + + + + net9.0 + latest + enable + enable + false + true + + + + + + + + + + + + + + + + + + +