vqm 0.1.9

A vector, quaternion, and matrix library targeted at embedded systems and robotics.
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164

use core::ops::{Mul, Neg};
use num_traits::float::FloatCore;

use crate::{MathConstants, Quaternion, TrigonometricMethods, Vector2d, Vector3d};

/// `RollPitchYaw` `struct { roll: f32, pitch: f32, yaw: f32 }`<br>
pub type RollPitchYawf32 = RollPitchYaw<f32>;
/// `RollPitchYaw` `struct { roll: f64, pitch: f64, yaw: f64 }`<br>
pub type RollPitchYawf64 = RollPitchYaw<f64>;

/// `RollPitch` `struct { roll: f32, pitch: f32 }`<br>
pub type RollPitchf32 = RollPitch<f32>;
/// `RollPitch` `struct { roll: f64, pitch: f64 }`<br><br>
pub type RollPitchf64 = RollPitch<f64>;

/// Roll and Pitch bundled for convenience.
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct RollPitch<T> {
    pub roll: T,
    pub pitch: T,
}

impl<T> RollPitch<T>
where
    T: Copy,
{
    /// Create a `RollPitch`.
    #[inline]
    pub const fn new(roll: T, pitch: T) -> Self {
        Self { roll, pitch }
    }

    /// Create a `RollPitch` from a `Vector2d` assuming the North East Down (NED) convention.
    #[inline]
    pub fn from_vector_ned(v: Vector2d<T>) -> Self {
        Self { roll: v.y, pitch: v.x }
    }
}

impl<T> RollPitch<T>
where
    T: Copy + Mul<Output = T> + MathConstants,
{
    #[inline]
    pub fn to_degrees(self) -> Self {
        Self { roll: self.roll * T::RADIANS_TO_DEGREES, pitch: self.pitch * T::RADIANS_TO_DEGREES }
    }
    #[inline]
    pub fn to_radians(self) -> Self {
        Self { roll: self.roll * T::DEGREES_TO_RADIANS, pitch: self.pitch * T::DEGREES_TO_RADIANS }
    }
}

impl<T> RollPitch<T>
where
    T: Copy + FloatCore,
{
    #[inline]
    pub fn abs(self) -> Self {
        Self { roll: self.roll.abs(), pitch: self.pitch.abs() }
    }
    #[inline]
    pub fn clamp(self, min: T, max: T) -> Self {
        Self { roll: self.roll.clamp(min, max), pitch: self.pitch.clamp(min, max) }
    }
}

impl<T> From<RollPitch<T>> for Quaternion<T>
where
    T: Copy + TrigonometricMethods + FloatCore,
{
    #[inline]
    fn from(angles: RollPitch<T>) -> Self {
        Quaternion::from_roll_pitch_angles_radians(angles.roll, angles.pitch)
    }
}

/// Roll, Pitch, and Yaw bundled for convenience.<br><br>
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct RollPitchYaw<T> {
    pub roll: T,
    pub pitch: T,
    pub yaw: T,
}

impl<T> RollPitchYaw<T>
where
    T: Copy + Neg<Output = T>,
{
    /// Create a `RollPitchYaw`.
    #[inline]
    pub const fn new(roll: T, pitch: T, yaw: T) -> Self {
        Self { roll, pitch, yaw }
    }

    /// Create a `RollPitchYaw` from a `Vector3d` assuming the North East Down (NED) convention.
    #[inline]
    pub fn from_vector_ned(v: Vector3d<T>) -> Self {
        Self { roll: v.y, pitch: v.x, yaw: -v.z }
    }
}

impl<T> RollPitchYaw<T>
where
    T: Copy + Mul<Output = T> + MathConstants,
{
    #[inline]
    pub fn to_degrees(self) -> Self {
        Self {
            roll: self.roll * T::RADIANS_TO_DEGREES,
            pitch: self.pitch * T::RADIANS_TO_DEGREES,
            yaw: self.yaw * T::RADIANS_TO_DEGREES,
        }
    }
    #[inline]
    pub fn to_radians(self) -> Self {
        Self {
            roll: self.roll * T::DEGREES_TO_RADIANS,
            pitch: self.pitch * T::DEGREES_TO_RADIANS,
            yaw: self.yaw * T::RADIANS_TO_DEGREES,
        }
    }
}

impl<T> RollPitchYaw<T>
where
    T: Copy + FloatCore,
{
    #[inline]
    pub fn abs(self) -> Self {
        Self { roll: self.roll.abs(), pitch: self.pitch.abs(), yaw: self.yaw.abs() }
    }
    #[inline]
    pub fn clamp(self, min: T, max: T) -> Self {
        Self { roll: self.roll.clamp(min, max), pitch: self.pitch.clamp(min, max), yaw: self.yaw.clamp(min, max) }
    }
}

impl<T> From<RollPitchYaw<T>> for Quaternion<T>
where
    T: Copy + TrigonometricMethods + FloatCore,
{
    #[inline]
    fn from(angles: RollPitchYaw<T>) -> Self {
        Quaternion::from_roll_pitch_yaw_angles_radians(angles.roll, angles.pitch, angles.yaw)
    }
}

#[cfg(test)]
mod tests {
    #[allow(unused)]
    use super::*;
    #[cfg(feature = "serde")]
    use serde::{Deserialize, Serialize};

    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}

    #[test]
    fn normal_types() {
        is_full::<RollPitchf32>();
        is_full::<RollPitchYawf32>();
    }
}