veloren_common/comp/ori.rs
1use crate::util::{Dir, Plane, Projection};
2use core::f32::consts::{FRAC_PI_2, PI, TAU};
3use serde::{Deserialize, Serialize};
4use specs::Component;
5use vek::{Quaternion, Vec2, Vec3};
6
7// Orientation
8#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
9#[serde(into = "SerdeOri")]
10#[serde(from = "SerdeOri")]
11pub struct Ori(Quaternion<f32>);
12
13impl Default for Ori {
14 /// Returns the default orientation (no rotation; default Dir)
15 fn default() -> Self { Self(Quaternion::identity()) }
16}
17
18impl Ori {
19 pub fn new(quat: Quaternion<f32>) -> Self {
20 #[cfg(debug_assertions)]
21 {
22 let v4 = quat.into_vec4();
23 debug_assert!(v4.map(f32::is_finite).reduce_and());
24 debug_assert!(v4.is_normalized());
25 }
26 Self(quat)
27 }
28
29 /// Tries to convert into a Dir and then the appropriate rotation
30 pub fn from_unnormalized_vec<T>(vec: T) -> Option<Self>
31 where
32 T: Into<Vec3<f32>>,
33 {
34 Dir::from_unnormalized(vec.into()).map(Self::from)
35 }
36
37 /// Look direction as a vector (no pedantic normalization performed)
38 pub fn look_vec(self) -> Vec3<f32> { self.to_quat() * *Dir::default() }
39
40 /// Get the internal quaternion representing the rotation from
41 /// `Dir::default()` to this orientation.
42 ///
43 /// The operation is a cheap copy.
44 pub fn to_quat(self) -> Quaternion<f32> {
45 debug_assert!(self.is_normalized());
46 self.0
47 }
48
49 /// Look direction (as a Dir it is pedantically normalized)
50 pub fn look_dir(&self) -> Dir { self.to_quat() * Dir::default() }
51
52 pub fn up(&self) -> Dir { self.pitched_up(PI / 2.0).look_dir() }
53
54 pub fn down(&self) -> Dir { self.pitched_down(PI / 2.0).look_dir() }
55
56 pub fn left(&self) -> Dir { self.yawed_left(PI / 2.0).look_dir() }
57
58 pub fn right(&self) -> Dir { self.yawed_right(PI / 2.0).look_dir() }
59
60 pub fn slerp(ori1: Self, ori2: Self, s: f32) -> Self {
61 Self(Quaternion::slerp(ori1.0, ori2.0, s).normalized())
62 }
63
64 #[must_use]
65 pub fn slerped_towards(self, ori: Ori, s: f32) -> Self { Self::slerp(self, ori, s) }
66
67 /// Multiply rotation quaternion by `q`
68 /// (the rotations are in local vector space).
69 ///
70 /// ```
71 /// use vek::{Quaternion, Vec3};
72 /// use veloren_common::{comp::Ori, util::Dir};
73 ///
74 /// let ang = 90_f32.to_radians();
75 /// let roll_right = Quaternion::rotation_y(ang);
76 /// let pitch_up = Quaternion::rotation_x(ang);
77 ///
78 /// let ori1 = Ori::from(Dir::new(Vec3::unit_x()));
79 /// let ori2 = Ori::default().rotated(roll_right).rotated(pitch_up);
80 ///
81 /// assert!((ori1.look_dir().dot(*ori2.look_dir()) - 1.0).abs() <= f32::EPSILON);
82 /// ```
83 #[must_use]
84 pub fn rotated(self, q: Quaternion<f32>) -> Self {
85 Self((self.to_quat() * q.normalized()).normalized())
86 }
87
88 /// Premultiply rotation quaternion by `q`
89 /// (the rotations are in global vector space).
90 ///
91 /// ```
92 /// use vek::{Quaternion, Vec3};
93 /// use veloren_common::{comp::Ori, util::Dir};
94 ///
95 /// let ang = 90_f32.to_radians();
96 /// let roll_right = Quaternion::rotation_y(ang);
97 /// let pitch_up = Quaternion::rotation_x(ang);
98 ///
99 /// let ori1 = Ori::from(Dir::up());
100 /// let ori2 = Ori::default().prerotated(roll_right).prerotated(pitch_up);
101 ///
102 /// assert!((ori1.look_dir().dot(*ori2.look_dir()) - 1.0).abs() <= f32::EPSILON);
103 /// ```
104 #[must_use]
105 pub fn prerotated(self, q: Quaternion<f32>) -> Self {
106 Self((q.normalized() * self.to_quat()).normalized())
107 }
108
109 /// Take `global` into this Ori's local vector space
110 ///
111 /// ```
112 /// use vek::Vec3;
113 /// use veloren_common::{comp::Ori, util::Dir};
114 ///
115 /// let ang = 90_f32.to_radians();
116 /// let (fw, left, up) = (Dir::default(), Dir::left(), Dir::up());
117 ///
118 /// let ori = Ori::default().rolled_left(ang).pitched_up(ang);
119 /// approx::assert_relative_eq!(ori.global_to_local(fw).dot(*-up), 1.0);
120 /// approx::assert_relative_eq!(ori.global_to_local(left).dot(*fw), 1.0);
121 /// let ori = Ori::default().rolled_right(ang).pitched_up(2.0 * ang);
122 /// approx::assert_relative_eq!(ori.global_to_local(up).dot(*left), 1.0);
123 /// ```
124 pub fn global_to_local<T>(&self, global: T) -> <Quaternion<f32> as std::ops::Mul<T>>::Output
125 where
126 Quaternion<f32>: std::ops::Mul<T>,
127 {
128 self.to_quat().inverse() * global
129 }
130
131 /// Take `local` into the global vector space
132 ///
133 /// ```
134 /// use vek::Vec3;
135 /// use veloren_common::{comp::Ori, util::Dir};
136 ///
137 /// let ang = 90_f32.to_radians();
138 /// let (fw, left, up) = (Dir::default(), Dir::left(), Dir::up());
139 ///
140 /// let ori = Ori::default().rolled_left(ang).pitched_up(ang);
141 /// approx::assert_relative_eq!(ori.local_to_global(fw).dot(*left), 1.0);
142 /// approx::assert_relative_eq!(ori.local_to_global(left).dot(*-up), 1.0);
143 /// let ori = Ori::default().rolled_right(ang).pitched_up(2.0 * ang);
144 /// approx::assert_relative_eq!(ori.local_to_global(up).dot(*left), 1.0);
145 /// ```
146 pub fn local_to_global<T>(&self, local: T) -> <Quaternion<f32> as std::ops::Mul<T>>::Output
147 where
148 Quaternion<f32>: std::ops::Mul<T>,
149 {
150 self.to_quat() * local
151 }
152
153 #[must_use]
154 pub fn to_horizontal(self) -> Self {
155 // We don't use Self::look_dir to avoid the extra normalization step within
156 // Dir's Quaternion Mul impl
157 let fw = self.to_quat() * Dir::default().to_vec();
158 // Check that dir is not straight up/down
159 // Uses a multiple of EPSILON to be safe
160 // We can just check z since beyond floating point errors `fw` should be
161 // normalized
162 if 1.0 - fw.z.abs() > f32::EPSILON * 4.0 {
163 // We know direction lies in the xy plane so we only need to compute a rotation
164 // about the z-axis
165 let Vec2 { x, y } = fw.xy().normalized();
166 // Negate x and swap coords since we want to compute the angle from y+
167 let quat = rotation_2d(Vec2::new(y, -x), Vec3::unit_z());
168
169 Self(quat)
170 } else {
171 // if the direction is straight down, pitch up, or if straight up, pitch down
172 if fw.z < 0.0 {
173 self.pitched_up(FRAC_PI_2)
174 } else {
175 self.pitched_down(FRAC_PI_2)
176 }
177 // TODO: test this alternative for speed and correctness compared to
178 // current impl
179 //
180 // removes a branch
181 //
182 // use core::f32::consts::FRAC_1_SQRT_2;
183 // let cos = FRAC_1_SQRT_2;
184 // let sin = -FRAC_1_SQRT_2 * fw.z.signum();
185 // let axis = Vec3::unit_x();
186 // let scalar = cos;
187 // let vector = sin * axis;
188 // Self((self.0 * Quaternion::from_scalar_and_vec3((scalar,
189 // vector))).normalized())
190 }
191 }
192
193 /// Find the angle between two `Ori`s
194 ///
195 /// NOTE: This finds the angle of the quaternion between the two `Ori`s
196 /// which can involve rolling and thus can be larger than simply the
197 /// angle between vectors at the start and end points.
198 ///
199 /// Returns angle in radians
200 pub fn angle_between(self, other: Self) -> f32 {
201 // Compute quaternion from one ori to the other
202 // https://www.mathworks.com/matlabcentral/answers/476474-how-to-find-the-angle-between-two-quaternions#answer_387973
203 let between = self.to_quat().conjugate() * other.to_quat();
204 // Then compute it's angle
205 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
206 //
207 // NOTE: acos is very sensitive to errors at small angles
208 // - https://www.researchgate.net/post/How_do_I_calculate_the_smallest_angle_between_two_quaternions
209 // - see angle_between unit test epislons
210 let angle = 2.0 * between.w.clamp(-1.0, 1.0).acos();
211 if angle < PI { angle } else { TAU - angle }
212 }
213
214 pub fn dot(self, other: Self) -> f32 { self.look_vec().dot(other.look_vec()) }
215
216 #[must_use]
217 pub fn pitched_up(self, angle_radians: f32) -> Self {
218 self.rotated(Quaternion::rotation_x(angle_radians))
219 }
220
221 #[must_use]
222 pub fn pitched_down(self, angle_radians: f32) -> Self {
223 self.rotated(Quaternion::rotation_x(-angle_radians))
224 }
225
226 #[must_use]
227 pub fn yawed_left(self, angle_radians: f32) -> Self {
228 self.rotated(Quaternion::rotation_z(angle_radians))
229 }
230
231 #[must_use]
232 pub fn yawed_right(self, angle_radians: f32) -> Self {
233 self.rotated(Quaternion::rotation_z(-angle_radians))
234 }
235
236 #[must_use]
237 pub fn rolled_left(self, angle_radians: f32) -> Self {
238 self.rotated(Quaternion::rotation_y(-angle_radians))
239 }
240
241 #[must_use]
242 pub fn rolled_right(self, angle_radians: f32) -> Self {
243 self.rotated(Quaternion::rotation_y(angle_radians))
244 }
245
246 /// Returns a version which is rolled such that its up points towards `dir`
247 /// as much as possible without pitching or yawing
248 #[must_use]
249 pub fn rolled_towards(self, dir: Dir) -> Self {
250 dir.projected(&Plane::from(self.look_dir()))
251 .map_or(self, |dir| self.prerotated(self.up().rotation_between(dir)))
252 }
253
254 /// Returns a version which has been pitched towards `dir` as much as
255 /// possible without yawing or rolling
256 #[must_use]
257 pub fn pitched_towards(self, dir: Dir) -> Self {
258 dir.projected(&Plane::from(self.right()))
259 .map_or(self, |dir_| {
260 self.prerotated(self.look_dir().rotation_between(dir_))
261 })
262 }
263
264 /// Returns a version which has been yawed towards `dir` as much as possible
265 /// without pitching or rolling
266 #[must_use]
267 pub fn yawed_towards(self, dir: Dir) -> Self {
268 dir.projected(&Plane::from(self.up())).map_or(self, |dir_| {
269 self.prerotated(self.look_dir().rotation_between(dir_))
270 })
271 }
272
273 /// Returns a version without sideways tilt (roll)
274 ///
275 /// ```
276 /// use veloren_common::comp::Ori;
277 ///
278 /// let ang = 45_f32.to_radians();
279 /// let zenith = vek::Vec3::unit_z();
280 ///
281 /// let rl = Ori::default().rolled_left(ang);
282 /// assert!((rl.up().angle_between(zenith) - ang).abs() <= f32::EPSILON);
283 /// assert!(rl.uprighted().up().angle_between(zenith) <= f32::EPSILON);
284 ///
285 /// let pd_rr = Ori::default().pitched_down(ang).rolled_right(ang);
286 /// let pd_upr = pd_rr.uprighted();
287 ///
288 /// assert!((pd_upr.up().angle_between(zenith) - ang).abs() <= f32::EPSILON);
289 ///
290 /// let ang1 = pd_upr.rolled_right(ang).up().angle_between(zenith);
291 /// let ang2 = pd_rr.up().angle_between(zenith);
292 /// assert!((ang1 - ang2).abs() <= f32::EPSILON);
293 /// ```
294 #[must_use]
295 pub fn uprighted(self) -> Self { self.look_dir().into() }
296
297 fn is_normalized(&self) -> bool { self.0.into_vec4().is_normalized() }
298}
299
300/// Produce a quaternion from an axis to rotate about and a 2D point on the unit
301/// circle to rotate to
302///
303/// NOTE: the provided axis and 2D vector must be normalized
304fn rotation_2d(Vec2 { x, y }: Vec2<f32>, axis: Vec3<f32>) -> Quaternion<f32> {
305 // Skip needing the angle for quaternion construction by computing cos/sin
306 // directly from the normalized x value
307 //
308 // scalar = cos(theta / 2)
309 // vector = axis * sin(theta / 2)
310 //
311 // cos(a / 2) = +/- ((1 + cos(a)) / 2)^0.5
312 // sin(a / 2) = +/- ((1 - cos(a)) / 2)^0.5
313 //
314 // scalar = +/- sqrt((1 + cos(a)) / 2)
315 // vector = vec3(0, 0, 1) * +/- sqrt((1 - cos(a)) / 2)
316 //
317 // cos(a) = x / |xy| => x (when normalized)
318
319 // Prevent NaNs from negative sqrt (float errors can put this slightly over 1.0)
320 let x = x.clamp(-1.0, 1.0);
321
322 let scalar = ((1.0 + x) / 2.0).sqrt() * y.signum();
323 let vector = axis * ((1.0 - x) / 2.0).sqrt();
324
325 // This is normalized by our construction above
326 Quaternion::from_scalar_and_vec3((scalar, vector))
327}
328
329impl From<Dir> for Ori {
330 fn from(dir: Dir) -> Self {
331 // Check that dir is not straight up/down
332 // Uses a multiple of EPSILON to be safe
333 let quat = if 1.0 - dir.z.abs() > f32::EPSILON * 4.0 {
334 // Compute rotation that will give an "upright" orientation (no
335 // rolling):
336 let xy_len = dir.xy().magnitude();
337 let xy_norm = dir.xy() / xy_len;
338 // Rotation to get to this projected point from the default direction of y+
339 // Negate x and swap coords since we want to compute the angle from y+
340 let yaw = rotation_2d(Vec2::new(xy_norm.y, -xy_norm.x), Vec3::unit_z());
341 // Rotation to then rotate up/down to the match the input direction
342 // In this rotated space the xy_len becomes the distance along the x axis
343 // And since we rotated around the z-axis the z value is unchanged
344 let pitch = rotation_2d(Vec2::new(xy_len, dir.z), Vec3::unit_x());
345
346 (yaw * pitch).normalized()
347 } else {
348 // Nothing in particular can be considered upright if facing up or down
349 // so we just produce a quaternion that will rotate to that direction
350 // (once again rotating from y+)
351 let pitch = PI / 2.0 * dir.z.signum();
352 Quaternion::rotation_x(pitch)
353 };
354
355 Self(quat)
356 }
357}
358
359impl From<Vec3<f32>> for Ori {
360 fn from(dir: Vec3<f32>) -> Self { Dir::from_unnormalized(dir).unwrap_or_default().into() }
361}
362
363impl From<Quaternion<f32>> for Ori {
364 fn from(quat: Quaternion<f32>) -> Self { Self::new(quat) }
365}
366
367/*
368impl From<vek::quaternion::repr_simd::Quaternion<f32>> for Ori {
369 fn from(
370 vek::quaternion::repr_simd::Quaternion { x, y, z, w }: vek::quaternion::repr_simd::Quaternion<f32>,
371 ) -> Self {
372 Self::from(Quaternion { x, y, z, w })
373 }
374}
375*/
376
377impl From<Ori> for Quaternion<f32> {
378 fn from(Ori(q): Ori) -> Self { q }
379}
380
381/*
382impl From<Ori> for vek::quaternion::repr_simd::Quaternion<f32> {
383 fn from(Ori(Quaternion { x, y, z, w }): Ori) -> Self {
384 vek::quaternion::repr_simd::Quaternion { x, y, z, w }
385 }
386}
387*/
388
389impl From<Ori> for Dir {
390 fn from(ori: Ori) -> Self { ori.look_dir() }
391}
392
393impl From<Ori> for Vec3<f32> {
394 fn from(ori: Ori) -> Self { ori.look_vec() }
395}
396
397/*
398impl From<Ori> for vek::vec::repr_simd::Vec3<f32> {
399 fn from(ori: Ori) -> Self { vek::vec::repr_simd::Vec3::from(ori.look_vec()) }
400}
401*/
402
403impl From<Ori> for Vec2<f32> {
404 fn from(ori: Ori) -> Self { ori.look_dir().to_horizontal().unwrap_or_default().xy() }
405}
406
407/*
408impl From<Ori> for vek::vec::repr_simd::Vec2<f32> {
409 fn from(ori: Ori) -> Self { vek::vec::repr_simd::Vec2::from(ori.look_vec().xy()) }
410}
411*/
412
413// Validate at Deserialization
414#[derive(Copy, Clone, Default, Debug, PartialEq, Serialize, Deserialize)]
415struct SerdeOri(Quaternion<f32>);
416
417impl From<SerdeOri> for Ori {
418 fn from(serde_quat: SerdeOri) -> Self {
419 let quat: Quaternion<f32> = serde_quat.0;
420 if quat.into_vec4().map(f32::is_nan).reduce_or() {
421 tracing::warn!(
422 ?quat,
423 "Deserialized rotation quaternion containing NaNs, replacing with default"
424 );
425 Default::default()
426 } else if !Self(quat).is_normalized() {
427 tracing::warn!(
428 ?quat,
429 "Deserialized unnormalized rotation quaternion (magnitude: {}), replacing with \
430 default",
431 quat.magnitude()
432 );
433 Default::default()
434 } else {
435 Self::new(quat)
436 }
437 }
438}
439
440impl From<Ori> for SerdeOri {
441 fn from(other: Ori) -> SerdeOri { SerdeOri(other.to_quat()) }
442}
443
444impl Component for Ori {
445 type Storage = specs::VecStorage<Self>;
446}
447
448#[cfg(test)]
449mod tests {
450 use super::*;
451
452 // Helper method to produce Dirs at different angles to test
453 fn dirs() -> impl Iterator<Item = Dir> {
454 let angles = 32;
455 (0..angles).flat_map(move |i| {
456 let theta = PI * 2.0 * (i as f32) / (angles as f32);
457
458 let v = Vec3::unit_y();
459 let q = Quaternion::rotation_x(theta);
460 let dir_1 = Dir::new(q * v);
461
462 let v = Vec3::unit_z();
463 let q = Quaternion::rotation_y(theta);
464 let dir_2 = Dir::new(q * v);
465
466 let v = Vec3::unit_x();
467 let q = Quaternion::rotation_z(theta);
468 let dir_3 = Dir::new(q * v);
469
470 [dir_1, dir_2, dir_3]
471 })
472 }
473
474 #[test]
475 fn to_horizontal() {
476 let to_horizontal = |dir: Dir| {
477 let ori = Ori::from(dir);
478
479 let horizontal = ori.to_horizontal();
480
481 approx::assert_relative_eq!(horizontal.look_dir().xy().magnitude(), 1.0);
482 approx::assert_relative_eq!(horizontal.look_dir().z, 0.0);
483 // Check correctness by comparing with Dir::to_horizontal
484 if let Some(dir_h) = ori.look_dir().to_horizontal() {
485 let quat_correct = Quaternion::<f32>::rotation_from_to_3d(Dir::default(), dir_h);
486 #[rustfmt::skip]
487 assert!(
488 dir_h
489 .map2(*horizontal.look_dir(), |d, o| approx::relative_eq!(d, o, epsilon = f32::EPSILON * 4.0))
490 .reduce_and(),
491 "\n\
492 Original: {:?}\n\
493 Dir::to_horizontal: {:?}\n\
494 Ori::to_horizontal(as dir): {:?}\n\
495 Ori::to_horizontal(as quat): {:?}\n\
496 Correct quaternion {:?}",
497 ori.look_dir(),
498 dir_h,
499 horizontal.look_dir(),
500 horizontal,
501 quat_correct,
502 );
503 }
504 };
505
506 dirs().for_each(to_horizontal);
507 }
508
509 #[test]
510 fn angle_between() {
511 let axis_list = (-16..17)
512 .map(|i| i as f32 / 16.0)
513 .flat_map(|fraction| {
514 [
515 Vec3::new(1.0 - fraction, fraction, 0.0),
516 Vec3::new(0.0, 1.0 - fraction, fraction),
517 Vec3::new(fraction, 0.0, 1.0 - fraction),
518 ]
519 })
520 .collect::<Vec<_>>();
521 // Iterator over some angles between 0 and 180
522 let angles = (0..129).map(|i| i as f32 / 128.0 * PI);
523
524 for angle_a in angles.clone() {
525 for angle_b in angles.clone() {
526 for axis in axis_list.iter().copied() {
527 let ori_a = Ori(Quaternion::rotation_3d(angle_a, axis));
528 let ori_b = Ori(Quaternion::rotation_3d(angle_b, axis));
529
530 let angle = (angle_a - angle_b).abs();
531 let epsilon = match angle {
532 angle if angle > 0.5 => f32::EPSILON * 20.0,
533 angle if angle > 0.2 => 0.00001,
534 angle if angle > 0.01 => 0.0001,
535 _ => 0.002,
536 };
537 approx::assert_relative_eq!(
538 ori_a.angle_between(ori_b),
539 angle,
540 epsilon = epsilon,
541 );
542 }
543 }
544 }
545 }
546
547 #[test]
548 fn from_to_dir() {
549 let from_to = |dir: Dir| {
550 let ori = Ori::from(dir);
551
552 assert!(ori.is_normalized(), "ori {:?}\ndir {:?}", ori, dir);
553 assert!(
554 approx::relative_eq!(ori.look_dir().dot(*dir), 1.0),
555 "Ori::from(dir).look_dir() != dir\ndir: {:?}\nOri::from(dir).look_dir(): {:?}",
556 dir,
557 ori.look_dir(),
558 );
559 approx::assert_relative_eq!((ori.to_quat() * Dir::default()).dot(*dir), 1.0);
560 };
561
562 dirs().for_each(from_to);
563 }
564
565 #[test]
566 fn orthogonal_dirs() {
567 let ori = Ori::default();
568 let def = Dir::default();
569 for dir in &[ori.up(), ori.down(), ori.left(), ori.right()] {
570 approx::assert_relative_eq!(dir.dot(*def), 0.0);
571 }
572 }
573}