sciforge_lib/maths/complex/
quaternion.rs1use std::fmt;
2use std::ops::{Add, Div, Mul, Neg, Sub};
3
4#[derive(Clone, Copy, Debug, PartialEq)]
5pub struct Quaternion {
6 pub w: f64,
7 pub x: f64,
8 pub y: f64,
9 pub z: f64,
10}
11
12impl Quaternion {
13 pub fn new(w: f64, x: f64, y: f64, z: f64) -> Self {
14 Self { w, x, y, z }
15 }
16 pub fn identity() -> Self {
17 Self {
18 w: 1.0,
19 x: 0.0,
20 y: 0.0,
21 z: 0.0,
22 }
23 }
24 pub fn zero() -> Self {
25 Self {
26 w: 0.0,
27 x: 0.0,
28 y: 0.0,
29 z: 0.0,
30 }
31 }
32 pub fn pure(x: f64, y: f64, z: f64) -> Self {
33 Self { w: 0.0, x, y, z }
34 }
35
36 pub fn from_axis_angle(axis: [f64; 3], angle: f64) -> Self {
37 let half = angle * 0.5;
38 let s = half.sin();
39 let norm = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt();
40 if norm < 1e-30 {
41 return Self::identity();
42 }
43 Self {
44 w: half.cos(),
45 x: axis[0] / norm * s,
46 y: axis[1] / norm * s,
47 z: axis[2] / norm * s,
48 }
49 }
50
51 pub fn from_euler(roll: f64, pitch: f64, yaw: f64) -> Self {
52 let (sr, cr) = (roll * 0.5).sin_cos();
53 let (sp, cp) = (pitch * 0.5).sin_cos();
54 let (sy, cy) = (yaw * 0.5).sin_cos();
55 Self {
56 w: cr * cp * cy + sr * sp * sy,
57 x: sr * cp * cy - cr * sp * sy,
58 y: cr * sp * cy + sr * cp * sy,
59 z: cr * cp * sy - sr * sp * cy,
60 }
61 }
62
63 pub fn to_axis_angle(&self) -> ([f64; 3], f64) {
64 let n = (self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
65 let angle = 2.0 * n.atan2(self.w);
66 if n < 1e-30 {
67 ([0.0, 0.0, 1.0], 0.0)
68 } else {
69 ([self.x / n, self.y / n, self.z / n], angle)
70 }
71 }
72
73 pub fn to_rotation_matrix(&self) -> [[f64; 3]; 3] {
74 let (w, x, y, z) = (self.w, self.x, self.y, self.z);
75 [
76 [
77 1.0 - 2.0 * (y * y + z * z),
78 2.0 * (x * y - w * z),
79 2.0 * (x * z + w * y),
80 ],
81 [
82 2.0 * (x * y + w * z),
83 1.0 - 2.0 * (x * x + z * z),
84 2.0 * (y * z - w * x),
85 ],
86 [
87 2.0 * (x * z - w * y),
88 2.0 * (y * z + w * x),
89 1.0 - 2.0 * (x * x + y * y),
90 ],
91 ]
92 }
93
94 pub fn conj(&self) -> Self {
95 Self {
96 w: self.w,
97 x: -self.x,
98 y: -self.y,
99 z: -self.z,
100 }
101 }
102 pub fn norm_sq(&self) -> f64 {
103 self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z
104 }
105 pub fn norm(&self) -> f64 {
106 self.norm_sq().sqrt()
107 }
108
109 pub fn normalize(&self) -> Self {
110 let n = self.norm();
111 if n < 1e-30 {
112 return Self::identity();
113 }
114 Self {
115 w: self.w / n,
116 x: self.x / n,
117 y: self.y / n,
118 z: self.z / n,
119 }
120 }
121
122 pub fn inv(&self) -> Self {
123 let n = self.norm_sq();
124 let c = self.conj();
125 Self {
126 w: c.w / n,
127 x: c.x / n,
128 y: c.y / n,
129 z: c.z / n,
130 }
131 }
132
133 pub fn rotate_vector(&self, v: [f64; 3]) -> [f64; 3] {
134 let p = Quaternion::pure(v[0], v[1], v[2]);
135 let rotated = *self * p * self.conj();
136 [rotated.x, rotated.y, rotated.z]
137 }
138
139 pub fn dot(&self, other: &Self) -> f64 {
140 self.w * other.w + self.x * other.x + self.y * other.y + self.z * other.z
141 }
142
143 pub fn slerp(&self, other: &Self, t: f64) -> Self {
144 let mut dot = self.dot(other);
145 let mut other = *other;
146 if dot < 0.0 {
147 other = -other;
148 dot = -dot;
149 }
150 if dot > 0.9995 {
151 let result = Self {
152 w: self.w + t * (other.w - self.w),
153 x: self.x + t * (other.x - self.x),
154 y: self.y + t * (other.y - self.y),
155 z: self.z + t * (other.z - self.z),
156 };
157 return result.normalize();
158 }
159 let theta = dot.acos();
160 let sin_theta = theta.sin();
161 let a = ((1.0 - t) * theta).sin() / sin_theta;
162 let b = (t * theta).sin() / sin_theta;
163 Self {
164 w: a * self.w + b * other.w,
165 x: a * self.x + b * other.x,
166 y: a * self.y + b * other.y,
167 z: a * self.z + b * other.z,
168 }
169 }
170
171 pub fn scale(&self, s: f64) -> Self {
172 Self {
173 w: self.w * s,
174 x: self.x * s,
175 y: self.y * s,
176 z: self.z * s,
177 }
178 }
179
180 pub fn exp(&self) -> Self {
181 let vec_norm = (self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
182 let ew = self.w.exp();
183 if vec_norm < 1e-30 {
184 return Self {
185 w: ew,
186 x: 0.0,
187 y: 0.0,
188 z: 0.0,
189 };
190 }
191 let s = ew * vec_norm.sin() / vec_norm;
192 Self {
193 w: ew * vec_norm.cos(),
194 x: self.x * s,
195 y: self.y * s,
196 z: self.z * s,
197 }
198 }
199
200 pub fn ln(&self) -> Self {
201 let n = self.norm();
202 let vec_norm = (self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
203 if vec_norm < 1e-30 {
204 return Self {
205 w: n.ln(),
206 x: 0.0,
207 y: 0.0,
208 z: 0.0,
209 };
210 }
211 let s = (self.w / n).acos() / vec_norm;
212 Self {
213 w: n.ln(),
214 x: self.x * s,
215 y: self.y * s,
216 z: self.z * s,
217 }
218 }
219}
220
221impl Add for Quaternion {
222 type Output = Self;
223 fn add(self, rhs: Self) -> Self {
224 Self {
225 w: self.w + rhs.w,
226 x: self.x + rhs.x,
227 y: self.y + rhs.y,
228 z: self.z + rhs.z,
229 }
230 }
231}
232
233impl Sub for Quaternion {
234 type Output = Self;
235 fn sub(self, rhs: Self) -> Self {
236 Self {
237 w: self.w - rhs.w,
238 x: self.x - rhs.x,
239 y: self.y - rhs.y,
240 z: self.z - rhs.z,
241 }
242 }
243}
244
245impl Mul for Quaternion {
246 type Output = Self;
247 fn mul(self, rhs: Self) -> Self {
248 Self {
249 w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
250 x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
251 y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
252 z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
253 }
254 }
255}
256
257impl Div for Quaternion {
258 type Output = Self;
259 fn div(self, rhs: Self) -> Self {
260 self.mul(rhs.inv())
261 }
262}
263
264impl Neg for Quaternion {
265 type Output = Self;
266 fn neg(self) -> Self {
267 Self {
268 w: -self.w,
269 x: -self.x,
270 y: -self.y,
271 z: -self.z,
272 }
273 }
274}
275
276impl fmt::Display for Quaternion {
277 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
278 write!(f, "{} + {}i + {}j + {}k", self.w, self.x, self.y, self.z)
279 }
280}