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

use crate::bounding_volume;
use crate::math::Point;
use crate::num::Bounded;
use na::allocator::Allocator;
use na::base::{DefaultAllocator, DimName};
use na::{self, RealField};

/// Returns the index of the support point of a list of points.
pub fn support_point_id<N: RealField, D: DimName>(
    direction: &na::VectorN<N, D>,
    points: &[na::Point<N, D>],
) -> Option<usize>
where
    DefaultAllocator: Allocator<N, D>,
{
    let mut argmax = None;
    let _max: N = 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<N: RealField, D: DimName>(
    direction: &na::VectorN<N, D>,
    points: &[na::Point<N, D>],
    idx: &[usize],
) -> Option<usize>
where
    DefaultAllocator: Allocator<N, D>,
{
    let mut argmax = None;
    let _max: N = Bounded::max_value();
    let mut max = -_max;

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

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

    argmax
}

/// Scale and center the given set of point depending on their AABB.
pub fn normalize<N: RealField>(coords: &mut [Point<N>]) -> (Point<N>, N) {
    let aabb = bounding_volume::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)
}

/// Scale and translates the given set of point.
pub fn denormalize<N: RealField>(coords: &mut [Point<N>], center: &Point<N>, diag: N) {
    for c in coords.iter_mut() {
        *c = *c * diag + center.coords;
    }
}