Function matrix_from_params 

fn matrix_from_params(p: &[f64; 6], center: &[f64; 3]) -> [f64; 16]

The rigid 4×4 for a parameter vector: x_moving = center + R·(x_fixed - center) + t.