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

Fixed issues, added test project

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-31 10:11:08 +01:00
parent dd2cbf1646
commit f99d4fb1b7
12 changed files with 678 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -1 +1,6 @@
DoxyGen/DoxyWarnLogfile.txt
.vscode/settings.json
test/bin
test/obj
/bin
/obj

19
ControlCore.csproj Normal file
View File

@ -0,0 +1,19 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<GenerateAssemblyInfo>false</GenerateAssemblyInfo>
<GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute>
<TargetFramework>net9.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.11.1" />
<PackageReference Include="NUnit" Version="4.2.2" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
</ItemGroup>
</Project>

5
LinearAlgebra/Angle.cs Normal file
View File

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

78
LinearAlgebra/Quat32.cs
View File

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

20
LinearAlgebra/SwingTwist.cs
View File

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

186
LowLevelMessages.cs
View File

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

32
Participant.cs
View File

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

124
Thing.cs
View File

@ -1,7 +1,8 @@
using System.Collections.Generic;
using Passer.LinearAlgebra;
namespace Passer.Control.Core {
namespace Passer.Control.Core
{
/// <summary>
/// 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);
}

272
float16.cs Normal file
View File

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

36
test.sln Normal file
View File

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

17
test/UnitTest1.cs Normal file
View File

@ -0,0 +1,17 @@
namespace test;
using Passer.Control.Core;
public class Tests
{
[SetUp]
public void Setup()
{
}
[Test]
public void Test1()
{
Assert.Pass();
}
}

28
test/test.csproj Normal file
View File

@ -0,0 +1,28 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net9.0</TargetFramework>
<LangVersion>latest</LangVersion>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.11.1" />
<PackageReference Include="NUnit" Version="4.2.2" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
</ItemGroup>
<ItemGroup>
<Using Include="NUnit.Framework" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\ControlCore.csproj" />
</ItemGroup>
</Project>