pub mod jacobian;
mod mesh;
mod multi_mesh;
pub mod multi_param;
mod options;
mod params;
mod points_to_surface;
mod rotations;
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::mesh::*;
pub use self::multi_mesh::{
MMOpts, MulMeshAlignPoint, multi_mesh_adjustment, multi_mesh_adjustment_with_points,
};
pub use self::options::*;
pub use self::params::*;
pub use self::points_to_surface::*;
pub use self::rotations::RotationMatrices;
#[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),
)
}
pub fn distance_weight(d: f64, threshold: f64) -> f64 {
(threshold - d).ceil().clamp(0.0, 1.0)
}
pub fn normal_weight(n: &Vector3, n_ref: &Vector3) -> f64 {
n.dot(n_ref).ceil().max(0.0)
}
#[derive(Clone)]
pub struct RcParams3 {
pub rc: Point3,
shift0: Iso3,
shift1: Iso3,
x: AlignStorage3,
transform: Iso3,
inverse: Iso3,
rotations: RotationMatrices,
current_rc: Point3,
}
impl RcParams3 {
pub fn from_initial(initial: &Iso3, rc: &Point3) -> Self {
let rc_d = initial * rc;
let rotations = RotationMatrices::from_rotation(&initial.rotation);
let x = AlignStorage3::new(0.0, 0.0, 0.0, rotations.r.x, rotations.r.y, rotations.r.z);
let mut item = Self {
rc: *rc,
shift0: Iso3::translation(-rc.x, -rc.y, -rc.z),
shift1: Iso3::translation(rc_d.x, rc_d.y, rc_d.z),
x,
transform: Iso3::identity(),
inverse: Iso3::identity(),
rotations,
current_rc: rc_d,
};
item.compute();
item
}
pub fn rotations(&self) -> &RotationMatrices {
&self.rotations
}
pub fn current_rc(&self) -> &Point3 {
&self.current_rc
}
pub fn set(&mut self, x: &AlignStorage3) {
self.x = *x;
self.compute();
}
pub fn set_index(&mut self, index: usize, value: f64) {
self.x[index] = value;
self.compute();
}
pub fn x(&self) -> &AlignStorage3 {
&self.x
}
fn compute(&mut self) {
self.rotations = RotationMatrices::from_euler(self.x[3], self.x[4], self.x[5]);
let p = Iso3::from_parts(
Translation3::new(self.x.x, self.x.y, self.x.z),
self.rotations.q,
);
self.transform = self.shift1 * p * self.shift0;
self.inverse = self.transform.inverse();
self.current_rc = self.transform * self.rc;
}
pub fn transform(&self) -> &Iso3 {
&self.transform
}
pub fn inverse(&self) -> &Iso3 {
&self.inverse
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::linear_space;
use std::f64::consts::FRAC_PI_2;
use crate::common::random_geometry::RandomGeometry3;
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);
}
#[test]
fn check_distance_weight() {
let threshold = 30.0;
for x in linear_space(0.0, 50.0, 1000).iter() {
let ex = if *x > threshold { 0.0 } else { 1.0 };
let w = distance_weight(*x, threshold);
assert_relative_eq!(w, ex, epsilon = 1e-10);
}
let threshold = 0.5;
for x in linear_space(0.0, 1.0, 1000).iter() {
let ex = if *x > threshold { 0.0 } else { 1.0 };
let w = distance_weight(*x, threshold);
assert_relative_eq!(w, ex, epsilon = 1e-10);
}
}
#[test]
fn iso3_param_round_trips_stress_test_rc() {
let mut rg = RandomGeometry3::new();
for _ in 0..10000 {
let t = rg.iso3(10.0);
let rc = rg.point(10.0);
let p = RcParams3::from_initial(&t, &rc);
let t2 = p.transform();
assert_relative_eq!(t.to_matrix(), t2.to_matrix(), epsilon = 1e-10);
}
}
}