Implementing Priority Queue with Heap. Agents follow along path. Still need to optimize.

Assets/Scripts/Editors/CharacterResourceEditorWindow.cs

@@ -6,6 +6,7 @@ using System;
 using System.Linq;
 using System.IO;
 
+#if UNITY_EDITOR
 public class CharacterResourceEditorWindow : EditorWindow
 {
 	public static Dictionary<int, CharacterResource> characters = new Dictionary<int, CharacterResource>();
@@ -191,4 +192,5 @@ public class CharacterResourceEditorWindow : EditorWindow
 		newCharacterName = "";
 		newCharacterImageList = new Sprite[Enum.GetNames(typeof(CharacterExpression)).Length];
 	}
-}
\ No newline at end of file
+}
+#endif
\ No newline at end of file

Assets/Scripts/Editors/DialogueSceneMakerWindow.cs

@@ -6,6 +6,7 @@ using System.IO;
 using System;
 using System.Linq;
 
+#if UNITY_EDITOR
 public class DialogueSceneMakerWindow : EditorWindow
 {
     [SerializeField]
@@ -321,3 +322,4 @@ public class DialogueSceneMakerWindow : EditorWindow
 
     }
 }
+#endif
\ No newline at end of file

Assets/Scripts/Editors/NodalPathfinding2DEditor.cs

@@ -4,6 +4,7 @@ using UnityEngine;
 using UnityEditor;
 using BS;
 
+#if UNITY_EDITOR
 [CustomEditor(typeof(NodalPathfinding2D))]
 public class NodalPathfinding2DEditor : Editor
 {
@@ -23,3 +24,4 @@ public class NodalPathfinding2DEditor : Editor
         }
     }
 }
+#endif
\ No newline at end of file

Assets/Scripts/Generals/PriorityQueue.cs

 using System.Collections;
 using System.Collections.Generic;
+using UnityEngine;
 
 public class PriorityQueue<T>
 {
@@ -26,6 +27,11 @@ public class PriorityQueue<T>
 	{
 		return heap.Contains(item);
 	}
+
+	public void ValueUpdated(T item)
+	{
+		heap.HeapUpdated(item);
+	}
 }
 
 class Heap<T>
@@ -48,14 +54,17 @@ class Heap<T>
 	public void Insert(T item)
 	{
 		list.Add(item);
+		TryWithParent(list.Count - 1);
+		/*
 		int index = list.Count - 1;
-		while (comparison.Invoke(list[index], list[index / 2]) > 0)
+		while (comparison.Invoke(list[index], list[Parent(index)]) > 0)
 		{
 			T tmp = list[index];
-			list[index] = list[index / 2];
-			list[index / 2] = tmp;
-			index /= 2;
+			list[index] = list[Parent(index)];
+			list[Parent(index)] = tmp;
+			index = Parent(index);
 		}
+		*/
 	}
 
 	public T Pop()
