diff --git a/Assets/Scripts/Behaviours/PathfindingManager.cs b/Assets/Scripts/Behaviours/PathfindingManager.cs
index 557607a7..3ef2d131 100644
--- a/Assets/Scripts/Behaviours/PathfindingManager.cs
+++ b/Assets/Scripts/Behaviours/PathfindingManager.cs
@@ -190,7 +190,7 @@ namespace SanAndreasUnity.Behaviours
                     if (idM.Equals(targetId))
                     {
                         var dataM = GetResettedData();
-                        dataM.g = dataN.g + link.Length;
+                        dataM.g = dataN.g + CalculateLinkWeight(idN, idM);
                         float h = CalculateHeuristic(idM, targetId);
                         dataM.f = dataM.g + h;
                         dataM.parentId = idN;
@@ -205,7 +205,7 @@ namespace SanAndreasUnity.Behaviours
 
                     if (openList.Contains(idM))
                     {
-                        float gNew = dataN.g + link.Length;
+                        float gNew = dataN.g + CalculateLinkWeight(idN, idM);
                         float hNew = CalculateHeuristic(idM, targetId);
                         float fNew = gNew + hNew;
 
@@ -230,7 +230,7 @@ namespace SanAndreasUnity.Behaviours
                     else
                     {
                         var dataM = GetResettedData();
-                        dataM.g = dataN.g + link.Length;
+                        dataM.g = dataN.g + CalculateLinkWeight(idN, idM);
                         float h = CalculateHeuristic(idM, targetId);
                         dataM.f = dataM.g + h;
                         dataM.parentId = idN;
@@ -266,6 +266,11 @@ namespace SanAndreasUnity.Behaviours
             return NodeReader.GetDistanceBetweenNodes(source, destination);
         }
 
+        private float CalculateLinkWeight(PathNodeId parentId, PathNodeId neighbourId)
+        {
+            return NodeReader.GetDistanceBetweenNodes(parentId, neighbourId);
+        }
+
         private void RestoreModifiedDatas()
         {
             foreach (var id in m_modifiedDatas)