geo-nd 0.6.4

use crate::matrix_op as matrix;
use crate::vector_op as vector;
use crate::{Float, Num, f_const};

//a Notes on matrices of quaternions
// 1 - 2*j2 - 2*k2           2*i*j - 2*k*r        2*i*k + 2*j*r
//     2*i*j + 2*k*r     1 - 2*i2 - 2*k2          2*j*k - 2*i*r
//     2*i*k - 2*j*r         2*j*k + 2*i*r    1 - 2*i2 - 2*j2
//
// m[0] + m[4] + m[9] = 3 - 4(i^2+j^2+k^2) = 3 - 4(1-r^2) = 4r^2 - 1
// m[7] - m[5] = 4*i*r ( if <0 then i<0 )
// m[2] - m[6] = 4*j*r ( if <0 then j<0 )
// m[3] - m[1] = 4*k*r ( if <0 then k<0 )

//a Constructors and destructors
//fp new
/// Create a new quaternion
#[must_use]
#[inline]
pub fn new<V: Num>() -> [V; 4] {
    [V::ZERO, V::ZERO, V::ZERO, V::ONE]
}

//fp as_rijk
/// Return the breakdown of a quaternion
#[must_use]
#[inline]
pub fn as_rijk<V: Num>(v: &[V; 4]) -> (V, V, V, V) {
    (v[3], v[0], v[1], v[2])
}

//fp of_rijk
/// Create a quaternion from its components
#[must_use]
#[inline]
pub fn of_rijk<V: Num>(r: V, i: V, j: V, k: V) -> [V; 4] {
    [i, j, k, r]
}

//fp identity
/// Create an identity quaternion
#[must_use]
#[inline]
pub fn identity<V: Num>() -> [V; 4] {
    [V::ZERO, V::ZERO, V::ZERO, V::ONE]
}

//fp of_axis_angle
/// Find the quaternion for a rotation of an angle around an axis
#[must_use]
#[inline]
pub fn of_axis_angle<V: Float>(axis: &[V; 3], angle: V) -> [V; 4] {
    let (s, c) = V::sin_cos(angle * f_const!(V, 1, 2));
    let l = vector::length(axis);
    if l < V::epsilon() {
        identity()
    } else {
        let s = s / l;
        let i = s * axis[0];
        let j = s * axis[1];
        let k = s * axis[2];
        let r = c;
        [i, j, k, r]
    }
}

//fp as_axis_angle
/// Return the axis of the rotation and the angle from the quaternion
#[must_use]
#[inline]
pub fn as_axis_angle<V: Float>(q: &[V; 4]) -> ([V; 3], V) {
    let (r, i, j, k) = as_rijk(q);
    let i2 = i * i;
    let j2 = j * j;
    let k2 = k * k;
    let l = (i2 + j2 + k2).sqrt();
    if l < V::epsilon() {
        ([i, j, k], V::ZERO)
    } else {
        let rl = V::ONE / l;
        ([i * rl, j * rl, k * rl], V::atan2(l, r))
    }
}

//fp to_rotation3
/// Convert a matrix-3 from the quaternion
pub fn to_rotation3<V: Float>(q: &[V; 4], m: &mut [V; 9]) {
    let i2 = q[0] * q[0];
    let j2 = q[1] * q[1];
    let k2 = q[2] * q[2];
    let r2 = q[3] * q[3];

    let l2 = r2 + i2 + j2 + k2;
    let rl2 = V::ONE / l2;

    m[0] = (r2 + i2 - j2 - k2) * rl2;
    m[4] = (r2 - i2 + j2 - k2) * rl2;
    m[8] = (r2 - i2 - j2 + k2) * rl2;

    let drl2 = rl2 + rl2;

    m[1] = (q[0] * q[1] - q[2] * q[3]) * drl2;
    m[3] = (q[0] * q[1] + q[2] * q[3]) * drl2;

    m[2] = (q[2] * q[0] + q[1] * q[3]) * drl2;
    m[6] = (q[2] * q[0] - q[1] * q[3]) * drl2;

    m[5] = (q[1] * q[2] - q[0] * q[3]) * drl2;
    m[7] = (q[1] * q[2] + q[0] * q[3]) * drl2;
}

