1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
|
#lang typed/racket/base
(require racket/match
racket/list
(only-in math/flonum fl flvector+ flvector-)
(only-in racket/unsafe/ops unsafe-vector-ref)
racket/flonum)
(provide M3 m3-apply m3-transpose m3* m3-rotate-z m3-rotate-x
flv3-dot
flv3-normal
flv3-center
flv3->v
exact-vector3d
exact-vector3d-sublists
exact-polygon3d)
(define-type M3 (Vector FlVector FlVector FlVector))
(define-syntax-rule (dot x1 y1 z1 x2 y2 z2) (fl+ (fl+ (fl* x1 x2) (fl* y1 y2)) (fl* z1 z2)))
(: m3-apply (-> M3 FlVector FlVector))
(define (m3-apply m v)
(match-define (vector v1 v2 v3) m)
(define x (flvector-ref v 0))
(define y (flvector-ref v 1))
(define z (flvector-ref v 2))
(flvector (dot x y z (flvector-ref v1 0) (flvector-ref v1 1) (flvector-ref v1 2))
(dot x y z (flvector-ref v2 0) (flvector-ref v2 1) (flvector-ref v2 2))
(dot x y z (flvector-ref v3 0) (flvector-ref v3 1) (flvector-ref v3 2))))
(: m3-transpose (-> M3 M3))
(define (m3-transpose m)
(match-define (vector v1 v2 v3) m)
(vector (flvector (flvector-ref v1 0) (flvector-ref v2 0) (flvector-ref v3 0))
(flvector (flvector-ref v1 1) (flvector-ref v2 1) (flvector-ref v3 1))
(flvector (flvector-ref v1 2) (flvector-ref v2 2) (flvector-ref v3 2))))
(: m3* (-> M3 M3 M3))
(define (m3* m1 m2)
(match-define (vector v1 v2 v3) m1)
(define m (m3-transpose m2))
(vector (m3-apply m v1) (m3-apply m v2) (m3-apply m v3)))
(: m3-rotate-z (-> Real M3))
(define (m3-rotate-z theta)
(let ([theta (fl theta)])
(define cos-theta (flcos theta))
(define sin-theta (flsin theta))
(vector (flvector cos-theta (- sin-theta) 0.0)
(flvector sin-theta cos-theta 0.0)
(flvector 0.0 0.0 1.0))))
(: m3-rotate-x (-> Real M3))
(define (m3-rotate-x rho)
(let ([rho (fl rho)])
(define cos-rho (flcos rho))
(define sin-rho (flsin rho))
(vector (flvector 1.0 0.0 0.0)
(flvector 0.0 cos-rho (- sin-rho))
(flvector 0.0 sin-rho cos-rho))))
(: flv3-dot (-> FlVector FlVector Flonum))
(define (flv3-dot v1 v2)
(fl+ (fl* (flvector-ref v1 0)
(flvector-ref v2 0))
(fl+ (fl* (flvector-ref v1 1)
(flvector-ref v2 1))
(fl* (flvector-ref v1 2)
(flvector-ref v2 2)))))
(define default-normal (flvector 0.0 -1.0 0.0))
(: flv3-normal (-> (Listof FlVector) FlVector))
(define (flv3-normal vs)
(define n (length vs))
(cond
[(n . < . 3) default-normal]
[else
(match-define (list v1 v2) (take-right vs 2))
(define x1 (flvector-ref v1 0))
(define y1 (flvector-ref v1 1))
(define z1 (flvector-ref v1 2))
(define x2 (flvector-ref v2 0))
(define y2 (flvector-ref v2 1))
(define z2 (flvector-ref v2 2))
(define-values (x y z _x1 _y1 _z1 _x2 _y2 _z2)
(for/fold ([x : Flonum 0.0]
[y : Flonum 0.0]
[z : Flonum 0.0]
[x1 : Flonum x1]
[y1 : Flonum y1]
[z1 : Flonum z1]
[x2 : Flonum x2]
[y2 : Flonum y2]
[z2 : Flonum z2])
([v3 (in-list vs)])
(define x3 (flvector-ref v3 0))
(define y3 (flvector-ref v3 1))
(define z3 (flvector-ref v3 2))
(define x32 (fl- x3 x2))
(define y32 (fl- y3 y2))
(define z32 (fl- z3 z2))
(define x12 (fl- x1 x2))
(define y12 (fl- y1 y2))
(define z12 (fl- z1 z2))
(values (+ x (fl- (fl* y32 z12) (fl* z32 y12)))
(+ y (fl- (fl* z32 x12) (fl* x32 z12)))
(+ z (fl- (fl* x32 y12) (fl* y32 x12)))
x2 y2 z2
x3 y3 z3)))
(define m (flsqrt (fl+ (fl* x x) (fl+ (fl* y y) (fl* z z)))))
(if (fl> m 0.0)
(flvector (fl/ x m) (fl/ y m) (fl/ z m))
default-normal)]))
(: flv3-center (-> (Listof FlVector) FlVector))
(define (flv3-center vs)
(define xs (map (λ ([v : FlVector]) (flvector-ref v 0)) vs))
(define ys (map (λ ([v : FlVector]) (flvector-ref v 1)) vs))
(define zs (map (λ ([v : FlVector]) (flvector-ref v 2)) vs))
(flvector (* 0.5 (+ (apply min xs) (apply max xs)))
(* 0.5 (+ (apply min ys) (apply max ys)))
(* 0.5 (+ (apply min zs) (apply max zs)))))
(: flv3->v (-> FlVector (Vectorof Real)))
(define (flv3->v v)
(define x (flvector-ref v 0))
(define y (flvector-ref v 1))
(define z (flvector-ref v 2))
(vector x y z))
(: exact-vector3d (-> (Vectorof Real) (U #f (Vectorof Real))))
(define (exact-vector3d v)
(define n (vector-length v))
(cond [(= n 3)
(define v1 (unsafe-vector-ref v 0))
(define v2 (unsafe-vector-ref v 1))
(define v3 (unsafe-vector-ref v 2))
(cond [(and (exact? v1) (exact? v2) (exact? v3)) v]
[(and (rational? v1) (rational? v2) (rational? v3))
(vector (inexact->exact v1) (inexact->exact v2) (inexact->exact v3))]
[else #f])]
[else #f]))
(: sublists (All (A) (-> (Listof (U #f A)) (Listof (Listof A)))))
(define (sublists vs)
(define vss
(for/fold ([vss : (Listof (Listof A)) (list null)]) ([v (in-list vs)])
(cond [v (cons (cons v (car vss)) (cdr vss))]
[(null? (car vss)) vss]
[else (cons null vss)])))
(cond [(null? (car vss)) (cdr vss)]
[else vss]))
(: exact-vector3d-sublists (-> (Listof (Vectorof Real)) (Listof (Listof (Vectorof Real)))))
(define (exact-vector3d-sublists vs)
(sublists (map exact-vector3d vs)))
(: exact-polygon3d (All (L) (-> (Listof (Vectorof Real)) (Listof L)
(Values (Listof (Vectorof Real)) (Listof L)))))
(define (exact-polygon3d vs ls)
(cond
[(null? vs) (values null null)]
[else
(define-values (new-vs new-ls _)
(for/fold ([vs : (Listof (Vectorof Real)) null]
[ls : (Listof L) null]
[v1 : (Vectorof Real) (last vs)])
([v2 (in-list vs)]
[l (in-list ls)])
(cond [(equal? v1 v2) (values vs ls v2)]
[else
(define exact-v2 (exact-vector3d v2))
(if exact-v2
(values (cons exact-v2 vs) (cons l ls) v2)
(values vs ls v2))])))
(values (reverse new-vs) (reverse new-ls))]))
|
