Fix MoveLastBlock command (it now works most of the time)

extracommands/ExtraCommands.csproj

@@ -28,7 +28,13 @@
     <Reference Include="CommandLine">
       <HintPath>..\ref\RobocraftX.Common.dll</HintPath>
     </Reference>
+    <Reference Include="RobocraftX.Blocks">
+      <HintPath>..\ref\RobocraftX.Blocks.dll</HintPath>
+    </Reference>
     <Reference Include="RobocraftX.Character">
+      <HintPath>..\ref\RobocraftX.Character.dll</HintPath>
+    </Reference>
+    <Reference Include="RobocraftX.Input">
       <HintPath>..\ref\RobocraftX.Input.dll</HintPath>
     </Reference>
     <Reference Include="RobocraftX.MainGame">

extracommands/MoveBlocksCommandEngine.cs

@@ -7,12 +7,12 @@ using Svelto.ECS;
 using Svelto.ECS.EntityStructs;
 using Unity.Entities;
 using Svelto.Context;
-using Svelto.DataStructures;
 using Svelto.Tasks;
 using RobocraftX;
 using RobocraftX.SimulationModeState;
 using RobocraftX.UECS;
 using Unity.Transforms;
+using Unity.Mathematics;
 
 namespace ExtraCommands.Building
 {
@@ -51,13 +51,13 @@ namespace ExtraCommands.Building
         // Move block with highest index by vector (x,y,z)
         private void MoveLastBlockCommand(float x, float y, float z)
         {
+            uint count = entitiesDB.Count<PositionEntityStruct>(CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
             ref SimulationModeStateEntityStruct simMode = ref this.entitiesDB.QueryUniqueEntity<SimulationModeStateEntityStruct>(SimulationModeStateExclusiveGroups.GAME_STATE_GROUP);
             if (simMode.simulationMode != SimulationMode.Build)
             {
                 uREPL.Log.Error("Blocks can only be moved in Build Mode");
                 return;
             }
-            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})");
         }
@@ -66,9 +66,9 @@ namespace ExtraCommands.Building
         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);
+            ref GridRotationStruct gridStruct = ref this.entitiesDB.QueryEntity<GridRotationStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            ref LocalTransformEntityStruct transStruct = ref this.entitiesDB.QueryEntity<LocalTransformEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+            ref UECSPhysicsEntityStruct phyStruct = ref this.entitiesDB.QueryEntity<UECSPhysicsEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
             // main (persistent) position
             posStruct.position.x += translationVector.x;
             posStruct.position.y += translationVector.y;
@@ -91,16 +91,41 @@ namespace ExtraCommands.Building
 
         private float3 TranslateConnectedBlocks(uint blockID, float3 translationVector)
         {
-            newPosition = TranslateSingleBlock(blockID, 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))
+            //float3 newPosition = TranslateSingleBlock(blockID, translationVector);
+            HashSet<uint> processedCubes = new HashSet<uint>();
+            Stack<uint> cubeStack = new Stack<uint>();
+            cubeStack.Push(blockID);
+            uint count;
+            GridConnectionsEntityStruct blockConnections;
+            MachineGraphConnectionEntityStruct[] connections;
+
+            while (cubeStack.Count > 0) // find all inter-connected blocks
+            {
+                uint connectedBlockID = cubeStack.Pop();
+                processedCubes.Add(connectedBlockID);
+                ScalingEntityStruct scale = entitiesDB.QueryEntity<ScalingEntityStruct>(connectedBlockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+                uREPL.Log.Output($"Catching {connectedBlockID} with scale {scale.scale}");
+                blockConnections = entitiesDB.QueryEntity<GridConnectionsEntityStruct>(connectedBlockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP);
+                connections = entitiesDB.QueryEntities<MachineGraphConnectionEntityStruct>(blockConnections.connectionGroup, out count);
+
+                foreach (MachineGraphConnectionEntityStruct conn in connections)
+                {
+                    if (!processedCubes.Contains(conn.connectedBlock.entityID) 
+                        && blockConnections.isConnectionGroupAssigned 
+                        && (conn.oppositeConnectionEgid.entityID != 0u 
+                         || conn.oppositeConnectionEgid.entityID != conn.connectedBlock.entityID))
+                    {
+                        uREPL.Log.Output($"Block {connectedBlockID} connects to {conn.connectedBlock.entityID} (opposite {conn.oppositeConnectionEgid.entityID})");
+                        cubeStack.Push(conn.connectedBlock.entityID);
+                    }
+                }
+            }
+            foreach (uint id in processedCubes)
             {
                 TranslateSingleBlock(id, translationVector);
             }
-            return newPosition;
+            uREPL.Log.Output($"Found {processedCubes.Count} connected blocks");
+            return this.entitiesDB.QueryEntity<PositionEntityStruct>(blockID, CommonExclusiveGroups.OWNED_BLOCKS_GROUP).position;
         }
 
         // unused; for future reference