use na;
#[cfg(not(feature = "std"))]
use na::ComplexField; use crate::math::Real;
#[cfg(feature = "dim2")]
use crate::query;
use crate::query::gjk::{self, CSOPoint, VoronoiSimplex};
use crate::query::{Ray, RayCast, RayIntersection};
#[cfg(all(feature = "std", feature = "dim2"))]
use crate::shape::ConvexPolygon;
#[cfg(all(feature = "std", feature = "dim3"))]
use crate::shape::ConvexPolyhedron;
use crate::shape::{Capsule, FeatureId, Segment, SupportMap};
#[cfg(feature = "dim3")]
use crate::shape::{Cone, Cylinder};
use num::Zero;
pub fn local_ray_intersection_with_support_map_with_params<G: ?Sized + SupportMap>(
shape: &G,
simplex: &mut VoronoiSimplex,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
let supp = shape.local_support_point(&-ray.dir);
simplex.reset(CSOPoint::single_point(supp - ray.origin.coords));
let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact);
if !solid {
inter.and_then(|(time_of_impact, normal)| {
if time_of_impact.is_zero() {
let ndir = ray.dir.normalize();
let supp = shape.local_support_point(&ndir);
let eps = na::convert::<f64, Real>(0.001f64);
let shift = (supp - ray.origin).dot(&ndir) + eps;
let new_ray = Ray::new(ray.origin + ndir * shift, -ray.dir);
simplex.reset(CSOPoint::single_point(supp - new_ray.origin.coords));
gjk::cast_local_ray(shape, simplex, &new_ray, shift + eps).and_then(
|(time_of_impact, outward_normal)| {
let time_of_impact = shift - time_of_impact;
if time_of_impact <= max_time_of_impact {
Some(RayIntersection::new(
time_of_impact,
-outward_normal,
FeatureId::Unknown,
))
} else {
None
}
},
)
} else {
Some(RayIntersection::new(
time_of_impact,
normal,
FeatureId::Unknown,
))
}
})
} else {
inter.map(|(time_of_impact, normal)| {
RayIntersection::new(time_of_impact, normal, FeatureId::Unknown)
})
}
}
#[cfg(feature = "dim3")]
impl RayCast for Cylinder {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
#[cfg(feature = "dim3")]
impl RayCast for Cone {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
impl RayCast for Capsule {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
#[cfg(feature = "dim3")]
#[cfg(feature = "std")]
impl RayCast for ConvexPolyhedron {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
#[cfg(feature = "dim2")]
#[cfg(feature = "std")]
impl RayCast for ConvexPolygon {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
#[allow(unused_variables)]
impl RayCast for Segment {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_time_of_impact: Real,
solid: bool,
) -> Option<RayIntersection> {
#[cfg(feature = "dim2")]
{
use crate::math::Vector;
let seg_dir = self.scaled_direction();
let (s, t, parallel) = query::details::closest_points_line_line_parameters_eps(
&ray.origin,
&ray.dir,
&self.a,
&seg_dir,
crate::math::DEFAULT_EPSILON,
);
if parallel {
let dpos = self.a - ray.origin;
let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros);
if dpos.dot(&normal).abs() < crate::math::DEFAULT_EPSILON {
let dist1 = dpos.dot(&ray.dir);
let dist2 = dist1 + seg_dir.dot(&ray.dir);
match (dist1 >= 0.0, dist2 >= 0.0) {
(true, true) => {
let time_of_impact = dist1.min(dist2) / ray.dir.norm_squared();
if time_of_impact > max_time_of_impact {
None
} else if dist1 <= dist2 {
Some(RayIntersection::new(
time_of_impact,
normal,
FeatureId::Vertex(0),
))
} else {
Some(RayIntersection::new(
dist2 / ray.dir.norm_squared(),
normal,
FeatureId::Vertex(1),
))
}
}
(true, false) | (false, true) => {
Some(RayIntersection::new(0.0, normal, FeatureId::Face(0)))
}
(false, false) => {
None
}
}
} else {
None
}
} else if s >= 0.0 && s <= max_time_of_impact && t >= 0.0 && t <= 1.0 {
let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros);
if normal.dot(&ray.dir) > 0.0 {
Some(RayIntersection::new(s, -normal, FeatureId::Face(1)))
} else {
Some(RayIntersection::new(s, normal, FeatureId::Face(0)))
}
} else {
None
}
}
#[cfg(feature = "dim3")]
{
local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_time_of_impact,
solid,
)
}
}
}