use crate::common::{PCoords, SPCoords};
use crate::geom2::align2::{AlignStorage2, AlignValues2};
use parry2d_f64::na::{Dim, Matrix, RawStorageMut, Storage, U3};
pub fn point_surf_jacobian(
p: &impl PCoords<2>,
c: &impl SPCoords<2>,
align: &AlignValues2,
) -> AlignStorage2 {
let mut result = AlignStorage2::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.drz(p).dot(&dir), align.dof.rz);
result
}
pub fn point_surf_jacobian_rev(
p: &impl PCoords<2>,
c: &impl SPCoords<2>,
align: &AlignValues2,
) -> AlignStorage2 {
let mut result = AlignStorage2::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.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: &AlignStorage2, matrix: &mut Matrix<f64, R, U3, S>, row: usize)
where
R: Dim,
S: RawStorageMut<f64, R, U3> + Storage<f64, R, U3>,
{
matrix.row_mut(row).copy_from_slice(j.as_slice());
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::dist;
use crate::common::random_geometry::RandomGeometry2;
use crate::geom2::align2::{AlignOrigin2, AlignParams2, AlignSurfMatch2};
use crate::geom2::{Point2, SurfacePoint2, UnitVec2, Vector2};
use approx::assert_relative_eq;
use std::f64::consts::PI;
const NUMERIC_EPS: f64 = 1e-7;
fn residual(p: &Point2, c: &AlignSurfMatch2) -> f64 {
dist(p, &c.point) * c.scalar_projection(p).signum()
}
fn make_match(p: &Point2, offset: Vector2, flip: bool) -> AlignSurfMatch2 {
let c_point = p + offset;
let sign = if flip { -1.0 } else { 1.0 };
let normal = UnitVec2::new_normalize((p - c_point) * sign);
AlignSurfMatch2::new(c_point, normal, true, 1.0)
}
fn numeric(params: &AlignParams2, p_local: &Point2, c: &AlignSurfMatch2, index: usize) -> f64 {
let stored = params.storage();
let mut w0 = params.clone();
let mut w1 = params.clone();
w0.set_index(index, stored[index] - NUMERIC_EPS);
w1.set_index(index, stored[index] + NUMERIC_EPS);
let p0 = w0.compute_values().transform * p_local;
let p1 = w1.compute_values().transform * p_local;
(residual(&p1, c) - residual(&p0, c)) / (2.0 * NUMERIC_EPS)
}
#[test]
fn partials_at_identity() {
let params = AlignParams2::from_origin(None);
let p = Point2::new(1.0, 0.0);
let c = make_match(&p, Vector2::new(-1.0, 0.0), false);
let j = point_surf_jacobian(&p, &c, ¶ms.compute_values());
assert_relative_eq!(j.x, 1.0, epsilon = 1e-10);
assert_relative_eq!(j.y, 0.0, epsilon = 1e-10);
assert_relative_eq!(j.z, 0.0, epsilon = 1e-10);
}
#[test]
fn flipped_normal_flips_the_sign() {
let params = AlignParams2::from_origin(None);
let p = Point2::new(1.0, 0.0);
let c = make_match(&p, Vector2::new(-1.0, 0.0), true);
let j = point_surf_jacobian(&p, &c, ¶ms.compute_values());
assert_relative_eq!(j.x, -1.0, epsilon = 1e-10);
}
#[test]
fn locked_dof_zeroes_its_column() {
let dof = crate::geom2::align2::Dof3::new(false, true, false);
let params = AlignParams2::from_origin(Some(dof));
let p = Point2::new(1.0, 2.0);
let c = make_match(&p, Vector2::new(-0.5, -0.3), false);
let j = point_surf_jacobian(&p, &c, ¶ms.compute_values());
assert_eq!(j.x, 0.0);
assert_eq!(j.z, 0.0);
assert_ne!(j.y, 0.0);
}
#[test]
fn stress_against_numeric() {
let mut rg = RandomGeometry2::new();
for _ in 0..10000 {
let local = rg.iso2(10.0);
let offset = rg.iso2(10.0);
let params = AlignParams2::new(AlignOrigin2::Local(local), Some(offset), None)
.with_storage(rg.vector(PI));
let p_local = rg.point(10.0);
let p = params.compute_values().transform * p_local;
let dev = rg.unit_vec().into_inner() * rg.f64(0.1, 2.0);
let c = make_match(&p, dev, rg.bool());
let expected = [
numeric(¶ms, &p_local, &c, 0),
numeric(¶ms, &p_local, &c, 1),
numeric(¶ms, &p_local, &c, 2),
];
let j = point_surf_jacobian(&p, &c, ¶ms.compute_values());
assert_relative_eq!(j.x, expected[0], epsilon = 1e-5);
assert_relative_eq!(j.y, expected[1], epsilon = 1e-5);
assert_relative_eq!(j.z, expected[2], epsilon = 1e-5);
}
}
fn surf_rev_residual(
params: &AlignParams2,
p: &Point2,
c_local: &Point2,
n_local: &UnitVec2,
) -> f64 {
let sp = SurfacePoint2::new(*c_local, *n_local).transformed_by(¶ms.compute_transform());
dist(p, &sp.point) * sp.scalar_projection(p).signum()
}
fn surf_rev_numeric(
params: &AlignParams2,
p: &Point2,
c_local: &Point2,
n_local: &UnitVec2,
index: usize,
) -> f64 {
let mut lo = params.clone();
lo.set_index(index, params.storage()[index] - NUMERIC_EPS);
let mut hi = params.clone();
hi.set_index(index, params.storage()[index] + NUMERIC_EPS);
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_EPS)
}
#[test]
fn stress_surf_rev_against_numeric() {
let mut rg = RandomGeometry2::from_seed(0x5eed_a11c);
for _ in 0..2000 {
let params =
AlignParams2::new(AlignOrigin2::Local(rg.iso2(5.0)), Some(rg.iso2(5.0)), None)
.with_storage(AlignStorage2::new(
rg.f64_sym(3.0),
rg.f64_sym(3.0),
rg.f64_sym(0.8),
));
let c_local = rg.point(5.0);
let n_local = rg.unit_vec();
let sp =
SurfacePoint2::new(c_local, n_local).transformed_by(¶ms.compute_transform());
let p = sp.point + sp.normal.into_inner() * rg.f64_sym(4.0).abs().max(0.5);
let analytic = point_surf_jacobian_rev(&p, &sp, ¶ms.compute_values());
for i in 0..3 {
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 = AlignParams2::from_origin(None);
let values = params.compute_values();
let c = Point2::new(1.0, 2.0);
let n = UnitVec2::new_normalize(Vector2::new(0.0, 1.0));
let p = Point2::new(1.0, 3.5);
let sp = SurfacePoint2::new(c, n);
let fwd = point_surf_jacobian(&p, &sp, &values);
let rev = point_surf_jacobian_rev(&p, &sp, &values);
for i in 0..2 {
assert_relative_eq!(fwd[i], -rev[i], epsilon = 1e-12);
}
}
#[test]
fn surf_rev_respects_locked_dof() {
let dof = crate::geom2::align2::Dof3::new(true, false, false);
let params = AlignParams2::from_origin(Some(dof));
let values = params.compute_values();
let c = Point2::new(1.0, 2.0);
let n = UnitVec2::new_normalize(Vector2::new(0.0, 1.0));
let p = Point2::new(2.0, 3.0);
let rev = point_surf_jacobian_rev(&p, &SurfacePoint2::new(c, n), &values);
for i in [1usize, 2] {
assert_eq!(
rev[i], 0.0,
"locked parameter {i} should have a zero column"
);
}
assert_ne!(rev[0], 0.0);
}
}