Improved debug logging

LinearAlgebra/src/Decomposition.cs

@@ -0,0 +1,287 @@
+using System;
+namespace LinearAlgebra {
+    class QR {
+        // QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (uint colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(ix) of A
+                float[] v = new float[nRows];
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    v[rowIx] = A.data[rowIx, colIx];
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    float dotProd = 0;
+                    for (int i = 0; i < nRows; i++)
+                        dotProd += Q[i, colIx2] * v[i];
+                    for (int i = 0; i < nRows; i++)
+                        v[i] -= dotProd * Q[i, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(ix) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += v[rowIx] * v[rowIx];
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = v[rowIx] / norm;
+
+                // Store the coefficients of R
+                for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
+                    R[colIx2, colIx] = 0;
+                    for (int k = 0; k < nRows; k++)
+                        R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
+                }
+            }
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+
+        // Reduced QR Decomposition of a matrix A
+        public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
+            int nRows = A.nRows;
+            int nCols = A.nCols;
+
+            float[,] Q = new float[nRows, nCols];
+            float[,] R = new float[nCols, nCols];
+
+            // Perform Gram-Schmidt orthogonalization
+            for (int colIx = 0; colIx < nCols; colIx++) {
+
+                // Step 1: v = column(colIx) of A
+                float[] columnIx = new float[nRows];
+                bool isZeroColumn = true;
+                for (int rowIx = 0; rowIx < nRows; rowIx++) {
+                    columnIx[rowIx] = A.data[rowIx, colIx];
+                    if (columnIx[rowIx] != 0)
+                        isZeroColumn = false;
+                }
+                if (isZeroColumn) {
+                    for (int rowIx = 0; rowIx < nRows; rowIx++)
+                        Q[rowIx, colIx] = 0;
+                    // Set corresponding R element to 0
+                    R[colIx, colIx] = 0;
+
+                    Console.WriteLine($"zero column {colIx}");
+
+                    continue;
+                }
+
+                // Step 2: Subtract projections of v onto previous q's (orthogonalize)
+                for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
+                    // Compute the dot product of v and column(colIx2) of Q
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
+                    // Subtract the projection from v
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
+                }
+
+                // Step 3: Normalize v to get column(colIx) of Q
+                float norm = 0;
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    norm += columnIx[rowIx] * columnIx[rowIx];
+                if (norm == 0)
+                    throw new Exception("invalid value");
+
+                norm = (float)Math.Sqrt(norm);
+
+                for (int rowIx = 0; rowIx < nRows; rowIx++)
+                    Q[rowIx, colIx] = columnIx[rowIx] / norm;
+
+                // Here is where it deviates from the Full QR Decomposition !
+
+                // Step 4: Compute the row(colIx) of R
+                for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
+                    float dotProduct = 0;
+                    for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
+                        dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
+                    R[colIx, colIx2] = dotProduct;
+                }
+            }
+            if (!float.IsFinite(R[0, 0]))
+                throw new Exception("invalid value");
+
+            return (new Matrix2(Q), new Matrix2(R));
+        }
+    }
+
+    class SVD {
+        // According to ChatGPT, Mathnet uses  Golub-Reinsch SVD algorithm
+        // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
+        // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
+        // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
+        //
+        // 2. Diagonalization: Once the matrix is in bidiagonal form,
+        // the singular values are computed using an iterative process
+        // (typically involving QR factorization or Jacobi rotations) until convergence.
+        // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
+        //
+        // 3. Computing UU and VTVT: After obtaining the singular values,
+        // the left singular vectors UU and right singular vectors VTVT are computed
+        // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.
+
+        // Bidiagnolizations through Householder transformations
+        public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] U1 = new float[m, m]; // Left orthogonal matrix
+            float[,] V1 = new float[n, n]; // Right orthogonal matrix
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Initialize U1 and V1 as identity matrices
+            for (int i = 0; i < m; i++)
+                U1[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                V1[i, i] = 1;
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Step 4: Update U1 with the Householder reflection (U1 * Householder)
+                for (int i = j; i < m; i++)
+                    U1[i, j] = y[i - j] / alpha;
+
+                // Step 5: Update V1 (storing the Householder vector y)
+                // Correct indexing: we only need to store part of y in V1 from index j to n
+                for (int i = j; i < n; i++)
+                    V1[j, i] = B[j, i];
+
+                // Repeat steps for further columns if necessary
+            }
+            return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
+        }
+
+        public static Matrix2 Bidiagonalize(Matrix2 A) {
+            int m = A.nRows; // Rows of A
+            int n = A.nCols; // Columns of A
+            float[,] B = A.Clone().data; // Copy A to B for transformation
+
+            // Perform Householder reflections to create a bidiagonal matrix B
+            for (int j = 0; j < n; j++) {
+                // Step 1: Construct the Householder vector y
+                float[] y = new float[m - j];
+                for (int i = j; i < m; i++)
+                    y[i - j] = B[i, j];
+
+                // Step 2: Compute the norm and scalar alpha
+                float norm = 0;
+                for (int i = 0; i < y.Length; i++)
+                    norm += y[i] * y[i];
+                norm = (float)Math.Sqrt(norm);
+
+                if (B[j, j] > 0)
+                    norm = -norm;
+
+                float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
+
+                // Step 3: Apply the reflection to zero out below diagonal
+                for (int k = j; k < n; k++) {
+                    float dot = 0;
+                    for (int i = j; i < m; i++)
+                        dot += y[i - j] * B[i, k];
+                    dot /= r;
+
+                    for (int i = j; i < m; i++)
+                        B[i, k] -= 2 * dot * y[i - j];
+                }
+
+                // Repeat steps for further columns if necessary
+            }
+            return new Matrix2(B);
+        }
+
+        // QR Iteration for diagonalization of a bidiagonal matrix B
+        public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
+            int m = B.nRows;
+            int n = B.nCols;
+
+            Matrix2 U = new(m, m); // Left singular vectors (U)
+            Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
+            float[] singularValues = new float[Math.Min(m, n)]; // Singular values
+
+            // Initialize U and Vt as identity matrices
+            for (int i = 0; i < m; i++)
+                U.data[i, i] = 1;
+            for (int i = 0; i < n; i++)
+                Vt.data[i, i] = 1;
+
+            // Perform QR iterations
+            float tolerance = 1e-7f; //1e-12f; for double
+            bool converged = false;
+            while (!converged) {
+                // Perform QR decomposition on the matrix B
+                (Matrix2 Q, Matrix2 R) = QR.Decomposition(B);
+
+                // Update B to be the product Q * R //R * Q
+                B = R * Q;
+
+                // Accumulate the transformations in U and Vt
+                U *= Q;
+                Vt *= R;
+
+                // Check convergence by looking at the off-diagonal elements of B
+                converged = true;
+                for (int i = 0; i < m - 1; i++) {
+                    for (int j = i + 1; j < n; j++) {
+                        if (Math.Abs(B.data[i, j]) > tolerance) {
+                            converged = false;
+                            break;
+                        }
+                    }
+                }
+            }
+
+            // Extract singular values (diagonal elements of B)
+            for (int i = 0; i < Math.Min(m, n); i++)
+                singularValues[i] = B.data[i, i];
+
+            return (new Matrix1(singularValues), U, Vt);
+        }
+
+        public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
+            if (A.nRows != A.nCols)
+                throw new ArgumentException("SVD: matrix A has to be square.");
+
+            Matrix2 B = Bidiagonalize(A);
+            (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
+            return (U, S, Vt);
+        }
+    }
+}