@@ -69,11 +78,13 @@ class Heap<T>
 
 	private void Heapify()
 	{
+		TryWithChildren(0);
+		/*
 		int index = 0;
 		while (true)
 		{
-			int left = 2 * index + 1;
-			int right = 2 * index + 2;
+			int left = Left(index);
+			int right = Right(index);
 
 			int target = index;
 			if (left < list.Count && comparison.Invoke(list[left], list[index]) > 0)
@@ -82,7 +93,10 @@ class Heap<T>
 			}
 			if (right < list.Count && comparison.Invoke(list[right], list[index]) > 0)
 			{
-				target = right;
+				if (comparison.Invoke(list[left], list[right]) < 0)
+				{
+					target = right;
+				}
 			}
 			if (index != target)
 			{
@@ -96,5 +110,113 @@ class Heap<T>
 				break;
 			}
 		}
+		*/
+	}
+
+	public bool isHeap()
+	{
+		for(int i = 0; i < list.Count / 2; i++)
+		{
+			int left = 2 * i + 1;
+			int right = 2 * i + 2;
+			if (comparison.Invoke(list[i], list[left]) < 0)
+			{
+				return false;
+			}
+			if (right < list.Count && comparison.Invoke(list[i], list[right]) < 0)
+			{
+				return false;
+			}
+		}
+		return true;
+	}
+
+	public void HeapUpdated(T item)
+	{
+		TryWithParent(list.IndexOf(item));
+		TryWithChildren(list.IndexOf(item));
+
+		/*
+		if (!list.Contains(item))
+			Debug.LogError("Invalid item");
+		int index = list.IndexOf(item);
+		int left = 2 * index + 1;
+		int right = 2 * index + 2;
+		if (comparison.Invoke(list[index], list[(index - 1) / 2]) > 0)
+		{
+			while(comparison.Invoke(list[index], list[(index -1) / 2]) > 0)
+			{
+				T tmp = list[index];
+				list[index] = list[(index - 1) / 2];
+				list[(index - 1) / 2] = tmp;
+				index /= 2;
+			}
+		}
+		else if (left < list.Count && comparison.Invoke(list[index], list[left]) < 0)
+		{
+			T tmp = list[index];
+			list[index] = list[left];
+			list[left] = tmp;
+		}
+		else if (right < list.Count && comparison.Invoke(list[index], list[right]) < 0)
+		{
+			T tmp = list[index];
+			list[index] = list[right];
+			list[right] = tmp;
+		}
+		*/
+	}
+
+	private int Left(int index)
+	{
+		return index * 2 + 1;
+	}
+
+	private int Right(int index)
+	{
+		return index * 2 + 2;
+	}
+
+	private int Parent(int index)
+	{
+		return (index - 1) / 2;
+	}
+
+	private void TryWithChildren(int index)
+	{
+		
+		if (Left(index) >= list.Count)
+		{
+			return;
+		}
+		int biggerChild;
+		if (Right(index) >= list.Count)
+		{
+			biggerChild = Left(index);
+		}
+		else
+		{
+			biggerChild = comparison.Invoke(list[Left(index)], list[Right(index)]) >= 0 ? Left(index) : Right(index);
+		}
+
+		if (comparison.Invoke(list[biggerChild], list[index]) > 0)
+		{
+			T tmp = list[index];
+			list[index] = list[biggerChild];
+			list[biggerChild] = tmp;
+			TryWithChildren(biggerChild);
+		}
+
+	}
+
+	private void TryWithParent(int index)
+	{
+		if(comparison.Invoke(list[index], list[Parent(index)]) > 0)
+		{
+			T tmp = list[index];
+			list[index] = list[Parent(index)];
+			list[Parent(index)] = tmp;
+			TryWithParent(Parent(index));
+		}
 	}
 }
\ No newline at end of file

Assets/Scripts/NodalPathfinding/NodalPathfinding2DAgent.cs

