voxelize_rs/
lib.rs

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(&center, &halfsize, &tri) {
383                        result.push(Vector3::new(x, y, z));
384                    }
385                }
386            }
387        }
388    }
389    result
390}