box3d-rust 0.2.1

// Port of box3d-cpp-reference/test/test_math.c
// SPDX-FileCopyrightText: 2023 Erin Catto
// SPDX-License-Identifier: MIT

use crate::core::is_double_precision;
use crate::math_functions::*;
use core::mem::size_of;

// 0.0023 degrees
const ATAN_TOL: f32 = 0.00004;

fn ensure_small(value: f32, tolerance: f32) {
    // Matches the C ENSURE_SMALL macro, which is inclusive: pass when
    // -tol <= value <= tol.
    assert!(
        !(value < -tolerance || tolerance < value),
        "|{value}| > tolerance {tolerance}"
    );
}

#[test]
fn cos_sin_atan2_over_angle_sweep() {
    let mut t = -10.0f32;
    while t < 10.0 {
        let angle = PI * t;
        let cs = compute_cos_sin(angle);
        let c = angle.cos();
        let s = angle.sin();

        // The cosine and sine approximations are accurate to about 0.1 degrees (0.002 radians)
        ensure_small(cs.cosine - c, 0.002);
        ensure_small(cs.sine - s, 0.002);

        let xn = unwind_angle(angle);
        let a = atan2(s, c);
        assert!(is_valid_float(a));

        let mut diff = abs_float(a - xn);

        // The two results can be off by 360 degrees (-pi and pi)
        if diff > PI {
            diff -= 2.0 * PI;
        }

        // The approximate atan2 is quite accurate
        ensure_small(diff, ATAN_TOL);

        t += 0.01;
    }
}

#[test]
fn atan2_matches_std_atan2_on_grid() {
    let mut y = -1.0f32;
    while y <= 1.0 {
        let mut x = -1.0f32;
        while x <= 1.0 {
            let a1 = atan2(y, x);
            let a2 = y.atan2(x);
            let diff = abs_float(a1 - a2);
            assert!(is_valid_float(a1));
            ensure_small(diff, ATAN_TOL);
            x += 0.01;
        }
        y += 0.01;
    }
}

#[test]
fn atan2_axis_cases() {
    for (y, x) in [
        (1.0f32, 0.0f32),
        (-1.0, 0.0),
        (0.0, 1.0),
        (0.0, -1.0),
        (0.0, 0.0),
    ] {
        let a1 = atan2(y, x);
        let a2 = y.atan2(x);
        let diff = abs_float(a1 - a2);
        assert!(is_valid_float(a1));
        ensure_small(diff, ATAN_TOL);
    }
}

#[test]
fn vector_ops() {
    let zero = VEC3_ZERO;
    let one = Vec3 {
        x: 1.0,
        y: 1.0,
        z: 1.0,
    };
    let two = Vec3 {
        x: 2.0,
        y: 2.0,
        z: 2.0,
    };

    let v = add(one, two);
    assert!(v.x == 3.0 && v.y == 3.0);

    let v = sub(zero, two);
    assert!(v.x == -2.0 && v.y == -2.0);

    let v = add(two, two);
    assert!(v.x != 5.0 && v.y != 5.0);
}

#[test]
fn transform_composition_and_inverse() {
    let two = Vec3 {
        x: 2.0,
        y: 2.0,
        z: 2.0,
    };

    let axis = normalize(Vec3 {
        x: -0.75,
        y: 0.5,
        z: 1.0,
    });
    let transform1 = Transform {
        p: Vec3 {
            x: -2.0,
            y: 3.0,
            z: 0.0,
        },
        q: QUAT_IDENTITY,
    };
    let transform2 = Transform {
        p: Vec3 {
            x: 1.0,
            y: 0.0,
            z: 0.0,
        },
        q: make_quat_from_axis_angle(axis, PI),
    };

    let transform = mul_transforms(transform2, transform1);

    let v = transform_point(transform2, transform_point(transform1, two));
    let u = transform_point(transform, two);

    ensure_small(u.x - v.x, 10.0 * f32::EPSILON);
    ensure_small(u.y - v.y, 10.0 * f32::EPSILON);

    let v = transform_point(transform1, two);
    let v = inv_transform_point(transform1, v);

    ensure_small(v.x - two.x, 8.0 * f32::EPSILON);
    ensure_small(v.y - two.y, 8.0 * f32::EPSILON);

    let rel_transform = inv_mul_transforms(transform1, transform2);
    let v = inv_transform_point(transform1, transform_point(transform2, two));
    let u = transform_point(rel_transform, two);
    ensure_small(u.x - v.x, 10.0 * f32::EPSILON);
    ensure_small(u.y - v.y, 10.0 * f32::EPSILON);
}

