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

Assets/Scenes/Pathfinding.unity

@@ -154,16 +154,16 @@ BoxCollider2D:
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
   m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
+    border: {x: 0, y: 0, z: 0, w: 0}
     pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
+    oldSize: {x: 1, y: 1}
     newSize: {x: 0.16, y: 0.16}
     adaptiveTilingThreshold: 0.5
     drawMode: 0
     adaptiveTiling: 0
   m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
+  m_Size: {x: 1, y: 1}
   m_EdgeRadius: 0
 --- !u!212 &38041424
 SpriteRenderer:
@@ -204,7 +204,7 @@ SpriteRenderer:
   m_SortingLayerID: 0
   m_SortingLayer: 0
   m_SortingOrder: 0
-  m_Sprite: {fileID: 10911, guid: 0000000000000000f000000000000000, type: 0}
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
   m_Color: {r: 1, g: 1, b: 1, a: 1}
   m_FlipX: 0
   m_FlipY: 0
@@ -223,8 +223,8 @@ Transform:
   m_PrefabAsset: {fileID: 0}
   m_GameObject: {fileID: 38041422}
   m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
-  m_LocalPosition: {x: -4.76, y: -4.97, z: 0}
-  m_LocalScale: {x: 10, y: 5, z: 0}
+  m_LocalPosition: {x: -7.54, y: -4.11, z: 0}
+  m_LocalScale: {x: 1, y: 5, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
   m_RootOrder: 5
@@ -261,10 +261,13 @@ MonoBehaviour:
   bounds:
     m_Center: {x: 0, y: 0, z: 0}
     m_Extent: {x: 10, y: 10, z: 0}
-  pointInterval: 0.3
+  pointInterval: 0.768
+  agentRadius: 0.1
   blockMask:
     serializedVersion: 2
     m_Bits: 256
+  isOptimizing: 1
+  complementLevel: 10
   enableGizmos: 1
 --- !u!4 &178212235
 Transform:
@@ -313,16 +316,16 @@ BoxCollider2D:
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
   m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
+    border: {x: 0, y: 0, z: 0, w: 0}
     pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
+    oldSize: {x: 1, y: 1}
     newSize: {x: 0.16, y: 0.16}
     adaptiveTilingThreshold: 0.5
     drawMode: 0
     adaptiveTiling: 0
   m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
+  m_Size: {x: 1, y: 1}
   m_EdgeRadius: 0
 --- !u!212 &614196140
 SpriteRenderer:
@@ -363,7 +366,7 @@ SpriteRenderer:
   m_SortingLayerID: 0
   m_SortingLayer: 0
   m_SortingOrder: 0
-  m_Sprite: {fileID: 10911, guid: 0000000000000000f000000000000000, type: 0}
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
   m_Color: {r: 1, g: 1, b: 1, a: 1}
   m_FlipX: 0
   m_FlipY: 0
@@ -383,7 +386,7 @@ Transform:
   m_GameObject: {fileID: 614196138}
   m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
   m_LocalPosition: {x: 3.74, y: -1.72, z: 0}
-  m_LocalScale: {x: 50.72, y: 10, z: 16.9931}
+  m_LocalScale: {x: 1, y: 10, z: 16.9931}
   m_Children: []
   m_Father: {fileID: 0}
   m_RootOrder: 3
@@ -398,15 +401,16 @@ GameObject:
   m_Component:
   - component: {fileID: 620200137}
   - component: {fileID: 620200136}
-  - component: {fileID: 620200135}
   - component: {fileID: 620200134}
+  - component: {fileID: 620200138}
+  - component: {fileID: 620200135}
   m_Layer: 0
   m_Name: Agent (8)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &620200134
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -422,10 +426,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &620200135
-BoxCollider2D:
+--- !u!58 &620200135
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -438,18 +442,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &620200136
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -512,8 +506,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 9
+  m_RootOrder: 10
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &620200138
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 620200133}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &852157120
 GameObject:
   m_ObjectHideFlags: 0
@@ -524,15 +539,16 @@ GameObject:
   m_Component:
   - component: {fileID: 852157124}
   - component: {fileID: 852157123}
-  - component: {fileID: 852157122}
   - component: {fileID: 852157121}
+  - component: {fileID: 852157125}
+  - component: {fileID: 852157122}
   m_Layer: 0
   m_Name: Agent (2)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &852157121
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -548,10 +564,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &852157122
-BoxCollider2D:
+--- !u!58 &852157122
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -564,18 +580,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &852157123
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -638,8 +644,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 15
+  m_RootOrder: 16
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &852157125
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 852157120}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1002756477
 GameObject:
   m_ObjectHideFlags: 0
@@ -673,16 +700,16 @@ BoxCollider2D:
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
   m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
+    border: {x: 0, y: 0, z: 0, w: 0}
     pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
