use crate::Result;
use crate::common::{Line, PCoords};
use parry3d_f64::na::{AbstractRotation, Isometry, Point, SVector, Unit};
use serde::{Deserialize, Serialize};
#[cfg(test)]
use approx::{AbsDiffEq, RelativeEq};
#[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq)]
pub struct SurfacePoint<const D: usize> {
pub point: Point<f64, D>,
pub normal: Unit<SVector<f64, D>>,
}
impl<const D: usize> SurfacePoint<D> {
pub fn new(point: Point<f64, D>, normal: Unit<SVector<f64, D>>) -> Self {
Self { point, normal }
}
pub fn new_normalize(point: Point<f64, D>, normal: SVector<f64, D>) -> Self {
Self::new(point, Unit::new_normalize(normal))
}
pub fn at_distance(&self, distance: f64) -> Point<f64, D> {
self.point + self.normal.as_ref() * distance
}
pub fn scalar_projection(&self, other: &impl PCoords<D>) -> f64 {
self.normal.dot(&(other.coords() - self.point.coords))
}
pub fn projection(&self, other: &impl PCoords<D>) -> Point<f64, D> {
self.at_distance(self.scalar_projection(other))
}
pub fn reversed(&self) -> Self {
Self::new(self.point, -self.normal)
}
pub fn transformed_by<R>(&self, t: &Isometry<f64, R, D>) -> Self
where
R: AbstractRotation<f64, D>,
{
Self::new(t * self.point, t * self.normal)
}
pub fn planar_distance(&self, other: &impl PCoords<D>) -> f64 {
let projection = self.projection(other);
(projection.coords - other.coords()).norm()
}
pub fn shifted(&self, offset: f64) -> Self {
let new_point = self.point + self.normal.as_ref() * offset;
Self::new(new_point, self.normal)
}
pub fn as_line(&self) -> Line<D> {
Line::new(self.point, self.normal.into_inner())
}
pub fn slerp(&self, other: &Self, t: f64) -> Self {
let n = self.normal.slerp(&other.normal, t);
let v = other.point - self.point;
Self::new(self.point + v * t, n)
}
}
#[cfg(test)]
impl<const D: usize> AbsDiffEq<Self> for SurfacePoint<D> {
type Epsilon = f64;
fn default_epsilon() -> Self::Epsilon {
1e-8
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
self.point.abs_diff_eq(&other.point, epsilon)
&& self.normal.abs_diff_eq(&other.normal, epsilon)
}
}
#[cfg(test)]
impl<const D: usize> RelativeEq for SurfacePoint<D> {
fn default_max_relative() -> Self::Epsilon {
1e-8
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
self.point.relative_eq(&other.point, epsilon, max_relative)
&& self
.normal
.relative_eq(&other.normal, epsilon, max_relative)
}
}
pub fn surface_point_vector<const D: usize>(
points: &[Point<f64, D>],
normals: &[SVector<f64, D>],
) -> Result<Vec<SurfacePoint<D>>> {
if points.len() != normals.len() {
return Err("The number of points and normals must be the same".into());
}
Ok(points
.iter()
.zip(normals.iter())
.map(|(p, n)| SurfacePoint::new_normalize(*p, *n))
.collect())
}
pub trait SurfacePointCollection<const D: usize> {
fn clone_points(&self) -> Vec<Point<f64, D>>;
fn clone_normals(&self) -> Vec<Unit<SVector<f64, D>>>;
}
#[cfg(test)]
mod tests {
use super::*;
use crate::SurfacePoint2;
use approx::assert_relative_eq;
use parry3d_f64::na::{Point2, Point3, Vector2, Vector3};
fn sp2(px: f64, py: f64, nx: f64, ny: f64) -> SurfacePoint<2> {
SurfacePoint::new_normalize(Point2::new(px, py), Vector2::new(nx, ny))
}
fn sp3(px: f64, py: f64, pz: f64, nx: f64, ny: f64, nz: f64) -> SurfacePoint<3> {
SurfacePoint::new_normalize(Point3::new(px, py, pz), Vector3::new(nx, ny, nz))
}
#[test]
fn new_stores_point_and_normal() {
let sp = sp2(1.0, 2.0, 0.0, 1.0);
assert_relative_eq!(sp.point, Point2::new(1.0, 2.0));
assert_relative_eq!(sp.normal.into_inner(), Vector2::new(0.0, 1.0));
}
#[test]
fn new_normalize_normalizes_the_vector() {
let sp = SurfacePoint::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 3.0));
assert_relative_eq!(sp.normal.into_inner().norm(), 1.0, epsilon = 1e-12);
assert_relative_eq!(
sp.normal.into_inner(),
Vector2::new(0.0, 1.0),
epsilon = 1e-12
);
}
#[test]
fn at_distance_positive_moves_along_normal() {
let sp = sp2(0.0, 0.0, 0.0, 1.0);
let p = sp.at_distance(3.0);
assert_relative_eq!(p, Point2::new(0.0, 3.0), epsilon = 1e-12);
}
#[test]
fn at_distance_negative_moves_opposite_normal() {
let sp = sp2(1.0, 1.0, 1.0, 0.0);
let p = sp.at_distance(-2.0);
assert_relative_eq!(p, Point2::new(-1.0, 1.0), epsilon = 1e-12);
}
#[test]
fn at_distance_zero_returns_the_origin_point() {
let sp = sp3(3.0, 4.0, 5.0, 0.0, 0.0, 1.0);
assert_relative_eq!(sp.at_distance(0.0), sp.point, epsilon = 1e-12);
}
#[test]
fn scalar_projection_point_ahead_of_plane() {
let sp = sp2(0.0, 0.0, 0.0, 1.0); let other = Point2::new(0.0, 5.0);
assert_relative_eq!(sp.scalar_projection(&other), 5.0, epsilon = 1e-12);
}
#[test]
fn scalar_projection_point_behind_plane() {
let sp = sp2(0.0, 0.0, 0.0, 1.0); let other = Point2::new(1.0, -3.0);
assert_relative_eq!(sp.scalar_projection(&other), -3.0, epsilon = 1e-12);
}
#[test]
fn scalar_projection_point_on_plane_is_zero() {
let sp = sp2(0.0, 0.0, 1.0, 0.0); let other = Point2::new(0.0, 7.0); assert_relative_eq!(sp.scalar_projection(&other), 0.0, epsilon = 1e-12);
}
#[test]
fn scalar_projection_offset_origin() {
let sp = sp2(0.0, 2.0, 0.0, 1.0); let other = Point2::new(0.0, 5.0);
assert_relative_eq!(sp.scalar_projection(&other), 3.0, epsilon = 1e-12);
}
#[test]
fn projection_lands_on_the_line() {
let sp = sp2(0.0, 0.0, 0.0, 1.0); let other = Point2::new(4.0, 3.0);
let proj = sp.projection(&other);
assert_relative_eq!(proj, Point2::new(0.0, 3.0), epsilon = 1e-12);
}
#[test]
fn projection_of_point_already_on_line_is_itself() {
let sp = sp2(0.0, 0.0, 0.0, 1.0);
let on_line = Point2::new(0.0, 7.0);
assert_relative_eq!(sp.projection(&on_line), on_line, epsilon = 1e-12);
}
#[test]
fn reversed_flips_normal_keeps_point() {
let sp = sp2(1.0, 2.0, 0.0, 1.0);
let rev = sp.reversed();
assert_relative_eq!(rev.point, sp.point);
assert_relative_eq!(
rev.normal.into_inner(),
-sp.normal.into_inner(),
epsilon = 1e-12
);
}
#[test]
fn reversed_twice_is_identity() {
let sp = sp3(1.0, 2.0, 3.0, 1.0, 1.0, 0.0);
let twice = sp.reversed().reversed();
assert_relative_eq!(twice.point, sp.point);
assert_relative_eq!(
twice.normal.into_inner(),
sp.normal.into_inner(),
epsilon = 1e-12
);
}
#[test]
fn planar_distance_measures_lateral_offset() {
let sp = sp2(0.0, 0.0, 0.0, 1.0);
let other = Point2::new(3.0, 7.0);
assert_relative_eq!(sp.planar_distance(&other), 3.0, epsilon = 1e-12);
}
#[test]
fn planar_distance_zero_for_point_on_line() {
let sp = sp2(0.0, 0.0, 0.0, 1.0);
let on_line = Point2::new(0.0, 5.0);
assert_relative_eq!(sp.planar_distance(&on_line), 0.0, epsilon = 1e-12);
}
#[test]
fn planar_distance_3d() {
let sp = sp3(0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
let other = Point3::new(3.0, 4.0, 10.0); assert_relative_eq!(sp.planar_distance(&other), 5.0, epsilon = 1e-12);
}
#[test]
fn shifted_moves_point_keeps_normal() {
let sp = sp2(0.0, 0.0, 0.0, 1.0); let shifted = sp.shifted(4.0);
assert_relative_eq!(shifted.point, Point2::new(0.0, 4.0), epsilon = 1e-12);
assert_relative_eq!(
shifted.normal.into_inner(),
sp.normal.into_inner(),
epsilon = 1e-12
);
}
#[test]
fn shifted_negative_offset() {
let sp = sp2(0.0, 5.0, 0.0, 1.0); let shifted = sp.shifted(-3.0);
assert_relative_eq!(shifted.point, Point2::new(0.0, 2.0), epsilon = 1e-12);
}
#[test]
fn surface_point_vector_builds_correctly() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
let normals = vec![Vector2::new(0.0, 1.0), Vector2::new(0.0, 1.0)];
let spv = surface_point_vector(&points, &normals).unwrap();
assert_eq!(spv.len(), 2);
assert_relative_eq!(spv[0].point, points[0]);
assert_relative_eq!(spv[1].point, points[1]);
assert_relative_eq!(spv[0].normal.into_inner(), normals[0]);
assert_relative_eq!(spv[1].normal.into_inner(), normals[1]);
}
#[test]
fn surface_point_vector_length_mismatch_returns_error() {
let points = vec![Point2::new(0.0, 0.0)];
let normals: Vec<Vector2<f64>> = vec![];
assert!(surface_point_vector(&points, &normals).is_err());
}
#[test]
fn slerp() {
let sp0 = SurfacePoint2::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0));
let sp1 = SurfacePoint2::new_normalize(Point2::new(1.0, 0.0), Vector2::new(1.0, 0.0));
assert_relative_eq!(sp0, sp0.slerp(&sp1, 0.0));
assert_relative_eq!(sp1, sp0.slerp(&sp1, 1.0));
let e = SurfacePoint2::new_normalize(Point2::new(0.5, 0.0), Vector2::new(1.0, 1.0));
assert_relative_eq!(e, sp0.slerp(&sp1, 0.5));
}
}