Fix move so connected cubes are moved (UNTESTED)

extracommands/MoveBlocksCommandEngine.cs

@@ -2,10 +2,12 @@ using System;
 using System.Collections.Generic;
 using RobocraftX.Multiplayer;
 using RobocraftX.Common;
+using RobocraftX.Blocks;
 using Svelto.ECS;
 using Svelto.ECS.EntityStructs;
 using Unity.Entities;
 using Svelto.Context;
+using Svelto.DataStructures;
 using Svelto.Tasks;
 using RobocraftX;
 using RobocraftX.SimulationModeState;
@@ -37,37 +39,13 @@ namespace ExtraCommands.Building
                 uREPL.Log.Error("Blocks can only be moved in Build Mode");
                 return;
             }
-            uint posCount, gridCount, transCount, phyCount;
-            PositionEntityStruct[] posStructs = this.entitiesDB.QueryEntities<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out posCount);
-            GridRotationStruct[] gridStructs = this.entitiesDB.QueryEntities<GridRotationStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out gridCount);
-            LocalTransformEntityStruct[] transStructs = this.entitiesDB.QueryEntities<LocalTransformEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out transCount);
-            UECSPhysicsEntityStruct[] phyStructs = this.entitiesDB.QueryEntities<UECSPhysicsEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out phyCount);
-            if (posCount != gridCount || gridCount != transCount || transCount != phyCount)
+            uint count = this.entitiesDB.Count<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            float3 translationVector = new float3(x,y,z);
+            for (uint i = 0; i < count; i++)
             {
-                uREPL.Log.Error("Block lists returned are not the same length!"); // they're arrays, not lists
-                return;
-            }
-            for (uint i = 0; i < posCount; i++)
-            {
-                ref PositionEntityStruct posStruct = ref posStructs[i]; // main (persistent) position
-                posStruct.position.x += x;
-                posStruct.position.y += y;
-                posStruct.position.z += z;
-                ref GridRotationStruct gridStruct = ref gridStructs[i]; // main (persistent) position
-                gridStruct.position.x += x;
-                gridStruct.position.y += y;
-                gridStruct.position.z += z;
-                ref LocalTransformEntityStruct transStruct = ref transStructs[i]; // rendered position
-                transStruct.position.x += x;
-                transStruct.position.y += y;
-                transStruct.position.z += z;
-                ref UECSPhysicsEntityStruct phyStruct = ref phyStructs[i]; // collision position
-                this.physWorld.EntityManager.SetComponentData(phyStruct.uecsEntity, new Translation
-                {
-                    Value = posStruct.position
-                });
+                TranslateSingleBlock(i, translationVector);
             }
-            uREPL.Log.Output($"Moved {posCount} blocks");
+            uREPL.Log.Output($"Moved {count} blocks");
         }
 
         // Move block with highest index by vector (x,y,z)
@@ -79,34 +57,48 @@ namespace ExtraCommands.Building
                 uREPL.Log.Error("Blocks can only be moved in Build Mode");
                 return;
             }
-            uint posCount, gridCount, transCount, phyCount;
-            PositionEntityStruct[] posStructs = this.entitiesDB.QueryEntities<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out posCount);
-            GridRotationStruct[] gridStructs = this.entitiesDB.QueryEntities<GridRotationStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out gridCount);
-            LocalTransformEntityStruct[] transStructs = this.entitiesDB.QueryEntities<LocalTransformEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out transCount);
-            UECSPhysicsEntityStruct[] phyStructs = this.entitiesDB.QueryEntities<UECSPhysicsEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP, out phyCount);
-            if (posCount == 0 || transCount == 0 || phyCount == 0)
-            {
-                uREPL.Log.Error("No block found");
-                return;
-            }
-            ref PositionEntityStruct posStruct = ref posStructs[posCount-1]; // main (persistent) position
-            posStruct.position.x += x;
-            posStruct.position.y += y;
-            posStruct.position.z += z;
-            ref GridRotationStruct gridStruct = ref gridStructs[gridCount-1]; // main (persistent) position
-            gridStruct.position.x += x;
-            gridStruct.position.y += y;
-            gridStruct.position.z += z;
-            ref LocalTransformEntityStruct transStruct = ref transStructs[transCount-1]; // rendered position
-            transStruct.position.x += x;
-            transStruct.position.y += y;
-            transStruct.position.z += z;
-            ref UECSPhysicsEntityStruct phyStruct = ref phyStructs[phyCount-1]; // collision position
+            uint count = this.entitiesDB.Count<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            float3 newPos = TranslateConnectedBlocks(count-1, new float3(x,y,z));            
+            uREPL.Log.Output($"Moved block to ({newPos.x},{newPos.y},{newPos.z})");
+        }
+
+        // Move the block denoted by blockID by translationVector (old_position += translationVector)
+        private float3 TranslateSingleBlock(uint blockID, float3 translationVector)
+        {
+            ref PositionEntityStruct posStruct = ref this.entitiesDB.QueryEntity<PositionEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            ref GridRotationStruct gridStructs = this.entitiesDB.QueryEntity<GridRotationStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            ref LocalTransformEntityStruct transStructs = this.entitiesDB.QueryEntity<LocalTransformEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            ref UECSPhysicsEntityStruct phyStructs = this.entitiesDB.QueryEntity<UECSPhysicsEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            // main (persistent) position
+            posStruct.position.x += translationVector.x;
+            posStruct.position.y += translationVector.y;
+            posStruct.position.z += translationVector.z;
+            // placement grid  position
+            gridStruct.position.x += translationVector.x;
+            gridStruct.position.y += translationVector.y;
+            gridStruct.position.z += translationVector.z;
+            // rendered position
+            transStruct.position.x += translationVector.x;
+            transStruct.position.y += translationVector.y;
+            transStruct.position.z += translationVector.z;
+            // collision position
             this.physWorld.EntityManager.SetComponentData(phyStruct.uecsEntity, new Translation
             {
                 Value = posStruct.position
             });
-            uREPL.Log.Output($"Moved block to ({posStruct.position.x},{posStruct.position.y},{posStruct.position.z})");
+            return posStruct.position;
+        }
+
+        private void TranslateConnectedBlocks(uint blockID, float3 translationVector)
+        {
+            uint count = this.entitiesDB.Count<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            Stack<uint> cubeStack = new Stack<uint>(count);
+            FasterList<uint> cubeList = new FasterList<uint>(count);
+            ConnectedCubesUtility.TreeTraversal.GetConnectedCubes(this.entitiesDB, blockID, cubeStack, cubeList, false /*unused*/);
+            foreach (uint id in FasterList.ToArrayFast(cubeList))
+            {
+                TranslateSingleBlock(id, translationVector);
+            }
         }
 
         // unused; for future reference