use crate::common::align::multi_params::{BodyParams, MultiParams};
use crate::geom3::align3::{AlignOrigin3, AlignParams3, AlignStorage3, AlignValues3, Dof6};
use crate::geom3::{Iso3, Point3};
pub type MultiParams3 = MultiParams<AlignParams3, 6>;
impl BodyParams<6> for AlignParams3 {
type Point = Point3;
type Iso = Iso3;
type Dof = Dof6;
type Values = AlignValues3;
fn from_posed_center(center: &Point3, start: Option<Iso3>, dof: Option<Dof6>) -> Self {
let local = Iso3::translation(center.x, center.y, center.z);
let start = start.unwrap_or_else(Iso3::identity);
AlignParams3::new(AlignOrigin3::Local(local), Some(start * local), dof)
}
fn set_storage(&mut self, storage: AlignStorage3) {
AlignParams3::set_storage(self, storage);
}
fn compute_transform(&self) -> Iso3 {
AlignParams3::compute_transform(self)
}
fn compute_values(&self) -> AlignValues3 {
AlignParams3::compute_values(self)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::RandomGeometry3;
use crate::geom3::Vector3;
use crate::na::{DMatrix, DVector};
use approx::assert_relative_eq;
type Jacobian = DMatrix<f64>;
fn centers() -> Vec<Point3> {
vec![
Point3::new(1.0, 2.0, 3.0),
Point3::new(-4.0, 5.0, -6.0),
Point3::new(7.0, -8.0, 9.0),
Point3::origin(),
]
}
#[test]
fn the_static_body_contributes_no_parameters() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
assert_eq!(params.body_count(), 4);
assert_eq!(params.param_count(), 18);
assert_eq!(params.column_offset(1), None);
}
#[test]
fn columns_are_laid_out_in_body_order_skipping_the_static_one() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
assert_eq!(params.column_offset(0), Some(0));
assert_eq!(params.column_offset(1), None);
assert_eq!(params.column_offset(2), Some(6));
assert_eq!(params.column_offset(3), Some(12));
}
#[test]
fn a_static_body_at_index_zero_shifts_everything_down() {
let params = MultiParams3::from_centers(0, ¢ers(), None, None).unwrap();
assert_eq!(params.column_offset(0), None);
assert_eq!(params.column_offset(1), Some(0));
assert_eq!(params.column_offset(2), Some(6));
assert_eq!(params.column_offset(3), Some(12));
}
#[test]
fn initial_poses_are_reproduced_exactly_at_zero_parameters() {
let mut rg = RandomGeometry3::from_seed(0xb0d1e5);
let centers = centers();
for _ in 0..500 {
let initial: Vec<Iso3> = (0..centers.len()).map(|_| rg.iso3(10.0)).collect();
let params = MultiParams3::from_centers(2, ¢ers, Some(&initial), None).unwrap();
assert!(params.storage().iter().all(|v| *v == 0.0));
for (i, expected) in initial.iter().enumerate() {
assert_relative_eq!(
params.transform(i).to_matrix(),
expected.to_matrix(),
epsilon = 1e-10
);
}
}
}
#[test]
fn the_static_body_never_moves() {
let centers = centers();
let initial: Vec<Iso3> = vec![
Iso3::translation(1.0, 0.0, 0.0),
Iso3::translation(0.0, 2.0, 0.0),
Iso3::translation(0.0, 0.0, 3.0),
Iso3::identity(),
];
let mut params = MultiParams3::from_centers(1, ¢ers, Some(&initial), None).unwrap();
let before = params.transform(1);
params.set_storage(&DVector::from_element(params.param_count(), 0.35));
let after = params.transform(1);
assert_relative_eq!(before.to_matrix(), after.to_matrix(), epsilon = 1e-12);
}
#[test]
fn parameters_reach_the_body_they_belong_to() {
let centers = centers();
let mut params = MultiParams3::from_centers(1, ¢ers, None, None).unwrap();
let mut x = DVector::zeros(params.param_count());
x[params.column_offset(2).unwrap()] = 1.0;
params.set_storage(&x);
assert_relative_eq!(
params.transform(2).translation.vector,
Vector3::new(1.0, 0.0, 0.0),
epsilon = 1e-12
);
for other in [0usize, 1, 3] {
assert_relative_eq!(
params.transform(other).to_matrix(),
Iso3::identity().to_matrix(),
epsilon = 1e-12
);
}
}
#[test]
fn bodies_rotate_about_their_own_centers() {
let centers = centers();
let mut params = MultiParams3::from_centers(1, ¢ers, None, None).unwrap();
let mut x = DVector::zeros(params.param_count());
x[params.column_offset(0).unwrap() + 5] = 0.7;
params.set_storage(&x);
let moved = params.transform(0) * centers[0];
assert_relative_eq!(moved, centers[0], epsilon = 1e-12);
}
#[test]
fn locked_dof_are_enforced_on_every_body() {
let centers = centers();
let dof = Dof6::new(false, true, true, true, true, false);
let mut params = MultiParams3::from_centers(1, ¢ers, None, Some(dof)).unwrap();
params.set_storage(&DVector::from_element(params.param_count(), 0.5));
for body in [0usize, 2, 3] {
let storage = params.body(body).storage();
assert_eq!(storage[0], 0.0, "tx should be locked on body {body}");
assert_eq!(storage[5], 0.0, "rz should be locked on body {body}");
assert_eq!(storage[1], 0.5, "ty should be free on body {body}");
}
}
#[test]
fn jacobian_blocks_land_in_the_right_columns() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
let mut jac = Jacobian::zeros(3, params.param_count());
let values = AlignStorage3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
params.add_jacobian_block(&mut jac, 0, 2, &values);
let start = params.column_offset(2).unwrap();
for k in 0..6 {
assert_eq!(jac[(0, start + k)], values[k]);
}
assert_eq!(jac.row(0).iter().filter(|v| **v != 0.0).count(), 6);
assert_eq!(jac.row(1).iter().filter(|v| **v != 0.0).count(), 0);
}
#[test]
fn jacobian_blocks_for_the_static_body_are_dropped() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
let mut jac = Jacobian::zeros(1, params.param_count());
params.add_jacobian_block(&mut jac, 0, 1, &AlignStorage3::from_element(9.0));
assert!(jac.iter().all(|v| *v == 0.0));
}
#[test]
fn adding_jacobian_blocks_accumulates_rather_than_overwrites() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
let mut jac = Jacobian::zeros(1, params.param_count());
let values = AlignStorage3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
params.add_jacobian_block(&mut jac, 0, 0, &values);
params.add_jacobian_block(&mut jac, 0, 0, &values);
let start = params.column_offset(0).unwrap();
for k in 0..6 {
assert_eq!(jac[(0, start + k)], 2.0 * values[k]);
}
}
#[test]
fn compute_all_values_covers_every_body_in_order() {
let params = MultiParams3::from_centers(1, ¢ers(), None, None).unwrap();
let values = params.compute_all_values();
assert_eq!(values.len(), params.body_count());
for (i, v) in values.iter().enumerate() {
assert_relative_eq!(
v.transform.to_matrix(),
params.transform(i).to_matrix(),
epsilon = 1e-12
);
}
}
#[test]
fn degenerate_inputs_are_rejected() {
let one = vec![Point3::origin()];
assert!(MultiParams3::from_centers(0, &one, None, None).is_err());
let centers = centers();
assert!(MultiParams3::from_centers(9, ¢ers, None, None).is_err());
let short = vec![Iso3::identity(); 2];
assert!(MultiParams3::from_centers(0, ¢ers, Some(&short), None).is_err());
}
}