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

parry3d/query/ray/
ray_triangle.rs
