]> gitweb.michael.orlitzky.com - spline3.git/blob - src/Tetrahedron.hs
Fix the tetrahedron collision detection.
[spline3.git] / src / Tetrahedron.hs
1 module Tetrahedron
2 where
3
4 import Numeric.LinearAlgebra hiding (i, scale)
5 import Prelude hiding (LT)
6 import Test.QuickCheck (Arbitrary(..), Gen)
7
8 import Cardinal
9 import Comparisons (nearly_ge)
10 import FunctionValues
11 import Misc (factorial)
12 import Point
13 import RealFunction
14 import ThreeDimensional
15
16 data Tetrahedron = Tetrahedron { fv :: FunctionValues,
17 v0 :: Point,
18 v1 :: Point,
19 v2 :: Point,
20 v3 :: Point }
21 deriving (Eq)
22
23
24 instance Arbitrary Tetrahedron where
25 arbitrary = do
26 rnd_v0 <- arbitrary :: Gen Point
27 rnd_v1 <- arbitrary :: Gen Point
28 rnd_v2 <- arbitrary :: Gen Point
29 rnd_v3 <- arbitrary :: Gen Point
30 rnd_fv <- arbitrary :: Gen FunctionValues
31 return (Tetrahedron rnd_fv rnd_v0 rnd_v1 rnd_v2 rnd_v3)
32
33
34 instance Show Tetrahedron where
35 show t = "Tetrahedron:\n" ++
36 " fv: " ++ (show (fv t)) ++ "\n" ++
37 " v0: " ++ (show (v0 t)) ++ "\n" ++
38 " v1: " ++ (show (v1 t)) ++ "\n" ++
39 " v2: " ++ (show (v2 t)) ++ "\n" ++
40 " v3: " ++ (show (v3 t)) ++ "\n"
41
42
43 instance ThreeDimensional Tetrahedron where
44 center t = ((v0 t) + (v1 t) + (v2 t) + (v3 t)) `scale` (1/4)
45 contains_point t p =
46 (b0 t p) `nearly_ge` 0 &&
47 (b1 t p) `nearly_ge` 0 &&
48 (b2 t p) `nearly_ge` 0 &&
49 (b3 t p) `nearly_ge` 0
50
51
52 polynomial :: Tetrahedron -> (RealFunction Point)
53 polynomial t =
54 sum [ (c t i j k l) `cmult` (beta t i j k l) | i <- [0..3],
55 j <- [0..3],
56 k <- [0..3],
57 l <- [0..3],
58 i + j + k + l == 3]
59
60
61 -- | Returns the domain point of t with indices i,j,k,l.
62 -- Simply an alias for the domain_point function.
63 xi :: Tetrahedron -> Int -> Int -> Int -> Int -> Point
64 xi = domain_point
65
66 -- | Returns the domain point of t with indices i,j,k,l.
67 domain_point :: Tetrahedron -> Int -> Int -> Int -> Int -> Point
68 domain_point t i j k l
69 | i + j + k + l == 3 = weighted_sum `scale` (1/3)
70 | otherwise = error "domain point index out of bounds"
71 where
72 v0' = (v0 t) `scale` (fromIntegral i)
73 v1' = (v1 t) `scale` (fromIntegral j)
74 v2' = (v2 t) `scale` (fromIntegral k)
75 v3' = (v3 t) `scale` (fromIntegral l)
76 weighted_sum = v0' + v1' + v2' + v3'
77
78
79 -- | The Bernstein polynomial on t with indices i,j,k,l. Denoted by a
80 -- capital 'B' in the Sorokina/Zeilfelder paper.
81 beta :: Tetrahedron -> Int -> Int -> Int -> Int -> (RealFunction Point)
82 beta t i j k l
83 | (i + j + k + l == 3) =
84 coefficient `cmult` (b0_term * b1_term * b2_term * b3_term)
85 | otherwise = error "basis function index out of bounds"
86 where
87 denominator = (factorial i)*(factorial j)*(factorial k)*(factorial l)
88 coefficient = 6 / (fromIntegral denominator)
89 b0_term = (b0 t) `fexp` i
90 b1_term = (b1 t) `fexp` j
91 b2_term = (b2 t) `fexp` k
92 b3_term = (b3 t) `fexp` l
93
94
95 -- | The coefficient function. c t i j k l returns the coefficient
96 -- c_ijkl with respect to the tetrahedron t. The definition uses
97 -- pattern matching to mimic the definitions given in Sorokina and
98 -- Zeilfelder, pp. 84-86. If incorrect indices are supplied, the
99 -- function will simply error.
100 c :: Tetrahedron -> Int -> Int -> Int -> Int -> Double
101 c t 0 0 3 0 = eval (fv t) $
102 (1/8) * (I + F + L + T + LT + FL + FT + FLT)
103
104 c t 0 0 0 3 = eval (fv t) $
105 (1/8) * (I + F + R + T + RT + FR + FT + FRT)
106
107 c t 0 0 2 1 = eval (fv t) $
108 (5/24)*(I + F + T + FT) +
109 (1/24)*(L + FL + LT + FLT)
110
111 c t 0 0 1 2 = eval (fv t) $
112 (5/24)*(I + F + T + FT) +
113 (1/24)*(R + FR + RT + FRT)
114
115 c t 0 1 2 0 = eval (fv t) $
116 (5/24)*(I + F) +
117 (1/8)*(L + T + FL + FT) +
118 (1/24)*(LT + FLT)
119
120 c t 0 1 0 2 = eval (fv t) $
121 (5/24)*(I + F) +
122 (1/8)*(R + T + FR + FT) +
123 (1/24)*(RT + FRT)
124
125 c t 0 1 1 1 = eval (fv t) $
126 (13/48)*(I + F) +
127 (7/48)*(T + FT) +
128 (1/32)*(L + R + FL + FR) +
129 (1/96)*(LT + RT + FLT + FRT)
130
131 c t 0 2 1 0 = eval (fv t) $
132 (13/48)*(I + F) +
133 (17/192)*(L + T + FL + FT) +
134 (1/96)*(LT + FLT) +
135 (1/64)*(R + D + FR + FD) +
136 (1/192)*(RT + LD + FRT + FLD)
137
138 c t 0 2 0 1 = eval (fv t) $
139 (13/48)*(I + F) +
140 (17/192)*(R + T + FR + FT) +
141 (1/96)*(RT + FRT) +
142 (1/64)*(L + D + FL + FD) +
143 (1/192)*(RD + LT + FLT + FRD)
144
145 c t 0 3 0 0 = eval (fv t) $
146 (13/48)*(I + F) +
147 (5/96)*(L + R + T + D + FL + FR + FT + FD) +
148 (1/192)*(RT + RD + LT + LD + FRT + FRD + FLT + FLD)
149
150 c t 1 0 2 0 = eval (fv t) $
151 (1/4)*I +
152 (1/6)*(F + L + T) +
153 (1/12)*(LT + FL + FT)
154
155 c t 1 0 0 2 = eval (fv t) $
156 (1/4)*I +
157 (1/6)*(F + R + T) +
158 (1/12)*(RT + FR + FT)
159
160 c t 1 0 1 1 = eval (fv t) $
161 (1/3)*I +
162 (5/24)*(F + T) +
163 (1/12)*FT +
164 (1/24)*(L + R) +
165 (1/48)*(LT + RT + FL + FR)
166
167 c t 1 1 1 0 = eval (fv t) $
168 (1/3)*I +
169 (5/24)*F +
170 (1/8)*(L + T) +
171 (5/96)*(FL + FT) +
172 (1/48)*(D + R + LT) +
173 (1/96)*(FD + LD + RT + FR)
174
175 c t 1 1 0 1 = eval (fv t) $
176 (1/3)*I +
177 (5/24)*F +
178 (1/8)*(R + T) +
179 (5/96)*(FR + FT) +
180 (1/48)*(D + L + RT) +
181 (1/96)*(FD + LT + RD + FL)
182
183 c t 1 2 0 0 = eval (fv t) $
184 (1/3)*I +
185 (5/24)*F +
186 (7/96)*(L + R + T + D) +
187 (1/32)*(FL + FR + FT + FD) +
188 (1/96)*(RT + RD + LT + LD)
189
190 c t 2 0 1 0 = eval (fv t) $
191 (3/8)*I +
192 (7/48)*(F + T + L) +
193 (1/48)*(R + D + B + LT + FL + FT) +
194 (1/96)*(RT + BT + FR + FD + LD + BL)
195
196 c t 2 0 0 1 = eval (fv t) $
197 (3/8)*I +
198 (7/48)*(F + T + R) +
199 (1/48)*(L + D + B + RT + FR + FT) +
200 (1/96)*(LT + BT + FL + FD + RD + BR)
201
202 c t 2 1 0 0 = eval (fv t) $
203 (3/8)*I +
204 (1/12)*(T + R + L + D) +
205 (1/64)*(FT + FR + FL + FD) +
206 (7/48)*F +
207 (1/48)*B +
208 (1/96)*(RT + LD + LT + RD) +
209 (1/192)*(BT + BR + BL + BD)
210
211 c t 3 0 0 0 = eval (fv t) $
212 (3/8)*I +
213 (1/12)*(T + F + L + R + D + B) +
214 (1/96)*(LT + FL + FT + RT + BT + FR) +
215 (1/96)*(FD + LD + BD + BR + RD + BL)
216
217 c _ _ _ _ _ = error "coefficient index out of bounds"
218
219
220
221 -- | The matrix used in the tetrahedron volume calculation as given in
222 -- Lai & Schumaker, Definition 15.4, page 436.
223 vol_matrix :: Tetrahedron -> Matrix Double
224 vol_matrix t = (4><4)
225 [1, 1, 1, 1,
226 x1, x2, x3, x4,
227 y1, y2, y3, y4,
228 z1, z2, z3, z4 ]
229 where
230 (x1, y1, z1) = v0 t
231 (x2, y2, z2) = v1 t
232 (x3, y3, z3) = v2 t
233 (x4, y4, z4) = v3 t
234
235 -- | Computed using the formula from Lai & Schumaker, Definition 15.4,
236 -- page 436.
237 volume :: Tetrahedron -> Double
238 volume t
239 | (v0 t) == (v1 t) = 0
240 | (v0 t) == (v2 t) = 0
241 | (v0 t) == (v3 t) = 0
242 | (v1 t) == (v2 t) = 0
243 | (v1 t) == (v3 t) = 0
244 | (v2 t) == (v3 t) = 0
245 | otherwise = (1/6)*(det (vol_matrix t))
246
247
248 -- | The barycentric coordinates of a point with respect to v0.
249 b0 :: Tetrahedron -> (RealFunction Point)
250 b0 t point = (volume inner_tetrahedron) / (volume t)
251 where
252 inner_tetrahedron = t { v0 = point }
253
254
255 -- | The barycentric coordinates of a point with respect to v1.
256 b1 :: Tetrahedron -> (RealFunction Point)
257 b1 t point = (volume inner_tetrahedron) / (volume t)
258 where
259 inner_tetrahedron = t { v1 = point }
260
261
262 -- | The barycentric coordinates of a point with respect to v2.
263 b2 :: Tetrahedron -> (RealFunction Point)
264 b2 t point = (volume inner_tetrahedron) / (volume t)
265 where
266 inner_tetrahedron = t { v2 = point }
267
268
269 -- | The barycentric coordinates of a point with respect to v3.
270 b3 :: Tetrahedron -> (RealFunction Point)
271 b3 t point = (volume inner_tetrahedron) / (volume t)
272 where
273 inner_tetrahedron = t { v3 = point }