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

use crate::math::Real;
use crate::num::Bounded;
use na;
#[cfg(feature = "dim3")]
use {crate::bounding_volume, crate::math::Point};

/// Returns the index of the support point of a list of points.
pub fn support_point_id<const D: usize>(
    direction: &na::SVector<Real, D>,
    points: &[na::Point<Real, D>],
) -> Option<usize> {
    let mut argmax = None;
    let _max: Real = Bounded::max_value();
    let mut max = -_max;

    for (id, pt) in points.iter().enumerate() {
        let dot = direction.dot(&pt.coords);

        if dot > max {
            argmax = Some(id);
            max = dot;
        }
    }

    argmax
}

/// Returns the index of the support point of an indexed list of points.
pub fn indexed_support_point_id<I, const D: usize>(
    direction: &na::SVector<Real, D>,
    points: &[na::Point<Real, D>],
    idx: I,
) -> Option<usize>
where
    I: Iterator<Item = usize>,
{
    let mut argmax = None;
    let mut max = -Real::MAX;

    for i in idx.into_iter() {
        let dot = direction.dot(&points[i].coords);

        if dot > max {
            argmax = Some(i);
            max = dot;
        }
    }

    argmax
}

/// Returns the number `n` such that `points[idx.nth(n)]` is the support point.
#[cfg(feature = "dim3")] // We only use this in 3D right now.
pub fn indexed_support_point_nth<I, const D: usize>(
    direction: &na::SVector<Real, D>,
    points: &[na::Point<Real, D>],
    idx: I,
) -> Option<usize>
where
    I: Iterator<Item = usize>,
{
    let mut argmax = None;
    let mut max = -Real::MAX;

    for (k, i) in idx.into_iter().enumerate() {
        let dot = direction.dot(&points[i].coords);

        if dot > max {
            argmax = Some(k);
            max = dot;
        }
    }

    argmax
}

/// Scale and center the given set of point depending on their Aabb.
#[cfg(feature = "dim3")]
pub fn normalize(coords: &mut [Point<Real>]) -> (Point<Real>, Real) {
    let aabb = bounding_volume::details::local_point_cloud_aabb(&*coords);
    let diag = na::distance(&aabb.mins, &aabb.maxs);
    let center = aabb.center();

    for c in coords.iter_mut() {
        *c = (*c + (-center.coords)) / diag;
    }

    (center, diag)
}