//fp to_rotation4
/// Convert to a matrix-4 from a unit quaternion
pub fn to_rotation4<V: Float>(q: &[V; 4], m: &mut [V; 16]) {
    let i2 = q[0] * q[0];
    let j2 = q[1] * q[1];
    let k2 = q[2] * q[2];
    let r2 = q[3] * q[3];

    let l2 = r2 + i2 + j2 + k2;
    let rl2 = V::ONE / l2;

    m[0] = (r2 + i2 - j2 - k2) * rl2;
    m[5] = (r2 - i2 + j2 - k2) * rl2;
    m[10] = (r2 - i2 - j2 + k2) * rl2;

    let drl2 = rl2 + rl2;

    m[1] = (q[0] * q[1] - q[2] * q[3]) * drl2;
    m[4] = (q[0] * q[1] + q[2] * q[3]) * drl2;

    m[2] = (q[2] * q[0] + q[1] * q[3]) * drl2;
    m[8] = (q[2] * q[0] - q[1] * q[3]) * drl2;

    m[6] = (q[1] * q[2] - q[0] * q[3]) * drl2;
    m[9] = (q[1] * q[2] + q[0] * q[3]) * drl2;

    m[3] = V::ZERO;
    m[7] = V::ZERO;
    m[11] = V::ZERO;
    m[12] = V::ZERO;
    m[13] = V::ZERO;
    m[14] = V::ZERO;
    m[15] = V::ONE;
}

//fp of_rotation
/// Find the quaternion of a Matrix3 assuming it is purely a rotation
#[must_use]
pub fn of_rotation<V: Float>(m: &[V; 9]) -> [V; 4] {
    fn safe_sqrt<V: Float>(x: V) -> V {
        if x < V::ZERO { V::ZERO } else { x.sqrt() }
    }
    let half = f_const!(V, 1, 2);
    let r = safe_sqrt(V::ONE + m[0] + m[4] + m[8]) * half;
    let mut i = safe_sqrt(V::ONE + m[0] - m[4] - m[8]) * half;
    let mut j = safe_sqrt(V::ONE - m[0] + m[4] - m[8]) * half;
    let mut k = safe_sqrt(V::ONE - m[0] - m[4] + m[8]) * half;

    let r_i_4 = m[7] - m[5];
    let r_j_4 = m[2] - m[6];
    let r_k_4 = m[3] - m[1];
    if r_i_4 < -V::epsilon() {
        i = -i;
    }
    if r_j_4 < -V::epsilon() {
        j = -j;
    }
    if r_k_4 < -V::epsilon() {
        k = -k;
    }

    [i, j, k, r]
}

//fp look_at
/// Create quaternion for a rotation that maps unit dirn to (0,0,-1) and unit up to (0,1,0)
#[must_use]
pub fn look_at<V: Float>(dirn: &[V; 3], up: &[V; 3]) -> [V; 4] {
    let m = matrix::look_at3(dirn, up);
    of_rotation(&m)
}

//a Mapping functions
//cp invert
/// Get the quaternion inverse
#[must_use]
#[inline]
pub fn invert<V: Float>(a: &[V; 4]) -> [V; 4] {
    let l = vector::length_sq(a);
    let r_l = {
        if l < V::epsilon() {
            V::ZERO
        } else {
            V::ONE / l
        }
    };
    [-a[0] * r_l, -a[1] * r_l, -a[2] * r_l, a[3] * r_l]
}

//cp conjugate
/// Find the conjugate of a quaternion
#[must_use]
#[inline]
pub fn conjugate<V: Num>(a: &[V; 4]) -> [V; 4] {
    [-a[0], -a[1], -a[2], a[3]]
}

//cp normalize
/// Find the conjugate of a quaternion
#[must_use]
#[inline]
pub fn normalize<V: Float>(a: [V; 4]) -> [V; 4] {
    vector::normalize(a)
}

//cp rotate_x
/// Apply a rotation about the X-axis to this quaternion
///
/// Rotation about the X axis is c+i*s
///
/// `(c+i*s) * a[] = c*a - s*a[i] + i.s*a[r] - j.s*a[k] + k.s*a[j]`
#[must_use]
#[inline]
pub fn rotate_x<V: Float>(a: &[V; 4], angle: V) -> [V; 4] {
    let (s, c) = V::sin_cos(angle * f_const!(V, 1, 2));
    let i = a[0] * c + a[3] * s;
    let j = a[1] * c + a[2] * s;
    let k = a[2] * c - a[1] * s;
    let r = a[3] * c - a[0] * s;
    [i, j, k, r]
}

//cp rotate_y
/// Apply a rotation about the Y-axis to this quaternion
#[must_use]
#[inline]
pub fn rotate_y<V: Float>(a: &[V; 4], angle: V) -> [V; 4] {
    let (s, c) = V::sin_cos(angle * f_const!(V, 1, 2));
    let i = a[0] * c - a[2] * s;
    let j = a[1] * c + a[3] * s;
    let k = a[2] * c + a[0] * s;
    let r = a[3] * c - a[1] * s;
    [i, j, k, r]
}

