mod cloud;
mod information;
pub mod jacobian;
mod mesh;
mod multi_mesh;
mod multi_params;
mod options;
mod params;
mod points_to_surface;
use crate::UnitVec3;
use crate::common::{PCoords, SPCoords};
use crate::geom3::{Iso3, Point3, Vector3};
use crate::na::{SVector, Unit};
use parry3d_f64::na::{Translation3, UnitQuaternion, Vector6};
pub type AlignStorage3 = Vector6<f64>;
pub use self::cloud::CloudTarget3;
pub use self::information::*;
pub use self::mesh::*;
pub use self::multi_mesh::{
MeshAlignPoint, MultiOptions3, multi_mesh_adjustment, multi_mesh_adjustment_with_points,
};
pub use self::multi_params::MultiParams3;
pub use self::options::*;
pub use self::params::*;
pub use self::points_to_surface::*;
#[derive(Debug, Clone)]
pub struct AlignSurfMatch3 {
pub point: Point3,
pub normal: UnitVec3,
pub is_on: bool,
pub weight: f64,
pub sigma: f64,
}
impl AlignSurfMatch3 {
pub fn new(point: Point3, normal: UnitVec3, is_on: bool, weight: f64) -> Self {
Self {
point,
normal,
is_on,
weight,
sigma: 0.0,
}
}
pub fn with_sigma(&self, sigma: f64) -> Self {
Self {
sigma,
..self.clone()
}
}
}
impl PCoords<3> for AlignSurfMatch3 {
fn coords(&self) -> SVector<f64, 3> {
self.point.coords()
}
}
impl SPCoords<3> for AlignSurfMatch3 {
fn normal(&self) -> Unit<SVector<f64, 3>> {
self.normal
}
}
impl Default for AlignSurfMatch3 {
fn default() -> Self {
Self {
point: Point3::origin(),
normal: Vector3::x_axis(),
is_on: false,
weight: 0.0,
sigma: 0.0,
}
}
}
pub trait SurfaceTarget3: Sync + Send {
fn find_align_match(&self, p: &Point3) -> AlignSurfMatch3;
}
#[derive(Clone, Copy, Debug)]
pub struct Dof6 {
pub tx: bool,
pub ty: bool,
pub tz: bool,
pub rx: bool,
pub ry: bool,
pub rz: bool,
}
impl Dof6 {
pub fn new(tx: bool, ty: bool, tz: bool, rx: bool, ry: bool, rz: bool) -> Self {
Self {
tx,
ty,
tz,
rx,
ry,
rz,
}
}
pub fn all() -> Self {
Self {
tx: true,
ty: true,
tz: true,
rx: true,
ry: true,
rz: true,
}
}
}
impl Default for Dof6 {
fn default() -> Self {
Self::all()
}
}
#[derive(Clone, Copy, Debug)]
pub enum SampleMode {
All,
Random(usize),
Poisson(f64),
}
pub fn iso3_from_param(p: &AlignStorage3) -> Iso3 {
Iso3::from_parts(
Translation3::new(p.x, p.y, p.z),
UnitQuaternion::from_euler_angles(p.w, p.a, p.b),
)
}
#[cfg(test)]
mod tests {
use super::*;
use std::f64::consts::FRAC_PI_2;
use approx::assert_relative_eq;
#[test]
fn iso3_tx() {
let storage = AlignStorage3::new(1.0, 0.0, 0.0, 0.0, 0.0, 0.0);
let t = iso3_from_param(&storage);
let p = Point3::new(1.0, 0.0, 0.0);
let p2 = t * p;
assert_relative_eq!(p2.x, 2.0, epsilon = 1e-10);
}
#[test]
fn iso3_ty() {
let storage = AlignStorage3::new(0.0, 1.0, 0.0, 0.0, 0.0, 0.0);
let t = iso3_from_param(&storage);
let p = Point3::new(0.0, 1.0, 0.0);
let p2 = t * p;
assert_relative_eq!(p2.y, 2.0, epsilon = 1e-10);
}
#[test]
fn iso3_tz() {
let storage = AlignStorage3::new(0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
let t = iso3_from_param(&storage);
let p = Point3::new(0.0, 0.0, 1.0);
let p2 = t * p;
assert_relative_eq!(p2.z, 2.0, epsilon = 1e-10);
}
#[test]
fn iso3_rx() {
let storage = AlignStorage3::new(0.0, 0.0, 0.0, FRAC_PI_2, 0.0, 0.0);
let t = iso3_from_param(&storage);
let p = Point3::new(0.0, 1.0, 0.0);
let test = t * p;
let expected = Iso3::rotation(Vector3::x_axis().into_inner() * FRAC_PI_2) * p;
assert_relative_eq!(test, expected, epsilon = 1e-10);
}
#[test]
fn iso3_ry() {
let storage = AlignStorage3::new(0.0, 0.0, 0.0, 0.0, FRAC_PI_2, 0.0);
let t = iso3_from_param(&storage);
let p = Point3::new(0.0, 0.0, 1.0);
let test = t * p;
let expected = Iso3::rotation(Vector3::y_axis().into_inner() * FRAC_PI_2) * p;
assert_relative_eq!(test, expected, epsilon = 1e-10);
}
#[test]
fn iso3_rz() {
let storage = AlignStorage3::new(0.0, 0.0, 0.0, 0.0, 0.0, FRAC_PI_2);
let t = iso3_from_param(&storage);
let p = Point3::new(1.0, 0.0, 0.0);
let test = t * p;
let expected = Iso3::rotation(Vector3::z_axis().into_inner() * FRAC_PI_2) * p;
assert_relative_eq!(test, expected, epsilon = 1e-10);
}
}