use coeus_core::{ComputeBackend, CpuAddressableStorage, MoiraiBackend};
use ritk_image::Image;
use ritk_spatial::{Direction, Point, Spacing};
use ritk_tensor_ops::native as tensor_ops;
#[derive(Debug, Clone)]
pub struct AtlasAffineTransform<B: ComputeBackend, const D: usize> {
matrix: Vec<f32>,
translation: Vec<f32>,
center: Vec<f32>,
_backend: std::marker::PhantomData<B>,
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum AtlasAffineError {
MatrixShape {
actual_len: usize,
expected_len: usize,
},
TranslationLength {
actual_len: usize,
expected_len: usize,
},
CenterLength {
actual_len: usize,
expected_len: usize,
},
Extract(String),
Construct(String),
}
impl std::fmt::Display for AtlasAffineError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
match self {
Self::MatrixShape { actual_len, expected_len } => write!(
f,
"AtlasAffineTransform::new matrix length mismatch: expected [{expected_len}] elements ([D, D] for D), got [{actual_len}] elements"
),
Self::TranslationLength { actual_len, expected_len } => write!(
f,
"AtlasAffineTransform::new translation length mismatch: expected [{expected_len}] elements ([D] for D), got [{actual_len}] elements"
),
Self::CenterLength { actual_len, expected_len } => write!(
f,
"AtlasAffineTransform::new center length mismatch: expected [{expected_len}] elements ([D] for D), got [{actual_len}] elements"
),
Self::Extract(s) => write!(f, "atlas affine transform_points extract failed: {s}"),
Self::Construct(s) => write!(f, "atlas affine transform_points carrier construction failed: {s}") }
}
}
impl std::error::Error for AtlasAffineError {}
impl<B: ComputeBackend, const D: usize> AtlasAffineTransform<B, D> {
pub fn try_new(
matrix: &[f32],
translation: &[f32],
center: &[f32],
) -> Result<Self, AtlasAffineError> {
let expected = D * D;
if matrix.len() != expected {
return Err(AtlasAffineError::MatrixShape {
actual_len: matrix.len(),
expected_len: expected,
});
}
if translation.len() != D {
return Err(AtlasAffineError::TranslationLength {
actual_len: translation.len(),
expected_len: D,
});
}
if center.len() != D {
return Err(AtlasAffineError::CenterLength {
actual_len: center.len(),
expected_len: D,
});
}
Ok(Self {
matrix: matrix.to_vec(),
translation: translation.to_vec(),
center: center.to_vec(),
_backend: std::marker::PhantomData,
})
}
pub fn construct(matrix: &[f32], translation: &[f32], center: &[f32]) -> Self {
Self::try_new(matrix, translation, center).expect("AtlasAffineTransform::construct shape")
}
pub fn identity(center: Option<&[f32]>) -> Self {
let mut matrix = vec![0.0f32; D * D];
for i in 0..D {
matrix[i * (D + 1)] = 1.0;
}
let translation = vec![0.0f32; D];
let center = center
.map(|c| c.to_vec())
.unwrap_or_else(|| vec![0.0f32; D]);
Self::construct(&matrix, &translation, ¢er)
}
pub fn matrix(&self) -> &[f32] {
&self.matrix
}
pub fn translation(&self) -> &[f32] {
&self.translation
}
pub fn center(&self) -> &[f32] {
&self.center
}
pub fn transform_points<BB>(
&self,
points: &Image<f32, BB, 2>,
) -> Result<Image<f32, MoiraiBackend, 2>, AtlasAffineError>
where
BB: ComputeBackend,
BB::DeviceBuffer<f32>: CpuAddressableStorage<f32>,
{
let (point_values, point_shape_2d) = tensor_ops::extract_image_slice(points)
.map_err(|e| AtlasAffineError::Extract(e.to_string()))?;
if point_shape_2d[1] != D {
return Err(AtlasAffineError::Construct(format!(
"atlas affine transform_points expected last-axis width {D}, got {:?}",
point_shape_2d
)));
}
let n = point_shape_2d[0];
let mut out_values = vec![0.0f32; n * D];
for i in 0..n {
for k in 0..D {
let mut acc = 0.0f32;
for j in 0..D {
let centered_j = point_values[i * D + j] - self.center[j];
acc += self.matrix[k * D + j] * centered_j;
}
out_values[i * D + k] = acc + self.center[k] + self.translation[k];
}
}
Image::<f32, MoiraiBackend, 2>::from_flat(
out_values,
point_shape_2d,
Point::<2>::origin(),
Spacing::<2>::uniform(1.0),
Direction::<2>::identity(),
)
.map_err(|e| AtlasAffineError::Construct(e.to_string()))
}
}
fn euler_rotation_matrix<const D: usize>(angles: &[f32]) -> Vec<f32> {
if D == 3 {
let (cx, sx) = (angles[0].cos(), angles[0].sin());
let (cy, sy) = (angles[1].cos(), angles[1].sin());
let (cz, sz) = (angles[2].cos(), angles[2].sin());
vec![
cz * cy,
cz * sy * sx - sz * cx,
cz * sy * cx + sz * sx,
sz * cy,
sz * sy * sx + cz * cx,
sz * sy * cx - cz * sx,
-sy,
cy * sx,
cy * cx,
]
} else if D == 2 {
let (c, s) = (angles[0].cos(), angles[0].sin());
vec![c, -s, s, c]
} else {
let mut m = vec![0.0f32; D * D];
for i in 0..D {
m[i * (D + 1)] = 1.0;
}
m
}
}
fn quaternion_rotation_matrix(quat: &[f32]) -> Vec<f32> {
const QUAT_NORM_GUARD: f32 = 1e-12;
let norm = quat[0]
.mul_add(
quat[0],
quat[1].mul_add(quat[1], quat[2].mul_add(quat[2], quat[3] * quat[3])),
)
.sqrt()
+ QUAT_NORM_GUARD;
let x = (quat[0] / norm) as f64;
let y = (quat[1] / norm) as f64;
let z = (quat[2] / norm) as f64;
let w = (quat[3] / norm) as f64;
let (xx, yy, zz) = (x * x, y * y, z * z);
let (xy, xz, yz) = (x * y, x * z, y * z);
let (xw, yw, zw) = (x * w, y * w, z * w);
[
1.0 - 2.0 * (yy + zz),
2.0 * (xy - zw),
2.0 * (xz + yw),
2.0 * (xy + zw),
1.0 - 2.0 * (xx + zz),
2.0 * (yz - xw),
2.0 * (xz - yw),
2.0 * (yz + xw),
1.0 - 2.0 * (xx + yy),
]
.into_iter()
.map(|v| v as f32)
.collect()
}
impl<B: ComputeBackend, const D: usize> AtlasAffineTransform<B, D> {
pub fn from_translation(translation: &[f32]) -> Self {
let mut matrix = vec![0.0f32; D * D];
for i in 0..D {
matrix[i * (D + 1)] = 1.0;
}
Self::construct(&matrix, translation, &vec![0.0f32; D])
}
pub fn from_scale(scale: &[f32], center: &[f32]) -> Self {
let mut matrix = vec![0.0f32; D * D];
for i in 0..D {
matrix[i * (D + 1)] = scale[i];
}
Self::construct(&matrix, &vec![0.0f32; D], center)
}
pub fn from_euler_rigid(translation: &[f32], rotation: &[f32], center: &[f32]) -> Self {
let matrix = euler_rotation_matrix::<D>(rotation);
Self::construct(&matrix, translation, center)
}
}
impl<B: ComputeBackend> AtlasAffineTransform<B, 3> {
pub fn from_versor(translation: &[f32], quaternion: &[f32], center: &[f32]) -> Self {
let matrix = quaternion_rotation_matrix(quaternion);
Self::construct(&matrix, translation, center)
}
}