+    oldSize: {x: 1, y: 1}
     newSize: {x: 0.16, y: 0.16}
     adaptiveTilingThreshold: 0.5
     drawMode: 0
     adaptiveTiling: 0
   m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
+  m_Size: {x: 1, y: 1}
   m_EdgeRadius: 0
 --- !u!212 &1002756479
 SpriteRenderer:
@@ -723,7 +750,7 @@ SpriteRenderer:
   m_SortingLayerID: 0
   m_SortingLayer: 0
   m_SortingOrder: 0
-  m_Sprite: {fileID: 10911, guid: 0000000000000000f000000000000000, type: 0}
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
   m_Color: {r: 1, g: 1, b: 1, a: 1}
   m_FlipX: 0
   m_FlipY: 0
@@ -743,7 +770,7 @@ Transform:
   m_GameObject: {fileID: 1002756477}
   m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
   m_LocalPosition: {x: -0.032, y: 4.9, z: 0}
-  m_LocalScale: {x: 50.72, y: 10, z: 16.9931}
+  m_LocalScale: {x: 5, y: 1, z: 1}
   m_Children: []
   m_Father: {fileID: 0}
   m_RootOrder: 2
@@ -781,16 +808,16 @@ BoxCollider2D:
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
   m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
+    border: {x: 0, y: 0, z: 0, w: 0}
     pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
+    oldSize: {x: 1, y: 1}
     newSize: {x: 0.16, y: 0.16}
     adaptiveTilingThreshold: 0.5
     drawMode: 0
     adaptiveTiling: 0
   m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
+  m_Size: {x: 1, y: 1}
   m_EdgeRadius: 0
 --- !u!212 &1036674797
 SpriteRenderer:
@@ -831,7 +858,7 @@ SpriteRenderer:
   m_SortingLayerID: 0
   m_SortingLayer: 0
   m_SortingOrder: 0
-  m_Sprite: {fileID: 10911, guid: 0000000000000000f000000000000000, type: 0}
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
   m_Color: {r: 1, g: 1, b: 1, a: 1}
   m_FlipX: 0
   m_FlipY: 0
@@ -851,7 +878,7 @@ Transform:
   m_GameObject: {fileID: 1036674795}
   m_LocalRotation: {x: 0, y: 0, z: 0.2588191, w: 0.9659258}
   m_LocalPosition: {x: -5.48, y: -0.04, z: 0}
-  m_LocalScale: {x: 50.72, y: 10, z: 16.9931}
+  m_LocalScale: {x: 1, y: 10, z: 16.9931}
   m_Children: []
   m_Father: {fileID: 0}
   m_RootOrder: 4
@@ -866,15 +893,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1119483108}
   - component: {fileID: 1119483107}
-  - component: {fileID: 1119483106}
   - component: {fileID: 1119483105}
+  - component: {fileID: 1119483109}
+  - component: {fileID: 1119483106}
   m_Layer: 0
   m_Name: Agent (7)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1119483105
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -890,10 +918,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1119483106
-BoxCollider2D:
+--- !u!58 &1119483106
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -906,18 +934,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1119483107
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -980,8 +998,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 10
+  m_RootOrder: 11
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1119483109
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1119483104}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1304602414
 GameObject:
   m_ObjectHideFlags: 0
@@ -992,15 +1031,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1304602418}
   - component: {fileID: 1304602417}
-  - component: {fileID: 1304602416}
   - component: {fileID: 1304602415}
+  - component: {fileID: 1304602419}
+  - component: {fileID: 1304602416}
   m_Layer: 0
   m_Name: Agent (9)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1304602415
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1016,10 +1056,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1304602416
-BoxCollider2D:
+--- !u!58 &1304602416
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1032,18 +1072,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1304602417
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1106,8 +1136,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 8
+  m_RootOrder: 9
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1304602419
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1304602414}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1324657432
 GameObject:
   m_ObjectHideFlags: 0
@@ -1141,16 +1192,16 @@ BoxCollider2D:
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
   m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
+    border: {x: 0, y: 0, z: 0, w: 0}
     pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
+    oldSize: {x: 1, y: 1}
     newSize: {x: 0.16, y: 0.16}
     adaptiveTilingThreshold: 0.5
     drawMode: 0
     adaptiveTiling: 0
   m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
+  m_Size: {x: 1, y: 1}
   m_EdgeRadius: 0
 --- !u!212 &1324657434
 SpriteRenderer:
@@ -1191,7 +1242,7 @@ SpriteRenderer:
   m_SortingLayerID: 0
   m_SortingLayer: 0
   m_SortingOrder: 0
-  m_Sprite: {fileID: 10911, guid: 0000000000000000f000000000000000, type: 0}
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
   m_Color: {r: 1, g: 1, b: 1, a: 1}
   m_FlipX: 0
   m_FlipY: 0
