use crate::math::{Isometry, Real, Vector};
use crate::query::details::ShapeCastOptions;
use crate::query::{Ray, RayCast, ShapeCastHit, ShapeCastStatus};
use crate::shape::{HalfSpace, RoundShapeRef, SupportMap};
pub fn cast_shapes_halfspace_support_map<G: ?Sized + SupportMap>(
pos12: &Isometry<Real>,
vel12: &Vector<Real>,
halfspace: &HalfSpace,
other: &G,
options: ShapeCastOptions,
) -> Option<ShapeCastHit> {
if !options.stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 {
return None;
}
let support_point = if options.target_distance > 0.0 {
let round_other = RoundShapeRef {
inner_shape: other,
border_radius: options.target_distance,
};
round_other.support_point(pos12, &-halfspace.normal)
} else {
other.support_point(pos12, &-halfspace.normal)
};
let closest_point = support_point;
let ray = Ray::new(closest_point, *vel12);
if let Some(time_of_impact) = halfspace.cast_local_ray(&ray, options.max_time_of_impact, true) {
if time_of_impact > options.max_time_of_impact {
return None;
}
let witness2 = support_point + *halfspace.normal * options.target_distance;
let mut witness1 = ray.point_at(time_of_impact);
witness1 -= *halfspace.normal * witness1.coords.dot(&halfspace.normal);
let status = if support_point.coords.dot(&halfspace.normal) < 0.0 {
ShapeCastStatus::PenetratingOrWithinTargetDist
} else {
ShapeCastStatus::Converged
};
Some(ShapeCastHit {
time_of_impact,
normal1: halfspace.normal,
normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal),
witness1,
witness2: pos12.inverse_transform_point(&witness2),
status,
})
} else {
None
}
}
pub fn cast_shapes_support_map_halfspace<G: ?Sized + SupportMap>(
pos12: &Isometry<Real>,
vel12: &Vector<Real>,
other: &G,
halfspace: &HalfSpace,
options: ShapeCastOptions,
) -> Option<ShapeCastHit> {
cast_shapes_halfspace_support_map(
&pos12.inverse(),
&-pos12.inverse_transform_vector(vel12),
halfspace,
other,
options,
)
.map(|hit| hit.swapped())
}