A* Algorithm implementation for el

astar.el

;; astar.el -- A* Algorithm implementation

(require 'cl)
 
(defstruct (astar-node-struct
            (:constructor astar-make-node-struct
                          (&key pair parent f-star)))
  pair parent f-star)

(defun astar-node-struct-pair-pair-equal (node pair)
  (equal (astar-node-struct-pair node) pair))
 
(defvar astar-buffer "*astar*")
(defvar astar-start-mark ?S)
(defvar astar-goal-mark ?G)
(defvar astar-wall-mark ?*)
(defvar astar-route-mark ?$)

(defun astar-get-point-pair (&optional pos)
  "現在の座標を返す。"
  (let ((pos (or pos (point))))
    (save-excursion
      (goto-char (point))
      (cons (current-column) (- (count-lines (point-min) pos) 1)))))
 
(defun astar-get-start-mark-pair ()
  "スタートの座標"
  (save-excursion
    (beginning-of-buffer)
    (re-search-forward (char-to-string astar-start-mark))
    (backward-char)
    (astar-get-point-pair)))
 
(defun astar-get-goal-mark-pair ()
  "ゴールの座標"
  (save-excursion
    (beginning-of-buffer)
    (re-search-forward (char-to-string astar-goal-mark))
    (backward-char)
    (astar-get-point-pair)))

(defun astar-get-load-pair-or-nil ()
  "もし現在の座標が通路の場合、座標を返す。それ以外の場合はnil。"
  (if (eq (char-after) astar-wall-mark)
      nil
    (astar-get-point-pair)))

(defun astar-get-adjoining-load-pairs (n)
  "隣接した通路の座標を返す。"
  (let* ((n (astar-node-struct-pair n))
         (x (car n))
         (y (1+ (cdr n)))
         pairs)
    (save-excursion
      (goto-line y)
      (move-to-column x)
      (dolist (horizontal-move-function '(forward-char backward-char))
        (save-excursion
          (funcall horizontal-move-function)
          (let ((pair (astar-get-load-pair-or-nil)))
            (when pair (setq pairs (cons pair pairs))))))
      (dolist (vertical-move-function '((lambda () (forward-line 1)) (lambda () (forward-line -1))))
        (save-excursion
          (funcall vertical-move-function)
          (move-to-column x)
          (let ((pair (astar-get-load-pair-or-nil)))
            (when pair (setq pairs (cons pair pairs))))))
      pairs)))


(defun astar-calc-interval (a b)
  "座標間距離"
  (let ((dx (abs (- (car a) (car b))))
        (dy (abs (- (cdr a) (cdr b)))))
    ;;(sqrt (+ (* dx dx) (* dy dy)))
    (+ dx dy)))

(defun astar-calc-cost (n m)
  "隣接した通路への移動コスト"
  (astar-calc-interval (astar-node-struct-pair n) m))

(defun astar-calc-h-star (m goal)
  "現在位置からゴールまでの距離"
  (astar-calc-interval m goal))

(defun astar-calc-g-star (n goal)
  "スタートから現在位置までの距離"
  (let ((f-star (astar-node-struct-f-star n))
        (h-star (astar-calc-h-star (astar-node-struct-pair n) goal)))
    (- f-star h-star)))

(defun astar-calc-f-dash (n m goal)
  "mをとおるゴールまでの推定距離"
  (let* ((g-star (astar-calc-g-star n goal))
         (h-star (astar-calc-h-star m goal))
         (cost   (astar-calc-cost n m))
         (f-dash (+ g-star h-star cost)))
    f-dash))
  

(defun astar-search-same-coordinate-node (pair open-list)
  "同じ座標のノードを取り出す"
  (find-if (lambda (n) (astar-node-struct-pair-pair-equal n pair))
           open-list))

(defun astar-solve-ready-to-next-starge (goal open-list close-list n)
  (let ((open-list (remove n open-list))
        (close-list (cons n close-list)))
    (dolist (m (astar-get-adjoining-load-pairs n))
      (let* ((f-dash (astar-calc-f-dash n m goal))
             (om (astar-search-same-coordinate-node m open-list))
             (cm (astar-search-same-coordinate-node m close-list)))
        (when (and om (< f-dash (astar-node-struct-f-star om)))
          (setq open-list
                (cons (astar-make-node-struct :pair m :parent n :f-star f-dash)
                      (remove om open-list))))
        (when (and cm (< f-dash (astar-node-struct-f-star cm)))
          (setq open-list
                (cons (astar-make-node-struct :pair m :parent n :f-star f-dash)
                      open-list))
          (setq close-list
                (remove cm open-list)))
        (when (not (or om cm))
          (setq open-list
                (cons (astar-make-node-struct :pair m :parent n :f-star f-dash)
                      open-list)))))
    (cons open-list close-list)))

(defun astar-search-minimum-f-star-node (open-list)
  "最小の推定距離のノードを取り出す"
  (car (sort open-list
             (lambda (a b) (< (astar-node-struct-f-star a) (astar-node-struct-f-star b))))))

 (defun astar-solve-search (goal open-list close-list)
   (let ((route nil))
     (block nil
       (while 1
         (when (null open-list)
           (return))
         (let ((n (astar-search-minimum-f-star-node open-list)))
           (when (astar-node-struct-pair-pair-equal n goal)
             (setq route n)
             (return))
           (let ((lists (astar-solve-ready-to-next-starge goal open-list close-list n)))
             (setq open-list (car lists))
             (setq close-list (cdr lists))))))
     route))

(defun astar-solve-inner ()
  (let* ((start (astar-get-start-mark-pair))
         (goal (astar-get-goal-mark-pair))
         (f-star (astar-calc-h-star start goal))
         (open-list (list (astar-make-node-struct :pair start :parent nil :f-star f-star)))
         (close-list '()))
    (astar-solve-search goal open-list close-list)))

(defun astar-node-to-pairs (node)
  (let ((result nil))
    (while node
      (setq result (cons (astar-node-struct-pair node) result))
      (setq node (astar-node-struct-parent node)))
    (reverse result)))

(defun aster-draw-route (result)
  (dolist (pair result)
    (goto-line (1+ (cdr pair)))
    (move-to-column (car pair))
    (when (and (not (eq (char-after) astar-start-mark))
               (not (eq (char-after) astar-goal-mark)))
      (delete-char 1)
      (insert-char astar-route-mark 1))
    ))

(defun astar-solve (filename)
  (interactive "finput file: ")
  (with-current-buffer (get-buffer-create astar-buffer)
    (switch-to-buffer (current-buffer))
    (erase-buffer)
    (insert-file-contents filename)
    (cond ((yes-or-no-p "solve?: ")
           (let ((result (astar-node-to-pairs (astar-solve-inner))))
             (if (not result)
                 (message "route not found.")
               (aster-draw-route result))))
          (t
           (message "nothing to do.")))))

;;(astar-solve "data.txt")
;;(astar-solve "data2.txt")

data.txt

**************************
*S* *                    *
* * *  *  *************  *
* *   *    ************  *
*    *                   *
************** ***********
*                        *
** ***********************
*      *              G  *
*  *      *********** *  *
*    *        ******* *  *
*       *                *
**************************

data2.txt

**************************
*S* *                    *
* * *  *  *************  *
* *   *    ************  *
*    *                   *
************** ***********
*                        *
** ***********************
*      *              G  *
*  *    ************* *  *
*    *        ******* *  *
*       *                *
**************************