diff --git a/common/src/comp/character_state.rs b/common/src/comp/character_state.rs index 1feb6ed264..9d27425d39 100644 --- a/common/src/comp/character_state.rs +++ b/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, diff --git a/common/src/comp/ori.rs b/common/src/comp/ori.rs index 5531c94829..8ac46732d6 100644 --- a/common/src/comp/ori.rs +++ b/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 for Ori { fn from(dir: Dir) -> Self { - let from = Dir::default(); - let q = Quaternion::::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::::rotation_from_to_3d(*from, *dir) + }; + + Self(quat) } } impl From> for Ori { - fn from(dir: Vec3) -> Self { - let dir = Dir::from_unnormalized(dir).unwrap_or_default(); - let from = Dir::default(); - let q = Quaternion::::rotation_from_to_3d(*from, *dir).normalized(); - - Self(q).uprighted() - } + fn from(dir: Vec3) -> Self { Dir::from_unnormalized(dir).unwrap_or_default().into() } } impl From> 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); }; diff --git a/common/src/states/utils.rs b/common/src/states/utils.rs index 3b554b4f07..0a4f599120 100644 --- a/common/src/states/utils.rs +++ b/common/src/states/utils.rs @@ -387,24 +387,29 @@ pub fn handle_orientation( efficiency: f32, dir_override: Option, ) { + 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 diff --git a/common/systems/src/character_behavior.rs b/common/systems/src/character_behavior.rs index f43fc53a05..d3ee92c2ba 100644 --- a/common/systems/src/character_behavior.rs +++ b/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