@@ -1211,10 +1262,10 @@ Transform:
   m_GameObject: {fileID: 1324657432}
   m_LocalRotation: {x: 0, y: 0, z: 0.7071068, w: 0.7071068}
   m_LocalPosition: {x: 3.33, y: -6.5, z: 0}
-  m_LocalScale: {x: 50, y: 5, z: 0}
+  m_LocalScale: {x: 1, y: 5, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 6
+  m_RootOrder: 7
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 90}
 --- !u!1 &1353820625
 GameObject:
@@ -1226,15 +1277,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1353820629}
   - component: {fileID: 1353820628}
-  - component: {fileID: 1353820627}
   - component: {fileID: 1353820626}
+  - component: {fileID: 1353820630}
+  - component: {fileID: 1353820627}
   m_Layer: 0
   m_Name: Agent (6)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1353820626
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1250,10 +1302,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1353820627
-BoxCollider2D:
+--- !u!58 &1353820627
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1266,18 +1318,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1353820628
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1340,8 +1382,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 11
+  m_RootOrder: 12
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1353820630
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1353820625}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1546514102
 GameObject:
   m_ObjectHideFlags: 0
@@ -1352,15 +1415,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1546514106}
   - component: {fileID: 1546514105}
-  - component: {fileID: 1546514104}
   - component: {fileID: 1546514103}
+  - component: {fileID: 1546514107}
+  - component: {fileID: 1546514104}
   m_Layer: 0
   m_Name: Agent (1)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1546514103
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1376,10 +1440,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1546514104
-BoxCollider2D:
+--- !u!58 &1546514104
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1392,18 +1456,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1546514105
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1466,8 +1520,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 16
+  m_RootOrder: 17
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1546514107
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1546514102}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1592952600
 GameObject:
   m_ObjectHideFlags: 0
@@ -1478,8 +1553,9 @@ GameObject:
   m_Component:
   - component: {fileID: 1592952604}
   - component: {fileID: 1592952603}
-  - component: {fileID: 1592952602}
   - component: {fileID: 1592952601}
+  - component: {fileID: 1592952605}
+  - component: {fileID: 1592952602}
   m_Layer: 0
   m_Name: Agent
   m_TagString: Untagged
@@ -1502,10 +1578,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1592952602
-BoxCollider2D:
+--- !u!58 &1592952602
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1518,18 +1594,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1592952603
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1592,8 +1658,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 7
+  m_RootOrder: 8
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1592952605
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1592952600}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1627406709
 GameObject:
   m_ObjectHideFlags: 0
@@ -1647,7 +1734,7 @@ Camera:
   far clip plane: 1000
   field of view: 60
   orthographic: 1
-  orthographic size: 5
+  orthographic size: 10.444298
   m_Depth: -1
   m_CullingMask:
     serializedVersion: 2
@@ -1687,15 +1774,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1651584787}
   - component: {fileID: 1651584786}
-  - component: {fileID: 1651584785}
   - component: {fileID: 1651584784}
+  - component: {fileID: 1651584788}
+  - component: {fileID: 1651584785}
   m_Layer: 0
   m_Name: Agent (5)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1651584784
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1711,10 +1799,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1651584785
-BoxCollider2D:
+--- !u!58 &1651584785
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1727,18 +1815,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1651584786
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1801,8 +1879,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 12
+  m_RootOrder: 13
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1651584788
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1651584783}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &1669406767
 GameObject:
   m_ObjectHideFlags: 0
@@ -1813,15 +1912,16 @@ GameObject:
   m_Component:
   - component: {fileID: 1669406771}
   - component: {fileID: 1669406770}
-  - component: {fileID: 1669406769}
   - component: {fileID: 1669406768}
+  - component: {fileID: 1669406772}
+  - component: {fileID: 1669406769}
   m_Layer: 0
   m_Name: Agent (3)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &1669406768
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1837,10 +1937,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &1669406769
-BoxCollider2D:
+--- !u!58 &1669406769
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1853,18 +1953,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &1669406770
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -1927,8 +2017,29 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 14
+  m_RootOrder: 15
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &1669406772
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1669406767}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
 --- !u!1 &2003659733
 GameObject:
   m_ObjectHideFlags: 0
@@ -1939,15 +2050,16 @@ GameObject:
   m_Component:
   - component: {fileID: 2003659737}
   - component: {fileID: 2003659736}
-  - component: {fileID: 2003659735}
   - component: {fileID: 2003659734}
+  - component: {fileID: 2003659738}
+  - component: {fileID: 2003659735}
   m_Layer: 0
   m_Name: Agent (4)
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!114 &2003659734
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1963,10 +2075,10 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
-  moveSpeed: 0
+  moveSpeed: 3
   isGizmos: 1