#[test]
fn quat_between_vectors_and_mul() {
    let axis = Vec3 {
        x: 0.0,
        y: 0.0,
        z: 1.0,
    };
    let q1 = make_quat_from_axis_angle(axis, -0.5 * PI);
    let q2 = compute_quat_between_unit_vectors(
        Vec3 {
            x: 1.0,
            y: 0.0,
            z: 0.0,
        },
        Vec3 {
            x: 0.0,
            y: -1.0,
            z: 0.0,
        },
    );

    ensure_small(q1.v.x - q2.v.x, f32::EPSILON);
    ensure_small(q1.v.y - q2.v.y, f32::EPSILON);
    ensure_small(q1.v.z - q2.v.z, f32::EPSILON);
    ensure_small(q1.s - q2.s, f32::EPSILON);

    let q3 = normalize_quat(Quat {
        v: Vec3 {
            x: 1.0,
            y: -2.0,
            z: 3.0,
        },
        s: 4.0,
    });
    let q4 = inv_mul_quat(q3, q1);
    let q5 = mul_quat(q3, q4);
    ensure_small(q1.v.x - q5.v.x, f32::EPSILON);
    ensure_small(q1.v.y - q5.v.y, f32::EPSILON);
    ensure_small(q1.v.z - q5.v.z, f32::EPSILON);
    ensure_small(q1.s - q5.s, f32::EPSILON);

    let q6 = compute_quat_between_unit_vectors(
        Vec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Vec3 {
            x: 0.0,
            y: -1.0,
            z: 0.0,
        },
    );
    ensure_small(q6.s, f32::EPSILON);
    let _ = q6;
}

#[test]
fn quat_between_unit_vectors_grid() {
    let v = normalize(Vec3 {
        x: 0.2,
        y: -0.5,
        z: 3.0,
    });
    let mut z = -1.0f32;
    while z <= 1.0 {
        let mut y = -1.0f32;
        while y <= 1.0 {
            let mut x = -1.0f32;
            while x <= 1.0 {
                if x == 0.0 && y == 0.0 && z == 0.0 {
                    x += 0.02;
                    continue;
                }

                let u = normalize(Vec3 { x, y, z });

                let r = compute_quat_between_unit_vectors(v, u);
                assert!(is_valid_quat(r));

                let w = rotate_vector(r, v);

                ensure_small(
                    dot(r.v, cross(u, w)) - scalar_triple_product(r.v, u, w),
                    f32::EPSILON,
                );

                // The quaternion between vectors can have lots of round off error at large angles.
                ensure_small(w.x - u.x, 0.001);
                ensure_small(w.y - u.y, 0.001);
                ensure_small(w.z - u.z, 0.001);

                // Twist angle testing
                let mut twist = if r.s < 0.0 {
                    atan2(-r.v.z, -r.s)
                } else {
                    atan2(r.v.z, r.s)
                };
                twist *= 2.0;
                assert!((-PI..=PI).contains(&twist));

                x += 0.02;
            }
            y += 0.02;
        }
        z += 0.02;
    }
}

#[test]
fn twist_angle_polarity_case() {
    // More twist angle testing
    let q = Quat {
        v: Vec3 {
            x: -0.0558656752,
            y: -0.188799798,
            z: 0.00689807534,
        },
        s: -0.980401039,
    };
    let mut twist = if q.s < 0.0 {
        atan2(-q.v.z, -q.s)
    } else {
        atan2(q.v.z, q.s)
    };
    twist *= 2.0;
    assert!((-PI..=PI).contains(&twist));
}

