Merge commit '2cdf8f61037fe583c4fa2e9a856e899c255cf638'

Examples/BB2B/BB2B.cs

@@ -9,7 +9,7 @@ namespace RoboidControl {
         readonly TouchSensor touchRight;
         const float speed = 0.5f;
 
-        public BB2B(Participant owner) : base(owner) {
+        public BB2B(Thing parent = default) : base(parent) {
             this.name = "BB2B";
             this.SetMotors(new Motor(this), new Motor(this));
             this.SetDriveDimensions(0.064f, 0.128f);

Examples/BB2B/BB2B_Encoder.cs

@@ -9,15 +9,15 @@ namespace RoboidControl {
         readonly TouchSensor touchRight;
         const float speed = 180.0f; // wheel rotation speed in degrees
 
-        public BB2B_Encoder(Participant owner) : base(owner) {
+        public BB2B_Encoder(Thing parent) : base(parent) {
             this.name = "BB2B";
             this.SetDriveDimensions(0.064f, 0.128f);
 
             // Update the basic motors to motors with encoder
-            EncoderMotor leftMotor = new(this, new RelativeEncoder()) {
+            ControlledMotor leftMotor = new(this, new RelativeEncoder()) {
                 position = new Spherical(0.064f, Direction.left)
             };
-            EncoderMotor rightMotor = new(this, new RelativeEncoder()) {
+            ControlledMotor rightMotor = new(this, new RelativeEncoder()) {
                 position = new Spherical(0.064f, Direction.right)
             };
             this.SetMotors(leftMotor, rightMotor);

Examples/BB2B/Program.cs

@@ -3,7 +3,7 @@ using RoboidControl;
 
 class Program {
     static void Main() {
-        BB2B bb2b = new(ParticipantUDP.Isolated());
+        BB2B bb2b = new();
 
         while (true) {
             bb2b.Update();

LinearAlgebra/src/Angle.cs

@@ -1,17 +1,128 @@
 using System;
+using System.Reflection.Emit;
 
 namespace LinearAlgebra {
 
-    /// <summary>
+    public class Angle {
-    /// %Angle utilities
-    /// </summary>
-    public static 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;
-
         public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
-        public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F;
+        public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F;
+
+        private Angle(float degrees) {
+            this.value = degrees;
+        }
+        private readonly float value = 0;
+
+        public static readonly Angle zero = new(0);
+
+        public static Angle Degrees(float degrees) {
+            // Reduce it to (-180..180]
+            if (float.IsFinite(degrees)) {
+                while (degrees < -180)
+                    degrees += 360;
+                while (degrees >= 180)
+                    degrees -= 360;
+            }
+            return new Angle(degrees);
+        }
+
+        public static Angle Radians(float radians) {
+            // Reduce it to (-pi..pi]
+            if (float.IsFinite(radians)) {
+                while (radians <= -Math.PI)
+                    radians += 2 * (float)Math.PI;
+                while (radians > Math.PI)
+                    radians -= 2 * (float)Math.PI;
+            }
+
+            return new Angle(radians * Rad2Deg);
+
+        }
+
+        public static Angle Revolutions(float revolutions) {
+            // reduce it to (-0.5 .. 0.5]
+            if (float.IsFinite(revolutions)) {
+                // Get the integer part
+                int integerPart = (int)revolutions;
+
+                // Get the decimal part
+                revolutions -= integerPart;
+                if (revolutions < -0.5)
+                    revolutions += 1;
+                if (revolutions >= 0.5)
+                    revolutions -= 1;
+            }
+            return new Angle(revolutions * 360);
+        }
+
+        public float inDegrees {
+            get { return this.value; }
+        }
+
+        public float inRadians {
+            get { return this.value * Deg2Rad; }
+        }
+
+        public float inRevolutions {
+            get { return this.value / 360.0f; }
+        }
+
+        /// <summary>
+        /// Get the sign of the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>-1 when the angle is negative, 1 when it is positive and 0 in all other cases</returns>
+        public static int Sign(Angle a) {
+            if (a.value < 0)
+                return -1;
+            if (a.value > 0)
+                return 1;
+            return 0;
+        }
+
+        /// <summary>
+        /// Returns the magnitude of the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>The positive magnitude of the angle</returns>
+        /// Negative values are negated to get a positive result
+        public static Angle Abs(Angle a) {
+            if (Sign(a) < 0)
+                return -a;
+            else
+                return a;
+        }
+
+        /// <summary>
+        /// Negate the angle
+        /// </summary>
+        /// <param name="a">The angle</param>
+        /// <returns>The negated angle</returns>
+        /// The negation of -180 is still -180 because the range is (-180..180]
+        public static Angle operator -(Angle a) {
+            Angle r = new(-a.value);
+            return r;
+        }
+
+        /// <summary>
+        /// Subtract two angles
+        /// </summary>
+        /// <param name="a1">Angle 1</param>
+        /// <param name="a2">Angle 2</param>
+        /// <returns>The result of the subtraction</returns>
+        public static Angle operator -(Angle a1, Angle a2) {
+            Angle r = new(a1.value - a2.value);
+            return r;
+        }
+        /// <summary>
+        /// Add two angles
+        /// </summary>
+        /// <param name="a1">Angle 1</param>
+        /// <param name="a2">Angle 2</param>
+        /// <returns>The result of the addition</returns>
+        public static Angle operator +(Angle a1, Angle a2) {
+            Angle r = new(a1.value + a2.value);
+            return r;
+        }
 
         /// <summary>
         /// Clamp the angle between the given min and max values
@@ -21,11 +132,38 @@ namespace LinearAlgebra {
         /// <param name="max">The maximum angle</param>
         /// <returns>The clamped angle</returns>
         /// Angles are normalized
-        public static float Clamp(float angle, float min, float max) {
+        public static float Clamp(Angle angle, Angle min, Angle max) {
-            float normalizedAngle = Normalize(angle);
+            return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees);
-            return Float.Clamp(normalizedAngle, min, max);
         }
 
+        /// <summary>
+        /// Rotate from one angle to the other with a maximum degrees
+        /// </summary>
+        /// <param name="fromAngle">Starting angle</param>
+        /// <param name="toAngle">Target angle</param>
+        /// <param name="maxAngle">Maximum angle to rotate</param>
+        /// <returns>The resulting angle</returns>
+        /// This function is compatible with radian and degrees angles
+        public static Angle MoveTowards(Angle fromAngle, Angle toAngle, float maxDegrees) {
+            maxDegrees = Math.Max(0, maxDegrees);  // filter out negative distances
+            Angle d = toAngle - fromAngle;
+            float dDegrees = Abs(d).inDegrees;
+            d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees));
+            if (Sign(d) < 0)
+                d = -d;
+            return fromAngle + d;
+        }
+    }
+
+    /// <summary>
+    /// %Angle utilities
+    /// </summary>
+    public static class Angles {
+        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;
+
+
         /// <summary>
         /// Determine the angle difference, result is a normalized angle
         /// </summary>
@@ -53,21 +191,6 @@ namespace LinearAlgebra {
             return angle;
         }
 
-        /// <summary>
-        /// Rotate from one angle to the other with a maximum degrees
-        /// </summary>
-        /// <param name="fromAngle">Starting angle</param>
-        /// <param name="toAngle">Target angle</param>
-        /// <param name="maxAngle">Maximum angle to rotate</param>
-        /// <returns>The resulting angle</returns>
-        /// This function is compatible with radian and degrees angles
-        public static float MoveTowards(float fromAngle, float toAngle, float maxAngle) {
-            float d = toAngle - fromAngle;
-            d = Normalize(d);
-            d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle);
-            return fromAngle + d;
-        }
-
         /// <summary>
         /// Map interval of angles between vectors [0..Pi] to interval [0..1]
         /// </summary>

LinearAlgebra/src/Direction.cs

@@ -25,6 +25,10 @@ namespace LinearAlgebra
             horizontal = 0;
             vertical = 0;
         }
+        public Direction(Angle horizontal, Angle vertical) {
+            this.horizontal = horizontal.inDegrees;
+
+        }
         // public Direction(float horizontal, float vertical) {
         //     this.horizontal = horizontal;
         //     this.vertical = vertical;
@@ -64,7 +68,7 @@ namespace LinearAlgebra
 
                 public Vector3Float ToVector3()
         {
-            float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad;
+            float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad;
             float horizontalRad = this.horizontal * Angle.Deg2Rad;
             float cosVertical = (float)Math.Cos(verticalRad);
             float sinVertical = (float)Math.Sin(verticalRad);

LinearAlgebra/src/Spherical.cs

@@ -83,7 +83,7 @@ namespace LinearAlgebra {
             if (distance == 0.0f)
                 return Spherical.zero;
             else {
-                float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
+                float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg;
                 float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
                 return Spherical.Degrees(distance, horizontalAngle, verticalAngle);
             }
@@ -106,7 +106,7 @@ namespace LinearAlgebra {
         // }
 
         public Vector3 ToVector3() {
-            float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
+            float verticalRad = (float)(Math.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);

LinearAlgebra/src/SwingTwist.cs

@@ -29,6 +29,13 @@ namespace LinearAlgebra {
         }
 
 #if UNITY_5_3_OR_NEWER
+        public static SwingTwist FromQuaternion(Quaternion q) {
+            // q.ToAngles(out float right, out float up, out float forward);
+            UnityEngine.Vector3 angles = q.eulerAngles;
+            SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z);
+            return r;
+        }
+
         public Quaternion ToQuaternion() {
             Quaternion q = Quaternion.Euler(-this.swing.vertical,
                                    this.swing.horizontal,

LinearAlgebra/test/AngleTest.cs

@@ -1,169 +1,207 @@
 #if !UNITY_5_6_OR_NEWER
+using System;
+using System.Formats.Asn1;
 using NUnit.Framework;
 
-namespace LinearAlgebra.Test
+namespace LinearAlgebra.Test {
-{
+    public class AngleTests {
-    public class Tests
-    {
         [SetUp]
-        public void Setup()
+        public void Setup() {
-        {
         }
 
         [Test]
-        public void Normalize()
+        public void Construct() {
-        {
+            // Degrees
-            float r = 0;
+            float angle = 0.0f;
+            Angle a = Angle.Degrees(angle);
+            Assert.AreEqual(angle, a.inDegrees);
 
-            r = Angle.Normalize(90);
+            angle = -180.0f;
-            Assert.AreEqual(r, 90, "Normalize 90");
+            a = Angle.Degrees(angle);
+            Assert.AreEqual(angle, a.inDegrees);
 
-            r = Angle.Normalize(-90);
+            angle = 270.0f;
-            Assert.AreEqual(r, -90, "Normalize -90");
+            a = Angle.Degrees(angle);
+            Assert.AreEqual(-90, a.inDegrees);
 
-            r = Angle.Normalize(270);
+            // Radians
-            Assert.AreEqual(r, -90, "Normalize 270");
+            angle = 0.0f;
+            a = Angle.Radians(angle);
+            Assert.AreEqual(angle, a.inRadians);
 
-            r = Angle.Normalize(270 + 360);
+            angle = (float)-Math.PI;
-            Assert.AreEqual(r, -90, "Normalize 270+360");
+            a = Angle.Radians(angle);
+            Assert.AreEqual(angle, a.inRadians);
 
-            r = Angle.Normalize(-270);
+            angle = (float)Math.PI * 1.5f;
-            Assert.AreEqual(r, 90, "Normalize -270");
+            a = Angle.Radians(angle);
+            Assert.AreEqual(-Math.PI * 0.5f, a.inRadians);
 
-            r = Angle.Normalize(-270 - 360);
+            // Revolutions
-            Assert.AreEqual(r, 90, "Normalize -270-360");
+            angle = 0.0f;
+            a = Angle.Revolutions(angle);
+            Assert.AreEqual(angle, a.inRevolutions);
 
-            r = Angle.Normalize(0);
+            angle = -0.5f;
-            Assert.AreEqual(r, 0, "Normalize 0");
+            a = Angle.Revolutions(angle);
+            Assert.AreEqual(angle, a.inRevolutions);
 
-            r = Angle.Normalize(float.PositiveInfinity);
+            angle = 0.75f;
-            Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY");
+            a = Angle.Revolutions(angle);
+            Assert.AreEqual(-0.25f, a.inRevolutions);
 
-            r = Angle.Normalize(float.NegativeInfinity);
-            Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY");
         }
 
+        // [Test]
+        // public void Normalize() {
+        //     float r = 0;
+
+        //     r = Angle.Normalize(90);
+        //     Assert.AreEqual(r, 90, "Normalize 90");
+
+        //     r = Angle.Normalize(-90);
+        //     Assert.AreEqual(r, -90, "Normalize -90");
+
+        //     r = Angle.Normalize(270);
+        //     Assert.AreEqual(r, -90, "Normalize 270");
+
+        //     r = Angle.Normalize(270 + 360);
+        //     Assert.AreEqual(r, -90, "Normalize 270+360");
+
+        //     r = Angle.Normalize(-270);
+        //     Assert.AreEqual(r, 90, "Normalize -270");
+
+        //     r = Angle.Normalize(-270 - 360);
+        //     Assert.AreEqual(r, 90, "Normalize -270-360");
+
+        //     r = Angle.Normalize(0);
+        //     Assert.AreEqual(r, 0, "Normalize 0");
+
+        //     r = Angle.Normalize(float.PositiveInfinity);
+        //     Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY");
+
+        //     r = Angle.Normalize(float.NegativeInfinity);
+        //     Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY");
+        // }
+
         [Test]
-        public void Clamp()
+        public void Clamp() {
-        {
             float r = 0;
 
-            r = Angle.Clamp(1, 0, 2);
+            r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2));
             Assert.AreEqual(r, 1, "Clamp 1 0 2");
 
-            r = Angle.Clamp(-1, 0, 2);
+            r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2));
             Assert.AreEqual(r, 0, "Clamp -1 0 2");
 
-            r = Angle.Clamp(3, 0, 2);
+            r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2));
             Assert.AreEqual(r, 2, "Clamp 3 0 2");
 
-            r = Angle.Clamp(1, 0, 0);
+            r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0));
             Assert.AreEqual(r, 0, "Clamp 1 0 0");
 
-            r = Angle.Clamp(0, 0, 0);
+            r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0));
             Assert.AreEqual(r, 0, "Clamp 0 0 0");
 
-            r = Angle.Clamp(0, 1, -1);
+            r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1));
             Assert.AreEqual(r, 1, "Clamp 0 1 -1");
 
-            r = Angle.Clamp(1, 0, float.PositiveInfinity);
+            r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity));
             Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY");
 
-            r = Angle.Clamp(1, float.NegativeInfinity, 1);
+            r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(float.NegativeInfinity), Angle.Degrees(1));
             Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1");
         }
 
-        [Test]
+        // [Test]
-        public void Difference()
+        // public void Difference() {
-        {
+        //     float r = 0;
-            float r = 0;
 
-            r = Angle.Difference(0, 90);
+        //     r = Angle.Difference(0, 90);
-            Assert.AreEqual(r, 90, "Difference 0 90");
+        //     Assert.AreEqual(r, 90, "Difference 0 90");
 
-            r = Angle.Difference(0, -90);
+        //     r = Angle.Difference(0, -90);
-            Assert.AreEqual(r, -90, "Difference 0 -90");
+        //     Assert.AreEqual(r, -90, "Difference 0 -90");
 
-            r = Angle.Difference(0, 270);
+        //     r = Angle.Difference(0, 270);
-            Assert.AreEqual(r, -90, "Difference 0 270");
+        //     Assert.AreEqual(r, -90, "Difference 0 270");
 
-            r = Angle.Difference(0, -270);
+        //     r = Angle.Difference(0, -270);
-            Assert.AreEqual(r, 90, "Difference 0 -270");
+        //     Assert.AreEqual(r, 90, "Difference 0 -270");
 
-            r = Angle.Difference(90, 0);
+        //     r = Angle.Difference(90, 0);
-            Assert.AreEqual(r, -90, "Difference 90 0");
+        //     Assert.AreEqual(r, -90, "Difference 90 0");
 
-            r = Angle.Difference(-90, 0);
+        //     r = Angle.Difference(-90, 0);
-            Assert.AreEqual(r, 90, "Difference -90 0");
+        //     Assert.AreEqual(r, 90, "Difference -90 0");
 
-            r = Angle.Difference(0, 0);
+        //     r = Angle.Difference(0, 0);
-            Assert.AreEqual(r, 0, "Difference 0 0");
+        //     Assert.AreEqual(r, 0, "Difference 0 0");
 
-            r = Angle.Difference(90, 90);
+        //     r = Angle.Difference(90, 90);
-            Assert.AreEqual(r, 0, "Difference 90 90");
+        //     Assert.AreEqual(r, 0, "Difference 90 90");
 
-            r = Angle.Difference(0, float.PositiveInfinity);
+        //     r = Angle.Difference(0, float.PositiveInfinity);
-            Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY");
+        //     Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY");
 
-            r = Angle.Difference(0, float.NegativeInfinity);
+        //     r = Angle.Difference(0, float.NegativeInfinity);
-            Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY");
+        //     Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY");
 
-            r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity);
+        //     r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity);
-            Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY");
+        //     Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY");
-        }
+        // }
 
         [Test]
-        public void MoveTowards()
+        public void MoveTowards() {
-        {
+            Angle r = Angle.zero;
-            float r = 0;
 
-            r = Angle.MoveTowards(0, 90, 30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30);
-            Assert.AreEqual(r, 30, "MoveTowards 0 90 30");
+            Assert.AreEqual(r.inDegrees, 30, "MoveTowards 0 90 30");
 
-            r = Angle.MoveTowards(0, 90, 90);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90);
-            Assert.AreEqual(r, 90, "MoveTowards 0 90 90");
+            Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 90");
 
-            r = Angle.MoveTowards(0, 90, 180);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180);
-            Assert.AreEqual(r, 90, "MoveTowards 0 90 180");
+            Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 180");
 
-            r = Angle.MoveTowards(0, 90, 270);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270);
-            Assert.AreEqual(r, 90, "MoveTowrads 0 90 270");
+            Assert.AreEqual(r.inDegrees, 90, "MoveTowrads 0 90 270");
 
-            r = Angle.MoveTowards(0, 90, -30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30);
-            Assert.AreEqual(r, -30, "MoveTowards 0 90 -30");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 -30");
 
-            r = Angle.MoveTowards(0, -90, -30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30);
-            Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -30");
 
-            r = Angle.MoveTowards(0, -90, -90);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90);
-            Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -90");
 
-            r = Angle.MoveTowards(0, -90, -180);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180);
-            Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -180");
 
-            r = Angle.MoveTowards(0, -90, -270);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270);
-            Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -90 -270");
 
-            r = Angle.MoveTowards(0, 90, 0);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0);
-            Assert.AreEqual(r, 0, "MoveTowards 0 90 0");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 0");
 
-            r = Angle.MoveTowards(0, 0, 0);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0);
-            Assert.AreEqual(r, 0, "MoveTowards 0 0 0");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 0 0");
 
