Yay pathfinding! (AGAIN (YES IT ACTUALLY WORKS NOW))

DepthsBelow/DepthsBelow/Component/PathFinder.cs

@@ -166,7 +166,7 @@ namespace DepthsBelow.Component
 			if (path == null)
 				Debug.WriteLine(this.Parent.ToString() + ": Path not found!");
 
-			this.CollisionMap = new byte[100, 100];
+			//this.CollisionMap = new byte[100, 100];
 
 			return path;
 		}

DepthsBelow/DepthsBelow/Interface.cs

@@ -193,23 +193,59 @@ namespace DepthsBelow
                         Point mainpath = Point.Zero;
                         Stack<Soldier> soldierStack = new Stack<Soldier>();
                         //core.Squad.Where(a=>a.Selected).ToList().ForEach(a => soldierStack.Push(a));
-                        foreach (var soldier in core.Squad)
+                        /*foreach (var soldier in core.Squad)
                         {
                             if (soldier.Selected)
                             {
                                 soldierStack.Push(soldier);
                             }
+                        }*/
+
+						// Don't move into other soldiers
+						bool blocked = false;
+						foreach (var soldier in core.Squad)
+							if (soldier.Position == mouseGridPos)
+								blocked = true;
+
+						if (!blocked)
+						{
+							byte[,] collisionMap = Core.Map.GetCollisionMap();
+
+							// Try pathfinding
+							foreach (var unit in core.Squad)
+							{
+								if (unit.Selected)
+								{
+									if (mainpath == Point.Zero)
+									{
+										mainpath = unit.Position;
+									}
+									Vector2 fromMain = new Vector2((mainpath.X - unit.Position.X)*Grid.TileSize,
+									                               (mainpath.Y - unit.Position.Y)*Grid.TileSize);
+									var goal = Grid.WorldToGrid(mouseWorldPos - fromMain);
+									unit.GetComponent<Component.PathFinder>().Goal = goal;
+									if (unit.GetComponent<Component.PathFinder>().IsMoving == false)
+									{
+										// If a path was not found, add the unit to the stack
+										soldierStack.Push(unit);
+									}
+									else
+									{
+										collisionMap[goal.X, goal.Y] = 0;
+									}
+								}
 							}
 
-                        byte[,] collisionMap = Core.Map.GetCollisionMap();
+							// Loop in a spiral and assign the soldiers in the stack their own unblocked positions
-                        int X=5;
+							int X = 10;
-                        int Y=5;
+							int Y = 10;
 							int x, y, dx, dy;
 							x = y = dx = 0;
 							dy = -1;
 							int t = (int) MathHelper.Max(X, Y);
 							int maxI = t*t;
-                        for(int i =0; i < maxI; i++){
+							for (int i = 0; i < maxI; i++)
+							{
 								if ((-X/2 < x && x <= X/2) && (-Y/2 < y && y <= Y/2))
 								{
 									Point point = new Point(x + mouseGridPos.X, y + mouseGridPos.Y);
@@ -229,13 +265,15 @@ namespace DepthsBelow
 										{
 											soldier.GetComponent<Component.PathFinder>().Goal = point;
 											Console.WriteLine(x + " " + y);
-                                        Console.WriteLine(soldier.GetComponent<Component.PathFinder>().Goal.X + " " + soldier.GetComponent<Component.PathFinder>().Goal.Y);
+											Console.WriteLine(soldier.GetComponent<Component.PathFinder>().Goal.X + " " +
+											                  soldier.GetComponent<Component.PathFinder>().Goal.Y);
 										}
 									}
 
 
 								}
-    	                    if( (x == y) || ((x < 0) && (x == -y)) || ((x > 0) && (x == 1-y))){
+								if ((x == y) || ((x < 0) && (x == -y)) || ((x > 0) && (x == 1 - y)))
+								{
 									t = dx;
 									dx = -dy;
 									dy = t;
@@ -243,18 +281,7 @@ namespace DepthsBelow
 								x += dx;
 								y += dy;
 							}
-                        //foreach (var unit in core.Squad)
+						}
-                        //{
-                        //    if (unit.Selected)
-                        //    {
-                        //        if (mainpath==Point.Zero)
-                        //        {
-                        //            mainpath = unit.Position;
-                        //        }
-                        //        Vector2 fromMain = new Vector2((mainpath.X - unit.Position.X) * 30,(mainpath.Y-unit.Position.Y) * 30);
-                        //        unit.GetComponent<Component.PathFinder>().Goal = Grid.WorldToGrid(mouseWorldPos - fromMain);
-                        //    }
-                        //}
 					}
 				}
 			};

DepthsBelow/DepthsBelow/SmallEnemy.cs

@@ -61,6 +61,12 @@ namespace DepthsBelow
 		                   };
 	        AddComponent(stat);
         }
+		public override void Remove()
+		{
+			Swarm.Remove(this);
+
+			base.Remove();
+		}
 
         public bool Selected
         {
@@ -148,7 +154,7 @@ namespace DepthsBelow
                                     {
                                         if (Utility.AttackTest(this.GetComponent<Component.Stat>(), entity, Utility.CalculateHitChance(this.GetComponent<Component.Stat>(), this.Transform.World.Position, entity)))
                                         {
-                                            return;
+                                            break;
                                         }
                                     }
                                 }

DepthsBelow/DepthsBelow/Soldier.cs

@@ -84,6 +84,13 @@ namespace DepthsBelow
 			AddComponent(stat);
 		}
 
+		public override void Remove()
+		{
+			Squad.Remove(this);
+
+			base.Remove();
+		}
+
 		public bool Selected
 		{
 			get { return _selected; }