Site forwarding other participants' things (but not working properly)

Unity/DifferentialDrive.cs

@@ -126,6 +126,10 @@ namespace RoboidControl.Unity {
                     // Use smoothing to emulate motor inertia
                     rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
                     rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
+
+                    core.position = LinearAlgebra.Spherical.FromVector3(this.transform.localPosition);
+                    core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
+
                 }
             }
         }

Unity/Resources/DifferentialDrive.prefab

@@ -201,11 +201,11 @@ MonoBehaviour:
   m_GameObject: {fileID: 3377575892836316963}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: d47256d84ddc6fc4b95bed051f75f267, type: 3}
+  m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}
-  wheelRadius: 0
+  wheelRadius: 0.025
 --- !u!135 &3720747953092717687
 SphereCollider:
   m_ObjectHideFlags: 0
@@ -236,7 +236,7 @@ GameObject:
   serializedVersion: 6
   m_Component:
   - component: {fileID: 6798369561388671537}
-  - component: {fileID: 2975045340952151157}
+  - component: {fileID: 1296007780791728683}
   - component: {fileID: 4118868690347991998}
   m_Layer: 0
   m_Name: DifferentialDrive
@@ -264,7 +264,7 @@ Transform:
   - {fileID: 213193100111257669}
   m_Father: {fileID: 0}
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
---- !u!114 &2975045340952151157
+--- !u!114 &1296007780791728683
 MonoBehaviour:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
@@ -273,15 +273,15 @@ MonoBehaviour:
   m_GameObject: {fileID: 4326386140118987503}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: d93f04a0dd496484c86f546a73574309, type: 3}
+  m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}
   leftMotor: {fileID: 0}
   rightMotor: {fileID: 0}
-  leftWheel: {fileID: 0}
+  leftWheel: {fileID: 6773972788910618332}
-  rightWheel: {fileID: 0}
+  rightWheel: {fileID: 7425139233737877139}
-  casterWheel: {fileID: 0}
+  casterWheel: {fileID: 894333789048616995}
 --- !u!54 &4118868690347991998
 Rigidbody:
   m_ObjectHideFlags: 0
@@ -435,11 +435,11 @@ MonoBehaviour:
   m_GameObject: {fileID: 5122915782100933114}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: d47256d84ddc6fc4b95bed051f75f267, type: 3}
+  m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}
-  wheelRadius: 0
+  wheelRadius: 0.025
 --- !u!135 &8150853676422519226
 SphereCollider:
   m_ObjectHideFlags: 0

Unity/Resources/TouchSensor.prefab

@@ -92,7 +92,7 @@ GameObject:
   serializedVersion: 6
   m_Component:
   - component: {fileID: 839661735326876684}
-  - component: {fileID: 1922171662874522792}
+  - component: {fileID: 4006928432304472165}
   - component: {fileID: 5295705618145761524}
   - component: {fileID: 5884202444353424411}
   m_Layer: 0
@@ -118,7 +118,7 @@ Transform:
   - {fileID: 1122722937167266542}
   m_Father: {fileID: 0}
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
---- !u!114 &1922171662874522792
+--- !u!114 &4006928432304472165
 MonoBehaviour:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
@@ -127,7 +127,7 @@ MonoBehaviour:
   m_GameObject: {fileID: 6521541507066528382}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: 74fd22179b1a3ac4186b7dc083d24e06, type: 3}
+  m_Script: {fileID: 11500000, guid: ab969221c3007d441b18dd7387cf22d4, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}

Unity/SiteServer.cs

@@ -54,8 +54,8 @@ namespace RoboidControl.Unity {
                         participant.port = participantUDP.port;
                     }
 
-                    foreach (RoboidControl.Thing thing in this.site.things)
+                    // foreach (RoboidControl.Thing thing in this.site.things)
-                        participant.coreParticipant.SendThingInfo(thing);
+                    //     participant.coreParticipant.SendThingInfo(thing);
 
                     break;
                 case ThingMsg.id:

Unity/Thing.cs

@@ -74,11 +74,13 @@ namespace RoboidControl.Unity {
             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);
+                // core.position = LinearAlgebra.Spherical.FromVector3(this.transform.localPosition);                
             }
 
             if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
                 Vector3 axis = core.angularVelocity.direction.ToVector3();
                 this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