-            r = Angle.MoveTowards(0, 0, 30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30);
-            Assert.AreEqual(r, 0, "MoveTowrads 0 0 30");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 0 30");
 
-            r = Angle.MoveTowards(0, 90, float.PositiveInfinity);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), float.PositiveInfinity);
-            Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY");
+            Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 INFINITY");
 
-            r = Angle.MoveTowards(0, float.PositiveInfinity, 30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity), 30);
-            Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30");
+            Assert.AreEqual(r.inDegrees, 30, "MoveTowrads 0 INFINITY 30");
 
-            r = Angle.MoveTowards(0, -90, float.NegativeInfinity);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), float.NegativeInfinity);
-            Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -INFINITY");
 
-            r = Angle.MoveTowards(0, float.NegativeInfinity, -30);
+            r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.NegativeInfinity), -30);
-            Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30");
+            Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -INFINITY -30");
 
         }
     }

LinearAlgebra/test/DirectionTest.cs

@@ -0,0 +1,19 @@
+#if !UNITY_5_6_OR_NEWER
+using NUnit.Framework;
+
+namespace LinearAlgebra.Test {
+    public class DirectionTest {
+        [SetUp]
+        public void Setup() {
+        }
+
+        [Test]
+        public void Compare() {
+            Direction d = Direction.Degrees(45, 135);
+            bool r;
+            r = d == new Direction(Angle.Degrees(45), Angle.Degrees(135));
+            Assert.True(r);
+        }
+    };
+}
+#endif

