diff --git a/README.md b/README.md
index d47175c..ac38cec 100644
--- a/README.md
+++ b/README.md
@@ -150,7 +150,33 @@ this.aStarInstance = new AStarFinder({
     width: 8,
     height: 8
   },
-  heuristicFunction: "Manhattan"
+  heuristic: "Manhattan"
+});
+```
+
+Set a maxCost for each grid node to enable using cost in the pathfinding calculation allowing you to make nodes less preferable.
+
+> 0 = walkable
+> 2 = walkable but not prefered
+> 5 = not walkable
+
+``` ts
+let myMatrix = [
+  [0, 0, 2, 2, 2, 2, 0, 0],
+  [0, 0, 2, 2, 2, 2, 0, 5],
+  [0, 0, 5, 5, 0, 5, 5, 0],
+  [0, 0, 5, 0, 0, 0, 5, 0],
+  [0, 0, 0, 0, 0, 0, 5, 0],
+  [5, 5, 5, 0, 5, 0, 5, 0],
+  [0, 0, 0, 0, 5, 0, 5, 0],
+  [0, 0, 5, 0, 0, 0, 0, 0]
+];
+
+this.aStarInstance = new AStarFinder({
+  grid: {
+    matrix: myMatrix,
+    maxCost: 5,
+  }
 });
 ```
 
diff --git a/lib/core/grid.ts b/lib/core/grid.ts
index 29ca50e..586d9c2 100644
--- a/lib/core/grid.ts
+++ b/lib/core/grid.ts
@@ -6,6 +6,7 @@ export class Grid {
   readonly width: number;
   readonly height: number;
   readonly numberOfFields: number;
+  readonly maxCost: number;
 
   // The node grid
   private gridNodes: Node[][];
@@ -22,6 +23,8 @@ export class Grid {
       this.numberOfFields = this.width * this.height;
     }
 
+    this.maxCost = aParams.maxCost || 0;
+
     // Create and generate the matrix
     this.gridNodes = this.buildGridWithNodes(
       aParams.matrix || undefined,
@@ -85,10 +88,15 @@ export class Grid {
      */
     for (let y = 0; y < height; y++) {
       for (let x = 0; x < width; x++) {
-        if (matrix[y][x]) {
-          newGrid[y][x].setIsWalkable(false);
+        if (this.maxCost) {
+          newGrid[y][x].setIsWalkable(matrix[y][x] < this.maxCost);
+          newGrid[y][x].setCost(matrix[y][x]);
         } else {
-          newGrid[y][x].setIsWalkable(true);
+          if (matrix[y][x]) {
+            newGrid[y][x].setIsWalkable(false);
+          } else {
+            newGrid[y][x].setIsWalkable(true);
+          }
         }
       }
     }
@@ -198,7 +206,8 @@ export class Grid {
         cloneGrid[y][x] = new Node({
           id: id,
           position: { x: x, y: y },
-          walkable: this.gridNodes[y][x].getIsWalkable()
+          walkable: this.gridNodes[y][x].getIsWalkable(),
+          cost: this.gridNodes[y][x].getCost(),
         });
 
         id++;
diff --git a/lib/core/node.ts b/lib/core/node.ts
index af99b9f..cf57136 100644
--- a/lib/core/node.ts
+++ b/lib/core/node.ts
@@ -11,6 +11,7 @@ export class Node {
   private isOnClosedList: boolean;
   private isOnOpenList: boolean;
   private isWalkable: boolean;
+  private cost: number;
 
   constructor(aParams: INodeConstructor) {
     this.id = aParams.id;
@@ -23,6 +24,7 @@ export class Node {
     this.isOnClosedList = false;
     this.isOnOpenList = false;
     this.isWalkable = aParams.walkable || true;
+    this.cost = aParams.cost || 0;
   }
 
   /**
@@ -89,6 +91,10 @@ export class Node {
     return this.isWalkable;
   }
 
+  public getCost(): number {
+    return this.cost;
+  }
+
   /**
    * Setter functions
    */
@@ -107,4 +113,8 @@ export class Node {
   public setIsWalkable(isWalkable: boolean): void {
     this.isWalkable = isWalkable;
   }
+
+  public setCost(cost: number): void {
+    this.cost = cost;
+  }
 }
diff --git a/lib/finders/astar-finder.ts b/lib/finders/astar-finder.ts
index b758b64..da201dd 100644
--- a/lib/finders/astar-finder.ts
+++ b/lib/finders/astar-finder.ts
@@ -31,7 +31,8 @@ export class AStarFinder {
       width: aParams.grid.width,
       height: aParams.grid.height,
       matrix: aParams.grid.matrix || undefined,
-      densityOfObstacles: aParams.grid.densityOfObstacles || 0
+      densityOfObstacles: aParams.grid.densityOfObstacles || 0,
+      maxCost: aParams.grid.maxCost || 0,
     });
 
     // Init lists
@@ -146,8 +147,9 @@ export class AStarFinder {
         // Calculate the g value of the neightbor
         const nextGValue =
           currentNode.getGValue() +
+          neightbor.getCost() +
           (neightbor.position.x !== currentNode.position.x ||
-          neightbor.position.y! == currentNode.position.y
+            neightbor.position.y! == currentNode.position.y
             ? this.weight
             : this.weight * 1.41421);
 
diff --git a/lib/interfaces/astar.interfaces.ts b/lib/interfaces/astar.interfaces.ts
index 3aff437..99d6ccb 100644
--- a/lib/interfaces/astar.interfaces.ts
+++ b/lib/interfaces/astar.interfaces.ts
@@ -14,12 +14,14 @@ export interface IGridConstructor {
   height?: number;
   matrix?: number[][];
   densityOfObstacles?: number;
+  maxCost?: number;
 }
 
 export interface INodeConstructor {
   id: number;
   position: IPoint;
   walkable?: boolean;
+  cost?: number;
 }
 
 export interface IPoint {