---- !u!61 &2003659735
-BoxCollider2D:
+--- !u!58 &2003659735
+CircleCollider2D:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
@@ -1979,18 +2091,8 @@ BoxCollider2D:
   m_UsedByEffector: 0
   m_UsedByComposite: 0
   m_Offset: {x: 0, y: 0}
-  m_SpriteTilingProperty:
-    border: {x: 0.049999997, y: 0.049999997, z: 0.049999997, w: 0.049999997}
-    pivot: {x: 0.5, y: 0.5}
-    oldSize: {x: 0.16, y: 0.16}
-    newSize: {x: 0.16, y: 0.16}
-    adaptiveTilingThreshold: 0.5
-    drawMode: 0
-    adaptiveTiling: 0
-  m_AutoTiling: 0
   serializedVersion: 2
-  m_Size: {x: 0.16, y: 0.16}
-  m_EdgeRadius: 0
+  m_Radius: 0.05
 --- !u!212 &2003659736
 SpriteRenderer:
   m_ObjectHideFlags: 0
@@ -2053,5 +2155,134 @@ Transform:
   m_LocalScale: {x: 2, y: 2, z: 0}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 13
+  m_RootOrder: 14
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!50 &2003659738
+Rigidbody2D:
+  serializedVersion: 4
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 2003659733}
+  m_BodyType: 0
+  m_Simulated: 1
+  m_UseFullKinematicContacts: 0
+  m_UseAutoMass: 0
+  m_Mass: 1
+  m_LinearDrag: 10
+  m_AngularDrag: 0.05
+  m_GravityScale: 0
+  m_Material: {fileID: 0}
+  m_Interpolate: 0
+  m_SleepingMode: 1
+  m_CollisionDetection: 0
+  m_Constraints: 4
+--- !u!1 &2143134981
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 2143134984}
+  - component: {fileID: 2143134983}
+  - component: {fileID: 2143134982}
+  m_Layer: 8
+  m_Name: New Sprite (5)
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!61 &2143134982
+BoxCollider2D:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 2143134981}
+  m_Enabled: 1
+  m_Density: 1
+  m_Material: {fileID: 0}
+  m_IsTrigger: 0
+  m_UsedByEffector: 0
+  m_UsedByComposite: 0
+  m_Offset: {x: 0, y: 0}
+  m_SpriteTilingProperty:
+    border: {x: 0, y: 0, z: 0, w: 0}
+    pivot: {x: 0.5, y: 0.5}
+    oldSize: {x: 1, y: 1}
+    newSize: {x: 0.16, y: 0.16}
+    adaptiveTilingThreshold: 0.5
+    drawMode: 0
+    adaptiveTiling: 0
+  m_AutoTiling: 0
+  serializedVersion: 2
+  m_Size: {x: 1, y: 1}
+  m_EdgeRadius: 0
+--- !u!212 &2143134983
+SpriteRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 2143134981}
+  m_Enabled: 1
+  m_CastShadows: 0
+  m_ReceiveShadows: 0
+  m_DynamicOccludee: 1
+  m_MotionVectors: 1
+  m_LightProbeUsage: 1
+  m_ReflectionProbeUsage: 1
+  m_RayTracingMode: 0
+  m_RenderingLayerMask: 1
+  m_RendererPriority: 0
+  m_Materials:
+  - {fileID: 10754, guid: 0000000000000000f000000000000000, type: 0}
+  m_StaticBatchInfo:
+    firstSubMesh: 0
+    subMeshCount: 0
+  m_StaticBatchRoot: {fileID: 0}
+  m_ProbeAnchor: {fileID: 0}
+  m_LightProbeVolumeOverride: {fileID: 0}
+  m_ScaleInLightmap: 1
+  m_ReceiveGI: 1
+  m_PreserveUVs: 0
+  m_IgnoreNormalsForChartDetection: 0
+  m_ImportantGI: 0
+  m_StitchLightmapSeams: 1
+  m_SelectedEditorRenderState: 0
+  m_MinimumChartSize: 4
+  m_AutoUVMaxDistance: 0.5
+  m_AutoUVMaxAngle: 89
+  m_LightmapParameters: {fileID: 0}
+  m_SortingLayerID: 0
+  m_SortingLayer: 0
+  m_SortingOrder: 0
+  m_Sprite: {fileID: 21300000, guid: 738cbf3d8a61f0944bb883e4e1283ab1, type: 3}
+  m_Color: {r: 1, g: 1, b: 1, a: 1}
+  m_FlipX: 0
+  m_FlipY: 0
+  m_DrawMode: 0
+  m_Size: {x: 0.16, y: 0.16}
+  m_AdaptiveModeThreshold: 0.5
+  m_SpriteTileMode: 0
+  m_WasSpriteAssigned: 1
+  m_MaskInteraction: 0
+  m_SpriteSortPoint: 0
+--- !u!4 &2143134984
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 2143134981}
+  m_LocalRotation: {x: -0, y: -0, z: -0.50265735, w: 0.86448574}
+  m_LocalPosition: {x: -3.93, y: -7.72, z: 0}
+  m_LocalScale: {x: 1, y: 5, z: 0}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 6
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: -60.352}

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>();
@@ -192,3 +193,4 @@ public class CharacterResourceEditorWindow : EditorWindow
 		newCharacterImageList = new Sprite[Enum.GetNames(typeof(CharacterExpression)).Length];
 	}
 }
