Add Plane type, Projection trait, fix upright() and add doc tests

common/src/comp/ori.rs

@@ -1,4 +1,4 @@
-use crate::util::Dir;
+use crate::util::{Dir, Plane, Projection};
 use serde::{Deserialize, Serialize};
 use specs::Component;
 use specs_idvs::IdvStorage;
@@ -24,21 +24,18 @@ impl Ori {
     }
 
     /// Tries to convert into a Dir and then the appropriate rotation
-    pub fn from_unnormalized_vec(vec: Vec3<f32>) -> Option<Self> {
-        Dir::from_unnormalized(vec).map(Self::from)
+    pub fn from_unnormalized_vec<T>(vec: T) -> Option<Self>
+    where
+        T: Into<Vec3<f32>>,
+    {
+        Dir::from_unnormalized(vec.into()).map(Self::from)
     }
 
     pub fn to_vec(self) -> Vec3<f32> { *self.look_dir() }
 
     pub fn to_quat(self) -> Quaternion<f32> { self.0 }
 
-    /// Transform the vector from local into global vector space
-    pub fn relative_to_world(&self, vec: Vec3<f32>) -> Vec3<f32> { self.0 * vec }
-
-    /// Transform the vector from global into local vector space
-    pub fn relative_to_self(&self, vec: Vec3<f32>) -> Vec3<f32> { self.0.inverse() * vec }
-
-    pub fn look_dir(&self) -> Dir { Dir::new(self.0.normalized() * *Dir::default()) }
+    pub fn look_dir(&self) -> Dir { Dir::new(self.0 * *Dir::default()) }
 
     pub fn up(&self) -> Dir { self.pitched_up(PI / 2.0).look_dir() }
 
@@ -55,10 +52,28 @@ impl Ori {
     pub fn slerped_towards(self, ori: Ori, s: f32) -> Self { Self::slerp(self, ori, s) }
 
     /// Multiply rotation quaternion by `q`
+    /// (the rotations are in local vector space).
     pub fn rotated(self, q: Quaternion<f32>) -> Self { Self((self.0 * q).normalized()) }
 
     /// Premultiply rotation quaternion by `q`
-    pub fn rotated_world(self, q: Quaternion<f32>) -> Self { Self((q * self.0).normalized()) }
+    /// (the rotations are in global vector space).
+    pub fn prerotated(self, q: Quaternion<f32>) -> Self { Self((q * self.0).normalized()) }
+
+    /// Take `global` into this Ori's local vector space
+    pub fn global_to_local<T>(&self, global: T) -> <Quaternion<f32> as std::ops::Mul<T>>::Output
+    where
+        Quaternion<f32>: std::ops::Mul<T>,
+    {
+        self.0.inverse() * global
+    }
+
+    /// Take `local` into the global vector space
+    pub fn local_to_global<T>(&self, local: T) -> <Quaternion<f32> as std::ops::Mul<T>>::Output
+    where
+        Quaternion<f32>: std::ops::Mul<T>,
+    {
+        self.0 * local
+    }
 
     pub fn pitched_up(self, angle_radians: f32) -> Self {
         self.rotated(Quaternion::rotation_x(angle_radians))
@@ -85,24 +100,46 @@ impl Ori {
     }
 
     /// Returns a version without sideways tilt (roll)
-    pub fn uprighted(self) -> Self { self.look_dir().into() }
+    ///
+    /// # Examples
+    /// ```
+    /// use veloren_common::comp::Ori;
+    ///
+    /// let ang = 45_f32.to_radians();
+    /// let zenith = vek::Vec3::unit_z();
+    ///
+    /// let rl = Ori::default().rolled_left(ang);
+    /// assert!((rl.up().angle_between(zenith) - ang).abs() < std::f32::EPSILON);
+    /// assert!(rl.uprighted().up().angle_between(zenith) < std::f32::EPSILON);
+    ///
+    /// let pd_rr = Ori::default().pitched_down(ang).rolled_right(ang);
+    /// let pd_upr = pd_rr.uprighted();
+    ///
+    /// assert!((pd_upr.up().angle_between(zenith) - ang).abs() < std::f32::EPSILON);
+    ///
+    /// let ang1 = pd_upr.rolled_right(ang).up().angle_between(zenith);
+    /// let ang2 = pd_rr.up().angle_between(zenith);
+    /// assert!((ang1 - ang2).abs() < std::f32::EPSILON);
+    /// ```
+    pub fn uprighted(self) -> Self {
+        let fw = self.look_dir();
+        match Dir::new(Vec3::unit_z()).projected(&Plane::from(fw)) {
+            Some(dir_p) => {
+                let up = self.up();
+                let go_right_s = fw.cross(*up).dot(*dir_p).signum();
+                self.rolled_right(up.angle_between(*dir_p) * go_right_s)
+            },
+            None => self,
+        }
+    }
 
     fn is_normalized(&self) -> bool { self.0.into_vec4().is_normalized() }
 }
 
 impl From<Dir> for Ori {
     fn from(dir: Dir) -> Self {
-        // rotate horizontally first and then vertically to prevent rolling
         let from = *Dir::default();
-        let q1 = (*dir * Vec3::new(1.0, 1.0, 0.0))
-            .try_normalized()
-            .map(|hv| Quaternion::<f32>::rotation_from_to_3d(from, hv).normalized())
-            .unwrap_or_default();
-        let q2 = (from + Vec3::new(0.0, 0.0, dir.z))
-            .try_normalized()
-            .map(|to| Quaternion::<f32>::rotation_from_to_3d(from, to).normalized())
-            .unwrap_or_default();
-        Self((q1 * q2).normalized())
+        Self::from(Quaternion::<f32>::rotation_from_to_3d(from, *dir)).uprighted()
     }
 }
 
@@ -118,7 +155,7 @@ impl From<vek::quaternion::repr_simd::Quaternion<f32>> for Ori {
     fn from(
         vek::quaternion::repr_simd::Quaternion { x, y, z, w }: vek::quaternion::repr_simd::Quaternion<f32>,
     ) -> Self {
-        Self(Quaternion { x, y, z, w }.normalized())
+        Self::from(Quaternion { x, y, z, w })
     }
 }
 
@@ -136,10 +173,18 @@ impl From<Ori> for Vec3<f32> {
     fn from(ori: Ori) -> Self { *ori.look_dir() }
 }
 
+impl From<Ori> for vek::vec::repr_simd::Vec3<f32> {
+    fn from(ori: Ori) -> Self { vek::vec::repr_simd::Vec3::from(*ori.look_dir()) }
+}
+
 impl From<Ori> for Vec2<f32> {
     fn from(ori: Ori) -> Self { ori.look_dir().xy() }
 }
 
+impl From<Ori> for vek::vec::repr_simd::Vec2<f32> {
+    fn from(ori: Ori) -> Self { vek::vec::repr_simd::Vec2::from(ori.look_dir().xy()) }
+}
+
 // Validate at Deserialization
 #[derive(Copy, Clone, Default, Debug, PartialEq, Serialize, Deserialize)]
 struct SerdeOri(Quaternion<f32>);

common/src/util/dir.rs

@@ -1,3 +1,4 @@
+use super::{Plane, Projection};
 use serde::{Deserialize, Serialize};
 use tracing::warn;
 use vek::*;
@@ -95,8 +96,31 @@ impl std::ops::Deref for Dir {
     fn deref(&self) -> &Vec3<f32> { &self.0 }
 }
 
-impl From<Vec3<f32>> for Dir {
-    fn from(dir: Vec3<f32>) -> Self { Dir::new(dir) }
+impl From<Dir> for Vec3<f32> {
+    fn from(dir: Dir) -> Self { *dir }
+}
+
+impl std::ops::Mul<Dir> for Quaternion<f32> {
+    type Output = Dir;
+
+    fn mul(self, dir: Dir) -> Self::Output { Dir::new(self * *dir) }
+}
+
+impl Projection<Plane> for Dir {
+    type Output = Option<Self>;
+
+    fn projected(self, plane: &Plane) -> Self::Output {
+        Dir::from_unnormalized(plane.projection(*self))
+    }
+}
+
+impl Projection<Dir> for Vec3<f32> {
+    type Output = Vec3<f32>;
+
+    fn projected(self, dir: &Dir) -> Self::Output {
+        let dir = **dir;
+        self.dot(dir) * dir
+    }
 }
 
 impl std::ops::Neg for Dir {

common/src/util/mod.rs

@@ -2,6 +2,8 @@ mod color;
 pub mod dir;
 pub mod find_dist;
 mod option;
+pub mod plane;
+pub mod projection;
 pub mod userdata_dir;
 
 pub const GIT_VERSION: &str = include_str!(concat!(env!("OUT_DIR"), "/githash"));
@@ -28,6 +30,8 @@ lazy_static::lazy_static! {
 pub use color::*;
 pub use dir::*;
 pub use option::*;
+pub use plane::*;
+pub use projection::*;
 
 #[cfg(feature = "tracy")] pub use tracy_client;
 

common/src/util/plane.rs

@@ -0,0 +1,53 @@
+use super::{Dir, Projection};
+use serde::{Deserialize, Serialize};
+use vek::*;
+
+/// Plane
+
+// plane defined by its normal and origin
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Plane {
+    pub normal: Dir,
+    /// Distance from origin in the direction of normal
+    pub d: f32,
+}
+
+impl Plane {
+    pub fn new(dir: Dir) -> Self { Self::from(dir) }
+
+    pub fn distance(&self, to: Vec3<f32>) -> f32 { self.normal.dot(to) - self.d }
+
+    // fn center(&self) -> Vec3<f32> { *self.normal * self.d }
+
+    pub fn projection(&self, v: Vec3<f32>) -> Vec3<f32> { v - *self.normal * self.distance(v) }
+
+    pub fn xy() -> Self { Plane::from(Dir::new(Vec3::unit_z())) }
+
+    pub fn yz() -> Self { Plane::from(Dir::new(Vec3::unit_x())) }
+
+    pub fn zx() -> Self { Plane::from(Dir::new(Vec3::unit_y())) }
+}
+
+impl From<Dir> for Plane {
+    fn from(dir: Dir) -> Self {
+        Plane {
+            normal: dir,
+            d: 0.0,
+        }
+    }
+}
+
+impl Projection<Plane> for Vec3<f32> {
+    type Output = Vec3<f32>;
+
+    fn projected(self, plane: &Plane) -> Self::Output { plane.projection(self) }
+}
+
+impl<T> Projection<Plane> for Extent2<T>
+where
+    T: Projection<Plane, Output = T>,
+{
+    type Output = Self;
+
+    fn projected(self, plane: &Plane) -> Self::Output { self.map(|v| v.projected(plane)) }
+}

common/src/util/projection.rs

@@ -0,0 +1,28 @@
+use vek::{Vec2, Vec3};
+
+/// Projection trait for projection of linear types and shapes
+pub trait Projection<T> {
+    type Output;
+
+    fn projected(self, onto: &T) -> Self::Output;
+}
+
+// Impls
+
+impl Projection<Vec2<f32>> for Vec2<f32> {
+    type Output = Self;
+
+    fn projected(self, v: &Self) -> Self::Output {
+        let v = *v;
+        self.dot(v) * v / v.magnitude_squared()
+    }
+}
+
+impl Projection<Vec3<f32>> for Vec3<f32> {
+    type Output = Self;
+
+    fn projected(self, v: &Self) -> Self::Output {
+        let v = *v;
+        v * self.dot(v) / v.magnitude_squared()
+    }
+}