Cost of nodes are represented by a matrix (called world), heuristic cost estimate/g score/f score are all represented in matrices.
The world and its size are stored in global dynamic variables. It looks like this:
1 1 1 1 1 1999 1999 1999 1999 1 1 1 1 1 1 1 1999 1999 1999 1999 1 1 1 1 1
So the shortest path should be in a shape like the number 2.
(ns typedclj.rhizome
(:require [flatland.ordered.set :as fos]))
(use 'clojure.pprint)
(defmacro epprint [expr]
`(do (pprint '~expr)
(pprint ~expr)))
(defmacro epprints [& exprs]
(list* 'do (map (fn [x] (list 'epprint x))
exprs)))
(epprints (inc 1) (inc 2))
(def world [[1 1 1 1 1]
[1999 1999 1999 1999 1]
[1 1 1 1 1]
[1 1999 1999 1999 1999]
[1 1 1 1 1]])
(def ^:dynamic *world-size* 5)
(def ^:dynamic *step-cost* 100)
;(alter-var-root #'*world-size* (constantly 5))
(defn neighbors
([yx]
(neighbors [[-1 0] [1 0] [0 -1] [0 1]] *world-size* yx))
([deltas *world-size* yx]
(filter (fn [new-yx] (every? #(< -1 % *world-size*)
new-yx))
(map #(vec (map + yx %)) deltas))))
(defn heuristic-cost-estimate [[y x]]
(* *step-cost*
(- (+ *world-size* *world-size*) y x 2)))
(defn mapmat [f mat]
(mapv (fn [row]
(mapv f row))
mat))
(defn mapmats [f & mats]
(apply mapv (fn [& rows]
(apply mapv f rows))
mats))
(defn randmat []
(repeatedly *world-size*
(fn [] (repeatedly
*world-size*
#(rand-int 10)))))
(defn min-by [f coll]
(when (seq coll)
(reduce (fn [min this]
(if (> (f min) (f this)) this min))
coll)))
(pprint (randmat))
(let [m1 (randmat)
m2 (randmat)
m3 (randmat)]
(epprints m1 m2 m3)
(mapmats + m1 m2 m3))
(defn constant-matrix [c]
(vec (repeat *world-size*
(vec (repeat *world-size* c)))))
(defn construct-path
[came-from]
(loop [path [[4 4]]]
(let [t (peek path)]
(if (= t [0 0])
path
(recur (conj path (get-in came-from t)))))))
(def astart []
(let [coords (for [i (range *world-size*)]
(for [j (range *world-size*)]
[i j]))
h-score (mapmat heuristic-cost-estimate coords)
start [0 0]
goal-y (dec *world-size*)
goal-x goal-y
goal [goal-x goal-y]]
(loop [closedset (fos/ordered-set)
openset (fos/ordered-set [0 0])
came-from (constant-matrix [nil nil])
g-score (assoc-in (constant-matrix 1e8) start 0)
f-score (mapmats + g-score h-score)
]
(let [
current (min-by #(get-in f-score %) openset)
openset (disj openset current)
closedset (conj closedset current)
nbrs (filter (complement closedset)
(neighbors current))
openset (union openset (set nbrs))
]
(if (= current goal)
(construct-path came-from)
(if (empty? openset)
false
(let [[came-from g-score f-score]
(reduce (fn [[cf gs fs :as to-be-updated] nbr]
(let [current-g-score (get-in g-score current)
nbr-g-score (get-in g-score nbr)
nbr-cost (get-in world nbr)
tentative-g-score (+ current-g-score
nbr-cost)]
(if (>= tentative-g-score nbr-g-score)
to-be-updated
[(assoc-in cf nbr current)
(assoc-in gs nbr tentative-g-score)
(assoc-in fs nbr (+ tentative-g-score
(get-in h-score nbr)))])))
[came-from g-score f-score]
nbrs)]
(epprints nbrs
current
goal
closedset
openset
came-from
g-score
f-score)
(recur closedset openset came-from g-score f-score))))))))
Any suggestion for improvement is welcome.