+                // core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
             }
         }
 

src/Participants/ParticipantUDP.cs

@@ -228,31 +228,34 @@ namespace RoboidControl {
 
         protected virtual void UpdateOtherThings(ulong currentTimeMS) {
             for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) {
-                Participant participant = Participant.participants[ownerIx];
+                Participant owner = Participant.participants[ownerIx];
-                if (participant == null || participant == this)
+                if (owner == null || owner == this)
                     continue;
 
-                participant.Update();
+                owner.Update();
-                if (this.isIsolated)
+                // if (this.isIsolated)
-                    continue;
-
-                // if (currentTimeMS > this.nextSendUpdate) {
-                //     for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
-                //         Thing thing = participant.things[thingIx];
-                //         if (thing == null)
                 //     continue;
 
-                //         // PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                if (currentTimeMS > this.nextSendUpdate) {
-                //         // this.Send(participant, poseMsg);
+                    for (int thingIx = 0; thingIx < owner.things.Count; thingIx++) {
-                //         // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                        Thing thing = owner.things[thingIx];
-                //         // this.Send(participant, binaryMsg);
+                        if (thing == null)
-                //         // participant.Send(new PoseMsg(thing.owner.networkId, thing));
+                            continue;
-                //         // participant.Send(new BinaryMsg(thing.owner.networkId, thing));
+
-                //     }
+                        foreach (Participant participant in Participant.participants) {
-                //     this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
+                            if (participant == this || participant == owner)
-                // }
+                                continue;
+
+                            Console.WriteLine($"[{thing.owner.networkId}/{thing.id}] Pose ---> {participant.networkId}");
+                            PoseMsg poseMsg = new(thing.owner.networkId, thing, true);
+                            participant.Send(poseMsg);
                         }
                     }
+                }
+            }
+            if (currentTimeMS > this.nextSendUpdate)
+                this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
+        }
 
         #endregion Update
 
@@ -326,6 +329,7 @@ namespace RoboidControl {
                             sender = AddParticipant(ipAddress, endPoint.Port, this);
                             sender.isRemote = true;
 
+
                             UpdateEvent e = new() {
                                 messageId = NetworkIdMsg.Id,
                                 participant = sender
@@ -496,8 +500,6 @@ namespace RoboidControl {
             Thing thing = owner.Get(msg.networkId, msg.thingId);
             if (thing != null)
                 thing.name = msg.name;
-            else
-                Console.WriteLine("--- Could not find thing");
 
             ForwardMessage(sender, msg);
         }
@@ -534,8 +536,6 @@ namespace RoboidControl {
                 thing.linearVelocity = msg.linearVelocity;
             if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
                 thing.angularVelocity = msg.angularVelocity;
-
-            ForwardMessage(sender, msg);
         }
 
         protected virtual void Process(Participant sender, BinaryMsg msg) {

src/Participants/SiteServer.cs

@@ -101,14 +101,41 @@ namespace RoboidControl {
 
         protected override void Process(Participant sender, ParticipantMsg msg) {
             base.Process(sender, msg);
-            if (msg.networkId != sender.networkId) {
+            if (msg.networkId == sender.networkId)
-                //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
+                return;
+
+            if (sender.component != null) {
+                Console.WriteLine("Already created Unity participant");
+                return;
+            }
+
             sender.Send(new NetworkIdMsg(sender.networkId));
             UpdateEvent e = new() {
                 messageId = ParticipantMsg.Id,
                 participant = sender
             };
             this.updateQueue.Enqueue(e);
+
+            foreach (Thing thing in this.things)
+                sender.SendThingInfo(thing);
+
+            foreach (Participant owner in Participant.participants) {
+                if (owner == null || owner == this)
+                    continue;
+
+                Console.WriteLine($"send thinginfo for {owner.things.Count} things");
+                foreach (Thing thing in owner.things) {
+                    if (thing == null)
+                        continue;
+
+                    foreach (Participant participant in Participant.participants) {
+                        if (participant == this || participant == owner)
+                            continue;
+
+                        Console.WriteLine($"[{thing.owner.networkId}/{thing.id}] Thing Info ---> {participant.networkId}");
+                        participant.SendThingInfo(thing);
+                    }
+                }
             }
         }