veloren_common/util/
dir.rs

1use crate::comp::{self, Ori};
2
3use super::{Plane, Projection};
4use rand::Rng;
5use serde::{Deserialize, Serialize};
6use tracing::warn;
7use vek::*;
8
9/// Type representing a direction using Vec3 that is normalized and NaN free
10/// These properties are enforced actively via panics when `debug_assertions` is
11/// enabled
12#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
13#[serde(into = "SerdeDir")]
14#[serde(from = "SerdeDir")]
15pub struct Dir(Vec3<f32>);
16impl Default for Dir {
17    fn default() -> Self { Self::forward() }
18}
19
20// Validate at Deserialization
21#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
22struct SerdeDir(Vec3<f32>);
23impl From<SerdeDir> for Dir {
24    fn from(dir: SerdeDir) -> Self {
25        let dir = dir.0;
26        if dir.map(f32::is_nan).reduce_or() {
27            warn!(
28                ?dir,
29                "Deserialized dir containing NaNs, replacing with default"
30            );
31            Default::default()
32        } else if !dir.is_normalized() {
33            warn!(
34                ?dir,
35                "Deserialized unnormalized dir, replacing with default"
36            );
37            Default::default()
38        } else {
39            Self(dir)
40        }
41    }
42}
43
44impl From<Dir> for SerdeDir {
45    fn from(other: Dir) -> SerdeDir { SerdeDir(*other) }
46}
47/*pub enum TryFromVec3Error {
48    ContainsNans,
49    NotNormalized,
50}
51
52impl TryFrom<Vec3<f32>> for Dir {
53    type Error = TryFromVec3Error;
54
55    fn try_from(v: Vec3) -> Result<Self, TryFromVec3Error> {
56        if v.map(f32::is_nan).reduce_or() {
57            Err(TryFromVec3Error::ContainsNans)
58        } else {
59            v.try_normalized()
60                .map(|n| Self(n))
61                .ok_or(TryFromVec3Error::NotNormalized)
62        }
63    }
64}*/
65
66impl Dir {
67    pub fn new(dir: Vec3<f32>) -> Self {
68        debug_assert!(!dir.map(f32::is_nan).reduce_or());
69        debug_assert!(dir.is_normalized());
70        Self(dir)
71    }
72
73    pub fn from_unnormalized(dirs: Vec3<f32>) -> Option<Self> {
74        dirs.try_normalized().map(|dir| {
75            #[cfg(debug_assertions)]
76            {
77                if dir.map(f32::is_nan).reduce_or() {
78                    panic!("{} => {}", dirs, dir);
79                }
80            }
81            Self(dir)
82        })
83    }
84
85    /// Generates a random direction that has a z component of 0
86    pub fn random_2d(rng: &mut impl Rng) -> Self {
87        let a = rng.random_range(0.0..std::f32::consts::TAU);
88        // This will always be normalized.
89        Self::new(Vec3::new(a.cos(), a.sin(), 0.0))
90    }
91
92    pub fn slerp(from: Self, to: Self, factor: f32) -> Self {
93        Self(slerp_normalized(from.0, to.0, factor))
94    }
95
96    #[must_use]
97    pub fn slerped_to(self, to: Self, factor: f32) -> Self {
98        Self(slerp_normalized(self.0, to.0, factor))
99    }
100
101    /// Note: this uses `from` if `to` is unnormalizable
102    pub fn slerp_to_vec3(from: Self, to: Vec3<f32>, factor: f32) -> Self {
103        Self(slerp_to_unnormalized(from.0, to, factor).unwrap_or_else(|e| e))
104    }
105
106    pub fn rotation_between(&self, to: Self) -> Quaternion<f32> {
107        Quaternion::<f32>::rotation_from_to_3d(self.0, to.0)
108    }
109
110    pub fn rotation(&self) -> Quaternion<f32> { Self::default().rotation_between(*self) }
111
112    pub fn is_valid(&self) -> bool { !self.0.map(f32::is_nan).reduce_or() && self.is_normalized() }
113
114    pub fn up() -> Self { Dir::new(Vec3::<f32>::unit_z()) }
115
116    pub fn down() -> Self { -Dir::new(Vec3::<f32>::unit_z()) }
117
118    pub fn left() -> Self { -Dir::new(Vec3::<f32>::unit_x()) }
119
120    pub fn right() -> Self { Dir::new(Vec3::<f32>::unit_x()) }
121
122    pub fn forward() -> Self { Dir::new(Vec3::<f32>::unit_y()) }
123
124    pub fn back() -> Self { -Dir::new(Vec3::<f32>::unit_y()) }
125
126    pub fn to_horizontal(self) -> Option<Self> { Self::from_unnormalized(self.xy().into()) }
127
128    pub fn vec(&self) -> &Vec3<f32> { &self.0 }
129
130    pub fn to_vec(self) -> Vec3<f32> { self.0 }
131
132    pub fn merge_z(self, look_dir: Self) -> Self {
133        // This code just gets look_dir without Z part
134        // and normalizes it. This is what `xy_dir is`.
135        //
136        // Then we find rotation between xy_dir and look_dir
137        // which gives us quaternion how of what rotation we need
138        // to do to get Z part we want.
139        //
140        // Then we construct Ori without Z part
141        // and applying `pitch` to get needed orientation.
142        let xy_dir = Dir::from_unnormalized(Vec3::new(self.x, self.y, 0.0)).unwrap_or_default();
143        let pitch = xy_dir.rotation_between(self);
144
145        Ori::from(Vec3::new(look_dir.x, look_dir.y, 0.0))
146            .prerotated(pitch)
147            .look_dir()
148    }
149
150    /// Generate a direction corresponding to an entity looking toward a target
151    /// entity given their respective positions, bodies, and scales.
152    pub fn look_toward(
153        this_pos: &comp::Pos,
154        this_body: Option<&comp::Body>,
155        this_scale: Option<&comp::Scale>,
156        tgt_pos: &comp::Pos,
157        tgt_body: Option<&comp::Body>,
158        tgt_scale: Option<&comp::Scale>,
159    ) -> Option<Self> {
160        let eye_offset = this_body.map_or(0.0, |b| b.eye_height(this_scale.map_or(1.0, |s| s.0)));
161        let tgt_eye_offset = tgt_body.map_or(0.0, |b| b.eye_height(tgt_scale.map_or(1.0, |s| s.0)));
162        Dir::from_unnormalized(
163            Vec3::new(tgt_pos.0.x, tgt_pos.0.y, tgt_pos.0.z + tgt_eye_offset)
164                - Vec3::new(this_pos.0.x, this_pos.0.y, this_pos.0.z + eye_offset),
165        )
166    }
167}
168
169impl std::ops::Deref for Dir {
170    type Target = Vec3<f32>;
171
172    fn deref(&self) -> &Vec3<f32> { &self.0 }
173}
174
175impl From<Dir> for Vec3<f32> {
176    fn from(dir: Dir) -> Self { *dir }
177}
178
179impl Projection<Plane> for Dir {
180    type Output = Option<Self>;
181
182    fn projected(self, plane: &Plane) -> Self::Output {
183        Dir::from_unnormalized(plane.projection(*self))
184    }
185}
186
187impl Projection<Dir> for Vec3<f32> {
188    type Output = Vec3<f32>;
189
190    fn projected(self, dir: &Dir) -> Self::Output {
191        let dir = **dir;
192        self.dot(dir) * dir
193    }
194}
195
196impl std::ops::Mul<Dir> for Quaternion<f32> {
197    type Output = Dir;
198
199    fn mul(self, dir: Dir) -> Self::Output { Dir((self * *dir).normalized()) }
200}
201
202impl std::ops::Neg for Dir {
203    type Output = Dir;
204
205    fn neg(self) -> Dir { Dir::new(-self.0) }
206}
207
208/// Begone ye NaN's
209/// Slerp two `Vec3`s skipping the slerp if their directions are very close
210/// This avoids a case where `vek`s slerp produces NaN's
211/// Additionally, it avoids unnecessary calculations if they are near identical
212/// Assumes `from` and `to` are normalized and returns a normalized vector
213#[inline(always)]
214fn slerp_normalized(from: Vec3<f32>, to: Vec3<f32>, factor: f32) -> Vec3<f32> {
215    debug_assert!(!to.map(f32::is_nan).reduce_or());
216    debug_assert!(!from.map(f32::is_nan).reduce_or());
217    // Ensure from is normalized
218    #[cfg(debug_assertions)]
219    {
220        let unnormalized = {
221            let len_sq = from.magnitude_squared();
222            !(0.999..=1.001).contains(&len_sq)
223        };
224
225        if unnormalized {
226            panic!("Called slerp_normalized with unnormalized `from`: {}", from);
227        }
228    }
229
230    // Ensure to is normalized
231    #[cfg(debug_assertions)]
232    {
233        let unnormalized = {
234            let len_sq = from.magnitude_squared();
235            !(0.999..=1.001).contains(&len_sq)
236        };
237
238        if unnormalized {
239            panic!("Called slerp_normalized with unnormalized `to`: {}", to);
240        }
241    }
242
243    let dot = from.dot(to);
244    if dot >= 1.0 - 1E-6 {
245        // Close together, just use to
246        return to;
247    }
248
249    let (from, to, factor) = if dot < -0.999 {
250        // Not linearly independent (slerp will fail since it doesn't check for this)
251        // Instead we will choose a midpoint and slerp from or to that depending on the
252        // factor
253        let mid_dir = if from.z.abs() > 0.999 {
254            // If vec's lie along the z-axis default to (1, 0, 0) as midpoint
255            Vec3::unit_x()
256        } else {
257            // Default to picking midpoint in the xy plane
258            Vec3::new(from.y, -from.x, 0.0).normalized()
259        };
260
261        if factor > 0.5 {
262            (mid_dir, to, factor * 2.0 - 1.0)
263        } else {
264            (from, mid_dir, factor * 2.0)
265        }
266    } else {
267        (from, to, factor)
268    };
269
270    let slerped = Vec3::slerp(from, to, factor);
271    let slerped_normalized = slerped.normalized();
272    // Ensure normalization worked
273    // This should not be possible but I will leave it here for now just in case
274    // something was missed
275    #[cfg(debug_assertions)]
276    {
277        if !slerped_normalized.is_normalized() || slerped_normalized.map(f32::is_nan).reduce_or() {
278            panic!(
279                "Failed to normalize {:?} produced from:\nslerp(\n    {:?},\n    {:?},\n    \
280                 {:?},\n)\nWith result: {:?})",
281                slerped, from, to, factor, slerped_normalized
282            );
283        }
284    }
285
286    slerped_normalized
287}
288
289/// Begone ye NaN's
290/// Slerp two `Vec3`s skipping the slerp if their directions are very close
291/// This avoids a case where `vek`s slerp produces NaN's
292/// Additionally, it avoids unnecessary calculations if they are near identical
293/// Assumes `from` is normalized and returns a normalized vector, but `to`
294/// doesn't need to be normalized
295/// Returns `Err(from)`` if `to` is unnormalizable
296// TODO: in some cases we might want to base the slerp rate on the magnitude of
297// `to` for example when `to` is velocity and `from` is orientation
298fn slerp_to_unnormalized(
299    from: Vec3<f32>,
300    to: Vec3<f32>,
301    factor: f32,
302) -> Result<Vec3<f32>, Vec3<f32>> {
303    to.try_normalized()
304        .map(|to| slerp_normalized(from, to, factor))
305        .ok_or(from)
306}
veloren_common/util/dir.rs

veloren_common/util/
dir.rs