//cp rotate_z
/// Apply a rotation about the Z-axis to this quaternion
#[must_use]
#[inline]
pub fn rotate_z<V: Float>(a: &[V; 4], angle: V) -> [V; 4] {
    let (s, c) = V::sin_cos(angle * f_const!(V, 1, 2));
    let i = a[0] * c + a[1] * s;
    let j = a[1] * c - a[0] * s;
    let k = a[2] * c + a[3] * s;
    let r = a[3] * c - a[2] * s;
    [i, j, k, r]
}

//cp multiply
/// Multiply two quaternions together
#[must_use]
#[inline]
pub fn multiply<V: Num>(a: &[V; 4], b: &[V; 4]) -> [V; 4] {
    let i = a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1];
    let j = a[1] * b[3] + a[3] * b[1] + a[2] * b[0] - a[0] * b[2];
    let k = a[2] * b[3] + a[3] * b[2] + a[0] * b[1] - a[1] * b[0];
    let r = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2];
    [i, j, k, r]
}

//cp divide
/// Multiply one quaternion by the conjugate of the other / len2 of other
#[must_use]
#[inline]
pub fn divide<V: Float>(a: &[V; 4], b: &[V; 4]) -> [V; 4] {
    let l2 = vector::length_sq(b);
    if l2 < V::epsilon() {
        [V::ZERO; 4]
    } else {
        let i = a[0] * b[3] - a[3] * b[0] - a[1] * b[2] + a[2] * b[1];
        let j = a[1] * b[3] - a[3] * b[1] - a[2] * b[0] + a[0] * b[2];
        let k = a[2] * b[3] - a[3] * b[2] - a[0] * b[1] + a[1] * b[0];
        let r = a[3] * b[3] + a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
        [i / l2, j / l2, k / l2, r / l2]
    }
}

//fp nlerp
/// A simple normalized LERP from one quaterion to another (not spherical)
#[must_use]
#[inline]
pub fn nlerp<V: Float>(t: V, in0: &[V; 4], in1: &[V; 4]) -> [V; 4] {
    normalize(vector::mix(in0, in1, t))
}

//a Operational functions
//fp distance_sq
/// Get a measure of the 'distance' between two unit quaternions
#[inline]
pub fn distance_sq<V: Float>(a: &[V; 4], b: &[V; 4]) -> V {
    V::ONE - (a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]).abs()
}

//fp distance
/// Get a measure of the 'distance' between two quaternions
#[inline]
pub fn distance<V: Float>(a: &[V; 4], b: &[V; 4]) -> V {
    distance_sq(a, b).sqrt()
}

//fp to_euler
/// Convert the quaternion to a bank, heading, altitude tuple - applied in that order
#[must_use]
pub fn to_euler<V: Float>(q: &[V; 4]) -> (V, V, V) {
    let zero = V::ZERO;
    let one = V::ONE;
    let two = one + one;
    let almost_half = f_const!(V, 499_999, 1_000_000);
    let halfpi = V::from_f32(1.570796326_f32).unwrap();
    let i = q[0];
    let j = q[1];
    let k = q[2];
    let r = q[3];
    let test = i * j + r * k;
    let (heading, attitude, bank) = {
        if test > almost_half {
            (two * V::atan2(i, r), halfpi, zero)
        } else if test < -almost_half {
            (-two * V::atan2(i, r), -halfpi, zero)
        } else {
            let i2 = i * i;
            let j2 = j * j;
            let k2 = k * k;
            (
                V::atan2(two * j * r - two * i * k, V::ONE - two * j2 - two * k2),
                V::asin(two * test),
                V::atan2(two * i * r - two * j * k, V::ONE - two * i2 - two * k2),
            )
        }
    };
    (bank, heading, attitude)
}

//fp apply3
/// Apply the quaternion to a vector3
#[must_use]
pub fn apply3<V: Float>(q: &[V; 4], v: &[V; 3]) -> [V; 3] {
    let one = V::ONE;
    let two = one + one;
    let (r, i, j, k) = as_rijk(q);
    let x = (r * r + i * i - j * j - k * k) * v[0]
        + two * (i * k + r * j) * v[2]
        + two * (i * j - r * k) * v[1];
    let y = (r * r - i * i + j * j - k * k) * v[1]
        + two * (j * i + r * k) * v[0]
        + two * (j * k - r * i) * v[2];
    let z = (r * r - i * i - j * j + k * k) * v[2]
        + two * (k * j + r * i) * v[1]
        + two * (k * i - r * j) * v[0];
    [x, y, z]
}

