diff --git a/CHANGELOG.md b/CHANGELOG.md index 46e5dd2..cec28d0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,10 +6,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [unreleased] +## [0.7.1 - 2026-05-14] + ### Added - New frame-tagged 3×3 matrix primitives in `affn::matrix3`: `FrameMatrix3` for general matrices and `SymmetricFrameMatrix3` for symmetry-preserving covariance-style blocks. - Rotation similarity helpers on the new matrix types: `rotated_by::(&Rotation3)` for frame-changing `R · M · Rᵀ` transforms, with numerical re-symmetrization for `SymmetricFrameMatrix3`. - `Rotation3::apply_vec` for rotating typed `Vector`/`Displacement` values while preserving units and retagging the output frame. +- New frame-tagged 6×6 matrix primitives in `affn::matrix6`: `FrameMatrix6` for general 6×6 matrices and `BlockDiagRotation6` for block-diagonal `blockdiag(R, R)` frame-change transforms on `[position; velocity]` state vectors. Uses the instantaneous-rotation convention (time-derivative `Ṙ` ignored), which is correct for covariance transport and fixed-epoch STM frame changes. +- Additional helpers on `FrameMatrix3`: `from_diagonal`, matrix–matrix product `mat_mul`, transpose `transpose`, and frame-retag `retag`. +- `Acceleration` semantic type alias for `Vector` where `U` is an acceleration unit, representing the second time-derivative of position. Available from `affn::cartesian`, the crate root, and `affn::prelude`. +- `Force` semantic type alias for `Vector` where `U` is a force unit (e.g. `Newton`, `Kilonewton`), representing the physical cause of acceleration via Newton's second law. Available from `affn::cartesian`, the crate root, and `affn::prelude`. ### Changed - `Position` now implements `PartialEq`, comparing both Cartesian coordinates and `center_params` so affine points only compare equal when they represent the same location relative to the same parameterized center state. diff --git a/Cargo.lock b/Cargo.lock index 089f5ff..03ef12b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -4,7 +4,7 @@ version = 4 [[package]] name = "affn" -version = "0.7.0" +version = "0.7.1" dependencies = [ "affn-derive", "criterion", @@ -15,7 +15,7 @@ dependencies = [ [[package]] name = "affn-derive" -version = "0.7.0" +version = "0.7.1" dependencies = [ "proc-macro2", "quote", @@ -372,9 +372,9 @@ dependencies = [ [[package]] name = "qtty" -version = "0.7.1" +version = "0.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f529220548fba3c86bdf30115782d93aa8bc80963ba30d1763f8e6037a77f295" +checksum = "61c018436452aeeb42be95367e44877b9237508908ee2d0f0ea6c13bcb969dea" dependencies = [ "qtty-core", "qtty-derive", @@ -382,9 +382,9 @@ dependencies = [ [[package]] name = "qtty-core" -version = "0.7.1" +version = "0.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c53264e49b419b9a4f39d1d51ddc10857c579568ae54fe8effbb43e5f832cbc2" +checksum = "1e4049b4883c30d10d9febc7a24088c4a5690e0f9fbdb63d91ab679352040744" dependencies = [ "libm", "qtty-derive", @@ -394,9 +394,9 @@ dependencies = [ [[package]] name = "qtty-derive" -version = "0.7.1" +version = "0.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a4eeedf0593d54273f6d67b46b4f381901290728327e0006b40c624cfaf0dfe8" +checksum = "5c3ddfe91f467976a8bd7cab95750e3ffab4eb1b2875bb939fd9466e02790300" dependencies = [ "proc-macro2", "quote", diff --git a/Cargo.toml b/Cargo.toml index 8e43ef9..4518055 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -2,7 +2,7 @@ [package] name = "affn" -version = "0.7.0" +version = "0.7.1" edition = "2021" authors = ["VPRamon "] description = "Affine geometry primitives: strongly-typed coordinate systems, reference frames, and centers for scientific computing." @@ -21,8 +21,8 @@ serde = ["dep:serde", "qtty/serde"] astro = ["qtty/astro"] [dependencies] -affn-derive = { version = "0.7.0", path = "affn-derive" } -qtty = "0.7.1" +affn-derive = { version = "0.7.1", path = "affn-derive" } +qtty = "0.8" serde = { version = "1.0", default-features = false, features = ["derive"], optional = true } [dev-dependencies] diff --git a/README.md b/README.md index 25e9a49..70d8393 100644 --- a/README.md +++ b/README.md @@ -33,8 +33,8 @@ Add the dependency: ```toml [dependencies] -affn = "0.7.0" -qtty = "0.7.1" +affn = "0.7.1" +qtty = "0.8.1" ``` Define a center + frame and do basic affine algebra: diff --git a/affn-derive/Cargo.toml b/affn-derive/Cargo.toml index 8b8dbf5..4cc6f80 100644 --- a/affn-derive/Cargo.toml +++ b/affn-derive/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "affn-derive" -version = "0.7.0" +version = "0.7.1" edition = "2021" authors = ["VPRamon "] description = "Derive macros for affn: ReferenceFrame and ReferenceCenter" diff --git a/affn-derive/src/center.rs b/affn-derive/src/center.rs index 8ebc367..f2ec7c3 100644 --- a/affn-derive/src/center.rs +++ b/affn-derive/src/center.rs @@ -69,40 +69,61 @@ pub(crate) fn parse_center_attributes(input: &DeriveInput) -> syn::Result
{ + if nv.path.is_ident("name") { + if let Expr::Lit(expr_lit) = &nv.value { + if let Lit::Str(lit_str) = &expr_lit.lit { + attrs.name = Some(lit_str.value()); + continue; + } } - } - return Err(syn::Error::new_spanned( - &nv.value, - "expected string literal for `name`", - )); - } else if nv.path.is_ident("params") { - if let Expr::Path(expr_path) = &nv.value { - attrs.params = Some(Type::Path(syn::TypePath { - qself: None, - path: expr_path.path.clone(), - })); - continue; - } - return Err(syn::Error::new_spanned( - &nv.value, - "expected type for `params`", - )); - } else if nv.path.is_ident("affine") { - if let Expr::Lit(expr_lit) = &nv.value { - if let Lit::Bool(lit_bool) = &expr_lit.lit { - attrs.affine = lit_bool.value(); + return Err(syn::Error::new_spanned( + &nv.value, + "expected string literal for `name`", + )); + } else if nv.path.is_ident("params") { + if let Expr::Path(expr_path) = &nv.value { + attrs.params = Some(Type::Path(syn::TypePath { + qself: None, + path: expr_path.path.clone(), + })); continue; } + return Err(syn::Error::new_spanned( + &nv.value, + "expected type for `params`", + )); + } else if nv.path.is_ident("affine") { + if let Expr::Lit(expr_lit) = &nv.value { + if let Lit::Bool(lit_bool) = &expr_lit.lit { + attrs.affine = lit_bool.value(); + continue; + } + } + return Err(syn::Error::new_spanned( + &nv.value, + "expected boolean for `affine`", + )); + } else { + return Err(syn::Error::new_spanned( + &nv.path, + format!( + "unknown `center` attribute `{}`; \ + known attributes are: name, params, affine", + nv.path + .get_ident() + .map(|id| id.to_string()) + .unwrap_or_else(|| "".into()) + ), + )); } + } + _ => { return Err(syn::Error::new_spanned( - &nv.value, - "expected boolean for `affine`", + &meta, + "unknown `center` attribute; expected a name=value attribute \ + such as `name = \"...\"`, `params = MyType`, or `affine = true`", )); } } diff --git a/affn-derive/src/frame.rs b/affn-derive/src/frame.rs index db6b6c3..4994c28 100644 --- a/affn-derive/src/frame.rs +++ b/affn-derive/src/frame.rs @@ -284,9 +284,30 @@ pub(crate) fn parse_frame_attributes(input: &DeriveInput) -> syn::Result".into()) + ), + )); } } - _ => {} + _ => { + return Err(syn::Error::new_spanned( + &meta, + "unknown `frame` attribute; expected a name=value attribute \ + such as `name = \"...\"`, `polar = \"...\"`, \ + `azimuth = \"...\"`, `distance = \"...\"`, \ + `ellipsoid = \"...\"`, or the bare flag `inherent`", + )); + } } } } diff --git a/src/cartesian/mod.rs b/src/cartesian/mod.rs index 9644398..4d78739 100644 --- a/src/cartesian/mod.rs +++ b/src/cartesian/mod.rs @@ -114,6 +114,6 @@ pub use xyz::XYZ; pub use direction::Direction; pub use position::{CenterParamsMismatchError, Position}; -pub use vector::{Displacement, Vector, Velocity}; +pub use vector::{Acceleration, Displacement, Force, Vector, Velocity}; pub use line_of_sight::{line_of_sight, line_of_sight_with_distance, try_line_of_sight}; diff --git a/src/cartesian/vector.rs b/src/cartesian/vector.rs index 62434de..bd7915c 100644 --- a/src/cartesian/vector.rs +++ b/src/cartesian/vector.rs @@ -389,6 +389,20 @@ pub type Displacement = Vector; /// Velocities represent rates of change of position. pub type Velocity = Vector; +/// An acceleration vector (free vector with acceleration unit). +/// +/// This is a semantic alias for [`Vector`] where `U` is an acceleration +/// unit (e.g. `Per, Second>`). +/// Accelerations represent the second time-derivative of position. +pub type Acceleration = Vector; + +/// A force vector (free vector with force unit). +/// +/// This is a semantic alias for [`Vector`] where `U` is a force unit +/// (e.g. `Newton`, `Kilonewton`). +/// Forces represent the physical cause of acceleration via Newton's second law. +pub type Force = Vector; + // ============================================================================= // Tests // ============================================================================= diff --git a/src/frames/astro.rs b/src/frames/astro.rs index 116f3cd..ddf2e75 100644 --- a/src/frames/astro.rs +++ b/src/frames/astro.rs @@ -10,49 +10,10 @@ use crate::ops::Rotation3; use crate::DeriveReferenceFrame; -use qtty::angular::Radians; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -// ============================================================================= -// IAU 2006 frame-bias constants (IERS Conventions 2010, §5.4.4, Table 5.2b) -// ============================================================================= -// -// These are the three small angles that define the constant rotation -// relating the GCRS / ICRS axes to the **mean** equator and equinox of -// J2000.0 (i.e. the EquatorialMeanJ2000 / EME2000 / FK5-J2000 axes): -// -// ξ₀ = −0.0166170″ (offset of CIP at J2000 from mean pole, x) -// η₀ = −0.0068192″ (offset of CIP at J2000 from mean pole, y) -// dα₀ = −0.0146″ (offset of ICRS RA origin from mean equinox) -// -// Per IERS Conventions (2010), Eq. (5.32), the bias matrix is -// -// B = R1(−η₀) · R2( ξ₀) · R3(dα₀) -// -// where R1, R2, R3 are the *passive* (frame-rotating) elementary rotations -// of Eq. (5.4). In affn `Rotation3::r{x,y,z}` are the *active* rotations, -// so `R_i(θ) = Rotation3::r{x,y,z}(−θ)`, giving the equivalent form -// -// B = Rotation3::rx( η₀) · Rotation3::ry(−ξ₀) · Rotation3::rz(−dα₀) -// -// The resulting matrix agrees with SOFA `iauBp06` `rb` to ≲1e-15. - -const FRAME_BIAS_DALPHA0_ARCSEC: f64 = -0.0146; -const FRAME_BIAS_XI0_ARCSEC: f64 = -0.0166170; -const FRAME_BIAS_ETA0_ARCSEC: f64 = -0.0068192; - -const ARCSEC_TO_RAD: f64 = std::f64::consts::PI / 648_000.0; - -#[inline] -fn frame_bias_gcrs_to_eme2000() -> Rotation3 { - let xi0 = Radians::new(FRAME_BIAS_XI0_ARCSEC * ARCSEC_TO_RAD); - let eta0 = Radians::new(FRAME_BIAS_ETA0_ARCSEC * ARCSEC_TO_RAD); - let da0 = Radians::new(FRAME_BIAS_DALPHA0_ARCSEC * ARCSEC_TO_RAD); - Rotation3::rx(eta0) * Rotation3::ry(-xi0) * Rotation3::rz(-da0) -} - // ============================================================================= // Equatorial frames (ra/dec) // ============================================================================= @@ -76,7 +37,8 @@ fn frame_bias_gcrs_to_eme2000() -> Rotation3 { /// | `ICRS` ↔ [`EME2000`] | IAU 2006 frame bias **B** | ≈ 23 mas | /// /// The frame-bias rotation between `ICRS` and `EME2000` is the same matrix -/// that connects [`GCRS`] to [`EME2000`] (see [`GCRS::frame_bias_to_eme2000`]). +/// that connects [`GCRS`] to [`EME2000`]. The bias constants and +/// transform methods live in `siderust::astro::frame_bias`. /// /// # References /// * IAU 1997 Resolution B2 (definition of the ICRS). @@ -146,8 +108,8 @@ pub struct EquatorialMeanJ2000; /// /// (IERS Conventions 2010, Table 5.2b). The angular magnitude of `B` is /// ≈ 23 mas (≈ 1.1 × 10⁻⁷ rad). Because `B` is epoch-independent, no time -/// argument is needed to convert between `GCRS` and `EME2000`; see -/// [`EME2000::frame_bias_to_gcrs`] and [`GCRS::frame_bias_to_eme2000`]. +/// argument is needed to convert between `GCRS` and `EME2000`. The bias +/// constants and transform methods live in `siderust::astro::frame_bias`. /// /// # References /// * IERS Conventions (2010), §5.4.4 and Table 5.2b. @@ -198,7 +160,7 @@ pub struct EquatorialTrueOfDate; /// differ by the constant **IAU 2006 frame-bias rotation** `B` of /// magnitude ≈ 23 mas, originating from the small offset between the /// ICRS pole / equinox and the dynamical mean pole / equinox at J2000.0. -/// See [`GCRS::frame_bias_to_eme2000`]. +/// The bias constants and transform methods live in `siderust::astro::frame_bias`. /// /// # CIO-based reduction chain /// @@ -222,11 +184,9 @@ pub struct GCRS; // Canonical frame relationships among ICRS / ICRF / GCRS / EME2000 // ============================================================================= // -// These inherent methods expose the *direction-only* rotation that connects -// each pair, so callers can verify and use the relationship without going -// through a higher-level transform pipeline. All matrices are constants -// (epoch-independent), as required by IAU 2006 / IERS Conventions 2010 -// §5.4.4 for the frame bias. +// These inherent methods expose the *direction-only* (identity) rotation that +// connects each pair. Frame-bias methods (GCRS ↔ EME2000, ICRS ↔ EME2000) +// are domain-specific and live in `siderust::astro::frame_bias`. impl ICRS { /// Direction-frame rotation from `ICRS` to [`ICRF`]. @@ -251,15 +211,6 @@ impl ICRS { pub fn direction_rotation_to_gcrs() -> Rotation3 { Rotation3::IDENTITY } - - /// Frame-bias rotation `B` from `ICRS` to [`EME2000`]. - /// - /// Identical to [`GCRS::frame_bias_to_eme2000`]. Magnitude ≈ 23 mas. - #[inline] - #[must_use] - pub fn frame_bias_to_eme2000() -> Rotation3 { - frame_bias_gcrs_to_eme2000() - } } impl ICRF { @@ -287,49 +238,6 @@ impl GCRS { pub fn direction_rotation_to_icrs() -> Rotation3 { Rotation3::IDENTITY } - - /// IAU 2006 frame-bias rotation `B` from [`GCRS`] to [`EME2000`]. - /// - /// Built from the IERS Conventions (2010) Table 5.2b angles - /// - /// ```text - /// ξ₀ = −0.0166170″ - /// η₀ = −0.0068192″ - /// dα₀ = −0.0146″ - /// ``` - /// - /// using the parametrisation of Eq. (5.32) of the same document, - /// - /// ```text - /// B = R1(−η₀) · R2(ξ₀) · R3(dα₀). - /// ``` - /// - /// The matrix is **constant** (epoch-independent) and its rotation - /// angle is approximately 23 milli-arcseconds (≈ 1.1 × 10⁻⁷ rad). - /// It agrees with SOFA `iauBp06` `rb` at J2000.0 to ≲ 1 × 10⁻¹⁵. - #[inline] - #[must_use] - pub fn frame_bias_to_eme2000() -> Rotation3 { - frame_bias_gcrs_to_eme2000() - } -} - -impl EME2000 { - /// Inverse IAU 2006 frame-bias rotation `Bᵀ` from [`EME2000`] to - /// [`GCRS`]. Exact algebraic inverse of [`GCRS::frame_bias_to_eme2000`]. - #[inline] - #[must_use] - pub fn frame_bias_to_gcrs() -> Rotation3 { - frame_bias_gcrs_to_eme2000().inverse() - } - - /// Inverse IAU 2006 frame-bias rotation from [`EME2000`] to [`ICRS`]. - /// Identical to [`EME2000::frame_bias_to_gcrs`] for direction purposes. - #[inline] - #[must_use] - pub fn frame_bias_to_icrs() -> Rotation3 { - frame_bias_gcrs_to_eme2000().inverse() - } } /// Celestial Intermediate Reference System (CIRS). diff --git a/src/lib.rs b/src/lib.rs index bf07546..f167a40 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -47,6 +47,24 @@ //! assert_eq!(MyCenter::center_name(), "MyCenter"); //! ``` //! +//! Typos in derive attributes are compile errors: +//! +//! ```compile_fail +//! use affn::prelude::*; +//! +//! #[derive(Debug, Copy, Clone, ReferenceFrame)] +//! #[frame(azimth = "Equatorial")] // typo: should be "name" +//! struct BadFrame; +//! ``` +//! +//! ```compile_fail +//! use affn::prelude::*; +//! +//! #[derive(Debug, Copy, Clone, ReferenceCenter)] +//! #[center(nme = "Earth")] // typo: should be "name" +//! struct BadCenter; +//! ``` +//! //! ## Algebraic Rules //! //! The type system enforces mathematical correctness: @@ -126,6 +144,9 @@ pub(crate) mod serde_utils; // Frame-tagged 3×3 matrix primitives pub mod matrix3; +// Frame-tagged 6×6 matrix and block-diagonal rotation helper +pub mod matrix6; + // Re-export derive macros from affn-derive // Named with Derive prefix to avoid conflicts with trait names pub use affn_derive::{ @@ -142,8 +163,8 @@ pub use ops::{Isometry3, Rotation3, Translation3}; // Re-export concrete Position/Direction types for standalone usage pub use cartesian::{ - CenterParamsMismatchError, Direction as CartesianDirection, Displacement, Position, Vector, - Velocity, + Acceleration, CenterParamsMismatchError, Direction as CartesianDirection, Displacement, Force, + Position, Vector, Velocity, }; pub use conic::{ ClassifiedPeriapsisParam, ClassifiedSemiMajorAxisParam, ConicKind, ConicOrientation, @@ -175,7 +196,8 @@ pub mod prelude { // Core coordinate types pub use crate::cartesian::{ - Direction as CartesianDirection, Displacement, Position, Vector, Velocity, + Acceleration, Direction as CartesianDirection, Displacement, Force, Position, Vector, + Velocity, }; pub use crate::conic::{ ClassifiedPeriapsisParam, ClassifiedSemiMajorAxisParam, ConicKind, ConicOrientation, diff --git a/src/matrix3.rs b/src/matrix3.rs index 54d25c0..faafc9b 100644 --- a/src/matrix3.rs +++ b/src/matrix3.rs @@ -56,6 +56,7 @@ //! ``` use core::marker::PhantomData; +use core::ops::Mul; use crate::ops::Rotation3; @@ -133,6 +134,152 @@ impl FrameMatrix3 { Self::from_array(d) } + /// Diagonal matrix with the given elements on the diagonal and zeros elsewhere. + pub fn from_diagonal(diag: [f64; 3]) -> Self { + let mut d = [[0.0_f64; 3]; 3]; + for i in 0..3 { + d[i][i] = diag[i]; + } + Self::from_array(d) + } + + /// Matrix–matrix product `self · rhs`, frame-tag preserving. + /// + /// Both operands must be expressed in the same frame `F`. + pub fn mat_mul(&self, rhs: &Self) -> Self { + let a = &self.data; + let b = &rhs.data; + let mut out = [[0.0_f64; 3]; 3]; + for (i, out_row) in out.iter_mut().enumerate() { + for (k, &aik) in a[i].iter().enumerate() { + if aik == 0.0 { + continue; + } + for (j, out_elt) in out_row.iter_mut().enumerate() { + *out_elt += aik * b[k][j]; + } + } + } + FrameMatrix3::from_array(out) + } + + /// Add `other` into `self` element-wise, in place. + pub fn add_in_place(&mut self, other: &FrameMatrix3) { + for i in 0..3 { + for j in 0..3 { + self.data[i][j] += other.data[i][j]; + } + } + } + + /// Multiply every element by the scalar `s`, in place. + pub fn scale_in_place(&mut self, s: f64) { + for row in &mut self.data { + for v in row.iter_mut() { + *v *= s; + } + } + } + + /// Add the outer product `a ⊗ b` (i.e. `aᵢ · bⱼ`) into `self` in place. + pub fn add_outer_product_in_place(&mut self, a: [f64; 3], b: [f64; 3]) { + for (i, data_row) in self.data.iter_mut().enumerate() { + for (j, data_elt) in data_row.iter_mut().enumerate() { + *data_elt += a[i] * b[j]; + } + } + } + + // ------------------------------------------------------------------------- + // Similarity transforms + // ------------------------------------------------------------------------- + + /// General similarity transform `self · m · selfᵀ`, relabelling the result + /// as frame `G`. + /// + /// Treat `self` as a rotation matrix `R : F → G` (rows of `self` are the + /// basis vectors of `G` expressed in `F`). Given a general matrix `m` in + /// frame `F`, this computes `R · m · Rᵀ` and tags the result as belonging + /// to frame `G`. + /// + /// For covariance-like inputs that must remain symmetric, use + /// [`FrameMatrix3::similarity`] instead, which also numerically re-symmetrises + /// the output. + pub fn similarity_general(&self, m: &FrameMatrix3) -> FrameMatrix3 { + let r = &self.data; + let mut tmp = [[0.0_f64; 3]; 3]; + for (i, tmp_row) in tmp.iter_mut().enumerate() { + for (k, &rik) in r[i].iter().enumerate() { + if rik == 0.0 { + continue; + } + for (j, tmp_elt) in tmp_row.iter_mut().enumerate() { + *tmp_elt += rik * m.data[k][j]; + } + } + } + // result = tmp · Rᵀ (Rᵀ[k][j] = R[j][k]) + let mut res = [[0.0_f64; 3]; 3]; + for (i, res_row) in res.iter_mut().enumerate() { + for (k, &tik) in tmp[i].iter().enumerate() { + if tik == 0.0 { + continue; + } + for (j, res_elt) in res_row.iter_mut().enumerate() { + *res_elt += tik * r[j][k]; + } + } + } + FrameMatrix3::from_array(res) + } + + /// Symmetric similarity transform `self · m · selfᵀ`, returning a + /// [`SymmetricFrameMatrix3`]. + /// + /// Treat `self` as a rotation matrix `R : F → G`. Given a symmetric matrix + /// `m` in frame `F`, the result `R · m · Rᵀ` is algebraically symmetric. A + /// final symmetrisation step averages `(result[i][j] + result[j][i]) / 2` + /// to prevent floating-point drift. + /// + /// For a general (non-symmetric) matrix, use [`FrameMatrix3::similarity_general`]. + pub fn similarity(&self, m: &SymmetricFrameMatrix3) -> SymmetricFrameMatrix3 { + // Compute R · m · Rᵀ via two passes. + let r = &self.data; + let mut tmp = [[0.0_f64; 3]; 3]; + for (i, tmp_row) in tmp.iter_mut().enumerate() { + for (k, &rik) in r[i].iter().enumerate() { + if rik == 0.0 { + continue; + } + for (j, tmp_elt) in tmp_row.iter_mut().enumerate() { + *tmp_elt += rik * m.data[k][j]; + } + } + } + let mut raw = [[0.0_f64; 3]; 3]; + for (i, raw_row) in raw.iter_mut().enumerate() { + for (k, &tik) in tmp[i].iter().enumerate() { + if tik == 0.0 { + continue; + } + for (j, raw_elt) in raw_row.iter_mut().enumerate() { + *raw_elt += tik * r[j][k]; + } + } + } + // Symmetrize to guard against floating-point drift. + let mut data = [[0.0_f64; 3]; 3]; + for (i, row) in data.iter_mut().enumerate() { + for (j, slot) in row.iter_mut().enumerate() { + *slot = 0.5 * (raw[i][j] + raw[j][i]); + } + } + SymmetricFrameMatrix3 { + data, + _frame: PhantomData, + } + } + /// Compute the similarity transform `R · self · Rᵀ`, tagging the result /// as belonging to frame `G`. /// @@ -170,6 +317,28 @@ impl FrameMatrix3 { } } +// --------------------------------------------------------------------------- +// Operator overloads for FrameMatrix3 +// --------------------------------------------------------------------------- + +/// Matrix multiplication `A * B → A·B` (frame-tag preserving). +impl Mul> for FrameMatrix3 { + type Output = FrameMatrix3; + #[inline] + fn mul(self, rhs: FrameMatrix3) -> FrameMatrix3 { + self.mat_mul(&rhs) + } +} + +/// Matrix multiplication by reference `&A * &B → A·B`. +impl Mul<&FrameMatrix3> for &FrameMatrix3 { + type Output = FrameMatrix3; + #[inline] + fn mul(self, rhs: &FrameMatrix3) -> FrameMatrix3 { + self.mat_mul(rhs) + } +} + // ============================================================================= // SymmetricFrameMatrix3 // ============================================================================= @@ -258,6 +427,39 @@ impl SymmetricFrameMatrix3 { Self::from_diagonal([1.0, 1.0, 1.0]) } + /// Add `other` into `self` element-wise, in place. + /// + /// The caller is responsible for ensuring `other` is symmetric; no + /// additional symmetrisation is performed. + pub fn add_in_place(&mut self, other: &SymmetricFrameMatrix3) { + for i in 0..3 { + for j in 0..3 { + self.data[i][j] += other.data[i][j]; + } + } + } + + /// Multiply every element by the scalar `s`, in place. + pub fn scale_in_place(&mut self, s: f64) { + for row in &mut self.data { + for v in row.iter_mut() { + *v *= s; + } + } + } + + /// Add the symmetric outer product `a ⊗ aᵀ` (i.e. `aᵢ · aⱼ`) into `self` + /// in place. + /// + /// Because `a ⊗ aᵀ` is always symmetric, the result remains symmetric. + pub fn add_outer_product_in_place(&mut self, a: [f64; 3]) { + for i in 0..3 { + for j in 0..3 { + self.data[i][j] += a[i] * a[j]; + } + } + } + /// Compute `R · self · Rᵀ`, tagging the result as belonging to frame `G`. /// /// For a rotation `R` and symmetric `self`, the result `R·M·Rᵀ` is @@ -490,4 +692,128 @@ mod tests { "trace changed: {trace_out} != {trace_in}" ); } + + // ------------------------------------------------------------------------- + // New algebra: mat_mul, from_diagonal, similarity, in-place ops + // ------------------------------------------------------------------------- + + #[test] + fn frame_matrix3_from_diagonal() { + let m = FrameMatrix3::::from_diagonal([2.0, 3.0, 5.0]); + let a = m.as_array(); + assert_eq!(a[0][0], 2.0); + assert_eq!(a[1][1], 3.0); + assert_eq!(a[2][2], 5.0); + assert_eq!(a[0][1], 0.0); + assert_eq!(a[1][2], 0.0); + } + + #[test] + fn frame_matrix3_mat_mul_identity() { + // A * I = A + let data = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]; + let a = FrameMatrix3::::from_array(data); + let eye = FrameMatrix3::::identity(); + let result = a.mat_mul(&eye); + for (i, row) in result.as_array().iter().enumerate() { + for (j, v) in row.iter().enumerate() { + assert!( + (*v - data[i][j]).abs() < 1e-14, + "A*I mismatch at [{i}][{j}]: {v} != {}", + data[i][j] + ); + } + } + } + + #[test] + fn frame_matrix3_mat_mul_known() { + // A = [[1,2],[3,4],[0,0]] (3x3 padded), B = [[5,6],[7,8],[0,0]] (padded) + // A·B[0][0] = 1*5 + 2*7 + 0 = 19; A·B[0][1] = 1*6 + 2*8 = 22 + // A·B[1][0] = 3*5 + 4*7 = 43; A·B[1][1] = 3*6 + 4*8 = 50 + let a = FrameMatrix3::::from_array([[1.0, 2.0, 0.0], [3.0, 4.0, 0.0], [0.0, 0.0, 1.0]]); + let b = FrameMatrix3::::from_array([[5.0, 6.0, 0.0], [7.0, 8.0, 0.0], [0.0, 0.0, 1.0]]); + let c = a.mat_mul(&b); + assert!((c.as_array()[0][0] - 19.0).abs() < 1e-14); + assert!((c.as_array()[0][1] - 22.0).abs() < 1e-14); + assert!((c.as_array()[1][0] - 43.0).abs() < 1e-14); + assert!((c.as_array()[1][1] - 50.0).abs() < 1e-14); + assert!((c.as_array()[2][2] - 1.0).abs() < 1e-14); + } + + #[test] + fn frame_matrix3_mul_operator() { + let eye = FrameMatrix3::::identity(); + let result = eye * eye; + assert_eq!(result.as_array(), eye.as_array()); + } + + #[test] + fn frame_matrix3_similarity_general_identity_rotation() { + let data = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]; + let m = FrameMatrix3::::from_array(data); + let eye = FrameMatrix3::::identity(); + let result: FrameMatrix3 = eye.similarity_general(&m); + for (i, row) in result.as_array().iter().enumerate() { + for (j, v) in row.iter().enumerate() { + assert!( + (*v - data[i][j]).abs() < 1e-14, + "sim_general identity failed at [{i}][{j}]" + ); + } + } + } + + #[test] + fn frame_matrix3_similarity_round_trip() { + use qtty::angular::Radians; + // rotate diagonal cov by 45° around Z, then back + let m = SymmetricFrameMatrix3::::from_diagonal([1.0, 4.0, 9.0]); + let r45 = Rotation3::rz(Radians::new(std::f64::consts::FRAC_PI_4)); + let r45_mat = FrameMatrix3::::from_array(*r45.as_matrix()); + let rotated: SymmetricFrameMatrix3 = r45_mat.similarity(&m); + + let r45_inv = r45.inverse(); + let r45_inv_mat = FrameMatrix3::::from_array(*r45_inv.as_matrix()); + let back: SymmetricFrameMatrix3 = r45_inv_mat.similarity(&rotated); + let orig = m.as_array(); + let result = back.as_array(); + for i in 0..3 { + for j in 0..3 { + assert!( + (result[i][j] - orig[i][j]).abs() < 1e-12, + "round-trip similarity failed at [{i}][{j}]: {} != {}", + result[i][j], + orig[i][j] + ); + } + } + } + + #[test] + fn frame_matrix3_in_place_ops() { + let mut m = FrameMatrix3::::from_diagonal([1.0, 2.0, 3.0]); + let other = FrameMatrix3::::from_diagonal([1.0, 1.0, 1.0]); + m.add_in_place(&other); + assert_eq!(m.as_array()[0][0], 2.0); + assert_eq!(m.as_array()[1][1], 3.0); + assert_eq!(m.as_array()[2][2], 4.0); + m.scale_in_place(2.0); + assert_eq!(m.as_array()[0][0], 4.0); + m.add_outer_product_in_place([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]); + assert_eq!(m.as_array()[0][1], 1.0); + } + + #[test] + fn symmetric_in_place_ops() { + let mut m = SymmetricFrameMatrix3::::from_diagonal([1.0, 2.0, 3.0]); + let other = SymmetricFrameMatrix3::::from_diagonal([1.0, 1.0, 1.0]); + m.add_in_place(&other); + assert_eq!(m.diagonal(), [2.0, 3.0, 4.0]); + m.scale_in_place(0.5); + assert_eq!(m.diagonal(), [1.0, 1.5, 2.0]); + m.add_outer_product_in_place([1.0, 0.0, 0.0]); + assert_eq!(m.as_array()[0][0], 2.0); + assert_eq!(m.as_array()[0][1], 0.0); // stays symmetric + } } diff --git a/src/matrix6.rs b/src/matrix6.rs new file mode 100644 index 0000000..82d9793 --- /dev/null +++ b/src/matrix6.rs @@ -0,0 +1,609 @@ +// SPDX-License-Identifier: AGPL-3.0-only +// Copyright (C) 2026 Vallés Puig, Ramon + +//! Frame-tagged 6×6 matrix and block-diagonal rotation helper. +//! +//! This module provides two types for typed 6-dimensional state-space algebra: +//! +//! - [`FrameMatrix6`] — a general 6×6 matrix stored as a flat `[[f64;6];6]` +//! array, tagged with the reference frame `F`. Supports constructors, block +//! accessors, matrix multiplication, transpose, and frame relabelling. +//! +//! - [`BlockDiagRotation6`] — a helper for the block-diagonal rotation +//! `T = blockdiag(R, R)` that arises when rotating a 6-dimensional Cartesian +//! state-space covariance or state-transition matrix between frames. All +//! block operations are performed as independent 3×3 similarity transforms, +//! avoiding the explicit 6×6 assembly. +//! +//! ## Convention: instantaneous rotation +//! +//! Both types implement the **instantaneous** convention: the rotation basis +//! is treated as constant at the moment of evaluation. The time-derivative +//! `Ṙ` of the rotation (which would introduce Coriolis-like off-diagonal +//! blocks) is *ignored*. This is correct for covariance transport and for +//! STM frame changes when the rotation is evaluated at a fixed epoch. +//! +//! ## Block layout +//! +//! All 6×6 types use `[r, v]` ordering: +//! +//! ```text +//! M = [ top_left top_right ] rows/cols 0..2 = position +//! [ bot_left bot_right ] rows/cols 3..5 = velocity +//! ``` +//! +//! ## Example +//! +//! ```rust +//! use affn::matrix6::{FrameMatrix6, BlockDiagRotation6}; +//! use affn::matrix3::{FrameMatrix3, SymmetricFrameMatrix3}; +//! use affn::ops::Rotation3; +//! use qtty::angular::Radians; +//! +//! #[derive(Debug, Copy, Clone)] +//! struct F; +//! impl affn::frames::ReferenceFrame for F { +//! fn frame_name() -> &'static str { "F" } +//! } +//! +//! #[derive(Debug, Copy, Clone)] +//! struct G; +//! impl affn::frames::ReferenceFrame for G { +//! fn frame_name() -> &'static str { "G" } +//! } +//! +//! // Build a 6×6 identity and verify block round-trip. +//! let eye6 = FrameMatrix6::::identity(); +//! let tl = eye6.top_left(); +//! assert!((tl.as_array()[0][0] - 1.0).abs() < 1e-15); +//! +//! // Block-diagonal rotation with identity → should be a no-op. +//! let r = Rotation3::IDENTITY; +//! let rot6 = BlockDiagRotation6::::from_rotation(r); +//! let cov = SymmetricFrameMatrix3::::from_diagonal([1.0, 4.0, 9.0]); +//! let zero_rv = FrameMatrix3::::zero(); +//! let (rr, rv, vv) = rot6.apply_to_symmetric_blocks::(&cov, &zero_rv, &cov); +//! assert!((rr.as_array()[0][0] - 1.0).abs() < 1e-15); +//! assert!((vv.as_array()[1][1] - 4.0).abs() < 1e-15); +//! let _ = rv; // cross-block is all zeros +//! ``` + +use core::marker::PhantomData; +use core::ops::Mul; + +use crate::matrix3::{FrameMatrix3, SymmetricFrameMatrix3}; +use crate::ops::Rotation3; + +// ============================================================================= +// FrameMatrix6 +// ============================================================================= + +/// Frame-tagged 6×6 matrix stored as a row-major `[[f64; 6]; 6]` array. +/// +/// The phantom parameter `F` is the reference frame the matrix is expressed +/// in. The block layout is `[r, v]` ordered: rows/columns 0–2 are the +/// position subspace, rows/columns 3–5 are the velocity subspace. +/// +/// Use [`FrameMatrix6::from_blocks`] to construct from four 3×3 blocks, and +/// the `top_left()` / `top_right()` / `bottom_left()` / `bottom_right()` +/// accessors to recover them. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct FrameMatrix6 { + data: [[f64; 6]; 6], + _frame: PhantomData, +} + +impl FrameMatrix6 { + // ------------------------------------------------------------------------- + // Constructors + // ------------------------------------------------------------------------- + + /// Wrap a raw row-major 6×6 array as a matrix in frame `F`. + #[inline] + pub fn from_array(data: [[f64; 6]; 6]) -> Self { + Self { + data, + _frame: PhantomData, + } + } + + /// Zero 6×6 matrix in frame `F`. + #[inline] + pub fn zero() -> Self { + Self::from_array([[0.0; 6]; 6]) + } + + /// 6×6 identity matrix in frame `F`. + pub fn identity() -> Self { + let mut d = [[0.0_f64; 6]; 6]; + for (i, row) in d.iter_mut().enumerate() { + row[i] = 1.0; + } + Self::from_array(d) + } + + /// Construct from four 3×3 blocks in `[r, v]` order: + /// + /// ```text + /// M = [ top_left top_right ] + /// [ bot_left bot_right ] + /// ``` + pub fn from_blocks( + top_left: FrameMatrix3, + top_right: FrameMatrix3, + bottom_left: FrameMatrix3, + bottom_right: FrameMatrix3, + ) -> Self { + let tl = top_left.as_array(); + let tr = top_right.as_array(); + let bl = bottom_left.as_array(); + let br = bottom_right.as_array(); + let mut d = [[0.0_f64; 6]; 6]; + for (i, d_row) in d.iter_mut().enumerate().take(3) { + for (j, d_elt) in d_row.iter_mut().enumerate().take(3) { + *d_elt = tl[i][j]; + } + for (j, d_elt) in d_row[3..].iter_mut().enumerate() { + *d_elt = tr[i][j]; + } + } + for (i, d_row) in d[3..].iter_mut().enumerate() { + for (j, d_elt) in d_row.iter_mut().enumerate().take(3) { + *d_elt = bl[i][j]; + } + for (j, d_elt) in d_row[3..].iter_mut().enumerate() { + *d_elt = br[i][j]; + } + } + Self::from_array(d) + } + + // ------------------------------------------------------------------------- + // Accessors + // ------------------------------------------------------------------------- + + /// Borrow the underlying row-major 6×6 array. + #[inline] + pub fn as_array(&self) -> &[[f64; 6]; 6] { + &self.data + } + + /// Copy the top-left 3×3 block (rows 0–2, cols 0–2) as a [`FrameMatrix3`]. + pub fn top_left(&self) -> FrameMatrix3 { + self.extract_block(0, 0) + } + + /// Copy the top-right 3×3 block (rows 0–2, cols 3–5) as a [`FrameMatrix3`]. + pub fn top_right(&self) -> FrameMatrix3 { + self.extract_block(0, 3) + } + + /// Copy the bottom-left 3×3 block (rows 3–5, cols 0–2) as a [`FrameMatrix3`]. + pub fn bottom_left(&self) -> FrameMatrix3 { + self.extract_block(3, 0) + } + + /// Copy the bottom-right 3×3 block (rows 3–5, cols 3–5) as a [`FrameMatrix3`]. + pub fn bottom_right(&self) -> FrameMatrix3 { + self.extract_block(3, 3) + } + + fn extract_block(&self, row_off: usize, col_off: usize) -> FrameMatrix3 { + let mut out = [[0.0_f64; 3]; 3]; + for (i, out_row) in out.iter_mut().enumerate() { + for (j, out_elt) in out_row.iter_mut().enumerate() { + *out_elt = self.data[row_off + i][col_off + j]; + } + } + FrameMatrix3::from_array(out) + } + + // ------------------------------------------------------------------------- + // Operations + // ------------------------------------------------------------------------- + + /// Transpose of this matrix. + pub fn transpose(&self) -> Self { + let mut out = [[0.0_f64; 6]; 6]; + for (i, out_row) in out.iter_mut().enumerate() { + for (j, out_elt) in out_row.iter_mut().enumerate() { + *out_elt = self.data[j][i]; + } + } + Self::from_array(out) + } + + /// Matrix–matrix product `self · rhs`, frame-tag preserving. + pub fn mat_mul(&self, rhs: &Self) -> Self { + let a = &self.data; + let b = &rhs.data; + let mut out = [[0.0_f64; 6]; 6]; + for (i, out_row) in out.iter_mut().enumerate() { + for (k, &aik) in a[i].iter().enumerate() { + if aik == 0.0 { + continue; + } + for (j, out_elt) in out_row.iter_mut().enumerate() { + *out_elt += aik * b[k][j]; + } + } + } + Self::from_array(out) + } + + /// Re-tag the matrix as belonging to frame `G`, without changing data. + /// + /// Use only when the data is already expressed in `G` by some other means; + /// the type system cannot verify this. + #[inline] + pub fn relabel(self) -> FrameMatrix6 { + FrameMatrix6 { + data: self.data, + _frame: PhantomData, + } + } +} + +/// Matrix multiplication `A * B → A·B` (frame-tag preserving). +impl Mul> for FrameMatrix6 { + type Output = FrameMatrix6; + #[inline] + fn mul(self, rhs: FrameMatrix6) -> FrameMatrix6 { + self.mat_mul(&rhs) + } +} + +/// Matrix multiplication by reference `&A * &B → A·B`. +impl Mul<&FrameMatrix6> for &FrameMatrix6 { + type Output = FrameMatrix6; + #[inline] + fn mul(self, rhs: &FrameMatrix6) -> FrameMatrix6 { + self.mat_mul(rhs) + } +} + +// ============================================================================= +// BlockDiagRotation6 +// ============================================================================= + +/// Block-diagonal rotation `T = blockdiag(R, R)` for 6-dimensional state-space +/// transforms. +/// +/// Given a 3×3 rotation `R`, this type represents the 6×6 operator +/// +/// ```text +/// T = [ R 0 ] +/// [ 0 R ] +/// ``` +/// +/// which simultaneously rotates the position and velocity sub-blocks by the +/// same rotation `R`. +/// +/// ## Instantaneous convention +/// +/// The time-derivative `Ṙ` of the rotation is **ignored**. This is the +/// correct choice for instantaneous covariance and STM frame changes at a +/// fixed epoch. If Coriolis corrections are required (e.g., for a rotating +/// atmosphere drag model), apply them separately. +/// +/// ## Efficient block-wise computation +/// +/// Instead of assembling and multiplying the full 6×6 matrix, all transforms +/// are performed as three independent 3×3 similarity operations: +/// +/// ```text +/// T · M · Tᵀ = [ R·Mrr·Rᵀ R·Mrv·Rᵀ ] +/// [ R·Mvr·Rᵀ R·Mvv·Rᵀ ] +/// ``` +/// +/// For symmetric `M` (covariance) `Mvr = Mrvᵀ` and the result is again +/// symmetric, which is exploited in [`BlockDiagRotation6::apply_to_symmetric_blocks`]. +#[derive(Debug, Clone, Copy)] +pub struct BlockDiagRotation6 { + /// The 3×3 rotation matrix R stored as a [`FrameMatrix3`]. + /// + /// Semantically, rows of `r` are the basis vectors of the *target* frame + /// expressed in frame `F`, i.e. `R` transforms vectors from `F` to the + /// target frame `G`. + r: FrameMatrix3, +} + +impl BlockDiagRotation6 { + /// Construct from a [`Rotation3`]. + /// + /// The rotation `r` should encode the transform `R : F → G` for whatever + /// target frame `G` the methods are called with. + #[inline] + pub fn from_rotation(r: Rotation3) -> Self { + Self { + r: FrameMatrix3::from_array(*r.as_matrix()), + } + } + + /// Construct from a [`FrameMatrix3`] (e.g., a precomputed rotation + /// matrix from the local-orbital-frame construction). + #[inline] + pub fn from_matrix(r: FrameMatrix3) -> Self { + Self { r } + } + + /// Apply `T · M · Tᵀ` (with `T = blockdiag(R, R)`) to a general 6×6 + /// matrix `m6`, returning the result tagged as frame `G`. + /// + /// The computation is performed block-wise: + /// + /// ```text + /// T · M · Tᵀ = [ R · Mrr · Rᵀ R · Mrv · Rᵀ ] + /// [ R · Mvr · Rᵀ R · Mvv · Rᵀ ] + /// ``` + /// + /// No assumption is made about the symmetry of `m6`. + pub fn apply_to_state_matrix(&self, m6: &FrameMatrix6) -> FrameMatrix6 { + let tl: FrameMatrix3 = self.r.similarity_general(&m6.top_left()); + let tr: FrameMatrix3 = self.r.similarity_general(&m6.top_right()); + let bl: FrameMatrix3 = self.r.similarity_general(&m6.bottom_left()); + let br: FrameMatrix3 = self.r.similarity_general(&m6.bottom_right()); + FrameMatrix6::from_blocks(tl, tr, bl, br) + } + + /// Apply `T · P · Tᵀ` (with `T = blockdiag(R, R)`) to a symmetric + /// covariance stored as three 3×3 blocks `(Prr, Prv, Pvv)`. + /// + /// The computation is performed block-wise: + /// + /// ```text + /// Prr' = R · Prr · Rᵀ (symmetric) + /// Prv' = R · Prv · Rᵀ (general) + /// Pvv' = R · Pvv · Rᵀ (symmetric) + /// ``` + /// + /// `Pvr = Prvᵀ` is derived on demand and never stored. + /// + /// # Arguments + /// + /// * `rr` — position–position covariance block (symmetric). + /// * `rv` — position–velocity cross-covariance block (general). + /// * `vv` — velocity–velocity covariance block (symmetric). + /// + /// # Returns + /// + /// `(rr', rv', vv')` in frame `G`. + pub fn apply_to_symmetric_blocks( + &self, + rr: &SymmetricFrameMatrix3, + rv: &FrameMatrix3, + vv: &SymmetricFrameMatrix3, + ) -> ( + SymmetricFrameMatrix3, + FrameMatrix3, + SymmetricFrameMatrix3, + ) { + let rr_out: SymmetricFrameMatrix3 = self.r.similarity(rr); + let rv_out: FrameMatrix3 = self.r.similarity_general(rv); + let vv_out: SymmetricFrameMatrix3 = self.r.similarity(vv); + (rr_out, rv_out, vv_out) + } +} + +// ============================================================================= +// Tests +// ============================================================================= + +#[cfg(test)] +mod tests { + use super::*; + use qtty::angular::Radians; + + #[derive(Debug, Copy, Clone)] + struct F1; + impl crate::frames::ReferenceFrame for F1 { + fn frame_name() -> &'static str { + "F1" + } + } + + #[derive(Debug, Copy, Clone)] + struct F2; + impl crate::frames::ReferenceFrame for F2 { + fn frame_name() -> &'static str { + "F2" + } + } + + // ------------------------------------------------------------------------- + // FrameMatrix6 + // ------------------------------------------------------------------------- + + #[test] + fn frame_matrix6_from_to_array_round_trip() { + let mut data = [[0.0_f64; 6]; 6]; + for (i, row) in data.iter_mut().enumerate() { + for (j, elt) in row.iter_mut().enumerate() { + *elt = (i * 6 + j) as f64; + } + } + let m = FrameMatrix6::::from_array(data); + assert_eq!(m.as_array(), &data); + } + + #[test] + fn frame_matrix6_identity() { + let eye = FrameMatrix6::::identity(); + for (i, row) in eye.as_array().iter().enumerate() { + for (j, &val) in row.iter().enumerate() { + let expected = if i == j { 1.0 } else { 0.0 }; + assert_eq!(val, expected); + } + } + } + + #[test] + fn frame_matrix6_from_blocks_round_trip() { + let tl = FrameMatrix3::::from_diagonal([1.0, 2.0, 3.0]); + let tr = FrameMatrix3::::from_diagonal([4.0, 5.0, 6.0]); + let bl = FrameMatrix3::::from_diagonal([7.0, 8.0, 9.0]); + let br = FrameMatrix3::::from_diagonal([10.0, 11.0, 12.0]); + let m6 = FrameMatrix6::from_blocks(tl, tr, bl, br); + + let tl2 = m6.top_left(); + let tr2 = m6.top_right(); + let bl2 = m6.bottom_left(); + let br2 = m6.bottom_right(); + + assert_eq!(tl2.as_array(), tl.as_array()); + assert_eq!(tr2.as_array(), tr.as_array()); + assert_eq!(bl2.as_array(), bl.as_array()); + assert_eq!(br2.as_array(), br.as_array()); + } + + #[test] + fn frame_matrix6_mul_identity() { + let eye = FrameMatrix6::::identity(); + let result = eye * eye; + for (i, row) in result.as_array().iter().enumerate() { + for (j, &val) in row.iter().enumerate() { + let expected = if i == j { 1.0 } else { 0.0 }; + assert!((val - expected).abs() < 1e-14); + } + } + } + + #[test] + fn frame_matrix6_mul_block() { + // Construct M6 from block-diagonal (2, 3, 5 on tl and br), multiply by itself. + let tl = FrameMatrix3::::from_diagonal([2.0, 3.0, 5.0]); + let zero = FrameMatrix3::::zero(); + let br = FrameMatrix3::::from_diagonal([7.0, 11.0, 13.0]); + let m6 = FrameMatrix6::from_blocks(tl, zero, zero, br); + let m6sq = m6.mat_mul(&m6); + // (2²=4, 3²=9, 5²=25) in top-left; (7²=49, 11²=121, 13²=169) in bottom-right + let tl2 = m6sq.top_left(); + let br2 = m6sq.bottom_right(); + assert!((tl2.as_array()[0][0] - 4.0).abs() < 1e-14); + assert!((tl2.as_array()[1][1] - 9.0).abs() < 1e-14); + assert!((tl2.as_array()[2][2] - 25.0).abs() < 1e-14); + assert!((br2.as_array()[0][0] - 49.0).abs() < 1e-14); + assert!((br2.as_array()[1][1] - 121.0).abs() < 1e-14); + assert!((br2.as_array()[2][2] - 169.0).abs() < 1e-14); + } + + #[test] + fn frame_matrix6_transpose() { + let tl = + FrameMatrix3::::from_array([[1.0, 2.0, 3.0], [0.0, 4.0, 5.0], [0.0, 0.0, 6.0]]); + let tr = FrameMatrix3::::from_diagonal([7.0, 8.0, 9.0]); + let bl = FrameMatrix3::::zero(); + let br = FrameMatrix3::::identity(); + let m6 = FrameMatrix6::from_blocks(tl, tr, bl, br); + let m6t = m6.transpose(); + for (i, row_t) in m6t.as_array().iter().enumerate() { + for (j, &v) in row_t.iter().enumerate() { + assert!((v - m6.as_array()[j][i]).abs() < 1e-15); + } + } + } + + #[test] + fn frame_matrix6_relabel() { + let eye: FrameMatrix6 = FrameMatrix6::identity(); + let eye2: FrameMatrix6 = eye.relabel(); + assert_eq!(eye2.as_array(), eye.as_array()); + } + + // ------------------------------------------------------------------------- + // BlockDiagRotation6 + // ------------------------------------------------------------------------- + + #[test] + fn block_diag_rotation6_identity_is_noop() { + let rot6 = BlockDiagRotation6::::from_rotation(Rotation3::IDENTITY); + let rr = SymmetricFrameMatrix3::::from_diagonal([1.0, 4.0, 9.0]); + let rv = FrameMatrix3::::zero(); + let vv = SymmetricFrameMatrix3::::from_diagonal([0.01, 0.04, 0.09]); + let (rr2, rv2, vv2) = rot6.apply_to_symmetric_blocks::(&rr, &rv, &vv); + for (i, rr2_row) in rr2.as_array().iter().enumerate() { + assert!((rr2_row[i] - rr.as_array()[i][i]).abs() < 1e-15); + assert!((vv2.as_array()[i][i] - vv.as_array()[i][i]).abs() < 1e-15); + for (j, &val) in rv2.as_array()[i].iter().enumerate() { + let _ = j; // preserve index binding for clarity + assert!(val.abs() < 1e-15); + } + } + } + + #[test] + fn block_diag_rotation6_round_trip() { + // Rotate by 45° around Z, then rotate back; should recover the original. + let r_fwd = Rotation3::rz(Radians::new(std::f64::consts::FRAC_PI_4)); + let r_bwd = r_fwd.inverse(); + + let rr = SymmetricFrameMatrix3::::from_upper([ + [4.0, 1.2, 0.5], + [0.0, 9.0, 0.3], + [0.0, 0.0, 2.0], + ]); + let rv = + FrameMatrix3::::from_array([[0.1, 0.02, 0.0], [0.03, 0.2, 0.01], [0.0, 0.0, 0.1]]); + let vv = SymmetricFrameMatrix3::::from_upper([ + [0.01, 0.001, 0.0], + [0.0, 0.02, 0.0], + [0.0, 0.0, 0.015], + ]); + + let rot_fwd = BlockDiagRotation6::::from_rotation(r_fwd); + let rot_bwd = BlockDiagRotation6::::from_rotation(r_bwd); + + let (rr2, rv2, vv2) = rot_fwd.apply_to_symmetric_blocks::(&rr, &rv, &vv); + let (rr3, rv3, vv3) = rot_bwd.apply_to_symmetric_blocks::(&rr2, &rv2, &vv2); + + for (i, rr_row) in rr3.as_array().iter().enumerate() { + for (j, &rr_val) in rr_row.iter().enumerate() { + let expected_rr = rr.as_array()[i][j]; + assert!( + (rr_val - expected_rr).abs() < 1e-12, + "rr round-trip failed at [{i}][{j}]: {} != {}", + rr_val, + expected_rr + ); + } + for (j, &rv_val) in rv3.as_array()[i].iter().enumerate() { + let expected_rv = rv.as_array()[i][j]; + assert!( + (rv_val - expected_rv).abs() < 1e-12, + "rv round-trip failed at [{i}][{j}]: {} != {}", + rv_val, + expected_rv + ); + } + for (j, &vv_val) in vv3.as_array()[i].iter().enumerate() { + let expected_vv = vv.as_array()[i][j]; + assert!( + (vv_val - expected_vv).abs() < 1e-12, + "vv round-trip failed at [{i}][{j}]: {} != {}", + vv_val, + expected_vv + ); + } + } + } + + #[test] + fn block_diag_rotation6_apply_to_state_matrix_identity() { + // T * I6 * T^T = I6 for any rotation T. + let r = Rotation3::rz(Radians::new(1.23)); + let rot6 = BlockDiagRotation6::::from_rotation(r); + let eye6 = FrameMatrix6::::identity(); + let result: FrameMatrix6 = rot6.apply_to_state_matrix(&eye6); + // The similarity of the identity block is still the identity block + for (i, row) in result.as_array().iter().enumerate() { + for (j, &val) in row.iter().enumerate() { + let expected = if i == j { 1.0 } else { 0.0 }; + assert!( + (val - expected).abs() < 1e-13, + "T*I6*T^T mismatch at [{i}][{j}]: {} != {expected}", + val + ); + } + } + } +} diff --git a/tests/test_astro_frame_relationships.rs b/tests/test_astro_frame_relationships.rs index c1df3a7..abf8b98 100644 --- a/tests/test_astro_frame_relationships.rs +++ b/tests/test_astro_frame_relationships.rs @@ -1,5 +1,7 @@ -//! Tests for the canonical relationships among the four IAU/IERS markers -//! `ICRS`, `ICRF`, `EME2000`, `GCRS` exposed by `affn::frames::astro`. +//! Tests for the canonical identity relationships among the IAU/IERS markers +//! `ICRS`, `ICRF`, and `GCRS` exposed by `affn::frames::astro`. +//! +//! Frame-bias tests (GCRS ↔ EME2000) live in `siderust::astro::frame_bias`. //! //! Conventions checked here: //! @@ -8,41 +10,12 @@ //! * `ICRS` ↔ `GCRS`: as a *direction* frame, GCRS is bit-identical to ICRS //! (per IAU 2000 Resolution B1.3); the only difference is the spatial //! origin, encoded by the center type. -//! * `GCRS` ↔ `EME2000`: a constant frame-bias rotation `B` of magnitude -//! ≈ 23 mas (≈ 1.1 × 10⁻⁷ rad), as documented in IERS Conventions (2010) -//! §5.4.4 / Table 5.2b. #![cfg(feature = "astro")] -use affn::frames::{EME2000, GCRS, ICRF, ICRS}; +use affn::frames::{GCRS, ICRF, ICRS}; use affn::ops::Rotation3; -/// Convert a `Rotation3` to its rotation angle (radians) via the trace. -fn rotation_angle_rad(r: &Rotation3) -> f64 { - let m = r.as_matrix(); - let trace = m[0][0] + m[1][1] + m[2][2]; - let cos = ((trace - 1.0) * 0.5).clamp(-1.0, 1.0); - cos.acos() -} - -/// Maximum elementwise absolute difference between two 3×3 rotation matrices, -/// useful as an angular-separation proxy on rotation matrices that are very -/// close to identity (where the trace-based formula loses precision). -fn max_abs_diff(a: &Rotation3, b: &Rotation3) -> f64 { - let am = a.as_matrix(); - let bm = b.as_matrix(); - let mut max = 0.0_f64; - for i in 0..3 { - for j in 0..3 { - let d = (am[i][j] - bm[i][j]).abs(); - if d > max { - max = d; - } - } - } - max -} - // ───────────────────────────────────────────────────────────────────────────── // ICRS ↔ ICRF // ───────────────────────────────────────────────────────────────────────────── @@ -79,112 +52,3 @@ fn gcrs_to_icrs_direction_is_bit_exact_identity() { let r = GCRS::direction_rotation_to_icrs(); assert_eq!(r, Rotation3::IDENTITY); } - -// ───────────────────────────────────────────────────────────────────────────── -// GCRS ↔ EME2000 (frame bias B; IERS Conventions 2010 §5.4.4 / Table 5.2b) -// ───────────────────────────────────────────────────────────────────────────── - -#[test] -fn gcrs_to_eme2000_frame_bias_magnitude_is_about_23_mas() { - let b = GCRS::frame_bias_to_eme2000(); - - // ~23 mas ≈ 1.115e-7 rad. Bound generously (1e-8 .. 5e-7 rad, i.e. - // ~2 mas .. ~100 mas) to lock down the order of magnitude without - // pinning it to a specific PFW06/IERS variant down to the last bit. - let theta = rotation_angle_rad(&b); - assert!( - (1.0e-8..5.0e-7).contains(&theta), - "frame-bias angle {theta:e} rad is outside the documented ≈23 mas range", - ); - - // Tighter sanity: ~23 mas == 23e-3 * pi/648000 rad ≈ 1.115e-7. - let nominal = 23.0e-3 * std::f64::consts::PI / 648_000.0; - assert!( - (theta - nominal).abs() < 0.5 * nominal, - "frame-bias angle {theta:e} rad differs from the nominal 23 mas \ - ({nominal:e} rad) by more than 50%", - ); -} - -#[test] -fn gcrs_to_eme2000_is_not_identity() { - // Sanity: the bias is *not* zero — that would silently mean we are - // treating GCRS and EME2000 as the same frame, which is wrong. - let b = GCRS::frame_bias_to_eme2000(); - assert_ne!(b, Rotation3::IDENTITY); - assert!(max_abs_diff(&b, &Rotation3::IDENTITY) > 1.0e-9); -} - -#[test] -fn gcrs_eme2000_roundtrip_is_identity_to_1e_minus_15() { - // Round-trip GCRS → EME2000 → GCRS must be the identity to within - // double-precision noise (well under 1e-15 angular separation). - let fwd = GCRS::frame_bias_to_eme2000(); - let inv = EME2000::frame_bias_to_gcrs(); - let round = fwd * inv; - - let diff = max_abs_diff(&round, &Rotation3::IDENTITY); - assert!( - diff < 1.0e-15, - "GCRS→EME2000→GCRS deviates from identity by {diff:e} \ - (> 1e-15 angular separation)", - ); - - // And in the other direction. - let round_other = inv * fwd; - let diff_other = max_abs_diff(&round_other, &Rotation3::IDENTITY); - assert!( - diff_other < 1.0e-15, - "EME2000→GCRS→EME2000 deviates from identity by {diff_other:e}", - ); -} - -#[test] -fn icrs_and_gcrs_share_the_same_frame_bias_to_eme2000() { - // Because GCRS and ICRS are direction-identical, their frame-bias - // rotations to EME2000 must be the *same* matrix (bit-equal). - let from_gcrs = GCRS::frame_bias_to_eme2000(); - let from_icrs = ICRS::frame_bias_to_eme2000(); - assert_eq!(from_gcrs, from_icrs); -} - -#[test] -fn frame_bias_off_diagonal_signs_match_iers_convention() { - // IERS / SOFA convention for the IAU 2006 bias rb at J2000.0: - // rb[0][1] < 0, rb[1][0] > 0, - // rb[0][2] > 0, rb[2][0] < 0, - // rb[1][2] > 0, rb[2][1] < 0. - // (See `siderust/.../bias.rs::bias_off_diagonal_signs_match_sofa`.) - let b = GCRS::frame_bias_to_eme2000(); - let m = b.as_matrix(); - assert!( - m[0][1] < 0.0, - "rb[0][1] should be negative, got {}", - m[0][1] - ); - assert!( - m[1][0] > 0.0, - "rb[1][0] should be positive, got {}", - m[1][0] - ); - assert!( - m[0][2] > 0.0, - "rb[0][2] should be positive, got {}", - m[0][2] - ); - assert!( - m[2][0] < 0.0, - "rb[2][0] should be negative, got {}", - m[2][0] - ); - assert!( - m[1][2] > 0.0, - "rb[1][2] should be positive, got {}", - m[1][2] - ); - assert!( - m[2][1] < 0.0, - "rb[2][1] should be negative, got {}", - m[2][1] - ); -}