#lang racket
(provide
matrix?
matrix-vals
m-lines
m-cols
m-frame
m-cs
m-n
m-rotation
m-scaling
m-translation
m-line
m-column
m-translation-c
m-neg
+m
-m
*m
m*p
m-transform
m-zero
m-identity
list<-matrix
vector-lines<-matrix
vector-cols<-matrix)
(define (v*v v1 v2)
(foldl + 0 (vector->list (vector-map * v1 v2))))
(define (matrix-write m port mode)
(define (print-line v)
(write-string
(string-join
(vector->list
(vector-map (λ (e) (format "~a" e)) v))
" ")
port))
(write-string "m(" port)
(print-line (vector-copy (matrix-vals m) 0 4))
(newline port)
(write-string " " port)
(print-line (vector-copy (matrix-vals m) 4 8))
(newline port)
(write-string " " port)
(print-line (vector-copy (matrix-vals m) 8 12))
(newline port)
(write-string " " port)
(print-line (vector-copy (matrix-vals m) 12 16))
(write-string ")" port))
(struct matrix (vals)
#:property prop:custom-write matrix-write)
(define (m-lines v1 v2 v3)
(let ((x1 (vector-ref v1 0))
(y1 (vector-ref v1 1))
(z1 (vector-ref v1 2))
(w1 (vector-ref v1 3)))
(let ((x2 (vector-ref v2 0))
(y2 (vector-ref v2 1))
(z2 (vector-ref v2 2))
(w2 (vector-ref v2 3)))
(let ((x3 (vector-ref v3 0))
(y3 (vector-ref v3 1))
(z3 (vector-ref v3 2))
(w3 (vector-ref v3 3)))
(matrix
(vector x1 y1 z1 w1
x2 y2 z2 w2
x3 y3 z3 w3
0 0 0 1))))))
(define m-cols
(case-lambda
((v1 v2 v3 v4)
(let ((x1 (vector-ref v1 0))
(y1 (vector-ref v1 1))
(z1 (vector-ref v1 2)))
(let ((x2 (vector-ref v2 0))
(y2 (vector-ref v2 1))
(z2 (vector-ref v2 2)))
(let ((x3 (vector-ref v3 0))
(y3 (vector-ref v3 1))
(z3 (vector-ref v3 2)))
(let ((x4 (vector-ref v4 0))
(y4 (vector-ref v4 1))
(z4 (vector-ref v4 2)))
(matrix
(vector x1 x2 x3 x4
y1 y2 y3 y4
z1 z2 z3 z4
0 0 0 1)))))))
((v)
(let ((x1 (vector-ref v 0))
(y1 (vector-ref v 1))
(z1 (vector-ref v 2)))
(let ((x2 (vector-ref v 4))
(y2 (vector-ref v 5))
(z2 (vector-ref v 6)))
(let ((x3 (vector-ref v 8))
(y3 (vector-ref v 9))
(z3 (vector-ref v 10)))
(let ((x4 (vector-ref v 12))
(y4 (vector-ref v 13))
(z4 (vector-ref v 14)))
(matrix
(vector x1 x2 x3 x4
y1 y2 y3 y4
z1 z2 z3 z4
0 0 0 1)))))))))
(define (m-frame c x y)
(let ((xc (norm x))
(zc (norm (cross-c x y))))
(let ((yc (norm (cross-c zc xc))))
(m-cols
(vector<-xyz xc)
(vector<-xyz yc)
(vector<-xyz zc)
(vector<-xyz c)))))
(define (m-cs c x y)
(m-frame c (-c x c) (-c y c)))
(define (m-n c n)
(let ((z (norm n)))
(let ((x (norm (collinear-cross-c z))))
(let ((y (norm (cross-c z x))))
(m-cols
(vector<-xyz x)
(vector<-xyz y)
(vector<-xyz z)
(vector<-xyz c))))))
(define (m-rotation a n)
(define (m-rotate-aux n1 n2 n3)
(let ((c (cos a))
(s (sin a))
(t (- 1 (cos a)))
(p (norm (xyz n1 n2 n3))))
(let ((x (xyz-x p))
(y (xyz-y p))
(z (xyz-z p)))
(let ((r00 (+ (* t x x) c))
(r01 (- (* t x y) (* s z)))
(r02 (+ (* t x z) (* s y)))
(r10 (+ (* t x y) (* s z)))
(r11 (+ (* t y y) c))
(r12 (- (* t y z) (* s x)))
(r20 (- (* t x z) (* s y))) (r21 (+ (* t y z) (* s x)))
(r22 (+ (* t z z) c)))
(matrix
(vector r00 r01 r02 0
r10 r11 r12 0
r20 r21 r22 0
0 0 0 1))))))
(if (or (= a 0) (=c? n u0))
m-identity
(apply m-rotate-aux (list-of-coord n))))
(define list-of-coord "BUM")
(define m-scaling
(case-lambda
((x y z)
(matrix
(vector x 0 0 0
0 y 0 0
0 0 z 0
0 0 0 1)))
((c)
(m-scaling (xyz-x c) (xyz-y c) (xyz-z c)))))
(define m-translation
(case-lambda
((x y z)
(matrix
(vector 1 0 0 x
0 1 0 y
0 0 1 z
0 0 0 1)))
((c)
(m-translation (xyz-x c) (xyz-y c) (xyz-z c)))))
(define (m-line m l)
(vector-copy (matrix-vals m) (* l 4) (+ (* l 4) 4)))
(define (m-column m c)
(let ((vals (matrix-vals m)))
(vector
(vector-ref vals c)
(vector-ref vals (+ c 4))
(vector-ref vals (+ c 8))
(vector-ref vals (+ c 12)))))
(define (m-translation-c m)
(let ((vals (matrix-vals m))
(c 3))
(xyz
(vector-ref vals c)
(vector-ref vals (+ c 4))
(vector-ref vals (+ c 8)))))
(define (m-neg m)
(matrix
(vector-map - (matrix-vals m))))
(define (+m m1 m2)
(matrix
(vector-map + (matrix-vals m1) (matrix-vals m2))))
(define (-m m1 m2)
(+m m1 (m-neg m2)))
(define (*m m1 m2)
(matrix
(list->vector
(flatten
(for/list ((i 4))
(for/list ((j 4))
(v*v (m-line m1 i) (m-column m2 j))))))))
(define (m*p m p (vector? #f))
(let* ((w (if vector? 0 1))
(v (vector (xyz-x p) (xyz-y p) (xyz-z p) w)))
(xyz (v*v (m-line m 0) v)
(v*v (m-line m 1) v)
(v*v (m-line m 2) v))))
(define (m-transform t a n s)
(*m
(*m
(m-scaling (xyz-x s) (xyz-y s) (xyz-z s))
(m-rotation a n))
(m-translation (xyz-x t) (xyz-y t) (xyz-z t))))
(define m-zero
(matrix
(vector 0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0)))
(define m-identity
(matrix
(vector 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1)))
(define (list<-matrix m)
(vector->list (matrix-vals m)))
(define (vector-lines<-matrix m)
(vector-copy (matrix-vals m)))
(define (vector-cols<-matrix m)
(let ((v (matrix-vals m)))
(let ((x1 (vector-ref v 0))
(y1 (vector-ref v 1))
(z1 (vector-ref v 2))
(w1 (vector-ref v 3)))
(let ((x2 (vector-ref v 4))
(y2 (vector-ref v 5))
(z2 (vector-ref v 6))
(w2 (vector-ref v 7)))
(let ((x3 (vector-ref v 8))
(y3 (vector-ref v 9))
(z3 (vector-ref v 10))
(w3 (vector-ref v 11)))
(vector x1 x2 x3 0
y1 y2 y3 0
z1 z2 z3 0
w1 w2 w3 1))))))
(provide
norm cross-c
dot-c
position?
position-cs
xyz xyz-on xyz-on-cs
xyz-on-points
xyz-on-z-rotation
xyz-from-normal
world-xy-cs
world-yz-cs
world-zx-cs
world-yx-cs
world-zy-cs
world-xz-cs
xy
yz
xz
x y z
xyz-x xyz-y xyz-z
(rename-out [xyz-x cx] [xyz-y cy] [xyz-z cz])
vector<-xyz
roundc =c? +x +y +z +xy +xz +yz +xyz
pol +pol pol-rho pol-phi
cyl +cyl cyl-rho cyl-phi cyl-z
sph +sph sph-rho sph-phi sph-psi
+rho +phi +r +psi
+c -c *c /c
u0
ux
uy
uz
uxy
uyz
uxz
uxyz
-ux
-uy
-uz
-uxy
-uyz
-uxz
-uxyz
pi/6
pi/4
pi/3
pi/2
3pi/2
2pi
3pi
4pi
-pi/6
-pi/4
-pi/3
-pi/2
-3pi/2
-pi
-2pi
-3pi
-4pi
coterminal
distance
tr-matrix
position-and-height
inverted-position-and-height
(rename-out (sxyz-cs-o cs-o)
(sxyz-cs-x cs-x)
(sxyz-cs-y cs-y)
(sxyz-cs-z cs-z))
as-origin
as-world
)
(struct position (cs))
(struct 2d-position position ())
(struct 3d-position position ())
(define (fprint-3d-position sxyz port mode)
(fprintf port "#<xyz:~A ~A ~A>"
(sxyz-x sxyz)
(sxyz-y sxyz)
(sxyz-z sxyz))
(let ((cs (position-cs sxyz)))
(when (and cs (not (std-cs? cs)))
(write-string " | " port)
(write cs port)))
(write-string ">" port))
(struct sxyz 3d-position (x y z)
#:property
prop:custom-write fprint-3d-position)
(struct srpz 3d-position (rho phi z))
(struct srpp 3d-position (rho phi psi))
(struct sxyz-cs (o x y z)
#:property
prop:custom-write (lambda (cs port mode)
(fprintf port "#cs[o:~A ux:~A uy:~A uz:~A]"
(sxyz-cs-o cs)
(sxyz-cs-x cs)
(sxyz-cs-y cs)
(sxyz-cs-z cs))))
(define (=c? c0 c1)
(or (eq? c0 c1)
(and (eq? (position-cs c0) (position-cs c1))
(= (xyz-x c0) (xyz-x c1))
(= (xyz-y c0) (xyz-y c1))
(= (xyz-z c0) (xyz-z c1)))))
(define (roundc c)
(sxyz (position-cs c)
(round (sxyz-x c))
(round (sxyz-y c))
(round (sxyz-z c))))
(define world-xy-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 1 0 0) (sxyz #f 0 1 0) (sxyz #f 0 0 1)))
(define world-yx-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 1 0) (sxyz #f 1 0 0) (sxyz #f 0 0 -1)))
(define world-yz-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 1 0) (sxyz #f 0 0 1) (sxyz #f 1 0 0)))
(define world-zy-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 0 1) (sxyz #f 0 1 0) (sxyz #f -1 0 0)))
(define world-zx-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 0 1) (sxyz #f 1 0 0) (sxyz #f 0 1 0)))
(define world-xz-cs
(sxyz-cs (sxyz #f 0 0 0) (sxyz #f 1 0 0) (sxyz #f 0 0 1) (sxyz #f 0 -1 0)))
(define std-cs world-xy-cs)
(define (std-cs? cs)
(eq? cs std-cs))
(define (as-origin p)
(let ((cs (position-cs p)))
(let ((o (sxyz-cs-o cs)))
(sxyz (sxyz-cs (sxyz
#f (+ (sxyz-x p) (sxyz-x o))
(+ (sxyz-y p) (sxyz-y o))
(+ (sxyz-z p) (sxyz-z o)))
(sxyz-cs-x cs)
(sxyz-cs-y cs)
(sxyz-cs-z cs))
0 0 0))))
(define (as-world p)
(let ((cs (position-cs p)))
(if (or (not cs) (std-cs? cs))
p
(m*p (tr-matrix cs) p))))
(define (xyz x y z)
(sxyz std-cs x y z))
(define (xy x y)
(xyz x y 0))
(define (xz x z)
(xyz x 0 z))
(define (yz y z)
(xyz 0 y z))
(define (x x)
(xyz x 0 0))
(define (y y)
(xyz 0 y 0))
(define (z z)
(xyz 0 0 z))
(define (xyz-on o x y)
(sxyz (sxyz-cs o x y (norm (cross-c x y)))
0 0 0))
(define (xyz-on-points p0 p1 p2 p3)
(let ((n0 (-c p1 p0))
(n1 (-c p2 p0)))
(let ((n2 (cross-c n0 n1)))
(if (< (dot-c n2 (-c p3 p0)) 0)
(xyz-on p0 n0 n1)
(xyz-on p0 n1 n0)))))
(define (xyz-on-cs x y z cs)
(sxyz cs x y z))
(define (xyz-on-z-rotation x y z a)
(sxyz (sxyz-cs (xyz x y z) (pol 1 a) (pol 1 (+ a pi/2)) uz)
0 0 0))
(define (+xyz p dx dy dz)
(cond [(sxyz? p)
(sxyz (position-cs p)
(+ (sxyz-x p) dx)
(+ (sxyz-y p) dy)
(+ (sxyz-z p) dz))]
[else
(error '+xyz "Implement conversions for ~A" p)]))
(define (+x p dx)
(+xyz p dx 0 0))
(define (+y p dy)
(+xyz p 0 dy 0))
(define (+z p dz)
(+xyz p 0 0 dz))
(define (+xy p dx dy)
(+xyz p dx dy 0))
(define (+xz p dx dz)
(+xyz p dx 0 dz))
(define (+yz p dy dz)
(+xyz p 0 dy dz))
(define (xyz-x p)
(cond [(sxyz? p)
(sxyz-x p)]
[(srpz? p)
(error "Implement conversions")]))
(define (xyz-y p)
(cond [(sxyz? p)
(sxyz-y p)]
[(srpz? p)
(error "Implement conversions")]))
(define (xyz-z p)
(cond [(sxyz? p)
(sxyz-z p)]
[(srpz? p)
(srpz-z p)]))
(define (cyl rho phi z)
(xyz (* rho (cos phi))
(* rho (sin phi))
z))
(define (+cyl p rho phi z)
(+c p (cyl rho phi z)))
(define (cyl-rho p)
(let ((x (xyz-x p))
(y (xyz-y p)))
(sqrt (+ (* x x) (* y y)))))
(define (cyl-phi c)
(sph-phi c))
(define (cyl-z c)
(xyz-z c))
(define (pol rho phi)
(cyl rho phi 0))
(define (+pol p rho phi)
(+cyl p rho phi 0))
(define pol-rho cyl-rho)
(define pol-phi cyl-phi)
(define (sph rho phi psi)
(let ((sin-psi (sin psi)))
(xyz (* rho (cos phi) sin-psi)
(* rho (sin phi) sin-psi)
(* rho (cos psi)))))
(define (+sph p rho phi psi)
(+c p (sph rho phi psi)))
(define (sph-rho p)
(let ((x (xyz-x p))
(y (xyz-y p))
(z (xyz-z p)))
(sqrt (+ (* x x) (* y y) (* z z)))))
(define (sph-phi p)
(let ((x (xyz-x p))
(y (xyz-y p)))
(if (= 0 x y)
0
(atan (xyz-y p) (xyz-x p)))))
(define (sph-psi p)
(let ((x (xyz-x p))
(y (xyz-y p))
(z (xyz-z p)))
(if (= 0 x y z)
0
(atan (sqrt (+ (* x x) (* y y)))
z))))
(define (+rho c rho)
(cyl (+ (cyl-rho c) rho) (cyl-phi c) (cyl-z c)))
(define (+phi c phi)
(cyl (cyl-rho c) (+ (cyl-phi c) phi) (cyl-z c)))
(define (+r c r)
(sph (+ (sph-rho c) r) (sph-phi c) (sph-psi c)))
(define (+psi c psi)
(sph (sph-rho c) (sph-phi c) (+ (sph-psi c) psi)))
(define (+c p0 p1)
(sxyz (position-cs p0)
(+ (sxyz-x p0) (sxyz-x p1))
(+ (sxyz-y p0) (sxyz-y p1))
(+ (sxyz-z p0) (sxyz-z p1))))
(define (-c p0 p1)
(sxyz (position-cs p0)
(- (sxyz-x p0) (sxyz-x p1))
(- (sxyz-y p0) (sxyz-y p1))
(- (sxyz-z p0) (sxyz-z p1))))
(define (*c p/r r/p)
(let-values ([(p r)
(if (number? p/r)
(values r/p p/r)
(values p/r r/p))])
(sxyz (position-cs p)
(* (sxyz-x p) r)
(* (sxyz-y p) r)
(* (sxyz-z p) r))))
(define (/c p r)
(sxyz (position-cs p)
(/ (sxyz-x p) r)
(/ (sxyz-y p) r)
(/ (sxyz-z p) r)))
(define u0 (xyz 0 0 0))
(define ux (xyz 1 0 0))
(define uy (xyz 0 1 0))
(define uz (xyz 0 0 1))
(define -ux (xyz -1 0 0))
(define -uy (xyz 0 -1 0))
(define -uz (xyz 0 0 -1))
(define uxy (xy 1 1))
(define uyz (yz 1 1))
(define uxz (xz 1 1))
(define -uxy (xy -1 -1))
(define -uyz (yz -1 -1))
(define -uxz (xz -1 -1))
(define uxyz (xyz 1 1 1))
(define -uxyz (xyz -1 -1 -1))
(define pi/6 (/ pi 6))
(define pi/4 (/ pi 4))
(define pi/3 (/ pi 3))
(define pi/2 (/ pi 2))
(define 3pi/2 (* 3 pi/2))
(define 2pi (* 2 pi))
(define 3pi (* 3 pi))
(define 4pi (* 4 pi))
(define -pi/6 (/ pi -6))
(define -pi/4 (/ pi -4))
(define -pi/3 (/ pi -3))
(define -pi/2 (/ pi -2))
(define -3pi/2 (* -3 pi/2))
(define -pi (* -1 pi))
(define -2pi (* -2 pi))
(define -3pi (* -3 pi))
(define -4pi (* -4 pi))
(define (coterminal radians)
(let ((k (truncate (/ radians 2pi))))
(- radians (* k 2pi))))
(define (distance p0 p1)
(let ((dx (- (xyz-x p1) (xyz-x p0)))
(dy (- (xyz-y p1) (xyz-y p0)))
(dz (- (xyz-z p1) (xyz-z p0))))
(sqrt (+ (* dx dx) (* dy dy) (* dz dz)))))
(define-syntax (let-coords stx)
(syntax-case stx ()
((_ (((x y z) p) ...) body ...)
(syntax/loc stx
(let ((x (xyz-x p)) ...
(y (xyz-y p)) ...
(z (xyz-z p)) ...)
body ...)))))
(define (perpendicular-vector v)
(let-coords (((x y z) v))
(cond ((= z 0)
(xyz 0 0 1))
((= y 0)
(xyz 0 -1 0))
((= x 0)
(xyz 1 0 0))
(else
(let ((x z)
(y z)
(z (- (+ x y)))) (let ((l (sqrt (+ (* x x) (* y y) (* z z)))))
(xyz (/ x l) (/ y l) (/ z l))))))))
(define (xyz-from-normal p n)
(let ((x (xyz-x n))
(y (xyz-y n))
(z (xyz-z n)))
(let ((z-axis n))
(let ((v0 (perpendicular-vector n)))
(let ((x-axis (cross-c n v0)))
(let ((y-axis (cross-c z-axis x-axis)))
(sxyz (sxyz-cs p (norm x-axis) (norm y-axis) (norm z-axis))
0 0 0)))))))
(define (vector<-xyz p)
(vector (sxyz-x p) (sxyz-y p) (sxyz-z p)))
(define (tr-matrix cs)
(m-cols (vector<-xyz (sxyz-cs-x cs))
(vector<-xyz (sxyz-cs-y cs))
(vector<-xyz (sxyz-cs-z cs))
(vector<-xyz (sxyz-cs-o cs))))
(define (position-and-height p0 h/p1)
(if (number? h/p1)
(values p0 h/p1)
(let ((p0w (as-world p0))
(p1w (as-world h/p1)))
(values (xyz-from-normal p0w (-c p1w p0w))
(distance p0w p1w)))))
(define (inverted-position-and-height p0 h/p1)
(if (number? h/p1)
(values (xyz-from-normal (+z p0 h/p1) -uz)
h/p1)
(values (xyz-from-normal h/p1 (-c p0 h/p1))
(distance p0 h/p1))))
(define (norm v)
(let-coords (((x y z) v))
(let ((l (sqrt (+ (* x x) (* y y) (* z z)))))
(xyz (/ x l) (/ y l) (/ z l)))))
(define (dot-c c1 c2)
(+
(* (xyz-x c1) (xyz-x c2))
(* (xyz-y c1) (xyz-y c2))
(* (xyz-z c1) (xyz-z c2))))
(define (cross-c v0 v1)
(let-coords (((x0 y0 z0) v0)
((x1 y1 z1) v1))
(xyz (- (* y0 z1) (* z0 y1))
(- (* z0 x1) (* x0 z1))
(- (* x0 y1) (* y0 x1)))))
(define (collinear-cross-c c)
(define (collinear-pxp-1)
(xyz 0 (xyz-z c) (- (xyz-y c))))
(define (collinear-pxp-2)
(xyz (xyz-z c) 0 (- (xyz-x c))))
(define (collinear-pxp-3)
(xyz (xyz-y c) (- (xyz-x c)) 0))
(let ((n (collinear-pxp-1)))
(if (=c? u0 n)
(let ((n (collinear-pxp-2)))
(if (=c? u0 n)
(collinear-pxp-3)
n))
n)))
(define (angle-c c1 c2 (normal #f))
(define (aux)
(acos
(dot-c
(*c c1 (/ (sph-rho c1)))
(*c c2 (/ (sph-rho c2))))))
(define (continuous-aux)
(let ((p (cross-c c1 c2)))
(if (or
(=c? u0 p)
(> (dot-c normal p) 0))
(aux)
(- 2pi (aux)))))
(if normal
(continuous-aux)
(aux)))
(define (collinearity-c c1 c2)
(define (n//n n1 n2)
(cond ((= n1 n2 0) #t)
((= n2 0) 0)
(else (/ n1 n2))))
(define (ns//ns)
(filter
(error "FINISH") (list
(n//n (xyz-x c1) (xyz-x c2))
(n//n (xyz-y c1) (xyz-y c2))
(n//n (xyz-z c1) (xyz-z c2)))))
(let ((ns (ns//ns)))
(cond ((empty? ns) 0)
((apply = (cons (first ns) ns)) (first ns))
(else 0))))