diff --git a/src/engine/math/graph.ts b/src/engine/math/graph.ts
index 8240dfe1..49148e72 100644
--- a/src/engine/math/graph.ts
+++ b/src/engine/math/graph.ts
@@ -6,6 +6,22 @@ import type { Vector } from './vector';
  */
 export type G_UUID = string & { readonly __brand: unique symbol };
 
+/**
+ * Interface to define data with a custom id
+ */
+export interface DataWithId {
+  id: G_UUID;
+}
+
+/**
+ * Return true if obj implement DataWithId interface
+ * @param obj value to test
+ * @returns true if obj implement DataWithId
+ */
+const isDataWithId = (obj: unknown): obj is DataWithId => {
+  return (obj as DataWithId)?.id !== undefined && typeof (obj as DataWithId).id === 'string';
+};
+
 interface EdgeOptionsWithWeight {
   weight: number;
   useEuclidean?: false;
@@ -84,9 +100,7 @@ export class Graph<T> {
    */
   addNodes(nodes: T[]): Map<G_UUID, Node<T>> {
     for (const node of nodes) {
-      const thisNewNode = new Node(node);
-      this._nodes.set(thisNewNode.id, thisNewNode);
-      this.adjacencyList.set(thisNewNode.id, new Set());
+      this.addNode(node);
     }
     return this._nodes;
   }
@@ -236,6 +250,17 @@ export class Graph<T> {
     return this.adjacencyList.get(node1.id)?.has(node2.id) ?? false;
   }
 
+  /**
+   * Reset the graph
+   *
+   * Clear all nodes and edges from the graph.
+   */
+  reset(): void {
+    this._nodes.clear();
+    this._edges.clear();
+    this.adjacencyList.clear();
+  }
+
   /**
    * Performs a breadth-first search (BFS) on the graph starting from the given node.
    *
@@ -331,11 +356,11 @@ export class Graph<T> {
     const unvisited: Node<T>[] = [];
     const resultArray: Array<{ node: Node<T>; distance: number; previous: Node<T> | null }> = [];
 
-    //fill unvisited
-    this.nodes.forEach((node) => unvisited.push(node));
-
-    //fill resultArray
-    this.nodes.forEach((node) => resultArray.push({ node, distance: Infinity, previous: null }));
+    //fill unvisited and resultArray
+    this.nodes.forEach((node) => {
+      unvisited.push(node);
+      resultArray.push({ node, distance: Infinity, previous: null });
+    });
 
     //start with starting node
     //add startingnode to result array
@@ -401,8 +426,9 @@ export class Graph<T> {
         }
       }
 
+      // add test for -1 index representing unreachable node
       if (lowestDistanceIndex === -1) {
-        return [];
+        break;
       }
 
       current = resultArray[lowestDistanceIndex].node;
@@ -461,7 +487,9 @@ export class Graph<T> {
     const path: Node<T>[] = [];
     let current: Node<T> | null | undefined = endNode;
     const distance = dAnalysis.find((node) => node.node === endNode)?.distance as number;
-
+    if (distance === Infinity) {
+      return { path: [], distance: Infinity };
+    }
     while (current != null) {
       path.push(current);
 
@@ -679,11 +707,12 @@ export class Edge<T> {
  * @template T The type of data stored in this node.
  */
 export class Node<T> {
-  private _id: G_UUID = GraphUUId.generateUUID();
+  private _id: G_UUID;
   private _data: T;
   private _edges: Set<Edge<T>>;
 
   constructor(data: T) {
+    this._id = isDataWithId(data) ? data.id : GraphUUId.generateUUID();
     this._data = data;
     this._edges = new Set();
   }
diff --git a/src/spec/vitest/graph-spec.ts b/src/spec/vitest/graph-spec.ts
index c95715f3..5fefbdc8 100644
--- a/src/spec/vitest/graph-spec.ts
+++ b/src/spec/vitest/graph-spec.ts
@@ -233,12 +233,13 @@ describe('A Graph', () => {
       //  |     |
       // 2|     |1
       //  |     |
-      //  C --- D
+      //  C --- D     E (unreachable node)
       //     8
       const nodeA = graph.addNode('A');
       const nodeB = graph.addNode('B');
       const nodeC = graph.addNode('C');
       const nodeD = graph.addNode('D');
+      const nodeE = graph.addNode('E');
 
       graph.addEdge(nodeA, nodeB, { weight: 5 });
       graph.addEdge(nodeA, nodeC, { weight: 2 });
@@ -247,7 +248,7 @@ describe('A Graph', () => {
 
       const result = graph.dijkstra(nodeA);
 
-      expect(result.length).toBe(4);
+      expect(result.length).toBe(5);
       expect(result[0].node).toBe(nodeA);
       expect(result[0].distance).toBe(0);
       expect(result[1].node).toBe(nodeB);
@@ -256,6 +257,8 @@ describe('A Graph', () => {
       expect(result[2].distance).toBe(2);
       expect(result[3].node).toBe(nodeD);
       expect(result[3].distance).toBe(6);
+      expect(result[4].node).toBe(nodeE);
+      expect(result[4].distance).toBe(Infinity);
     });
 
     it('should find shortest path between two nodes', () => {
@@ -284,14 +287,18 @@ describe('A Graph', () => {
       expect(result.distance).toBe(9);
     });
 
-    it('should return empty path when no path exists', () => {
+    it('should return graph with Infinity distance', () => {
       const nodeA = graph.addNode('A');
       const nodeB = graph.addNode('B');
 
       // No edge connecting A and B
       const result = graph.dijkstra(nodeA);
 
-      expect(result.length).toBe(0);
+      expect(result.length).toBe(2);
+      expect(result[0].node).toBe(nodeA);
+      expect(result[0].distance).toBe(0);
+      expect(result[1].node).toBe(nodeB);
+      expect(result[1].distance).toBe(Infinity);
     });
 
     it('should handle zero-distance path (same node)', () => {