LinearAlgebra/test/SphericalTest.cs

@@ -1,4 +1,5 @@
 #if !UNITY_5_6_OR_NEWER
+using System;
 using NUnit.Framework;
 
 namespace LinearAlgebra.Test {
@@ -13,7 +14,19 @@ namespace LinearAlgebra.Test {
             Spherical s = Spherical.FromVector3(v);
             Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1");
             Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1");
-            Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1");
+            Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 0 0 1");
+
+            v = new(0, 1, 0);
+            s = Spherical.FromVector3(v);
+            Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0");
+            Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 1 0");
+            Assert.AreEqual(90.0f, s.direction.vertical, "s.vert 0 1 0");
+
+            v = new(1, 0, 0);
+            s = Spherical.FromVector3(v);
+            Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0");
+            Assert.AreEqual(90.0f, s.direction.horizontal, "s.hor 1 0 0");
+            Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 1 0 0");
         }
 
         [Test]
@@ -23,7 +36,17 @@ namespace LinearAlgebra.Test {
             Spherical r = Spherical.zero;
 
             r = v1 + v2;
-            Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)");
+            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");
+
+            r = v1;
+            r += v2;
+            Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)");
+
+            v2 = Spherical.Degrees(1, 0, 90);
+            r = v1 + v2;
+            Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)");
+            Assert.AreEqual(45.0f, r.direction.horizontal, "Addition(1 0 90)");
+            Assert.AreEqual(45.0f, r.direction.vertical, "Addition(1 0 90)");
         }
     }
 }

README.md

@@ -11,3 +11,21 @@ The documentation for Roboid Control for C# is found at https://docs.roboidcontr
 
 - RoboidControl::Thing
 - RoboidControl::Participant
+
+# Get Started
+
+## Unity
+
+The Unity environment can use the same RoboidControl code as every other C# code, but needs a *starter* wrapper around it to make the things visibile. For example, to start the BB2B example in Unity one needs to write a BB2B_Starter.cs component as follows:
+```
+using RoboidControl.Unity;
+
+public class BB2B_Starter : SiteServer {
+    void Start() {
+        new RoboidControl.BB2B();    
+    }
+}
+```
+This component then should be attached to a GameObject in the scene.
+
+It is possible to create a Site Server in Unity by just adding the `SiteServer` Component to a GameObject in the scene. When this is run, other roboids will be able to connect to this site then.

Unity/DifferentialDrive.cs

@@ -42,8 +42,6 @@ namespace RoboidControl.Unity {
             return differentialDrive;
         }
 
-        public Motor leftMotor;
-        public Motor rightMotor;
         /// <summary>
         /// The left wheel of the differential drive
         /// </summary>
@@ -73,8 +71,41 @@ namespace RoboidControl.Unity {
         /// Handle the binary event indicating a configuration change
         /// </summary>
         protected override void HandleBinary() {
-            HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
+            // Ignore it when the wheel radius is not set
-            HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);
+            if (coreDrive.wheelRadius <= 0)
+                return;
+
+            // Destroy any (generic) thing with the same id
+            if (leftWheel == null) {
+                Thing[] things = FindObjectsOfType<Thing>();
+                foreach (Thing thing in things) {
+                    if (thing.core.id == coreDrive.leftWheel.id)
+                        Destroy(thing.gameObject);
+                }
+            }
+            if (leftWheel == null && coreDrive.leftWheel != null)
+                leftWheel = Wheel.Create(this.transform, coreDrive.leftWheel.id);
+            if (leftWheel != null) {
+                leftWheel.core ??= coreDrive.leftWheel;
+                SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
+                leftWheelCollider.radius = coreDrive.wheelRadius;
+            }
+
+            // Destroy any (generic) thing with the same id
+            if (rightWheel == null) {
+                Thing[] things = FindObjectsOfType<Thing>();
+                foreach (Thing thing in things) {
+                    if (thing.core.id == coreDrive.rightWheel.id)
+                        Destroy(thing.gameObject);
+                }
+            }
+            if (rightWheel == null && coreDrive.rightWheel != null)
+                rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
+            if (rightWheel != null) {
+                rightWheel.core ??= coreDrive.rightWheel;
+                SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
+                rightWheelCollider.radius = coreDrive.wheelRadius;
+            }
 
             if (casterWheel == null) {
                 GameObject gameObj = new("Caster wheel");
@@ -82,31 +113,12 @@ namespace RoboidControl.Unity {
                 casterWheel = gameObj.AddComponent<SphereCollider>();
                 casterWheel.material = Wheel.slidingWheel;
             }
-            if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
             casterWheel.radius = coreDrive.wheelRadius;
             // Put it in the middle of the back
             // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
             float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
             casterWheel.center = new Vector3(0, 0, -wheelSeparation);
         }
-        }
-
-        private void HandleWheelBinary(RoboidControl.Motor coreMotor, ref Motor motor, ref Wheel wheel) {
-            if (coreMotor == null)
-                return;
-
-            if (motor == null) {
-                motor = coreMotor.component as Motor;
-                if (motor == null)
-                    motor = Motor.Create(coreMotor);
-                wheel.transform.SetParent(motor.transform);
-            }
-                else if (motor.core.id != coreMotor.id) {
-                    motor = coreMotor.component as Motor;
-                    if (motor != null)
-                        wheel.transform.SetParent(motor.transform);
-                }
-        }
 
         /// <summary>
         /// Update the Unity representation state
