View of /lisp/tools/dist.l

Parent Directory

Revision Log

Initial revision

;; ------------------------------------------- ;;
;; dist.l (distance between point, line, ... ) ;;
;; ------------------------------------------- ;;

(defun distance-points (a b)
  (lets ((x0 (first a)) (y0 (second a))
         (x1 (first b)) (y1 (second b))
         (dx (difference x1 x0)) (dy (difference y1 y0)))
    (fix (plus (times dx dx) (times dy dy)))))

(defun distance-point-line (p l)
  (lets ((A (first l))
	 (B (second l))
	 (PA (sqrt (float (distance-points p A))))
	 (PB (sqrt (float (distance-points p B))))
	 (AB (sqrt (float (distance-points A B))))
	 (d  (fix (difference (plus PA PB) AB))))
    (* d d)))

(comment ;; old version
 ;; -------------------------------- ;;
 ;; distance overflow integer range! ;;
 ;; -------------------------------- ;;
 ;; ------------------------------------------------- ;;
 ;; (discance-pointe-line (10 20) ((30 40) (50 60)))  ;;
 ;; line: dy*(x-x0) = dx*(y-y0)                       ;;
 ;;  i.e. dy*x - dx*y + (-dy*x0 + dx*y0) = 0          ;;
 ;;        ax  +  by  + c                = 0          ;;
 ;; ------------------------------------------------- ;;
 (defun distance-point-line (p l)
   (lets ((x  (float (car p)))    (y  (float (cadr p)))
	  (x0 (float (caar l)))   (y0 (float (cadar l)))
	  (x1 (float (caadr l)))  (y1 (float (cadadr l)))
	  (dx (-$ x1 x0))  (dy (-$ y1 y0))
	  (a dy)
	  (b (minus dx))
	  (c (+$ (*$ (minus dy) x0) (*$ dx y0)))
	  (ax+by+c (+$ (*$ a x) (*$ b y) c))
	  (a^2+b^2 (+$ (*$ a a) (*$ b b)))
	  (distance (//$ (*$ ax+by+c ax+by+c) a^2+b^2)))
     (fix distance)))
 
 (setq *rad->deg* (//$ 90.0 (arccos 0.0)))
 (defun radian->degree (rad)
   (fix (*$ *rad->deg* rad))))

(defun cos-see-angle-point-line (p l)
  (lets ((x  (float (first p))) (y (float (second p)))
	 (px (-$ (float (caar l)) x))
	 (py (-$ (float (cadar l)) y))
	 (qx (-$ (float (caadr l)) x))
	 (qy (-$ (float (cadadr l)) y))
	 (plen (sqrt (+$ (*$ px px) (*$ py py))))
	 (qlen (sqrt (+$ (*$ qx qx) (*$ qy qy))))
	 (inner  (+$ (*$ px qx) (*$ py qy)))
	 (cos-theta (//$ inner (*$ plen qlen))))
    cos-theta))

(defun nth-of-nearest-point (org points)
  (let ((kouho-distance (distance-points org (car points)))
        (kouho-point    0))
    (do ((rest (cdr points) (cdr rest))
	 (i 1 (1+ i)))
	((null rest) kouho-point)
	(lets ((now (car rest))
	       (now-distance (distance-points org now)))
	  (cond ((< now-distance kouho-distance)
		 (setq kouho-distance now-distance
		       kouho-point i)))))))

(defun nth-of-nearest-line (org lines)
  (let ((kouho-distance (distance-point-line org (car lines)))
        (kouho-line    0))
    (do ((rest (cdr lines) (cdr rest))
	 (i 1 (1+ i)))
	((null rest) kouho-line)
	(lets ((now (car rest))
	       (now-distance (distance-point-line org now)))
	  (cond ((< now-distance kouho-distance)
		 (setq kouho-distance now-distance
		       kouho-line i)))))))

;; ------------------------------------------------- ;;
;; (setq org    '(0 0)
;;      points '((10 10) (20 20) (4 3) (2 10) (1 1))
;;      lines  '(((11 10) (20 20))
;;               (( 4  3) ( 2 10))
;;               (( 2  1) ( 2  0))
;;               (( 1  0) ( 0  1))))
;;
;; (print (nth-of-nearest-point org points))
;; (print (nth-of-nearest-line  org lines))
;; ------------------------------------------------- ;;