@@ -3,6 +3,8 @@ using System.Collections.Generic;
 using UnityEngine;
 
 namespace BS {
+	[RequireComponent(typeof(Rigidbody2D))]
+	[RequireComponent(typeof(CircleCollider2D))]
     public class NodalPathfinding2DAgent : MonoBehaviour
     {
         public NodalPathfinding2D pathFinder;
@@ -13,6 +15,8 @@ namespace BS {
 
         float t = 0.5f;
 
+		private Rigidbody2D rb;
+
         public bool isGizmos = true;
 
         private void OnDrawGizmos()
@@ -28,11 +32,16 @@ namespace BS {
                     Gizmos.DrawLine(path[i] + new Vector3(0,0,1), path[i + 1] + new Vector3(0, 0, 1));
                 }
                 Gizmos.color = Color.red;
-                Gizmos.DrawSphere(destination + new Vector3(0, 0, 1), 0.5f);
+                Gizmos.DrawSphere(destination + new Vector3(0, 0, 1), 0.2f);
             }
         }
 
-        private void Update()
+		private void Start()
+		{
+			rb = GetComponent<Rigidbody2D>();
+		}
+
+		private void Update()
         {
 			/*
             t -= Time.deltaTime;
@@ -42,29 +51,65 @@ namespace BS {
                 t = 0.5f;
             }
 			*/
-
-
-            if (Input.GetKeyDown(KeyCode.Space))
+			/*
+			if (pathFinder.isBaked)
+			{
+				MoveTo(new Vector2(Random.Range(pathFinder.bounds.min.x, pathFinder.bounds.max.x), Random.Range(pathFinder.bounds.min.y, pathFinder.bounds.max.y)));
+			}
+			*/
+			if (Input.GetKeyDown(KeyCode.Space))
             {
                 transform.position = new Vector2(Random.Range(pathFinder.bounds.min.x, pathFinder.bounds.max.x), Random.Range(pathFinder.bounds.min.y, pathFinder.bounds.max.y));
             }
             if (Input.GetMouseButtonDown(1))
             {
-                MoveTo(Camera.main.ScreenToWorldPoint(Input.mousePosition));
+				Vector3 destination = Camera.main.ScreenToWorldPoint(Input.mousePosition);
+				destination.z = 0;
+
+				MoveTo(destination);
             }
             //path = pathFinder.GetPathGreedy(transform.position, destination);
-        }
+		}
 
         public void MoveTo(Vector3 destination)
         {
             this.destination = destination;
-            //path = pathFinder.GetPathGreedy(transform.position, destination);
-            path = pathFinder.GetPathAstar(transform.position, destination);
-        }
+			//path = pathFinder.GetPathGreedy(transform.position, destination);
+			if (path.Count > 1 && Vector2.Distance(path[path.Count - 1], destination) < pathFinder.pointInterval)
+				path[path.Count - 1] = destination;
+			else
+			{
+				path = pathFinder.GetPathAstar(transform.position, destination);
+			}
+		}
 
 		public void Move(Vector2 direction)
 		{
 
 		}
+
+		private void FixedUpdate()
+		{
+			if (path.Count > 0)
+			{
+				transform.position += (path[0] - transform.position).normalized * moveSpeed * Time.fixedDeltaTime;
+				//rb.velocity = Vector3.Lerp(rb.velocity, (path[0] - transform.position).normalized * moveSpeed, 0.5f);
+				//Debug.Log((path[0] - transform.position).normalized.magnitude);
+				rb.velocity = (path[0] - transform.position).normalized * moveSpeed;
+
+				if (path.Count > 1 && Vector2.Distance(transform.position, path[0]) < 0.1f && Vector2.Dot(path[1] - path[0], path[0] - transform.position) < 0)
+				{
+					path.RemoveAt(0);
+				}
+				else if (path.Count > 1 && Vector2.Distance(transform.position, path[0]) < pathFinder.pointInterval / 2)
+				{
+					path.RemoveAt(0);
+				}
+				else if (Vector2.Distance(transform.position, destination) < 0.05f)
+				{
+					path.RemoveAt(0);
+				}
+			}
+		}
 	}
 }
\ No newline at end of file

Assets/white.png.meta