#[test]
fn matrix3_invert_and_solve() {
    let m = Matrix3 {
        cx: Vec3 {
            x: 3.0,
            y: 1.0,
            z: -1.0,
        },
        cy: Vec3 {
            x: -1.0,
            y: 3.0,
            z: 1.0,
        },
        cz: Vec3 {
            x: 1.0,
            y: -1.0,
            z: 3.0,
        },
    };
    let inv_m = invert_matrix(m);
    let a = mul_mm(m, inv_m);
    ensure_small(a.cx.x - 1.0, f32::EPSILON);
    ensure_small(a.cx.y, f32::EPSILON);
    ensure_small(a.cx.z, f32::EPSILON);
    ensure_small(a.cy.x, f32::EPSILON);
    ensure_small(a.cy.y - 1.0, f32::EPSILON);
    ensure_small(a.cy.z, f32::EPSILON);
    ensure_small(a.cz.x, f32::EPSILON);
    ensure_small(a.cz.y, f32::EPSILON);
    ensure_small(a.cz.z - 1.0, f32::EPSILON);

    let v = Vec3 {
        x: 1.0,
        y: -2.0,
        z: 3.0,
    };
    let u = mul_mv(inv_m, mul_mv(m, v));
    ensure_small(v.x - u.x, f32::EPSILON);
    ensure_small(v.y - u.y, f32::EPSILON);
    ensure_small(v.z - u.z, f32::EPSILON);

    let w = mul_mv(inv_m, v);
    let u = solve3(m, v);
    ensure_small(w.x - u.x, f32::EPSILON);
    ensure_small(w.y - u.y, f32::EPSILON);
    ensure_small(w.z - u.z, f32::EPSILON);
}

#[test]
fn mat2_invert_and_solve() {
    let m = Mat2 {
        cx: Vec2 { x: 3.0, y: 1.0 },
        cy: Vec2 { x: -1.0, y: 3.0 },
    };
    let inv_m = invert2(m);
    let a = mul_mm2(m, inv_m);
    ensure_small(a.cx.x - 1.0, f32::EPSILON);
    ensure_small(a.cx.y, f32::EPSILON);
    ensure_small(a.cy.x, f32::EPSILON);
    ensure_small(a.cy.y - 1.0, f32::EPSILON);

    let v2 = Vec2 { x: 1.0, y: -2.0 };
    let u2 = mul_mv2(inv_m, mul_mv2(m, v2));
    ensure_small(v2.x - u2.x, f32::EPSILON);
    ensure_small(v2.y - u2.y, f32::EPSILON);

    let w = mul_mv2(inv_m, v2);
    let u2 = solve2(m, v2);
    ensure_small(w.x - u2.x, f32::EPSILON);
    ensure_small(w.y - u2.y, f32::EPSILON);

    let w = mul_mv2(m, u2);
    ensure_small(w.x - v2.x, 10.0 * f32::EPSILON);
    ensure_small(w.y - v2.y, 10.0 * f32::EPSILON);
}

#[test]
fn float_double_cast_round_trip() {
    // Deterministic sequential values stand in for C's RandomFloat: assert the
    // float↔double cast round-trips exactly.
    let mut a = -50.0f32;
    for _ in 0..100 {
        let b = a as f64;
        let c = b as f32;
        assert_eq!(c, a);
        a += 1.013579;
    }
}

#[test]
fn nlerp_twist_error_bound() {
    let q1 = QUAT_IDENTITY;
    let q2 = make_quat_from_axis_angle(VEC3_AXIS_Z, 0.5 * PI);
    let n = 100;
    for i in 0..=n {
        let alpha = i as f32 / n as f32;
        let q = nlerp(q1, q2, alpha);
        let angle = get_twist_angle(q);
        ensure_small(alpha * 0.5 * PI - angle, 1.0 * DEG_TO_RAD);
    }
}

