Merge branch 'main' of https://git.passer.life/RoboidControl/RoboidControl-Csharp

LinearAlgebra/src/Angle.cs

@@ -12,8 +12,8 @@ namespace LinearAlgebra
         // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
         // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
 
-        public const float Deg2Rad = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
+        public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
-        public const float Rad2Deg = ((float)Math.PI * 2) / 360.0f; //57.29578F;
+        public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F;
 
         /// <summary>
         /// Clamp the angle between the given min and max values

LinearAlgebra/src/Direction.cs

@@ -1,3 +1,8 @@
+using System;
+#if UNITY_5_3_OR_NEWER
+using Vector3Float = UnityEngine.Vector3;
+#endif
+
 namespace LinearAlgebra
 {
 
@@ -32,6 +37,23 @@ namespace LinearAlgebra
                 this.horizontal += 180;
                 this.vertical = 180 - this.vertical;
             }
+        }
+
+                public Vector3Float ToVector3()
+        {
+            float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad;
+            float horizontalRad = this.horizontal * Angle.Deg2Rad;
+            float cosVertical = (float)Math.Cos(verticalRad);
+            float sinVertical = (float)Math.Sin(verticalRad);
+            float cosHorizontal = (float)Math.Cos(horizontalRad);
+            float sinHorizontal = (float)Math.Sin(horizontalRad);
+
+            float x = sinVertical * sinHorizontal;
+            float y = cosVertical;
+            float z = sinVertical * cosHorizontal;
+
+            Vector3Float v = new(x, y, z);
+            return v;
         }
     }
 

LinearAlgebra/src/SwingTwist.cs

@@ -1,3 +1,8 @@
+using System.Numerics;
+#if UNITY_5_3_OR_NEWER
+using Quaternion = UnityEngine.Quaternion;
+#endif
+
 namespace LinearAlgebra
 {
 
@@ -27,6 +32,15 @@ namespace LinearAlgebra
             SwingTwist r = new SwingTwist(up, right, forward);
             return r;
         }
+
+#if UNITY_5_3_OR_NEWER
+        public Quaternion ToQuaternion() {
+            Quaternion q = Quaternion.Euler(-this.swing.vertical,
+                                   this.swing.horizontal,
+                                   this.twist);
+                                   return q;
+        }
+#endif        
     }
 
 }

LinearAlgebra/test/AngleTest.cs

@@ -1,3 +1,4 @@
+#if !UNITY_5_6_OR_NEWER
 using NUnit.Framework;
 
 namespace LinearAlgebra.Test
@@ -167,3 +168,4 @@ namespace LinearAlgebra.Test
         }
     }
 }
+#endif

Unity/SiteServer.cs

@@ -22,7 +22,7 @@ namespace RoboidControl.Unity {
         }
 
         public void HandleNewThing(RoboidControl.Thing thing) {
-            //Debug.Log("Handle New thing event");
+            Debug.Log($"Handle New thing event for {thing}");
             site.Add(thing, false);
             thingQueue.Enqueue(thing);
         }

Unity/Thing.cs

@@ -58,18 +58,19 @@ namespace RoboidControl.Unity {
             if (core == null)
                 return;
 
-            if (core.linearVelocity != null) {
+            if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
                 this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self);
-            }
+            } else if (core.positionUpdated)
-            if (core.angularVelocity != null) {
-                Vector3 angularVelocity = core.angularVelocity.ToVector3();
-                this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
-            }
-
-            if (core.hasPosition)
                 this.transform.localPosition = core.position.ToVector3();
-            //this.transform.localRotation = core.orientation.ToQuaternion();
+            
+            if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
+                Vector3 angularVelocity = core.angularVelocity.ToVector3();
+                Vector3 axis = core.angularVelocity.direction.ToVector3();
+                this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
+                //this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
+            } else if (core.orientationUpdated)
+                this.transform.localRotation = core.orientation.ToQuaternion();
 
             if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
                 string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));

src/Participant.cs

