plasma-prp 0.1.0

//! Transform propagation — parent-child coordinate hierarchy.
//!
//! plCoordinateInterface maintains local-to-parent and local-to-world matrices.
//! When a parent moves, all children recompute their world transforms.
//!
//! C++ ref: pnSceneObject/plCoordinateInterface.h/.cpp

use glam::{Mat4, Vec3};

/// Multiply two 4x4 matrices stored as [f32; 16] in row-major order.
/// hsMatrix44 is row-major: fMap[row][col], flat index = row*4+col.
pub fn mat44_multiply(a: &[f32; 16], b: &[f32; 16]) -> [f32; 16] {
    // glam::Mat4 is column-major, so we transpose on import/export
    let ma = Mat4::from_cols_array(a).transpose();
    let mb = Mat4::from_cols_array(b).transpose();
    let result = ma * mb;
    result.transpose().to_cols_array()
}

/// Extract translation from a row-major 4x4 matrix.
/// Translation is at fMap[0][3]=m[3], fMap[1][3]=m[7], fMap[2][3]=m[11].
pub fn mat44_translation(m: &[f32; 16]) -> Vec3 {
    Vec3::new(m[3], m[7], m[11])
}

/// Create an identity 4x4 matrix.
pub fn mat44_identity() -> [f32; 16] {
    [
        1.0, 0.0, 0.0, 0.0,
        0.0, 1.0, 0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, 0.0, 1.0,
    ]
}

/// Check if a matrix is identity.
pub fn mat44_is_identity(m: &[f32; 16]) -> bool {
    let id = mat44_identity();
    m.iter()
        .zip(id.iter())
        .all(|(a, b)| (a - b).abs() < 1e-6)
}

/// Transform a point by a row-major 4x4 matrix.
pub fn mat44_transform_point(m: &[f32; 16], p: Vec3) -> Vec3 {
    Vec3::new(
        p.x * m[0] + p.y * m[1] + p.z * m[2]  + m[3],
        p.x * m[4] + p.y * m[5] + p.z * m[6]  + m[7],
        p.x * m[8] + p.y * m[9] + p.z * m[10] + m[11],
    )
}

/// Transform a direction (no translation) by a row-major 4x4 matrix.
pub fn mat44_transform_direction(m: &[f32; 16], d: Vec3) -> Vec3 {
    Vec3::new(
        d.x * m[0] + d.y * m[1] + d.z * m[2],
        d.x * m[4] + d.y * m[5] + d.z * m[6],
        d.x * m[8] + d.y * m[9] + d.z * m[10],
    )
}

/// Propagate transforms through a parent-child hierarchy.
///
/// Given a parent's local_to_world matrix and a child's local_to_parent matrix,
/// compute the child's local_to_world.
pub fn propagate_transform(
    parent_local_to_world: &[f32; 16],
    child_local_to_parent: &[f32; 16],
) -> [f32; 16] {
    mat44_multiply(parent_local_to_world, child_local_to_parent)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_identity() {
        let id = mat44_identity();
        assert!(mat44_is_identity(&id));
    }

    #[test]
    fn test_transform_point() {
        // Row-major translation matrix: translate by (1, 2, 3)
        // fMap[0][3]=1, fMap[1][3]=2, fMap[2][3]=3
        let m = [
            1.0, 0.0, 0.0, 1.0,  // row 0: m[3] = translate X
            0.0, 1.0, 0.0, 2.0,  // row 1: m[7] = translate Y
            0.0, 0.0, 1.0, 3.0,  // row 2: m[11] = translate Z
            0.0, 0.0, 0.0, 1.0,  // row 3
        ];
        let p = Vec3::new(0.0, 0.0, 0.0);
        let result = mat44_transform_point(&m, p);
        assert!((result.x - 1.0).abs() < 0.01);
        assert!((result.y - 2.0).abs() < 0.01);
        assert!((result.z - 3.0).abs() < 0.01);
    }

    #[test]
    fn test_propagate() {
        // Row-major: translation in column 3 of each row
        let parent = [
            1.0, 0.0, 0.0, 10.0,  // translate X=10
            0.0, 1.0, 0.0, 0.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 1.0,
        ];
        let child = [
            1.0, 0.0, 0.0, 5.0,   // translate X=5
            0.0, 1.0, 0.0, 0.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 1.0,
        ];
        let result = propagate_transform(&parent, &child);
        let pos = mat44_translation(&result);
        assert!((pos.x - 15.0).abs() < 0.01);
    }
}