1pub use nalgebra::Vector3;
2use std::f32;
3
4pub struct Aabb {
5 min: Vector3<f32>,
6 max: Vector3<f32>,
7}
8
9impl Aabb {
10 pub fn new() -> Aabb {
11 Aabb {
12 min: Vector3::new(f32::INFINITY, f32::INFINITY, f32::INFINITY),
13 max: Vector3::new(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY),
14 }
15 }
16
17 pub fn expand(&mut self, p: &Vector3<f32>) {
18 if self.min.x > p.x {
19 self.min.x = p.x
20 }
21
22 if self.min.y > p.y {
23 self.min.y = p.y
24 }
25
26 if self.min.z > p.z {
27 self.min.z = p.z
28 }
29
30 if self.max.x < p.x {
31 self.max.x = p.x
32 }
33
34 if self.max.y < p.y {
35 self.max.y = p.y
36 }
37
38 if self.max.z < p.z {
39 self.max.z = p.z
40 }
41 }
42}
43
44pub struct Triangle {
45 p0: Vector3<f32>,
46 p1: Vector3<f32>,
47 p2: Vector3<f32>,
48}
49
50impl Triangle {
51 pub fn new(p0: Vector3<f32>, p1: Vector3<f32>, p2: Vector3<f32>) -> Triangle {
52 Triangle { p0, p1, p2 }
53 }
54
55 pub fn aabb(&self) -> Aabb {
56 let mut aabb = Aabb::new();
57 aabb.expand(&self.p0);
58 aabb.expand(&self.p1);
59 aabb.expand(&self.p2);
60 aabb
61 }
62}
63
64fn clamp_to_voxel_min(p: f32, size: f32) -> f32 {
65 let vox = (p + p.signum() * size * 0.5) / size;
66 vox.floor() * size
67}
68
69fn clamp_to_voxel_max(p: f32, size: f32) -> f32 {
70 let vox = (p + p.signum() * size * 0.5) / size;
71 vox.ceil() * size
72}
73
74fn find_min_max(x0: f32, x1: f32, x2: f32) -> (f32, f32) {
75 let mut min = x0;
76 let mut max = x0;
77
78 if x1 < min {
79 min = x1;
80 }
81 if x1 > max {
82 max = x1;
83 }
84 if x2 < min {
85 min = x2;
86 }
87 if x2 > max {
88 max = x2;
89 }
90
91 (min, max)
92}
93
94fn plane_box_overlap(normal: &Vector3<f32>, vert: &Vector3<f32>, maxbox: &Vector3<f32>) -> bool {
95 let mut vmin = Vector3::new(0.0f32, 0.0f32, 0.0f32);
96 let mut vmax = Vector3::new(0.0f32, 0.0f32, 0.0f32);
97
98 if normal.x > 0.0 {
99 vmin.x = -maxbox.x - vert.x;
100 vmax.x = maxbox.x - vert.x;
101 } else {
102 vmin.x = maxbox.x - vert.x;
103 vmax.x = -maxbox.x - vert.x;
104 }
105
106 if normal.y > 0.0 {
107 vmin.y = -maxbox.y - vert.y;
108 vmax.y = maxbox.y - vert.y;
109 } else {
110 vmin.y = maxbox.y - vert.y;
111 vmax.y = -maxbox.y - vert.y;
112 }
113
114 if normal.z > 0.0 {
115 vmin.z = -maxbox.z - vert.z;
116 vmax.z = maxbox.z - vert.z;
117 } else {
118 vmin.z = maxbox.z - vert.z;
119 vmax.z = -maxbox.z - vert.z;
120 }
121
122 if normal.dot(&vmin) > 0.0 {
123 return false;
124 }
125 if normal.dot(&vmax) >= 0.0 {
126 return true;
127 }
128
129 false
130}
131
132fn axis_test_x01(
133 a: f32,
134 b: f32,
135 fa: f32,
136 fb: f32,
137 v0: &Vector3<f32>,
138 v2: &Vector3<f32>,
139 boxhalfsize: &Vector3<f32>,
140) -> bool {
141 let p0 = a * v0.y - b * v0.z;
142 let p2 = a * v2.y - b * v2.z;
143
144 let (min, max) = if p0 < p2 { (p0, p2) } else { (p2, p0) };
145
146 let rad = fa * boxhalfsize.y + fb * boxhalfsize.z;
147
148 if min > rad || max < -rad {
149 return false;
150 }
151
152 true
153}
154
155fn axis_test_x2(
156 a: f32,
157 b: f32,
158 fa: f32,
159 fb: f32,
160 v0: &Vector3<f32>,
161 v1: &Vector3<f32>,
162 boxhalfsize: &Vector3<f32>,
163) -> bool {
164 let p0 = a * v0.y - b * v0.z;
165 let p1 = a * v1.y - b * v1.z;
166
167 let (min, max) = if p0 < p1 { (p0, p1) } else { (p1, p0) };
168
169 let rad = fa * boxhalfsize.y + fb * boxhalfsize.z;
170
171 if min > rad || max < -rad {
172 return false;
173 }
174
175 true
176}
177
178fn axis_test_y02(
179 a: f32,
180 b: f32,
181 fa: f32,
182 fb: f32,
183 v0: &Vector3<f32>,
184 v2: &Vector3<f32>,
185 boxhalfsize: &Vector3<f32>,
186) -> bool {
187 let p0 = -a * v0.x - b * v0.z;
188 let p2 = -a * v2.x - b * v2.z;
189
190 let (min, max) = if p0 < p2 { (p0, p2) } else { (p2, p0) };
191
192 let rad = fa * boxhalfsize.x + fb * boxhalfsize.z;
193
194 if min > rad || max < -rad {
195 return false;
196 }
197
198 true
199}
200
201fn axis_test_y1(
202 a: f32,
203 b: f32,
204 fa: f32,
205 fb: f32,
206 v0: &Vector3<f32>,
207 v1: &Vector3<f32>,
208 boxhalfsize: &Vector3<f32>,
209) -> bool {
210 let p0 = -a * v0.x - b * v0.z;
211 let p1 = -a * v1.x - b * v1.z;
212
213 let (min, max) = if p0 < p1 { (p0, p1) } else { (p1, p0) };
214
215 let rad = fa * boxhalfsize.x + fb * boxhalfsize.z;
216
217 if min > rad || max < -rad {
218 return false;
219 }
220
221 true
222}
223
224fn axis_test_z12(
225 a: f32,
226 b: f32,
227 fa: f32,
228 fb: f32,
229 v1: &Vector3<f32>,
230 v2: &Vector3<f32>,
231 boxhalfsize: &Vector3<f32>,
232) -> bool {
233 let p1 = a * v1.x - b * v1.y;
234 let p2 = a * v2.x - b * v2.y;
235
236 let (min, max) = if p1 < p2 { (p1, p2) } else { (p2, p1) };
237
238 let rad = fa * boxhalfsize.x + fb * boxhalfsize.y;
239
240 if min > rad || max < -rad {
241 return false;
242 }
243
244 true
245}
246
247fn axis_test_z0(
248 a: f32,
249 b: f32,
250 fa: f32,
251 fb: f32,
252 v0: &Vector3<f32>,
253 v1: &Vector3<f32>,
254 boxhalfsize: &Vector3<f32>,
255) -> bool {
256 let p0 = a * v0.x - b * v0.y;
257 let p1 = a * v1.x - b * v1.y;
258
259 let (min, max) = if p0 < p1 { (p0, p1) } else { (p1, p0) };
260
261 let rad = fa * boxhalfsize.x + fb * boxhalfsize.y;
262
263 if min > rad || max < -rad {
264 return false;
265 }
266
267 true
268}
269
270fn triangle_box_overlap(
271 boxcenter: &Vector3<f32>,
272 boxhalfsize: &Vector3<f32>,
273 tri: &Triangle,
274) -> bool {
275 let v0 = tri.p0 - boxcenter;
276 let v1 = tri.p1 - boxcenter;
277 let v2 = tri.p2 - boxcenter;
278 let e0 = v1 - v0;
279 let e1 = v2 - v1;
280 let e2 = v0 - v2;
281
282 {
283 let fex = e0.x.abs();
284 let fey = e0.y.abs();
285 let fez = e0.z.abs();
286 if !axis_test_x01(e0.z, e0.y, fez, fey, &v0, &v2, &boxhalfsize) {
287 return false;
288 }
289 if !axis_test_y02(e0.z, e0.x, fez, fex, &v0, &v2, &boxhalfsize) {
290 return false;
291 }
292 if !axis_test_z12(e0.y, e0.x, fey, fex, &v1, &v2, &boxhalfsize) {
293 return false;
294 }
295 }
296
297 {
298 let fex = e1.x.abs();
299 let fey = e1.y.abs();
300 let fez = e1.z.abs();
301 if !axis_test_x01(e1.z, e1.y, fez, fey, &v0, &v2, &boxhalfsize) {
302 return false;
303 }
304 if !axis_test_y02(e1.z, e1.x, fez, fex, &v0, &v2, &boxhalfsize) {
305 return false;
306 }
307 if !axis_test_z0(e1.y, e1.x, fey, fex, &v0, &v1, &boxhalfsize) {
308 return false;
309 }
310 }
311
312 {
313 let fex = e2.x.abs();
314 let fey = e2.y.abs();
315 let fez = e2.z.abs();
316 if !axis_test_x2(e2.z, e2.y, fez, fey, &v0, &v1, &boxhalfsize) {
317 return false;
318 }
319 if !axis_test_y1(e2.z, e2.x, fez, fex, &v0, &v1, &boxhalfsize) {
320 return false;
321 }
322 if !axis_test_z12(e2.y, e2.x, fey, fex, &v1, &v2, &boxhalfsize) {
323 return false;
324 }
325 }
326
327 let (min, max) = find_min_max(v0.x, v1.x, v2.x);
328 if min > boxhalfsize.x || max < -boxhalfsize.x {
329 return false;
330 }
331
332 let (min, max) = find_min_max(v0.y, v1.y, v2.y);
333 if min > boxhalfsize.y || max < -boxhalfsize.y {
334 return false;
335 }
336
337 let (min, max) = find_min_max(v0.z, v1.z, v2.z);
338 if min > boxhalfsize.z || max < -boxhalfsize.z {
339 return false;
340 }
341
342 let normal = e0.cross(&e1);
343 if !plane_box_overlap(&normal, &v0, &boxhalfsize) {
344 return false;
345 }
346
347 true
348}
349
350pub fn voxelize(triangles: &Vec<Triangle>, size: &Vector3<f32>) -> Vec<Vector3<f32>> {
351 let hs = size * 0.5;
352 let mut result = vec![];
353 for tri in triangles {
354 let mut aabb = tri.aabb();
355
356 aabb.min.x = clamp_to_voxel_min(aabb.min.x, size.x);
357 aabb.min.y = clamp_to_voxel_min(aabb.min.y, size.y);
358 aabb.min.z = clamp_to_voxel_min(aabb.min.z, size.z);
359 aabb.max.x = clamp_to_voxel_max(aabb.max.x, size.x);
360 aabb.max.y = clamp_to_voxel_max(aabb.max.y, size.y);
361 aabb.max.z = clamp_to_voxel_max(aabb.max.z, size.z);
362
363 let range_x = ((aabb.max.x - aabb.min.x) / size.x) as i32;
364 let range_y = ((aabb.max.y - aabb.min.y) / size.y) as i32;
365 let range_z = ((aabb.max.z - aabb.min.z) / size.z) as i32;
366
367 for iz in 0..range_z {
368 let z = aabb.min.z + (iz as f32) * size.z;
369 for iy in 0..range_y {
370 let y = aabb.min.y + (iy as f32) * size.y;
371 for ix in 0..range_x {
372 let x = aabb.min.x + (ix as f32) * size.x;
373
374 let local_aabb = Aabb {
375 min: Vector3::new(x - hs.x, y - hs.y, z - hs.z),
376 max: Vector3::new(x + hs.x, y + hs.y, z + hs.z),
377 };
378
379 let halfsize = (local_aabb.max - local_aabb.min) * 0.5;
380 let center = local_aabb.min + halfsize;
381
382 if triangle_box_overlap(¢er, &halfsize, &tri) {
383 result.push(Vector3::new(x, y, z));
384 }
385 }
386 }
387 }
388 }
389 result
390}