@@ -114,12 +126,12 @@ namespace RoboidControl.Unity {
         protected override void FixedUpdate() {
             base.FixedUpdate();
 
-            if (rb != null && leftMotor != null && rightMotor != null) {
+            if (rb != null && leftWheel != null && rightWheel != null) {
-                float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.wheelRadius;
+                float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
-                float rightWheelVelocity = rightMotor.rotationSpeed * (2 * Mathf.PI) * rightWheel.wheelRadius;
+                float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
 
-                if (leftWheel != null && rightWheel != null) {
+                // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
-                    float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
+                float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
                 float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
                 float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
 
@@ -129,7 +141,6 @@ namespace RoboidControl.Unity {
             }
         }
     }
-    }
 
 }
 #endif

Unity/Editor/Configurator.cs

@@ -0,0 +1,88 @@
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEditor;
+using UnityEditor.Callbacks;
+using UnityEditor.PackageManager.Requests;
+using UnityEditor.PackageManager;
+
+namespace RoboidControl.Unity
+{
+    [InitializeOnLoad]
+    public class ConfigurationCheck
+    {
+        static ConfigurationCheck()
+        {
+            RetrievePackageList();
+        }
+
+        protected static ListRequest request;
+        public static List<string> packageNameList;
+
+        public static void RetrievePackageList()
+        {
+            request = Client.List();    // List packages installed for the Project
+            EditorApplication.update += Progress;
+        }
+
+        public static void Progress()
+        {
+            if (request.IsCompleted)
+            {
+                if (request.Status == StatusCode.Success)
+                {
+                    packageNameList = new List<string>();
+                    foreach (UnityEditor.PackageManager.PackageInfo package in request.Result)
+                        packageNameList.Add(package.name);
+                        
+                    DidReloadScripts();                    
+                }
+                else if (request.Status >= StatusCode.Failure)
+                    Debug.Log(request.Error.message);
+
+                EditorApplication.update -= Progress;
+            }
+        }
+
+        //[DidReloadScripts]
+        protected static void DidReloadScripts()
+        {
+            if (packageNameList == null)
+                return;
+            CheckExtension(
+                packageNameList.Contains("com.unity.cloud.gltfast"), "GLTF");
+        }
+
+        protected static void CheckExtension(bool enabled, string define)
+        {
+            if (enabled)
+                GlobalDefine(define);
+            else
+                GlobalUndefine(define);
+        }
+
+        public static void GlobalDefine(string name)
+        {
+            //Debug.Log("Define " + name);
+            string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup);
+            if (!scriptDefines.Contains(name))
+            {
+                string newScriptDefines = scriptDefines + " " + name;
+                if (EditorUserBuildSettings.selectedBuildTargetGroup != 0)
+                    PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines);
+            }
+        }
+
+        public static void GlobalUndefine(string name)
+        {
+            //Debug.Log("Undefine " + name);
+            string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup);
+            if (scriptDefines.Contains(name))
+            {
+                int playMakerIndex = scriptDefines.IndexOf(name);
+                string newScriptDefines = scriptDefines.Remove(playMakerIndex, name.Length);
+                PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines);
+            }
+
+        }
+    }
+}

Unity/Motor.cs

@@ -34,8 +34,8 @@ namespace RoboidControl.Unity {
                 motor = gameObj.AddComponent<Motor>();
                 motor.Init(coreMotor);
 
-                // Rigidbody rb = gameObj.AddComponent<Rigidbody>();
+                Rigidbody rb = gameObj.AddComponent<Rigidbody>();
-                // rb.isKinematic = true;
+                rb.isKinematic = true;
             }
             return motor;
         }