+fileFormatVersion: 2
+guid: 738cbf3d8a61f0944bb883e4e1283ab1
+TextureImporter:
+  internalIDToNameTable: []
+  externalObjects: {}
+  serializedVersion: 10
+  mipmaps:
+    mipMapMode: 0
+    enableMipMap: 0
+    sRGBTexture: 1
+    linearTexture: 0
+    fadeOut: 0
+    borderMipMap: 0
+    mipMapsPreserveCoverage: 0
+    alphaTestReferenceValue: 0.5
+    mipMapFadeDistanceStart: 1
+    mipMapFadeDistanceEnd: 3
+  bumpmap:
+    convertToNormalMap: 0
+    externalNormalMap: 0
+    heightScale: 0.25
+    normalMapFilter: 0
+  isReadable: 0
+  streamingMipmaps: 0
+  streamingMipmapsPriority: 0
+  grayScaleToAlpha: 0
+  generateCubemap: 6
+  cubemapConvolution: 0
+  seamlessCubemap: 0
+  textureFormat: 1
+  maxTextureSize: 2048
+  textureSettings:
+    serializedVersion: 2
+    filterMode: -1
+    aniso: -1
+    mipBias: -100
+    wrapU: 1
+    wrapV: 1
+    wrapW: 1
+  nPOTScale: 0
+  lightmap: 0
+  compressionQuality: 50
+  spriteMode: 1
+  spriteExtrude: 1
+  spriteMeshType: 1
+  alignment: 0
+  spritePivot: {x: 0.5, y: 0.5}
+  spritePixelsToUnits: 100
+  spriteBorder: {x: 0, y: 0, z: 0, w: 0}
+  spriteGenerateFallbackPhysicsShape: 1
+  alphaUsage: 1
+  alphaIsTransparency: 1
+  spriteTessellationDetail: -1
+  textureType: 8
+  textureShape: 1
+  singleChannelComponent: 0
+  maxTextureSizeSet: 0
+  compressionQualitySet: 0
+  textureFormatSet: 0
+  platformSettings:
+  - serializedVersion: 3
+    buildTarget: DefaultTexturePlatform
+    maxTextureSize: 2048
+    resizeAlgorithm: 0
+    textureFormat: -1
+    textureCompression: 1
+    compressionQuality: 50
+    crunchedCompression: 0
+    allowsAlphaSplitting: 0
+    overridden: 0
+    androidETC2FallbackOverride: 0
+    forceMaximumCompressionQuality_BC6H_BC7: 0
+  spriteSheet:
+    serializedVersion: 2
+    sprites: []
+    outline: []
+    physicsShape: []
+    bones: []
+    spriteID: 5e97eb03825dee720800000000000000
+    internalID: 0
+    vertices: []
+    indices: 
+    edges: []
+    weights: []
+    secondaryTextures: []
+  spritePackingTag: 
+  pSDRemoveMatte: 0
+  pSDShowRemoveMatteOption: 0
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

ProjectSettings/EditorBuildSettings.asset

@@ -5,7 +5,10 @@ EditorBuildSettings:
   m_ObjectHideFlags: 0
   serializedVersion: 2
   m_Scenes:
+  - enabled: 0
+    path: 
+    guid: 00000000000000000000000000000000
   - enabled: 1
-    path: Assets/Scenes/SampleScene.unity
-    guid: 2cda990e2423bbf4892e6590ba056729
+    path: Assets/Scenes/Pathfinding.unity
+    guid: 716f3ccf18fe3444cabbb5ea0ddc9b6b
   m_configObjects: {}

ProjectSettings/GraphicsSettings.asset

@@ -3,7 +3,7 @@
 --- !u!30 &1
 GraphicsSettings:
   m_ObjectHideFlags: 0
-  serializedVersion: 12
+  serializedVersion: 13
   m_Deferred:
     m_Mode: 1
     m_Shader: {fileID: 69, guid: 0000000000000000f000000000000000, type: 0}
@@ -31,6 +31,9 @@ GraphicsSettings:
   m_AlwaysIncludedShaders:
   - {fileID: 10753, guid: 0000000000000000f000000000000000, type: 0}
   - {fileID: 10770, guid: 0000000000000000f000000000000000, type: 0}
+  - {fileID: 16000, guid: 0000000000000000f000000000000000, type: 0}
+  - {fileID: 16001, guid: 0000000000000000f000000000000000, type: 0}
+  - {fileID: 17000, guid: 0000000000000000f000000000000000, type: 0}
   m_PreloadedShaders: []
   m_SpritesDefaultMaterial: {fileID: 10754, guid: 0000000000000000f000000000000000,
     type: 0}
@@ -55,3 +58,5 @@ GraphicsSettings:
   m_AlbedoSwatchInfos: []
   m_LightsUseLinearIntensity: 0
   m_LightsUseColorTemperature: 0
+  m_LogWhenShaderIsCompiled: 0
+  m_AllowEnlightenSupportForUpgradedProject: 1