com_croftsoft_core/math/quat/
mod.rs

1// =============================================================================
2//! - Quaternion
3//!
4//! # Metadata
5//! - Copyright: &copy; 2008 - 2022 [`CroftSoft Inc`]
6//! - Author: [`David Wallace Croft`]
7//! - Rust version: 2022-10-20
8//! - Rust since: 2022-10-10
9//! - Java version: 2008-05-09
10//! - Java since: 2008-05-02
11//!
12//! # History
13//! - Adapted from the Java package com.croftsoft.core.math.quat
14//!   - In the Java-based [`CroftSoft Core Library`]
15//!
16//! [`CroftSoft Core Library`]: https://www.croftsoft.com/library/code/
17//! [`CroftSoft Inc`]: https://www.croftsoft.com/
18//! [`David Wallace Croft`]: https://www.croftsoft.com/people/david/
19// =============================================================================
20
21use super::{
22  axis::AxisAngle,
23  matrix::structures::{RotationDegrees, RotationRadians},
24};
25use std::ops::{Mul, MulAssign};
26
27#[cfg(test)]
28mod test;
29
30// Structures ------------------------------------------------------------------
31
32#[derive(Clone, Copy, Debug, PartialEq)]
33pub struct Quat {
34  pub w: f64,
35  pub x: f64,
36  pub y: f64,
37  pub z: f64,
38}
39
40// Associated Functions --------------------------------------------------------
41
42impl Quat {
43  pub fn dot_product(
44    quat0: &Self,
45    quat1: &Self,
46  ) -> Self {
47    Quat {
48      w: quat0.w * quat1.w,
49      x: quat0.x * quat1.x,
50      y: quat0.y * quat1.y,
51      z: quat0.z * quat1.z,
52    }
53  }
54
55  pub fn multiply_quat_with_quat(
56    quat0: &Self,
57    quat1: &Self,
58  ) -> Self {
59    let Quat {
60      w: w0,
61      x: x0,
62      y: y0,
63      z: z0,
64    } = quat0;
65    let Quat {
66      w: w1,
67      x: x1,
68      y: y1,
69      z: z1,
70    } = quat1;
71    Quat {
72      w: w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1,
73      x: y0 * z1 - z0 * y1 + w0 * x1 + x0 * w1,
74      y: z0 * x1 - x0 * z1 + w0 * y1 + y0 * w1,
75      z: x0 * y1 - y0 * x1 + w0 * z1 + z0 * w1,
76    }
77  }
78}
79
80// Methods ---------------------------------------------------------------------
81
82impl Quat {
83  pub fn matches_closely(
84    &self,
85    other: &Self,
86    tolerance: f64,
87  ) -> bool {
88    (self.w - other.w).abs() <= tolerance
89      && (self.x - other.x).abs() <= tolerance
90      && (self.y - other.y).abs() <= tolerance
91      && (self.z - other.z).abs() <= tolerance
92  }
93
94  pub fn matches_exactly(
95    &self,
96    other: &Self,
97  ) -> bool {
98    self.w == other.w
99      && self.x == other.x
100      && self.y == other.y
101      && self.z == other.z
102  }
103
104  pub fn multiply_with_quat(
105    &mut self,
106    multiplier: &Quat,
107  ) -> &mut Self {
108    let product = Quat::multiply_quat_with_quat(self, multiplier);
109    self.w = product.w;
110    self.x = product.x;
111    self.y = product.y;
112    self.z = product.z;
113    self
114  }
115}
116
117// Operator Mul ----------------------------------------------------------------
118
119impl Mul<Quat> for Quat {
120  type Output = Quat;
121
122  fn mul(
123    self,
124    rhs: Quat,
125  ) -> Self::Output {
126    Quat::multiply_quat_with_quat(&self, &rhs)
127  }
128}
129
130impl Mul<Quat> for &Quat {
131  type Output = Quat;
132
133  fn mul(
134    self,
135    rhs: Quat,
136  ) -> Self::Output {
137    Quat::multiply_quat_with_quat(self, &rhs)
138  }
139}
140
141impl Mul<&Quat> for Quat {
142  type Output = Quat;
143
144  fn mul(
145    self,
146    rhs: &Quat,
147  ) -> Self::Output {
148    Quat::multiply_quat_with_quat(&self, rhs)
149  }
150}
151
152impl Mul<&Quat> for &Quat {
153  type Output = Quat;
154
155  fn mul(
156    self,
157    rhs: &Quat,
158  ) -> Self::Output {
159    Quat::multiply_quat_with_quat(self, rhs)
160  }
161}
162
163// Operator MulAssign ----------------------------------------------------------
164
165impl MulAssign<Quat> for Quat {
166  fn mul_assign(
167    &mut self,
168    rhs: Quat,
169  ) {
170    self.multiply_with_quat(&rhs);
171  }
172}
173
174impl MulAssign<Quat> for &mut Quat {
175  fn mul_assign(
176    &mut self,
177    rhs: Quat,
178  ) {
179    self.multiply_with_quat(&rhs);
180  }
181}
182
183impl MulAssign<&Quat> for Quat {
184  fn mul_assign(
185    &mut self,
186    rhs: &Quat,
187  ) {
188    self.multiply_with_quat(rhs);
189  }
190}
191
192impl MulAssign<&Quat> for &mut Quat {
193  fn mul_assign(
194    &mut self,
195    rhs: &Quat,
196  ) {
197    self.multiply_with_quat(rhs);
198  }
199}
200
201// Trait Default ---------------------------------------------------------------
202
203impl Default for Quat {
204  fn default() -> Self {
205    Self {
206      w: 1.0,
207      x: 0.0,
208      y: 0.0,
209      z: 0.0,
210    }
211  }
212}
213
214// Trait From ------------------------------------------------------------------
215
216impl From<Quat> for AxisAngle {
217  fn from(quat: Quat) -> Self {
218    let Quat {
219      w,
220      x,
221      y,
222      z,
223    } = quat;
224    let sin_theta_over_2_sq = 1.0 - w * w;
225    if sin_theta_over_2_sq <= 0.0 {
226      return AxisAngle::default();
227    }
228    let one_over_sin_theta_over_2 = 1.0 / sin_theta_over_2_sq.sqrt();
229    AxisAngle {
230      radians: 2.0 * w.acos(),
231      x: x * one_over_sin_theta_over_2,
232      y: y * one_over_sin_theta_over_2,
233      z: z * one_over_sin_theta_over_2,
234    }
235  }
236}
237
238impl From<RotationDegrees> for Quat {
239  fn from(rotation_degrees: RotationDegrees) -> Self {
240    Quat::from(RotationRadians::from(rotation_degrees))
241  }
242}
243
244impl From<RotationRadians> for Quat {
245  fn from(rotation_radians: RotationRadians) -> Self {
246    let axis_angle_x = AxisAngle {
247      radians: rotation_radians.x,
248      x: 0.0,
249      y: 0.0,
250      z: 1.0,
251    };
252    let axis_angle_y = AxisAngle {
253      radians: rotation_radians.y,
254      x: 0.0,
255      y: 1.0,
256      z: 0.0,
257    };
258    let axis_angle_z = AxisAngle {
259      radians: rotation_radians.z,
260      x: 0.0,
261      y: 0.0,
262      z: 1.0, // TODO: Is this right?
263    };
264    let quat_x = Quat::from(axis_angle_x);
265    let quat_y = Quat::from(axis_angle_y);
266    let quat_z = Quat::from(axis_angle_z);
267    quat_z * quat_y * quat_x
268  }
269}
com_croftsoft_core/math/quat/mod.rs

com_croftsoft_core/math/quat/
mod.rs
