rg3d_core/math/
mod.rs

1// Clippy complains about normal mathematical symbols like A, B, C for quadratic equation.
2#![allow(clippy::many_single_char_names)]
3
4pub mod aabb;
5pub mod frustum;
6pub mod plane;
7pub mod ray;
8pub mod triangulator;
9
10use crate::algebra::{RealField, SimdRealField};
11use crate::{
12    algebra::{Matrix3, Matrix4, Scalar, UnitQuaternion, Vector2, Vector3},
13    math::ray::IntersectionResult,
14    num_traits::{NumAssign, Zero},
15    visitor::{Visit, VisitResult, Visitor},
16};
17use std::ops::{Index, IndexMut};
18
19#[derive(Copy, Clone, Debug, PartialEq)]
20pub struct Rect<T>
21where
22    T: NumAssign + Scalar + PartialOrd + Copy,
23{
24    pub position: Vector2<T>,
25    pub size: Vector2<T>,
26}
27
28impl<T> Default for Rect<T>
29where
30    T: NumAssign + Scalar + PartialOrd + Copy,
31{
32    fn default() -> Self {
33        Self {
34            position: Vector2::new(Zero::zero(), Zero::zero()),
35            size: Vector2::new(Zero::zero(), Zero::zero()),
36        }
37    }
38}
39
40impl<T> Rect<T>
41where
42    T: NumAssign + Scalar + PartialOrd + Copy,
43{
44    pub fn new(x: T, y: T, w: T, h: T) -> Self {
45        Self {
46            position: Vector2::new(x, y),
47            size: Vector2::new(w, h),
48        }
49    }
50
51    #[must_use = "this method creates new instance of rect"]
52    pub fn inflate(&self, dw: T, dh: T) -> Self {
53        Self {
54            position: Vector2::new(self.position.x - dw, self.position.y - dh),
55            size: Vector2::new(self.size.x + dw + dw, self.size.y + dh + dh),
56        }
57    }
58
59    #[must_use = "this method creates new instance of rect"]
60    pub fn deflate(&self, dw: T, dh: T) -> Self {
61        Self {
62            position: Vector2::new(self.position.x + dw, self.position.y + dh),
63            size: Vector2::new(self.size.x - (dw + dw), self.size.y - (dh + dh)),
64        }
65    }
66
67    pub fn contains(&self, pt: Vector2<T>) -> bool {
68        pt.x >= self.position.x
69            && pt.x <= self.position.x + self.size.x
70            && pt.y >= self.position.y
71            && pt.y <= self.position.y + self.size.y
72    }
73
74    /// Extends rect to contain given point.
75    ///
76    /// # Notes
77    ///
78    /// To build bounding rectangle you should correctly initialize initial rectangle:
79    ///
80    /// ```
81    /// # use rg3d_core::algebra::Vector2;
82    /// # use rg3d_core::math::Rect;
83    ///
84    /// let vertices = [Vector2::new(1.0, 2.0), Vector2::new(-3.0, 5.0)];
85    ///
86    /// // This is important part, it must have "invalid" state to correctly
87    /// // calculate bounding rect. Rect::default will give invalid result!
88    /// let mut bounding_rect = Rect::new(f32::MAX, f32::MAX, 0.0, 0.0);
89    ///
90    /// for &v in &vertices {
91    ///     bounding_rect.push(v);
92    /// }
93    /// ```
94    pub fn push(&mut self, p: Vector2<T>) {
95        if p.x < self.position.x {
96            self.position.x = p.x;
97        }
98        if p.y < self.position.y {
99            self.position.y = p.y;
100        }
101
102        let right_bottom = self.right_bottom_corner();
103
104        if p.x > right_bottom.x {
105            self.size.x = p.x - self.position.x;
106        }
107        if p.y > right_bottom.y {
108            self.size.y = p.y - self.position.y;
109        }
110    }
111
112    #[must_use = "this method creates new instance of rect"]
113    pub fn clip_by(&self, other: Rect<T>) -> Rect<T> {
114        let mut clipped = *self;
115
116        if clipped.position.x < other.position.x {
117            clipped.position.x = other.position.x;
118            clipped.size.x -= other.position.x - clipped.position.x;
119        }
120        if clipped.position.y < other.position.y {
121            clipped.position.y = other.position.y;
122            clipped.size.y -= other.position.y - clipped.position.y;
123        }
124
125        let clipped_right_bottom = clipped.right_bottom_corner();
126        let other_right_bottom = other.right_bottom_corner();
127
128        if clipped_right_bottom.x > other_right_bottom.x {
129            clipped.size.x -= clipped_right_bottom.x - other_right_bottom.x;
130        }
131        if clipped_right_bottom.y > other_right_bottom.y {
132            clipped.size.y -= clipped_right_bottom.y - other_right_bottom.y;
133        }
134
135        clipped
136    }
137
138    pub fn intersects(&self, other: Rect<T>) -> bool {
139        if other.position.x < self.position.x + self.size.x
140            && self.position.x < other.position.x + other.size.x
141            && other.position.y < self.position.y + self.size.y
142        {
143            self.position.y < other.position.y + other.size.y
144        } else {
145            false
146        }
147    }
148
149    #[must_use = "this method creates new instance of rect"]
150    pub fn translate(&self, translation: Vector2<T>) -> Self {
151        Self {
152            position: Vector2::new(
153                self.position.x + translation.x,
154                self.position.y + translation.y,
155            ),
156            size: self.size,
157        }
158    }
159
160    pub fn intersects_circle(&self, circle: Vector2<T>, radius: T) -> bool {
161        let r = self.position.x + self.size.x;
162        let b = self.position.y + self.size.y;
163        // find the closest point to the circle within the rectangle
164        let closest_x = if circle.x < self.position.x {
165            self.position.x
166        } else if circle.x > r {
167            r
168        } else {
169            circle.x
170        };
171        let closest_y = if circle.y < self.position.y {
172            self.position.y
173        } else if circle.y > b {
174            b
175        } else {
176            circle.y
177        };
178        // calculate the distance between the circle's center and this closest point
179        let distance_x = circle.x - closest_x;
180        let distance_y = circle.y - closest_y;
181        // if the distance is less than the circle's radius, an intersection occurs
182        let distance_squared = (distance_x * distance_x) + (distance_y * distance_y);
183        distance_squared < (radius * radius)
184    }
185
186    pub fn extend_to_contain(&mut self, other: Rect<T>) {
187        if other.position.x < self.position.x {
188            self.position.x = other.position.x;
189        }
190        if other.position.y < self.position.y {
191            self.position.y = other.position.y;
192        }
193        let self_right_bottom_corner = self.right_bottom_corner();
194        let other_right_bottom_corner = other.right_bottom_corner();
195        if other_right_bottom_corner.x > self_right_bottom_corner.x {
196            self.size.x += other_right_bottom_corner.x - self_right_bottom_corner.x;
197        }
198        if other_right_bottom_corner.y > self_right_bottom_corner.y {
199            self.size.y += other_right_bottom_corner.y - self_right_bottom_corner.y;
200        }
201    }
202
203    #[inline(always)]
204    pub fn left_top_corner(&self) -> Vector2<T> {
205        self.position
206    }
207
208    #[inline(always)]
209    pub fn right_top_corner(&self) -> Vector2<T> {
210        Vector2::new(self.position.x + self.size.x, self.position.y)
211    }
212
213    #[inline(always)]
214    pub fn right_bottom_corner(&self) -> Vector2<T> {
215        Vector2::new(self.position.x + self.size.x, self.position.y + self.size.y)
216    }
217
218    #[inline(always)]
219    pub fn left_bottom_corner(&self) -> Vector2<T> {
220        Vector2::new(self.position.x, self.position.y + self.size.y)
221    }
222
223    #[inline(always)]
224    pub fn w(&self) -> T {
225        self.size.x
226    }
227
228    #[inline(always)]
229    pub fn h(&self) -> T {
230        self.size.y
231    }
232
233    #[inline(always)]
234    pub fn x(&self) -> T {
235        self.position.x
236    }
237
238    #[inline(always)]
239    pub fn y(&self) -> T {
240        self.position.y
241    }
242}
243
244impl<T> Visit for Rect<T>
245where
246    T: NumAssign + Scalar + Visit + PartialOrd + Copy + 'static,
247{
248    fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
249        visitor.enter_region(name)?;
250
251        self.position.x.visit("X", visitor)?;
252        self.position.y.visit("Y", visitor)?;
253        self.size.x.visit("W", visitor)?;
254        self.size.y.visit("H", visitor)?;
255
256        visitor.leave_region()
257    }
258}
259
260#[derive(Copy, Clone)]
261pub enum PlaneClass {
262    XY,
263    YZ,
264    XZ,
265}
266
267#[inline]
268#[allow(clippy::useless_let_if_seq)]
269pub fn classify_plane(normal: Vector3<f32>) -> PlaneClass {
270    let mut longest = 0.0f32;
271    let mut class = PlaneClass::XY;
272
273    if normal.x.abs() > longest {
274        longest = normal.x.abs();
275        class = PlaneClass::YZ;
276    }
277
278    if normal.y.abs() > longest {
279        longest = normal.y.abs();
280        class = PlaneClass::XZ;
281    }
282
283    if normal.z.abs() > longest {
284        class = PlaneClass::XY;
285    }
286
287    class
288}
289
290#[inline]
291pub fn get_polygon_normal(polygon: &[Vector3<f32>]) -> Result<Vector3<f32>, &'static str> {
292    let mut normal = Vector3::default();
293
294    for (i, current) in polygon.iter().enumerate() {
295        let next = polygon[(i + 1) % polygon.len()];
296        normal.x += (current.y - next.y) * (current.z + next.z);
297        normal.y += (current.z - next.z) * (current.x + next.x);
298        normal.z += (current.x - next.x) * (current.y + next.y);
299    }
300
301    normal
302        .try_normalize(f32::EPSILON)
303        .ok_or("Unable to get normal of degenerated polygon!")
304}
305
306#[inline]
307pub fn get_signed_triangle_area(v1: Vector2<f32>, v2: Vector2<f32>, v3: Vector2<f32>) -> f32 {
308    0.5 * (v1.x * (v3.y - v2.y) + v2.x * (v1.y - v3.y) + v3.x * (v2.y - v1.y))
309}
310
311#[inline]
312pub fn vec3_to_vec2_by_plane(
313    plane_class: PlaneClass,
314    normal: Vector3<f32>,
315    point: Vector3<f32>,
316) -> Vector2<f32> {
317    match plane_class {
318        PlaneClass::XY => {
319            if normal.z < 0.0 {
320                Vector2::new(point.y, point.x)
321            } else {
322                Vector2::new(point.x, point.y)
323            }
324        }
325        PlaneClass::XZ => {
326            if normal.y < 0.0 {
327                Vector2::new(point.x, point.z)
328            } else {
329                Vector2::new(point.z, point.x)
330            }
331        }
332        PlaneClass::YZ => {
333            if normal.x < 0.0 {
334                Vector2::new(point.z, point.y)
335            } else {
336                Vector2::new(point.y, point.z)
337            }
338        }
339    }
340}
341
342#[inline]
343pub fn is_point_inside_2d_triangle(
344    point: Vector2<f32>,
345    pt_a: Vector2<f32>,
346    pt_b: Vector2<f32>,
347    pt_c: Vector2<f32>,
348) -> bool {
349    let ba = pt_b - pt_a;
350    let ca = pt_c - pt_a;
351
352    let vp = point - pt_a;
353
354    let ba_dot_ba = ba.dot(&ba);
355    let ca_dot_ba = ca.dot(&ba);
356    let ca_dot_ca = ca.dot(&ca);
357
358    let dot_02 = ca.dot(&vp);
359    let dot_12 = ba.dot(&vp);
360
361    let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2));
362
363    // calculate barycentric coordinates
364    let u = (ba_dot_ba * dot_02 - ca_dot_ba * dot_12) * inv_denom;
365    let v = (ca_dot_ca * dot_12 - ca_dot_ba * dot_02) * inv_denom;
366
367    (u >= 0.0) && (v >= 0.0) && (u + v < 1.0)
368}
369
370#[inline]
371pub fn wrap_angle(angle: f32) -> f32 {
372    let two_pi = 2.0 * std::f32::consts::PI;
373
374    if angle > 0.0 {
375        angle % two_pi
376    } else {
377        (angle + two_pi) % two_pi
378    }
379}
380
381#[inline]
382pub fn clampf(v: f32, min: f32, max: f32) -> f32 {
383    if v < min {
384        min
385    } else if v > max {
386        max
387    } else {
388        v
389    }
390}
391
392/// There are two versions of remainder, the standard `%` operator which does `x - (x/y).trunc()*y` and IEEE remainder which does `x - (x/y).round()*y`.
393#[inline]
394pub fn ieee_remainder(x: f32, y: f32) -> f32 {
395    x - (x / y).round() * y
396}
397
398#[inline]
399pub fn round_to_step(x: f32, step: f32) -> f32 {
400    x - ieee_remainder(x, step)
401}
402
403#[inline]
404pub fn wrapf(mut n: f32, mut min_limit: f32, mut max_limit: f32) -> f32 {
405    if n >= min_limit && n <= max_limit {
406        return n;
407    }
408
409    if max_limit == 0.0 && min_limit == 0.0 {
410        return 0.0;
411    }
412
413    max_limit -= min_limit;
414
415    let offset = min_limit;
416    min_limit = 0.0;
417    n -= offset;
418
419    let num_of_max = (n / max_limit).abs().floor();
420
421    if n >= max_limit {
422        n -= num_of_max * max_limit;
423    } else if n < min_limit {
424        n += (num_of_max + 1.0) * max_limit;
425    }
426
427    n + offset
428}
429
430#[inline(always)]
431pub fn lerpf(a: f32, b: f32, t: f32) -> f32 {
432    a + (b - a) * t
433}
434
435// https://en.wikipedia.org/wiki/Cubic_Hermite_spline
436#[inline]
437pub fn cubicf(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 {
438    let t2 = t * t;
439    let t3 = t2 * t;
440    let scale = (p1 - p0).abs();
441
442    (2.0 * t3 - 3.0 * t2 + 1.0) * p0
443        + (t3 - 2.0 * t2 + t) * m0 * scale
444        + (-2.0 * t3 + 3.0 * t2) * p1
445        + (t3 - t2) * m1 * scale
446}
447
448#[inline]
449pub fn cubicf_derivative(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 {
450    let t2 = t * t;
451    let scale = (p1 - p0).abs();
452
453    (6.0 * t2 - 6.0 * t) * p0
454        + (3.0 * t2 - 4.0 * t + 1.0) * m0 * scale
455        + (6.0 * t - 6.0 * t2) * p1
456        + (3.0 * t2 - 2.0 * t) * m1 * scale
457}
458
459#[inline]
460pub fn inf_sup_cubicf(p0: f32, p1: f32, m0: f32, m1: f32) -> (f32, f32) {
461    // Find two `t`s where derivative of cubicf is zero - these will be
462    // extreme points of the spline. Then get the values at those `t`s
463    let d = -(9.0 * p0 * p0 + 6.0 * p0 * (-3.0 * p1 + m1 + m0) + 9.0 * p1 * p1
464        - 6.0 * p1 * (m1 + m0)
465        + m1 * m1
466        + m1 * m0
467        + m0 * m0)
468        .sqrt();
469    let k = 3.0 * (2.0 * p0 - 2.0 * p1 + m1 + m0);
470    let v = 3.0 * p0 - 3.0 * p1 + m1 + 2.0 * m0;
471    let t0 = (-d + v) / k;
472    let t1 = (d + v) / k;
473    (cubicf(p0, p1, t0, m0, m1), cubicf(p0, p1, t1, m0, m1))
474}
475
476#[inline]
477pub fn get_farthest_point(points: &[Vector3<f32>], dir: Vector3<f32>) -> Vector3<f32> {
478    let mut n_farthest = 0;
479    let mut max_dot = -f32::MAX;
480    for (i, point) in points.iter().enumerate() {
481        let dot = dir.dot(point);
482        if dot > max_dot {
483            n_farthest = i;
484            max_dot = dot
485        }
486    }
487    points[n_farthest]
488}
489
490#[inline]
491pub fn get_barycentric_coords(
492    p: &Vector3<f32>,
493    a: &Vector3<f32>,
494    b: &Vector3<f32>,
495    c: &Vector3<f32>,
496) -> (f32, f32, f32) {
497    let v0 = *b - *a;
498    let v1 = *c - *a;
499    let v2 = *p - *a;
500
501    let d00 = v0.dot(&v0);
502    let d01 = v0.dot(&v1);
503    let d11 = v1.dot(&v1);
504    let d20 = v2.dot(&v0);
505    let d21 = v2.dot(&v1);
506    let denom = d00 * d11 - d01.powi(2);
507
508    let v = (d11 * d20 - d01 * d21) / denom;
509    let w = (d00 * d21 - d01 * d20) / denom;
510    let u = 1.0 - v - w;
511
512    (u, v, w)
513}
514
515#[inline]
516pub fn get_barycentric_coords_2d(
517    p: Vector2<f32>,
518    a: Vector2<f32>,
519    b: Vector2<f32>,
520    c: Vector2<f32>,
521) -> (f32, f32, f32) {
522    let v0 = b - a;
523    let v1 = c - a;
524    let v2 = p - a;
525
526    let d00 = v0.dot(&v0);
527    let d01 = v0.dot(&v1);
528    let d11 = v1.dot(&v1);
529    let d20 = v2.dot(&v0);
530    let d21 = v2.dot(&v1);
531    let inv_denom = 1.0 / (d00 * d11 - d01.powi(2));
532
533    let v = (d11 * d20 - d01 * d21) * inv_denom;
534    let w = (d00 * d21 - d01 * d20) * inv_denom;
535    let u = 1.0 - v - w;
536
537    (u, v, w)
538}
539
540#[inline]
541pub fn barycentric_to_world(
542    bary: (f32, f32, f32),
543    pa: Vector3<f32>,
544    pb: Vector3<f32>,
545    pc: Vector3<f32>,
546) -> Vector3<f32> {
547    pa.scale(bary.0) + pb.scale(bary.1) + pc.scale(bary.2)
548}
549
550#[inline]
551pub fn barycentric_is_inside(bary: (f32, f32, f32)) -> bool {
552    (bary.0 >= 0.0) && (bary.1 >= 0.0) && (bary.0 + bary.1 < 1.0)
553}
554
555#[inline]
556pub fn is_point_inside_triangle(p: &Vector3<f32>, vertices: &[Vector3<f32>; 3]) -> bool {
557    let ba = vertices[1] - vertices[0];
558    let ca = vertices[2] - vertices[0];
559    let vp = *p - vertices[0];
560
561    let ba_dot_ba = ba.dot(&ba);
562    let ca_dot_ba = ca.dot(&ba);
563    let ca_dot_ca = ca.dot(&ca);
564
565    let dot02 = ca.dot(&vp);
566    let dot12 = ba.dot(&vp);
567
568    let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2));
569
570    // Calculate barycentric coordinates
571    let u = (ba_dot_ba * dot02 - ca_dot_ba * dot12) * inv_denom;
572    let v = (ca_dot_ca * dot12 - ca_dot_ba * dot02) * inv_denom;
573
574    (u >= 0.0) && (v >= 0.0) && (u + v < 1.0)
575}
576
577#[inline]
578pub fn triangle_area(a: Vector3<f32>, b: Vector3<f32>, c: Vector3<f32>) -> f32 {
579    (b - a).cross(&(c - a)).norm() * 0.5
580}
581
582#[inline]
583pub fn solve_quadratic(a: f32, b: f32, c: f32) -> Option<[f32; 2]> {
584    let discriminant = b * b - 4.0 * a * c;
585    if discriminant < 0.0 {
586        // No real roots
587        None
588    } else {
589        // Dont care if quadratic equation has only one root (discriminant == 0), this is edge-case
590        // which requires additional branching instructions which is not good for branch-predictor in CPU.
591        let _2a = 2.0 * a;
592        let discr_root = discriminant.sqrt();
593        let r1 = (-b + discr_root) / _2a;
594        let r2 = (-b - discr_root) / _2a;
595        Some([r1, r2])
596    }
597}
598
599#[inline]
600pub fn spherical_to_cartesian(azimuth: f32, elevation: f32, radius: f32) -> Vector3<f32> {
601    let x = radius * elevation.sin() * azimuth.sin();
602    let y = radius * elevation.cos();
603    let z = -radius * elevation.sin() * azimuth.cos();
604    Vector3::new(x, y, z)
605}
606
607#[inline]
608pub fn ray_rect_intersection(
609    rect: Rect<f32>,
610    origin: Vector2<f32>,
611    dir: Vector2<f32>,
612) -> Option<IntersectionResult> {
613    let min = rect.left_top_corner();
614    let max = rect.right_bottom_corner();
615
616    let (mut tmin, mut tmax) = if dir.x >= 0.0 {
617        ((min.x - origin.x) / dir.x, (max.x - origin.x) / dir.x)
618    } else {
619        ((max.x - origin.x) / dir.x, (min.x - origin.x) / dir.x)
620    };
621
622    let (tymin, tymax) = if dir.y >= 0.0 {
623        ((min.y - origin.y) / dir.y, (max.y - origin.y) / dir.y)
624    } else {
625        ((max.y - origin.y) / dir.y, (min.y - origin.y) / dir.y)
626    };
627
628    if tmin > tymax || tymin > tmax {
629        return None;
630    }
631    if tymin > tmin {
632        tmin = tymin;
633    }
634    if tymax < tmax {
635        tmax = tymax;
636    }
637    if tmin <= 1.0 && tmax >= 0.0 {
638        Some(IntersectionResult {
639            min: tmin,
640            max: tmax,
641        })
642    } else {
643        None
644    }
645}
646
647#[derive(Clone, Debug, Default)]
648#[repr(C)]
649pub struct TriangleEdge {
650    a: u32,
651    b: u32,
652}
653
654impl PartialEq for TriangleEdge {
655    fn eq(&self, other: &Self) -> bool {
656        self.a == other.a && self.b == other.b || self.a == other.b && self.b == other.a
657    }
658}
659
660impl Eq for TriangleEdge {}
661
662#[derive(Clone, PartialEq, Eq, Debug, Default, Hash)]
663#[repr(C)]
664pub struct TriangleDefinition(pub [u32; 3]);
665
666impl TriangleDefinition {
667    #[inline]
668    pub fn indices(&self) -> &[u32] {
669        self.as_ref()
670    }
671
672    #[inline]
673    pub fn indices_mut(&mut self) -> &mut [u32] {
674        self.as_mut()
675    }
676
677    #[inline]
678    pub fn edges(&self) -> [TriangleEdge; 3] {
679        [
680            TriangleEdge {
681                a: self.0[0],
682                b: self.0[1],
683            },
684            TriangleEdge {
685                a: self.0[1],
686                b: self.0[2],
687            },
688            TriangleEdge {
689                a: self.0[2],
690                b: self.0[0],
691            },
692        ]
693    }
694}
695
696impl Visit for TriangleDefinition {
697    fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
698        visitor.enter_region(name)?;
699
700        self.0[0].visit("A", visitor)?;
701        self.0[1].visit("B", visitor)?;
702        self.0[2].visit("C", visitor)?;
703
704        visitor.leave_region()
705    }
706}
707
708impl Index<usize> for TriangleDefinition {
709    type Output = u32;
710
711    #[inline]
712    fn index(&self, index: usize) -> &Self::Output {
713        &self.0[index]
714    }
715}
716
717impl IndexMut<usize> for TriangleDefinition {
718    #[inline]
719    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
720        &mut self.0[index]
721    }
722}
723
724pub trait PositionProvider: Sized {
725    fn position(&self) -> Vector3<f32>;
726}
727
728impl PositionProvider for Vector3<f32> {
729    #[inline]
730    fn position(&self) -> Vector3<f32> {
731        *self
732    }
733}
734
735impl AsRef<[u32]> for TriangleDefinition {
736    #[inline]
737    fn as_ref(&self) -> &[u32] {
738        &self.0
739    }
740}
741
742impl AsMut<[u32]> for TriangleDefinition {
743    #[inline]
744    fn as_mut(&mut self) -> &mut [u32] {
745        &mut self.0
746    }
747}
748
749/// Tries to find a point closest to given point.
750///
751/// # Notes
752///
753/// O(n) complexity.
754#[inline]
755pub fn get_closest_point<P: PositionProvider>(points: &[P], point: Vector3<f32>) -> Option<usize> {
756    let mut closest_sqr_distance = f32::MAX;
757    let mut closest_index = None;
758    for (i, vertex) in points.iter().enumerate() {
759        let sqr_distance = (vertex.position() - point).norm_squared();
760        if sqr_distance < closest_sqr_distance {
761            closest_sqr_distance = sqr_distance;
762            closest_index = Some(i);
763        }
764    }
765    closest_index
766}
767
768#[inline]
769pub fn get_closest_point_triangles<P: PositionProvider>(
770    points: &[P],
771    triangles: &[TriangleDefinition],
772    triangle_indices: &[u32],
773    point: Vector3<f32>,
774) -> Option<usize> {
775    let mut closest_sqr_distance = f32::MAX;
776    let mut closest_index = None;
777    for triangle_index in triangle_indices {
778        let triangle = triangles.get(*triangle_index as usize).unwrap();
779        for point_index in triangle.0.iter() {
780            let vertex = points.get(*point_index as usize).unwrap();
781            let sqr_distance = (vertex.position() - point).norm_squared();
782            if sqr_distance < closest_sqr_distance {
783                closest_sqr_distance = sqr_distance;
784                closest_index = Some(*point_index as usize);
785            }
786        }
787    }
788    closest_index
789}
790
791#[inline]
792pub fn get_closest_point_triangle_set<P: PositionProvider>(
793    points: &[P],
794    triangles: &[TriangleDefinition],
795    point: Vector3<f32>,
796) -> Option<usize> {
797    let mut closest_sqr_distance = f32::MAX;
798    let mut closest_index = None;
799    for triangle in triangles {
800        for point_index in triangle.0.iter() {
801            let vertex = points.get(*point_index as usize).unwrap();
802            let sqr_distance = (vertex.position() - point).norm_squared();
803            if sqr_distance < closest_sqr_distance {
804                closest_sqr_distance = sqr_distance;
805                closest_index = Some(*point_index as usize);
806            }
807        }
808    }
809    closest_index
810}
811
812#[derive(Debug, PartialEq)]
813pub struct SmoothAngle {
814    /// Current angle in radians.
815    pub angle: f32,
816
817    /// Target angle in radians.
818    pub target: f32,
819
820    /// Turn speed in radians per second (rad/s)
821    pub speed: f32,
822}
823
824impl SmoothAngle {
825    #[inline]
826    pub fn set_target(&mut self, angle: f32) -> &mut Self {
827        self.target = angle;
828        self
829    }
830
831    #[inline]
832    pub fn update(&mut self, dt: f32) -> &mut Self {
833        self.target = wrap_angle(self.target);
834        self.angle = wrap_angle(self.angle);
835        if !self.at_target() {
836            let delta = self.speed * dt;
837            if self.distance().abs() > delta {
838                self.angle += self.turn_direction() * delta;
839            } else {
840                self.angle = self.target;
841            }
842        }
843        self
844    }
845
846    #[inline]
847    pub fn set_speed(&mut self, speed: f32) -> &mut Self {
848        self.speed = speed;
849        self
850    }
851
852    #[inline]
853    pub fn set_angle(&mut self, angle: f32) -> &mut Self {
854        self.angle = angle;
855        self
856    }
857
858    #[inline]
859    pub fn angle(&self) -> f32 {
860        self.angle
861    }
862
863    #[inline]
864    pub fn at_target(&self) -> bool {
865        (self.target - self.angle).abs() <= f32::EPSILON
866    }
867
868    #[inline]
869    pub fn distance(&self) -> f32 {
870        let diff = (self.target - self.angle + std::f32::consts::PI) % std::f32::consts::TAU
871            - std::f32::consts::PI;
872        if diff < -std::f32::consts::PI {
873            diff + std::f32::consts::TAU
874        } else {
875            diff
876        }
877    }
878
879    #[inline]
880    fn turn_direction(&self) -> f32 {
881        let distance = self.distance();
882
883        if distance < 0.0 {
884            if distance < -std::f32::consts::PI {
885                1.0
886            } else {
887                -1.0
888            }
889        } else if distance > std::f32::consts::PI {
890            -1.0
891        } else {
892            1.0
893        }
894    }
895}
896
897impl Default for SmoothAngle {
898    fn default() -> Self {
899        Self {
900            angle: 0.0,
901            target: 0.0,
902            speed: 1.0,
903        }
904    }
905}
906
907impl Visit for SmoothAngle {
908    fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult {
909        visitor.enter_region(name)?;
910
911        self.angle.visit("Angle", visitor)?;
912        self.target.visit("Target", visitor)?;
913        self.speed.visit("Speed", visitor)?;
914
915        visitor.leave_region()
916    }
917}
918
919#[derive(Copy, Clone, Hash, PartialOrd, PartialEq, Ord, Eq)]
920pub enum RotationOrder {
921    XYZ,
922    XZY,
923    YZX,
924    YXZ,
925    ZXY,
926    ZYX,
927}
928
929#[inline]
930pub fn quat_from_euler<T: SimdRealField + RealField + Copy + Clone>(
931    euler_radians: Vector3<T>,
932    order: RotationOrder,
933) -> UnitQuaternion<T> {
934    let qx = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), euler_radians.x);
935    let qy = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), euler_radians.y);
936    let qz = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), euler_radians.z);
937    match order {
938        RotationOrder::XYZ => qz * qy * qx,
939        RotationOrder::XZY => qy * qz * qx,
940        RotationOrder::YZX => qx * qz * qy,
941        RotationOrder::YXZ => qz * qx * qy,
942        RotationOrder::ZXY => qy * qx * qz,
943        RotationOrder::ZYX => qx * qy * qz,
944    }
945}
946
947pub trait Matrix4Ext<T: Scalar> {
948    fn side(&self) -> Vector3<T>;
949    fn up(&self) -> Vector3<T>;
950    fn look(&self) -> Vector3<T>;
951    fn position(&self) -> Vector3<T>;
952    fn basis(&self) -> Matrix3<T>;
953}
954
955impl<T: Scalar + Default + Copy + Clone> Matrix4Ext<T> for Matrix4<T> {
956    fn side(&self) -> Vector3<T> {
957        Vector3::new(self[0], self[1], self[2])
958    }
959
960    fn up(&self) -> Vector3<T> {
961        Vector3::new(self[4], self[5], self[6])
962    }
963
964    fn look(&self) -> Vector3<T> {
965        Vector3::new(self[8], self[9], self[10])
966    }
967
968    fn position(&self) -> Vector3<T> {
969        Vector3::new(self[12], self[13], self[14])
970    }
971
972    fn basis(&self) -> Matrix3<T> {
973        self.fixed_resize::<3, 3>(T::default())
974    }
975}
976
977pub trait Matrix3Ext<T: Scalar> {
978    fn side(&self) -> Vector3<T>;
979    fn up(&self) -> Vector3<T>;
980    fn look(&self) -> Vector3<T>;
981}
982
983impl<T: Scalar + Copy + Clone> Matrix3Ext<T> for Matrix3<T> {
984    fn side(&self) -> Vector3<T> {
985        Vector3::new(self[0], self[1], self[2])
986    }
987
988    fn up(&self) -> Vector3<T> {
989        Vector3::new(self[3], self[4], self[5])
990    }
991
992    fn look(&self) -> Vector3<T> {
993        Vector3::new(self[6], self[7], self[8])
994    }
995}
996
997pub trait Vector3Ext {
998    fn follow(&mut self, other: &Self, fraction: f32);
999
1000    fn sqr_distance(&self, other: &Self) -> f32;
1001
1002    fn non_uniform_scale(&self, other: &Self) -> Self;
1003}
1004
1005impl Vector3Ext for Vector3<f32> {
1006    #[inline]
1007    fn follow(&mut self, other: &Self, fraction: f32) {
1008        self.x += (other.x - self.x) * fraction;
1009        self.y += (other.y - self.y) * fraction;
1010        self.z += (other.z - self.z) * fraction;
1011    }
1012
1013    #[inline]
1014    fn sqr_distance(&self, other: &Self) -> f32 {
1015        (self - other).norm_squared()
1016    }
1017
1018    #[inline]
1019    fn non_uniform_scale(&self, other: &Self) -> Self {
1020        Self::new(self.x * other.x, self.y * other.y, self.z * other.z)
1021    }
1022}
1023
1024pub trait Vector2Ext {
1025    fn follow(&mut self, other: &Self, fraction: f32);
1026
1027    fn per_component_min(&self, other: &Self) -> Self;
1028    fn per_component_max(&self, other: &Self) -> Self;
1029}
1030
1031impl Vector2Ext for Vector2<f32> {
1032    #[inline]
1033    fn follow(&mut self, other: &Self, fraction: f32) {
1034        self.x += (other.x - self.x) * fraction;
1035        self.y += (other.y - self.y) * fraction;
1036    }
1037
1038    #[inline]
1039    fn per_component_min(&self, other: &Self) -> Self {
1040        Self::new(self.x.min(other.x), self.y.min(other.y))
1041    }
1042
1043    #[inline]
1044    fn per_component_max(&self, other: &Self) -> Self {
1045        Self::new(self.x.max(other.x), self.y.max(other.y))
1046    }
1047}
1048
1049/// Returns rotation quaternion that represents rotation basis with Z axis aligned on `vec`.
1050/// This function handles singularities for you.
1051#[inline]
1052pub fn vector_to_quat(vec: Vector3<f32>) -> UnitQuaternion<f32> {
1053    let dot = vec.normalize().dot(&Vector3::y());
1054
1055    if dot.abs() > 1.0 - 10.0 * f32::EPSILON {
1056        // Handle singularity when vector is collinear with Y axis.
1057        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -dot.signum() * 90.0f32.to_radians())
1058    } else {
1059        UnitQuaternion::face_towards(&vec, &Vector3::y())
1060    }
1061}
1062
1063#[cfg(test)]
1064mod test {
1065    use crate::algebra::Vector2;
1066    use crate::math::Rect;
1067    use crate::math::SmoothAngle;
1068
1069    #[test]
1070    fn ray_rect_intersection() {
1071        let rect = Rect::new(0.0, 0.0, 10.0, 10.0);
1072
1073        // Edge-case: Horizontal ray.
1074        assert!(super::ray_rect_intersection(
1075            rect,
1076            Vector2::new(-1.0, 5.0),
1077            Vector2::new(1.0, 0.0)
1078        )
1079        .is_some());
1080
1081        // Edge-case: Vertical ray.
1082        assert!(super::ray_rect_intersection(
1083            rect,
1084            Vector2::new(5.0, -1.0),
1085            Vector2::new(0.0, 1.0)
1086        )
1087        .is_some());
1088    }
1089
1090    #[test]
1091    fn smooth_angle() {
1092        let mut angle = SmoothAngle {
1093            angle: 290.0f32.to_radians(),
1094            target: 90.0f32.to_radians(),
1095            speed: 100.0f32.to_radians(),
1096        };
1097
1098        while !angle.at_target() {
1099            println!("{}", angle.update(1.0).angle().to_degrees());
1100        }
1101    }
1102}
rg3d_core/math/mod.rs

rg3d_core/math/
mod.rs
