mecha10-core 0.6.5

Mecha10 core runtime and foundational types
Documentation 
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

//! Small `[f32; N]` vector/quaternion helpers shared by message types that carry a 3D
//! position or orientation (`Odometry`, `Pose`, `Imu`, `Twist`, ...). These types can't
//! share an impl block since they're distinct public structs, so the handful of genuinely
//! duplicated math (Euclidean distance, vector magnitude, quaternion normalization) lives
//! here instead of being copy-pasted per type.

/// Euclidean distance between two 3D points.
pub(crate) fn distance_3d(a: [f32; 3], b: [f32; 3]) -> f32 {
    let dx = a[0] - b[0];
    let dy = a[1] - b[1];
    let dz = a[2] - b[2];
    (dx * dx + dy * dy + dz * dz).sqrt()
}

/// Euclidean distance between two points, ignoring the z component.
pub(crate) fn distance_2d(a: [f32; 3], b: [f32; 3]) -> f32 {
    let dx = a[0] - b[0];
    let dy = a[1] - b[1];
    (dx * dx + dy * dy).sqrt()
}

/// Magnitude (L2 norm) of a 3D vector.
pub(crate) fn magnitude_3d(v: [f32; 3]) -> f32 {
    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
}

/// Normalizes a quaternion `[x, y, z, w]` in place. No-op if the quaternion is zero.
pub(crate) fn normalize_quaternion(q: &mut [f32; 4]) {
    let mag = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
    if mag > 0.0 {
        *q = [q[0] / mag, q[1] / mag, q[2] / mag, q[3] / mag];
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn distance_3d_measures_euclidean_distance_in_all_three_axes() {
        assert_eq!(distance_3d([0.0, 0.0, 0.0], [3.0, 4.0, 0.0]), 5.0);
        assert_eq!(distance_3d([1.0, 2.0, 3.0], [1.0, 2.0, 3.0]), 0.0);
        assert_eq!(distance_3d([0.0, 0.0, 0.0], [0.0, 0.0, 5.0]), 5.0);
    }

    #[test]
    fn distance_2d_ignores_the_z_component() {
        // Same x/y offset as the 3D case, but a large z difference that distance_2d must ignore.
        assert_eq!(distance_2d([0.0, 0.0, 0.0], [3.0, 4.0, 100.0]), 5.0);
    }

    #[test]
    fn magnitude_3d_computes_the_l2_norm() {
        assert_eq!(magnitude_3d([3.0, 4.0, 0.0]), 5.0);
        assert_eq!(magnitude_3d([0.0, 0.0, 0.0]), 0.0);
    }

    #[test]
    fn normalize_quaternion_scales_to_unit_length() {
        let mut q = [0.0, 0.0, 0.0, 2.0];
        normalize_quaternion(&mut q);
        assert_eq!(q, [0.0, 0.0, 0.0, 1.0]);
    }

    #[test]
    fn normalize_quaternion_is_a_no_op_for_a_zero_quaternion() {
        let mut q = [0.0, 0.0, 0.0, 0.0];
        normalize_quaternion(&mut q);
        assert_eq!(q, [0.0, 0.0, 0.0, 0.0]);
    }
}