/[geometry]/geometry/geometry.lisp
ViewVC logotype

Contents of /geometry/geometry.lisp

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1.1 - (show annotations)
Thu Oct 4 14:02:46 2007 UTC (6 years, 6 months ago) by ecant
Branch: MAIN
CVS Tags: HEAD
Added various 2D geometry routines and a small McCLIM app to test a proportion of them.
1 ;; References: http://local.wasp.uwa.edu.au/~pbourke/geometry/
2
3 (defpackage :geometry
4 (:use :cl)
5 (:export
6
7 ;; Dimensions
8 :lerp
9 :lerp2
10 :clamp
11 ;:square
12 ;:determinant
13
14 ;; Points
15 :distance
16 :distance-squared
17
18 ;; Lines/Vectors
19 :scalar-product
20
21 :point-is-on-line-p
22 :point-is-on-line-segment-p
23 :lines-are-parallel-p
24 :lines-are-identical-p
25
26 :line-intersection
27 :segment-intersection
28
29 ;; Circles
30 :line-circle-intersection
31 :circle-circle-intersection
32 ))
33 (in-package :geometry)
34
35
36 ;; Dimensions
37 (defun lerp (a b u)
38 "Linearly Interpolate from A to B by U.
39 The return value when U = 0 is A, U = 0.5 is halfway from A to B, U = 1 is B."
40 (+ a (* u (- b a))))
41
42 (defun lerp2 (a1 b1 a2 b2 u)
43 "Linearly Interpolate from (A1,A1) to (B1,B2) by U."
44 (values (lerp a1 a2 u)
45 (lerp b1 b2 u)))
46
47 (defun clamp (min x max)
48 "Clamp x between min and max."
49 (cond ((< x min) min)
50 ((> x max) max)
51 (t x)))
52
53 (defun square (x) (* x x))
54
55 ;;; Compute the determinant
56 ;;; |a b|
57 ;;; | | = ad - bc
58 ;;; |c d|
59 (defun determinant (a b c d)
60 (- (* a d) (* b c)))
61
62
63 ;; Points
64 (defun distance (x1 y1 x2 y2)
65 "Return the straight line distance between two points, uses Pythagoras."
66 (sqrt (+ (square (- x1 x2))
67 (square (- y1 y2)))))
68
69 (defun distance-squared (x1 y1 x2 y2)
70 "Return the straight line distance between two points squared, uses Pythagoras."
71 (+ (square (- x1 x2))
72 (square (- y1 y2))))
73
74 ;; Lines/Vectors
75 (defun scalar-product (x1 y1 x2 y2)
76 "return the scalar product beween two vectors"
77 (+ (* x1 x2) (* y1 y2)))
78
79 (defun point-is-on-line-p (x1 y1 x2 y2 xp yp)
80 "return true iff (xp,yp) is on the line through (x1,y1) and (x2,y2)"
81 (= (* (- xp x1) (- y2 y1))
82 (* (- yp y1) (- x2 x1))))
83
84 (defun point-is-on-line-segment-p (x1 y1 x2 y2 xp yp)
85 "return true iff (xp,yp) is on the line segment between (x1,y1) and (x2,y2)"
86 (and (point-is-on-line-p x1 y1 x2 y2 xp yp)
87 (<= x1 xp x2)
88 (<= y1 yp y2)))
89
90 (defun lines-are-parallel-p (x11 y11 x12 y12 x21 y21 x22 y22)
91 "return true iff the lines are parallel"
92 (= (* (- x12 x11) (- y22 y21))
93 (* (- x22 x21) (- y12 y11))))
94
95 (defun lines-are-identical-p (x11 y11 x12 y12 x21 y21 x22 y22)
96 (and (lines-are-parallel-p x11 y11 x12 y12 x21 y21 x22 y22)
97 (point-is-on-line-p x11 y11 x12 y12 x21 y21)))
98
99 ;;; Return the point of intersection of two lines, one through
100 ;;; (x11,y11) and (x12,y12) and the other one through (x21,y21)
101 ;;; and (x22,y22), as two values, x and y.
102 ;;;
103 ;;; If the lines are the same, return T instead.
104 ;;;
105 ;;; If the lines are parallel but not the same, return NIL instead.
106 ;;;
107 ;;; Mathworld says that to find the intersection of two lines,
108 ;;; one going through (x11,y11) and (x12,y12) and the other one
109 ;;; going through (x21,y21) and (x22,y22) you compute:
110 ;;;
111 ;;;
112 ;;; || x11 y11 | |
113 ;;; || | x11 - x12 |
114 ;;; || x12 y12 | |
115 ;;; | |
116 ;;; || x21 y21 | |
117 ;;; || | x21 - x22 |
118 ;;; || x22 y22 | |
119 ;;; x = --------------------------
120 ;;; | x11 - x12 y11 - y12 |
121 ;;; | |
122 ;;; | x21 - x22 y21 - y22 |
123 ;;;
124 ;;;
125 ;;; || x11 y11 | |
126 ;;; || | y11 - y12 |
127 ;;; || x12 y12 | |
128 ;;; | |
129 ;;; || x21 y21 | |
130 ;;; || | y21 - y22 |
131 ;;; || x22 y22 | |
132 ;;; y = --------------------------
133 ;;; | x11 - x12 y11 - y12 |
134 ;;; | |
135 ;;; | x21 - x22 y21 - y22 |
136 ;;;
137 ;;;
138 ;;; This will of course fail when
139 ;;;
140 ;;; | x11 - x12 y11 - y12 |
141 ;;; | | = 0
142 ;;; | x21 - x22 y21 - y22 |
143 ;;;
144 ;;; If we assume that we are not given degenerate lines with
145 ;;; two identical points, this happens when the lines are parallel.
146 (defun line-intersection (x11 y11 x12 y12 x21 y21 x22 y22)
147 (cond ((lines-are-identical-p x11 y11 x12 y12 x21 y21 x22 y22) t)
148 ((lines-are-parallel-p x11 y11 x12 y12 x21 y21 x22 y22) nil)
149 (t (let* ((dx1 (- x11 x12))
150 (dx2 (- x21 x22))
151 (dy1 (- y11 y12))
152 (dy2 (- y21 y22))
153 (a (determinant x11 y11 x12 y12))
154 (b (determinant x21 y21 x22 y22))
155 (c (determinant dx1 dy1 dx2 dy2)))
156 (values (/ (determinant a dx1 b dx2) c)
157 (/ (determinant a dy1 b dy2) c))))))
158
159 ;;; Return the point of intersection of two line segments,
160 ;;; (x11,y11) -- (x12,y12) and (x21,y21) -- (x22,y22),
161 ;;; as two values, x and y.
162 ;;;
163 ;;; If there are several points of intersection, return T instead.
164 ;;;
165 ;;; If there are no points of intersection, return NIL instead.
166 ;;;
167 (defun segment-intersection (x11 y11 x12 y12 x21 y21 x22 y22)
168 (multiple-value-bind (x y)
169 (line-intersection x11 y11 x12 y12 x21 y21 x22 y22)
170 (case x
171 ((nil) nil)
172 ((t) (cond ((and (= x11 x21)
173 (= y11 y21)
174 (not (point-is-on-line-segment-p x11 y11 x12 y12 x22 y22))
175 (not (point-is-on-line-segment-p x21 y21 x22 y22 x12 y12)))
176 (values x11 y11))
177 ((and (= x11 x22)
178 (= y11 y22)
179 (not (point-is-on-line-segment-p x11 y11 x12 y12 x21 y21))
180 (not (point-is-on-line-segment-p x21 y21 x22 y22 x12 y12)))
181 (values x11 y11))
182 ((and (= x12 x21)
183 (= y12 y21)
184 (not (point-is-on-line-segment-p x11 y11 x12 y12 x22 y22))
185 (not (point-is-on-line-segment-p x21 y21 x22 y22 x11 y11)))
186 (values x12 y12))
187 ((and (= x12 x22)
188 (= y12 y22)
189 (not (point-is-on-line-segment-p x11 y11 x12 y12 x21 y21))
190 (not (point-is-on-line-segment-p x21 y21 x22 y22 x11 y11)))
191 (values x12 y12))
192 (t t)))
193 (t (cond ((and (or (<= x11 x x12)
194 (<= x12 x x11))
195 (or (<= x21 x x22)
196 (<= x22 x x21))
197 (or (<= y11 y y12)
198 (<= y12 y y11))
199 (or (<= y21 y y22)
200 (<= y22 y y21)))
201 (values x y))
202 (t nil))))))
203
204 ;; Circles
205 (defun line-circle-intersection (x1 y1 x2 y2 cx cy r)
206 "Return either zero one or two points of intersection between the line (x1,y1) (x2,y2) and the circle centered at (cx,cy) with radius r"
207 (let* ((a (+ (expt (- x2 x1) 2) (expt (- y2 y1) 2)))
208 (b (* 2 (+ (* (- x2 x1) (- x1 cx))
209 (* (- y2 y1) (- y1 cy)))))
210 (c (- (+ (expt cx 2) (expt cy 2)
211 (expt x1 2) (expt y1 2))
212 (* 2 (+ (* cx x1)
213 (* cy y1)))
214 (* r r)))
215 (discriminant (- (* b b) (* 4 a c))))
216 (cond ((= discriminant 0)
217 (multiple-value-call #'lerp2 x1 y1 x2 y2
218 (/ (- b)
219 2 a)))
220 ((> discriminant 0)
221 (let* ((d-sqrt (sqrt discriminant))
222 (u1 (/ (+ (- b) d-sqrt) 2 a))
223 (u2 (/ (- (- b) d-sqrt) 2 a)))
224 (multiple-value-bind (ix1 iy1)
225 (lerp2 x1 y1 x2 y2 u1)
226 (multiple-value-bind (ix2 iy2)
227 (lerp2 x1 y1 x2 y2 u2)
228 (values ix1 iy1 ix2 iy2))))))))
229
230
231 (defun circle-circle-intersection (x1 y1 r1 x2 y2 r2)
232 "Return the two points of intersection of the two circles circumferences, or nil"
233 (let ((d (distance x1 y1 x2 y2)))
234 (if (<= (abs (- r1 r2)) d (+ r1 r2))
235 (let* ((a (/ (+ (- (* r1 r1) (* r2 r2)) (* d d)) 2 d))
236 (h (sqrt (- (* r1 r1) (* a a))))
237 (x3 (+ x1 (/ (* a (- x2 x1)) d)))
238 (y3 (+ y1 (/ (* a (- y2 y1)) d))))
239 (values
240 (+ x3 (/ (* h (- y2 y1)) d))
241 (- y3 (/ (* h (- x2 x1)) d))
242 (- x3 (/ (* h (- y2 y1)) d))
243 (+ y3 (/ (* h (- x2 x1)) d))))
244 (values))))

  ViewVC Help
Powered by ViewVC 1.1.5