@@ -43,7 +43,7 @@ namespace RoboidControl.Unity {
         /// <summary>
         /// The maximum rotation speed of the motor in rotations per second
         /// </summary>
-        public float maxSpeed = 2;
+        public float maxSpeed = 5;
 
         /// <summary>
         /// The actual rotation speed in rotations per second

Unity/Participant.cs

@@ -8,16 +8,6 @@ namespace RoboidControl.Unity {
 
         public RoboidControl.Participant coreParticipant;
 
-        // public Thing GetThing(RoboidControl.Thing coreThing) {
-        //     // Not efficient or full correct but will do for now
-        //     Thing[] things = FindObjectsOfType<Thing>();
-        //     foreach (Thing thing in things) {
-        //         if (thing.core.id == coreThing.id)
-        //             return thing;
-        //     }
-        //     return null;
-        // }
-
         protected virtual void Update() {
             if (coreParticipant == null)
                 return;
@@ -46,12 +36,12 @@ namespace RoboidControl.Unity {
                     DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
                     coreDrive.component = differentialDrive;
                     break;
-                case RoboidControl.Motor coreMotor:
+                // case RoboidControl.Motor coreMotor:
-                    Motor wheel = Motor.Create(coreMotor);
+                //     //Wheel wheel = Wheel.Create(coreMotor);
-                    coreMotor.component = wheel;
+                //     //coreMotor.component = wheel;
-                    // We need to know the details (though a binary msg)
+                //     // We need to know the details (though a binary msg)
-                    // before we can create the wheel reliably
+                //     // before we can create the wheel reliably
-                    break;
+                //     break;
                 case RoboidControl.Thing coreThing:
                     Debug.Log("Handle Thing");
                     if (coreThing.component == null) {

Unity/RelativeEncoder.cs

@@ -56,7 +56,7 @@ namespace RoboidControl.Unity {
 
             // Normalize angles to range [-180..180)
             // Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions
-            float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x);
+            float deltaAngle = Angle.Degrees(deltaRotation.eulerAngles.x).inDegrees;
             this.rotationSpeed = deltaAngle / Time.deltaTime;
         }
 

Unity/Resources/DifferentialDrive_disabled.prefab

@@ -0,0 +1,546 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!1 &135949056663158942
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 693610334404048217}
+  - component: {fileID: 874278397287993211}
+  - component: {fileID: 8149674613691108646}
+  - component: {fileID: 7453252590388621785}
+  m_Layer: 0
+  m_Name: Body
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &693610334404048217
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 135949056663158942}
+  serializedVersion: 2
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0.01, z: -0.01}
+  m_LocalScale: {x: 0.12, y: 0.05, z: 0.2}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 6798369561388671537}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!33 &874278397287993211
+MeshFilter:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 135949056663158942}
+  m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!23 &8149674613691108646
+MeshRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 135949056663158942}
+  m_Enabled: 1
+  m_CastShadows: 1
+  m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
+  m_StaticShadowCaster: 0
+  m_MotionVectors: 1
+  m_LightProbeUsage: 1
+  m_ReflectionProbeUsage: 1
+  m_RayTracingMode: 2
+  m_RayTraceProcedural: 0
+  m_RenderingLayerMask: 1
+  m_RendererPriority: 0
+  m_Materials:
+  - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
+  m_StaticBatchInfo:
+    firstSubMesh: 0
+    subMeshCount: 0
+  m_StaticBatchRoot: {fileID: 0}
+  m_ProbeAnchor: {fileID: 0}
+  m_LightProbeVolumeOverride: {fileID: 0}
+  m_ScaleInLightmap: 1
+  m_ReceiveGI: 1
+  m_PreserveUVs: 0
+  m_IgnoreNormalsForChartDetection: 0
+  m_ImportantGI: 0
+  m_StitchLightmapSeams: 1
+  m_SelectedEditorRenderState: 3
+  m_MinimumChartSize: 4
+  m_AutoUVMaxDistance: 0.5
+  m_AutoUVMaxAngle: 89
+  m_LightmapParameters: {fileID: 0}
+  m_SortingLayerID: 0
+  m_SortingLayer: 0
+  m_SortingOrder: 0
+  m_AdditionalVertexStreams: {fileID: 0}
+--- !u!65 &7453252590388621785
+BoxCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 135949056663158942}
+  m_Material: {fileID: 0}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_LayerOverridePriority: 0
+  m_IsTrigger: 0
+  m_ProvidesContacts: 0
+  m_Enabled: 0
+  serializedVersion: 3
+  m_Size: {x: 1, y: 1, z: 1}
+  m_Center: {x: -3.4930155e-36, y: 0, z: 0}
+--- !u!1 &3154420579555598162
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 213193100111257669}
+  - component: {fileID: 894333789048616995}
+  m_Layer: 0
+  m_Name: Caster wheel
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &213193100111257669
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3154420579555598162}
+  serializedVersion: 2
+  m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: -0.1}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 6798369561388671537}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!135 &894333789048616995
+SphereCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3154420579555598162}
+  m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_LayerOverridePriority: 0
+  m_IsTrigger: 0
+  m_ProvidesContacts: 0
+  m_Enabled: 1
+  serializedVersion: 3
+  m_Radius: 0.00001
+  m_Center: {x: 0, y: 0, z: 0}
+--- !u!1 &3377575892836316963
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 7997617267105515189}
+  - component: {fileID: 6773972788910618332}
+  - component: {fileID: 3720747953092717687}
+  m_Layer: 0
+  m_Name: Left wheel
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &7997617267105515189
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3377575892836316963}
+  serializedVersion: 2
+  m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
+  m_LocalPosition: {x: -0.08, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children:
+  - {fileID: 9144497505704104684}
+  m_Father: {fileID: 6798369561388671537}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &6773972788910618332
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3377575892836316963}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  participant: {fileID: 0}
+  maxSpeed: 5
+  rotationSpeed: 0
+--- !u!135 &3720747953092717687
+SphereCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3377575892836316963}
+  m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_LayerOverridePriority: 0
+  m_IsTrigger: 0
+  m_ProvidesContacts: 0
+  m_Enabled: 1
+  serializedVersion: 3
+  m_Radius: 0.00001
+  m_Center: {x: 0, y: 0, z: 0}
+--- !u!1 &4326386140118987503
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 6798369561388671537}
+  - component: {fileID: 2975045340952151157}
+  - component: {fileID: 4118868690347991998}
+  m_Layer: 0
+  m_Name: DifferentialDrive_disabled
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &6798369561388671537
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4326386140118987503}
+  serializedVersion: 2
+  m_LocalRotation: {x: 1.276433e-21, y: -1.082074e-19, z: -2.3980766e-36, w: 1}
+  m_LocalPosition: {x: 2.1932227e-37, y: -0.06941543, z: -1.3084181e-19}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children:
+  - {fileID: 7997617267105515189}
+  - {fileID: 2660791421672615984}
+  - {fileID: 693610334404048217}
+  - {fileID: 213193100111257669}
+  m_Father: {fileID: 0}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &2975045340952151157
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4326386140118987503}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  participant: {fileID: 0}
+  leftWheel: {fileID: 6773972788910618332}
+  rightWheel: {fileID: 7425139233737877139}
+  casterWheel: {fileID: 894333789048616995}
+--- !u!54 &4118868690347991998
+Rigidbody:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4326386140118987503}
+  serializedVersion: 4
+  m_Mass: 0.1
+  m_Drag: 0
+  m_AngularDrag: 0.05
+  m_CenterOfMass: {x: 0, y: 0, z: 0}
+  m_InertiaTensor: {x: 1, y: 1, z: 1}
+  m_InertiaRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ImplicitCom: 1
+  m_ImplicitTensor: 1
+  m_UseGravity: 1
+  m_IsKinematic: 0
+  m_Interpolate: 0
+  m_Constraints: 0
+  m_CollisionDetection: 0
+--- !u!1 &4927629757198460283
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 9144497505704104684}
+  - component: {fileID: 8353236605004633556}
+  - component: {fileID: 1652944855748388758}
+  m_Layer: 0
+  m_Name: Cube
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &9144497505704104684
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4927629757198460283}
+  serializedVersion: 2
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 0.01, y: 0.05, z: 0.05}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 7997617267105515189}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!33 &8353236605004633556
+MeshFilter:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4927629757198460283}
+  m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!23 &1652944855748388758
+MeshRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 4927629757198460283}
+  m_Enabled: 1
+  m_CastShadows: 1
+  m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
+  m_StaticShadowCaster: 0
+  m_MotionVectors: 1
+  m_LightProbeUsage: 1
+  m_ReflectionProbeUsage: 1
+  m_RayTracingMode: 2
+  m_RayTraceProcedural: 0
+  m_RenderingLayerMask: 1
+  m_RendererPriority: 0
+  m_Materials:
+  - {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2}
+  m_StaticBatchInfo:
+    firstSubMesh: 0
+    subMeshCount: 0
+  m_StaticBatchRoot: {fileID: 0}
+  m_ProbeAnchor: {fileID: 0}
+  m_LightProbeVolumeOverride: {fileID: 0}
+  m_ScaleInLightmap: 1
+  m_ReceiveGI: 1
+  m_PreserveUVs: 0
+  m_IgnoreNormalsForChartDetection: 0
+  m_ImportantGI: 0
+  m_StitchLightmapSeams: 1
+  m_SelectedEditorRenderState: 3
+  m_MinimumChartSize: 4
+  m_AutoUVMaxDistance: 0.5
+  m_AutoUVMaxAngle: 89
+  m_LightmapParameters: {fileID: 0}
+  m_SortingLayerID: 0
+  m_SortingLayer: 0
+  m_SortingOrder: 0
+  m_AdditionalVertexStreams: {fileID: 0}
+--- !u!1 &5122915782100933114
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 2660791421672615984}
+  - component: {fileID: 7425139233737877139}
+  - component: {fileID: 8150853676422519226}
+  m_Layer: 0
+  m_Name: Right wheel
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &2660791421672615984
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5122915782100933114}
+  serializedVersion: 2
+  m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
+  m_LocalPosition: {x: 0.08, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children:
+  - {fileID: 2324127387642107303}
+  m_Father: {fileID: 6798369561388671537}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &7425139233737877139
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5122915782100933114}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  participant: {fileID: 0}
+  maxSpeed: 5
+  rotationSpeed: 0
+--- !u!135 &8150853676422519226
+SphereCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5122915782100933114}
+  m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_LayerOverridePriority: 0
+  m_IsTrigger: 0
+  m_ProvidesContacts: 0
+  m_Enabled: 1
+  serializedVersion: 3
+  m_Radius: 0.00001
+  m_Center: {x: 0, y: 0, z: 0}
+--- !u!1 &5663058822615269368
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 2324127387642107303}
+  - component: {fileID: 7927909525546489266}
+  - component: {fileID: 1038362976985791207}
+  m_Layer: 0
+  m_Name: Cube
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &2324127387642107303
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5663058822615269368}
+  serializedVersion: 2
+  m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 0.01, y: 0.049999997, z: 0.049999997}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 2660791421672615984}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!33 &7927909525546489266
+MeshFilter:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5663058822615269368}
+  m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!23 &1038362976985791207
+MeshRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 5663058822615269368}
+  m_Enabled: 1
+  m_CastShadows: 1
+  m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
+  m_StaticShadowCaster: 0
+  m_MotionVectors: 1
+  m_LightProbeUsage: 1
+  m_ReflectionProbeUsage: 1
+  m_RayTracingMode: 2
+  m_RayTraceProcedural: 0
+  m_RenderingLayerMask: 1
+  m_RendererPriority: 0
+  m_Materials:
+  - {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2}
+  m_StaticBatchInfo:
+    firstSubMesh: 0
+    subMeshCount: 0
+  m_StaticBatchRoot: {fileID: 0}
+  m_ProbeAnchor: {fileID: 0}
+  m_LightProbeVolumeOverride: {fileID: 0}
+  m_ScaleInLightmap: 1
+  m_ReceiveGI: 1
+  m_PreserveUVs: 0
+  m_IgnoreNormalsForChartDetection: 0
+  m_ImportantGI: 0
+  m_StitchLightmapSeams: 1
+  m_SelectedEditorRenderState: 3
+  m_MinimumChartSize: 4
+  m_AutoUVMaxDistance: 0.5
+  m_AutoUVMaxAngle: 89
+  m_LightmapParameters: {fileID: 0}
+  m_SortingLayerID: 0
+  m_SortingLayer: 0
+  m_SortingOrder: 0
+  m_AdditionalVertexStreams: {fileID: 0}

Unity/SiteServer.cs

