#![allow(unused_parens)] use crate::bounding_volume::SimdAabb;
use crate::math::{Point, Real, SimdReal, SIMD_WIDTH};
use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor};
use crate::query::visitors::CompositePointContainmentTest;
use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
use crate::shape::{
FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedSimdCompositeShape,
};
use na;
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
use crate::shape::{Compound, Polyline};
impl PointQuery for Polyline {
#[inline]
fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
self.project_local_point_and_get_location(point, solid).0
}
#[inline]
fn project_local_point_and_get_feature(
&self,
point: &Point<Real>,
) -> (PointProjection, FeatureId) {
let mut visitor =
PointCompositeShapeProjWithFeatureBestFirstVisitor::new(self, point, false);
let (proj, (id, feature)) = self.qbvh().traverse_best_first(&mut visitor).unwrap().1;
let polyline_feature = self.segment_feature_to_polyline_feature(id, feature);
(proj, polyline_feature)
}
#[inline]
fn contains_local_point(&self, point: &Point<Real>) -> bool {
let mut visitor = CompositePointContainmentTest::new(self, point);
let _ = self.qbvh().traverse_depth_first(&mut visitor);
visitor.found
}
}
impl PointQuery for TriMesh {
#[inline]
fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
self.project_local_point_and_get_location(point, solid).0
}
#[inline]
fn project_local_point_and_get_feature(
&self,
point: &Point<Real>,
) -> (PointProjection, FeatureId) {
#[cfg(feature = "dim3")]
if self.pseudo_normals().is_some() {
let (proj, (id, _feature)) = self.project_local_point_and_get_location(point, false);
let feature_id = FeatureId::Face(id);
return (proj, feature_id);
}
let solid = cfg!(feature = "dim2");
let mut visitor =
PointCompositeShapeProjWithFeatureBestFirstVisitor::new(self, point, solid);
let (proj, (id, _feature)) = self.qbvh().traverse_best_first(&mut visitor).unwrap().1;
let feature_id = FeatureId::Face(id);
(proj, feature_id)
}
#[inline]
fn contains_local_point(&self, point: &Point<Real>) -> bool {
#[cfg(feature = "dim3")]
if self.pseudo_normals.is_some() {
return self
.project_local_point_and_get_location(point, true)
.0
.is_inside;
}
let mut visitor = CompositePointContainmentTest::new(self, point);
let _ = self.qbvh().traverse_depth_first(&mut visitor);
visitor.found
}
fn project_local_point_with_max_dist(
&self,
pt: &Point<Real>,
solid: bool,
max_dist: Real,
) -> Option<PointProjection> {
self.project_local_point_and_get_location_with_max_dist(pt, solid, max_dist)
.map(|proj| proj.0)
}
}
impl PointQuery for Compound {
#[inline]
fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
let mut visitor = PointCompositeShapeProjBestFirstVisitor::new(self, point, solid);
self.qbvh().traverse_best_first(&mut visitor).unwrap().1 .0
}
#[inline]
fn project_local_point_and_get_feature(
&self,
point: &Point<Real>,
) -> (PointProjection, FeatureId) {
(self.project_local_point(point, false), FeatureId::Unknown)
}
#[inline]
fn contains_local_point(&self, point: &Point<Real>) -> bool {
let mut visitor = CompositePointContainmentTest::new(self, point);
let _ = self.qbvh().traverse_depth_first(&mut visitor);
visitor.found
}
}
impl PointQueryWithLocation for Polyline {
type Location = (u32, SegmentPointLocation);
#[inline]
fn project_local_point_and_get_location(
&self,
point: &Point<Real>,
solid: bool,
) -> (PointProjection, Self::Location) {
let mut visitor =
PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid);
self.qbvh().traverse_best_first(&mut visitor).unwrap().1
}
}
impl PointQueryWithLocation for TriMesh {
type Location = (u32, TrianglePointLocation);
#[inline]
#[allow(unused_mut)] fn project_local_point_and_get_location(
&self,
point: &Point<Real>,
solid: bool,
) -> (PointProjection, Self::Location) {
self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX)
.unwrap()
}
fn project_local_point_and_get_location_with_max_dist(
&self,
point: &Point<Real>,
solid: bool,
max_dist: Real,
) -> Option<(PointProjection, Self::Location)> {
let mut visitor =
PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid);
#[allow(unused_mut)] if let Some((_, (mut proj, (part_id, location)))) =
self.qbvh()
.traverse_best_first_node(&mut visitor, 0, max_dist)
{
#[cfg(feature = "dim3")]
if let Some(pseudo_normals) = self.pseudo_normals() {
let pseudo_normal = match location {
TrianglePointLocation::OnFace(..) | TrianglePointLocation::OnSolid => {
Some(self.triangle(part_id).scaled_normal())
}
TrianglePointLocation::OnEdge(i, _) => pseudo_normals
.edges_pseudo_normal
.get(part_id as usize)
.map(|pn| pn[i as usize]),
TrianglePointLocation::OnVertex(i) => {
let idx = self.indices()[part_id as usize];
pseudo_normals
.vertices_pseudo_normal
.get(idx[i as usize] as usize)
.copied()
}
};
if let Some(pseudo_normal) = pseudo_normal {
let dpt = point - proj.point;
proj.is_inside = dpt.dot(&pseudo_normal) <= 0.0;
}
}
Some((proj, (part_id, location)))
} else {
None
}
}
}
macro_rules! gen_visitor(
($Visitor: ident, $project_local_point: ident, $project_point: ident $(, $Location: ty, $extra_info: ident)* $(| $args: ident)* $(where $PartShapeBound: ident)*) => {
pub struct $Visitor<'a, S> {
shape: &'a S,
point: &'a Point<Real>,
simd_point: Point<SimdReal>,
solid: bool,
}
impl<'a, S> $Visitor<'a, S> {
pub fn new(shape: &'a S, point: &'a Point<Real>, solid: bool) -> Self {
Self {
shape,
point,
simd_point: Point::splat(*point),
solid,
}
}
}
impl<'a, S> SimdBestFirstVisitor<S::PartId, SimdAabb> for $Visitor<'a, S>
where S: TypedSimdCompositeShape
$(, $Location: Copy)*
$(, S::PartShape: $PartShapeBound)* {
type Result = (PointProjection, (S::PartId $(, $Location)*));
#[inline]
fn visit(
&mut self,
best: Real,
aabb: &SimdAabb,
data: Option<[Option<&S::PartId>; SIMD_WIDTH]>,
) -> SimdBestFirstVisitStatus<Self::Result> {
let dist = aabb.distance_to_local_point(&self.simd_point);
let mask = dist.simd_lt(SimdReal::splat(best));
if let Some(data) = data {
let mut weights = [0.0; SIMD_WIDTH];
let mut results = [None; SIMD_WIDTH];
let bitmask = mask.bitmask();
for ii in 0..SIMD_WIDTH {
if (bitmask & (1 << ii)) != 0 && data[ii].is_some() {
let mut is_inside = false;
let subshape_id = *data[ii].unwrap();
self.shape.map_typed_part_at(subshape_id, |part_pos, part_shape, _| {
let (proj $(, $extra_info)*) = if let Some(part_pos) = part_pos {
part_shape.$project_point(
part_pos,
self.point
$(, self.$args)*
)
} else {
part_shape.$project_local_point(
self.point
$(, self.$args)*
)
};
is_inside = proj.is_inside;
weights[ii] = na::distance(self.point, &proj.point);
results[ii] = Some((proj, (subshape_id $(, $extra_info)*)));
});
if self.solid && is_inside {
return SimdBestFirstVisitStatus::ExitEarly(results[ii]);
}
}
}
SimdBestFirstVisitStatus::MaybeContinue {
weights: SimdReal::from(weights),
mask,
results,
}
} else {
SimdBestFirstVisitStatus::MaybeContinue {
weights: dist,
mask,
results: [None; SIMD_WIDTH],
}
}
}
}
}
);
gen_visitor!(
PointCompositeShapeProjBestFirstVisitor,
project_local_point,
project_point | solid
);
gen_visitor!(
PointCompositeShapeProjWithLocationBestFirstVisitor,
project_local_point_and_get_location,
project_point_and_get_location,
<S::PartShape as PointQueryWithLocation>::Location,
extra_info | solid
where PointQueryWithLocation
where Copy
);
gen_visitor!(
PointCompositeShapeProjWithFeatureBestFirstVisitor,
project_local_point_and_get_feature,
project_point_and_get_feature,
FeatureId,
extra_info
);