//fp apply4
/// Apply the quaternion to a vector3
#[must_use]
pub fn apply4<V: Float>(q: &[V; 4], v: &[V; 4]) -> [V; 4] {
    let one = V::ONE;
    let two = one + one;
    let (r, i, j, k) = as_rijk(q);
    let x = (r * r + i * i - j * j - k * k) * v[0]
        + two * (i * k + r * j) * v[2]
        + two * (i * j - r * k) * v[1];
    let y = (r * r - i * i + j * j - k * k) * v[1]
        + two * (j * i + r * k) * v[0]
        + two * (j * k - r * i) * v[2];
    let z = (r * r - i * i - j * j + k * k) * v[2]
        + two * (k * j + r * i) * v[1]
        + two * (k * i - r * j) * v[0];
    [x, y, z, v[3]]
}

//fp weighted_average_pair
/// Calculate the weighted average of two unit quaternions
///
/// w_a + w_b must be 1.
///
/// See <http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf>
/// Averaging Quaternions by F. Landis Markley
#[must_use]
pub fn weighted_average_pair<V: Float>(qa: &[V; 4], w_a: V, qb: &[V; 4], w_b: V) -> [V; 4] {
    let four = V::from_u8(4).unwrap();
    let (ra, ia, ja, ka) = as_rijk(qa);
    let (rb, ib, jb, kb) = as_rijk(qb);
    let w_diff = w_a - w_b;
    let q1_q2 = ra * rb + ia * ib + ja * jb + ka * kb;
    let z_sq = w_diff * w_diff + four * w_a * w_b * q1_q2 * q1_q2;
    let z = z_sq.sqrt();
    let rw_a_sq = w_a * (z + w_diff) / z / (z + w_a + w_b);
    let rw_b_sq = w_b * (z - w_diff) / z / (z + w_a + w_b);
    let rw_a = rw_a_sq.sqrt();
    let rw_b = rw_b_sq.sqrt() * q1_q2.signum();
    of_rijk(
        rw_a * ra + rw_b * rb,
        rw_a * ia + rw_b * ib,
        rw_a * ja + rw_b * jb,
        rw_a * ka + rw_b * kb,
    )
}

//fp weighted_average_many
/// Calculate the weighted average of many unit quaternions
///
/// weights need not add up to 1, but must be nonzero
///
/// This is an approximation compared to the Landis Markley paper
#[must_use]
pub fn weighted_average_many<I: Iterator<Item = (V, [V; 4])>, V: Float>(
    mut values_iter: I,
) -> [V; 4] {
    let (mut weight_ih, mut value_ih) = values_iter
        .next()
        .expect("weighted_average_many MUST be invoked with at least one quaternion");
    let mut next_values = Vec::new();
    loop {
        if let Some((second_weight, second_value)) = values_iter.next() {
            let w12 = weight_ih + second_weight;
            let av = weighted_average_pair(
                &value_ih,
                weight_ih / w12,
                &second_value,
                second_weight / w12,
            );
            next_values.push((w12, av));
        } else {
            // No value to pair with in-hand; if there are none on the
            // list then the iterator only had one entry, so return that
            if next_values.is_empty() {
                return value_ih;
            } else {
                // Iterator must have returned at least 2n+1 (n>=1) entries
                //
                // next_values must be 'n' long already, make it n+1 and break
                next_values.push((weight_ih, value_ih));
                break;
            }
        }
        // In-hand values have been consumed, fill them up or finish
        if let Some((w, v)) = values_iter.next() {
            // Iterator must have returned at least 3 values now
            weight_ih = w;
            value_ih = v;
        } else {
            // Iterator has returned 2n values with n>=1
            break;
        }
    }
    if next_values.len() == 1 {
        next_values[0].1
    } else {
        let values_iter = next_values.into_iter();
        weighted_average_many(values_iter)
    }
}

//fp get_rotation_of_vec_to_vec
/// Get a quaternion that is a rotation of one vector to another
///
/// The vectors must be unit vectors
#[must_use]
pub fn rotation_of_vec_to_vec<V: Float>(a: &[V; 3], b: &[V; 3]) -> [V; 4] {
    let obtuse = vector::dot(a, b) < V::ZERO;
    let cp = vector::cross_product3(a, b);
    let sa = vector::length(&cp);
    let angle = sa.asin();
    let angle = if obtuse {
        V::from_f32(3.141592653_f32).unwrap() - angle
    } else {
        angle
    };
    of_axis_angle(&cp, angle)
}