use super::*;
use crate::geom3::align3::params::AlignValues3;
use parry2d_f64::na::Dim;
use parry3d_f64::na::{Matrix, RawStorageMut, Storage, U6};
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
}
pub fn point_surf_jacobian_rev(
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
};
let dir = -dir;
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::UnitVec3;
use crate::common::random_geometry::RandomGeometry3;
use crate::geom3::{Point3, SurfacePoint3, Vector3};
use approx::assert_relative_eq;
use parry3d_f64::na::{Dyn, Owned};
const NUMERIC_EPSILON: f64 = 1e-8;
fn surf_rev_residual(
params: &AlignParams3,
p: &Point3,
c_local: &Point3,
n_local: &UnitVec3,
) -> f64 {
let t = params.compute_transform();
let c = t * c_local;
let n = t.rotation * *n_local;
let sp = SurfacePoint3::new(c, n);
crate::common::dist(p, &c) * sp.scalar_projection(p).signum()
}
fn surf_rev_numeric(
params: &AlignParams3,
p: &Point3,
c_local: &Point3,
n_local: &UnitVec3,
index: usize,
) -> f64 {
let mut lo = params.clone();
lo.set_index(index, params.storage()[index] - NUMERIC_EPSILON);
let mut hi = params.clone();
hi.set_index(index, params.storage()[index] + NUMERIC_EPSILON);
let r_lo = surf_rev_residual(&lo, p, c_local, n_local);
let r_hi = surf_rev_residual(&hi, p, c_local, n_local);
(r_hi - r_lo) / (2.0 * NUMERIC_EPSILON)
}
#[test]
fn stress_surf_rev_against_numeric() {
let mut rg = RandomGeometry3::from_seed(0x5eed_a11c);
for _ in 0..2000 {
let params =
AlignParams3::new(AlignOrigin3::Local(rg.iso3(5.0)), Some(rg.iso3(5.0)), None)
.with_storage(AlignStorage3::from_iterator((0..6).map(|i| {
if i < 3 {
rg.f64_sym(3.0)
} else {
rg.f64_sym(0.8)
}
})));
let c_local = rg.point(5.0);
let n_local = rg.unit_vec();
let t = params.compute_transform();
let c = t * c_local;
let n = t.rotation * n_local;
let p = c + n.into_inner() * rg.f64_sym(4.0).abs().max(0.5);
let analytic =
point_surf_jacobian_rev(&p, &SurfacePoint3::new(c, n), ¶ms.compute_values());
for i in 0..6 {
let numeric = surf_rev_numeric(¶ms, &p, &c_local, &n_local, i);
assert_relative_eq!(analytic[i], numeric, epsilon = 1e-6);
}
}
}
#[test]
fn surf_rev_is_the_negative_of_the_forward_form_for_translations() {
let params = AlignParams3::from_origin(None);
let values = params.compute_values();
let c = Point3::new(1.0, 2.0, 3.0);
let n = UnitVec3::new_normalize(Vector3::new(0.0, 0.0, 1.0));
let p = Point3::new(1.0, 2.0, 4.5);
let sp = SurfacePoint3::new(c, n);
let fwd = point_surf_jacobian(&p, &sp, &values);
let rev = point_surf_jacobian_rev(&p, &sp, &values);
for i in 0..3 {
assert_relative_eq!(fwd[i], -rev[i], epsilon = 1e-12);
}
}
#[test]
fn surf_rev_respects_locked_dof() {
let dof = Dof6::new(true, false, true, false, true, false);
let params = AlignParams3::from_origin(Some(dof));
let values = params.compute_values();
let c = Point3::new(1.0, 2.0, 3.0);
let n = UnitVec3::new_normalize(Vector3::new(0.0, 1.0, 1.0));
let p = Point3::new(2.0, 3.0, 5.0);
let rev = point_surf_jacobian_rev(&p, &SurfacePoint3::new(c, n), &values);
for i in [1usize, 3, 5] {
assert_eq!(
rev[i], 0.0,
"locked parameter {i} should have a zero column"
);
}
}
#[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);
}
}