Unity/SiteServer.cs

@@ -18,6 +18,7 @@ namespace RoboidControl.Unity {
         }
 
         void OnApplicationQuit() {
+            if (site != null)
                 site.Close();
         }
 

Unity/Thing.cs

@@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
             core.component = this;
 
             SiteServer siteServer = FindAnyObjectByType<SiteServer>();
-            if (siteServer == null) {
+            if (siteServer == null || siteServer.site == null) {
                 Debug.LogWarning("No site server found");
                 return;
             }
@@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
         }
 
         private void PoseChanged() {
-            Debug.Log($"{this} pose changed");
+            // Debug.Log($"{this} pose changed");
             if (core.positionUpdated)
                 this.transform.localPosition = core.position.ToVector3();
             if (core.orientationUpdated)

src/Messages/BinaryMsg.cs

@@ -12,7 +12,7 @@ namespace RoboidControl {
         /// The length of the message, excluding the binary data
         /// </summary>
         /// For the total size of the message this.bytes.Length should be added to this value.
-        public const byte length = 2;
+        public const byte length = 4;
         /// <summary>
         /// The network ID identifying the thing
         /// </summary>
@@ -23,23 +23,15 @@ namespace RoboidControl {
         public byte thingId;
         
         public Thing thing;        
-
+        /// <summary>
+        /// The length of the data
+        /// </summary>
+        public byte dataLength;
         /// <summary>
         /// The binary data
         /// </summary>
-        public byte[] bytes;
+        public byte[] data;
 
-        /// <summary>
-        /// Create a new message for sending
-        /// </summary>
-        /// <param name="networkId">The netowork ID of the thing</param>
-        /// <param name="thingId">The ID of the thing</param>
-        /// <param name="bytes">The binary data for the thing</param>
-        public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
-            this.networkId = networkId;
-            this.thingId = thingId;
-            this.bytes = bytes;
-        }
         /// <summary>
         /// Create an empty message for sending
         /// </summary>
@@ -49,31 +41,34 @@ namespace RoboidControl {
             this.networkId = networkId;
             this.thingId = thing.id;
             this.thing = thing;
-            this.bytes = this.thing.GenerateBinary();
+            this.data = this.thing.GenerateBinary();
-            // if (this.bytes.Length > 0)
+            this.dataLength = (byte)this.data.Length;
-            //     System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
         }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public BinaryMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
-            byte length = (byte)(buffer.Length - ix);
+            this.dataLength = buffer[ix++];
-            this.bytes = new byte[length];
+            this.data = new byte[this.dataLength];
-            for (uint bytesIx = 0; bytesIx < length; bytesIx++)
+            for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
-                this.bytes[bytesIx] = buffer[ix++];
+                this.data[dataIx] = buffer[ix++];
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {            
-            if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
+            if (buffer.Length < BinaryMsg.length + this.data.Length || this.data.Length == 0)
                 return 0;
             
+#if DEBUG
+            System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
+#endif
             byte ix = 0;
             buffer[ix++] = BinaryMsg.Id;
             buffer[ix++] = this.networkId;
             buffer[ix++] = this.thingId;
-            foreach (byte b in bytes)
+            buffer[ix++] = this.dataLength;
+            foreach (byte b in data)
                 buffer[ix++] = b;
 
             return ix;

src/Messages/ModelUrlMsg.cs

@@ -21,6 +21,10 @@ namespace RoboidControl {
         /// </summary>
         public byte thingId;
         /// <summary>
+        /// The length of the url string, excluding the null terminator
+        /// </summary>
+        public byte urlLength;
+        /// <summary>
         /// The URL of the model
         /// </summary>
         public string url = null;
@@ -33,6 +37,7 @@ namespace RoboidControl {
         public ModelUrlMsg(byte networkId, Thing thing) {
             this.networkId = networkId;
             this.thingId = thing.id;
+            this.urlLength = (byte)thing.modelUrl.Length;
             this.url = thing.modelUrl;
         }
         /// <summary>
@@ -41,26 +46,30 @@ namespace RoboidControl {
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="url">The URL to send</param>
-        public ModelUrlMsg(byte networkId, byte thingId, string url) {
+        // public ModelUrlMsg(byte networkId, byte thingId, string url) {
-            this.networkId = networkId;
+        //     this.networkId = networkId;
-            this.thingId = thingId;
+        //     this.thingId = thingId;
-            this.url = url;
+        //     this.urlLength = (byte)url.Length;
-        }
+        //     this.url = url;
+        // }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public ModelUrlMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
 
-            int strlen = buffer[ix++];
+            this.urlLength = buffer[ix++];
-            url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
+            this.url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.urlLength);
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
-            if (this.url == null)
+            if (string.IsNullOrEmpty(this.url))
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send ModelUrlMsg [{this.networkId}/{this.thingId}] {this.urlLength} {this.url}");
+#endif
             byte ix = 0;
             buffer[ix++] = ModelUrlMsg.Id;
             buffer[ix++] = this.networkId;

src/Messages/NameMsg.cs

@@ -20,10 +20,11 @@ namespace RoboidControl {
         /// The ID of the thing
         /// </summary>
         public byte thingId;
+
         /// <summary>
         /// The length of the name, excluding null terminator
         /// </summary>
-        public byte len;
+        public byte nameLength;
         /// <summary>
         /// The name of the thing, not terminated with a null character
         /// </summary>
@@ -38,6 +39,7 @@ namespace RoboidControl {
             this.networkId = networkId;
             this.thingId = thing.id;
             this.name = thing.name;
+            this.nameLength = (byte)this.name.Length;
         }
         /// <summary>
         /// Create a new message for sending
@@ -45,34 +47,34 @@ namespace RoboidControl {
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="name">The name of the thing</param>
-        public NameMsg(byte networkId, byte thingId, string name) {
+        // public NameMsg(byte networkId, byte thingId, string name) {
-            this.networkId = networkId;
+        //     this.networkId = networkId;
-            this.thingId = thingId;
+        //     this.thingId = thingId;
-            this.name = name;
+        //     this.name = name;
-        }
+        // }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public NameMsg(byte[] buffer) {
             byte ix = 1;
             this.networkId = buffer[ix++];
             this.thingId = buffer[ix++];
-            int strlen = buffer[ix++];
+            this.nameLength = buffer[ix++];
-            this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
+            this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
-            if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
+            if (buffer.Length < NameMsg.length + this.name.Length || string.IsNullOrEmpty(this.name))
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send NameMsg [{this.networkId}/{this.thingId}] {this.nameLength} {this.name}");
+#endif
             byte ix = 0;
             buffer[ix++] = NameMsg.Id;
             buffer[ix++] = this.networkId;
             buffer[ix++] = this.thingId;
-
+            buffer[ix++] = this.nameLength;
-            int nameLength = this.name.Length;
+            for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
-
-            buffer[ix++] = (byte)nameLength;
-            for (int nameIx = 0; nameIx < nameLength; nameIx++)
                 buffer[ix++] = (byte)this.name[nameIx];
 
             return ix;

src/Messages/NetworkIdMsg.cs

@@ -34,6 +34,9 @@ namespace RoboidControl {
             if (buffer.Length < NetworkIdMsg.length)
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send NetworkIdMsg {this.networkId}");
+#endif
             buffer[0] = NetworkIdMsg.Id;
             buffer[1] = this.networkId;
             return NetworkIdMsg.length;

src/Messages/PoseMsg.cs

@@ -88,6 +88,34 @@ namespace RoboidControl {
             this.linearVelocity = linearVelocity;
             this.angularVelocity = angularVelocity;
         }
+        public PoseMsg(byte networkId, Thing thing, bool force = false) {
+            this.networkId = networkId;
+            this.thingId = thing.id;
+
+            this.poseType = 0;
+            if (thing.positionUpdated || force) {
+                this.position = thing.position;
+                this.poseType |= Pose_Position;
+                //thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
+            }
+            if (thing.orientationUpdated || force) {
+                this.orientation = thing.orientation;
+                this.poseType |= Pose_Orientation;
+                //thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
+            }
+            if (thing.linearVelocityUpdated) {
+                this.linearVelocity = thing.linearVelocity;
+                this.poseType |= Pose_LinearVelocity;
+                thing.linearVelocityUpdated = false;
+            }
+            if (thing.angularVelocityUpdated) {
+                this.angularVelocity = thing.angularVelocity;
+                this.poseType |= Pose_AngularVelocity;
+                thing.angularVelocityUpdated = false;
+            }
+
+        }
+
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public PoseMsg(byte[] buffer) : base(buffer) {
             byte ix = 1;

src/Messages/TextMsg.cs

@@ -13,6 +13,10 @@ namespace RoboidControl {
         /// </summary>
         public const byte length = 2;
         /// <summary>
+        /// The length of the text without the null terminator
+        /// </summary>
+        public byte textLength;
+        /// <summary>
         /// The text
         /// </summary>
         public string text = "";
@@ -22,20 +26,27 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="text">The text to send</param>
         public TextMsg(string text) {
+            this.textLength = (byte)text.Length;
             this.text = text;
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
-        public TextMsg(byte[] buffer) : base(buffer) { }
+        public TextMsg(byte[] buffer) : base(buffer) {
+            this.textLength = buffer[0];
+            this.text = System.Text.Encoding.UTF8.GetString(buffer, 1, this.textLength);
+         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {
             if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0)
                 return 0;
 
+#if DEBUG
+            System.Console.WriteLine($"Send TextMsg {this.textLength} {this.text}");
+#endif
             byte ix = 0;
             buffer[ix++] = TextMsg.Id;
-            buffer[ix++] = (byte)this.text.Length;
+            buffer[ix++] = this.textLength;
             for (int textIx = 0; textIx < this.text.Length; textIx++)
                 buffer[ix++] = (byte)this.text[textIx];
             return ix;

src/Messages/ThingMsg.cs

@@ -77,6 +77,9 @@ namespace RoboidControl {
             if (buffer.Length < ThingMsg.length)
                 return 0;
                 
+#if DEBUG
+            System.Console.WriteLine($"Send ThingMsg [{this.networkId}/{this.thingId}] {this.thingType} {this.parentId}");
+#endif
             byte ix = 0;
             buffer[ix++] = ThingMsg.id;
             buffer[ix++] = this.networkId;

src/Participant.cs

@@ -44,6 +44,16 @@ namespace RoboidControl {
         /// </summary>
         protected readonly List<Thing> things = new List<Thing>();
 
+        public virtual void Update(ulong currentTimeMS = 0) {
+            int n = this.things.Count;
+            for (int ix = 0; ix < n; ix++) {
+                Thing thing = this.things[ix];
+                if (thing != null)
+                    thing.Update(currentTimeMS, true);                
+            }
+
+        }
+
         /// <summary>
         /// Get a thing with the given ids
         /// </summary>

src/ParticipantUDP.cs

@@ -9,7 +9,7 @@ namespace RoboidControl {
     /// <summary>
     /// A participant is used for communcation between things
     /// </summary>
-    public class LocalParticipant : Participant {
+    public class ParticipantUDP : Participant {
         public byte[] buffer = new byte[1024];
         public ulong publishInterval = 3000; // = 3 seconds
 
@@ -26,7 +26,7 @@ namespace RoboidControl {
         /// <summary>
         /// Create a porticiapnt
         /// </summary>
-        public LocalParticipant() {
+        public ParticipantUDP() {
             //senders.Add(this);
         }
 
@@ -34,7 +34,7 @@ namespace RoboidControl {
         /// Create a participant with the give UDP port
         /// </summary>
         /// <param name="port">The port number on which to communicate</param>
-        public LocalParticipant(int port) : this() {
+        public ParticipantUDP(int port) : this() {
             this.port = port;
         }
 
@@ -43,7 +43,7 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="ipAddress">The ip address of the site server</param>
         /// <param name="port">The port number of the site server</param>
-        public LocalParticipant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
+        public ParticipantUDP(string ipAddress = "0.0.0.0", int port = 7681) : this() {
             this.ipAddress = ipAddress;
             this.port = port;
 
@@ -61,15 +61,15 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="udpClient">UDP client to use for communication</param>
         /// <param name="port">The port number on which to communicate</param>
-        public LocalParticipant(UdpClient udpClient, int port) : this() {
+        public ParticipantUDP(UdpClient udpClient, int port) : this() {
             this.udpClient = udpClient;
             this.port = port;
         }
 
-        private static LocalParticipant isolatedParticipant = null;
+        private static ParticipantUDP isolatedParticipant = null;
-        public static LocalParticipant Isolated() {
+        public static ParticipantUDP Isolated() {
             if (isolatedParticipant == null)
-                isolatedParticipant = new LocalParticipant(0);
+                isolatedParticipant = new ParticipantUDP(0);
             return isolatedParticipant;
         }
 
@@ -94,20 +94,20 @@ namespace RoboidControl {
 
         protected readonly Dictionary<byte, Func<Participant, byte, byte, Thing>> thingMsgProcessors = new();
 
-        public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
+        // public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
-        public void Register(byte thingType, ThingConstructor constr) {
+        // public void Register(byte thingType, ThingConstructor constr) {
-            thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
+        //     thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
-        }
+        // }
 
-        public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
+        // public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
-            Register<ThingClass>((byte)thingType);
+        //     Register<ThingClass>((byte)thingType);
-        }
+        // }
 
-        public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
+        // public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
-            thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
+        //     thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
-                Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
+        //         Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
-            Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
+        //     Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
-        }
+        // }
 
         #endregion Init
 
@@ -134,7 +134,7 @@ namespace RoboidControl {
         }
 
         protected ulong nextPublishMe = 0;
-        public virtual void Update(ulong currentTimeMS = 0) {
+        public override void Update(ulong currentTimeMS = 0) {
             if (currentTimeMS == 0) {
 #if UNITY_5_3_OR_NEWER
                 currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
@@ -152,13 +152,20 @@ namespace RoboidControl {
                 Thing thing = this.things[ix];
                 if (thing != null && thing.parent == null) {
                     thing.Update(currentTimeMS, true);
-                    // if (thing.owner != this) {
+                    // if (this.isIsolated == false) {
-                    //     BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                    if (thing.owner != this) { // should not happen....
-                    //     this.Send(thing.owner, binaryMsg);
+                        PoseMsg poseMsg = new(thing.owner.networkId, thing);
-                    // }
+                        this.Send(thing.owner, poseMsg);
+                        BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                        this.Send(thing.owner, binaryMsg);
                     }
                 }
             }
+            for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) {
+                Participant owner = this.owners[ownerIx];
+                owner.Update(currentTimeMS);
+            }
+        }
 
         public virtual void Publish() {
             this.Publish(new ParticipantMsg(this.networkId));
@@ -169,7 +176,7 @@ namespace RoboidControl {
         #region Send
 
         public void SendThingInfo(Participant owner, Thing thing) {
-            Console.WriteLine("Send thing info");
+            // Console.WriteLine("Send thing info");
             this.Send(owner, new ThingMsg(this.networkId, thing));
             this.Send(owner, new NameMsg(this.networkId, thing));
             this.Send(owner, new ModelUrlMsg(this.networkId, thing));
@@ -188,7 +195,7 @@ namespace RoboidControl {
         }
 
         public void PublishThingInfo(Thing thing) {
-            //Console.WriteLine("Publish thing info");
+            // Console.WriteLine("Publish thing info");
             this.Publish(new ThingMsg(this.networkId, thing));
             this.Publish(new NameMsg(this.networkId, thing));
             this.Publish(new ModelUrlMsg(this.networkId, thing));
@@ -200,7 +207,7 @@ namespace RoboidControl {
             if (bufferSize <= 0)
                 return true;
 
-            // Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
+            Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
             this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
             return true;
         }
@@ -227,7 +234,7 @@ namespace RoboidControl {
 
         #region Receive
 
-        public void ReceiveData(byte[] data, Participant remoteParticipant) {
+        public void ReceiveData(byte[] data, Participant sender) {
             byte msgId = data[0];
             if (msgId == 0xFF) {
                 // Timeout
@@ -236,29 +243,29 @@ namespace RoboidControl {
 
             switch (msgId) {
                 case ParticipantMsg.Id: // 0xA0 / 160
-                    this.Process(remoteParticipant, new ParticipantMsg(data));
+                    this.Process(sender, new ParticipantMsg(data));
                     break;
                 case NetworkIdMsg.Id: // 0xA1 / 161
-                    this.Process(remoteParticipant, new NetworkIdMsg(data));
+                    this.Process(sender, new NetworkIdMsg(data));
                     break;
                 case InvestigateMsg.Id: // 0x81
                     // result = await InvestigateMsg.Receive(dataStream, client, packetSize);
                     break;
                 case ThingMsg.id: // 0x80 / 128
-                    this.Process(remoteParticipant, new ThingMsg(data));
+                    this.Process(sender, new ThingMsg(data));
                     break;
                 case NameMsg.Id: // 0x91 / 145
-                    this.Process(remoteParticipant, new NameMsg(data));
+                    this.Process(sender, new NameMsg(data));
                     break;
                 case ModelUrlMsg.Id: // 0x90 / 144
-                    this.Process(remoteParticipant, new ModelUrlMsg(data));
+                    this.Process(sender, new ModelUrlMsg(data));
                     break;
                 case PoseMsg.Id: // 0x10 / 16
-                    this.Process(remoteParticipant, new PoseMsg(data));
+                    this.Process(sender, new PoseMsg(data));
                     // result = await PoseMsg.Receive(dataStream, client, packetSize);
                     break;
                 case BinaryMsg.Id: // 0xB1 / 177
-                    this.Process(remoteParticipant, new BinaryMsg(data));
+                    this.Process(sender, new BinaryMsg(data));
                     break;
                 case TextMsg.Id: // 0xB0 / 176
                     // result = await TextMsg.Receive(dataStream, client, packetSize);
@@ -275,10 +282,17 @@ namespace RoboidControl {
 
         #region Process
 
-        protected virtual void Process(Participant sender, ParticipantMsg msg) { }
+        protected virtual void Process(Participant sender, ParticipantMsg msg) {
+#if DEBUG
+            Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
+#endif
+        }
 
         protected virtual void Process(Participant sender, NetworkIdMsg msg) {
-            Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
+#if DEBUG            
+            Console.WriteLine($"{this.name} Process SiteMsg {this.networkId} -> {msg.networkId}");
+#endif
+
             if (this.networkId != msg.networkId) {
                 this.networkId = msg.networkId;
                 foreach (Thing thing in this.things) //Thing.GetAllThings())
@@ -286,53 +300,76 @@ namespace RoboidControl {
             }
         }
 
-        protected virtual void Process(Participant sender, InvestigateMsg msg) { }
+        protected virtual void Process(Participant sender, InvestigateMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: InvestigateMsg [{msg.networkId}/{msg.thingId}]");
+#endif
+        }
 
         protected virtual void Process(Participant sender, ThingMsg msg) {
-            //Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}");
+#endif            
         }
 
         protected virtual void Process(Participant sender, NameMsg msg) {
-            Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}");
+#endif
+
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null)
                 thing.name = msg.name;
         }
 
         protected virtual void Process(Participant sender, ModelUrlMsg msg) {
-            Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}");
+#endif
+
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null)
                 thing.modelUrl = msg.url;
         }
 
         protected virtual void Process(Participant sender, PoseMsg msg) {
-            //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+#endif            
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing != null) {
                 if ((msg.poseType & PoseMsg.Pose_Position) != 0)
                     thing.position = msg.position;
-                if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
+                if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
                     thing.orientation = msg.orientation;
-                    Console.Write($"{thing.id} orientation changed");
-                }
                 if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
                     thing.linearVelocity = msg.linearVelocity;
                 if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
                     thing.angularVelocity = msg.angularVelocity;
-
             }
         }
 
         protected virtual void Process(Participant sender, BinaryMsg msg) {
-            // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
+#if DEBUG            
+            Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
+#endif            
             Thing thing = sender.Get(msg.networkId, msg.thingId);
-            thing?.ProcessBinary(msg.bytes);
+            thing?.ProcessBinary(msg.data);
         }
 
-        protected virtual void Process(Participant sender, TextMsg temsgxt) { }
+        protected virtual void Process(Participant sender, TextMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: Process TextMsg {msg.textLength} {msg.text}");
+#endif            
 
-        protected virtual void Process(Participant sender, DestroyMsg msg) { }
+        }
+
+        protected virtual void Process(Participant sender, DestroyMsg msg) {
+#if DEBUG
+            Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]");
+#endif            
+
+        }
 
         private void ForwardMessage(IMessage msg) {
             // foreach (Participant client in senders) {

src/SiteServer.cs

@@ -7,7 +7,7 @@ namespace RoboidControl {
     /// <summary>
     /// A site server is a participant which provides a shared simulated environment 
     /// </summary>
-    public class SiteServer : LocalParticipant {
+    public class SiteServer : ParticipantUDP {
 
         public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
 
@@ -29,7 +29,7 @@ namespace RoboidControl {
                 new AsyncCallback(result => ReceiveUDP(result)),
                 new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
 
-            Register<TouchSensor>(Thing.Type.TouchSensor);
+            // Register<TouchSensor>(Thing.Type.TouchSensor);
         }
 
         /// <summary>
@@ -42,11 +42,12 @@ namespace RoboidControl {
         public override void Publish() {
         }
 
-        protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
+        protected override void Process(Participant sender, ParticipantMsg msg) {
-            if (msg.networkId == 0) {
+            base.Process(sender, msg);
-                Console.WriteLine($"{this.name} received New Participant -> {remoteParticipant.networkId}");
+            //if (msg.networkId == 0) {
-                this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.networkId));
+                //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
-            }
+                this.Send(sender, new NetworkIdMsg(sender.networkId));
+            //}
         }
 
         protected override void Process(Participant sender, NetworkIdMsg msg) { }
@@ -56,15 +57,15 @@ namespace RoboidControl {
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing == null) {
                 Thing newThing = null;
-                if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
+                // if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
-                    //Console.WriteLine("Found thing message processor");
+                //     //Console.WriteLine("Found thing message processor");
-                    if (msgProcessor != null)
+                //     if (msgProcessor != null)
-                        newThing = msgProcessor(sender, msg.networkId, msg.thingId);
+                //         newThing = msgProcessor(sender, msg.networkId, msg.thingId);
-                }
+                // }
-                if (newThing == null) {
+                // if (newThing == null) {
                     newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
                     // Console.WriteLine("Created generic new core thing");
-                }
+                // }
                 if (msg.parentId != 0) {
                     Thing parentThing = Get(msg.networkId, msg.parentId);
                     if (parentThing == null)

src/Thing.cs

@@ -49,11 +49,12 @@ namespace RoboidControl {
         public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
             this.owner = owner;
             this.type = thingType;
+            if (this.owner != null)
                 this.owner.Add(this);
             if (invokeEvent)
                 InvokeNewThing(this);
         }
-        public Thing(Participant owner) : this(owner, Type.Undetermined) {}
+        public Thing(Participant owner) : this(owner, Type.Undetermined) { }
         /// <summary>
         /// Create a new thing for a participant
         /// </summary>
@@ -65,7 +66,7 @@ namespace RoboidControl {
         /// Create a new thing without communication abilities
         /// </summary>
         /// <param name="thingType">The type of thing</param>
-        public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(LocalParticipant.Isolated(), thingType, invokeEvent) {
+        public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, invokeEvent) {
         }
 
         /// <summary>
@@ -286,10 +287,6 @@ namespace RoboidControl {
             }
         }
         /// <summary>
-        /// Event triggered when the orientation has changed
-        /// </summary>
-        //public event ChangeHandler OnOrientationChanged = delegate { };
-        /// <summary>
         /// Boolean indicating the thing has an updated orientation
         /// </summary>
         public bool orientationUpdated = false;
@@ -303,7 +300,7 @@ namespace RoboidControl {
             set {
                 if (_linearVelocity != value) {
                     _linearVelocity = value;
-                    hasLinearVelocity = _linearVelocity.distance != 0;
+                    linearVelocityUpdated = true;
                     OnLinearVelocityChanged?.Invoke(_linearVelocity);
                 }
             }
@@ -315,7 +312,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated linear velocity
         /// </summary>
-        public bool hasLinearVelocity = false;
+        public bool linearVelocityUpdated = false;
 
         private Spherical _angularVelocity = Spherical.zero;
         /// <summary>
@@ -326,7 +323,7 @@ namespace RoboidControl {
             set {
                 if (_angularVelocity != value) {
                     _angularVelocity = value;
-                    hasAngularVelocity = _angularVelocity.distance != 0;
+                    angularVelocityUpdated = true;
                     OnAngularVelocityChanged?.Invoke(_angularVelocity);
                 }
             }
@@ -338,7 +335,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated angular velocity
         /// </summary>
-        public bool hasAngularVelocity = false;
+        public bool angularVelocityUpdated = false;
 
 #if UNITY_5_3_OR_NEWER
         /// <summary>

src/Things/DifferentialDrive.cs

@@ -8,7 +8,7 @@ namespace RoboidControl {
         public DifferentialDrive() { }
         /// @brief Create a differential drive with networking support
         /// @param participant The local participant
-        public DifferentialDrive(LocalParticipant participant) : base(participant, Type.Undetermined) { }
+        public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
 
         /// @brief Configures the dimensions of the drive
         /// @param wheelDiameter The diameter of the wheels in meters

src/Things/TouchSensor.cs

@@ -26,7 +26,7 @@ namespace RoboidControl {
 
         public TouchSensor(Thing parent) : base(parent) { }
 
-        public LocalParticipant thisParticipant;
+        public ParticipantUDP thisParticipant;
 
         /// <summary>
         /// Value which is true when the sensor is touching something, false otherwise