+#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)
@@ -81,9 +92,12 @@ class Heap<T>
 				target = left;
 			}
 			if (right < list.Count && comparison.Invoke(list[right], list[index]) > 0)
+			{
+				if (comparison.Invoke(list[left], list[right]) < 0)
 				{
 					target = right;
 				}
+			}
 			if (index != target)
 			{
 				T tmp = list[index];
@@ -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/NodalPathfinding2D.cs

@@ -2,6 +2,7 @@
 using System.Collections.Generic;
 using UnityEngine;
 using UnityEditor;
+using System;
 
 namespace BS
 {
@@ -10,34 +11,63 @@ namespace BS
         public Bounds bounds;
         [Range(0.1f, 1f)]
         public float pointInterval = 0.5f;
-
+		public float agentRadius = 0.5f;
         
         public LayerMask blockMask;
         private Dictionary<Vector2Int, Node> nodes = new Dictionary<Vector2Int, Node>();
 
         public bool isBaked { get { return nodes.Count > 0; } }
 
+		public bool isOptimizing = true;
+		[Range(2,10)]
+		public int complementLevel = 2;
+
 		public bool enableGizmos = true;
 
+#if UNITY_EDITOR
 
 		private void OnDrawGizmos()
         {
             if (!enableGizmos)
                 return;
+
+
+			Gizmos.color = Color.red;
+			Vector3 mousePosition = Event.current.mousePosition;
+			Ray ray = HandleUtility.GUIPointToWorldRay(mousePosition);
+			mousePosition = ray.origin;
+			mousePosition.z = 0;
+			//Vector2Int index = FindNode(mousePosition, Vector2.up);
+			Vector2Int index = Vector2Int.zero;
+
 			if (isBaked)
             {
                 int xCount = Mathf.FloorToInt((bounds.max.x - bounds.min.x) / pointInterval) + 1;
                 int yCount = Mathf.FloorToInt((bounds.max.y - bounds.min.y) / pointInterval) + 1;
-                Gizmos.color = new Color(0, 1, 0, .3f);
                 foreach (var node in nodes.Values)
                 {
 					//Gizmos.color = new Color(node.gridPosition.x / (float)xCount, node.gridPosition.y / (float)yCount, 0, 1);
+					if (index == node.gridPosition)
+					{
+						Gizmos.color = Color.yellow;
 						Gizmos.DrawSphere(node.worldPositon, pointInterval / 5);
 						foreach (var adj in node.adjacencies)
 						{
 							Gizmos.DrawLine(node.worldPositon, nodes[adj].worldPositon);
 						}
 					}
+					else
+					{
+						Gizmos.color = Color.blue;
+						Gizmos.DrawSphere(node.worldPositon, pointInterval / 5);
+						Gizmos.color = new Color(0, 1, 0, .1f);
+						foreach (var adj in node.adjacencies)
+						{
+							Gizmos.DrawLine(node.worldPositon, nodes[adj].worldPositon);
+						}
+					}
+
+                }
             }
             else
             {
@@ -47,7 +77,7 @@ namespace BS
                 {
                     for (float j = bounds.min.y; j <= bounds.max.y; j += pointInterval * 2)
                     {
-                        if (Physics2D.OverlapPoint(new Vector2(i, j), blockMask))
+                        if (Physics2D.OverlapCircle(new Vector2(i, j), agentRadius, blockMask))
                             Gizmos.color = Color.red;
                         else
                             Gizmos.color = Color.green;
@@ -59,7 +89,7 @@ namespace BS
                 {
                     for (float j = bounds.min.y + pointInterval; j <= bounds.max.y; j += pointInterval * 2)
                     {
-                        if (Physics2D.OverlapPoint(new Vector2(i, j), blockMask))
+                        if (Physics2D.OverlapCircle(new Vector2(i, j), agentRadius, blockMask))
 							Gizmos.color = Color.red;
                         else
                             Gizmos.color = Color.green;
@@ -67,6 +97,20 @@ namespace BS
                     }
                 }
             }
+			Gizmos.color = Color.red;
+			Gizmos.DrawSphere(IndexToWorld(WorldToIndex(mousePosition, Vector2.right)), pointInterval / 5);
+
+			//Gizmos.color = Color.white;
+			//Gizmos.DrawSphere(nodes[FindNode(agent.transform.position, mousePosition - agent.transform.position)].worldPositon, pointInterval);
+			//Gizmos.DrawSphere(nodes[FindNode(mousePosition, agent.transform.position - mousePosition)].worldPositon, pointInterval);
+
+		}
+
+#endif
+		private void Start()
+		{
+			if (!isBaked)
+				BakeNodes();
 		}
 
 		public void BakeNodes()
@@ -76,6 +120,8 @@ namespace BS
             int xCount = Mathf.FloorToInt((bounds.max.x - bounds.min.x) / pointInterval) + 1;
             int yCount = Mathf.FloorToInt((bounds.max.y - bounds.min.y) / pointInterval) + 1;
 
+			complementLevel = complementLevel - complementLevel % 2;
+
             for (int i = 0; i < xCount; i++)
             {
                 for (int j = 0; j < yCount; j++)
@@ -83,14 +129,189 @@ namespace BS
                     if ((i + j) % 2 != 0)
                         continue;
                     Vector3 worldPosition = IndexToWorld(new Vector2Int(i, j));
-                    if (Physics2D.OverlapPoint(worldPosition, blockMask))
+                    if (Physics2D.OverlapCircle(worldPosition, agentRadius, blockMask))
                         continue;
                     Node newNode = new Node(new Vector2Int(i, j), worldPosition);
                     nodes.Add(newNode.gridPosition, newNode);
                 }
                 
+            }
+			if (isOptimizing)
+			{
+				List<Vector2Int> deleteList = new List<Vector2Int>();
+				Dictionary<Vector2Int, Node> allNodes = new Dictionary<Vector2Int, Node>(nodes);
+
+				for (int i = 0; i < xCount; i++)
+				{
+					for (int j = 0; j < yCount; j++)
+					{
+						Vector2Int pos = new Vector2Int(i, j);
+						if (!nodes.ContainsKey(pos))
+							continue;
+						int count = 0;
+						if (nodes.ContainsKey(pos + new Vector2Int(2, 0))) //Right
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(-2, 0))) //Left
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(0, 2))) // Up
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(0, -2))) // Down
+						{
+							count++;
+						}
+
+						if (nodes.ContainsKey(pos + new Vector2Int(-1, 1))) //UpLeft
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(1, 1))) //UpRight
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(-1, -1))) //DownLeft
+						{
+							count++;
+						}
+						if (nodes.ContainsKey(pos + new Vector2Int(1, -1))) //DownRight
+						{
+							count++;
+						}
+
+						if (count == 8)
+						{
+							deleteList.Add(pos);
+						}
+					}
 				}
 