@@ -73,8 +73,8 @@ namespace RoboidControl {
             if (foundThing == null) {
                 things.Add(thing);
 
-                if (invokeEvent)
+                // if (invokeEvent)
-                    Thing.InvokeNewThing(thing);
+                //     Thing.InvokeNewThing(thing);
                 // Console.Write($"Add thing {ipAddress}:{port}[{networkId}/{thing.id}]");
             }
             // else {

src/Thing.cs

@@ -42,20 +42,29 @@ namespace RoboidControl {
         #region Init
 
         /// <summary>
-        /// Create a new thing without communication abilities
+        /// Create a new thing for a participant
         /// </summary>
+        /// <param name="owner">The participant owning the thing</param>
         /// <param name="thingType">The type of thing</param>
-        public Thing(byte thingType = (byte)Type.Undetermined) : this(LocalParticipant.Isolated(), thingType) {
+        public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
+            this.owner = owner;
+            this.type = thingType;
+            this.owner.Add(this);
+            if (invokeEvent)
+                InvokeNewThing(this);
         }
-
         /// <summary>
         /// Create a new thing for a participant
         /// </summary>
         /// <param name="owner">The participant owning the thing</param>
         /// <param name="thingType">The type of thing</param>
-        public Thing(Participant owner, byte thingType = (byte)Type.Undetermined) {
+        public Thing(Participant owner, Type thingType = Type.Undetermined, bool invokeEvent = true) : this(owner, (byte)thingType, invokeEvent) {
-            this.owner = owner;
+        }
-            this.type = thingType;
+        /// <summary>
+        /// 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) {
         }
 
         /// <summary>
@@ -63,7 +72,7 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="parent">The parent thing</param>
         /// <param name="thingType">The type of thing</param>
-        public Thing(Thing parent, byte thingType = (byte)Type.Undetermined) : this(parent.owner, thingType) {
+        public Thing(Thing parent, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(parent.owner, thingType, invokeEvent) {
             this.parent = parent;
         }
 
@@ -304,7 +313,7 @@ namespace RoboidControl {
             set {
                 if (_linearVelocity != value) {
                     _linearVelocity = value;
-                    linearVelocityUpdated = true;
+                    hasLinearVelocity = _linearVelocity.distance != 0;
                     OnLinearVelocityChanged?.Invoke(_linearVelocity);
                 }
             }
@@ -316,7 +325,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated linear velocity
         /// </summary>
-        public bool linearVelocityUpdated = false;
+        public bool hasLinearVelocity = false;
 
         private Spherical _angularVelocity = Spherical.zero;
         /// <summary>
@@ -327,7 +336,7 @@ namespace RoboidControl {
             set {
                 if (_angularVelocity != value) {
                     _angularVelocity = value;
-                    angularVelocityUpdated = true;
+                    hasAngularVelocity = _angularVelocity.distance != 0;
                     OnAngularVelocityChanged?.Invoke(_angularVelocity);
                 }
             }
@@ -339,7 +348,7 @@ namespace RoboidControl {
         /// <summary>
         /// Boolean indicating the thing has an updated angular velocity
         /// </summary>
-        public bool angularVelocityUpdated = false;
+        public bool hasAngularVelocity = false;
 
 #if UNITY_5_3_OR_NEWER
         /// <summary>
@@ -375,6 +384,8 @@ namespace RoboidControl {
                     child.Update(currentTimeMs, recursively);
                 }
             }
+            positionUpdated = false;
+            orientationUpdated = false;
         }
 
         /// <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) { }
+        public DifferentialDrive(LocalParticipant participant) : base(participant, Type.Undetermined) { }
 
         /// @brief Configures the dimensions of the drive
         /// @param wheelDiameter The diameter of the wheels in meters

src/Things/DistanceSensor.cs

@@ -13,7 +13,7 @@ namespace RoboidControl {
         /// 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) { }
+        public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { }
         /// <summary>
         /// Create a distance sensor with the given ID
         /// </summary>

src/Things/TouchSensor.cs

@@ -12,7 +12,8 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="owner">The participant for with the sensor is needed</param>
         /// <param name="invokeEvent">True when the creation should trigger an event</param>
-        public TouchSensor(Participant owner) : base(owner) {
+        public TouchSensor(Participant owner) : base(owner, Type.TouchSensor) {
+            Console.Write("TouchSensor constructor");
             //touchedSomething = false;
             //thisParticipant = owner;
         }