/// @module std::math::rotation
/// 3D Rotation Math — Euler angles, rotation matrices, composition
///
/// Uses ZYX Euler angle convention: R = Rz(alpha) * Ry(beta) * Rx(gamma).
/// All angles are in radians. Matrices are 3x3 Mat<number>.
from std::core::math use { atan2 }
/// Convert ZYX Euler angles to a 3x3 rotation matrix.
///
/// @param alpha - Rotation about Z axis (yaw) in radians
/// @param beta - Rotation about Y axis (pitch) in radians
/// @param gamma - Rotation about X axis (roll) in radians
/// @returns 3x3 rotation matrix as Mat<number>
///
/// @example
/// euler_to_matrix(0, 0, 0) // identity matrix
pub fn euler_to_matrix(alpha: number, beta: number, gamma: number) {
let ca: number = cos(alpha)
let sa: number = sin(alpha)
let cb: number = cos(beta)
let sb: number = sin(beta)
let cg: number = cos(gamma)
let sg: number = sin(gamma)
// R = Rz(alpha) * Ry(beta) * Rx(gamma)
mat(3, 3, [
ca * cb, ca * sb * sg - sa * cg, ca * sb * cg + sa * sg,
sa * cb, sa * sb * sg + ca * cg, sa * sb * cg - ca * sg,
0.0 - sb, cb * sg, cb * cg
])
}
/// Extract ZYX Euler angles from a 3x3 rotation matrix.
///
/// Handles gimbal lock when beta is near +/- pi/2.
///
/// @param m - 3x3 rotation matrix
/// @returns [alpha, beta, gamma] angles in radians
///
/// @example
/// matrix_to_euler(euler_to_matrix(0.1, 0.2, 0.3)) // ~[0.1, 0.2, 0.3]
pub fn matrix_to_euler(m: Mat<number>) -> Array<number> {
let r20: number = m.row(2)[0]
// beta = -asin(r20), clamped to avoid NaN
let clamped: number = if r20 < -1.0 { -1.0 } else if r20 > 1.0 { 1.0 } else { r20 }
let beta: number = asin(0.0 - clamped)
let cb: number = cos(beta)
if abs(cb) > 0.0001 {
// Normal case
let alpha: number = atan2(m.row(1)[0], m.row(0)[0])
let gamma: number = atan2(m.row(2)[1], m.row(2)[2])
[alpha, beta, gamma]
} else {
// Gimbal lock: beta near +/- pi/2
let alpha: number = atan2(0.0 - m.row(0)[1], m.row(1)[1])
let gamma: number = 0.0
[alpha, beta, gamma]
}
}
/// Apply a rotation matrix to a set of points.
///
/// Computes rot * points^T, then transposes the result.
///
/// @param rot - 3x3 rotation matrix
/// @param points - Nx3 matrix of points (each row is a point)
/// @returns Nx3 matrix of rotated points
pub fn rotation_apply(rot: Mat<number>, points: Mat<number>) {
let pt: Mat<number> = points.transpose()
let rotated: Mat<number> = rot * pt
rotated.transpose()
}
/// Compute the inverse of a rotation matrix.
///
/// For orthogonal rotation matrices, the inverse equals the transpose.
///
/// @param rot - 3x3 rotation matrix
/// @returns Inverse (transpose) rotation matrix
pub fn rotation_inverse(rot: Mat<number>) {
rot.transpose()
}
/// Compose two rotations via matrix multiplication.
///
/// The resulting rotation applies r2 first, then r1.
///
/// @param r1 - First rotation matrix (applied second)
/// @param r2 - Second rotation matrix (applied first)
/// @returns Combined 3x3 rotation matrix
pub fn rotation_compose(r1: Mat<number>, r2: Mat<number>) -> Mat<number> {
let result: Mat<number> = r1 * r2
result
}
/// Create a Mat<number> from an array of row arrays.
///
/// @param rows - Array of 3-element arrays, e.g. [[1,0,0],[0,1,0],[0,0,1]]
/// @returns 3x3 Mat<number>
pub fn rotation_from_rows(rows: Array<Array<number>>) {
let nrows: int = rows.length
let ncols: int = rows[0].length
let mut flat: Array<number> = []
let mut i: int = 0
while i < nrows {
let mut j: int = 0
while j < ncols {
flat = flat.push(rows[i][j])
j = j + 1
}
i = i + 1
}
mat(nrows, ncols, flat)
}
/// Normalize Euler angles to the range [-pi, pi].
///
/// @param angles - Array of angles in radians
/// @returns Array of angles wrapped to [-pi, pi]
///
/// @example
/// normalize_euler([7.0, -7.0, 0.0]) // wrapped values
pub fn normalize_euler(angles: Array<number>) -> Array<number> {
let pi: number = 3.141592653589793
let two_pi: number = 6.283185307179586
angles.map(|a: number| {
let mut v: number = a
while v > pi {
v = v - two_pi
}
while v < 0.0 - pi {
v = v + two_pi
}
v
})
}