+				foreach(var delete in deleteList)
+				{
+					nodes.Remove(delete);
+				}
+
+				for (int i = complementLevel; i < xCount; i+= complementLevel)
+				{
+					for (int j = complementLevel; j < yCount; j += complementLevel)
+					{
+						Vector2Int pos = new Vector2Int(i, j);
+						if (nodes.ContainsKey(pos) || Physics2D.OverlapCircle(IndexToWorld(pos),agentRadius, blockMask))
+							continue;
+						Node node = new Node(pos, IndexToWorld(pos));
+						nodes.Add(node.gridPosition, node);
+					}
+				}
+
+
+				for (int i = 0; i < xCount; i++)
+				{
+					for (int j = 0; j < yCount; j++)
+					{
+						List<Vector2Int> adj = new List<Vector2Int>();
+						Vector2Int pos = new Vector2Int(i, j);
+						if (!nodes.ContainsKey(pos))
+							continue;
+						Node node = nodes[pos];
+						for (int k = 1; (node.gridPosition + new Vector2Int(2,0) * k).x < xCount; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(2, 0) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(2,0) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(2, 0) * k);
+								break;
+							}
+						}
+						for (int k = 1; (node.gridPosition + new Vector2Int(-2, 0) * k).x >= 0; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(-2, 0) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(-2, 0) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(-2, 0) * k);
+								break;
+							}
+						}
+						for (int k = 1; (node.gridPosition + new Vector2Int(0, 2) * k).y < yCount; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(0, 2) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(0, 2) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(0, 2) * k);
+								break;
+							}
+						}
+						for (int k = 1; (node.gridPosition + new Vector2Int(0, -2) * k).y >= 0; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(0, -2) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(0, -2) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(0, -2) * k);
+								break;
+							}
+						}
+
+						for (int k = 1;
+							(node.gridPosition + new Vector2Int(-1, 1) * k).x >= 0 &&
+							(node.gridPosition + new Vector2Int(-1, 1) * k).y < yCount; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(-1, 1) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(-1, 1) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(-1, 1) * k);
+								break;
+							}
+						}
+						for (int k = 1;
+							(node.gridPosition + new Vector2Int(1, 1) * k).x < xCount &&
+							(node.gridPosition + new Vector2Int(1, 1) * k).y < yCount; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(1, 1) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(1, 1) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(1, 1) * k);
+								break;
+							}
+						}
+						for (int k = 1;
+							(node.gridPosition + new Vector2Int(-1, -1) * k).x >= 0 &&
+							(node.gridPosition + new Vector2Int(-1, -1) * k).y >= 0; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(-1, -1) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(-1, -1) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(-1, -1) * k);
+								break;
+							}
+						}
+						for (int k = 1;
+							(node.gridPosition + new Vector2Int(1, -1) * k).x < xCount &&
+							(node.gridPosition + new Vector2Int(1, -1) * k).y >= 0; k++)
+						{
+							if (!allNodes.ContainsKey(node.gridPosition + new Vector2Int(1, -1) * k))
+								break;
+							if (nodes.ContainsKey(node.gridPosition + new Vector2Int(1, -1) * k))
+							{
+								adj.Add(node.gridPosition + new Vector2Int(1, -1) * k);
+								break;
+							}
+						}
+						node.adjacencies = adj;
+					}
+				}
+			}
+			else
+			{
 				for (int i = 0; i < xCount; i++)
 				{
 					for (int j = 0; j < yCount; j++)
@@ -137,6 +358,7 @@ namespace BS
 						nodes[pos] = node;
 					}
 				}
