Almost working pathfinding

DepthsBelow/DepthsBelow/Component/PathFinder.cs

@@ -29,6 +29,8 @@ namespace DepthsBelow.Component
 
 		public Point Start { get; private set; }
 
+		public byte[,] CollisionMap;
+
 		private Point goal;
 		public Point Goal
 		{
@@ -36,8 +38,8 @@ namespace DepthsBelow.Component
 			set 
 			{
 				this.goal = value;
-				this.Start = this.Parent.GetComponent<Transform>().Grid.Position;
+				//this.Start = this.Parent.GetComponent<Transform>().Grid.Position;
-				this.FindPath(this.Start, value);
+				this.FindPath(this.Parent.GetComponent<Transform>().Grid.Position, value, null);
 			}
 		}
 		
@@ -47,16 +49,7 @@ namespace DepthsBelow.Component
 		public PathFinder(Entity parent)
 			: base(parent)
 		{
-			pathFinder = new AStar.PathFinderFast(Core.Map.GetCollisionMap());
+			
-			pathFinder.Formula = AStar.HeuristicFormula.Manhattan;
-			pathFinder.Diagonals = false;
-			pathFinder.HeavyDiagonals = false;
-			pathFinder.HeuristicEstimate = 2;
-			pathFinder.PunishChangeDirection = true;
-			pathFinder.TieBreaker = false;
-			pathFinder.SearchLimit = 50000;
-			pathFinder.DebugProgress = false;
-			pathFinder.DebugFoundPath = false;
 		}
 
 		public override void Update(GameTime gameTime)
@@ -86,8 +79,25 @@ namespace DepthsBelow.Component
 			return (Node)nextNode;
 		}
 
-		public List<AStar.PathFinderNode> FindPath(Point start, Point goal)
+		private List<AStar.PathFinderNode> FindPath(Point start, Point goal, List<Point> appendCollisionMap)
 		{
+			byte[,] mapCollisionMap = Core.Map.GetCollisionMap();
+
+			if (appendCollisionMap != null)
+				foreach (var point in appendCollisionMap)
+					mapCollisionMap[point.X, point.Y] = 0;
+
+			pathFinder = new AStar.PathFinderFast(mapCollisionMap);
+			pathFinder.Formula = AStar.HeuristicFormula.Manhattan;
+			pathFinder.Diagonals = false;
+			pathFinder.HeavyDiagonals = false;
+			pathFinder.HeuristicEstimate = 2;
+			pathFinder.PunishChangeDirection = false;
+			pathFinder.TieBreaker = false;
+			pathFinder.SearchLimit = 50000;
+			pathFinder.DebugProgress = false;
+			pathFinder.DebugFoundPath = false;
+
 			var _start = new System.Drawing.Point(start.X, start.Y);
 			var _goal = new System.Drawing.Point(goal.X, goal.Y);
 			//path = Core.PathFinder.FindPath(_start, _goal);
@@ -95,7 +105,14 @@ namespace DepthsBelow.Component
 			if (path == null)
 				Debug.WriteLine(this.Parent.ToString() + ": Path not found!");
 
+			this.CollisionMap = new byte[100, 100];
+
 			return path;
 		}
+
+		public void RecreatePath(List<Point> appendCollisionMap)
+		{
+			FindPath(this.Parent.GetComponent<Transform>().Grid.Position, this.Goal, appendCollisionMap);
+		}
 	}
 }

DepthsBelow/DepthsBelow/Entity.cs

@@ -29,7 +29,7 @@ namespace DepthsBelow
 			}
 		}
 
-		private Core core;
+		protected Core core;
 
 		public Entity(Core core)
 		{

DepthsBelow/DepthsBelow/Map.cs

@@ -72,7 +72,7 @@ namespace DepthsBelow
 				{
 					if (mapObject.Type == "SquadStart")
 					{
-						var soldier = new Soldier(core);
+						var soldier = new Soldier(core, ref core.Squad);
 						// HACK: For some reason, the tile object coordinates are offset by one tile on the Y-axis in the Tiled map file (https://github.com/bjorn/tiled/issues/91)
 						var mapObjectPos = new Vector2(mapObject.Bounds.X, mapObject.Bounds.Y - Grid.TileSize);
 						var mapObjectGridPos = Grid.WorldToGrid(mapObjectPos);

DepthsBelow/DepthsBelow/MouseInput.cs

@@ -104,8 +104,8 @@ public MouseInput(Core core)
             }
             if (checkingDirection == true && ms.RightButton == ButtonState.Pressed)
             {
-                directionNow.X = ms.X;
+                directionNow.X = mouseWorldPos.X;
-                directionNow.Y = ms.Y;
+				directionNow.Y = mouseWorldPos.Y;
             }
             else
             {

DepthsBelow/DepthsBelow/Soldier.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using DepthsBelow.Component;
 using Microsoft.Xna.Framework;
 using Microsoft.Xna.Framework.Graphics;
@@ -13,13 +14,17 @@ namespace DepthsBelow
 		public static Point Origin;
 		private bool _selected;
 
+		public List<Soldier> Squad;
+
 		private PathFinder.Node nextNode;
 
-		public Soldier(Core core) : base(core)
+		public Soldier(Core core, ref List<Soldier> squad) : base(core)
 		{
 			if (Texture == null)
 				LoadContent(core);
 
+			this.Squad = squad;
+
 			Transform.World.Origin = new Vector2(16, 16);
 
 			this.Color = Color.White;
@@ -52,14 +57,36 @@ namespace DepthsBelow
 		{
 			base.Update(gameTime);
 
+			float elapsed = gameTime.ElapsedGameTime.Milliseconds / 1000.0f;
+
 			if (nextNode == null)
 				nextNode = GetComponent<PathFinder>().Next();
 
+			if (nextNode != null)
+			{
+				List<Point> soldierCollisions = new List<Point>();
+
+				foreach (var soldier in Squad)
+				{
+					if (soldier != this)
+					{
+						if (soldier.Transform.Grid == nextNode.Position)
+						{
+							soldierCollisions.Add(soldier.Transform.Grid);
+							GetComponent<PathFinder>().RecreatePath(soldierCollisions);
+							nextNode = GetComponent<PathFinder>().Next();
+							break;
+						}
+					}
+				}
+
+				
+
 				if (nextNode != null)
 				{
 					var nodeWorldPos = Grid.GridToWorld(nextNode.Position);
 					// HACK: Make this work properly with other speeds...
-				int speed = 2*4;
+					float speed = 4f;
 					if (Transform.World.X < nodeWorldPos.X)
 						Transform.World.X += speed;
 					if (Transform.World.X > nodeWorldPos.X)
@@ -71,6 +98,7 @@ namespace DepthsBelow
 
 					if (Transform.World == nodeWorldPos)
 						nextNode = GetComponent<PathFinder>().Next();
+				}
 					
 			}