nycto / astarnim Goto Github PK

import strformat
import astar


const
  Up* = (x: 0, y: 1, z: 0)
  Down* = (x: 0, y: -1, z: 0)
  West* = (x: -1, y: 0, z: 0)
  East* = (x: 1, y: 0, z: 0)
  South* = (x: 0, y: 0, z: 1)
  North* = (x: 0, y: 0, z: -1)

  directions* = [Up, Down, West, East, South, North]

type GNode* = object
  x: int
  y: int
  z: int

type Grid* = seq[seq[seq[GNode]]]

proc `$`(g: Grid): string =
  let h = g.len
  let l = g[0].len
  let w = g[0][0].len
  result = "({w=}, {h=}, {l=})".fmt

proc `$`(gn: GNode): string = "({gn.x=}, {gn.y=}, {gn.z=})".fmt

proc newGrid(): Grid =
  result = @[]
  for y in 0..4:
    var plane = newSeq[seq[GNode]]()
    for z in 0..4:
      var row = newSeq[Gnode]()
      for x in 0..4:
        let gnode = GNode(x: x, y: y, z: z)
        row.add(gnode)
      plane.add(row)
    result.add(plane)

iterator neighbors*(g: Grid, n: GNode): Gnode =
  for dir in directions:
    let x = n.x + dir.x
    let y = n.y + dir.y
    let z = n.z + dir.z
    let h = g.len
    let l = g[0].len
    let w = g[0][0].len
    if (y in 0..<h):
      if (z in 0..<l):
        if (x in 0..<w):
          yield g[y][z][x]

proc cost*(g: Grid, a, b: GNode): int = 1

proc heuristic*(g: Grid, next, goal: GNode): int = abs(goal.x - next.x) + abs(goal.y - next.y) + abs(goal.z - next.z)

when isMainModule:
  let g: Grid = newGrid()
  echo g

  let start: GNode = g[0][0][0]
  let goal: GNode = g[4][0][2]
  for n in g.neighbors(start):
    echo n, ", cost: ", g.cost(start, n), ", heuristic: ", g.heuristic(n, goal)

  
  for p in astar.path[Grid, GNode, int](g, start, goal):
    let gn: GNode = p
    echo gn