+			}
             Debug.Log("Baking End");
         }
 
@@ -154,7 +376,6 @@ namespace BS
             }
 
             Node cur =  nodes[WorldToIndex(from, to - from)];
-            Node goal = nodes[WorldToIndex(to, from - to)];
             path.Add(cur.worldPositon);
 
             Queue<Node> off = new Queue<Node>();
@@ -187,13 +408,18 @@ namespace BS
         public List<Vector3> GetPathAstar(Vector3 from, Vector3 to)
         {
             List<Vector3> path = new List<Vector3>();
-            if (WorldToIndex(from, to - from).x < 0 || WorldToIndex(to, from - to).x < 0)
+
+			Vector2Int startIndex = FindNode(from, to - from);
+			Vector2Int goalIndex = FindNode(to, from - to);
+
+			if (startIndex.x <0 || goalIndex.x < 0)
 			{
 				return path;
 			}
-            Node start = nodes[WorldToIndex(from, to - from)];
-            Node goal = nodes[WorldToIndex(to, from - to)];
-			//path.Add(cur.worldPositon);
+
+			Node start = nodes[startIndex];
+			Node goal = nodes[goalIndex];
+
 
 			PriorityQueue<Node> queue = new PriorityQueue<Node>((a, b) => a.score < b.score ? 1 : a.score == b.score ? 0 : -1);
             List<Node> closed = new List<Node>();
@@ -202,13 +428,18 @@ namespace BS
             while (queue.Count > 0)
             {
                 Node cur = queue.Dequeue();
-
+				/*
+				Vector2Int newGoalIndex = FindNode(to, cur.worldPositon - to);
+				if (newGoalIndex.x > 0)
+					goal = nodes[FindNode(to, cur.worldPositon - to)];
+				*/
                 if(cur.gridPosition == goal.gridPosition)
                 {
                     closed.Add(cur);
                     break;
                 }
 				
+
                 foreach (var adj in cur.adjacencies)
                 {
                     Node adjNode = nodes[adj];
@@ -222,6 +453,7 @@ namespace BS
                             adjNode.cost = cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon);
                             adjNode.CalculateScore(to);
                             adjNode.parent = cur;
+							queue.ValueUpdated(adjNode);
                         }
                     }
                     else if (!isClosed)
@@ -233,6 +465,7 @@ namespace BS
                     }
 
                 }
+
                 closed.Add(cur);
             }
 
@@ -250,13 +483,23 @@ namespace BS
                 }
             }
             path.Reverse();
+			if (path.Count > 1 && Physics2D.OverlapCircle(to, agentRadius, blockMask))
+			{
+				path.Add(Physics2D.Raycast(path[path.Count - 1], to - path[path.Count - 1], float.MaxValue, blockMask).point);
 
+			}
+			else
+			{
+				path.Add(to);
+			}
+			/*
 			foreach(var node in nodes.Values)
 			{
 				node.cost = 0;
 				node.parent = null;
 				node.score = 0;
 			}
+			*/
 			return path;
         }
 
@@ -277,42 +520,101 @@ namespace BS
             int minY = Mathf.FloorToInt((position.y - bounds.min.y) / pointInterval);
             int maxY = Mathf.CeilToInt((position.y - bounds.min.y) / pointInterval);
 
