Improve efficiency of states::utils::handle_orientation by reducing the conversions between Ori/Dir less frequent and optimizing the conversion of Dir -> Ori, also added a method to compute the angle between two Ori so that they don't need to be converted to Dir

common/src/comp/character_state.rs

@@ -37,6 +37,7 @@ pub struct StateUpdate {
 
 impl From<&JoinData<'_>> for StateUpdate {
     fn from(data: &JoinData) -> Self {
+        common_base::prof_span!("StateUpdate::from");
         StateUpdate {
             pos: *data.pos,
             vel: *data.vel,

common/src/comp/ori.rs

@@ -170,6 +170,18 @@ impl Ori {
             )
     }
 
+    /// Find the angle between two `Ori`s
+    ///
+    /// Returns angle in radians
+    pub fn angle_between(self, other: Self) -> f32 {
+        // Compute quaternion from one ori to the other
+        // https://www.mathworks.com/matlabcentral/answers/476474-how-to-find-the-angle-between-two-quaternions#answer_387973
+        let between = self.to_quat().conjugate() * other.to_quat();
+        // Then compute it's angle
+        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
+        2.0 * between.w.acos()
+    }
+
     pub fn pitched_up(self, angle_radians: f32) -> Self {
         self.rotated(Quaternion::rotation_x(angle_radians))
     }
@@ -239,38 +251,37 @@ impl Ori {
     /// 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::up().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,
-        }
-    }
+    pub fn uprighted(self) -> Self { self.look_dir().into() }
 
     fn is_normalized(&self) -> bool { self.0.into_vec4().is_normalized() }
 }
-
 impl From<Dir> for Ori {
     fn from(dir: Dir) -> Self {
-        let from = Dir::default();
-        let q = Quaternion::<f32>::rotation_from_to_3d(*from, *dir).normalized();
+        // Check that dir is not straight up/down
+        // Uses a multiple of EPSILON to be safe
+        let quat = if dir.z.abs() - 1.0 > f32::EPSILON * 4.0 {
+            // Compute rotation that will give an "upright" orientation (no rolling):
 
-        Self(q).uprighted()
+            // Rotation to get to this projected point from the default direction of y+
+            let yaw = dir.xy().normalized().y.acos() * dir.x.signum();
+            // Rotation to then rotate up/down to the match the input direction
+            let pitch = dir.z.asin();
+
+            (Quaternion::rotation_z(yaw) * Quaternion::rotation_x(pitch)).normalized()
+        } else {
+            // Nothing in particular can be considered upright if facing up or down
+            // so we just produce a quaternion that will rotate to that direction
+            let from = Dir::default();
+            // This calls normalized() internally
+            Quaternion::<f32>::rotation_from_to_3d(*from, *dir)
+        };
+
+        Self(quat)
     }
 }
 
 impl From<Vec3<f32>> for Ori {
-    fn from(dir: Vec3<f32>) -> Self {
-        let dir = Dir::from_unnormalized(dir).unwrap_or_default();
-        let from = Dir::default();
-        let q = Quaternion::<f32>::rotation_from_to_3d(*from, *dir).normalized();
-
-        Self(q).uprighted()
-    }
+    fn from(dir: Vec3<f32>) -> Self { Dir::from_unnormalized(dir).unwrap_or_default().into() }
 }
 
 impl From<Quaternion<f32>> for Ori {
@@ -359,6 +370,7 @@ mod tests {
         let from_to = |dir: Dir| {
             let ori = Ori::from(dir);
 
+            assert!(ori.is_normalized(), "ori {:?}\ndir {:?}", ori, dir);
             approx::assert_relative_eq!(ori.look_dir().dot(*dir), 1.0);
             approx::assert_relative_eq!((ori.to_quat() * Dir::default()).dot(*dir), 1.0);
         };

common/src/states/utils.rs

@@ -387,24 +387,29 @@ pub fn handle_orientation(
     efficiency: f32,
     dir_override: Option<Dir>,
 ) {
+    common_base::prof_span!("handle_orientation");
     // Direction is set to the override if one is provided, else if entity is
     // strafing or attacking the horiontal component of the look direction is used,
     // else the current horizontal movement direction is used
-    let dir = if let Some(dir_override) = dir_override {
-        dir_override
+    let target_ori = if let Some(dir_override) = dir_override {
+        dir_override.into()
     } else if is_strafing(data, update) || update.character.is_attack() {
-        data.inputs.look_dir.to_horizontal().unwrap_or_default()
+        data.inputs
+            .look_dir
+            .to_horizontal()
+            .unwrap_or_default()
+            .into()
     } else {
         Dir::from_unnormalized(data.inputs.move_dir.into())
-            .unwrap_or_else(|| data.ori.to_horizontal().look_dir())
+            .map_or_else(|| data.ori.to_horizontal(), |dir| dir.into())
     };
     let rate = {
-        let angle = update.ori.look_dir().angle_between(*dir);
+        let angle = update.ori.angle_between(target_ori);
         data.body.base_ori_rate() * efficiency * std::f32::consts::PI / angle
     };
     update.ori = update
         .ori
-        .slerped_towards(dir.into(), (data.dt.0 * rate).min(1.0));
+        .slerped_towards(target_ori, (data.dt.0 * rate).min(1.0));
 }
 
 /// Updates components to move player as if theyre swimming

common/systems/src/character_behavior.rs

@@ -16,6 +16,7 @@ use common::{
     terrain::TerrainGrid,
     uid::Uid,
 };
+use common_base::prof_span;
 use common_ecs::{Job, Origin, Phase, System};
 use std::time::Duration;
 
@@ -123,6 +124,7 @@ impl<'a> System<'a> for Sys {
         )
             .join()
         {
+            prof_span!("entity");
             // Being dead overrides all other states
             if health.map_or(false, |h| h.is_dead) {
                 // Do nothing