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}