@@ -41,6 +41,9 @@ namespace RoboidControl.Unity {
                     GameObject remoteParticipant = new GameObject("RemoteParticipant");
                     Participant participant = remoteParticipant.AddComponent<Participant>();
                     participant.coreParticipant = e.participant;
+                    participant.coreParticipant.component = participant;
+                    participant.ipAddress = e.participant.ipAddress;
+                    participant.port = e.participant.port;
                     break;
                 case ThingMsg.id:
                     HandleThingEvent(e);

Unity/Thing.cs

@@ -46,18 +46,18 @@ namespace RoboidControl.Unity {
         protected void Init(RoboidControl.Thing core) {
             this.core = core;
             this.core.component = this;
-            Participant[] participants = FindObjectsByType<Participant>(FindObjectsSortMode.None);
+            // This is wrong, it should get the owner, which is not the siteserver
-            foreach (Participant participant in participants) {
-                if (participant.ipAddress == core.owner.ipAddress && participant.port == core.owner.port)
-                    this.owner = participant;
-            }
             // this.owner = FindAnyObjectByType<SiteServer>();
             // core.owner = this.owner.coreParticipant;
+            this.owner = core.owner.component;
 
             if (core.parent != null && core.parent.component != null) {
                 this.transform.SetParent(core.parent.component.transform, false);
                 this.transform.localPosition = Vector3.zero;
             }
+            else {
+                this.transform.SetParent(core.owner.component.transform, false);
+            }
 
             if (core.position != null)
                 this.transform.localPosition = core.position.ToVector3();
@@ -88,6 +88,7 @@ namespace RoboidControl.Unity {
         /// </summary>
         protected virtual void FixedUpdate() {
             UpdateThing();
+            core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
         }
 
         /// <summary>
@@ -95,8 +96,8 @@ namespace RoboidControl.Unity {
         /// </summary>
         public void UpdateThing() {
             if (core == null) {
-                //Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
+                Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
-                //Destroy(this);
+                Destroy(this);
                 return;
             }
 
@@ -113,7 +114,7 @@ namespace RoboidControl.Unity {
                 case ThingMsg.id:
                     Debug.Log($"{this.core.id} Handle Thing");
                     if (core.parent == null)
-                        this.transform.SetParent(null, true);
+                        this.transform.SetParent(core.owner.component.transform, true);
                     else if (core.parent.component != null)
                         this.transform.SetParent(core.parent.component.transform, true);
                     break;
@@ -134,7 +135,7 @@ namespace RoboidControl.Unity {
                     this.HandlePose();
                     break;
                 case BinaryMsg.Id:
-                    // Debug.Log($"{this.core.id} Handle Binary");
+                    Debug.Log($"{this.core.id} Handle Binary");
                     this.HandleBinary();
                     break;
             }
@@ -195,19 +196,19 @@ namespace RoboidControl.Unity {
                     SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren<SkinnedMeshRenderer>();
 
 #if pHUMANOID4
-                if (parentThing.objectType == 7) {
+                // if (parentThing.objectType == 7) {
-                    HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
+                //     HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
-                    if (hc == null)
+                //     if (hc == null)
-                        hc = parentThing.gameObject.AddComponent<HumanoidControl>();
+                //         hc = parentThing.gameObject.AddComponent<HumanoidControl>();
 
-                    foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
+                //     foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
-                        if (meshRenderer.rootBone != null) {
+                //         if (meshRenderer.rootBone != null) {
-                            Debug.Log("Found a skinned mesh with bones");
+                //             Debug.Log("Found a skinned mesh with bones");
-                            hc.RetrieveBonesFrom(meshRenderer.rootBone);
+                //             hc.RetrieveBonesFrom(meshRenderer.rootBone);
-                            break;
+                //             break;
-                        }
+                //         }
-                    }
+                //     }
-                }
+                // }
 #endif
                     parentTransform.localScale = Vector3.one;
                     if (meshRenderers.Length > 0) {
@@ -259,6 +260,7 @@ namespace RoboidControl.Unity {
         /// This can update the position and/or orientation when the velocity of the thing is zero.
         /// If a velocity is not zero, the position and/or orientation update will be ignored
         protected virtual void HandlePose() {
+            this.transform.localRotation = core.orientation.ToQuaternion();
             if (core.linearVelocity.distance == 0)
                 this.transform.localPosition = core.position.ToVector3();
             if (core.angularVelocity.distance == 0)

Unity/TouchSensor.cs

@@ -45,8 +45,6 @@ namespace RoboidControl.Unity {
 			return touchSensor;
 		}
 
-		public bool touchedSomething = false;
-
 		/// <summary>
 		/// Handle touch trigger collider enter event
 		/// </summary>
@@ -59,25 +57,9 @@ namespace RoboidControl.Unity {
 			if (this.transform.root == other.transform.root)
 				return;			
 
-			Debug.Log("TOUCH!");
-			this.touchedSomething = true;
 			this.coreSensor.touchedSomething = true;
-			//this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
+			this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
 		}
-
-        // private void OnTriggerStay(Collider other) {
-		// 	// Don't detect trigger colliders
-		// 	if (other.isTrigger)
-		// 		return;
-		// 	// Don't touch yourself
-		// 	if (this.transform.root == other.transform.root)
-		// 		return;
-
-		// 	Debug.Log("Touching...");
-		// 	this.touchedSomething = true;
-		// 	this.coreSensor.touchedSomething = true;            
-        // }
-
 		/// <summary>
 		/// Handl touch trigger collider exit event
 		/// </summary>
@@ -86,8 +68,6 @@ namespace RoboidControl.Unity {
 			if (other.isTrigger)
 				return;
 
-			Debug.Log("TOUCH END!");
-			this.touchedSomething = false;
 			this.coreSensor.touchedSomething = false;
 		}
 	}

Unity/Wheel.cs

@@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
     /// <summary>
     /// The Unity representation of a Roboid Control wheel
     /// </summary>
-    public class Wheel : Thing {
+    public class Wheel : Motor {
         /// <summary>
         /// Create the Unity representation
         /// </summary>
         /// <param name="core">The core motor</param>
         /// <returns>The Unity representation of a motorised wheel</returns>
-        public static new Wheel Create(RoboidControl.Thing core) {
+        public static Wheel Create(RoboidControl.Motor core, float wheelRadius) {
             GameObject prefab = (GameObject)Resources.Load("Wheel");
             if (prefab != null) {
                 // Use resource prefab when available
@@ -38,7 +38,7 @@ namespace RoboidControl.Unity {
                 GameObject gameObj = Instantiate(prefab);
                 Wheel component = gameObj.GetComponent<Wheel>();
                 if (component != null)
-                    component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false);
+                    component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor);
                 return component;
             }
             else {
@@ -49,7 +49,8 @@ namespace RoboidControl.Unity {
                 SiteServer participant = FindAnyObjectByType<SiteServer>();
                 RoboidControl.Thing core = participant.coreParticipant.Get(thingId);
                 if (core == null)
-                    core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
+                    //core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
+                    core = RoboidControl.Thing.CreateRemote(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId);
                 else {
                     ;
                 }
@@ -62,8 +63,6 @@ namespace RoboidControl.Unity {
             }
         }
 
-        public float wheelRadius = 0;
-
         private static PhysicMaterial _slidingWheel;
         public static PhysicMaterial slidingWheel {
             get {

src/Participant.cs

@@ -14,6 +14,13 @@ namespace RoboidControl {
     /// It also maintains the communcation information to contact the participant.
     /// It is used as a basis for the local participant, but also as a reference to remote participants.
     public class Participant {
+
+        private Participant() {
+            this.root = Thing.CreateRoot(this);
+            this.root.name = "Root";
+            this.Add(this.root);
+        }
+
         /// <summary>
         /// Create a new participant with the given communcation info
         /// </summary>
@@ -22,10 +29,20 @@ namespace RoboidControl {
         public Participant(string ipAddress, int port, Participant localParticipant = null) {
             this.ipAddress = ipAddress;
             this.port = port;
+
+            this.root = Thing.CreateRoot(this);
+            this.root.name = "Root";
+
             if (localParticipant != null)
                 this.udpClient = localParticipant.udpClient;
         }
 
+        public static Participant localParticipant = new();
+        public static void ReplaceLocalParticipant(Participant participant) {
+            Participant.localParticipant = participant;
+        }
+
+        public Thing root = null;
         /// <summary>
         /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
         /// </summary>
@@ -35,6 +52,14 @@ namespace RoboidControl {
         /// </summary>
         public int port = 0;
 
+#if UNITY_5_3_OR_NEWER
+        /// <summary>
+        /// A reference to the representation of the thing in Unity
+        /// </summary>
+        [NonSerialized]
+        public Unity.Participant component = null;
+#endif
+
         public UdpClient udpClient = null;
 
         /// <summary>

src/Participants/ParticipantUDP.cs

@@ -35,6 +35,7 @@ namespace RoboidControl {
             if (this.port == 0)
                 this.isIsolated = true;
             Participant.AddParticipant(this);
+            Participant.ReplaceLocalParticipant(this);
         }
         /// <summary>
         ///  Create a participant which will try to connect to a site.
@@ -79,6 +80,7 @@ namespace RoboidControl {
                 this.remoteSite = new Participant(ipAddress, port, this);
 
             Participant.AddParticipant(this);
+            Participant.ReplaceLocalParticipant(this);
         }
 
         private static ParticipantUDP isolatedParticipant = null;

src/Participants/SiteServer.cs

@@ -132,10 +132,9 @@ namespace RoboidControl {
 
         protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) {
             return msg.thingType switch {
-                Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
+                //Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
-                Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId),
+                //Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId),
-                Thing.Type.UncontrolledMotor => new Motor(sender, msg.thingId),
+                _ => Thing.CreateRemote(sender, msg.thingType, msg.thingId)
-                _ => new Thing(sender, msg.thingType, msg.thingId),
             };
 
         }

src/Thing.cs

@@ -26,10 +26,11 @@ namespace RoboidControl {
             public const byte ControlledMotor = 0x06;
             public const byte UncontrolledMotor = 0x07;
             public const byte Servo = 0x08;
-            public const byte IncrementalEncoder = 0x19;
+            public const byte RelativeEncoder = 0x19;
             // Other
+            public const byte Root = 0x10;
             public const byte Roboid = 0x09;
-            public const byte HUmanoid = 0x0A;
+            public const byte Humanoid = 0x0A;
             public const byte ExternalSensor = 0x0B;
             public const byte Animator = 0x0C;
             public const byte DifferentialDrive = 0x0D;
@@ -37,6 +38,29 @@ namespace RoboidControl {
 
         #region Init
 
+        private Thing(Participant owner) {
+            this.type = Type.Root;
+
+            this.positionUpdated = true;
+            this.orientationUpdated = true;
+            this.hierarchyChanged = true;
+
+            this.owner = owner;
+            this.parent = null;
+        }
+
+        public static Thing CreateRoot(Participant owner) {
+            return new Thing(owner);
+        }
+
+        static Thing localRoot {
+            get {
+                Participant participant = Participant.localParticipant;
+                return participant.root;
+            }
+        }
+
+        /*
         /// <summary>
         /// Create a new thing without communication abilities
         /// </summary>
@@ -66,6 +90,7 @@ namespace RoboidControl {
                 this.owner.updateQueue.Enqueue(e);
             }
         }
+        */
 
         /// <summary>
         /// Create a new child thing
@@ -75,26 +100,39 @@ namespace RoboidControl {
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
         /// <note>The owner will be the same as the owner of the parent thing</note>
+        /*
         public Thing(Thing parent, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) {
             this.parent = parent;
         }
+        */
+        public Thing(byte thingType = Type.Undetermined, Thing parent = default) {
+            this.type = thingType;
 
-        // /// <summary>
+            this.positionUpdated = true;
-        // /// Create a new thing for the given participant
+            this.orientationUpdated = true;
-        // /// </summary>
+            this.hierarchyChanged = true;
-        // /// <param name="owner">The participant owning the thing</param>
+
-        // /// <param name="networkId">The network ID of the thing</param>
+            if (parent == default)
-        // /// <param name="thingId">The ID of the thing</param>
+                this.parent = Participant.localParticipant.root;
-        // /// <param name="thingType">The type of thing</param>
+            else
-        // public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) {
+                this.parent = parent;
-        //     this.owner = owner;
+
-        //     this.id = thingId;
+            this.owner = this.parent.owner;
-        //     this.type = thingType;
+            this.owner.Add(this, true);
-        //     this.networkId = networkId;
+
-        //     // Console.Write($"New thing added to {owner}");
+            Participant.UpdateEvent e = new() {
-        //     this.owner.Add(this);
+                messageId = ThingMsg.id,
-        //     InvokeNewThing(this);
+                thing = this
-        // }
+            };
+            this.owner.updateQueue.Enqueue(e);
+        }
+
+        public static Thing CreateRemote(Participant owner, byte thingType, byte thingId) {
+            Thing remoteThing = new(thingType, owner.root) {
+                id = thingId
+            };
+            return remoteThing;
+        }
 
         /// <summary>
         /// Function which can be used to create components in external engines.
@@ -302,6 +340,7 @@ namespace RoboidControl {
                 if (_orientation != value) {
                     _orientation = value;
                     orientationUpdated = true;
+                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                     //OnOrientationChanged?.Invoke();
                 }
             }
@@ -383,7 +422,7 @@ namespace RoboidControl {
             this.orientationUpdated = false;
             this.linearVelocityUpdated = false;
             this.angularVelocityUpdated = false;
-            //this.hierarchyChanged = false;
+            this.hierarchyChanged = false;
 
             // should recurse over children...
             if (recurse) {

src/Things/ControlledMotor.cs

@@ -0,0 +1,84 @@
+namespace RoboidControl {
+
+    /// @brief A motor with speed control
+    /// It uses a feedback loop from an encoder to regulate the speed
+    /// The speed is measured in revolutions per second.
+    public class ControlledMotor : Motor {
+        public ControlledMotor(Thing parent, RelativeEncoder encoder) : base(parent) {
+            this.encoder = encoder;
+         }
+        // Upgrade an existing motor with an encoder
+        public ControlledMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) {
+            this.motor = motor;
+            this.encoder = encoder;
+         }
+
+        readonly RelativeEncoder encoder;
+        readonly Motor motor;
+
+        public override float targetSpeed {
+            get {
+                if (this.motor != null)
+                    return this.motor.targetSpeed;
+                else
+                    return base.targetSpeed;
+            }
+            set {
+                if (this.motor != null)
+                    this.motor.targetSpeed = value;
+                else
+                    base.targetSpeed = value;
+            }
+        }
+
+        private float _targetAngularSpeed; // In degrees per second
+        public virtual float targetAngularSpeed {
+            get => _targetAngularSpeed;
+            set {
+                if (value != this._targetAngularSpeed) {
+                    this._targetAngularSpeed = value;
+                    updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
+                    owner.Send(new BinaryMsg(this));
+                }
+            }
+        }
+        /// @brief Get the actual speed from the encoder
+        /// @return The speed in angle per second
+        public virtual float actualAngularSpee {
+            get => encoder.angularSpeed;
+        }
+
+        float pidP = 0.1F;
+        float pidD = 0.0F;
+        //float pidI = 0.0F;
+
+        public override void Update(ulong currentTimeMs, bool recurse = false) {
+            float actualSpeed = this.encoder.angularSpeed;
+            // Simplified rotation direction, shouldbe improved
+            // This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction
+            if (this.targetAngularSpeed < 0)
+                actualSpeed = -actualSpeed;
+
+            float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f;
+
+            float error = this.targetAngularSpeed - actualSpeed;
+            float errorChange = (error - lastError) * deltaTime;
+
+            float delta = error * pidP + errorChange * pidD;
+
+            // Update the motor speed
+            if (motor != null)
+                motor.targetSpeed += delta;
+            else
+                base.targetSpeed += delta;
+            this.lastUpdateTime = currentTimeMs;
+        }
+
+        ulong lastUpdateTime = 0;
+        float lastError = 0;
+
+        // enum Direction { Forward = 1, Reverse = -1 };
+        // Direction rotationDirection;
+    };
+
+} // namespace RoboidControl

src/Things/DifferentialDrive.cs

@@ -1,3 +1,4 @@
+using System;
 using LinearAlgebra;
 
 namespace RoboidControl {
@@ -6,11 +7,13 @@ namespace RoboidControl {
     ///
     /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
     public class DifferentialDrive : Thing {
+        /*
         /// <summary>
         /// Create a differential drive without communication abilities
         /// </summary
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
-        public DifferentialDrive(bool invokeEvent = true) : base(Type.DifferentialDrive, invokeEvent) { }
+        public DifferentialDrive() : base(Type.DifferentialDrive) { }
+        
         /// <summary>
         /// Create a differential drive for a participant
         /// </summary>
@@ -28,15 +31,16 @@ namespace RoboidControl {
             // sendBinary = true;
             // owner.Send(new BinaryMsg(owner.networkId, this));
             // this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
-
         }
+        */
+
         /// <summary>
         /// Create a new child differential drive
         /// </summary>
         /// <param name="parent">The parent thing</param>
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
-        public DifferentialDrive(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.DifferentialDrive, thingId, invokeEvent) { }
+        public DifferentialDrive(Thing parent) : base(Type.DifferentialDrive, parent) { }
 
         /// @brief Configures the dimensions of the drive
         /// @param wheelDiameter The diameter of the wheels in meters
@@ -47,7 +51,7 @@ namespace RoboidControl {
         /// @sa SetLinearVelocity SetAngularVelocity
         public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) {
             this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
-            this.rpsToMs = wheelDiameter * Angle.pi;
+            this.rpsToMs = wheelDiameter * (float)Math.PI;
 
             if (wheelSeparation > 0) {
                 wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
@@ -101,9 +105,9 @@ namespace RoboidControl {
         /// Positive moves the robot in the forward direction.
         public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) {
             // This only works when the motor is a motor with encoder
-            if (this.leftWheel is EncoderMotor leftMotor)
+            if (this.leftWheel is ControlledMotor leftMotor)
                 leftMotor.targetAngularSpeed = angularSpeedLeft;
-            if (this.rightWheel is EncoderMotor rightMotor)
+            if (this.rightWheel is ControlledMotor rightMotor)
                 rightMotor.targetAngularSpeed = angularSpeedRight;
         }
 
@@ -160,8 +164,7 @@ namespace RoboidControl {
         public override void ProcessBinary(byte[] data) {
             byte ix = 0;
             byte leftWheelId = data[ix++];
-            Thing leftMotor = this.owner.Get(leftWheelId);
+            this.leftWheel = this.owner.Get(leftWheelId) as Motor;
-            this.leftWheel = leftMotor as Motor;
             byte rightWheelId = data[ix++];
             this.rightWheel = this.owner.Get(rightWheelId) as Motor;
             this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);

src/Things/DigitalSensor.cs

@@ -6,6 +6,7 @@ namespace RoboidControl {
     /// A sensor which can detect touches
     /// </summary>
     public class DigitalSensor : Thing {
+        /*
         /// <summary>
         /// Create a digital sensor without communication abilities
         /// </summary>
@@ -18,13 +19,14 @@ namespace RoboidControl {
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
         public DigitalSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.Switch, thingId, invokeEvent) { }
+        */
         /// <summary>
         /// Create a new child digital sensor
         /// </summary>
         /// <param name="parent">The parent thing</param>
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
-        public DigitalSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.Switch, thingId, invokeEvent) { }
+        public DigitalSensor(Thing parent) : base(Type.Switch, parent) { }
 
         /// <summary>
         /// Value which is true when the sensor is touching something, false otherwise

src/Things/DistanceSensor.cs

@@ -9,19 +9,23 @@ namespace RoboidControl {
         /// </summary>
         public float distance = 0;
 
+        /*
         /// <summary>
         /// Constructor for a new distance sensor
         /// </summary>
         /// <param name="participant">The participant for which the sensor is needed</param>
         public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { }
+        
         /// <summary>
         /// Create a distance sensor with the given ID
         /// </summary>
         /// <param name="owner">The participant for with the sensor is needed</param>
         /// <param name="networkId">The network ID of the sensor</param>
         /// <param name="thingId">The ID of the thing</param>
-        public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) {
+        public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) {}
-        }
+        */
+        public DistanceSensor(Thing parent): base(Type.DistanceSensor, parent) {}
+        
 
 #if UNITY_5_3_OR_NEWER
         /// @copydoc Passer::RoboidControl::Thing::CreateComponent              

src/Things/Motor.cs

@@ -3,10 +3,8 @@ using LinearAlgebra;
 namespace RoboidControl {
 
     public class Motor : Thing {
-        public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { }
+        //public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { }
-        public Motor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.UncontrolledMotor, thingId, invokeEvent) { }
+        public Motor(Thing parent) : base(Type.UncontrolledMotor, parent) { }
-        public Motor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.UncontrolledMotor, thingId, invokeEvent) { }
-
 
         /// @brief Motor turning direction
         public enum Direction {
@@ -40,7 +38,7 @@ namespace RoboidControl {
             return data;
         }
         public override void ProcessBinary(byte[] data) {
-            this.targetSpeed = (float)(sbyte)data[0] / 127;
+            this.targetSpeed = (float)data[0] / 127;
         }
     }
 

src/Things/RelativeEncoder.cs

@@ -9,7 +9,12 @@ namespace RoboidControl {
         /// full rotation
         /// @param distancePerRevolution The distance a wheel travels per full
         /// rotation
+        /*
         public RelativeEncoder(bool invokeEvent = true) : base(Type.IncrementalEncoder, invokeEvent) { }
+        */
+        public RelativeEncoder(Thing parent = default) : base(Type.RelativeEncoder, parent) {
+            
+        }
 
         protected float _rotationSpeed = 0;
         /// @brief Get the rotation speed since the previous call

src/Things/TemperatureSensor.cs

@@ -6,6 +6,7 @@ namespace RoboidControl {
     /// A temperature sensor
     /// </summary>
     public class TemperatureSensor : Thing {
+        /*
         /// <summary>
         /// Create a temperature sensor without communication abilities
         /// </summary>
@@ -18,14 +19,14 @@ namespace RoboidControl {
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
         public TemperatureSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TemperatureSensor, thingId, invokeEvent) { }
-
+        */
         /// <summary>
         /// Create a new child temperature sensor
         /// </summary>
         /// <param name="parent">The parent thing</param>
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
-        public TemperatureSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TemperatureSensor, thingId, invokeEvent) { }
+        public TemperatureSensor(Thing parent) : base(Type.TemperatureSensor, parent) { }
 
         /// <summary>
         /// The measured temperature

src/Things/TouchSensor.cs

@@ -6,6 +6,7 @@ namespace RoboidControl {
     /// A sensor which can detect touches
     /// </summary>
     public class TouchSensor : Thing {
+        /*
         /// <summary>
         /// Create a touch sensor without communication abilities
         /// </summary>
@@ -18,13 +19,14 @@ namespace RoboidControl {
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
         public TouchSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TouchSensor, thingId, invokeEvent) { }
+        */
         /// <summary>
         /// Create a new child touch sensor
         /// </summary>
         /// <param name="parent">The parent thing</param>
         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
-        public TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) {
+        public TouchSensor(Thing parent) : base(Type.TouchSensor, parent) {
             this.name = "TouchSensor";
          }
 
@@ -35,10 +37,9 @@ namespace RoboidControl {
         public bool touchedSomething {
             get { return _touchedSomething; }
             set {
-                //if (_touchedSomething != value) {
+                if (_touchedSomething != value) {
                     _touchedSomething = value;
-                    owner.Send(new BinaryMsg(this));
+                }
-                //}
                 touchUpdated = true;
             }
         }
@@ -58,24 +59,21 @@ namespace RoboidControl {
         /// <returns>A byte array with the binary data</returns>
         /// <remark>The byte array will be empty when the touch status has not changed</remark>
         public override byte[] GenerateBinary() {
-            if (this.component == null || !touchUpdated)
+            if (!touchUpdated)
                 return Array.Empty<byte>();
 
-            Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor;
             byte[] bytes = new byte[1];
-            bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0);
+            bytes[0] = (byte)(touchedSomething ? 1 : 0);
             touchUpdated = false;
-            Console.WriteLine($"Sending touch {bytes[0]}");
             return bytes;
         }
 
-        private bool externalTouch = false;
         /// <summary>
         /// Function used to process binary data received for this touch sensor
         /// </summary>
         /// <param name="bytes">The binary data to process</param>
         public override void ProcessBinary(byte[] bytes) {
-            this.externalTouch = (bytes[0] == 1);
+            this.touchedSomething |= (bytes[0] == 1);
         }
     }
 }

test/UnitTest1.cs

@@ -65,7 +65,7 @@ namespace RoboidControl.test {
         public void Test_ThingMsg() {
             SiteServer siteServer = new(7681);
             ParticipantUDP participant = new("127.0.0.1", 7681, 7682);
-            Thing thing = new(participant) {
+            Thing thing = new() {
                 name = "First Thing",
                 modelUrl = "https://passer.life/extras/ant.jpg"
             };