#[test]
fn arbitrary_perp_is_orthogonal() {
    let normal = Vec3 {
        x: 0.504055440,
        y: 0.621548057,
        z: 0.599671543,
    };
    let perp = arbitrary_perp(normal);
    ensure_small(dot(normal, perp), 2.0 * f32::EPSILON);
}

#[test]
fn world_position_boundary_helpers() {
    // World position boundary helpers. The query agrees with the built type sizes.
    assert_eq!(is_double_precision(), size_of::<Pos>() > size_of::<Vec3>());

    // Deltas and offsets round trip exactly for representable inputs in both modes.
    let a = Vec3 {
        x: 3.0,
        y: -5.0,
        z: 2.0,
    };
    let b = Vec3 {
        x: 1.0,
        y: 4.0,
        z: -6.0,
    };
    let pa = to_pos(a);
    let pb = to_pos(b);

    let d = sub_pos(pa, pb);
    let sub_ab = sub(a, b);
    assert!(d.x == sub_ab.x && d.y == sub_ab.y && d.z == sub_ab.z);

    let back = sub_pos(offset_pos(pb, sub_ab), pa);
    assert!(back.x == 0.0 && back.y == 0.0 && back.z == 0.0);

    let r = to_vec3(pa);
    assert!(r.x == a.x && r.y == a.y && r.z == a.z);

    assert!(is_valid_position(pa));

    // World transform relative ops match the pure float transform ops. Float mode is
    // bit identical, double mode keeps the relative result in float.
    let axis = normalize(Vec3 {
        x: 0.3,
        y: -0.7,
        z: 0.5,
    });
    let t_a = Transform {
        p: a,
        q: make_quat_from_axis_angle(axis, 0.4),
    };
    let t_b = Transform {
        p: b,
        q: make_quat_from_axis_angle(axis, -1.1),
    };
    let w_a = make_world_transform(t_a);
    let w_b = make_world_transform(t_b);
    assert!(is_valid_world_transform(w_a));

    let rel_ref = inv_mul_transforms(t_a, t_b);
    let rel = inv_mul_world_transforms(w_a, w_b);
    ensure_small(rel.p.x - rel_ref.p.x, 1.0e-5);
    ensure_small(rel.p.y - rel_ref.p.y, 1.0e-5);
    ensure_small(rel.p.z - rel_ref.p.z, 1.0e-5);
    ensure_small(rel.q.s - rel_ref.q.s, 1.0e-5);

    // Local point to world and back.
    let local = Vec3 {
        x: 0.5,
        y: -0.25,
        z: 1.5,
    };
    let back2 = inv_transform_world_point(w_a, transform_world_point(w_a, local));
    ensure_small(back2.x - local.x, 1.0e-5);
    ensure_small(back2.y - local.y, 1.0e-5);
    ensure_small(back2.z - local.z, 1.0e-5);

    // Compose with a local transform, then strip it back off.
    let rel_ab = inv_mul_world_transforms(w_a, mul_world_transforms(w_a, t_b));
    ensure_small(rel_ab.p.x - t_b.p.x, 1.0e-5);
    ensure_small(rel_ab.p.y - t_b.p.y, 1.0e-5);
    ensure_small(rel_ab.p.z - t_b.p.z, 1.0e-5);
}

/// Far from the origin the double layer keeps the relative result accurate where pure
/// float would quantize. Two poses one meter apart at x = 1e8.
#[cfg(feature = "double-precision")]
#[test]
fn large_world_relative_transform_at_1e8() {
    let base = Pos {
        x: 1.0e8,
        y: 0.0,
        z: 0.0,
    };
    let w_a = WorldTransform {
        p: base,
        q: QUAT_IDENTITY,
    };
    let w_b = WorldTransform {
        p: offset_pos(
            base,
            Vec3 {
                x: 1.0,
                y: 0.0,
                z: 0.0,
            },
        ),
        q: QUAT_IDENTITY,
    };
    let rel = inv_mul_world_transforms(w_a, w_b);
    assert!(rel.p.x == 1.0 && rel.p.y == 0.0 && rel.p.z == 0.0);
}