Improved network sync

Unity/Thing.cs

@@ -17,6 +17,8 @@ namespace RoboidControl.Unity {
         [field: SerializeField]
         public RoboidControl.Thing core { get; set; }
 
+        public SiteServer participant;
+
         private string modelUrl = null;
 
         /// <summary>
@@ -41,18 +43,22 @@ namespace RoboidControl.Unity {
                 new("Thing") :
                 new(core.name);
             Thing component = gameObj.AddComponent<Thing>();
+            component.Init(core);
+            return component;
+        }
 
-            component.core = core;
+        protected void Init(RoboidControl.Thing core) {
+            this.core = core;
+            this.participant = FindAnyObjectByType<SiteServer>();
             if (core.parent != null && core.parent.component != null)
-                gameObj.transform.SetParent(core.parent.component.transform, false);
+                this.transform.SetParent(core.parent.component.transform, false);
 
             if (core.position != null)
-                gameObj.transform.localPosition = core.position.ToVector3();
+                this.transform.localPosition = core.position.ToVector3();
             if (core.orientation != null)
-                gameObj.transform.localRotation = core.orientation.ToQuaternion();
+                this.transform.localRotation = core.orientation.ToQuaternion();
 
-            core.OnPoseChanged += component.PoseChanged;
+            core.OnPoseChanged += this.PoseChanged;
-            return component;
         }
 
         /// <summary>
@@ -70,7 +76,8 @@ namespace RoboidControl.Unity {
             //     this.transform.localPosition = core.position.ToVector3();
 
             if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
-                Vector3 angularVelocity = core.angularVelocity.ToVector3();
+                // Vector3 angularVelocity = core.angularVelocity.ToVector3();
+                // Debug.Log(angularVelocity);
                 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);

Unity/TouchSensor.cs

@@ -8,7 +8,7 @@ namespace RoboidControl.Unity {
 	/// </summary>
 	public class TouchSensor : Thing {
 
-		public SiteServer participant;
+		// public SiteServer participant;
 		/// <summary>
 		/// The core touch sensor
 		/// </summary>
@@ -24,18 +24,6 @@ namespace RoboidControl.Unity {
 				participant = FindAnyObjectByType<SiteServer>();
 				SetCoreThing(new RoboidControl.TouchSensor(participant.site));
 			}
-			// Somehow this does not work.
-
-			// Rigidbody rb = GetComponentInParent<Rigidbody>();
-			// if (rb == null) {
-			//     RoboidControl.Thing thing = core;
-			//     while (thing.parent != null)
-			//         thing = thing.parent;
-
-			//     Thing unityThing = thing.component;
-			//     rb = unityThing.gameObject.AddComponent<Rigidbody>();
-			//     rb.isKinematic = true;
-			// }
 		}
 
 		/// <summary>
@@ -48,6 +36,8 @@ namespace RoboidControl.Unity {
 				new(core.name) :
 				new("Touch Sensor");
 			TouchSensor component = gameObj.AddComponent<TouchSensor>();
+			component.Init(core);
+
 			Rigidbody rb = gameObj.AddComponent<Rigidbody>();
 			rb.isKinematic = true;
 
@@ -55,17 +45,6 @@ namespace RoboidControl.Unity {
 			collider.radius = 0.01f;
 			collider.isTrigger = true;
 
-			component.core = core;
-			component.participant = FindAnyObjectByType<SiteServer>();
-			core.thisParticipant = component.participant.site;
-
-			if (core.parent != null && core.parent.component != null) {
-				gameObj.transform.SetParent(core.parent.component.transform, false);
-			}
-
-			if (core.position != null)
-				gameObj.transform.localPosition = core.position.ToVector3();
-
 			if (gameObj.transform.parent != null && gameObj.transform.localPosition.magnitude > 0) {
 				collider.radius = Vector3.Distance(gameObj.transform.position, gameObj.transform.parent.position) / 2;
 				gameObj.transform.position = (gameObj.transform.position + gameObj.transform.parent.position) / 2;
@@ -80,12 +59,14 @@ namespace RoboidControl.Unity {
 			if (this.transform.root == other.transform.root)
 				return;
 
+			Debug.Log($"*** {this} Touch");
 			this.coreSensor.touchedSomething = true;
 		}
 		private void OnTriggerExit(Collider other) {
 			if (other.isTrigger)
 				return;
 
+			Debug.Log($"*** {this} Touch end");
 			this.coreSensor.touchedSomething = false;
 		}
 	}

src/Messages/BinaryMsg.cs

@@ -61,7 +61,7 @@ namespace RoboidControl {
                 return 0;
 
 #if DEBUG
-            // System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
+            System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
 #endif
             byte ix = 0;
             buffer[ix++] = BinaryMsg.Id;

src/Messages/DestroyMsg.cs

@@ -31,7 +31,10 @@ namespace RoboidControl {
             this.thingId = thingId;
         }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
-        public DestroyMsg(byte[] buffer) : base(buffer) { }
+        public DestroyMsg(byte[] buffer) : base(buffer) {
+            this.networkId = buffer[1];
+            this.thingId = buffer[2];
+         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {

src/Messages/PoseMsg.cs

@@ -96,12 +96,10 @@ namespace RoboidControl {
             if (thing.positionUpdated || force) {
                 this.position = thing.position;
                 this.poseType |= Pose_Position;
-                //thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
             }
             if (thing.orientationUpdated || force) {
                 this.orientation = thing.orientation;
                 this.poseType |= Pose_Orientation;
-                //thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
             }
             if (thing.linearVelocityUpdated) {
                 this.linearVelocity = thing.linearVelocity;

src/Participant.cs

@@ -42,7 +42,7 @@ namespace RoboidControl {
         /// <summary>
         /// The things managed by this participant
         /// </summary>
-        protected readonly List<Thing> things = new List<Thing>();
+        public readonly List<Thing> things = new List<Thing>();
 
         public virtual void Update(ulong currentTimeMS = 0) {
             int n = this.things.Count;

src/ParticipantUDP.cs

@@ -129,26 +129,49 @@ namespace RoboidControl {
                 this.nextPublishMe = currentTimeMS + this.publishInterval;
             }
 
+            UpdateMyThings(currentTimeMS);
+            UpdateOtherThings(currentTimeMS);
+        }
+
+        protected virtual void UpdateMyThings(ulong currentTimeMS) {
             int n = this.things.Count;
             for (int ix = 0; ix < n; ix++) {
                 Thing thing = this.things[ix];
-                if (thing != null && thing.parent == null) {
+                if (thing == null || thing.parent != null)
+                    continue;
+
                 thing.Update(currentTimeMS, true);
-                    // if (this.isIsolated == false) {
+                if (this.isIsolated || this.networkId == 0)
-                    if (thing.owner != this) { // should not happen....
+                    continue;
+
+                // Send to remote site
                 PoseMsg poseMsg = new(thing.owner.networkId, thing);
-                        this.Send(thing.owner, poseMsg);
+                this.Send(this.remoteSite, poseMsg);
                 BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
-                        this.Send(thing.owner, binaryMsg);
+                this.Send(this.remoteSite, binaryMsg);
-                    }
             }
         }
+
+        protected virtual void UpdateOtherThings(ulong currentTimeMS) {
             for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) {
-                Participant owner = Participant.participants[ownerIx];
+                Participant participant = Participant.participants[ownerIx];
-                owner.Update(currentTimeMS);
+                if (participant == null || participant == this)
+                    continue;
+
+                participant.Update(currentTimeMS);
+                if (this.isIsolated)
+                    continue;
+
+                foreach (Thing thing in participant.things) {
+                    PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                    this.Send(participant, poseMsg);
+                    BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                    this.Send(participant, binaryMsg);
                 }
             }
 
+        }
+
         public virtual void Publish() {
             this.Publish(new ParticipantMsg(this.networkId));
         }
@@ -253,6 +276,7 @@ namespace RoboidControl {
                     // result = await TextMsg.Receive(dataStream, client, packetSize);
                     break;
                 case DestroyMsg.Id: // 0x20 / 32
+                    this.Process(sender, new DestroyMsg(data));
                  // result = await DestroyMsg.Receive(dataStream, client, packetSize);
                     break;
                 default:
@@ -324,10 +348,14 @@ namespace RoboidControl {
                     thing.position = msg.position;
                 if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
                     thing.orientation = msg.orientation;
-                if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
+                if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) {
                     thing.linearVelocity = msg.linearVelocity;
-                if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
+                    //Console.Write($"linear velocity = {thing.linearVelocity.ToVector3()}");
+                }
+                if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) {
                     thing.angularVelocity = msg.angularVelocity;
+                    //Console.Write($"angular velocity = {thing.angularVelocity.ToVector3()}");
+                }
             }
         }
 
@@ -348,7 +376,7 @@ namespace RoboidControl {
 
         protected virtual void Process(Participant sender, DestroyMsg msg) {
 #if DEBUG
-            Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]");
+            Console.WriteLine($"Participant: Process Destroy Msg [{msg.networkId}/{msg.thingId}]");
 #endif            
 
         }

src/SiteServer.cs

@@ -30,6 +30,30 @@ namespace RoboidControl {
             this.udpClient?.Close();
         }
 
+        protected override void UpdateMyThings(ulong currentTimeMS) {
+            int n = this.things.Count;
+            for (int ix = 0; ix < n; ix++) {
+                Thing thing = this.things[ix];
+                if (thing == null || thing.parent != null)
+                    continue;
+
+                thing.Update(currentTimeMS, true);
+                if (this.isIsolated || this.networkId == 0)
+                    continue;
+
+                // Send to all other participants
+                foreach (Participant participant in Participant.participants) {
+                    if (participant == null || participant == this)
+                        continue;
+
+                    PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                    this.Send(participant, poseMsg);
+                    BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                    this.Send(participant, binaryMsg);
+                }
+            }
+        }
+
         public override void Publish() {
         }
 
@@ -44,30 +68,21 @@ namespace RoboidControl {
         protected override void Process(Participant sender, NetworkIdMsg msg) { }
 
         protected override void Process(Participant sender, ThingMsg msg) {
-            Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
+            Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             if (thing == null) {
-                Thing newThing = null;
+                thing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
-                // if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
-                //     //Console.WriteLine("Found thing message processor");
-                //     if (msgProcessor != null)
-                //         newThing = msgProcessor(sender, msg.networkId, msg.thingId);
-                // }
-                // if (newThing == null) {
-                newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
                 // Console.WriteLine("Created generic new core thing");
                 // }
+            }
+
             if (msg.parentId != 0) {
                 Thing parentThing = Get(msg.networkId, msg.parentId);
                 if (parentThing == null)
                     Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]");
                 else
-                        newThing.parent = parentThing;
+                    thing.parent = parentThing;
-                }
-                //Console.WriteLine("Adding to remote sender");
-                //sender.Add(newThing, false, true);
             }
         }
-
     }
 }

src/Thing.cs

@@ -367,6 +367,8 @@ namespace RoboidControl {
                 OnPoseChanged?.Invoke();
             this.positionUpdated = false;
             this.orientationUpdated = false;
+            this.linearVelocityUpdated = false;
+            this.angularVelocityUpdated = false;            
 
             // should recurse over children...
             if (recursively) {

src/Things/TouchSensor.cs

@@ -19,7 +19,7 @@ namespace RoboidControl {
         }
 
         public TouchSensor(Participant owner, byte networkId, byte thingId) : base(owner, networkId, thingId) {
-            Console.Write("TouchSensor constructor");
+            // Console.Write("TouchSensor constructor");
             //touchedSomething = false;
             //thisParticipant = participant;
         }
@@ -36,15 +36,13 @@ namespace RoboidControl {
         public bool touchedSomething {
             get { return _touchedSomething; }
             set {
+                if (_touchedSomething != value) {
+                    touchUpdated = true;
                     _touchedSomething = value;
-                if (thisParticipant != null && this.owner != thisParticipant) {
-                    BinaryMsg msg = new(networkId, this);
-                    this.thisParticipant.Send(this.owner, msg);
-                    // foreach (Participant remoteParticipant in thisParticipant.participants)
-                    //     thisParticipant.Send(remoteParticipant, msg);
                 }
             }
         }
+        private bool touchUpdated = false;
 
 #if UNITY_5_3_OR_NEWER
         /// @copydoc Passer::RoboidControl::Thing::CreateComponent 
@@ -55,8 +53,12 @@ namespace RoboidControl {
         }
 #endif
         public override byte[] GenerateBinary() {
+            if (!touchUpdated)
+                return new byte[0];
+
             byte[] buffer = new byte[1];
             buffer[0] = (byte)(touchedSomething ? 1 : 0);
+            touchUpdated = false;
             return buffer;
         }
     }