ncollide3d/transformation/
convex_hull3.rs

1use num::Bounded;
2use std::cmp::Ordering;
3
4use crate::procedural::{IndexBuffer, TriMesh};
5use crate::transformation::{
6    self,
7    convex_hull_utils::{denormalize, indexed_support_point_id, normalize, support_point_id},
8};
9use crate::utils;
10use na::{self, Matrix3, Point2, Point3, RealField, Vector3};
11
12/// Computes the convariance matrix of a set of points.
13#[cfg(not(feature = "improved_fixed_point_support"))]
14fn cov<N: RealField + Copy>(pts: &[Point3<N>]) -> Matrix3<N> {
15    let center = utils::center(pts);
16    let mut cov: Matrix3<N> = na::zero();
17    let normalizer: N = na::convert(1.0 / (pts.len() as f64));
18
19    for p in pts.iter() {
20        let cp = *p - center;
21        cov = cov + cp * (cp * normalizer).transpose();
22    }
23
24    cov
25}
26
27/// Computes the convex hull of a set of 3d points.
28pub fn convex_hull3<N: RealField + Copy>(points: &[Point3<N>]) -> TriMesh<N> {
29    assert!(
30        points.len() != 0,
31        "Cannot compute the convex hull of an empty set of point."
32    );
33
34    let mut points = points.to_vec();
35
36    let (norm_center, norm_diag) = normalize(&mut points[..]);
37
38    let mut undecidable_points = Vec::new();
39    let mut horizon_loop_facets = Vec::new();
40    let mut horizon_loop_ids = Vec::new();
41    let mut removed_facets = Vec::new();
42
43    let mut triangles;
44    let denormalizer;
45
46    match get_initial_mesh(&mut points[..], &mut undecidable_points) {
47        InitialMesh::Facets(facets, denorm) => {
48            triangles = facets;
49            denormalizer = denorm;
50        }
51        InitialMesh::ResultMesh(mut mesh) => {
52            denormalize(&mut mesh.coords[..], &norm_center, norm_diag);
53            return mesh;
54        }
55    }
56
57    let mut i = 0;
58    while i != triangles.len() {
59        horizon_loop_facets.clear();
60        horizon_loop_ids.clear();
61
62        if !triangles[i].valid {
63            i = i + 1;
64            continue;
65        }
66
67        // FIXME: use triangles[i].furthest_point instead.
68        let pt_id = indexed_support_point_id(
69            &triangles[i].normal,
70            &points[..],
71            &triangles[i].visible_points[..],
72        );
73
74        if let Some(point) = pt_id {
75            removed_facets.clear();
76
77            triangles[i].valid = false;
78            removed_facets.push(i);
79
80            for j in 0usize..3 {
81                compute_silhouette(
82                    triangles[i].adj[j],
83                    triangles[i].indirect_adj_id[j],
84                    point,
85                    &mut horizon_loop_facets,
86                    &mut horizon_loop_ids,
87                    &points[..],
88                    &mut removed_facets,
89                    &mut triangles[..],
90                );
91            }
92
93            if horizon_loop_facets.is_empty() {
94                // Due to inaccuracies, the silhouette could not be computed
95                // (the point seems to be visible from… every triangle).
96                let mut any_valid = false;
97                for j in i + 1..triangles.len() {
98                    if triangles[j].valid {
99                        any_valid = true;
100                    }
101                }
102
103                if any_valid {
104                    //                        println!("Warning: exitting an unfinished work.");
105                }
106
107                // FIXME: this is verry harsh.
108                triangles[i].valid = true;
109                break;
110            }
111
112            attach_and_push_facets3(
113                &horizon_loop_facets[..],
114                &horizon_loop_ids[..],
115                point,
116                &points[..],
117                &mut triangles,
118                &removed_facets[..],
119                &mut undecidable_points,
120            );
121        }
122
123        i = i + 1;
124    }
125
126    let mut idx = Vec::new();
127
128    for facet in triangles.iter() {
129        if facet.valid {
130            idx.push(Point3::new(
131                facet.pts[0] as u32,
132                facet.pts[1] as u32,
133                facet.pts[2] as u32,
134            ));
135        }
136    }
137
138    utils::remove_unused_points(&mut points, &mut idx[..]);
139
140    assert!(points.len() != 0, "Internal error: empty output mesh.");
141
142    for point in points.iter_mut() {
143        *point = denormalizer * *point;
144    }
145
146    denormalize(&mut points[..], &norm_center, norm_diag);
147
148    TriMesh::new(points, None, None, Some(IndexBuffer::Unified(idx)))
149}
150
151enum InitialMesh<N: RealField + Copy> {
152    Facets(Vec<TriangleFacet<N>>, Matrix3<N>),
153    ResultMesh(TriMesh<N>),
154}
155
156fn build_degenerate_mesh_point<N: RealField + Copy>(point: Point3<N>) -> TriMesh<N> {
157    let ta = Point3::new(0u32, 0, 0);
158    let tb = Point3::new(0u32, 0, 0);
159
160    TriMesh::new(
161        vec![point],
162        None,
163        None,
164        Some(IndexBuffer::Unified(vec![ta, tb])),
165    )
166}
167
168fn build_degenerate_mesh_segment<N: RealField + Copy>(
169    dir: &Vector3<N>,
170    points: &[Point3<N>],
171) -> TriMesh<N> {
172    let a = utils::point_cloud_support_point(dir, points);
173    let b = utils::point_cloud_support_point(&-*dir, points);
174
175    let ta = Point3::new(0u32, 1, 0);
176    let tb = Point3::new(1u32, 0, 0);
177
178    TriMesh::new(
179        vec![a, b],
180        None,
181        None,
182        Some(IndexBuffer::Unified(vec![ta, tb])),
183    )
184}
185
186fn get_initial_mesh<N: RealField + Copy>(
187    points: &mut [Point3<N>],
188    undecidable: &mut Vec<usize>,
189) -> InitialMesh<N> {
190    /*
191     * Compute the eigenvectors to see if the input datas live on a subspace.
192     */
193    let cov_mat;
194    let eigvec;
195    let eigval;
196
197    #[cfg(not(feature = "improved_fixed_point_support"))]
198    {
199        cov_mat = cov(points);
200        let eig = cov_mat.symmetric_eigen();
201        eigvec = eig.eigenvectors;
202        eigval = eig.eigenvalues;
203    }
204
205    #[cfg(feature = "improved_fixed_point_support")]
206    {
207        cov_mat = Matrix3::identity();
208        eigvec = Matrix3::identity();
209        eigval = Vector3::repeat(N::one());
210    }
211
212    let mut eigpairs = [
213        (eigvec.column(0).into_owned(), eigval[0]),
214        (eigvec.column(1).into_owned(), eigval[1]),
215        (eigvec.column(2).into_owned(), eigval[2]),
216    ];
217
218    /*
219     * Sort in deacreasing order wrt. eigenvalues.
220     */
221    eigpairs.sort_by(|a, b| {
222        if a.1 > b.1 {
223            Ordering::Less // `Less` and `Greater` are reversed.
224        } else if a.1 < b.1 {
225            Ordering::Greater
226        } else {
227            Ordering::Equal
228        }
229    });
230
231    /*
232     * Count the dimension the data lives in.
233     */
234    let mut dimension = 0;
235    while dimension < 3 {
236        if relative_eq!(
237            eigpairs[dimension].1,
238            na::zero(),
239            epsilon = na::convert(1.0e-7f64)
240        ) {
241            break;
242        }
243
244        dimension = dimension + 1;
245    }
246
247    match dimension {
248        0 => {
249            // The hull is a point.
250            InitialMesh::ResultMesh(build_degenerate_mesh_point(points[0].clone()))
251        }
252        1 => {
253            // The hull is a segment.
254            InitialMesh::ResultMesh(build_degenerate_mesh_segment(&eigpairs[0].0, points))
255        }
256        2 => {
257            // The hull is a triangle.
258            // Project into the principal plane…
259            let axis1 = &eigpairs[0].0;
260            let axis2 = &eigpairs[1].0;
261
262            let mut subspace_points = Vec::with_capacity(points.len());
263
264            for point in points.iter() {
265                subspace_points.push(Point2::new(
266                    point.coords.dot(axis1),
267                    point.coords.dot(axis2),
268                ))
269            }
270
271            // … and compute the 2d convex hull.
272            let idx = transformation::convex_hull2_idx(&subspace_points[..]);
273
274            // Finalize the result, triangulating the polyline.
275            let npoints = idx.len();
276            let coords = idx.into_iter().map(|i| points[i].clone()).collect();
277            let mut triangles = Vec::with_capacity(npoints + npoints - 4);
278
279            let a = 0u32;
280
281            for id in 1u32..npoints as u32 - 1 {
282                triangles.push(Point3::new(a, id, id + 1));
283                triangles.push(Point3::new(id, a, id + 1));
284            }
285
286            InitialMesh::ResultMesh(TriMesh::new(
287                coords,
288                None,
289                None,
290                Some(IndexBuffer::Unified(triangles)),
291            ))
292        }
293        3 => {
294            // The hull is a polyedra.
295            // Find a initial triangle lying on the principal plane…
296            let _1: N = na::one();
297            let diag = Vector3::new(_1 / eigval[0], _1 / eigval[1], _1 / eigval[2]);
298            let diag = Matrix3::from_diagonal(&diag);
299            let icov = eigvec * diag * eigvec.transpose();
300
301            for point in points.iter_mut() {
302                *point = Point3::origin() + icov * point.coords;
303            }
304
305            let p1 = support_point_id(&eigpairs[0].0, points).unwrap();
306            let p2 = support_point_id(&-eigpairs[0].0, points).unwrap();
307
308            let mut max_area = na::zero();
309            let mut p3 = usize::max_value();
310
311            for (i, point) in points.iter().enumerate() {
312                let area = utils::triangle_area(&points[p1], &points[p2], point);
313
314                if area > max_area {
315                    max_area = area;
316                    p3 = i;
317                }
318            }
319
320            assert!(
321                p3 != usize::max_value(),
322                "Internal convex hull error: no triangle found."
323            );
324
325            // Build two facets with opposite normals
326            let mut f1 = TriangleFacet::new(p1, p2, p3, points);
327            let mut f2 = TriangleFacet::new(p2, p1, p3, points);
328
329            // Link the facets together
330            f1.set_facets_adjascency(1, 1, 1, 0, 2, 1);
331            f2.set_facets_adjascency(0, 0, 0, 0, 2, 1);
332
333            let mut facets = vec![f1, f2];
334
335            // … and attribute visible points to each one of them.
336            // FIXME: refactor this with the two others.
337            let mut ignored = 0usize;
338            for point in 0..points.len() {
339                if point == p1 || point == p2 || point == p3 {
340                    continue;
341                }
342
343                let mut furthest = usize::max_value();
344                let mut furthest_dist = na::zero();
345
346                for (i, curr_facet) in facets.iter().enumerate() {
347                    if curr_facet.can_be_seen_by(point, points) {
348                        let distance = curr_facet.distance_to_point(point, points);
349
350                        if distance > furthest_dist {
351                            furthest = i;
352                            furthest_dist = distance;
353                        }
354                    }
355                }
356
357                if furthest != usize::max_value() {
358                    facets[furthest].add_visible_point(point, points);
359                } else {
360                    undecidable.push(point);
361                    ignored = ignored + 1;
362                }
363
364                // If none of the facet can be seen from the point, it is naturally deleted.
365            }
366
367            verify_facet_links(0, &facets[..]);
368            verify_facet_links(1, &facets[..]);
369
370            InitialMesh::Facets(facets, cov_mat)
371        }
372        _ => unreachable!(),
373    }
374}
375
376fn compute_silhouette<N: RealField + Copy>(
377    facet: usize,
378    indirect_id: usize,
379    point: usize,
380    out_facets: &mut Vec<usize>,
381    out_adj_idx: &mut Vec<usize>,
382    points: &[Point3<N>],
383    removed_facets: &mut Vec<usize>,
384    triangles: &mut [TriangleFacet<N>],
385) {
386    if triangles[facet].valid {
387        if !triangles[facet].can_be_seen_by_or_is_affinely_dependent_with_contour(
388            point,
389            points,
390            indirect_id,
391        ) {
392            out_facets.push(facet);
393            out_adj_idx.push(indirect_id);
394        } else {
395            triangles[facet].valid = false; // The facet must be removed from the convex hull.
396            removed_facets.push(facet);
397
398            compute_silhouette(
399                triangles[facet].adj[(indirect_id + 1) % 3],
400                triangles[facet].indirect_adj_id[(indirect_id + 1) % 3],
401                point,
402                out_facets,
403                out_adj_idx,
404                points,
405                removed_facets,
406                triangles,
407            );
408            compute_silhouette(
409                triangles[facet].adj[(indirect_id + 2) % 3],
410                triangles[facet].indirect_adj_id[(indirect_id + 2) % 3],
411                point,
412                out_facets,
413                out_adj_idx,
414                points,
415                removed_facets,
416                triangles,
417            );
418        }
419    }
420}
421
422fn verify_facet_links<N: RealField + Copy>(ifacet: usize, facets: &[TriangleFacet<N>]) {
423    let facet = &facets[ifacet];
424
425    for i in 0usize..3 {
426        let adji = &facets[facet.adj[i]];
427
428        assert!(
429            adji.adj[facet.indirect_adj_id[i]] == ifacet
430                && adji.first_point_from_edge(facet.indirect_adj_id[i])
431                    == facet.second_point_from_edge(adji.indirect_adj_id[facet.indirect_adj_id[i]])
432                && adji.second_point_from_edge(facet.indirect_adj_id[i])
433                    == facet.first_point_from_edge(adji.indirect_adj_id[facet.indirect_adj_id[i]])
434        )
435    }
436}
437
438fn attach_and_push_facets3<N: RealField + Copy>(
439    horizon_loop_facets: &[usize],
440    horizon_loop_ids: &[usize],
441    point: usize,
442    points: &[Point3<N>],
443    triangles: &mut Vec<TriangleFacet<N>>,
444    removed_facets: &[usize],
445    undecidable: &mut Vec<usize>,
446) {
447    // The horizon is built to be in CCW order.
448    let mut new_facets = Vec::with_capacity(horizon_loop_facets.len());
449
450    // Create new facets.
451    let mut adj_facet: usize;
452    let mut indirect_id: usize;
453
454    for i in 0..horizon_loop_facets.len() {
455        adj_facet = horizon_loop_facets[i];
456        indirect_id = horizon_loop_ids[i];
457
458        let facet = TriangleFacet::new(
459            point,
460            triangles[adj_facet].second_point_from_edge(indirect_id),
461            triangles[adj_facet].first_point_from_edge(indirect_id),
462            points,
463        );
464        new_facets.push(facet);
465    }
466
467    // Link the facets together.
468    for i in 0..horizon_loop_facets.len() {
469        let prev_facet;
470
471        if i == 0 {
472            prev_facet = triangles.len() + horizon_loop_facets.len() - 1;
473        } else {
474            prev_facet = triangles.len() + i - 1;
475        }
476
477        let middle_facet = horizon_loop_facets[i];
478        let next_facet = triangles.len() + (i + 1) % horizon_loop_facets.len();
479        let middle_id = horizon_loop_ids[i];
480
481        new_facets[i].set_facets_adjascency(prev_facet, middle_facet, next_facet, 2, middle_id, 0);
482        triangles[middle_facet].adj[middle_id] = triangles.len() + i; // The future id of curr_facet.
483        triangles[middle_facet].indirect_adj_id[middle_id] = 1;
484    }
485
486    // Assign to each facets some of the points which can see it.
487    // FIXME: refactor this with the others.
488    for curr_facet in removed_facets.iter() {
489        for visible_point in triangles[*curr_facet].visible_points.iter() {
490            if *visible_point == point {
491                continue;
492            }
493
494            let mut furthest = usize::max_value();
495            let mut furthest_dist = na::zero();
496
497            for (i, curr_facet) in new_facets.iter_mut().enumerate() {
498                if curr_facet.can_be_seen_by(*visible_point, points) {
499                    let distance = curr_facet.distance_to_point(*visible_point, points);
500
501                    if distance > furthest_dist {
502                        furthest = i;
503                        furthest_dist = distance;
504                    }
505                }
506            }
507
508            if furthest != usize::max_value() {
509                new_facets[furthest].add_visible_point(*visible_point, points);
510            }
511
512            // If none of the facet can be seen from the point, it is naturally deleted.
513        }
514    }
515
516    // Try to assign collinear points to one of the new facets.
517    let mut i = 0;
518
519    while i != undecidable.len() {
520        let mut furthest = usize::max_value();
521        let mut furthest_dist = na::zero();
522        let undecidable_point = undecidable[i];
523
524        for (j, curr_facet) in new_facets.iter_mut().enumerate() {
525            if curr_facet.can_be_seen_by(undecidable_point, points) {
526                let distance = curr_facet.distance_to_point(undecidable_point, points);
527
528                if distance > furthest_dist {
529                    furthest = j;
530                    furthest_dist = distance;
531                }
532            }
533        }
534
535        if furthest != usize::max_value() {
536            new_facets[furthest].add_visible_point(undecidable_point, points);
537            let _ = undecidable.swap_remove(i);
538        } else {
539            i = i + 1;
540        }
541    }
542
543    // Push facets.
544    // FIXME: can we avoid the tmp vector `new_facets` ?
545    for curr_facet in new_facets.into_iter() {
546        triangles.push(curr_facet);
547    }
548}
549
550struct TriangleFacet<N: RealField + Copy> {
551    valid: bool,
552    normal: Vector3<N>,
553    adj: [usize; 3],
554    indirect_adj_id: [usize; 3],
555    pts: [usize; 3],
556    visible_points: Vec<usize>,
557    furthest_point: usize,
558    furthest_distance: N,
559}
560
561impl<N: RealField + Copy> TriangleFacet<N> {
562    pub fn new(p1: usize, p2: usize, p3: usize, points: &[Point3<N>]) -> TriangleFacet<N> {
563        let p1p2 = points[p2] - points[p1];
564        let p1p3 = points[p3] - points[p1];
565
566        let mut normal = p1p2.cross(&p1p3);
567        if normal.normalize_mut().is_zero() {
568            panic!("ConvexHull hull failure: a facet must not be affinely dependent.");
569        }
570
571        TriangleFacet {
572            valid: true,
573            normal: normal,
574            adj: [0, 0, 0],
575            indirect_adj_id: [0, 0, 0],
576            pts: [p1, p2, p3],
577            visible_points: Vec::new(),
578            furthest_point: Bounded::max_value(),
579            furthest_distance: na::zero(),
580        }
581    }
582
583    pub fn add_visible_point(&mut self, pid: usize, points: &[Point3<N>]) {
584        let distance = self.distance_to_point(pid, points);
585
586        if distance > self.furthest_distance {
587            self.furthest_distance = distance;
588            self.furthest_point = pid;
589        }
590
591        self.visible_points.push(pid);
592    }
593
594    pub fn distance_to_point(&self, point: usize, points: &[Point3<N>]) -> N {
595        self.normal.dot(&(points[point] - points[self.pts[0]]))
596    }
597
598    pub fn set_facets_adjascency(
599        &mut self,
600        adj1: usize,
601        adj2: usize,
602        adj3: usize,
603        id_adj1: usize,
604        id_adj2: usize,
605        id_adj3: usize,
606    ) {
607        self.indirect_adj_id[0] = id_adj1;
608        self.indirect_adj_id[1] = id_adj2;
609        self.indirect_adj_id[2] = id_adj3;
610
611        self.adj[0] = adj1;
612        self.adj[1] = adj2;
613        self.adj[2] = adj3;
614    }
615
616    pub fn first_point_from_edge(&self, id: usize) -> usize {
617        self.pts[id]
618    }
619
620    pub fn second_point_from_edge(&self, id: usize) -> usize {
621        self.pts[(id + 1) % 3]
622    }
623
624    pub fn can_be_seen_by(&self, point: usize, points: &[Point3<N>]) -> bool {
625        let p0 = &points[self.pts[0]];
626        let p1 = &points[self.pts[1]];
627        let p2 = &points[self.pts[2]];
628        let pt = &points[point];
629
630        let _eps = N::default_epsilon();
631
632        (*pt - *p0).dot(&self.normal) > _eps * na::convert(100.0f64)
633            && !utils::is_affinely_dependent_triangle(p0, p1, pt)
634            && !utils::is_affinely_dependent_triangle(p0, p2, pt)
635            && !utils::is_affinely_dependent_triangle(p1, p2, pt)
636    }
637
638    pub fn can_be_seen_by_or_is_affinely_dependent_with_contour(
639        &self,
640        point: usize,
641        points: &[Point3<N>],
642        edge: usize,
643    ) -> bool {
644        let p0 = &points[self.first_point_from_edge(edge)];
645        let p1 = &points[self.second_point_from_edge(edge)];
646        let pt = &points[point];
647
648        let aff_dep = utils::is_affinely_dependent_triangle(p0, p1, pt)
649            || utils::is_affinely_dependent_triangle(p0, pt, p1)
650            || utils::is_affinely_dependent_triangle(p1, p0, pt)
651            || utils::is_affinely_dependent_triangle(p1, pt, p0)
652            || utils::is_affinely_dependent_triangle(pt, p0, p1)
653            || utils::is_affinely_dependent_triangle(pt, p1, p0);
654
655        (*pt - *p0).dot(&self.normal) >= na::zero() || aff_dep
656    }
657}
658
659#[cfg(test)]
660mod test {
661    #[cfg(feature = "dim3")]
662    use crate::procedural;
663    use crate::transformation;
664    #[cfg(feature = "dim2")]
665    use na::Point2;
666
667    #[cfg(feature = "dim2")]
668    #[test]
669    fn test_simple_convex_hull() {
670        let points = [
671            Point2::new(4.723881f32, 3.597233),
672            Point2::new(3.333363, 3.429991),
673            Point2::new(3.137215, 2.812263),
674        ];
675
676        let chull = transformation::convex_hull(points.as_slice());
677
678        assert!(chull.coords.len() == 3);
679    }
680
681    #[cfg(feature = "dim3")]
682    #[test]
683    fn test_ball_convex_hull() {
684        // This trigerred a failure to an affinely dependent facet.
685        let sphere = procedural::sphere(0.4f32, 20, 20, true);
686        let points = sphere.coords;
687        let chull = transformation::convex_hull(points.as_slice());
688
689        // dummy test, we are just checking that the construction did not fail.
690        assert!(chull.coords.len() == chull.coords.len());
691    }
692}