-            Vector2Int center;
-            if ((minX + minY) % 2 != 0)
+			Vector2Int[] adjs = { new Vector2Int(minX, minY), new Vector2Int(minX, maxY), new Vector2Int(maxX, minY), new Vector2Int(maxX, maxY) };
+            Vector2Int index = Vector2Int.zero;
+            float max = float.MinValue;
+
+            foreach (var adj in adjs)
             {
-                center =
-                    Vector2.Distance(IndexToWorld(minX, minY), position) > Vector2.Distance(IndexToWorld(maxX, maxY), position) ?
-                    new Vector2Int(maxX, maxY) :
-                    new Vector2Int(minX, minY);
-            }
-            else
+                float score = Vector2.Dot((IndexToWorld(adj) - position).normalized, direction.normalized) / (IndexToWorld(adj) - position).magnitude;
+                if (max < score)
                 {
-                center =
-                    Vector2.Distance(IndexToWorld(minX, maxY), position) > Vector2.Distance(IndexToWorld(maxX, minY), position) ?
-                    new Vector2Int(maxX, minY) :
-                    new Vector2Int(minX, maxY);
+                    index = adj;
+                    max = score;
+                }
+            }
+            return index;
         }
 
-            Vector2Int[] adjs = { center + Vector2Int.up, center + Vector2Int.down, center + Vector2Int.left, center + Vector2Int.right };
-
-            Vector2Int index = new Vector2Int(-1, -1);
+		private Vector2Int FindNode(Vector3 position, Vector2 direction)
+		{
+			if (isOptimizing)
+			{
+				Vector2Int index = WorldToIndex(position, direction);
+				if (nodes.ContainsKey(index))
+					return index;
+				Vector2Int target = index;
 				float max = float.MinValue;
 	
+				for (int i = index.x - index.x % complementLevel; i <= index.x + complementLevel - index.x % complementLevel; i++)
+				{
+					for (int j = index.y - index.y % complementLevel; j <= index.y + complementLevel - index.y % complementLevel; j++)
+					{
+						
+						Vector2Int key = new Vector2Int(i, j);
+						if (nodes.ContainsKey(key))
+						{
+							float score = Vector2.Dot(IndexToWorld(key) - position, direction.normalized) / (IndexToWorld(key) - position).magnitude;
+							if (max < score)
+							{
+								target = key;
+								max = score;
+							}
+						}
+					}
+				}
+				return target;
+			}
+			else
+			{
+				Vector2Int index = WorldToIndex(position, direction);
+				Vector2Int init = index;
+				if (nodes.ContainsKey(index))
+				{
+					return index;
+				}
+				/*
+				for (int i = 1; index == WorldToIndex(position, direction) ; i++)
+				{
+					float max = float.MinValue;
+					for (int j = -i; j <= i; j++)
+					{
+						for (int k = -i; k <= i; k++)
+						{
+							Vector2Int tmp = new Vector2Int(j, k);
+							if (nodes.ContainsKey(tmp))
+							{
+								float score = Vector2.Dot((IndexToWorld(tmp) - position).normalized, direction.normalized) / (IndexToWorld(tmp) - position).magnitude;
+								if (max < score)
+								{
+									max = score;
+									index = tmp;
+								}
+							}
+						}
+					}
+				}
+				*/
+				Vector2Int[] adjs = { index + Vector2Int.up, index + Vector2Int.down, index + Vector2Int.right, index + Vector2Int.left };
+				float max = float.MinValue;
 				foreach (var adj in adjs)
 				{
 					if (!nodes.ContainsKey(adj))
 						continue;
-                float score = Vector2.Dot(nodes[adj].worldPositon - position, direction.normalized);
+					float score = Vector2.Dot((IndexToWorld(adj) - position).normalized, direction.normalized) / (IndexToWorld(adj) - position).magnitude;
 					if (max < score)
 					{
 						index = adj;
 						max = score;
 					}
 				}
-            if (index.x < 0)
-                Debug.Log("Blocked!!");
+				if (index == init)
+				{
+					return new Vector2Int(-1, -1);
+				}
 				return index;
 			}
+		}
 
         public void ClearNodes()
         {
@@ -347,6 +649,7 @@ namespace BS
         public float cost;
         public float score;
         public Node parent;
+
         public void CalculateScore(Vector3 destination)
         {
             score = cost + GetHeuristic(destination);

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,8 +32,13 @@ 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 Start()
+		{
+			rb = GetComponent<Rigidbody2D>();
 		}
 
 		private void Update()
@@ -42,15 +51,22 @@ namespace BS {
                 t = 0.5f;
             }
 			*/
-
-
+			/*
+			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);
 		}
@@ -59,12 +75,41 @@ namespace BS {
         {
             this.destination = 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