parry2d_f64/query/ray/
ray_triangle.rs

1use crate::math::Real;
2#[cfg(feature = "dim2")]
3use crate::math::Vector;
4use crate::query::{Ray, RayCast, RayIntersection};
5use crate::shape::{FeatureId, Triangle};
6#[cfg(feature = "dim3")]
7use {crate::math::Point, na::Vector3};
8
9#[cfg(not(feature = "std"))]
10use na::ComplexField; // for .abs()
11
12impl RayCast for Triangle {
13    #[inline]
14    #[cfg(feature = "dim2")]
15    fn cast_local_ray_and_get_normal(
16        &self,
17        ray: &Ray,
18        max_time_of_impact: Real,
19        solid: bool,
20    ) -> Option<RayIntersection> {
21        let edges = self.edges();
22
23        if solid {
24            // Check if ray starts in triangle
25            let perp_sign1 = edges[0].scaled_direction().perp(&(ray.origin - edges[0].a)) > 0.0;
26            let perp_sign2 = edges[1].scaled_direction().perp(&(ray.origin - edges[1].a)) > 0.0;
27            let perp_sign3 = edges[2].scaled_direction().perp(&(ray.origin - edges[2].a)) > 0.0;
28
29            if perp_sign1 == perp_sign2 && perp_sign1 == perp_sign3 {
30                return Some(RayIntersection::new(0.0, Vector::y(), FeatureId::Face(0)));
31            }
32        }
33
34        let mut best = None;
35        let mut smallest_toi = Real::MAX;
36
37        for edge in &edges {
38            if let Some(inter) = edge.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)
39            {
40                if inter.time_of_impact < smallest_toi {
41                    smallest_toi = inter.time_of_impact;
42                    best = Some(inter);
43                }
44            }
45        }
46
47        best
48    }
49
50    #[inline]
51    #[cfg(feature = "dim3")]
52    fn cast_local_ray_and_get_normal(
53        &self,
54        ray: &Ray,
55        max_time_of_impact: Real,
56        _: bool,
57    ) -> Option<RayIntersection> {
58        let inter = local_ray_intersection_with_triangle(&self.a, &self.b, &self.c, ray)?.0;
59
60        if inter.time_of_impact <= max_time_of_impact {
61            Some(inter)
62        } else {
63            None
64        }
65    }
66}
67
68/// Computes the intersection between a triangle and a ray.
69///
70/// If an intersection is found, the time of impact, the normal and the barycentric coordinates of
71/// the intersection point are returned.
72#[cfg(feature = "dim3")]
73pub fn local_ray_intersection_with_triangle(
74    a: &Point<Real>,
75    b: &Point<Real>,
76    c: &Point<Real>,
77    ray: &Ray,
78) -> Option<(RayIntersection, Vector3<Real>)> {
79    let ab = *b - *a;
80    let ac = *c - *a;
81
82    // normal
83    let n = ab.cross(&ac);
84    let d = n.dot(&ray.dir);
85
86    // the normal and the ray direction are parallel
87    if d == 0.0 {
88        return None;
89    }
90
91    let ap = ray.origin - *a;
92    let t = ap.dot(&n);
93
94    // the ray does not intersect the halfspace defined by the triangle
95    if (t < 0.0 && d < 0.0) || (t > 0.0 && d > 0.0) {
96        return None;
97    }
98
99    let fid = if d < 0.0 { 0 } else { 1 };
100
101    let d = d.abs();
102
103    //
104    // intersection: compute barycentric coordinates
105    //
106    let e = -ray.dir.cross(&ap);
107
108    let mut v;
109    let mut w;
110    let time_of_impact;
111    let normal;
112
113    if t < 0.0 {
114        v = -ac.dot(&e);
115
116        if v < 0.0 || v > d {
117            return None;
118        }
119
120        w = ab.dot(&e);
121
122        if w < 0.0 || v + w > d {
123            return None;
124        }
125
126        let invd = 1.0 / d;
127        time_of_impact = -t * invd;
128        normal = -n.normalize();
129        v *= invd;
130        w *= invd;
131    } else {
132        v = ac.dot(&e);
133
134        if v < 0.0 || v > d {
135            return None;
136        }
137
138        w = -ab.dot(&e);
139
140        if w < 0.0 || v + w > d {
141            return None;
142        }
143
144        let invd = 1.0 / d;
145        time_of_impact = t * invd;
146        normal = n.normalize();
147        v *= invd;
148        w *= invd;
149    }
150
151    Some((
152        RayIntersection::new(time_of_impact, normal, FeatureId::Face(fid)),
153        Vector3::new(-v - w + 1.0, v, w),
154    ))
155}
parry2d_f64/query/ray/ray_triangle.rs

parry2d_f64/query/ray/
ray_triangle.rs
