use super::*;
use crate::geom3::align3::params::AlignValues3;
use crate::geom3::{Point3, SurfacePoint3};
use parry2d_f64::na::Dim;
use parry3d_f64::na::{Matrix, RawStorageMut, Storage, U6};
pub fn point_plane_jacobian(p: &Point3, c: &SurfacePoint3, params: &RcParams3) -> AlignStorage3 {
let s = c.scalar_projection(p).signum();
let from_rc = Point3::from(p - params.current_rc());
point_plane_core(s, c, from_rc, params)
}
pub fn point_plane_jacobian_rev(
p: &Point3,
c: &SurfacePoint3,
params: &RcParams3,
) -> AlignStorage3 {
let s = c.scalar_projection(p).signum();
let from_rc = Point3::from(c.point - params.current_rc());
point_plane_core(-s, c, from_rc, params)
}
fn point_plane_core(
s: f64,
c: &SurfacePoint3,
from_rc: Point3,
params: &RcParams3,
) -> AlignStorage3 {
let mut result = AlignStorage3::zeros();
let n = c.normal.into_inner() * s;
result[0] = n.x;
result[1] = n.y;
result[2] = n.z;
result[3] = n.dot(&(params.rotations().rd.x * from_rc).coords);
result[4] = n.dot(&(params.rotations().rd.y * from_rc).coords);
result[5] = n.dot(&(params.rotations().rd.z * from_rc).coords);
result
}
pub fn point_surf_jacobian(
p: &impl PCoords<3>,
c: &impl SPCoords<3>,
align: &AlignValues3,
) -> AlignStorage3 {
let mut result = AlignStorage3::zeros();
let sign = c.scalar_projection(p).signum();
let dev = p.coords() - c.coords();
let dir = if dev.norm_squared() < 1e-16 {
c.normal().into_inner()
} else {
dev.normalize() * sign
};
result[0] = val_or_zero(align.dtx.dot(&dir), align.dof.tx);
result[1] = val_or_zero(align.dty.dot(&dir), align.dof.ty);
result[2] = val_or_zero(align.dtz.dot(&dir), align.dof.tz);
result[3] = val_or_zero(align.drx(c).dot(&dir), align.dof.rx);
result[4] = val_or_zero(align.dry(c).dot(&dir), align.dof.ry);
result[5] = val_or_zero(align.drz(c).dot(&dir), align.dof.rz);
result
}
fn val_or_zero(value: f64, condition: bool) -> f64 {
if condition { value } else { 0.0 }
}
pub fn copy_jacobian<R, S>(j: &AlignStorage3, matrix: &mut Matrix<f64, R, U6, S>, row: usize)
where
R: Dim,
S: RawStorageMut<f64, R, U6> + Storage<f64, R, U6>,
{
matrix.row_mut(row).copy_from_slice(j.as_slice());
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom3::Point3;
use approx::assert_relative_eq;
use parry3d_f64::na::{Dyn, Owned};
use std::f64::consts::PI;
const NUMERIC_EPSILON: f64 = 1e-8;
fn point_plane_numeric(
params: &RcParams3,
p: &Point3,
closest: &SurfacePoint3,
index: usize,
) -> f64 {
let mut params = params.clone();
let t_i = params.transform().inverse();
let mut x = *params.x();
x[index] += NUMERIC_EPSILON;
params.set(&x);
let t = params.transform() * t_i;
let moved = t * *p;
let d0 = closest.scalar_projection(p).abs();
let d1 = closest.scalar_projection(&moved).abs();
(d1 - d0) / NUMERIC_EPSILON
}
fn point_plane_numeric_rev(
params: &RcParams3,
p: &Point3,
closest: &SurfacePoint3,
index: usize,
) -> f64 {
let mut params = params.clone();
let t_i = params.transform().inverse();
let mut x = *params.x();
x[index] += NUMERIC_EPSILON;
params.set(&x);
let t = params.transform() * t_i;
let d0 = closest.scalar_projection(p);
let moved = closest.transformed_by(&t);
let d1 = moved.scalar_projection(p);
(d1 - d0) / NUMERIC_EPSILON
}
#[test]
fn test_point_plane_translation() {
let p = Point3::new(1.0, 2.0, 3.0);
let c = Point3::new(0.0, 0.0, 0.0);
let sp = SurfacePoint3::new_normalize(c, p - c);
let initial = Iso3::from_parts(
Translation3::new(8.0, -5.0, -6.0),
UnitQuaternion::from_euler_angles(-0.2, 0.3, 0.5),
);
let rc = Point3::new(-1.0, -2.0, 3.0);
let params = RcParams3::from_initial(&initial, &(initial.inverse() * rc));
let test = point_plane_jacobian(&p, &sp, ¶ms);
assert_relative_eq!(
point_plane_numeric(¶ms, &p, &sp, 0),
test.x,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric(¶ms, &p, &sp, 1),
test.y,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric(¶ms, &p, &sp, 2),
test.z,
epsilon = 1e-6
);
}
#[test]
fn test_point_plane_simple_rotations() {
let p = Point3::new(1.0, 2.0, 3.0);
let c = Point3::new(1.0, 3.0, 4.0);
let sp = SurfacePoint3::new_normalize(c, p - c);
let rc = Point3::origin();
let params = RcParams3::from_initial(&Iso3::identity(), &rc);
let test = point_plane_jacobian(&p, &sp, ¶ms);
let expected_w = point_plane_numeric(¶ms, &p, &sp, 3);
let expected_a = point_plane_numeric(¶ms, &p, &sp, 4);
let expected_b = point_plane_numeric(¶ms, &p, &sp, 5);
assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
}
#[test]
fn test_point_plane_centered_rotations() {
let p = Point3::new(1.0, 2.0, 3.0);
let c = Point3::new(1.0, 3.0, 4.0);
let sp = SurfacePoint3::new_normalize(c, p - c);
let rc = Point3::new(-1.2, -3.5, -0.75);
let params = RcParams3::from_initial(&Iso3::identity(), &rc);
let test = point_plane_jacobian(&p, &sp, ¶ms);
let expected_w = point_plane_numeric(¶ms, &p, &sp, 3);
let expected_a = point_plane_numeric(¶ms, &p, &sp, 4);
let expected_b = point_plane_numeric(¶ms, &p, &sp, 5);
assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
}
#[test]
fn test_point_plane_initial_rz_rotation() {
let p1 = Point3::new(1.0, 0.0, 0.0);
let c = Point3::new(1.0, 0.0, 0.0);
let sp = SurfacePoint3::new_normalize(c, Vector3::new(0.0, 1.0, 0.0));
let rc1 = Point3::origin();
let initial = Iso3::from_parts(
Translation3::identity(),
UnitQuaternion::from_euler_angles(0.0, 0.0, -PI / 2.0),
);
let rc0 = initial.inverse() * rc1;
let p0 = initial.inverse() * p1;
let params = RcParams3::from_initial(&initial, &rc0);
assert_relative_eq!(*params.current_rc(), rc1, epsilon = 1e-6);
assert_relative_eq!(params.transform() * p0, p1, epsilon = 1e-6);
let test = point_plane_jacobian(&p1, &sp, ¶ms);
let expected_w = point_plane_numeric(¶ms, &p1, &sp, 3);
let expected_a = point_plane_numeric(¶ms, &p1, &sp, 4);
let expected_b = point_plane_numeric(¶ms, &p1, &sp, 5);
assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
}
#[test]
fn test_point_plane_rotation() {
let p1 = Point3::new(10.0, 20.0, 30.0);
let r1 = Point3::new(5.0, 8.0, 18.0);
let c = Point3::new(12.0, 18.0, 28.0);
let sp = SurfacePoint3::new_normalize(c, p1 - c);
let initial = Iso3::from_parts(
Translation3::new(8.0, -5.0, -6.0),
UnitQuaternion::from_euler_angles(-0.2, 0.3, 0.5),
);
let p0 = initial.inverse() * p1;
let r0 = initial.inverse() * r1;
let params = RcParams3::from_initial(&initial, &r0);
assert_relative_eq!(p1, params.transform() * p0, epsilon = 1e-8);
assert_relative_eq!(r1, params.transform() * r0, epsilon = 1e-8);
let test = point_plane_jacobian(&p1, &sp, ¶ms);
let expected_w = point_plane_numeric(¶ms, &p1, &sp, 3);
let expected_a = point_plane_numeric(¶ms, &p1, &sp, 4);
let expected_b = point_plane_numeric(¶ms, &p1, &sp, 5);
assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
}
fn rev_test() -> (RcParams3, Point3, SurfacePoint3, AlignStorage3) {
let p = Point3::new(2.0, 3.0, 4.0);
let cp = Point3::new(1.0, 2.0, 3.0);
let cn = Vector3::new(1.0, 1.0, 1.0);
let c = SurfacePoint3::new_normalize(cp, cn);
let rc = Point3::new(-0.5, -0.75, 3.25);
let initial = Iso3::from_parts(
Translation3::new(4.0, 3.0, 2.0),
UnitQuaternion::from_euler_angles(0.2, -0.3, 0.5),
);
let rc0 = initial.inverse() * rc;
let params = RcParams3::from_initial(&initial, &rc0);
let test = point_plane_jacobian_rev(&p, &c, ¶ms);
(params, p, c, test)
}
#[test]
fn test_point_plane_rev_translation() {
let (params, p, c, test) = rev_test();
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 0),
test.x,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 1),
test.y,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 2),
test.z,
epsilon = 1e-6
);
}
#[test]
fn test_point_plane_rev_rotation() {
let (params, p, c, test) = rev_test();
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 3),
test.w,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 4),
test.a,
epsilon = 1e-6
);
assert_relative_eq!(
point_plane_numeric_rev(¶ms, &p, &c, 5),
test.b,
epsilon = 1e-6
);
}
#[test]
fn test_jacobian_copy() {
let x = AlignStorage3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
let mut target = Matrix::<f64, Dyn, U6, Owned<f64, Dyn, U6>>::zeros(10);
copy_jacobian(&x, &mut target, 4);
assert_eq!(target[(4, 0)], 1.0);
assert_eq!(target[(4, 1)], 2.0);
assert_eq!(target[(4, 2)], 3.0);
assert_eq!(target[(4, 3)], 4.0);
assert_eq!(target[(4, 4)], 5.0);
assert_eq!(target[(4, 5)], 6.0);
}
}