-
Notifications
You must be signed in to change notification settings - Fork 0
/
Csg.hs
353 lines (315 loc) · 15.4 KB
/
Csg.hs
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
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
module Csg where
import Data.Map as M hiding (map, filter, split)
import Data.Set as S hiding (map, filter, split)
import Data.List as L hiding (map, filter, intersect)
import Data.Maybe as Q
import Debug.Trace
type Vector = [Double]
type Point = Vector
type Polyset = [Polygon]
type Triangle = [Int]
type Extent = (Point, Point)
data Polygon = P [Point] Plane Extent -- points (plane nnormal, distance) extent
deriving (Show, Eq, Ord)
data Polyhedron = Polyhedron [Point] [Triangle] Extent -- vertices, polygons, extent
deriving (Show, Eq, Ord)
data Intersection = Intersection [(Double, Point)] Line
deriving (Show, Eq, Ord)
data Relation = Coplanar | DoNotIntersect | Intersect Intersection -- polygon relationship
deriving (Show, Eq, Ord)
data Status = Inside | Outside | BoundarySame | BoundaryOpposite
deriving (Show, Eq, Ord, Enum)
data Line = Line Point Vector -- arbitrary point and unit direction vector
deriving (Show, Eq, Ord)
data Plane = Plane Vector Double -- unit normal, distance
deriving (Show, Eq, Ord)
cube_pts = [[0::Double,0.0,0.0],[0.0,0.0,1.0],[0.0,1.0,0.0],[0.0,1.0,1.0],[1.0,0.0,0.0],[1.0,0.0,1.0],[1.0,1.0,0.0],[1.0,1.0,1.0]]
cube_tris = [[0,1,2],[1,3,2],[0,4,1],[4,5,1],[0,2,4],[4,2,6],[4,6,5],[5,6,7],[5,7,3],[3,1,5],[2,7,6],[7,2,3]]
cube = poly cube_pts cube_tris
-- to compensate for floating point errors we compare but roughly, in dubio pro equality
precision :: Double
precision = 0.00001
(===) :: Double -> Double -> Bool
a === b = abs(a-b) < precision
(=/=) :: Double -> Double -> Bool
a =/= b = not $ a === b
(<<<) :: Double -> Double -> Bool
a <<< b = b-a > precision
(>>>) :: Double -> Double -> Bool
a >>> b = a-b > precision
(<=<) :: Double -> Double -> Bool
a <=< b = b-a >= -precision
(>=>) :: Double -> Double -> Bool
a >=> b = a-b >= -precision
-- construct memoizing structures from basic data
plane :: [Point] -> Plane
plane p = Plane n (dnull p n)
where n = nnormal p
poly :: [Point] -> [Triangle] -> Polyhedron
poly p t = Polyhedron p t (extent p)
pgon :: [Point] -> Polygon
pgon p = P p (plane p) (extent p)
polyFromList :: Polyset -> Polyhedron
polyFromList ps = poly pts tris
where pts = S.toList $ S.fromList (concat pp)
tris = map (map (\pt-> fromJust (L.elemIndex pt pts))) pp
points (P pts _ _) = pts
pp = map points ps
byIndex :: [Point] -> [Triangle] -> Polyset
byIndex pts tris = map (pgon.(map (pts!!))) tris
nnormal :: [Point] -> Vector
nnormal tri = norm $ normal tri
normal :: [Point] -> Vector
normal tri = cross u v
where u = vsub (tri!!1) (tri!!0)
v = vsub (tri!!2) (tri!!0)
extent :: [Point] -> Extent
extent pts = (map minimum list, map maximum list)
where list = transpose pts
pairs :: [Int] -> [(Int,Int)]
pairs l = [(x,y) | x<-l, y<-l, x/=y]
adjacent :: [[Int]] -> (Map Int (Set Int))
adjacent tris = foldl' (\m-> \p->insertWith (S.union) (fst p) (S.singleton (snd p)) m) M.empty $ concatMap pairs tris
-- todo: merge these two
overlaps :: Extent -> Extent -> Bool
overlaps (a0, a1) (b0, b1) = and (zipWith (<=<) a0 b1) && (and (zipWith (>=>) a1 b0))
overlap :: [(Double, Point)] -> [(Double, Point)] -> [(Double, Point)]
overlap a b = [max (a!!0) (b!!0), min (a!!1) (b!!1)]
lreverse :: Polyset -> Polyset
lreverse p = map reversePoly p
where reversePoly (P pp (Plane pn d) pe) = P (L.reverse pp) (Plane (vmul pn (-1)) (-d)) pe
csgUnion :: Polyhedron -> Polyhedron -> Polyhedron
csgUnion (Polyhedron pa ta ea) (Polyhedron pb tb eb)
= polyFromList $ nub (a ++ (b ++ (c ++ d)))
where (a, a', b', b) = splitBy ppa ea ppb eb
c = filter ((\x->(x == Outside) || (x == BoundarySame)).(inOrOut ppb)) a'
d = filter ((==Outside).(inOrOut ppa)) b'
ppa = byIndex pa ta
ppb = byIndex pb tb
csgInter :: Polyhedron -> Polyhedron -> Polyhedron
csgInter (Polyhedron pa ta ea) (Polyhedron pb tb eb)
= polyFromList $ nub (c ++ d)
where (a, a', b', b) = splitBy ppa ea ppb eb
c = filter ((\x->(x == Inside) || (x == BoundarySame)).(inOrOut ppb)) a'
d = filter ((==Inside).(inOrOut ppa)) b'
ppa = byIndex pa ta
ppb = byIndex pb tb
csgDiff :: Polyhedron -> Polyhedron -> Polyhedron
csgDiff (Polyhedron pa ta ea) (Polyhedron pb tb eb)
= polyFromList $ nub (a ++ (c ++ d))
where (a, a', b', b) = splitBy ppa ea ppb eb
c = filter ((\x->(x == Outside) || (x == BoundaryOpposite)).(inOrOut ppb)) a'
d = lreverse $ filter ((==Inside).(inOrOut ppa)) b'
ppa = byIndex pa ta
ppb = byIndex pb tb
splitBy :: Polyset -> Extent -> Polyset -> Extent -> (Polyset, Polyset, Polyset, Polyset)
-- returns all A outside B, all A inside B (subsected), all B inside A (subsected), all B outside A
splitBy ppa ea ppb eb =
if overlaps ea eb then
(snd all_pa, iter3, iter4, snd all_pb)
else (ppa, [], [], ppb)
where
iter4 = splitHbyH iter2 iter1
iter3 = splitHbyH iter1 iter2
iter2 = splitHbyH (fst all_pb) (fst all_pa)
iter1 = splitHbyH (fst all_pa) (fst all_pb)
all_pa = L.partition (\p->overlaps (ext p) eb) ppa
all_pb = L.partition (\p->overlaps (ext p) ea) ppb
ext :: Polygon -> Extent
ext (P _ _ e) = e
splitHbyP :: Polyset -> Polygon -> Polyset -- split all polys in a hedron by a single poly
splitHbyP as b = concatMap (snd.(split b)) as
where (P pb plb eb) = b
splitHbyH :: Polyset -> Polyset -> Polyset
splitHbyH as bs = foldl' splitHbyP as bs
vmul :: Vector -> Double -> Vector
vmul v d = map (*d) v
vmulv :: Vector -> Vector -> Vector
vmulv v v' = zipWith (*) v v'
vdiv :: Vector -> Double -> Vector
vdiv v d = map (/d) v
vsub :: Vector -> Vector -> Vector
vsub a b = zipWith (-) a b
vadd :: Vector -> Vector -> Vector
vadd a b = zipWith (+) a b
norm :: Vector -> Vector
norm a = a `vdiv` (len a)
dot :: Vector -> Vector -> Double
dot [] [] = 0
dot (x:xs) (y:ys) = x*y + dot xs ys
cross :: Vector -> Vector -> Vector
cross u v =[b * z - c * y,
c * x - a * z,
a * y - b * x]
where a = u!!0
b = u!!1
c = u!!2
x = v!!0
y = v!!1
z = v!!2
len :: Vector -> Double
len v = sqrt $ len2 v
len2 :: Vector -> Double
len2 v = dot v v
avg :: [Point] -> Point
avg po = vdiv (foldl1 vadd po) 3
dnull :: [Point] -> Vector -> Double
dnull p n = sum $ zipWith (*) n (p!!0)
dist :: Plane -> Point -> Double
dist (Plane n d) pt = pt `dot` n - d
intersect :: Polygon -> Polygon -> Relation
intersect a b = case cmp of
[EQ, EQ, EQ] -> Coplanar
[LT, LT, LT] -> DoNotIntersect
[GT, GT, GT] -> DoNotIntersect
otherwise ->
if (length sega > 0) && (length segb > 0) && (overlaps (extl sega) (extl segb))
then Intersect (Intersection over int) else DoNotIntersect
where rela = map (dist ppb) pa
relb = map (dist ppa) pb
cmp = map (compare 0) rela
int = interLine ppa ppb
over = overlap sega segb
(Plane na da) = ppa
(Plane nb db) = ppb
(P pa ppa ea) = a
(P pb ppb eb) = b
sega = interSeg a rela int
segb = interSeg b relb int
extl y = ([fst$y!!0], [fst$y!!1])
interLine :: Plane -> Plane -> Line
interLine (Plane np dp) (Plane nq dq)
| n!!0 =/= 0 = Line [0, s10, s11] n
| n!!2 =/= 0 = Line [s20, s21, 0] n
| otherwise = Line [s30, 0, s31] n
where n = cross np nq
(s10, s11) = solve (np!!1) (np!!2) dp (nq!!1) (nq!!2) dq
(s20, s21) = solve (np!!0) (np!!1) dp (nq!!0) (nq!!1) dq
(s30, s31) = solve (np!!0) (np!!2) dp (nq!!0) (nq!!2) dq
dobq :: (Ord a) => [a] -> [a]
dobq v | length v == 1 = v++v
| otherwise = v
interSeg :: Polygon -> [Double] -> Line -> [(Double, Point)]
interSeg po da l = dobq $ sort $ plist
where plist = concat ((interSegV po da l) ++ (interSegE po da l))
interSegV :: Polygon -> [Double] -> Line -> [[(Double, Vector)]]
interSegV (P po pp pe) da (Line p v) =
[if da!!i === 0
then [(len $ vsub pi p, pi)]
else []
| i<-[0..2],
let pi = po!!i
]
interSegE :: Polygon -> [Double] -> Line -> [[(Double, Vector)]]
interSegE (P po pp pe) da (Line p v) =
[if (di>>>0) && (dj<<<0)
then [(len $ vsub ip p, ip)]
else []
| i<-[0..2], j<-[0..2],
let pi = po!!i,
let pj = po!!j,
let di = da!!i,
let dj = da!!j,
let ip = vadd pi (vmul (vsub pj pi) ((-di)/(dj-di)))
]
collinear :: Polygon -> Bool
collinear (P po pp pe) = len2 (normal po) === 0
inPoly :: Polygon -> Point -> Bool -- including boundaries
inPoly (P po pp pe) pt =
(u >=> 0) && (v >=> 0) && ((u+v) <=< 1) -- obviously cloned from a non-functional source
where u = (d11 * d02 - d01 * d12) * inv
v = (d00 * d12 - d01 * d02) * inv
inv = 1 / (d00 * d11 - d01 * d01)
d00 = len2 v0
d01 = v0 `dot` v1
d02 = v0 `dot` v2
d11 = len2 v1
d12 = v1 `dot` v2
v0 = (po!!2) `vsub` p0
v1 = (po!!1) `vsub` p0
v2 = pt `vsub` p0
p0 = po!!0
trisect :: Polygon -> Point -> Polyset
trisect (P po pp pe) pt =
filter (not.collinear) $ map (pgon.(pt:)) pairs
where pairs = [last po, head po] : zipWith (\x-> \y-> [x,y]) po (tail po)
subsect :: Polyset -> Point -> Polyset
subsect ps pt = if length ps == 1 then
trisect (ps!!0) pt
else
concat subs
where subs = map (\po-> if inPoly po pt then trisect po pt else [po]) ps
split :: Polygon -> Polygon -> (Polyset, Polyset)
split a b = if overlaps ea eb then
case intersect a b of
Intersect (Intersection p l) ->
(subsect (trisect a (snd$p!!0)) (snd$p!!1),
subsect (trisect b (snd$p!!0)) (snd$p!!1))
Coplanar ->
let abyb = if any (inPoly a) pb then
foldl splitHbyP [a] (shuffles b (pnormal plb))
else [a]
bbya = if any (inPoly b) pa then
foldl splitHbyP [b] (shuffles a (pnormal pla))
else [b]
shuffles (P p pp pe) n = map pgon
[head p `vadd` n : tail p,
[head p, p!!1 `vadd` n, last p],
last p `vadd` n : init p]
in (abyb, bbya)
otherwise -> ([a],[b])
else ([a],[b])
where (P pa pla ea) = a
(P pb plb eb) = b
solve :: Double -> Double -> Double -> Double -> Double -> Double -> (Double, Double)
solve a b c d e f = ((b*f - c*e) / (-den),
(a*f - c*d) / (den))
where den = (a*e - b*d)
ray :: Polygon -> Line
ray (P po (Plane pn pd) _) = Line (avg po) pn
intPt :: Line -> Double -> Point
intPt (Line p0 n) dist = vadd p0 (vmul n dist)
perturb :: Line -> Line
perturb (Line p n) = Line p (vadd n [pi/97, -pi/101, pi/103]) -- random would be better
distPL :: Plane -> Point -> Line -> Double
distPL (Plane pn d) p0 (Line r rn) = (vsub p0 r) `dot` pn / (rn `dot` pn)
pnormal :: Plane -> Vector
pnormal (Plane n _) = n
pplane :: Polygon -> Plane
pplane (P _ p _) = p
debug :: (Show a) => String -> a -> a
debug msg a = trace ("\n["++msg++"] "++(show a)++"\n") a
type RelVector = (Double, Double, Double, Polygon)
fixOrientation :: Polyset -> Polyset
fixOrientation pp = a ++ (lreverse b)
where (a,b) = L.partition (hitsOdd pp) pp
hitsOdd ps p = let r = ray p in odd $ length $ nub $ map fst4 $ filter (analyze r) $ map (relVector r) pp
fst4 (a,_,_,_) = a
fixOrientation' :: Polyhedron -> Polyhedron
fixOrientation' (Polyhedron pp pt pe) = polyFromList $ fixOrientation $ byIndex pp pt
-- calculate all the distances needed to identify the position of a polygon relative to a ray
relVector :: Line -> Polygon -> RelVector
relVector r (P px plx ex) = (distPL plx (px!!0) r, dist plx bary, rn `dot` (pnormal plx), (P px plx ex))
where (Line bary rn) = r
inOrOut :: Polyset -> Polygon -> Status
inOrOut pp po = classify $ take 1 $ L.sortBy dabs $ filter (analyze r) $ map (relVector r) pp
where
r = ray po
dabs (a,_,_,_) (e,_,_,_) = compare (abs a) (abs e)
analyze :: Line -> RelVector -> Bool
analyze (Line bary rn) (dis, _, pro, p)
| dis <<< 0 = False
| isNaN dis = False
| dis >>> 0 && pro === 0 = False
| dis === 0 && pro =/= 0 = inPoly p bary
| dis >>> 0 && pro =/= 0 = inPoly p (intPt (Line bary rn) dis)
| dis === 0 && pro === 0 = let (Line bary' rn') = perturb (Line bary rn)
dis' = dist (pplane p) bary'
pro' = rn' `dot` (pnormal$pplane p)
in analyze (Line bary rn) (dis', 0, pro', p)
classify :: [RelVector] -> Status
classify [] = Outside
classify ((_, dxp, pxp, p):_) | dxp >>> 0 = Outside
| dxp <<< 0 = Inside
| pxp >>> 0 = BoundarySame
| pxp <<< 0 = BoundaryOpposite