経路探索アルゴリズムA*(エースター)を参考にGauche で書いてみました。
(use srfi-1) ;; List library (use srfi-43) ;; ベクタライブラリ (use gauche.interactive) (use slib) (require 'trace) (define maze-string (string-append "**************************\n" "*S* * *\n" "* * * * ************* *\n" "* * * ************ *\n" "* * *\n" "************** ***********\n" "* *\n" "** ***********************\n" "* * G *\n" "* * *********** * *\n" "* * ******* * *\n" "* * *\n" "**************************")) (define map-vect (list->vector (map list->vector (map string->list (string-split maze-string "\n"))))) ;; ===================================== ;; ノードクラス ;; ===================================== (define-class <node> () ((c :init-value #\null :init-keyword :c :accessor char) (x :init-value 0 :init-keyword :x :accessor x-of) (y :init-value 0 :init-keyword :y :accessor y-of) (f-star :init-value 0 :init-keyword :f-star :accessor f-star) (parent :init-value #f :init-keyword :parent :accessor parent))) (define-method wall? ((nd <node>)) (eq? (char nd) #\*)) (define-method gall? ((nd <node>)) (eq? (char nd) #\G)) (define-method start? ((nd <node>)) (eq? (char nd) #\S)) (define-method x-set! ((nd <node>) pos) (set! (x-of nd) pos)) (define-method fstar-set! ((nd <node>) value) (set! (f-star nd) value)) (define-method parent-set! ((nd <node>) p) (set! (parent nd) p)) (define-method char-set! ((nd <node>) c) (set! (char nd) c)) (define-method cost ((nd <node>) destination) ; 縦の距離と横の距離を加算 (+ (abs (- (x-of nd)(x-of destination))) (abs (- (y-of nd)(y-of destination))))) (define-method g-star ((nd <node>)) ; スタート -> n 推定値 f*(n) (- (f-star *start*) (h-star nd))) (define-method h-star ((nd <node>)) (cost nd *gall*)) ; n -> ゴールまでの 推定値 h*(n) (define-method schar ((nd <node>)) (string-append " " (string (char nd)))) (define-method string-fstar ((nd <node>)) (if (wall? nd) " ." (format #f "~2d" (f-star nd)))) (define-method show ((nd <node>)) (format #f "[~s x:~d y:~d f:~d p:~s]" (string (char nd)) (x-of nd) (y-of nd)(f-star nd) (let ((p (parent nd))) (if (eq? #f p) "<>" (format #f "<x:~d y:~d>" (x-of p) (y-of p)))))) ;; f_dash = g_star(m) + h_star(m) + cost(n,m) (define-method f-dash ((nd <node>) destination) (+ (g-star destination) (h-star destination) (cost nd destination))) ;; -->ノードクラス (define (show-node ls) (string-join (map show ls))) (define (make-node char x y f-star parent) (make <node> :c char :x x :y y :f-star f-star :parent parent)) ;; 文字の入った2次元ベクタから、 node オブジェクトのベクタマップを作って返す。 (define (node-vect map-v) (vector-map (lambda(y vect) ;; y 座標 行 (vector-map (lambda(x char)(make-node char x y 0 #f)) vect)) map-v)) ;; マップベクタ (define *vmap* (node-vect map-vect)) (define (get-node x y) (vector-ref (vector-ref *vmap* y) x)) (define *mp-height* (vector-length *vmap*)) (define *mp-width* (vector-length (vector-ref *vmap* 0))) ;; スタート、ゴール位置 (define *start* #f) (define *gall* #f) (vector-for-each (lambda(y vect) (vector-for-each (lambda(x node) (if (start? node)(set! *start* node)) (if (gall? node) (set! *gall* node))) vect)) *vmap*) (define openlist '()) ; 計算中のノード (define closelist '()) ; 計算済みのノード ;; 枠外・壁でなければノードのリストを返す。枠外・壁は'() (define (adjacent chk-pred x y) (let* ((node (get-node x y))) (if (and (chk-pred x y) (not (wall? node))) (cons node '()) '()))) ;; 縦横に隣接しているノードのリストを返す (define (adjacent_nodes x y) (let* ((out1 (adjacent (lambda(x y)(>= y 0)) x (- y 1))) ; 上 (out2 (adjacent (lambda(x y)(< y *mp-height*)) x (+ y 1) )) ; 下 (out3 (adjacent (lambda(x y)(>= x 0)) (- x 1) y)) ; 左 (out4 (adjacent (lambda(x y)(< x *mp-width*)) (+ x 1) y)) ; 右 (out (append out1 out2 out3 out4))) ;;(print ;; (string-append ;; "x:" (number->string x)" y:" (number->string y) (show-node out))) out)) ;; ノードにprocを摘要したマップを表示 (define (show-map proc) (let ((lmap (map vector->list (vector->list *vmap*)))) (map (lambda(ls) (print (string-join (map (lambda(x)(proc x)) ls)))) lmap))) ;; 経路更新 ・f-star を f-dashに置き換え、親を更新 (define (fstar-parent-set! fdash node next-node) (begin (fstar-set! next-node fdash) (parent-set! next-node node))) (define (update-if node fdash next-node) (if (< fdash (f-star next-node)) (fstar-parent-set! fdash node next-node))) ;; リストに追加 (define (push-elem ls elem) (append ls (cons elem '()))) ;; n に隣接している全てのノード（ここでは隣接ノードを m とおく）に対して以下の操作を行う (define (exec-next-node node adjoining-ls openlist closelist) (begin ;;(print "node :" (show node) "\n" ;; "next :" (string-join (map show adjoining-ls)) "\n" ;; "open :" (string-join (map show openlist)) "\n" ;; "close:" (string-join (map show closelist))) ;; ;;(print "-hit enter key-")(let ((char (read-char))) #f) ;;(sys-nanosleep 300000000);wait for 0.3 sec (if (> (length adjoining-ls) 0) ; 隣接ノード (let* ((next-node (car adjoining-ls)) (cdr-adjoining (cdr adjoining-ls)) (fdash (f-dash node next-node))) (cond ((find (lambda(x)(equal? x next-node)) openlist) (begin (update-if node fdash next-node) ;f-star、親を更新 (exec-next-node node cdr-adjoining openlist closelist))) ((find (lambda(x)(equal? x next-node)) closelist) (begin (if (< fdash (f-star next-node)) (begin (fstar-parent-set! fdash node next-node) (exec-next-node node cdr-adjoining (push-elem openlist next-node) (remove (lambda(x)(eq? x next-node)) closelist))) (exec-next-node node cdr-adjoining openlist closelist)))) (else (begin (fstar-parent-set! fdash node next-node) (exec-next-node node cdr-adjoining (push-elem openlist next-node) closelist))))) (cons openlist (cons closelist '()))))) ;; 経路にマーク (define (write_route node) (if (start? node) #f (begin ;; (print (show node)) (unless (gall? node )(char-set! node #\$)) (write_route (parent node))))) ;; 経路探索 (define (search_route openlist closelist) (call/cc (lambda (return) (if (> (length openlist) 0) (let* ((sorted-openlist (sort openlist (lambda (x y) (< (f-star x) (f-star y))))) (min-f-star (car sorted-openlist))) ; Openリスト中 f-star 最小 (let ((ret (exec-next-node min-f-star (adjacent_nodes (x-of min-f-star)(y-of min-f-star)) ; 隣接ノード (cdr sorted-openlist) (if (gall? min-f-star) (begin ;; (show-map string-fstar) (write_route *gall*) ;; 親をたどってマーク (show-map schar) (return "gall")) (push-elem closelist min-f-star))))) (search_route (car ret) (car(cdr ret)))) ) (return "gall"))))) ;; スタート S の最短経路コスト推定値 f*(S) ;; g*(S) = 0 (スタートから n) ;; f*(S) = g*(S) + h*(S) ---> f*(S) = h*(S) (fstar-set! *start* (h-star *start*)) ;;(trace search_route) (search_route (append openlist (list *start*)) closelist)
