diff --git a/.editorconfig b/.editorconfig index 4ee3fdb..1ec7f97 100644 --- a/.editorconfig +++ b/.editorconfig @@ -10,6 +10,10 @@ end_of_line = crlf charset = utf-8 trim_trailing_whitespace = false insert_final_newline = false +max_line_length = 80 [*.cs] -csharp_new_line_before_open_brace = none \ No newline at end of file +csharp_new_line_before_open_brace = none +# Suppress warnings everywhere +dotnet_diagnostic.IDE1006.severity = none +dotnet_diagnostic.IDE0130.severity = none \ No newline at end of file diff --git a/LinearAlgebra/Angle.cs b/LinearAlgebra/Angle.cs index 71c44a6..0c23840 100644 --- a/LinearAlgebra/Angle.cs +++ b/LinearAlgebra/Angle.cs @@ -1,7 +1,11 @@ using System; -class Angle -{ - public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); - public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; +namespace Passer.LinearAlgebra { + + public class Angle { + public const float pi = 3.1415927410125732421875F; + 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/Matrix.cs b/LinearAlgebra/Matrix.cs index c2e6334..9603048 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,11 +1,10 @@ using System; using System.Diagnostics; -using Vector3 = UnityEngine.Vector3; -using Vector2 = UnityEngine.Vector2; +using Passer.LinearAlgebra; public readonly struct Slice { - public int start {get;} - public int stop {get;} + public int start { get; } + public int stop { get; } public Slice(int start, int stop) { this.start = start; this.stop = stop; @@ -50,7 +49,7 @@ public class Matrix2 { return new Matrix2(resultData); } - public static Matrix2 SkewMatrix(Vector3 v) { + public static Matrix2 SkewMatrix(Vector3Float v) { float[,] result = new float[3, 3] { {0, -v.z, v.y}, {v.z, 0, -v.x}, @@ -137,12 +136,12 @@ public class Matrix2 { return new Matrix1(result); } - public static Vector3 operator *(Matrix2 A, Vector3 v) { - return new Vector3() { - x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, - y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, - z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z - }; + public static Vector3Float operator *(Matrix2 A, Vector3Float v) { + return new Vector3Float( + A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, + A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, + A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z + ); } @@ -263,14 +262,14 @@ public class Matrix1 { return new Matrix1(magnitude); } - public static Matrix1 FromVector2(Vector2 v) { + public static Matrix1 FromVector2(Vector2Float v) { float[] result = new float[2]; result[0] = v.x; result[1] = v.y; return new Matrix1(result); } - public static Matrix1 FromVector3(Vector3 v) { + public static Matrix1 FromVector3(Vector3Float v) { float[] result = new float[3]; result[0] = v.x; result[1] = v.y; @@ -278,18 +277,18 @@ public class Matrix1 { return new Matrix1(result); } - public Vector2 vector2 { + public Vector2Float vector2 { get { if (this.magnitude != 2) throw new System.ArgumentException("Matrix1 must be of size 2"); - return new Vector2(this.data[0], this.data[1]); + return new Vector2Float(this.data[0], this.data[1]); } } - public Vector3 vector3 { + public Vector3Float vector3 { get { if (this.magnitude != 3) throw new System.ArgumentException("Matrix1 must be of size 3"); - return new Vector3(this.data[0], this.data[1], this.data[2]); + return new Vector3Float(this.data[0], this.data[1], this.data[2]); } } @@ -345,178 +344,179 @@ public class Matrix1 { } } -public class Matrix { - private readonly uint rows = 0; - private readonly uint cols = 0; - private float[] data; +// public class Matrix { +// private readonly uint rows = 0; +// private readonly uint cols = 0; +// private float[] data; - public Matrix(uint rows, uint cols) { - this.rows = rows; - this.cols = cols; - } +// public Matrix(uint rows, uint cols) { +// this.rows = rows; +// this.cols = cols; +// } - public static float[,] Diagonal(float[] v) { - float[,] r = new float[v.Length, v.Length]; - for (int i = 0; i < v.Length; i++) { - r[i, i] = v[i]; - } - return r; - } +// public static float[,] Diagonal(float[] v) { +// float[,] r = new float[v.Length, v.Length]; +// for (int i = 0; i < v.Length; i++) { +// r[i, i] = v[i]; +// } +// return r; +// } - public static float[,] Transpose(float[,] m) { - int rows = m.GetLength(0); - int cols = m.GetLength(1); - float[,] r = new float[cols, rows]; - for (uint rowIx = 0; rowIx < rows; rowIx++) { - for (uint colIx = 0; colIx < cols; colIx++) - r[colIx, rowIx] = m[rowIx, colIx]; - } - return r; - // double checked code - } +// public static float[,] Transpose(float[,] m) { +// int rows = m.GetLength(0); +// int cols = m.GetLength(1); +// float[,] r = new float[cols, rows]; +// for (uint rowIx = 0; rowIx < rows; rowIx++) { +// for (uint colIx = 0; colIx < cols; colIx++) +// r[colIx, rowIx] = m[rowIx, colIx]; +// } +// return r; +// // double checked code +// } - public static void NegateColumn(float[,] m, uint colIx) { - for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { - m[rowIx, colIx] = -m[rowIx, colIx]; - } - } +// public static void NegateColumn(float[,] m, uint colIx) { +// for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { +// m[rowIx, colIx] = -m[rowIx, colIx]; +// } +// } - public static float Determinant(float[,] matrix) { - int n = matrix.GetLength(0); - if (n != matrix.GetLength(1)) - throw new System.ArgumentException("Matrix must be square."); +// public static float Determinant(float[,] matrix) { +// int n = matrix.GetLength(0); +// if (n != matrix.GetLength(1)) +// throw new System.ArgumentException("Matrix must be square."); - if (n == 1) - return matrix[0, 0]; // Base case for 1x1 matrix +// if (n == 1) +// return matrix[0, 0]; // Base case for 1x1 matrix - if (n == 2) // Base case for 2x2 matrix - return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; +// if (n == 2) // Base case for 2x2 matrix +// return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; - float det = 0; - for (int col = 0; col < n; col++) - det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); +// float det = 0; +// for (int col = 0; col < n; col++) +// det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); - return det; - } +// return det; +// } - // Helper function to compute the minor of a matrix - private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { - int n = matrix.GetLength(0); - float[,] minor = new float[n - 1, n - 1]; +// // Helper function to compute the minor of a matrix +// private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { +// int n = matrix.GetLength(0); +// float[,] minor = new float[n - 1, n - 1]; - int r = 0, c = 0; - for (int i = 0; i < n; i++) { - if (i == rowToRemove) continue; +// int r = 0, c = 0; +// for (int i = 0; i < n; i++) { +// if (i == rowToRemove) continue; - c = 0; - for (int j = 0; j < n; j++) { - if (j == colToRemove) continue; +// c = 0; +// for (int j = 0; j < n; j++) { +// if (j == colToRemove) continue; - minor[r, c] = matrix[i, j]; - c++; - } - r++; - } +// minor[r, c] = matrix[i, j]; +// c++; +// } +// r++; +// } - return minor; - } +// return minor; +// } - public static float[,] MultiplyMatrices(float[,] A, float[,] B) { - int rowsA = A.GetLength(0); - int colsA = A.GetLength(1); - int rowsB = B.GetLength(0); - int colsB = B.GetLength(1); +// public static float[,] MultiplyMatrices(float[,] A, float[,] B) { +// int rowsA = A.GetLength(0); +// int colsA = A.GetLength(1); +// int rowsB = B.GetLength(0); +// int colsB = B.GetLength(1); - if (colsA != rowsB) - throw new System.ArgumentException("Number of columns in A must match number of rows in B."); +// if (colsA != rowsB) +// throw new System.ArgumentException("Number of columns in A must match number of rows in B."); - float[,] result = new float[rowsA, colsB]; +// float[,] result = new float[rowsA, colsB]; - for (int i = 0; i < rowsA; i++) { - for (int j = 0; j < colsB; j++) { - float sum = 0.0f; - for (int k = 0; k < colsA; k++) - sum += A[i, k] * B[k, j]; +// for (int i = 0; i < rowsA; i++) { +// for (int j = 0; j < colsB; j++) { +// float sum = 0.0f; +// for (int k = 0; k < colsA; k++) +// sum += A[i, k] * B[k, j]; - result[i, j] = sum; - } - } +// result[i, j] = sum; +// } +// } - return result; - // double checked code - } +// return result; +// // double checked code +// } - public static float[] MultiplyMatrixVector(float[,] A, float[] v) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[] result = new float[rows]; +// public static float[] MultiplyMatrixVector(float[,] A, float[] v) { +// int rows = A.GetLength(0); +// int cols = A.GetLength(1); +// float[] result = new float[rows]; - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i] += A[i, j] * v[j]; - } - } +// for (int i = 0; i < rows; i++) { +// for (int j = 0; j < cols; j++) { +// result[i] += A[i, j] * v[j]; +// } +// } - return result; - } +// return result; +// } - // Vector-matrix multiplication - public static Vector3 MultiplyMatrixVector3(float[,] A, Vector3 v) { - return new Vector3() { - x = A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, - y = A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, - z = A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z - }; - } +// // Vector-matrix multiplication +// public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) { +// return new Vector3Float( +// A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, +// A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, +// A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z +// ); +// } - public static float[,] MultiplyMatrixScalar(float[,] A, float s) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[,] result = new float[rows, cols]; +// public static float[,] MultiplyMatrixScalar(float[,] A, float s) { +// int rows = A.GetLength(0); +// int cols = A.GetLength(1); +// float[,] result = new float[rows, cols]; - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i, j] += A[i, j] * s; - } - } +// for (int i = 0; i < rows; i++) { +// for (int j = 0; j < cols; j++) { +// result[i, j] += A[i, j] * s; +// } +// } - return result; - } +// return result; +// } - public static float[] GetColumn(float[,] M, int col) { - int rows = M.GetLength(0); - float[] column = new float[rows]; - for (int i = 0; i < rows; i++) { - column[i] = M[i, col]; - } - return column; - } +// public static float[] GetColumn(float[,] M, int col) { +// int rows = M.GetLength(0); +// float[] column = new float[rows]; +// for (int i = 0; i < rows; i++) { +// column[i] = M[i, col]; +// } +// return column; +// } - public static Vector3 GetRow3(float[,] M, int rowIx) { - int cols = M.GetLength(1); - Vector3 row = new(); - row.x = M[rowIx, 0]; - row.y = M[rowIx, 1]; - row.z = M[rowIx, 2]; - return row; - } +// public static Vector3Float GetRow3(float[,] M, int rowIx) { +// int cols = M.GetLength(1); +// Vector3Float row = new( +// M[rowIx, 0], +// M[rowIx, 1], +// M[rowIx, 2] +// ); +// return row; +// } - public static void SetRow3(float[,] M, int rowIx, Vector3 v) { - M[rowIx, 0] = v.x; - M[rowIx, 1] = v.y; - M[rowIx, 2] = v.z; - } +// public static void SetRow3(float[,] M, int rowIx, Vector3Float v) { +// M[rowIx, 0] = v.x; +// M[rowIx, 1] = v.y; +// M[rowIx, 2] = v.z; +// } - public static float Dot(float[] a, float[] b) { - float sum = 0; - for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; - return sum; - } +// public static float Dot(float[] a, float[] b) { +// float sum = 0; +// for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; +// return sum; +// } - public static float[,] IdentityMatrix(int size) { - float[,] I = new float[size, size]; - for (int i = 0; i < size; i++) I[i, i] = 1.0f; - return I; - } +// public static float[,] IdentityMatrix(int size) { +// float[,] I = new float[size, size]; +// for (int i = 0; i < size; i++) I[i, i] = 1.0f; +// return I; +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs index e9c2b5b..2b86a66 100644 --- a/LinearAlgebra/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -1,5 +1,3 @@ -using UnityEngine; - namespace Passer.LinearAlgebra { public class Spherical { public float distance; @@ -17,30 +15,30 @@ namespace Passer.LinearAlgebra { this.direction = direction; } - public static Spherical FromVector3(Vector3 v) { + public static Spherical FromVector3(Vector3Float v) { float distance = v.magnitude; if (distance == 0.0f) return new Spherical(distance, 0, 0); else { - float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg; - float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg; + float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float horizontalAngle = (float) Math.Atan2(v.x, v.z) * Angle.Rad2Deg; return new Spherical(distance, horizontalAngle, verticalAngle); } } - 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); + public Vector3Float ToVector3() { + float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.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); + Vector3Float v = new Vector3Float(x, y, z); return v; } } diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index c0bc6dd..c7ab61a 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,3 +1,7 @@ +#if UNITY_5_3_OR_NEWER +using Passer.LinearAlgebra.Vector3Float = UnityEngine.Vector3 +#else + namespace Passer.LinearAlgebra { public class Vector3Of { @@ -10,12 +14,20 @@ namespace Passer.LinearAlgebra { this.y = y; this.z = z; } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } } - public class Vector3Int : Vector3Of { - public Vector3Int(int x, int y, int z) : base(x, y, z) { } + public class Vector3Int(int x, int y, int z) : Vector3Of(x, y, z) { } - public class Vector3Float : Vector3Of { - public Vector3Float(float x, float y, float z) : base(x, y, z) { } + public class Vector3Float(float x, float y, float z) : Vector3Of(x, y, z) { + + public float magnitude { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } } -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/Messages/CustomMsg.cs b/Messages/CustomMsg.cs index 75e4311..e3c7acb 100644 --- a/Messages/CustomMsg.cs +++ b/Messages/CustomMsg.cs @@ -1,3 +1,4 @@ +#nullable enable namespace Passer.Control.Core { public class CustomMsg : IMessage { @@ -23,11 +24,13 @@ namespace Passer.Control.Core { public CustomMsg(byte networkId, Thing thing) : base() { this.networkId = networkId; this.thingId = thing.id; + this.bytes = []; } public override byte Serialize(ref byte[] buffer) { - if (bytes == null) + if (bytes.Length == 0) return 0; + byte ix = 0; buffer[ix++] = CustomMsg.Id; buffer[ix++] = this.networkId; diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs new file mode 100644 index 0000000..1701a53 --- /dev/null +++ b/Messages/DestroyMsg.cs @@ -0,0 +1,29 @@ + +namespace Passer.Control.Core { + + public class DestroyMsg : IMessage { + public const byte Id = 0x20; + public const byte length = 3; + public byte networkId; + public byte thingId; + + public DestroyMsg(byte[] buffer) : base(buffer) { } + + public override void Deserialize(byte[] buffer) { + this.networkId = buffer[0]; + this.thingId = buffer[1]; + } + + public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { + if (packetSize != length) + return false; + + byte[] buffer = await Receive(dataStream, packetSize); + DestroyMsg msg = new(buffer); + + client.messageQueue.Enqueue(msg); + return true; + } + } + +} \ No newline at end of file diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs new file mode 100644 index 0000000..7c6af47 --- /dev/null +++ b/Messages/InvestigateMsg.cs @@ -0,0 +1,46 @@ +namespace Passer.Control.Core { + + public class InvestigateMsg : IMessage { + public const byte Id = 0x81; + public const byte length = 3; + public byte networkId; + public byte thingId; + + public InvestigateMsg(byte networkId, byte thingId) { + this.networkId = networkId; + this.thingId = thingId; + } + public InvestigateMsg(byte[] buffer) : base(buffer) { } + + public override byte Serialize(ref byte[] buffer) { + byte ix = 0; + buffer[ix++] = InvestigateMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + return ix; + } + public override void Deserialize(byte[] buffer) { + uint ix = 0; + this.networkId = buffer[ix++]; + this.thingId = buffer[ix++]; + } + + //public static bool Send(Participant client, CoreThing thing) { + // InvestigateMsg msg = new(thing.networkId, thing.id); + // return SendMsg(client, msg); + //} + public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { + if (packetSize != length) + return false; + + byte[] buffer = await Receive(dataStream, packetSize); + InvestigateMsg msg = new(buffer); + //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); + + client.messageQueue.Enqueue(msg); + return true; + + } + } + +} \ No newline at end of file diff --git a/Messages/Messages.cs b/Messages/Messages.cs index 83ba4e5..c07368e 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,7 +1,3 @@ -using System.IO; -using System.Threading.Tasks; - -using Passer.LinearAlgebra; namespace Passer.Control.Core { public class IMessage { @@ -40,214 +36,4 @@ namespace Passer.Control.Core { } } - #region Investigate - - public class InvestigateMsg : IMessage { - public const byte Id = 0x81; - public const byte length = 3; - public byte networkId; - public byte thingId; - - public InvestigateMsg(byte networkId, byte thingId) { - this.networkId = networkId; - this.thingId = thingId; - } - public InvestigateMsg(byte[] buffer) : base(buffer) { } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = InvestigateMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - return ix; - } - public override void Deserialize(byte[] buffer) { - uint ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - } - - //public static bool Send(Participant client, CoreThing thing) { - // InvestigateMsg msg = new(thing.networkId, thing.id); - // return SendMsg(client, msg); - //} - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - if (packetSize != length) - return false; - - byte[] buffer = await Receive(dataStream, packetSize); - InvestigateMsg msg = new(buffer); - //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); - - client.messageQueue.Enqueue(msg); - return true; - - } - } - - #endregion Investigate - - #region Pose - - public class PoseMsg : IMessage { - public const byte Id = 0x10; - public const byte length = 4 + 4 + 4; - public byte networkId; - public byte thingId; - - public byte poseType; - public const byte Pose_Position = 0x01; - public const byte Pose_Orientation = 0x02; - public const byte Pose_LinearVelocity = 0x04; - public const byte Pose_AngularVelocity = 0x08; - - public Spherical position; - public SwingTwist orientation; - public Spherical linearVelocity; - public Spherical angularVelocity; - - public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) { - this.networkId = networkId; - this.thingId = thingId; - this.position = position; - this.orientation = orientation; - this.poseType = 0; - if (this.position != null) - this.poseType |= Pose_Position; - else - this.position = new Spherical(0, 0, 0); - if (this.orientation != null) - this.poseType |= Pose_Orientation; - else - this.orientation = SwingTwist.zero; - } - public PoseMsg(byte[] buffer) : base(buffer) { } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = PoseMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - buffer[ix++] = this.poseType; - - if ((poseType & Pose_Position) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.position); - if ((poseType & Pose_Orientation) != 0) - LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); - if ((poseType & Pose_LinearVelocity) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); - if ((poseType & Pose_AngularVelocity) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); - return ix; - } - public override void Deserialize(byte[] buffer) { - byte ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - this.poseType = buffer[ix++]; - - if ((poseType & Pose_Position) != 0) - this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - if ((poseType & Pose_Orientation) != 0) - this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); - if ((poseType & Pose_LinearVelocity) != 0) - this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - if ((poseType & Pose_AngularVelocity) != 0) - this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - } - - public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { - PoseMsg msg = new(client.networkId, thingId, position, orientation); - return SendMsg(client, msg); - } - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - PoseMsg msg = new(buffer); - - // Do no process poses with nwid == 0 (== local) - if (msg.networkId == 0) - return true; - - client.messageQueue.Enqueue(msg); - return true; - } - - public static bool SendTo(Participant participant, Thing thing) { - if (participant == null || thing == null) - return false; - - byte ix = 0; - participant.buffer[ix++] = PoseMsg.Id; - participant.buffer[ix++] = participant.networkId; - participant.buffer[ix++] = thing.id; - participant.buffer[ix++] = thing.poseUpdated; - - if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position); - if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null) - LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation); - if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity); - if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity); - - return participant.SendBuffer(ix); - } - } - - #endregion Pose - - #region Text - - public class TextMsg : IMessage { - public const byte Id = 0xB0; - public string text; - - public TextMsg(byte[] buffer) : base(buffer) { } - public override void Deserialize(byte[] buffer) { - uint ix = 0; - uint strlen = buffer[ix++]; - this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen); - } - - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - TextMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } - } - - #endregion - - #region Destroy - - public class DestroyMsg : IMessage { - public const byte Id = 0x20; - public const byte length = 3; - public byte networkId; - public byte thingId; - - public DestroyMsg(byte[] buffer) : base(buffer) { } - - public override void Deserialize(byte[] buffer) { - this.networkId = buffer[0]; - this.thingId = buffer[1]; - } - - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - if (packetSize != length) - return false; - - byte[] buffer = await Receive(dataStream, packetSize); - DestroyMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } - } - - - #endregion Destroy } diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs index 3d99f18..cb709d2 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -8,7 +8,7 @@ namespace Passer.Control.Core { public byte networkId; public byte thingId; public byte len; - public string? name = null; + public string name = ""; public NameMsg(byte networkId, Thing thing) { this.networkId = networkId; @@ -29,7 +29,7 @@ namespace Passer.Control.Core { } public override byte Serialize(ref byte[] buffer) { - if (this.name == null) + if (this.name.Length == 0) return 0; byte ix = 0; diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs new file mode 100644 index 0000000..9ea0748 --- /dev/null +++ b/Messages/PoseMsg.cs @@ -0,0 +1,109 @@ +using Passer.LinearAlgebra; + +namespace Passer.Control.Core { + + public class PoseMsg : IMessage { + public const byte Id = 0x10; + public const byte length = 4 + 4 + 4; + public byte networkId; + public byte thingId; + + public byte poseType; + public const byte Pose_Position = 0x01; + public const byte Pose_Orientation = 0x02; + public const byte Pose_LinearVelocity = 0x04; + public const byte Pose_AngularVelocity = 0x08; + + public Spherical position = Spherical.zero; + public SwingTwist orientation = SwingTwist.zero; + public Spherical linearVelocity = Spherical.zero; + public Spherical angularVelocity = Spherical.zero; + + public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) { + this.networkId = networkId; + this.thingId = thingId; + + this.position = position; + this.orientation = orientation; + + this.poseType = 0; + if (this.position != null) + this.poseType |= Pose_Position; + if (this.orientation != null) + this.poseType |= Pose_Orientation; + } + public PoseMsg(byte[] buffer) : base(buffer) { } + + public override byte Serialize(ref byte[] buffer) { + byte ix = 0; + buffer[ix++] = PoseMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + buffer[ix++] = this.poseType; + + if ((poseType & Pose_Position) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.position); + if ((poseType & Pose_Orientation) != 0) + LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); + if ((poseType & Pose_LinearVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); + if ((poseType & Pose_AngularVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); + return ix; + } + public override void Deserialize(byte[] buffer) { + byte ix = 0; + this.networkId = buffer[ix++]; + this.thingId = buffer[ix++]; + this.poseType = buffer[ix++]; + + if ((poseType & Pose_Position) != 0) + this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + if ((poseType & Pose_Orientation) != 0) + this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); + if ((poseType & Pose_LinearVelocity) != 0) + this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + if ((poseType & Pose_AngularVelocity) != 0) + this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + } + + public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { + PoseMsg msg = new(client.networkId, thingId, position, orientation); + return SendMsg(client, msg); + } + public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { + byte[] buffer = await Receive(dataStream, packetSize); + PoseMsg msg = new(buffer); + + // Do no process poses with nwid == 0 (== local) + if (msg.networkId == 0) + return true; + + client.messageQueue.Enqueue(msg); + return true; + } + + public static bool SendTo(Participant participant, Thing thing) { + if (participant == null || thing == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = PoseMsg.Id; + participant.buffer[ix++] = participant.networkId; + participant.buffer[ix++] = thing.id; + participant.buffer[ix++] = thing.poseUpdated; + + if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position); + if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null) + LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation); + if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity); + if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity); + + return participant.SendBuffer(ix); + } + } + +} \ No newline at end of file diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs new file mode 100644 index 0000000..10f14c6 --- /dev/null +++ b/Messages/TextMsg.cs @@ -0,0 +1,22 @@ +namespace Passer.Control.Core { + + public class TextMsg(byte[] buffer) : IMessage(buffer) { + public const byte Id = 0xB0; + public string text = ""; + + public override void Deserialize(byte[] buffer) { + uint ix = 0; + uint strlen = buffer[ix++]; + this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen); + } + + public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { + byte[] buffer = await Receive(dataStream, packetSize); + TextMsg msg = new(buffer); + + client.messageQueue.Enqueue(msg); + return true; + } + } + +} \ No newline at end of file diff --git a/Participant.cs b/Participant.cs index d95d39c..2e19888 100644 --- a/Participant.cs +++ b/Participant.cs @@ -1,3 +1,4 @@ +#nullable enable using System; using System.Collections.Generic; using System.Collections.Concurrent; @@ -13,8 +14,8 @@ namespace Passer.Control.Core { //public byte networkId = 0; public string name = "Participant"; - public IPEndPoint endPoint = null; - public UdpClient udpClient = null; + public IPEndPoint? endPoint = null; + public UdpClient? udpClient = null; public string broadcastIpAddress = "255.255.255.255"; public readonly ConcurrentQueue messageQueue = new(); @@ -64,7 +65,7 @@ namespace Passer.Control.Core { public List senders = new(); - public RemoteParticipant GetParticipant(string ipAddress, int port) { + public RemoteParticipant? GetParticipant(string ipAddress, int port) { //Console.WriteLine($"Get Participant {ipAddress}:{port}"); foreach (RemoteParticipant sender in senders) { if (sender.ipAddress == ipAddress && sender.port == port) @@ -81,7 +82,7 @@ namespace Passer.Control.Core { return participant; } - protected readonly Dictionary> thingMsgProcessors = new(); + protected readonly Dictionary> thingMsgProcessors = new(); public delegate Thing ThingConstructor(byte networkId, byte thingId); public void Register(byte thingType, ThingConstructor constr) { @@ -93,7 +94,7 @@ namespace Passer.Control.Core { } public void Register(byte thingType) where ThingClass : Thing { - thingMsgProcessors[thingType] = (byte networkId, byte thingId) => + thingMsgProcessors[thingType] = static (byte networkId, byte thingId) => Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass; Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); } @@ -103,17 +104,20 @@ namespace Passer.Control.Core { #region Update protected void ReceiveUDP(IAsyncResult result) { - if (udpClient == null || this.endPoint == null) + if (this.udpClient == null || this.endPoint == null) return; - byte[] data = udpClient.EndReceive(result, ref this.endPoint); + byte[] data = this.udpClient.EndReceive(result, ref this.endPoint); // This does not yet take multi-packet messages into account! + if (this.endPoint == null) + return; // We can receive our own publish (broadcast) packages. How do we recognize them???? // It is hard to determine our source port - RemoteParticipant remoteParticipant = this.GetParticipant(endPoint.Address.ToString(), endPoint.Port); + string ipAddress = this.endPoint.Address.ToString(); + RemoteParticipant? remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); if (remoteParticipant == null) - remoteParticipant = this.AddParticipant(endPoint.Address.ToString(), endPoint.Port); + remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port); ReceiveData(data, remoteParticipant); @@ -285,7 +289,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, NameMsg msg) { // Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); - Thing thing = sender.Get(msg.networkId, msg.thingId); + Thing? thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; } @@ -298,7 +302,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, CustomMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); - Thing thing = sender.Get(msg.networkId, msg.thingId); + Thing? thing = sender.Get(msg.networkId, msg.thingId); thing?.ProcessBinary(msg.bytes); } diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 67d8b13..7dce8da 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -17,18 +17,19 @@ namespace Passer.Control.Core { this.port = port; } - protected readonly List things = new(); + protected readonly List things = []; - public Thing Get(byte networkId, byte thingId) { - Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); - // if (thing == null) - // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); + public Thing? Get(byte networkId, byte thingId) { + Thing? thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); return thing; } + // if (thing == null) + // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); + public void Add(Thing thing, bool invokeEvent = true) { // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); - Thing foundThing = Get(thing.networkId, thing.id); + Thing? foundThing = Get(thing.networkId, thing.id); if (foundThing == null) { things.Add(thing); diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs index 0845d21..b88387b 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -5,9 +5,9 @@ namespace Passer.Control.Core { public class DistanceSensor : Thing { public float distance = 0; - public DistanceSensor() : base(true) { } + public DistanceSensor(RemoteParticipant participant) : base(participant, true) { } - public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { + public DistanceSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { } #if UNITY_5_3_OR_NEWER diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index b6e61b5..4617d47 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -2,11 +2,8 @@ using System; namespace Passer.Control.Core { - public class TemperatureSensor : Thing { + public class TemperatureSensor(Participant participant, byte networkId, byte thingId) : Thing(participant, networkId, thingId, (byte)Type.TemperatureSensor) { public float temp = 0; - - public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { - } public override void ProcessBinary(byte[] bytes) { byte ix = 0; diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index 1553ee8..6e03864 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,10 +1,9 @@ - namespace Passer.Control.Core { public class TouchSensor : Thing { - //public Thing touchedThing = null; public bool touchedSomething = false; - public TouchSensor(bool invokeEvent = true) : base(invokeEvent) { + public TouchSensor(RemoteParticipant participant, bool invokeEvent = true) : base(participant, invokeEvent) { + touchedSomething = false; } #if UNITY_5_3_OR_NEWER diff --git a/SiteServer.cs b/SiteServer.cs index f44916b..0fbc4a4 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -23,10 +23,10 @@ namespace Passer.Control.Core { } public void Close() { - this.udpClient.Close(); + this.udpClient?.Close(); } - public override void Publish() { + public override void Publish() { } protected override void Process(RemoteParticipant sender, ClientMsg msg) { @@ -40,13 +40,15 @@ namespace Passer.Control.Core { protected override void Process(RemoteParticipant sender, ThingMsg msg) { //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); - Thing thing = sender.Get(msg.networkId, msg.thingId); + Thing? thing = sender.Get(msg.networkId, msg.thingId); if (thing == null) { - Thing newThing; - if (thingMsgProcessors.ContainsKey(msg.thingType)) - newThing = thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId); - else - newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + Thing? newThing = null; + if (thingMsgProcessors.TryGetValue(msg.thingType, out Func? value)) { + if (value != null) + newThing = value(msg.networkId, msg.thingId); + } + newThing ??= new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + sender.Add(newThing); } } diff --git a/Thing.cs b/Thing.cs index c78851a..34e61a5 100644 --- a/Thing.cs +++ b/Thing.cs @@ -41,16 +41,16 @@ namespace Passer.Control.Core { public byte networkId; public byte id; - public event ChangeHandler OnParentChanged; - private Thing _parent; - public Thing parent { + public event ChangeHandler OnParentChanged = delegate {}; + private Thing? _parent; + public Thing? parent { get => _parent; set { if (_parent == value) return; if (value == null) { - _parent.RemoveChild(this); + _parent?.RemoveChild(this); _parent = null; } else { @@ -74,8 +74,8 @@ namespace Passer.Control.Core { public List children = new List(); public byte type; - public event ChangeHandler OnNameChanged; - private string _name; + public event ChangeHandler OnNameChanged = delegate {}; + private string _name = ""; public virtual string name { get => _name; set { @@ -86,11 +86,11 @@ namespace Passer.Control.Core { } } - public string modelUrl; + public string modelUrl = ""; public byte poseUpdated = 0x00; - public event ChangeHandler OnPositionChanged; - private Spherical _position; + public event ChangeHandler OnPositionChanged = delegate {}; + private Spherical _position = Spherical.zero; public Spherical position { get { return _position; } set { @@ -101,8 +101,8 @@ namespace Passer.Control.Core { } } - public event ChangeHandler OnOrientationChanged; - private SwingTwist _orientation; + public event ChangeHandler OnOrientationChanged = delegate {}; + private SwingTwist _orientation = SwingTwist.zero; public SwingTwist orientation { get { return _orientation; } set { @@ -113,8 +113,8 @@ namespace Passer.Control.Core { } } - public event SphericalHandler OnLinearVelocityChanged; - private Spherical _linearVelocity; + public event SphericalHandler OnLinearVelocityChanged = delegate {}; + private Spherical _linearVelocity = Spherical.zero; public Spherical linearVelocity { get => _linearVelocity; set { @@ -124,7 +124,7 @@ namespace Passer.Control.Core { } } } - public Spherical angularVelocity; + public Spherical angularVelocity = Spherical.zero; #if UNITY_5_3_OR_NEWER [NonSerialized] @@ -135,12 +135,8 @@ namespace Passer.Control.Core { #region Init - // public virtual void Init(bool invokeEvent = false) { - // if (invokeEvent) - // InvokeNewThing(this); - // } - - public Thing(bool invokeEvent = false) { + public Thing(RemoteParticipant participant, bool invokeEvent = false) { + this.participant = participant; if (invokeEvent) InvokeNewThing(this); } @@ -149,9 +145,6 @@ namespace Passer.Control.Core { this.id = thingId; this.type = thingType; this.networkId = networkId; - //this.Init(); - //OnNewThing?.Invoke(this); - //Thing.Add(this); } public virtual void CreateComponent() {} @@ -195,10 +188,10 @@ namespace Passer.Control.Core { //---------- All Things - private static readonly List allThings = new(); + private static readonly List allThings = []; public delegate void ThingHandler(Thing t); - public static event ThingHandler OnNewThing; + public static event ThingHandler OnNewThing = delegate {}; public static void InvokeNewThing(Thing thing) { OnNewThing?.Invoke(thing); } diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 00fd906..51c6baa 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -65,7 +65,7 @@ namespace ControlCore.test { public void Test_ThingMsg() { SiteServer siteServer = new(); Participant participant = new("127.0.0.1"); - Thing thing = new() { + Thing thing = new(participant) { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" };