use crate::SurfacePoint3;
use crate::na::Translation3;
use crate::sensors::SimulatedPointSensor;
use crate::{Iso3, Mesh3, Point3, PointCloud3, UnitVec3, Vector3};
use parry3d_f64::query::{Ray, RayCast};
use std::f64::consts::PI;
#[derive(Debug, Clone)]
pub struct LaserProfileGeom {
pub emitter_z: f64,
pub detector_y: f64,
pub detector_z: f64,
}
impl LaserProfileGeom {
pub fn new(emitter_z: f64, detector_y: f64, detector_z: f64) -> Self {
Self {
emitter_z,
detector_y,
detector_z,
}
}
}
#[derive(Debug, Clone)]
pub struct LaserProfile {
pub emitter_z: f64,
pub detector_y: f64,
pub detector_z: f64,
pub volume_width: f64,
pub volume_z_min: f64,
pub volume_z_max: f64,
pub resolution: usize,
pub angle_limit: Option<f64>,
}
impl LaserProfile {
#[allow(clippy::too_many_arguments)]
pub fn new(
emitter_z: f64,
detector_y: f64,
detector_z: f64,
volume_width: f64,
volume_z_min: f64,
volume_z_max: f64,
resolution: usize,
angle_limit: Option<f64>,
) -> Self {
Self {
emitter_z,
detector_y,
detector_z,
volume_width,
volume_z_min,
volume_z_max,
resolution,
angle_limit,
}
}
}
fn obstruction_limit(obstruction: Option<&Mesh3>, ray: &Ray, iso: &Iso3) -> f64 {
obstruction
.map(|ob| {
ob.tri_mesh()
.cast_ray(iso, ray, f64::MAX, false)
.unwrap_or(f64::MAX)
})
.unwrap_or(f64::MAX)
}
impl SimulatedPointSensor for LaserProfile {
fn get_points(
&self,
target: &Mesh3,
obstruction: Option<&Mesh3>,
iso: &Iso3,
) -> (PointCloud3, Option<Vec<f64>>) {
let limit = self.angle_limit.unwrap_or(PI / 2.0);
let mut points = Vec::new();
let mut normals = Vec::new();
let emitter = Point3::new(0.0, 0.0, self.emitter_z);
let detector = Point3::new(0.0, self.detector_y, self.detector_z);
let spacing = self.volume_width / (self.resolution - 1) as f64;
for i in 0..self.resolution {
let focus = Point3::new(i as f64 * spacing - self.volume_width / 2.0, 0.0, 0.0);
let surf_point = SurfacePoint3::new_normalize(emitter, focus - emitter);
let ray = Ray::from(&surf_point);
let (min_t, max_t) = min_max_t(&ray, self.volume_z_min, self.volume_z_max);
let ob_limit = obstruction_limit(obstruction, &ray, iso);
let range_limit = ob_limit.min(max_t);
if let Some(ri) =
target
.tri_mesh()
.cast_ray_and_get_normal(iso, &ray, range_limit, false)
{
if ri.time_of_impact < min_t {
continue;
}
let n = ri.normal * -1.0;
if ray.dir.angle(&n) > limit {
continue;
}
let impact: Point3 = ray.point_at(ri.time_of_impact);
let witness = Ray::new(emitter, impact - detector);
let ob_limit = obstruction_limit(obstruction, &witness, iso);
if ob_limit < 1.0 - 1e-4 {
continue;
}
if target
.tri_mesh()
.cast_ray(iso, &witness, 1.0 - 1e-4, false)
.is_some()
{
continue;
}
points.push(impact);
normals.push(UnitVec3::new_normalize(ri.normal));
}
}
let mut cloud = PointCloud3::new(points);
cloud
.set_point_normals(Some(normals))
.expect("normals were built one per point");
(cloud, None)
}
}
fn min_max_t(ray: &Ray, volume_z_min: f64, volume_z_max: f64) -> (f64, f64) {
let to_max = ray.origin.z - volume_z_max;
let to_min = ray.origin.z - volume_z_min;
let scale = 1.0 / -ray.dir.z;
(scale * to_max, scale * to_min)
}
#[derive(Debug, Clone)]
pub struct PanningLaserProfile {
laser_line: LaserProfile,
y_step: f64,
steps: usize,
}
impl PanningLaserProfile {
pub fn new(laser_line: LaserProfile, y_step: f64, steps: usize) -> Self {
Self {
laser_line,
y_step,
steps,
}
}
}
impl SimulatedPointSensor for PanningLaserProfile {
fn get_points(
&self,
target: &Mesh3,
obstruction: Option<&Mesh3>,
iso: &Iso3,
) -> (PointCloud3, Option<Vec<f64>>) {
let pan_vector = Vector3::new(0.0, self.y_step, 0.0);
let mut points = Vec::new();
let mut normals = Vec::new();
for i in 0..self.steps {
let shift = Translation3::from(-pan_vector * i as f64);
let inv = shift.inverse();
let t = shift * iso;
let (cloud, _) = self.laser_line.get_points(target, obstruction, &t);
points.extend(cloud.points().iter().map(|p| inv * p));
normals.extend(
cloud
.point_normals()
.expect("the laser line always attaches normals"),
);
}
let mut cloud = PointCloud3::new(points);
cloud
.set_point_normals(Some(normals))
.expect("normals were built one per point");
(cloud, None)
}
}
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
#[test]
fn time_of_flight_limits() {
let z_min = -5.0;
let z_max = 10.0;
let emitter = Point3::new(0.0, 0.0, 20.0);
let focus = Point3::new(13.0, 0.0, 0.0);
let surf_point = SurfacePoint3::new_normalize(emitter, focus - emitter);
let ray = Ray::from(&surf_point);
let (min_t, max_t) = min_max_t(&ray, z_min, z_max);
assert_relative_eq!(z_max, surf_point.at_distance(min_t).z, epsilon = 1e-8);
assert_relative_eq!(z_min, surf_point.at_distance(max_t).z, epsilon = 1e-8);
}
}