diff --git a/algos/common/generalized-astar/FixedMemoryGeneralizedAstarAutorouter.ts b/algos/common/generalized-astar/FixedMemoryGeneralizedAstarAutorouter.ts new file mode 100644 index 0000000..736847b --- /dev/null +++ b/algos/common/generalized-astar/FixedMemoryGeneralizedAstarAutorouter.ts @@ -0,0 +1,20 @@ +import type { IGeneralizedAstarAutorouter } from "./IGeneralizedAstarAutorouter" +import type { Node, Point, PointWithObstacleHit } from "./types" + +/** + * This is a version of GeneralizedAstarAutorouter that has completely fixed + * memory. Conceptually it is a precursor to a C implementation. + */ +export class FixedMemoryGeneralizedAstar + implements IGeneralizedAstarAutorouter +{ + computeG(current: Node, neighbor: Point): number { + throw new Error("computeG needs override") + } + computeH(node: Point): number { + throw new Error("computeH needs override") + } + getNeighbors(node: Node): Array { + throw new Error("getNeighbors needs override") + } +} diff --git a/algos/common/generalized-astar/GENERALIZED_ASTAR_README.md b/algos/common/generalized-astar/GENERALIZED_ASTAR_README.md new file mode 100644 index 0000000..1b615bf --- /dev/null +++ b/algos/common/generalized-astar/GENERALIZED_ASTAR_README.md @@ -0,0 +1,97 @@ +# Generalized Astar README + +This is a short introduction to our GeneralizedAstar class. This is a class you +can extend to create lots of different algorithms. Let's review how A\* works +and why and how you might want to extend it. + +## Autorouting and A\*, what paths are we finding? + +We have a list of squares (pads) that we want to connect with wires (traces), +we know the location of the pads but we don't know what the traces should look +like. The GeneralizedAstar class has an opinionated way of finding all the traces, +it says: + +1. let's just go through each pair of connected pads +2. find the approximate shortest path between the pads, make that path a trace, +3. repeat until we've routed the entire board + +You can extend the GeneralizedAstar class to change how step 2 works. You'd +think this would be a pretty straightforward process and for many pathfinding +solvers it is, but we want to make it _crazy fast_ and that's where things get +tricky. + +## A brief introduction to A\* + +A\* is a really good pathfinding algorithm, here's how it works: + +- store a queue of points, at the beginning it's just the starting point +- go to the starting point and find all of it's neighbors, score each neighbor + with TWO costs + - the `g` cost is the distance it's taken to get the that neighbor so far + - the `h` cost which is a guess for how far that neighbor is from the goal +- insert each new neighbor into the queue with it's total cost `g + h` +- select the point with the lowest cost from the queue and repeat! + +This is a much faster than other approaches of exploring paths. We even employ +some optimizations like the "greedy multipler" to make A\* run even faster (but +sometimes return less-than-optimal results) + +## Customizing `GeneralizedAstar` + +There are a couple ways that you can customize `GeneralizedAstar` to try out +new algorithms: + +- Override `getNeighbors` to change how you find the neighbors of a point +- Override `computeH` to change how you guess at the distance to the goal +- Override `computeG` to change how you compute the distance of a point so far + +Here are examples of reasons you might change or override each of these: + +- Override `computeH` and `computeG` to add penalties for vias +- Override `getNeighbors` to calculate the next neighbors by finding line + intersections using the [intersection jumping technique](https://blog.autorouting.com/p/the-intersection-jump-autorouter) +- Override `getNeighbors` to consider "bus lanes" or diagonal movements + +## What you can't do with `GeneralizedAstar` + +- Customize which traces are selected to go first (for this, we recommend + a higher-level algorithm that _orchestrates_ a `GeneralizedAstar` autorouter) + +## Show me an example of extending `GeneralizedAstar` + +Sure! Here's a version of `GeneralizedAstar` that does a fairly standard +grid-based search (however, unlike many grid-searches, this one can operate +on an infinitely sized grid!) + +```tsx +export class InfiniteGridAutorouter extends GeneralizedAstarAutorouter { + getNeighbors(node: Node): Array { + const dirs = [ + { x: 0, y: this.GRID_STEP }, + { x: this.GRID_STEP, y: 0 }, + { x: 0, y: -this.GRID_STEP }, + { x: -this.GRID_STEP, y: 0 }, + ] + + return dirs + .filter( + (dir) => !this.obstacles!.isObstacleAt(node.x + dir.x, node.y + dir.y) + ) + .map((dir) => ({ + x: node.x + dir.x, + y: node.y + dir.y, + })) + } +} +``` + +## Glossary + +If you're trying to understand `GeneralizedAstar`, it can help to know these +terms: + +- `closedSet` - The set of explored points +- `openSet` - The set of unexplored points +- `GREEDY_MULTIPLIER` - By default set to `1.1`, this makes the algorithm find + suboptimal paths because it will act more greedily, it can dramatically + increase the speed of the algorithm diff --git a/algos/common/generalized-astar/GeneralizedAstarAutorouter.ts b/algos/common/generalized-astar/GeneralizedAstarAutorouter.ts new file mode 100644 index 0000000..4fcbf37 --- /dev/null +++ b/algos/common/generalized-astar/GeneralizedAstarAutorouter.ts @@ -0,0 +1,566 @@ +import type { AnySoupElement, LayerRef, PCBSMTPad } from "@tscircuit/soup" +// import { QuadtreeObstacleList } from "./QuadtreeObstacleList" +import type { Node, Point, PointWithObstacleHit } from "./types" +import { manDist, nodeName } from "./util" + +import Debug from "debug" +import type { + Obstacle, + SimpleRouteConnection, + SimpleRouteJson, + SimplifiedPcbTrace, +} from "solver-utils" +import { getObstaclesFromRoute } from "solver-utils/getObstaclesFromRoute" +import { ObstacleList } from "./ObstacleList" +import { removePathLoops } from "solver-postprocessing/remove-path-loops" +import { addViasWhenLayerChanges } from "solver-postprocessing/add-vias-when-layer-changes" +import type { AnyCircuitElement } from "circuit-json" +import { Profiler } from "./Profiler" + +const debug = Debug("autorouting-dataset:astar") + +export interface PointWithLayer extends Point { + layer: string +} + +export type ConnectionSolveResult = + | { solved: false; connectionName: string } + | { solved: true; connectionName: string; route: PointWithLayer[] } + +const globalGeneralizedAstarAutorouterProfiler = new Profiler() + +export class GeneralizedAstarAutorouter { + profiler?: Profiler + openSet: Node[] = [] + openSetMap: Map = new Map() + closedSet: Set = new Set() + debug = false + + debugSolutions?: Record + debugMessage: string | null = null + debugTraceCount: number = 0 + + input: SimpleRouteJson + obstacles?: ObstacleList + allObstacles: Obstacle[] + startNode?: Node + goalPoint?: Point & { l: number } + GRID_STEP: number + OBSTACLE_MARGIN: number + MAX_ITERATIONS: number + + // Set this to MAX_ITERATIONS for best quality- but at the cost of some speed + MAX_OPEN_SET_SIZE: number = 200 + isRemovePathLoopsEnabled: boolean + /** + * Setting this greater than 1 makes the algorithm find suboptimal paths and + * act more greedy, but at greatly improves performance. + * + * Recommended value is between 1.1 and 1.5 + */ + GREEDY_MULTIPLIER = 1.1 + + iterations: number = -1 + + constructor(opts: { + input: SimpleRouteJson + startNode?: Node + goalPoint?: Point + GRID_STEP?: number + OBSTACLE_MARGIN?: number + MAX_ITERATIONS?: number + isRemovePathLoopsEnabled?: boolean + debug?: boolean + }) { + if (process.env.TSCIRCUIT_AUTOROUTER_PROFILING_ENABLED) { + this.profiler = globalGeneralizedAstarAutorouterProfiler + } + this.input = opts.input + this.allObstacles = opts.input.obstacles + this.startNode = opts.startNode + this.goalPoint = opts.goalPoint + ? ({ l: 0, ...opts.goalPoint } as any) + : undefined + this.GRID_STEP = opts.GRID_STEP ?? 0.1 + this.OBSTACLE_MARGIN = opts.OBSTACLE_MARGIN ?? 0.15 + this.MAX_ITERATIONS = opts.MAX_ITERATIONS ?? 100 + this.debug = opts.debug ?? debug.enabled + this.isRemovePathLoopsEnabled = opts.isRemovePathLoopsEnabled ?? false + if (this.debug) { + debug.enabled = true + } + + if (debug.enabled) { + this.debugSolutions = {} + this.debugMessage = "" + } + + if (this.profiler) { + for (const methodName of [ + "getNeighbors", + "solveOneStep", + "createObstacleList", + "_sortOpenSet", + ]) { + // @ts-ignore + const originalMethod = this[methodName] as Function + // @ts-ignore + this[methodName as keyof ObstacleList] = this.profiler!.wrapMethod( + `GenerlizedAstarAutorouter.${methodName}`, + originalMethod.bind(this), + ) + } + } + } + + /** + * Return points of interest for this node. Don't worry about checking if + * points are already visited. You must check that these neighbors are valid + * (not inside an obstacle) + * + * In a simple grid, this is just the 4 neighbors surrounding the node. + * + * In ijump-astar, this is the 2-4 surrounding intersections + */ + getNeighbors(node: Node): Array { + return [] + } + + isSameNode(a: Point, b: Point): boolean { + return manDist(a, b) < this.GRID_STEP + } + + /** + * Compute the cost of this path. In normal astar, this is just the length of + * the path, but you can override this term to penalize paths that are more + * complex. + */ + computeG(current: Node, neighbor: Point): number { + return current.g + manDist(current, neighbor) + } + + computeH(node: Point): number { + return manDist(node, this.goalPoint!) + } + + getNodeName(node: Point): string { + return nodeName(node, this.GRID_STEP) + } + + _sortOpenSet() { + // Not needed because we do an insert sort + // this.openSet.sort((a, b) => a.f - b.f) + // Limit the size of the openset + if (this.openSet.length > this.MAX_OPEN_SET_SIZE) { + this.openSet.splice(this.MAX_OPEN_SET_SIZE) + } + } + + solveOneStep(): { + solved: boolean + current: Node + newNeighbors: Node[] + } { + this.iterations += 1 + const { openSet, closedSet, GRID_STEP, goalPoint } = this + this._sortOpenSet() + + let current = openSet.shift()! + let currentNodeName = this.getNodeName(current) + + while (closedSet.has(currentNodeName)) { + current = openSet.shift()! + currentNodeName = this.getNodeName(current) + } + + this.openSetMap.delete(currentNodeName) + + const goalDist = this.computeH(current) + if (goalDist <= GRID_STEP * 2) { + return { + solved: true, + current, + newNeighbors: [], + } + } + + this.closedSet.add(currentNodeName) + + let newNeighbors: Node[] = [] + for (const neighbor of this.getNeighbors(current)) { + const neighborName = this.getNodeName(neighbor) + if (closedSet.has(neighborName)) continue + + const tentativeG = this.computeG(current, neighbor) + + const existingNeighbor = this.openSetMap.get(neighborName) + + if (!existingNeighbor || tentativeG < existingNeighbor.g) { + this.profiler?.startMeasurement("createNeighborNode") + const h = this.computeH(neighbor) + + const f = tentativeG + h * this.GREEDY_MULTIPLIER + + const neighborNode: Node = { + ...neighbor, + g: tentativeG, + h, + f, + obstacleHit: neighbor.obstacleHit ?? undefined, + manDistFromParent: manDist(current, neighbor), // redundant compute... + nodesInPath: current.nodesInPath + 1, + parent: current, + enterMarginCost: neighbor.enterMarginCost, + travelMarginCostFactor: neighbor.travelMarginCostFactor, + } + this.profiler?.endMeasurement("createNeighborNode") + + // Insert into openSet in sorted order by f value + this.profiler?.startMeasurement("openSetInsert") + if (this.openSetMap.has(neighborName)) { + throw new Error("Open set already has neighbor node") + } + this._binarySearchOpenSetInsert(neighborNode) + this.profiler?.endMeasurement("openSetInsert") + + newNeighbors.push(neighborNode) + } + } + + if (debug.enabled) { + this.drawDebugSolution({ current, newNeighbors }) + } + + return { + solved: false, + current, + newNeighbors, + } + } + + _binarySearchOpenSetInsert(neighborNode: Node) { + let left = 0 + let right = this.openSet.length - 1 + + while (left <= right) { + const mid = Math.floor((left + right) / 2) + if (this.openSet[mid].f > neighborNode.f) { + right = mid - 1 + } else { + left = mid + 1 + } + } + + this.openSet.splice(left, 0, neighborNode) + } + + getStartNode(connection: SimpleRouteConnection): Node { + return { + x: connection.pointsToConnect[0].x, + y: connection.pointsToConnect[0].y, + manDistFromParent: 0, + f: 0, + g: 0, + h: 0, + nodesInPath: 0, + parent: null, + } + } + + layerToIndex(layer: string): number { + return 0 + } + indexToLayer(index: number): string { + return "top" + } + + /** + * Add a preprocessing step before solving a connection to do adjust points + * based on previous iterations. For example, if a previous connection solved + * for a trace on the same net, you may want to preprocess the connection to + * solve for an easier start and end point + * + * The simplest way to do this is to run getConnectionWithAlternativeGoalBoxes + * with any pcb_traces created by previous iterations + */ + preprocessConnectionBeforeSolving( + connection: SimpleRouteConnection, + ): SimpleRouteConnection { + return connection + } + + solveConnection(connection: SimpleRouteConnection): ConnectionSolveResult { + if (connection.pointsToConnect.length > 2) { + throw new Error( + "GeneralizedAstarAutorouter doesn't currently support 2+ points in a connection", + ) + } + connection = this.preprocessConnectionBeforeSolving(connection) + + const { pointsToConnect } = connection + + this.iterations = 0 + this.closedSet = new Set() + this.startNode = this.getStartNode(connection) + this.goalPoint = { + ...pointsToConnect[pointsToConnect.length - 1], + l: this.layerToIndex(pointsToConnect[pointsToConnect.length - 1].layer), + } + this.openSet = [this.startNode] + this.openSetMap.clear() + this.openSetMap.set(this.getNodeName(this.startNode), this.startNode) + + while (this.iterations < this.MAX_ITERATIONS) { + const { solved, current } = this.solveOneStep() + + if (solved) { + let route: PointWithLayer[] = [] + let node: Node | null = current + while (node) { + const l: number | undefined = (node as any).l + route.unshift({ + x: node.x, + y: node.y, + // TODO: this layer should be included as part of the node + layer: + l !== undefined ? this.indexToLayer(l) : pointsToConnect[0].layer, + }) + node = node.parent + } + + if (debug.enabled) { + this.debugMessage += `t${this.debugTraceCount}: ${this.iterations} iterations\n` + } + + if (this.isRemovePathLoopsEnabled) { + route = removePathLoops(route) + } + + return { solved: true, route, connectionName: connection.name } + } + + if (this.openSet.length === 0) { + break + } + } + + if (debug.enabled) { + this.debugMessage += `t${this.debugTraceCount}: ${this.iterations} iterations (failed)\n` + } + + return { solved: false, connectionName: connection.name } + } + + createObstacleList({ + dominantLayer, + connection, + obstaclesFromTraces, + }: { + dominantLayer?: string + connection: SimpleRouteConnection + obstaclesFromTraces: Obstacle[] + }): ObstacleList { + return new ObstacleList( + this.allObstacles + .filter((obstacle) => !obstacle.connectedTo.includes(connection.name)) + // TODO obstacles on different layers should be filtered inside + // the algorithm, not for the entire connection, this is a hack in + // relation to https://github.com/tscircuit/tscircuit/issues/432 + .filter((obstacle) => obstacle.layers.includes(dominantLayer as any)) + .concat(obstaclesFromTraces ?? []), + ) + } + + /** + * Override this to implement smoothing strategies or incorporate new traces + * into a connectivity map + */ + postprocessConnectionSolveResult( + connection: SimpleRouteConnection, + result: ConnectionSolveResult, + ): ConnectionSolveResult { + return result + } + + /** + * By default, this will solve the connections in the order they are given, + * and add obstacles for each successfully solved connection. Override this + * to implement "rip and replace" rerouting strategies. + */ + solve(): ConnectionSolveResult[] { + const solutions: ConnectionSolveResult[] = [] + const obstaclesFromTraces: Obstacle[] = [] + this.debugTraceCount = 0 + for (const connection of this.input.connections) { + const dominantLayer = connection.pointsToConnect[0].layer ?? "top" + this.debugTraceCount += 1 + this.obstacles = this.createObstacleList({ + dominantLayer, + connection, + obstaclesFromTraces, + }) + let result = this.solveConnection(connection) + result = this.postprocessConnectionSolveResult(connection, result) + solutions.push(result) + + if (debug.enabled) { + this.drawDebugTraceObstacles(obstaclesFromTraces) + } + + if (result.solved) { + obstaclesFromTraces.push( + ...getObstaclesFromRoute( + result.route.map((p) => ({ + x: p.x, + y: p.y, + layer: p.layer ?? dominantLayer, + })), + connection.name, + ), + ) + } + } + + return solutions + } + + solveAndMapToTraces(): SimplifiedPcbTrace[] { + const solutions = this.solve() + + return solutions.flatMap((solution): SimplifiedPcbTrace[] => { + if (!solution.solved) return [] + return [ + { + type: "pcb_trace" as const, + pcb_trace_id: `pcb_trace_for_${solution.connectionName}`, + route: addViasWhenLayerChanges( + solution.route.map((point) => ({ + route_type: "wire" as const, + x: point.x, + y: point.y, + width: this.input.minTraceWidth, + layer: point.layer as LayerRef, + })), + ), + }, + ] + }) + } + + getDebugGroup(): string | null { + const dgn = `t${this.debugTraceCount}_iter[${this.iterations - 1}]` + if (this.iterations < 30) return dgn + if (this.iterations < 100 && this.iterations % 10 === 0) return dgn + if (this.iterations < 1000 && this.iterations % 100 === 0) return dgn + if (!this.debugSolutions) return dgn + return null + } + + drawDebugTraceObstacles(obstacles: Obstacle[]) { + const { debugTraceCount, debugSolutions } = this + for (const key in debugSolutions) { + if (key.startsWith(`t${debugTraceCount}_`)) { + debugSolutions[key].push( + ...obstacles.map( + (obstacle, i) => + ({ + type: "pcb_smtpad", + pcb_component_id: "", + layer: obstacle.layers[0], + width: obstacle.width, + shape: "rect", + x: obstacle.center.x, + y: obstacle.center.y, + pcb_smtpad_id: `trace_obstacle_${i}`, + height: obstacle.height, + }) as PCBSMTPad, + ), + ) + } + } + } + + drawDebugSolution({ + current, + newNeighbors, + }: { + current: Node + newNeighbors: Node[] + }) { + const debugGroup = this.getDebugGroup() + if (!debugGroup) return + + const { openSet, debugTraceCount, debugSolutions } = this + + debugSolutions![debugGroup] ??= [] + const debugSolution = debugSolutions![debugGroup]! + + debugSolution.push({ + type: "pcb_fabrication_note_text", + pcb_fabrication_note_text_id: `debug_note_${current.x}_${current.y}`, + font: "tscircuit2024", + font_size: 0.25, + text: "X" + (current.l !== undefined ? current.l : ""), + pcb_component_id: "", + layer: "top", + anchor_position: { + x: current.x, + y: current.y, + }, + anchor_alignment: "center", + }) + // Add all the openSet as small diamonds + for (let i = 0; i < openSet.length; i++) { + const node = openSet[i] + debugSolution.push({ + type: "pcb_fabrication_note_path", + pcb_component_id: "", + pcb_fabrication_note_path_id: `note_path_${node.x}_${node.y}`, + layer: "top", + route: [ + [0, 0.05], + [0.05, 0], + [0, -0.05], + [-0.05, 0], + [0, 0.05], + ].map(([dx, dy]) => ({ + x: node.x + dx, + y: node.y + dy, + })), + stroke_width: 0.01, + }) + // Add text that indicates the order of this point + debugSolution.push({ + type: "pcb_fabrication_note_text", + pcb_fabrication_note_text_id: `debug_note_${node.x}_${node.y}`, + font: "tscircuit2024", + font_size: 0.03, + text: i.toString(), + pcb_component_id: "", + layer: "top", + anchor_position: { + x: node.x, + y: node.y, + }, + anchor_alignment: "center", + }) + } + + if (current.parent) { + const path: Node[] = [] + let p: Node | null = current + while (p) { + path.unshift(p) + p = p.parent + } + debugSolution!.push({ + type: "pcb_fabrication_note_path", + pcb_component_id: "", + pcb_fabrication_note_path_id: `note_path_${current.x}_${current.y}`, + layer: "top", + route: path, + stroke_width: 0.01, + }) + } + } +} diff --git a/algos/common/generalized-astar/IGeneralizedAstarAutorouter.ts b/algos/common/generalized-astar/IGeneralizedAstarAutorouter.ts new file mode 100644 index 0000000..6a76ff0 --- /dev/null +++ b/algos/common/generalized-astar/IGeneralizedAstarAutorouter.ts @@ -0,0 +1,7 @@ +import type { Node, Point, PointWithObstacleHit } from "./types" + +export interface IGeneralizedAstarAutorouter { + computeG(current: Node, neighbor: Point): number + computeH(node: Point): number + getNeighbors(node: Node): Array +} diff --git a/algos/common/generalized-astar/ObstacleList.ts b/algos/common/generalized-astar/ObstacleList.ts new file mode 100644 index 0000000..68c61d5 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList.ts @@ -0,0 +1,202 @@ +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import type { + Direction, + DirectionDistances, + DirectionWithCollisionInfo, + Point, +} from "./types" +import { Profiler } from "./Profiler" + +declare global { + namespace NodeJS { + interface ProcessEnv { + TSCIRCUIT_AUTOROUTER_PROFILING_ENABLED?: string + } + } +} + +const globalObstacleListProfiler = new Profiler() + +/** + * A list of obstacles with functions for fast lookups, this default implementation + * has no optimizations, you should override this class to implement faster lookups + */ +export class ObstacleList { + profiler?: Profiler + protected obstacles: ObstacleWithEdges[] + protected GRID_STEP = 0.1 + + constructor(obstacles: Array) { + if (process.env.TSCIRCUIT_AUTOROUTER_PROFILING_ENABLED) { + this.profiler = globalObstacleListProfiler + } + this.obstacles = obstacles.map((obstacle) => ({ + ...obstacle, + left: obstacle.center.x - obstacle.width / 2, + right: obstacle.center.x + obstacle.width / 2, + top: obstacle.center.y + obstacle.height / 2, + bottom: obstacle.center.y - obstacle.height / 2, + })) + + if (this.profiler) { + for (const methodName of [ + "getObstacleAt", + "isObstacleAt", + "getDirectionDistancesToNearestObstacle", + "getOrthoDirectionCollisionInfo", + "getObstaclesOverlappingRegion", + ]) { + const originalMethod = this[ + methodName as keyof ObstacleList + ] as Function + // @ts-ignore + this[methodName as keyof ObstacleList] = this.profiler!.wrapMethod( + `ObstacleList.${methodName}`, + originalMethod.bind(this), + ) + } + } + } + + getObstacleAt(x: number, y: number, m?: number): Obstacle | null { + m ??= this.GRID_STEP + for (const obstacle of this.obstacles) { + const halfWidth = obstacle.width / 2 + m + const halfHeight = obstacle.height / 2 + m + if ( + x >= obstacle.center.x - halfWidth && + x <= obstacle.center.x + halfWidth && + y >= obstacle.center.y - halfHeight && + y <= obstacle.center.y + halfHeight + ) { + return obstacle + } + } + return null + } + + isObstacleAt(x: number, y: number, m?: number): boolean { + return this.getObstacleAt(x, y, m) !== null + } + + getDirectionDistancesToNearestObstacle( + x: number, + y: number, + ): DirectionDistances { + const { GRID_STEP } = this + const result: DirectionDistances = { + left: Infinity, + top: Infinity, + bottom: Infinity, + right: Infinity, + } + + for (const obstacle of this.obstacles) { + if (obstacle.type === "rect") { + const left = obstacle.center.x - obstacle.width / 2 - GRID_STEP + const right = obstacle.center.x + obstacle.width / 2 + GRID_STEP + const top = obstacle.center.y + obstacle.height / 2 + GRID_STEP + const bottom = obstacle.center.y - obstacle.height / 2 - GRID_STEP + + // Check left + if (y >= bottom && y <= top && x > left) { + result.left = Math.min(result.left, x - right) + } + + // Check right + if (y >= bottom && y <= top && x < right) { + result.right = Math.min(result.right, left - x) + } + + // Check top + if (x >= left && x <= right && y < top) { + result.top = Math.min(result.top, bottom - y) + } + + // Check bottom + if (x >= left && x <= right && y > bottom) { + result.bottom = Math.min(result.bottom, y - top) + } + } + } + + return result + } + + getOrthoDirectionCollisionInfo( + point: Point, + dir: Direction, + { margin = 0 }: { margin?: number } = {}, + ): DirectionWithCollisionInfo { + const { x, y } = point + const { dx, dy } = dir + let minDistance = Infinity + let collisionObstacle: ObstacleWithEdges | null = null + + for (const obstacle of this.obstacles) { + const leftMargin = obstacle.left - margin + const rightMargin = obstacle.right + margin + const topMargin = obstacle.top + margin + const bottomMargin = obstacle.bottom - margin + + let distance: number | null = null + + if (dx === 1 && dy === 0) { + // Right + if (y > bottomMargin && y < topMargin && x < obstacle.left) { + distance = obstacle.left - x + } + } else if (dx === -1 && dy === 0) { + // Left + if (y > bottomMargin && y < topMargin && x > obstacle.right) { + distance = x - obstacle.right + } + } else if (dx === 0 && dy === 1) { + // Up + if (x > leftMargin && x < rightMargin && y < obstacle.bottom) { + distance = obstacle.bottom - y + } + } else if (dx === 0 && dy === -1) { + // Down + if (x > leftMargin && x < rightMargin && y > obstacle.top) { + distance = y - obstacle.top + } + } + + if (distance !== null && distance < minDistance) { + minDistance = distance + collisionObstacle = obstacle + } + } + + return { + dx, + dy, + wallDistance: minDistance, + obstacle: collisionObstacle as ObstacleWithEdges, + } + } + + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + }): ObstacleWithEdges[] { + const obstacles: ObstacleWithEdges[] = [] + for (const obstacle of this.obstacles) { + const { left, right, top, bottom } = obstacle + + if ( + left >= region.minX && + right <= region.maxX && + top >= region.minY && + bottom <= region.maxY + ) { + obstacles.push(obstacle) + } + } + + return obstacles + } +} diff --git a/algos/common/generalized-astar/ObstacleList3d.ts b/algos/common/generalized-astar/ObstacleList3d.ts new file mode 100644 index 0000000..71d8a4c --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3d.ts @@ -0,0 +1,227 @@ +// ObstacleList3d.ts + +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import type { + Direction3d, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { getLayerIndex, getLayerNamesForLayerCount } from "./util" +import { ObstacleList } from "./ObstacleList" + +/** + * A list of obstacles with functions for fast lookups, this default implementation + * has no optimizations, you should override this class to implement faster lookups + */ +export class ObstacleList3d extends ObstacleList { + obstacles: ObstacleWithEdges3d[] + GRID_STEP = 0.1 + layerCount: number + + constructor(layerCount: number, obstacles: Array) { + super([]) + this.layerCount = layerCount + const availableLayers = getLayerNamesForLayerCount(layerCount) + this.obstacles = obstacles.flatMap((obstacle) => + obstacle.layers + .filter((layer) => availableLayers.includes(layer)) + .map((layer) => ({ + ...obstacle, + left: obstacle.center.x - obstacle.width / 2, + right: obstacle.center.x + obstacle.width / 2, + top: obstacle.center.y + obstacle.height / 2, + bottom: obstacle.center.y - obstacle.height / 2, + l: getLayerIndex(layerCount, layer), + })), + ) + } + + getObstacleAt(x: number, y: number, l: number, m?: number): Obstacle | null { + m ??= this.GRID_STEP + for (const obstacle of this.obstacles) { + if (obstacle.l !== l) continue // Only consider obstacles on the same layer + const halfWidth = obstacle.width / 2 + m + const halfHeight = obstacle.height / 2 + m + if ( + x >= obstacle.center.x - halfWidth && + x <= obstacle.center.x + halfWidth && + y >= obstacle.center.y - halfHeight && + y <= obstacle.center.y + halfHeight + ) { + return obstacle + } + } + return null + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + return this.getObstacleAt(x, y, l, m) !== null + } + + getDirectionDistancesToNearestObstacle3d( + x: number, + y: number, + l: number, + ): DirectionDistances3d { + const { GRID_STEP } = this + const result: DirectionDistances3d = { + left: Infinity, + top: Infinity, + bottom: Infinity, + right: Infinity, + } + + for (const obstacle of this.obstacles) { + if (obstacle.l !== l) continue // Only consider obstacles on the same layer + if (obstacle.type === "rect") { + const left = obstacle.center.x - obstacle.width / 2 - GRID_STEP + const right = obstacle.center.x + obstacle.width / 2 + GRID_STEP + const top = obstacle.center.y + obstacle.height / 2 + GRID_STEP + const bottom = obstacle.center.y - obstacle.height / 2 - GRID_STEP + + // Check left + if (y >= bottom && y <= top && x > left) { + result.left = Math.min(result.left, x - right) + } + + // Check right + if (y >= bottom && y <= top && x < right) { + result.right = Math.min(result.right, left - x) + } + + // Check top + if (x >= left && x <= right && y < top) { + result.top = Math.min(result.top, bottom - y) + } + + // Check bottom + if (x >= left && x <= right && y > bottom) { + result.bottom = Math.min(result.bottom, y - top) + } + } + } + + return result + } + + // cases: any = {} + getOrthoDirectionCollisionInfo( + point: Point3d, + dir: Direction3d, + { margin = 0 }: { margin?: number } = {}, + ): DirectionWithCollisionInfo3d { + const { x, y, l } = point + const { dx, dy, dl } = dir + // const xHash = [l, x.toFixed(3), dy, dl].join(",") + // const yHash = [l, y.toFixed(3), dx, dl].join(",") + // if (dy !== 0) { + // this.cases[xHash] ??= [] + // this.cases[xHash].push(y) + // } + // if (dx !== 0) { + // this.cases[yHash] ??= [] + // this.cases[yHash].push(x) + // } + let minDistance = Infinity + let collisionObstacle: ObstacleWithEdges | null = null + + if (dl !== 0) { + // Moving between layers + const newLayer = l + dl + // Check if there's an obstacle at the same (x, y) on the new layer + if (this.isObstacleAt(x, y, newLayer, margin)) { + minDistance = 1 // Distance to obstacle is 1 (layer change) + collisionObstacle = this.getObstacleAt( + x, + y, + newLayer, + margin, + ) as ObstacleWithEdges + } else { + minDistance = 1 // Distance to move to the next layer + } + + return { + dx, + dy, + dl, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } else { + // Moving within the same layer + for (const obstacle of this.obstacles) { + if (obstacle.l !== l) continue // Only consider obstacles on the same layer + + const leftMargin = obstacle.left - margin + const rightMargin = obstacle.right + margin + const topMargin = obstacle.top + margin + const bottomMargin = obstacle.bottom - margin + + let distance: number | null = null + + if (dx === 1 && dy === 0) { + // Right + if (y > bottomMargin && y < topMargin && x < obstacle.left) { + distance = obstacle.left - x + } + } else if (dx === -1 && dy === 0) { + // Left + if (y > bottomMargin && y < topMargin && x > obstacle.right) { + distance = x - obstacle.right + } + } else if (dx === 0 && dy === 1) { + // Up + if (x > leftMargin && x < rightMargin && y < obstacle.bottom) { + distance = obstacle.bottom - y + } + } else if (dx === 0 && dy === -1) { + // Down + if (x > leftMargin && x < rightMargin && y > obstacle.top) { + distance = y - obstacle.top + } + } + + if (distance !== null && distance < minDistance) { + minDistance = distance + collisionObstacle = obstacle + } + } + + return { + dx, + dy, + dl: 0, + wallDistance: minDistance, + obstacle: collisionObstacle as ObstacleWithEdges, + } + } + } + + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + l: number // Layer to check + }): ObstacleWithEdges[] { + const obstacles: ObstacleWithEdges[] = [] + for (const obstacle of this.obstacles) { + if (obstacle.l !== region.l) continue // Only consider obstacles on the specified layer + const { left, right, top, bottom } = obstacle + + if ( + left <= region.maxX && + right >= region.minX && + top >= region.minY && + bottom <= region.maxY + ) { + obstacles.push(obstacle) + } + } + + return obstacles + } +} diff --git a/algos/common/generalized-astar/ObstacleList3dF64V1.ts b/algos/common/generalized-astar/ObstacleList3dF64V1.ts new file mode 100644 index 0000000..e28574f --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dF64V1.ts @@ -0,0 +1,279 @@ +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import type { + Direction3d, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { getLayerIndex, getLayerNamesForLayerCount } from "./util" +import { ObstacleList } from "./ObstacleList" +import { ObstacleList3d } from "./ObstacleList3d" + +/** + * Indices for each obstacle property in the data array. + * Each obstacle is stored in this order: + * [centerX, centerY, width, height, l, top, bottom, left, right, typeCode] + */ +const CENTER_X = 0 +const CENTER_Y = 1 +const WIDTH = 2 +const HEIGHT = 3 +const LAYER = 4 +const TOP = 5 +const BOTTOM = 6 +const LEFT = 7 +const RIGHT = 8 +const TYPE = 9 + +// Number of numeric fields per obstacle +const STRIDE = 10 + +// We can define numeric codes for obstacle types. +// For simplicity, let's assume all obstacles are rect for now. +const TYPE_RECT = 0 + +export class ObstacleList3dF64V1 extends ObstacleList3d { + data: Float64Array + originalObstacles: ObstacleWithEdges3d[] + GRID_STEP = 0.1 + layerCount: number + + constructor(layerCount: number, obstacles: Array) { + super(layerCount, []) + this.layerCount = layerCount + const availableLayers = getLayerNamesForLayerCount(layerCount) + + // Filter and expand obstacles based on layers + const filtered: ObstacleWithEdges3d[] = obstacles.flatMap((obstacle) => + obstacle.layers + .filter((layer) => availableLayers.includes(layer)) + .map((layer) => ({ + ...obstacle, + left: obstacle.center.x - obstacle.width / 2, + right: obstacle.center.x + obstacle.width / 2, + top: obstacle.center.y + obstacle.height / 2, + bottom: obstacle.center.y - obstacle.height / 2, + l: getLayerIndex(layerCount, layer), + })), + ) + + this.originalObstacles = filtered + const count = filtered.length + this.data = new Float64Array(count * STRIDE) + + for (let i = 0; i < count; i++) { + const obs = filtered[i] + const base = i * STRIDE + this.data[base + CENTER_X] = obs.center.x + this.data[base + CENTER_Y] = obs.center.y + this.data[base + WIDTH] = obs.width + this.data[base + HEIGHT] = obs.height + this.data[base + LAYER] = obs.l + this.data[base + TOP] = obs.top + this.data[base + BOTTOM] = obs.bottom + this.data[base + LEFT] = obs.left + this.data[base + RIGHT] = obs.right + // Assuming type is always 'rect' for these obstacles. If needed, map other types. + this.data[base + TYPE] = TYPE_RECT + } + } + + getObstacleAt( + x: number, + y: number, + l: number, + m?: number, + ): ObstacleWithEdges | null { + m ??= this.GRID_STEP + const { data } = this + const count = data.length / STRIDE + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== l) continue + const halfWidth = data[base + WIDTH] / 2 + m + const halfHeight = data[base + HEIGHT] / 2 + m + const centerX = data[base + CENTER_X] + const centerY = data[base + CENTER_Y] + + if ( + x >= centerX - halfWidth && + x <= centerX + halfWidth && + y >= centerY - halfHeight && + y <= centerY + halfHeight + ) { + return this.originalObstacles[i] + } + } + return null + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + return this.getObstacleAt(x, y, l, m) !== null + } + + getDirectionDistancesToNearestObstacle3d( + x: number, + y: number, + l: number, + ): DirectionDistances3d { + const { GRID_STEP, data } = this + const count = data.length / STRIDE + + const result: DirectionDistances3d = { + left: Infinity, + top: Infinity, + bottom: Infinity, + right: Infinity, + } + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== l) continue + // Assume rect + const left = data[base + LEFT] - GRID_STEP + const right = data[base + RIGHT] + GRID_STEP + const top = data[base + TOP] + GRID_STEP + const bottom = data[base + BOTTOM] - GRID_STEP + + // Check left + if (y >= bottom && y <= top && x > left) { + result.left = Math.min(result.left, x - right) + } + + // Check right + if (y >= bottom && y <= top && x < right) { + result.right = Math.min(result.right, left - x) + } + + // Check top + if (x >= left && x <= right && y < top) { + result.top = Math.min(result.top, bottom - y) + } + + // Check bottom + if (x >= left && x <= right && y > bottom) { + result.bottom = Math.min(result.bottom, y - top) + } + } + + return result + } + + getOrthoDirectionCollisionInfo( + point: Point3d, + dir: Direction3d, + { margin = 0 }: { margin?: number } = {}, + ): DirectionWithCollisionInfo3d { + const { x, y, l } = point + const { dx, dy, dl } = dir + + if (dl !== 0) { + // Moving between layers + const newLayer = l + dl + let collisionObstacle: ObstacleWithEdges | null = null + let minDistance = 1 + if (this.isObstacleAt(x, y, newLayer, margin)) { + collisionObstacle = this.getObstacleAt( + x, + y, + newLayer, + margin, + ) as ObstacleWithEdges + } + + return { + dx, + dy, + dl, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } else { + // Moving within the same layer + const { data } = this + const count = data.length / STRIDE + let minDistance = Infinity + let collisionObstacle: ObstacleWithEdges | null = null + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== l) continue + + const leftMargin = data[base + LEFT] - margin + const rightMargin = data[base + RIGHT] + margin + const topMargin = data[base + TOP] + margin + const bottomMargin = data[base + BOTTOM] - margin + + let distance: number | null = null + + if (dx === 1 && dy === 0) { + // Right + if (y > bottomMargin && y < topMargin && x < leftMargin) { + distance = leftMargin - x + } + } else if (dx === -1 && dy === 0) { + // Left + if (y > bottomMargin && y < topMargin && x > rightMargin) { + distance = x - rightMargin + } + } else if (dx === 0 && dy === 1) { + // Up + if (x > leftMargin && x < rightMargin && y < bottomMargin) { + distance = bottomMargin - y + } + } else if (dx === 0 && dy === -1) { + // Down + if (x > leftMargin && x < rightMargin && y > topMargin) { + distance = y - topMargin + } + } + + if (distance !== null && distance < minDistance) { + minDistance = distance + collisionObstacle = this.originalObstacles[i] + } + } + + return { + dx, + dy, + dl: 0, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } + } + + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + l: number // Layer to check + }): ObstacleWithEdges[] { + const { data, originalObstacles } = this + const count = data.length / STRIDE + const obstacles: ObstacleWithEdges[] = [] + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== region.l) continue + const left = data[base + LEFT] + const right = data[base + RIGHT] + const top = data[base + TOP] + const bottom = data[base + BOTTOM] + + if ( + left <= region.maxX && + right >= region.minX && + top >= region.minY && + bottom <= region.maxY + ) { + obstacles.push(originalObstacles[i]) + } + } + + return obstacles + } +} diff --git a/algos/common/generalized-astar/ObstacleList3dF64V2.ts b/algos/common/generalized-astar/ObstacleList3dF64V2.ts new file mode 100644 index 0000000..d1f3a30 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dF64V2.ts @@ -0,0 +1,519 @@ +// ObstacleList3dFastWithSorting.ts + +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import type { + Direction3d, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { getLayerIndex, getLayerNamesForLayerCount } from "./util" +import { ObstacleList } from "./ObstacleList" +import { ObstacleList3d } from "./ObstacleList3d" + +const CENTER_X = 0 +const CENTER_Y = 1 +const WIDTH = 2 +const HEIGHT = 3 +const LAYER = 4 +const TOP = 5 +const BOTTOM = 6 +const LEFT = 7 +const RIGHT = 8 +// We can store a type if needed, currently unused in sorting. +// const TYPE = 9 +const STRIDE = 10 + +export class ObstacleList3dF64V2 extends ObstacleList3d { + data: Float64Array + originalObstacles: ObstacleWithEdges3d[] + GRID_STEP = 0.1 + layerCount: number + + // Arrays of obstacle indices per layer sorted by their boundaries + obstaclesByLeft: number[][] + obstaclesByRight: number[][] + obstaclesByTop: number[][] + obstaclesByBottom: number[][] + + minX: number + maxX: number + minY: number + maxY: number + + constructor(layerCount: number, obstacles: Array) { + super(layerCount, []) + this.layerCount = layerCount + const availableLayers = getLayerNamesForLayerCount(layerCount) + + const filtered: ObstacleWithEdges3d[] = obstacles.flatMap((obstacle) => + obstacle.layers + .filter((layer) => availableLayers.includes(layer)) + .map((layer) => ({ + ...obstacle, + left: obstacle.center.x - obstacle.width / 2, + right: obstacle.center.x + obstacle.width / 2, + top: obstacle.center.y + obstacle.height / 2, + bottom: obstacle.center.y - obstacle.height / 2, + l: getLayerIndex(layerCount, layer), + })), + ) + + this.originalObstacles = filtered + const count = filtered.length + this.data = new Float64Array(count * STRIDE) + + let minX = Infinity + let maxX = -Infinity + let minY = Infinity + let maxY = -Infinity + + for (let i = 0; i < count; i++) { + const obs = filtered[i] + const base = i * STRIDE + this.data[base + CENTER_X] = obs.center.x + this.data[base + CENTER_Y] = obs.center.y + this.data[base + WIDTH] = obs.width + this.data[base + HEIGHT] = obs.height + this.data[base + LAYER] = obs.l + this.data[base + TOP] = obs.top + this.data[base + BOTTOM] = obs.bottom + this.data[base + LEFT] = obs.left + this.data[base + RIGHT] = obs.right + + if (obs.left < minX) minX = obs.left + if (obs.right > maxX) maxX = obs.right + if (obs.bottom < minY) minY = obs.bottom + if (obs.top > maxY) maxY = obs.top + } + + // Compute cell sizes based on bounding box + // If no obstacles, handle gracefully + if (count === 0) { + minX = 0 + maxX = 1 + minY = 0 + maxY = 1 + } + + this.minX = minX + this.maxX = maxX + this.minY = minY + this.maxY = maxY + + // Initialize arrays + this.obstaclesByLeft = Array.from({ length: layerCount }, () => []) + this.obstaclesByRight = Array.from({ length: layerCount }, () => []) + this.obstaclesByTop = Array.from({ length: layerCount }, () => []) + this.obstaclesByBottom = Array.from({ length: layerCount }, () => []) + + // Distribute obstacle indices into these arrays + for (let i = 0; i < count; i++) { + const base = i * STRIDE + const l = this.data[base + LAYER] + const li = l | 0 // layer as integer + + this.obstaclesByLeft[li].push(i) + this.obstaclesByRight[li].push(i) + this.obstaclesByTop[li].push(i) + this.obstaclesByBottom[li].push(i) + } + + // Sort each array by their respective edges + for (let l = 0; l < layerCount; l++) { + this.obstaclesByLeft[l].sort( + (a, b) => this.data[a * STRIDE + LEFT] - this.data[b * STRIDE + LEFT], + ) + this.obstaclesByRight[l].sort( + (a, b) => this.data[a * STRIDE + RIGHT] - this.data[b * STRIDE + RIGHT], + ) + this.obstaclesByTop[l].sort( + (a, b) => this.data[a * STRIDE + TOP] - this.data[b * STRIDE + TOP], + ) + this.obstaclesByBottom[l].sort( + (a, b) => + this.data[a * STRIDE + BOTTOM] - this.data[b * STRIDE + BOTTOM], + ) + } + } + + getObstacleAt( + x: number, + y: number, + l: number, + m?: number, + ): ObstacleWithEdges | null { + m ??= this.GRID_STEP + const { data } = this + const count = data.length / STRIDE + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== l) continue + const halfWidth = data[base + WIDTH] / 2 + m + const halfHeight = data[base + HEIGHT] / 2 + m + const centerX = data[base + CENTER_X] + const centerY = data[base + CENTER_Y] + + if ( + x >= centerX - halfWidth && + x <= centerX + halfWidth && + y >= centerY - halfHeight && + y <= centerY + halfHeight + ) { + return this.originalObstacles[i] + } + } + return null + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + return this.getObstacleAt(x, y, l, m) !== null + } + + getDirectionDistancesToNearestObstacle3d( + x: number, + y: number, + l: number, + ): DirectionDistances3d { + // This method remains O(n). If needed, we could also optimize it similarly. + const { GRID_STEP, data } = this + const count = data.length / STRIDE + + const result: DirectionDistances3d = { + left: Infinity, + top: Infinity, + bottom: Infinity, + right: Infinity, + } + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== l) continue + + const left = data[base + LEFT] - GRID_STEP + const right = data[base + RIGHT] + GRID_STEP + const top = data[base + TOP] + GRID_STEP + const bottom = data[base + BOTTOM] - GRID_STEP + + // Check left + if (y >= bottom && y <= top && x > left) { + result.left = Math.min(result.left, x - right) + } + + // Check right + if (y >= bottom && y <= top && x < right) { + result.right = Math.min(result.right, left - x) + } + + // Check top + if (x >= left && x <= right && y < top) { + result.top = Math.min(result.top, bottom - y) + } + + // Check bottom + if (x >= left && x <= right && y > bottom) { + result.bottom = Math.min(result.bottom, y - top) + } + } + + return result + } + + /** + * Binary search utility: finds the insertion index for value in a sorted array, + * similar to Python's bisect_left. Returns index where value could be inserted + * to maintain sorted order. If exact match is found, returns that index. + */ + private bisectLeft( + array: number[], + getVal: (i: number) => number, + value: number, + ): number { + let low = 0 + let high = array.length + while (low < high) { + const mid = (low + high) >>> 1 + if (getVal(array[mid]) < value) low = mid + 1 + else high = mid + } + return low + } + + /** + * Binary search utility: finds the insertion index for value similar to bisect_right, + * returns the insertion point to the right of existing entries of value. + */ + private bisectRight( + array: number[], + getVal: (i: number) => number, + value: number, + ): number { + let low = 0 + let high = array.length + while (low < high) { + const mid = (low + high) >>> 1 + if (getVal(array[mid]) <= value) low = mid + 1 + else high = mid + } + return low + } + + getOrthoDirectionCollisionInfo( + point: Point3d, + dir: Direction3d, + { margin = 0 }: { margin?: number } = {}, + ): DirectionWithCollisionInfo3d { + const { x, y, l } = point + const { dx, dy, dl } = dir + + if (dl !== 0) { + // Layer change is unchanged - no spatial optimization for layer transitions. + const newLayer = l + dl + let collisionObstacle: ObstacleWithEdges | null = null + let minDistance = 1 + if (this.isObstacleAt(x, y, newLayer, margin)) { + collisionObstacle = this.getObstacleAt( + x, + y, + newLayer, + margin, + ) as ObstacleWithEdges + } + + return { + dx, + dy, + dl, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } + + // Moving within the same layer + // Determine which sorted array to use based on direction + const layerIndex = l | 0 + let candidates: number[] = [] + let edgeGetter: (i: number) => number + let forwardSearch = true + + if (dx === 1 && dy === 0) { + // Moving right, check obstaclesByLeft: + // We want the smallest left that is > x + candidates = this.obstaclesByLeft[layerIndex] + edgeGetter = (idx) => this.data[idx * STRIDE + LEFT] + // We'll binary search for the first obstacle with LEFT > x + const startIndex = this.bisectLeft(candidates, edgeGetter, x) + return this.findCollisionCandidateHorizontal( + candidates, + startIndex, + x, + y, + margin, + dx, + dy, + /*movingRight=*/ true, + ) + } else if (dx === -1 && dy === 0) { + // Moving left, check obstaclesByRight: + // We want the largest right that is < x. We'll use bisectLeft for x and then go one left. + candidates = this.obstaclesByRight[layerIndex] + edgeGetter = (idx) => this.data[idx * STRIDE + RIGHT] + // Find insertion for x in RIGHT array + const startIndex = this.bisectLeft(candidates, edgeGetter, x) - 1 + return this.findCollisionCandidateHorizontal( + candidates, + startIndex, + x, + y, + margin, + dx, + dy, + /*movingRight=*/ false, + ) + } else if (dx === 0 && dy === 1) { + // Moving up, check obstaclesByBottom: + // We want the smallest bottom that is > y + candidates = this.obstaclesByBottom[layerIndex] + edgeGetter = (idx) => this.data[idx * STRIDE + BOTTOM] + const startIndex = this.bisectLeft(candidates, edgeGetter, y) + return this.findCollisionCandidateVertical( + candidates, + startIndex, + x, + y, + margin, + dx, + dy, + /*movingUp=*/ true, + ) + } else if (dx === 0 && dy === -1) { + // Moving down, check obstaclesByTop: + // We want the largest top that is < y + candidates = this.obstaclesByTop[layerIndex] + edgeGetter = (idx) => this.data[idx * STRIDE + TOP] + const startIndex = this.bisectLeft(candidates, edgeGetter, y) - 1 + return this.findCollisionCandidateVertical( + candidates, + startIndex, + x, + y, + margin, + dx, + dy, + /*movingUp=*/ false, + ) + } else { + // No movement + return { dx, dy, dl: 0, wallDistance: Infinity, obstacle: null } + } + } + + private findCollisionCandidateHorizontal( + candidates: number[], + startIndex: number, + x: number, + y: number, + margin: number, + dx: number, + dy: number, + movingRight: boolean, + ): DirectionWithCollisionInfo3d { + // If movingRight = true, we start from startIndex and move forward + // If movingRight = false, we start from startIndex and move backward + const step = movingRight ? 1 : -1 + const { data } = this + let minDistance = Infinity + let collisionObstacle: ObstacleWithEdges | null = null + + for (let i = startIndex; i >= 0 && i < candidates.length; i += step) { + const idx = candidates[i] + const base = idx * STRIDE + + // Vertical overlap check: + const topMargin = data[base + TOP] + margin + const bottomMargin = data[base + BOTTOM] - margin + const leftMargin = data[base + LEFT] - margin + const rightMargin = data[base + RIGHT] + margin + + if (y > bottomMargin && y < topMargin) { + // Compute distance + let distance: number + if (dx === 1) { + // moving right: obstacle's left edge is relevant + distance = leftMargin - x + if (distance >= 0 && distance < minDistance) { + minDistance = distance + collisionObstacle = this.originalObstacles[idx] + break + } + } else { + // moving left: obstacle's right edge is relevant + distance = x - rightMargin + if (distance >= 0 && distance < minDistance) { + minDistance = distance + collisionObstacle = this.originalObstacles[idx] + break + } + } + } + } + + return { + dx, + dy, + dl: 0, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } + + private findCollisionCandidateVertical( + candidates: number[], + startIndex: number, + x: number, + y: number, + margin: number, + dx: number, + dy: number, + movingUp: boolean, + ): DirectionWithCollisionInfo3d { + // Similar logic for vertical movement + const step = movingUp ? 1 : -1 + const { data } = this + let minDistance = Infinity + let collisionObstacle: ObstacleWithEdges | null = null + + for (let i = startIndex; i >= 0 && i < candidates.length; i += step) { + const idx = candidates[i] + const base = idx * STRIDE + + const leftMargin = data[base + LEFT] - margin + const rightMargin = data[base + RIGHT] + margin + const topMargin = data[base + TOP] + margin + const bottomMargin = data[base + BOTTOM] - margin + + if (x > leftMargin && x < rightMargin) { + let distance: number + if (dy === 1) { + // moving up: obstacle's bottom edge is relevant + distance = bottomMargin - y + if (distance >= 0 && distance < minDistance) { + minDistance = distance + collisionObstacle = this.originalObstacles[idx] + break + } + } else { + // moving down: obstacle's top edge is relevant + distance = y - topMargin + if (distance >= 0 && distance < minDistance) { + minDistance = distance + collisionObstacle = this.originalObstacles[idx] + break + } + } + } + } + + return { + dx, + dy, + dl: 0, + wallDistance: minDistance, + obstacle: collisionObstacle, + } + } + + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + l: number // Layer to check + }): ObstacleWithEdges[] { + // This remains the same O(n) approach. It can also be improved using spatial indexing if needed. + const { data, originalObstacles } = this + const count = data.length / STRIDE + const obstacles: ObstacleWithEdges[] = [] + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + if (data[base + LAYER] !== region.l) continue + const left = data[base + LEFT] + const right = data[base + RIGHT] + const top = data[base + TOP] + const bottom = data[base + BOTTOM] + + if ( + left <= region.maxX && + right >= region.minX && + top >= region.minY && + bottom <= region.maxY + ) { + obstacles.push(originalObstacles[i]) + } + } + + return obstacles + } +} diff --git a/algos/common/generalized-astar/ObstacleList3dF64V3.ts b/algos/common/generalized-astar/ObstacleList3dF64V3.ts new file mode 100644 index 0000000..c06bdb0 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dF64V3.ts @@ -0,0 +1,152 @@ +// ObstacleList3dCellBased.ts + +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import type { + Direction3d, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { getLayerIndex, getLayerNamesForLayerCount } from "./util" +import { ObstacleList } from "./ObstacleList" +import { ObstacleList3d } from "./ObstacleList3d" +import { ObstacleList3dF64V2 } from "./ObstacleList3dF64V2" + +const CELL_COLS = 16 +const CELL_ROWS = 16 + +const CENTER_X = 0 +const CENTER_Y = 1 +const WIDTH = 2 +const HEIGHT = 3 +const LAYER = 4 +const TOP = 5 +const BOTTOM = 6 +const LEFT = 7 +const RIGHT = 8 +const STRIDE = 10 + +export class ObstacleList3dF64V3 extends ObstacleList3dF64V2 { + GRID_STEP = 0.1 + layerCount: number + + // Cells: each cell is lazily filled with obstacle indices + // Key: row * CELL_COLS + col + cells: (number[] | null)[] + + cellWidth: number + cellHeight: number + + constructor(layerCount: number, obstacles: Array) { + super(layerCount, obstacles) + this.layerCount = layerCount + const availableLayers = getLayerNamesForLayerCount(layerCount) + + const totalWidth = this.maxX - this.minX || 1 + const totalHeight = this.maxY - this.minY || 1 + this.cellWidth = totalWidth / CELL_COLS + this.cellHeight = totalHeight / CELL_ROWS + + // Initialize cells to null for lazy filling + this.cells = Array(CELL_COLS * CELL_ROWS).fill(null) + } + + private getCellIndexForPoint(x: number, y: number): number | null { + // Compute which cell (x,y) falls into + const col = Math.floor( + ((x - this.minX) / (this.maxX - this.minX)) * CELL_COLS, + ) + const row = Math.floor( + ((y - this.minY) / (this.maxY - this.minY)) * CELL_ROWS, + ) + + if (col < 0 || col >= CELL_COLS || row < 0 || row >= CELL_ROWS) { + return null // Point outside the bounding box range + } + return row * CELL_COLS + col + } + + private fillCell(cellIndex: number) { + // If the cell is not filled, we scan all obstacles to find which belong here + if (this.cells[cellIndex] !== null) return // Already filled + + const row = Math.floor(cellIndex / CELL_COLS) + const col = cellIndex % CELL_COLS + + const cellMinX = this.minX + col * this.cellWidth + const cellMaxX = cellMinX + this.cellWidth + const cellMinY = this.minY + row * this.cellHeight + const cellMaxY = cellMinY + this.cellHeight + + const { data } = this + const count = data.length / STRIDE + const cellObstacles: number[] = [] + + for (let i = 0; i < count; i++) { + const base = i * STRIDE + const left = data[base + LEFT] + const right = data[base + RIGHT] + const top = data[base + TOP] + const bottom = data[base + BOTTOM] + + // Check if obstacle overlaps this cell at all + if ( + right >= cellMinX && + left <= cellMaxX && + top >= cellMinY && + bottom <= cellMaxY + ) { + cellObstacles.push(i) + } + } + + this.cells[cellIndex] = cellObstacles + } + + getObstacleAt( + x: number, + y: number, + l: number, + m?: number, + ): ObstacleWithEdges | null { + m ??= this.GRID_STEP + + const cellIndex = this.getCellIndexForPoint(x, y) + if (cellIndex === null) { + // Out of range, just scan all or return null + // For simplicity, return null. If needed, handle differently. + return null + } + + this.fillCell(cellIndex) + const cellObstacles = this.cells[cellIndex]! + const { data } = this + + for (let idx of cellObstacles) { + const base = idx * STRIDE + // Check layer + if (data[base + LAYER] !== l) continue + + const halfWidth = data[base + WIDTH] / 2 + m + const halfHeight = data[base + HEIGHT] / 2 + m + const centerX = data[base + CENTER_X] + const centerY = data[base + CENTER_Y] + + if ( + x >= centerX - halfWidth && + x <= centerX + halfWidth && + y >= centerY - halfHeight && + y <= centerY + halfHeight + ) { + return this.originalObstacles[idx] + } + } + + return null + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + return this.getObstacleAt(x, y, l, m) !== null + } +} diff --git a/algos/common/generalized-astar/ObstacleList3dFloatArrays.ts b/algos/common/generalized-astar/ObstacleList3dFloatArrays.ts new file mode 100644 index 0000000..e6af310 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dFloatArrays.ts @@ -0,0 +1,30 @@ +import type { Obstacle } from "solver-utils" +import { ObstacleList3d } from "./ObstacleList3d" + +export class ObstacleList3dFloatArrays extends ObstacleList3d { + /** + * This is a flattened representation of obstacles, each obstacle is + * represented like so: + * [ left, top, right, bottom ] + */ + obstaclesF64: Float64Array + + constructor(layerCount: number, obstacles: Array) { + super(layerCount, obstacles) + + // Create float64 representation of obstacles + const internalObstacles = this.obstacles + + this.obstaclesF64 = new Float64Array(internalObstacles.length * 4) + + for (let i = 0; i < internalObstacles.length; i++) { + const obstacle = internalObstacles[i] + this.obstaclesF64[i * 5] = obstacle.left + this.obstaclesF64[i * 5 + 1] = obstacle.top + this.obstaclesF64[i * 5 + 2] = obstacle.right + this.obstaclesF64[i * 5 + 3] = obstacle.bottom + } + } + + // getObstacleAt(x: number, y: number, l: number, m?: number): Obstacle | null {} +} diff --git a/algos/common/generalized-astar/ObstacleList3dSectional.ts b/algos/common/generalized-astar/ObstacleList3dSectional.ts new file mode 100644 index 0000000..0b31832 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dSectional.ts @@ -0,0 +1,291 @@ +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import { ObstacleList3d } from "./ObstacleList3d" +import type { + Direction3d, + DirectionDistances, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + Obstacle3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { Profiler } from "./Profiler" + +type SectionId = `${number},${number}` + +const globalObstacleList3dSectionalProfiler = new Profiler() + +export class ObstacleList3dSectional implements ObstacleList3d { + /** + * Contains obstacles that are touching or in any way interacting with a small + * section + */ + sections: Record = {} + + /** + * Contains obstacles that are aligned to a section, i.e. they lie directly + * above, below, to the left, or to the right of a section (including the + * section). This is used to do ray intersections + */ + alignedSections: Record = {} + profiler?: Profiler | undefined + obstacles = [] + bounds: { minX: number; minY: number; maxX: number; maxY: number } + GRID_STEP: number = 0.1 + + sectionSizeX: number + sectionSizeY: number + numSectionsX: number + numSectionsY: number + + layerCount: number + + constructor(layerCount: number, obstacles: Array) { + if (process.env.TSCIRCUIT_AUTOROUTER_PROFILING_ENABLED) { + this.profiler = globalObstacleList3dSectionalProfiler + } + this.layerCount = layerCount + + this.bounds = { + minX: Infinity, + minY: Infinity, + maxX: -Infinity, + maxY: -Infinity, + } + + for (const obstacle of obstacles) { + this.bounds.minX = Math.min( + this.bounds.minX, + obstacle.center.x - obstacle.width / 2, + ) + this.bounds.minY = Math.min( + this.bounds.minY, + obstacle.center.y - obstacle.height / 2, + ) + this.bounds.maxX = Math.max( + this.bounds.maxX, + obstacle.center.x + obstacle.width / 2, + ) + this.bounds.maxY = Math.max( + this.bounds.maxY, + obstacle.center.y + obstacle.height / 2, + ) + } + + this.numSectionsX = 10 + this.numSectionsY = 10 + + this.sectionSizeX = + (this.bounds.maxX - this.bounds.minX) / this.numSectionsX + this.sectionSizeY = + (this.bounds.maxY - this.bounds.minY) / this.numSectionsY + + const obstaclesInSection: Record> = {} + const obstaclesInAlignedSection: Record> = {} + for (let sectionX = 0; sectionX < this.numSectionsX; sectionX++) { + for (let sectionY = 0; sectionY < this.numSectionsY; sectionY++) { + obstaclesInSection[`${sectionX},${sectionY}`] = [] + obstaclesInAlignedSection[`${sectionX},${sectionY}`] = [] + } + } + + for (const obstacle of obstacles) { + const sections = this._getSectionsOfObstacle(obstacle) + for (const section of sections) { + obstaclesInSection[section].push(obstacle) + } + + const alignedSections = this._getAlignedSectionsForObstacle(obstacle) + for (const section of alignedSections) { + obstaclesInAlignedSection[section].push(obstacle) + } + } + + this.sections = {} + for (const sectionId of Object.keys(obstaclesInSection)) { + const typedSectionId = sectionId as `${number},${number}` + this.sections[typedSectionId] = new ObstacleList3d( + layerCount, + obstaclesInSection[typedSectionId], + ) + } + + this.alignedSections = {} + for (const sectionId of Object.keys(obstaclesInAlignedSection)) { + const typedSectionId = sectionId as `${number},${number}` + this.alignedSections[typedSectionId] = new ObstacleList3d( + layerCount, + obstaclesInAlignedSection[typedSectionId], + ) + } + + if (this.profiler) { + for (const methodName of [ + "getObstacleAt", + "isObstacleAt", + "getDirectionDistancesToNearestObstacle", + "getOrthoDirectionCollisionInfo", + "getObstaclesOverlappingRegion", + ]) { + // @ts-ignore + const originalMethod = this[methodName] as Function + // @ts-ignore + this[methodName as keyof ObstacleList] = this.profiler!.wrapMethod( + `ObstacleList3dSectional.${methodName}`, + originalMethod.bind(this), + ) + } + } + } + + _getSectionXYForPoint(x: number, y: number): [number, number] { + let sectionX = Math.floor((x - this.bounds.minX) / this.sectionSizeX) + let sectionY = Math.floor((y - this.bounds.minY) / this.sectionSizeY) + // clamp + sectionX = Math.max(0, Math.min(this.numSectionsX - 1, sectionX)) + sectionY = Math.max(0, Math.min(this.numSectionsY - 1, sectionY)) + + return [sectionX, sectionY] + } + + _getSectionForPoint(x: number, y: number): SectionId { + const [sectionX, sectionY] = this._getSectionXYForPoint(x, y) + return `${sectionX},${sectionY}` + } + + _getSectionsOfRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + }) { + const [minSectionX, minSectionY] = this._getSectionXYForPoint( + region.minX, + region.minY, + ) + const [maxSectionX, maxSectionY] = this._getSectionXYForPoint( + region.maxX, + region.maxY, + ) + + const sections: SectionId[] = [] + for (let sectionX = minSectionX; sectionX <= maxSectionX; sectionX++) { + for (let sectionY = minSectionY; sectionY <= maxSectionY; sectionY++) { + sections.push(`${sectionX},${sectionY}`) + } + } + + return sections + } + + _getSectionsOfObstacle(obstacle: Obstacle): SectionId[] { + const { + center: { x, y }, + height, + width, + } = obstacle + return this._getSectionsOfRegion({ + minX: x - width / 2, + minY: y - height / 2, + maxX: x + width / 2, + maxY: y + height / 2, + }) + } + + _getAlignedSectionsForObstacle(obstacle: Obstacle): SectionId[] { + const { + center: { x, y }, + height, + width, + } = obstacle + const alignedSections: Set = new Set() + + const [minSectionX, minSectionY] = this._getSectionXYForPoint( + x - width / 2, + y - height / 2, + ) + const [maxSectionX, maxSectionY] = this._getSectionXYForPoint( + x + width / 2, + y + height / 2, + ) + + for (let sectionX = minSectionX; sectionX <= maxSectionX; sectionX++) { + // Add all sections above and below + for (let sectionY = 0; sectionY < this.numSectionsY; sectionY++) { + alignedSections.add(`${sectionX},${sectionY}`) + } + } + + for (let sectionY = minSectionY; sectionY <= maxSectionY; sectionY++) { + // Add all sections to the left and right + for (let sectionX = 0; sectionX < this.numSectionsX; sectionX++) { + alignedSections.add(`${sectionX},${sectionY}`) + } + } + + return Array.from(alignedSections) + } + + getObstacleAt(x: number, y: number, l: number, m?: number): Obstacle | null { + const sectionId = this._getSectionForPoint(x, y) + return this.sections[sectionId]?.getObstacleAt(x, y, l, m) ?? null + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + const sectionId = this._getSectionForPoint(x, y) + return this.sections[sectionId]?.isObstacleAt(x, y, l, m) ?? false + } + + getDirectionDistancesToNearestObstacle3d( + x: number, + y: number, + l: number, + ): DirectionDistances3d { + const sectionId = this._getSectionForPoint(x, y) + return this.alignedSections[ + sectionId + ].getDirectionDistancesToNearestObstacle3d(x, y, l) + } + getOrthoDirectionCollisionInfo( + point: Point3d, + dir: Direction3d, + opts?: { margin?: number }, + ): DirectionWithCollisionInfo3d { + const sectionId = this._getSectionForPoint(point.x, point.y) + return this.alignedSections[sectionId].getOrthoDirectionCollisionInfo( + point, + dir, + opts, + ) + } + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + l: number + }): ObstacleWithEdges[] { + const sections = this._getSectionsOfRegion(region) + + const obstacles: ObstacleWithEdges[] = [] + for (const section of sections) { + obstacles.push( + ...this.sections[section].getObstaclesOverlappingRegion(region), + ) + } + + // TODO dedupe obstacles + + return obstacles + } + + getDirectionDistancesToNearestObstacle( + x: number, + y: number, + ): DirectionDistances { + const sectionId = this._getSectionForPoint(x, y) + return this.alignedSections[ + sectionId + ].getDirectionDistancesToNearestObstacle(x, y) + } +} diff --git a/algos/common/generalized-astar/ObstacleList3dSectionalV2.ts b/algos/common/generalized-astar/ObstacleList3dSectionalV2.ts new file mode 100644 index 0000000..30d9f26 --- /dev/null +++ b/algos/common/generalized-astar/ObstacleList3dSectionalV2.ts @@ -0,0 +1,342 @@ +import type { Obstacle, ObstacleWithEdges } from "solver-utils" +import { ObstacleList3d } from "./ObstacleList3d" +import type { + Direction3d, + DirectionDistances, + DirectionDistances3d, + DirectionWithCollisionInfo3d, + Obstacle3d, + ObstacleWithEdges3d, + Point3d, +} from "./types" +import { Profiler } from "./Profiler" +import { ObstacleList3dF64V2 } from "./ObstacleList3dF64V2" + +type SectionId = `${number},${number}` + +const globalObstacleList3dSectionalV2Profiler = new Profiler() + +export class ObstacleList3dSectionalV2 implements ObstacleList3d { + /** + * Cached on-demand computed sections. + * Each section caches an ObstacleList3d containing obstacles intersecting that section. + */ + private sectionsCache: Record = {} + + /** + * Cached on-demand computed aligned sections. + * Each aligned section caches an ObstacleList3d containing obstacles aligned with that section. + */ + private alignedSectionsCache: Record = {} + + profiler?: Profiler | undefined + obstacles: Obstacle[] + bounds: { minX: number; minY: number; maxX: number; maxY: number } + GRID_STEP: number = 0.1 + + sectionSizeX: number + sectionSizeY: number + numSectionsX: number + numSectionsY: number + + layerCount: number + + constructor(layerCount: number, obstacles: Array) { + if (process.env.TSCIRCUIT_AUTOROUTER_PROFILING_ENABLED) { + this.profiler = globalObstacleList3dSectionalV2Profiler + } + this.layerCount = layerCount + this.obstacles = obstacles + + this.bounds = { + minX: Infinity, + minY: Infinity, + maxX: -Infinity, + maxY: -Infinity, + } + + for (const obstacle of obstacles) { + this.bounds.minX = Math.min( + this.bounds.minX, + obstacle.center.x - obstacle.width / 2, + ) + this.bounds.minY = Math.min( + this.bounds.minY, + obstacle.center.y - obstacle.height / 2, + ) + this.bounds.maxX = Math.max( + this.bounds.maxX, + obstacle.center.x + obstacle.width / 2, + ) + this.bounds.maxY = Math.max( + this.bounds.maxY, + obstacle.center.y + obstacle.height / 2, + ) + } + + // We choose a fixed number of sections for demonstration. + // This can be dynamic or configurable if needed. + this.numSectionsX = 10 + this.numSectionsY = 10 + + this.sectionSizeX = + (this.bounds.maxX - this.bounds.minX) / this.numSectionsX + this.sectionSizeY = + (this.bounds.maxY - this.bounds.minY) / this.numSectionsY + + if (this.profiler) { + for (const methodName of [ + "getObstacleAt", + "isObstacleAt", + "getDirectionDistancesToNearestObstacle", + "getOrthoDirectionCollisionInfo", + "getObstaclesOverlappingRegion", + ]) { + // @ts-ignore + const originalMethod = this[methodName] as Function + // @ts-ignore + this[methodName as keyof ObstacleList3d] = this.profiler!.wrapMethod( + `ObstacleList3dSectional.${methodName}`, + originalMethod.bind(this), + ) + } + } + } + + /** + * Compute sections that intersect with a given region. + */ + _getSectionsOfRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + }) { + const [minSectionX, minSectionY] = this._getSectionXYForPoint( + region.minX, + region.minY, + ) + const [maxSectionX, maxSectionY] = this._getSectionXYForPoint( + region.maxX, + region.maxY, + ) + + const sections: SectionId[] = [] + for (let sectionX = minSectionX; sectionX <= maxSectionX; sectionX++) { + for (let sectionY = minSectionY; sectionY <= maxSectionY; sectionY++) { + sections.push(`${sectionX},${sectionY}`) + } + } + + return sections + } + + _getSectionsOfObstacle(obstacle: Obstacle): SectionId[] { + const { + center: { x, y }, + height, + width, + } = obstacle + return this._getSectionsOfRegion({ + minX: x - width / 2, + minY: y - height / 2, + maxX: x + width / 2, + maxY: y + height / 2, + }) + } + + _getAlignedSectionsForObstacle(obstacle: Obstacle): SectionId[] { + const { + center: { x, y }, + height, + width, + } = obstacle + const alignedSections: Set = new Set() + + const [minSectionX, minSectionY] = this._getSectionXYForPoint( + x - width / 2, + y - height / 2, + ) + const [maxSectionX, maxSectionY] = this._getSectionXYForPoint( + x + width / 2, + y + height / 2, + ) + + // Vertical alignment (above/below): all sections with x between minSectionX and maxSectionX. + for (let sectionX = minSectionX; sectionX <= maxSectionX; sectionX++) { + for (let sectionY = 0; sectionY < this.numSectionsY; sectionY++) { + alignedSections.add(`${sectionX},${sectionY}`) + } + } + + // Horizontal alignment (left/right): all sections with y between minSectionY and maxSectionY. + for (let sectionY = minSectionY; sectionY <= maxSectionY; sectionY++) { + for (let sectionX = 0; sectionX < this.numSectionsX; sectionX++) { + alignedSections.add(`${sectionX},${sectionY}`) + } + } + + return Array.from(alignedSections) + } + + _getSectionXYForPoint(x: number, y: number): [number, number] { + let sectionX = Math.floor((x - this.bounds.minX) / this.sectionSizeX) + let sectionY = Math.floor((y - this.bounds.minY) / this.sectionSizeY) + // clamp + sectionX = Math.max(0, Math.min(this.numSectionsX - 1, sectionX)) + sectionY = Math.max(0, Math.min(this.numSectionsY - 1, sectionY)) + + return [sectionX, sectionY] + } + + _getSectionForPoint(x: number, y: number): SectionId { + const [sectionX, sectionY] = this._getSectionXYForPoint(x, y) + return `${sectionX},${sectionY}` + } + + /** + * Lazily compute and cache the ObstacleList3d for a given section. + */ + private _getSectionObstacleList(sectionId: SectionId): ObstacleList3d { + if (!this.sectionsCache[sectionId]) { + // Compute obstacles in this section + const [sectionXStr, sectionYStr] = sectionId.split(",") + const sectionX = parseInt(sectionXStr, 10) + const sectionY = parseInt(sectionYStr, 10) + + const sectionMinX = this.bounds.minX + sectionX * this.sectionSizeX + const sectionMinY = this.bounds.minY + sectionY * this.sectionSizeY + const sectionMaxX = sectionMinX + this.sectionSizeX + const sectionMaxY = sectionMinY + this.sectionSizeY + + const obstaclesInSection = this.obstacles.filter((obstacle) => { + const oxMin = obstacle.center.x - obstacle.width / 2 + const oxMax = obstacle.center.x + obstacle.width / 2 + const oyMin = obstacle.center.y - obstacle.height / 2 + const oyMax = obstacle.center.y + obstacle.height / 2 + + // Check if obstacle intersects with the section region + return ( + oxMax >= sectionMinX && + oxMin <= sectionMaxX && + oyMax >= sectionMinY && + oyMin <= sectionMaxY + ) + }) + + this.sectionsCache[sectionId] = new ObstacleList3dF64V2( + this.layerCount, + obstaclesInSection, + ) + } + return this.sectionsCache[sectionId] + } + + /** + * Lazily compute and cache the ObstacleList3d for a given aligned section. + */ + private _getAlignedSectionObstacleList(sectionId: SectionId): ObstacleList3d { + if (!this.alignedSectionsCache[sectionId]) { + // Compute obstacles aligned with this section + const [sectionXStr, sectionYStr] = sectionId.split(",") + const sectionX = parseInt(sectionXStr, 10) + const sectionY = parseInt(sectionYStr, 10) + + // Find all obstacles that would appear in the alignedSections of this section. + // Aligned sections contain all obstacles that share x or y alignment. + // In other words, if the obstacle spans horizontally above, below, + // or vertically to the left/right of this section, it's included. + + // To find aligned obstacles, we basically consider: + // - All obstacles that would intersect at least one section with the same sectionX (vertically aligned). + // - All obstacles that would intersect at least one section with the same sectionY (horizontally aligned). + + const obstaclesInAlignedSection = this.obstacles.filter((obstacle) => { + const obstacleSections = this._getSectionsOfObstacle(obstacle) + + // If the obstacle intersects any section with the same X index or + // any section with the same Y index, it's aligned. + return obstacleSections.some((sid) => { + const [oxStr, oyStr] = sid.split(",") + const ox = parseInt(oxStr, 10) + const oy = parseInt(oyStr, 10) + return ox === sectionX || oy === sectionY + }) + }) + + this.alignedSectionsCache[sectionId] = new ObstacleList3dF64V2( + this.layerCount, + obstaclesInAlignedSection, + ) + } + return this.alignedSectionsCache[sectionId] + } + + getObstacleAt(x: number, y: number, l: number, m?: number): Obstacle | null { + const sectionId = this._getSectionForPoint(x, y) + return ( + this._getSectionObstacleList(sectionId)?.getObstacleAt(x, y, l, m) ?? null + ) + } + + isObstacleAt(x: number, y: number, l: number, m?: number): boolean { + const sectionId = this._getSectionForPoint(x, y) + return ( + this._getSectionObstacleList(sectionId)?.isObstacleAt(x, y, l, m) ?? false + ) + } + + getDirectionDistancesToNearestObstacle3d( + x: number, + y: number, + l: number, + ): DirectionDistances3d { + const sectionId = this._getSectionForPoint(x, y) + return this._getAlignedSectionObstacleList( + sectionId, + ).getDirectionDistancesToNearestObstacle3d(x, y, l) + } + + getOrthoDirectionCollisionInfo( + point: Point3d, + dir: Direction3d, + opts?: { margin?: number }, + ): DirectionWithCollisionInfo3d { + const sectionId = this._getSectionForPoint(point.x, point.y) + return this._getAlignedSectionObstacleList( + sectionId, + ).getOrthoDirectionCollisionInfo(point, dir, opts) + } + + getObstaclesOverlappingRegion(region: { + minX: number + minY: number + maxX: number + maxY: number + l: number + }): ObstacleWithEdges[] { + const sections = this._getSectionsOfRegion(region) + + const obstacles: ObstacleWithEdges[] = [] + for (const section of sections) { + obstacles.push( + ...this._getSectionObstacleList(section).getObstaclesOverlappingRegion( + region, + ), + ) + } + + // TODO: Deduplicate obstacles if necessary. + return obstacles + } + + getDirectionDistancesToNearestObstacle( + x: number, + y: number, + ): DirectionDistances { + const sectionId = this._getSectionForPoint(x, y) + return this._getAlignedSectionObstacleList( + sectionId, + ).getDirectionDistancesToNearestObstacle(x, y) + } +} diff --git a/algos/common/generalized-astar/Profiler.ts b/algos/common/generalized-astar/Profiler.ts new file mode 100644 index 0000000..f180bee --- /dev/null +++ b/algos/common/generalized-astar/Profiler.ts @@ -0,0 +1,97 @@ +export class Profiler { + private measurements: Record< + string, + { + totalTime: number + calls: number + lastStart?: number + } + > = {} + + startMeasurement(name: string) { + if (!this.measurements[name]) { + this.measurements[name] = { totalTime: 0, calls: 0 } + } + this.measurements[name].lastStart = performance.now() + } + + endMeasurement(name: string) { + const measurement = this.measurements[name] + if (!measurement || measurement.lastStart === undefined) return + + const duration = performance.now() - measurement.lastStart + measurement.totalTime += duration + measurement.calls += 1 + measurement.lastStart = undefined + } + + wrapMethod( + name: string, + fn: (...args: T) => R, + ): (...args: T) => R { + return (...args: T) => { + this.startMeasurement(name) + const result = fn(...args) + this.endMeasurement(name) + return result + } + } + + getResults() { + const results: Record< + string, + { + totalTime: number + calls: number + averageTime: number + } + > = {} + + for (const [name, data] of Object.entries(this.measurements)) { + results[name] = { + totalTime: data.totalTime, + calls: data.calls, + averageTime: data.totalTime / (data.calls || 1), + } + } + + return results + } + + getResultsPretty(): Record< + string, + { + totalTime: string + calls: number + averageTime: string + } + > { + const results = this.getResults() + const prettyResults: Record< + string, + { + totalTime: string + calls: number + averageTime: string + } + > = {} + + // Sort keys by totalTime in descending order + const sortedKeys = Object.keys(results).sort( + (a, b) => results[b].totalTime - results[a].totalTime, + ) + + for (const key of sortedKeys) { + prettyResults[key] = { + totalTime: `${results[key].totalTime.toFixed(2)}ms`, + calls: results[key].calls, + averageTime: `${(results[key].averageTime * 1000).toFixed(1)}us`, + } + } + return prettyResults + } + + reset() { + this.measurements = {} + } +} diff --git a/algos/common/generalized-astar/types.ts b/algos/common/generalized-astar/types.ts new file mode 100644 index 0000000..ac11bb7 --- /dev/null +++ b/algos/common/generalized-astar/types.ts @@ -0,0 +1,114 @@ +import type { Obstacle } from "autorouting-dataset/lib/types" + +export type Direction3d = { + dx: number + dy: number + /** + * Always integer, usually -1 or 1, -1 indicating to go towards the top layer, + * 1 indicating to go down a layer towards the bottom layer + */ + dl: number +} + +export interface Point3d { + x: number + y: number + l: number // Layer +} + +export interface Point3dWithObstacleHit extends Point3d { + obstacleHit?: Obstacle | null +} + +export interface Node3d extends Node { + l: number + parent: Node3d | null +} + +export interface Obstacle3d extends Obstacle { + l: number +} + +export interface ObstacleWithEdges3d extends Obstacle3d { + top: number + bottom: number + left: number + right: number +} + +export interface DirectionWithCollisionInfo3d extends Direction3d { + wallDistance: number + obstacle: Obstacle | null +} + +export interface DirectionDistances3d { + left: number + top: number + bottom: number + right: number +} + +export interface DirectionDistances { + left: number + top: number + bottom: number + right: number +} + +export interface Direction { + dx: number + dy: number +} + +export interface DirectionWithWallDistance extends Direction { + wallDistance: number +} + +export interface DirectionWithCollisionInfo extends Direction { + wallDistance: number + obstacle: Obstacle | null +} + +export interface Point { + x: number + y: number +} + +export interface PointWithObstacleHit extends Point { + obstacleHit?: Obstacle | null + + /** + * Used in multi-margin autorouter to penalize traveling close to the wall + */ + travelMarginCostFactor?: number + enterMarginCost?: number +} + +export interface PointWithWallDistance extends Point { + wallDistance: number +} + +export interface Node extends Point { + /** Distance from the parent node (along path) */ + g: number + /** Heuristic distance from the goal */ + h: number + /** Distance score for this node (g + h) */ + f: number + /** Manhattan Distance from the parent node */ + manDistFromParent: number + nodesInPath: number + obstacleHit?: Obstacle + parent: Node | null + + /** + * Used in multi-margin autorouter to penalize traveling close to the wall + */ + travelMarginCostFactor?: number + enterMarginCost?: number + + /** + * Layer index, not needed for single-layer autorouters + */ + l?: number +} diff --git a/algos/common/generalized-astar/util.ts b/algos/common/generalized-astar/util.ts new file mode 100644 index 0000000..d2c19bf --- /dev/null +++ b/algos/common/generalized-astar/util.ts @@ -0,0 +1,48 @@ +import type { LayerRef } from "@tscircuit/soup" +import type { Point3d, Direction3d } from "./types" + +export function dirFromAToB(nodeA: Point3d, nodeB: Point3d): Direction3d { + const dx = nodeB.x > nodeA.x ? 1 : nodeB.x < nodeA.x ? -1 : 0 + const dy = nodeB.y > nodeA.y ? 1 : nodeB.y < nodeA.y ? -1 : 0 + const dl = nodeB.l > nodeA.l ? 1 : nodeB.l < nodeA.l ? -1 : 0 + return { dx, dy, dl } +} + +export function manDist(a: Point3d, b: Point3d): number { + return Math.abs(a.x - b.x) + Math.abs(a.y - b.y) + Math.abs(a.l - b.l) +} + +export const LAYER_COUNT_INDEX_MAP: Record = { + 1: ["top"], + 2: ["top", "bottom"], + 4: ["top", "inner1", "inner2", "bottom"], +} + +export const getLayerNamesForLayerCount = (layerCount: number): string[] => { + return LAYER_COUNT_INDEX_MAP[layerCount] +} + +export function getLayerIndex(layerCount: number, layer: string): number { + const layerArray = LAYER_COUNT_INDEX_MAP[layerCount] + const index = layerArray.indexOf(layer) + if (index === -1) { + throw new Error( + `Invalid layer for getLayerIndex (for layerCount === ${layerCount}): "${layer}"`, + ) + } + return index +} + +export function indexToLayer(layerCount: number, index: number): string { + const layerArray = LAYER_COUNT_INDEX_MAP[layerCount] + const layer = layerArray[index] + if (!layer) { + throw new Error( + `Invalid index for indexToLayer (for layerCount === ${layerCount}): "${index}"`, + ) + } + return layer +} + +export const nodeName = (node: Point3d, GRID_STEP: number = 0.1): string => + `${Math.round(node.x / GRID_STEP)},${Math.round(node.y / GRID_STEP)}` diff --git a/algos/infinite-grid-ijump-astar/v2/lib/GeneralizedAstar.ts b/algos/infinite-grid-ijump-astar/v2/lib/GeneralizedAstar.ts index 1d50777..7f2a54d 100644 --- a/algos/infinite-grid-ijump-astar/v2/lib/GeneralizedAstar.ts +++ b/algos/infinite-grid-ijump-astar/v2/lib/GeneralizedAstar.ts @@ -1,495 +1 @@ -import type { AnySoupElement, LayerRef, PCBSMTPad } from "@tscircuit/soup" -// import { QuadtreeObstacleList } from "./QuadtreeObstacleList" -import type { Node, Point, PointWithObstacleHit } from "./types" -import { manDist, nodeName } from "./util" - -import Debug from "debug" -import type { - Obstacle, - SimpleRouteConnection, - SimpleRouteJson, - SimplifiedPcbTrace, -} from "solver-utils" -import { getObstaclesFromRoute } from "solver-utils/getObstaclesFromRoute" -import { ObstacleList } from "./ObstacleList" -import { removePathLoops } from "solver-postprocessing/remove-path-loops" -import { addViasWhenLayerChanges } from "solver-postprocessing/add-vias-when-layer-changes" -import type { AnyCircuitElement } from "circuit-json" - -const debug = Debug("autorouting-dataset:astar") - -export interface PointWithLayer extends Point { - layer: string -} - -export type ConnectionSolveResult = - | { solved: false; connectionName: string } - | { solved: true; connectionName: string; route: PointWithLayer[] } - -export class GeneralizedAstarAutorouter { - openSet: Node[] = [] - closedSet: Set = new Set() - debug = false - - debugSolutions?: Record - debugMessage: string | null = null - debugTraceCount: number = 0 - - input: SimpleRouteJson - obstacles?: ObstacleList - allObstacles: Obstacle[] - startNode?: Node - goalPoint?: Point & { l: number } - GRID_STEP: number - OBSTACLE_MARGIN: number - MAX_ITERATIONS: number - isRemovePathLoopsEnabled: boolean - /** - * Setting this greater than 1 makes the algorithm find suboptimal paths and - * act more greedy, but at greatly improves performance. - * - * Recommended value is between 1.1 and 1.5 - */ - GREEDY_MULTIPLIER = 1.1 - - iterations: number = -1 - - constructor(opts: { - input: SimpleRouteJson - startNode?: Node - goalPoint?: Point - GRID_STEP?: number - OBSTACLE_MARGIN?: number - MAX_ITERATIONS?: number - isRemovePathLoopsEnabled?: boolean - debug?: boolean - }) { - this.input = opts.input - this.allObstacles = opts.input.obstacles - this.startNode = opts.startNode - this.goalPoint = opts.goalPoint - ? ({ l: 0, ...opts.goalPoint } as any) - : undefined - this.GRID_STEP = opts.GRID_STEP ?? 0.1 - this.OBSTACLE_MARGIN = opts.OBSTACLE_MARGIN ?? 0.15 - this.MAX_ITERATIONS = opts.MAX_ITERATIONS ?? 100 - this.debug = opts.debug ?? debug.enabled - this.isRemovePathLoopsEnabled = opts.isRemovePathLoopsEnabled ?? false - if (this.debug) { - debug.enabled = true - } - - if (debug.enabled) { - this.debugSolutions = {} - this.debugMessage = "" - } - } - - /** - * Return points of interest for this node. Don't worry about checking if - * points are already visited. You must check that these neighbors are valid - * (not inside an obstacle) - * - * In a simple grid, this is just the 4 neighbors surrounding the node. - * - * In ijump-astar, this is the 2-4 surrounding intersections - */ - getNeighbors(node: Node): Array { - return [] - } - - isSameNode(a: Point, b: Point): boolean { - return manDist(a, b) < this.GRID_STEP - } - - /** - * Compute the cost of this path. In normal astar, this is just the length of - * the path, but you can override this term to penalize paths that are more - * complex. - */ - computeG(current: Node, neighbor: Point): number { - return current.g + manDist(current, neighbor) - } - - computeH(node: Point): number { - return manDist(node, this.goalPoint!) - } - - getNodeName(node: Point): string { - return nodeName(node, this.GRID_STEP) - } - - solveOneStep(): { - solved: boolean - current: Node - newNeighbors: Node[] - } { - this.iterations += 1 - const { openSet, closedSet, GRID_STEP, goalPoint } = this - openSet.sort((a, b) => a.f - b.f) - - const current = openSet.shift()! - const goalDist = this.computeH(current) - if (goalDist <= GRID_STEP * 2) { - return { - solved: true, - current, - newNeighbors: [], - } - } - - this.closedSet.add(this.getNodeName(current)) - - let newNeighbors: Node[] = [] - for (const neighbor of this.getNeighbors(current)) { - if (closedSet.has(this.getNodeName(neighbor))) continue - - const tentativeG = this.computeG(current, neighbor) - - const existingNeighbor = this.openSet.find((n) => - this.isSameNode(n, neighbor), - ) - - if (!existingNeighbor || tentativeG < existingNeighbor.g) { - const h = this.computeH(neighbor) - - const f = tentativeG + h * this.GREEDY_MULTIPLIER - - const neighborNode: Node = { - ...neighbor, - g: tentativeG, - h, - f, - obstacleHit: neighbor.obstacleHit ?? undefined, - manDistFromParent: manDist(current, neighbor), // redundant compute... - nodesInPath: current.nodesInPath + 1, - parent: current, - enterMarginCost: neighbor.enterMarginCost, - travelMarginCostFactor: neighbor.travelMarginCostFactor, - } - - openSet.push(neighborNode) - newNeighbors.push(neighborNode) - } - } - - if (debug.enabled) { - openSet.sort((a, b) => a.f - b.f) - this.drawDebugSolution({ current, newNeighbors }) - } - - return { - solved: false, - current, - newNeighbors, - } - } - - getStartNode(connection: SimpleRouteConnection): Node { - return { - x: connection.pointsToConnect[0].x, - y: connection.pointsToConnect[0].y, - manDistFromParent: 0, - f: 0, - g: 0, - h: 0, - nodesInPath: 0, - parent: null, - } - } - - layerToIndex(layer: string): number { - return 0 - } - indexToLayer(index: number): string { - return "top" - } - - /** - * Add a preprocessing step before solving a connection to do adjust points - * based on previous iterations. For example, if a previous connection solved - * for a trace on the same net, you may want to preprocess the connection to - * solve for an easier start and end point - * - * The simplest way to do this is to run getConnectionWithAlternativeGoalBoxes - * with any pcb_traces created by previous iterations - */ - preprocessConnectionBeforeSolving( - connection: SimpleRouteConnection, - ): SimpleRouteConnection { - return connection - } - - solveConnection(connection: SimpleRouteConnection): ConnectionSolveResult { - if (connection.pointsToConnect.length > 2) { - throw new Error( - "GeneralizedAstarAutorouter doesn't currently support 2+ points in a connection", - ) - } - connection = this.preprocessConnectionBeforeSolving(connection) - - const { pointsToConnect } = connection - - this.iterations = 0 - this.closedSet = new Set() - this.startNode = this.getStartNode(connection) - this.goalPoint = { - ...pointsToConnect[pointsToConnect.length - 1], - l: this.layerToIndex(pointsToConnect[pointsToConnect.length - 1].layer), - } - this.openSet = [this.startNode] - - while (this.iterations < this.MAX_ITERATIONS) { - const { solved, current } = this.solveOneStep() - - if (solved) { - let route: PointWithLayer[] = [] - let node: Node | null = current - while (node) { - const l: number | undefined = (node as any).l - route.unshift({ - x: node.x, - y: node.y, - // TODO: this layer should be included as part of the node - layer: - l !== undefined ? this.indexToLayer(l) : pointsToConnect[0].layer, - }) - node = node.parent - } - - if (debug.enabled) { - this.debugMessage += `t${this.debugTraceCount}: ${this.iterations} iterations\n` - } - - if (this.isRemovePathLoopsEnabled) { - route = removePathLoops(route) - } - - return { solved: true, route, connectionName: connection.name } - } - - if (this.openSet.length === 0) { - break - } - } - - if (debug.enabled) { - this.debugMessage += `t${this.debugTraceCount}: ${this.iterations} iterations (failed)\n` - } - - return { solved: false, connectionName: connection.name } - } - - createObstacleList({ - dominantLayer, - connection, - obstaclesFromTraces, - }: { - dominantLayer?: string - connection: SimpleRouteConnection - obstaclesFromTraces: Obstacle[] - }): ObstacleList { - return new ObstacleList( - this.allObstacles - .filter((obstacle) => !obstacle.connectedTo.includes(connection.name)) - // TODO obstacles on different layers should be filtered inside - // the algorithm, not for the entire connection, this is a hack in - // relation to https://github.com/tscircuit/tscircuit/issues/432 - .filter((obstacle) => obstacle.layers.includes(dominantLayer as any)) - .concat(obstaclesFromTraces ?? []), - ) - } - - /** - * Override this to implement smoothing strategies or incorporate new traces - * into a connectivity map - */ - postprocessConnectionSolveResult( - connection: SimpleRouteConnection, - result: ConnectionSolveResult, - ): ConnectionSolveResult { - return result - } - - /** - * By default, this will solve the connections in the order they are given, - * and add obstacles for each successfully solved connection. Override this - * to implement "rip and replace" rerouting strategies. - */ - solve(): ConnectionSolveResult[] { - const solutions: ConnectionSolveResult[] = [] - const obstaclesFromTraces: Obstacle[] = [] - this.debugTraceCount = 0 - for (const connection of this.input.connections) { - const dominantLayer = connection.pointsToConnect[0].layer ?? "top" - this.debugTraceCount += 1 - this.obstacles = this.createObstacleList({ - dominantLayer, - connection, - obstaclesFromTraces, - }) - let result = this.solveConnection(connection) - result = this.postprocessConnectionSolveResult(connection, result) - solutions.push(result) - - if (debug.enabled) { - this.drawDebugTraceObstacles(obstaclesFromTraces) - } - - if (result.solved) { - obstaclesFromTraces.push( - ...getObstaclesFromRoute( - result.route.map((p) => ({ - x: p.x, - y: p.y, - layer: p.layer ?? dominantLayer, - })), - connection.name, - ), - ) - } - } - - return solutions - } - - solveAndMapToTraces(): SimplifiedPcbTrace[] { - const solutions = this.solve() - - return solutions.flatMap((solution): SimplifiedPcbTrace[] => { - if (!solution.solved) return [] - return [ - { - type: "pcb_trace" as const, - pcb_trace_id: `pcb_trace_for_${solution.connectionName}`, - route: addViasWhenLayerChanges( - solution.route.map((point) => ({ - route_type: "wire" as const, - x: point.x, - y: point.y, - width: this.input.minTraceWidth, - layer: point.layer as LayerRef, - })), - ), - }, - ] - }) - } - - getDebugGroup(): string | null { - const dgn = `t${this.debugTraceCount}_iter[${this.iterations - 1}]` - if (this.iterations < 30) return dgn - if (this.iterations < 100 && this.iterations % 10 === 0) return dgn - if (this.iterations < 1000 && this.iterations % 100 === 0) return dgn - if (!this.debugSolutions) return dgn - return null - } - - drawDebugTraceObstacles(obstacles: Obstacle[]) { - const { debugTraceCount, debugSolutions } = this - for (const key in debugSolutions) { - if (key.startsWith(`t${debugTraceCount}_`)) { - debugSolutions[key].push( - ...obstacles.map( - (obstacle, i) => - ({ - type: "pcb_smtpad", - pcb_component_id: "", - layer: obstacle.layers[0], - width: obstacle.width, - shape: "rect", - x: obstacle.center.x, - y: obstacle.center.y, - pcb_smtpad_id: `trace_obstacle_${i}`, - height: obstacle.height, - }) as PCBSMTPad, - ), - ) - } - } - } - - drawDebugSolution({ - current, - newNeighbors, - }: { - current: Node - newNeighbors: Node[] - }) { - const debugGroup = this.getDebugGroup() - if (!debugGroup) return - - const { openSet, debugTraceCount, debugSolutions } = this - - debugSolutions![debugGroup] ??= [] - const debugSolution = debugSolutions![debugGroup]! - - debugSolution.push({ - type: "pcb_fabrication_note_text", - pcb_fabrication_note_text_id: `debug_note_${current.x}_${current.y}`, - font: "tscircuit2024", - font_size: 0.25, - text: "X" + (current.l !== undefined ? current.l : ""), - pcb_component_id: "", - layer: "top", - anchor_position: { - x: current.x, - y: current.y, - }, - anchor_alignment: "center", - }) - // Add all the openSet as small diamonds - for (let i = 0; i < openSet.length; i++) { - const node = openSet[i] - debugSolution.push({ - type: "pcb_fabrication_note_path", - pcb_component_id: "", - pcb_fabrication_note_path_id: `note_path_${node.x}_${node.y}`, - layer: "top", - route: [ - [0, 0.05], - [0.05, 0], - [0, -0.05], - [-0.05, 0], - [0, 0.05], - ].map(([dx, dy]) => ({ - x: node.x + dx, - y: node.y + dy, - })), - stroke_width: 0.01, - }) - // Add text that indicates the order of this point - debugSolution.push({ - type: "pcb_fabrication_note_text", - pcb_fabrication_note_text_id: `debug_note_${node.x}_${node.y}`, - font: "tscircuit2024", - font_size: 0.03, - text: i.toString(), - pcb_component_id: "", - layer: "top", - anchor_position: { - x: node.x, - y: node.y, - }, - anchor_alignment: "center", - }) - } - - if (current.parent) { - const path: Node[] = [] - let p: Node | null = current - while (p) { - path.unshift(p) - p = p.parent - } - debugSolution!.push({ - type: "pcb_fabrication_note_path", - pcb_component_id: "", - pcb_fabrication_note_path_id: `note_path_${current.x}_${current.y}`, - layer: "top", - route: path, - stroke_width: 0.01, - }) - } - } -} +export * from "algos/common/generalized-astar/GeneralizedAstarAutorouter" diff --git a/algos/infinite-grid-ijump-astar/v2/lib/types.ts b/algos/infinite-grid-ijump-astar/v2/lib/types.ts index 0ce70ff..6c3fbaf 100644 --- a/algos/infinite-grid-ijump-astar/v2/lib/types.ts +++ b/algos/infinite-grid-ijump-astar/v2/lib/types.ts @@ -1,66 +1 @@ -import type { Obstacle } from "autorouting-dataset/lib/types" - -export interface DirectionDistances { - left: number - top: number - bottom: number - right: number -} - -export interface Direction { - dx: number - dy: number -} - -export interface DirectionWithWallDistance extends Direction { - wallDistance: number -} - -export interface DirectionWithCollisionInfo extends Direction { - wallDistance: number - obstacle: Obstacle | null -} - -export interface Point { - x: number - y: number -} - -export interface PointWithObstacleHit extends Point { - obstacleHit?: Obstacle | null - - /** - * Used in multi-margin autorouter to penalize traveling close to the wall - */ - travelMarginCostFactor?: number - enterMarginCost?: number -} - -export interface PointWithWallDistance extends Point { - wallDistance: number -} - -export interface Node extends Point { - /** Distance from the parent node (along path) */ - g: number - /** Heuristic distance from the goal */ - h: number - /** Distance score for this node (g + h) */ - f: number - /** Manhattan Distance from the parent node */ - manDistFromParent: number - nodesInPath: number - obstacleHit?: Obstacle - parent: Node | null - - /** - * Used in multi-margin autorouter to penalize traveling close to the wall - */ - travelMarginCostFactor?: number - enterMarginCost?: number - - /** - * Layer index, not needed for single-layer autorouters - */ - l?: number -} +export * from "algos/common/generalized-astar/types" diff --git a/algos/infinite-grid-ijump-astar/v2/lib/util.ts b/algos/infinite-grid-ijump-astar/v2/lib/util.ts index 766eb26..43e454f 100644 --- a/algos/infinite-grid-ijump-astar/v2/lib/util.ts +++ b/algos/infinite-grid-ijump-astar/v2/lib/util.ts @@ -24,10 +24,7 @@ export const dirFromAToB = (a: Point, b: Point): { dx: number; dy: number } => { } export const distAlongDir = (A: Point, B: Point, dir: Direction): number => { - return ( - Math.abs(A.x - B.x) * Math.abs(dir.dx) + - Math.abs(A.y - B.y) * Math.abs(dir.dy) - ) + return Math.abs((A.x - B.x) * dir.dx) + Math.abs((A.y - B.y) * dir.dy) } export const nodeName = (node: Point, GRID_STEP: number = 0.1): string => diff --git a/algos/multi-layer-ijump/MultilayerIjump.ts b/algos/multi-layer-ijump/MultilayerIjump.ts index 60a4350..c3efb7b 100644 --- a/algos/multi-layer-ijump/MultilayerIjump.ts +++ b/algos/multi-layer-ijump/MultilayerIjump.ts @@ -26,7 +26,7 @@ import type { SimpleRouteConnection, SimpleRouteJson, } from "autorouting-dataset/lib/solver-utils/SimpleRouteJson" -import { ObstacleList3d } from "./ObstacleList3d" +import { ObstacleList3d } from "algos/common/generalized-astar/ObstacleList3d" import type { Obstacle } from "autorouting-dataset/lib/types" import { PcbConnectivityMap, @@ -39,6 +39,11 @@ import { getAlternativeGoalBoxes, getConnectionWithAlternativeGoalBoxes, } from "autorouting-dataset/lib/solver-utils/getAlternativeGoalBoxes" +import { ObstacleList3dSectional } from "algos/common/generalized-astar/ObstacleList3dSectional" +import { ObstacleList3dF64V1 } from "algos/common/generalized-astar/ObstacleList3dF64V1" +import { ObstacleList3dF64V2 } from "algos/common/generalized-astar/ObstacleList3dF64V2" +import { ObstacleList3dF64V3 } from "algos/common/generalized-astar/ObstacleList3dF64V3" +import { ObstacleList3dSectionalV2 } from "algos/common/generalized-astar/ObstacleList3dSectionalV2" export class MultilayerIjump extends GeneralizedAstarAutorouter { MAX_ITERATIONS: number = 500 @@ -62,6 +67,8 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { GOAL_RUSH_FACTOR: number = 1.1 + cachedMarginOption: { margin: number } + // TODO we need to travel far enough away from the goal so that we're not // hitting a pad, which means we need to know the bounds of the goal // The simplest way to do this is to change SimpleJsonInput to include a @@ -122,6 +129,25 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { travelCostFactor: 2, }, ] + this.cachedMarginOption = { + margin: this.OBSTACLE_MARGIN, + } + + if (this.profiler) { + for (const methodName of [ + "preprocessConnectionBeforeSolving", + "hasSpaceForVia", + "getNeighborsSurroundingGoal", + ]) { + // @ts-ignore + const originalMethod = this[methodName] as Function + // @ts-ignore + this[methodName as keyof ObstacleList] = this.profiler!.wrapMethod( + `GenerlizedAstarAutorouter.${methodName}`, + originalMethod.bind(this), + ) + } + } } preprocessConnectionBeforeSolving( @@ -189,7 +215,9 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { ) } - return new ObstacleList3d( + // return new ObstacleList3dSectionalV2( + return new ObstacleList3dF64V3( + // return new ObstacleList3d( this.layerCount, this.allObstacles .filter((obstacle) => !obstacle.connectedTo.includes(bestConnectionId)) @@ -302,6 +330,7 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { forwardDir = dirFromAToB(node.parent, node) } + this.profiler?.startMeasurement("getNeighbors.travelDirs1") /** * Get the possible next directions (excluding backwards direction), and * excluding the forward direction if we just ran into a wall @@ -330,7 +359,9 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { travelDirs1.push({ dx: 0, dy: 0, dl: -1 }) } } + this.profiler?.endMeasurement("getNeighbors.travelDirs1") + this.profiler?.startMeasurement("getNeighbors.travelDirs2") const travelDirs2 = travelDirs1 .filter((dir) => { // If we have a parent, don't go backwards towards the parent @@ -354,16 +385,16 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { const collisionInfo = obstacles.getOrthoDirectionCollisionInfo( node, dir, - { - margin: this.OBSTACLE_MARGIN, - }, + this.cachedMarginOption, ) return collisionInfo }) // Filter out directions that are too close to the wall .filter((dir) => !(dir.wallDistance < this.OBSTACLE_MARGIN)) + this.profiler?.endMeasurement("getNeighbors.travelDirs2") + this.profiler?.startMeasurement("getNeighbors.travelDirs3") /** * Figure out how far to travel. There are a couple reasons we would stop * traveling: @@ -424,6 +455,7 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { let overcomeDistance: number | null = null if (node?.obstacleHit) { + this.profiler?.startMeasurement("getNeighbors.overcomeDistance") overcomeDistance = getDistanceToOvercomeObstacle({ node, travelDir, @@ -433,6 +465,7 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { OBSTACLE_MARGIN: this.OBSTACLE_MARGIN, SHOULD_DETECT_CONJOINED_OBSTACLES: true, }) + this.profiler?.endMeasurement("getNeighbors.overcomeDistance") } const goalDistAlongTravelDir = distAlongDir(node, goalPoint, travelDir) @@ -516,6 +549,7 @@ export class MultilayerIjump extends GeneralizedAstarAutorouter { } } } + this.profiler?.endMeasurement("getNeighbors.travelDirs3") return travelDirs3.map((dir) => ({ x: node.x + dir.dx * dir.travelDistance, diff --git a/algos/seveibar/seveibar1/index.ts b/algos/seveibar/seveibar1/index.ts new file mode 100644 index 0000000..9f2de65 --- /dev/null +++ b/algos/seveibar/seveibar1/index.ts @@ -0,0 +1,25 @@ +import { GeneralizedAstarAutorouter } from "algos/common/generalized-astar/GeneralizedAstarAutorouter" +import { ObstacleList } from "algos/common/generalized-astar/ObstacleList" +import type { + PointWithObstacleHit, + Node, +} from "algos/common/generalized-astar/types" +import type { Obstacle } from "solver-utils" +import type { SimpleRouteConnection } from "solver-utils/SimpleRouteJson" + +/** + * seveibar1 is a variation of the multi-layer ijump autorouter that uses more + * efficient memory access and tries to avoid some slow paths in the original + * multi-layer autorouter. + * + * This is created by profiling the operations of each of the major components + * of the original MultilayerIjump autorouter and optimizing each component + * independently. + * + * There are three main time-consuming components of the autorouter: + * - GeneralizedAstarAutorouter (the base class that determines what point + * is highest in the priority queue) + * - ObstacleList + * - getNeighbors + */ +export class Seveibar1 extends GeneralizedAstarAutorouter {} diff --git a/algos/seveibar/seveibar1/tests/__snapshots__/keyboard-route-test-profile.snap.svg b/algos/seveibar/seveibar1/tests/__snapshots__/keyboard-route-test-profile.snap.svg new file mode 100644 index 0000000..d81b6f3 --- /dev/null +++ b/algos/seveibar/seveibar1/tests/__snapshots__/keyboard-route-test-profile.snap.svg @@ -0,0 +1,13 @@ + \ No newline at end of file diff --git a/algos/seveibar/seveibar1/tests/keyboard-route-test-profile.test.tsx b/algos/seveibar/seveibar1/tests/keyboard-route-test-profile.test.tsx new file mode 100644 index 0000000..f181f65 --- /dev/null +++ b/algos/seveibar/seveibar1/tests/keyboard-route-test-profile.test.tsx @@ -0,0 +1,48 @@ +import { getKeyboardGenerator } from "autorouting-dataset/lib/generators/keyboards" +import { expect, test } from "bun:test" +import { getFullConnectivityMapFromCircuitJson } from "circuit-json-to-connectivity-map" +import { convertCircuitJsonToPcbSvg } from "circuit-to-svg" +import { getSimpleRouteJson } from "solver-utils" +import { MultilayerIjump } from "algos/multi-layer-ijump/MultilayerIjump" + +test("multi-layer ijump keyboard", async () => { + const durations: Array<[number, number]> = [] + let soup: any + let autorouter: MultilayerIjump = null as any + let result: any + for (let i = 0; i < 20; i++) { + soup = await getKeyboardGenerator().getExample({ seed: 7 + i }) + const connMap = getFullConnectivityMapFromCircuitJson(soup) + const input = getSimpleRouteJson(soup, { layerCount: 2, connMap }) + + autorouter = new MultilayerIjump({ + input, + connMap, + optimizeWithGoalBoxes: true, + }) + + const start = process.hrtime() + result = autorouter.solveAndMapToTraces() + const duration = process.hrtime(start) + durations.push(duration) + } + + const totalDuration = durations.reduce( + (acc, duration) => { + return [acc[0] + duration[0], acc[1] + duration[1]] + }, + [0, 0], + ) + console.log("\n\n-----------------------------\n\n") + console.log( + `TIME TO ROUTE: ${totalDuration[0] * 1000 + totalDuration[1] / 1e6}ms`, + ) + console.table(autorouter.obstacles.profiler!.getResultsPretty()) + console.table(autorouter.profiler!.getResultsPretty()) + + // console.log(autorouter.obstacles.cases) + + expect( + convertCircuitJsonToPcbSvg(soup.concat(result as any) as any), + ).toMatchSvgSnapshot(import.meta.path) +}) diff --git a/module/lib/solver-utils/getAlternativeGoalBoxes.ts b/module/lib/solver-utils/getAlternativeGoalBoxes.ts index b0fe34b..6f479d5 100644 --- a/module/lib/solver-utils/getAlternativeGoalBoxes.ts +++ b/module/lib/solver-utils/getAlternativeGoalBoxes.ts @@ -40,6 +40,13 @@ export function getAlternativeGoalBoxes(params: { })) } +/** + * Takes a connection and a connectivity map, then swaps the pointsToConnect + * with more optimal points. + * + * For example, we may see there is an easier or closer way to connect two + * points because of a trace that has already been routed. + */ export const getConnectionWithAlternativeGoalBoxes = (params: { connection: SimpleRouteConnection pcbConnMap: PcbConnectivityMap