use crate::Result;
use crate::na::{DMatrix, DVector, SVector};
pub trait BodyParams<const N: usize>: Clone {
type Point;
type Iso: Copy;
type Dof: Copy;
type Values;
fn from_posed_center(
center: &Self::Point,
start: Option<Self::Iso>,
dof: Option<Self::Dof>,
) -> Self;
fn set_storage(&mut self, storage: SVector<f64, N>);
fn compute_transform(&self) -> Self::Iso;
fn compute_values(&self) -> Self::Values;
}
#[derive(Clone, Debug)]
pub struct MultiParams<P: BodyParams<N>, const N: usize> {
static_i: usize,
bodies: Vec<P>,
storage: DVector<f64>,
}
impl<P: BodyParams<N>, const N: usize> MultiParams<P, N> {
pub fn new(static_i: usize, bodies: Vec<P>) -> Result<Self> {
if bodies.len() < 2 {
return Err(format!(
"a multi-body alignment needs at least two bodies, but {} were given",
bodies.len()
)
.into());
}
if static_i >= bodies.len() {
return Err(format!(
"the static body index {} is out of range for {} bodies",
static_i,
bodies.len()
)
.into());
}
let storage = DVector::zeros((bodies.len() - 1) * N);
let mut item = Self {
static_i,
bodies,
storage,
};
item.distribute();
Ok(item)
}
pub fn from_centers(
static_i: usize,
centers: &[P::Point],
initial: Option<&[P::Iso]>,
dof: Option<P::Dof>,
) -> Result<Self> {
if let Some(initial) = initial
&& initial.len() != centers.len()
{
return Err(format!(
"there are {} rotation centers but {} initial transforms",
centers.len(),
initial.len()
)
.into());
}
let bodies = centers
.iter()
.enumerate()
.map(|(i, c)| P::from_posed_center(c, initial.map(|t| t[i]), dof))
.collect();
Self::new(static_i, bodies)
}
pub fn body_count(&self) -> usize {
self.bodies.len()
}
pub fn param_count(&self) -> usize {
self.storage.len()
}
pub fn storage(&self) -> &DVector<f64> {
&self.storage
}
pub fn set_storage(&mut self, x: &DVector<f64>) {
self.storage.copy_from(x);
self.distribute();
}
pub fn column_offset(&self, body: usize) -> Option<usize> {
if body == self.static_i {
None
} else if body > self.static_i {
Some((body - 1) * N)
} else {
Some(body * N)
}
}
pub fn body(&self, body: usize) -> &P {
&self.bodies[body]
}
pub fn transform(&self, body: usize) -> P::Iso {
self.bodies[body].compute_transform()
}
pub fn compute_all_values(&self) -> Vec<P::Values> {
self.bodies.iter().map(|b| b.compute_values()).collect()
}
pub fn add_jacobian_block(
&self,
matrix: &mut DMatrix<f64>,
row: usize,
body: usize,
values: &SVector<f64, N>,
) {
if let Some(start) = self.column_offset(body) {
for (k, v) in values.iter().enumerate() {
matrix[(row, start + k)] += *v;
}
}
}
fn distribute(&mut self) {
for i in 0..self.bodies.len() {
if let Some(start) = self.column_offset(i) {
let slice = self.storage.fixed_rows::<N>(start).into_owned();
self.bodies[i].set_storage(slice);
}
}
}
}