| 1 : |
ktanaka |
1.1 |
;; ------------------------------------------- ;; |
| 2 : |
|
|
;; dist.l (distance between point, line, ... ) ;; |
| 3 : |
|
|
;; ------------------------------------------- ;; |
| 4 : |
|
|
|
| 5 : |
|
|
(defun distance-points (a b) |
| 6 : |
|
|
(lets ((x0 (first a)) (y0 (second a)) |
| 7 : |
|
|
(x1 (first b)) (y1 (second b)) |
| 8 : |
|
|
(dx (difference x1 x0)) (dy (difference y1 y0))) |
| 9 : |
|
|
(fix (plus (times dx dx) (times dy dy))))) |
| 10 : |
|
|
|
| 11 : |
|
|
(defun distance-point-line (p l) |
| 12 : |
|
|
(lets ((A (first l)) |
| 13 : |
|
|
(B (second l)) |
| 14 : |
|
|
(PA (sqrt (float (distance-points p A)))) |
| 15 : |
|
|
(PB (sqrt (float (distance-points p B)))) |
| 16 : |
|
|
(AB (sqrt (float (distance-points A B)))) |
| 17 : |
|
|
(d (fix (difference (plus PA PB) AB)))) |
| 18 : |
|
|
(* d d))) |
| 19 : |
|
|
|
| 20 : |
|
|
(comment ;; old version |
| 21 : |
|
|
;; -------------------------------- ;; |
| 22 : |
|
|
;; distance overflow integer range! ;; |
| 23 : |
|
|
;; -------------------------------- ;; |
| 24 : |
|
|
;; ------------------------------------------------- ;; |
| 25 : |
|
|
;; (discance-pointe-line (10 20) ((30 40) (50 60))) ;; |
| 26 : |
|
|
;; line: dy*(x-x0) = dx*(y-y0) ;; |
| 27 : |
|
|
;; i.e. dy*x - dx*y + (-dy*x0 + dx*y0) = 0 ;; |
| 28 : |
|
|
;; ax + by + c = 0 ;; |
| 29 : |
|
|
;; ------------------------------------------------- ;; |
| 30 : |
|
|
(defun distance-point-line (p l) |
| 31 : |
|
|
(lets ((x (float (car p))) (y (float (cadr p))) |
| 32 : |
|
|
(x0 (float (caar l))) (y0 (float (cadar l))) |
| 33 : |
|
|
(x1 (float (caadr l))) (y1 (float (cadadr l))) |
| 34 : |
|
|
(dx (-$ x1 x0)) (dy (-$ y1 y0)) |
| 35 : |
|
|
(a dy) |
| 36 : |
|
|
(b (minus dx)) |
| 37 : |
|
|
(c (+$ (*$ (minus dy) x0) (*$ dx y0))) |
| 38 : |
|
|
(ax+by+c (+$ (*$ a x) (*$ b y) c)) |
| 39 : |
|
|
(a^2+b^2 (+$ (*$ a a) (*$ b b))) |
| 40 : |
|
|
(distance (//$ (*$ ax+by+c ax+by+c) a^2+b^2))) |
| 41 : |
|
|
(fix distance))) |
| 42 : |
|
|
|
| 43 : |
|
|
(setq *rad->deg* (//$ 90.0 (arccos 0.0))) |
| 44 : |
|
|
(defun radian->degree (rad) |
| 45 : |
|
|
(fix (*$ *rad->deg* rad)))) |
| 46 : |
|
|
|
| 47 : |
|
|
(defun cos-see-angle-point-line (p l) |
| 48 : |
|
|
(lets ((x (float (first p))) (y (float (second p))) |
| 49 : |
|
|
(px (-$ (float (caar l)) x)) |
| 50 : |
|
|
(py (-$ (float (cadar l)) y)) |
| 51 : |
|
|
(qx (-$ (float (caadr l)) x)) |
| 52 : |
|
|
(qy (-$ (float (cadadr l)) y)) |
| 53 : |
|
|
(plen (sqrt (+$ (*$ px px) (*$ py py)))) |
| 54 : |
|
|
(qlen (sqrt (+$ (*$ qx qx) (*$ qy qy)))) |
| 55 : |
|
|
(inner (+$ (*$ px qx) (*$ py qy))) |
| 56 : |
|
|
(cos-theta (//$ inner (*$ plen qlen)))) |
| 57 : |
|
|
cos-theta)) |
| 58 : |
|
|
|
| 59 : |
|
|
(defun nth-of-nearest-point (org points) |
| 60 : |
|
|
(let ((kouho-distance (distance-points org (car points))) |
| 61 : |
|
|
(kouho-point 0)) |
| 62 : |
|
|
(do ((rest (cdr points) (cdr rest)) |
| 63 : |
|
|
(i 1 (1+ i))) |
| 64 : |
|
|
((null rest) kouho-point) |
| 65 : |
|
|
(lets ((now (car rest)) |
| 66 : |
|
|
(now-distance (distance-points org now))) |
| 67 : |
|
|
(cond ((< now-distance kouho-distance) |
| 68 : |
|
|
(setq kouho-distance now-distance |
| 69 : |
|
|
kouho-point i))))))) |
| 70 : |
|
|
|
| 71 : |
|
|
(defun nth-of-nearest-line (org lines) |
| 72 : |
|
|
(let ((kouho-distance (distance-point-line org (car lines))) |
| 73 : |
|
|
(kouho-line 0)) |
| 74 : |
|
|
(do ((rest (cdr lines) (cdr rest)) |
| 75 : |
|
|
(i 1 (1+ i))) |
| 76 : |
|
|
((null rest) kouho-line) |
| 77 : |
|
|
(lets ((now (car rest)) |
| 78 : |
|
|
(now-distance (distance-point-line org now))) |
| 79 : |
|
|
(cond ((< now-distance kouho-distance) |
| 80 : |
|
|
(setq kouho-distance now-distance |
| 81 : |
|
|
kouho-line i))))))) |
| 82 : |
|
|
|
| 83 : |
|
|
;; ------------------------------------------------- ;; |
| 84 : |
|
|
;; (setq org '(0 0) |
| 85 : |
|
|
;; points '((10 10) (20 20) (4 3) (2 10) (1 1)) |
| 86 : |
|
|
;; lines '(((11 10) (20 20)) |
| 87 : |
|
|
;; (( 4 3) ( 2 10)) |
| 88 : |
|
|
;; (( 2 1) ( 2 0)) |
| 89 : |
|
|
;; (( 1 0) ( 0 1)))) |
| 90 : |
|
|
;; |
| 91 : |
|
|
;; (print (nth-of-nearest-point org points)) |
| 92 : |
|
|
;; (print (nth-of-nearest-line org lines)) |
| 93 : |
|
|
;; ------------------------------------------------- ;; |
Annotation of /lisp/tools/dist.l
Parent Directory
| 
Revision Log