feat: Implement Node class for D* Lite pathfinding algorithm with traversal cost calculation
This commit is contained in:
61
server/pathfinding/Node.js
Normal file
61
server/pathfinding/Node.js
Normal file
@@ -0,0 +1,61 @@
|
|||||||
|
/**
|
||||||
|
* Node - Represents a pathfinding node in the D* Lite algorithm
|
||||||
|
* Each node wraps a Point and stores pathfinding metadata
|
||||||
|
*/
|
||||||
|
import { Point } from './Point.js';
|
||||||
|
|
||||||
|
export class Node {
|
||||||
|
constructor(point) {
|
||||||
|
this.point = point;
|
||||||
|
this.key = point.toKey();
|
||||||
|
|
||||||
|
// D* Lite values
|
||||||
|
this.g = Infinity; // Cost from start to this node
|
||||||
|
this.rhs = Infinity; // One-step lookahead cost
|
||||||
|
|
||||||
|
// Whether this node is blocked (wall, unbreakable block, etc.)
|
||||||
|
this.blocked = false;
|
||||||
|
|
||||||
|
// Whether this node contains a mineable block (costs more to traverse but possible)
|
||||||
|
this.mineable = false;
|
||||||
|
|
||||||
|
// The block data at this position (if known)
|
||||||
|
this.blockData = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate the D* Lite key pair for priority queue ordering
|
||||||
|
* @param {Point} start - The current start position
|
||||||
|
* @param {number} km - The key modifier (updated on robot movement)
|
||||||
|
*/
|
||||||
|
calculateKey(start, km = 0) {
|
||||||
|
const minVal = Math.min(this.g, this.rhs);
|
||||||
|
return [
|
||||||
|
minVal + start.euclideanDistanceTo(this.point) + km,
|
||||||
|
minVal
|
||||||
|
];
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Check if this node is consistent (g === rhs)
|
||||||
|
*/
|
||||||
|
isConsistent() {
|
||||||
|
return this.g === this.rhs;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Get the traversal cost to move to this node
|
||||||
|
* - Blocked nodes: Infinity
|
||||||
|
* - Mineable blocks: 2 (higher cost to prefer open paths)
|
||||||
|
* - Open space: 1
|
||||||
|
*/
|
||||||
|
getTraversalCost() {
|
||||||
|
if (this.blocked) return Infinity;
|
||||||
|
if (this.mineable) return 2;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
toString() {
|
||||||
|
return `Node(${this.point}, g=${this.g}, rhs=${this.rhs}, blocked=${this.blocked})`;
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user