dsh/dual_camera_system/tests/test_coord_utils.py

import sys
import os
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

import math
from core.coord_utils import spherical_to_pan_tilt, pan_tilt_to_vector, compute_sample_grid


def test_spherical_front():
    pan, tilt = spherical_to_pan_tilt(0.0, 0.0, 1.0)
    assert abs(pan - 0.0) < 1e-6
    assert abs(tilt - 0.0) < 1e-6


def test_spherical_right():
    pan, tilt = spherical_to_pan_tilt(1.0, 0.0, 0.0)
    assert abs(pan - 90.0) < 1e-6
    assert abs(tilt - 0.0) < 1e-6


def test_spherical_up():
    # pan is undefined at zenith (gimbal lock), so only tilt is asserted.
    pan, tilt = spherical_to_pan_tilt(0.0, 1.0, 0.0)
    assert abs(tilt - 90.0) < 1e-6


def test_pan_tilt_to_vector_roundtrip():
    for pan in [0, 45, 90, 180, 270]:
        for tilt in [-20, 0, 20]:
            x, y, z = pan_tilt_to_vector(pan, tilt)
            pan2, tilt2 = spherical_to_pan_tilt(x, y, z)
            assert abs(pan - pan2) < 1e-6 or abs(abs(pan - pan2) - 360) < 1e-6
            assert abs(tilt - tilt2) < 1e-6


def test_spherical_zero_vector():
    pan, tilt = spherical_to_pan_tilt(0.0, 0.0, 0.0)
    assert abs(pan - 0.0) < 1e-6
    assert abs(tilt - 0.0) < 1e-6


def test_spherical_down():
    pan, tilt = spherical_to_pan_tilt(0.0, -1.0, 0.0)
    assert abs(tilt - (-90.0)) < 1e-6


def test_spherical_pan_360_boundary():
    pan_in, tilt_in = 359.0, 0.0
    x, y, z = pan_tilt_to_vector(pan_in, tilt_in)
    pan_out, tilt_out = spherical_to_pan_tilt(x, y, z)
    assert abs(pan_out - pan_in) < 1e-6
    assert abs(tilt_out - tilt_in) < 1e-6


def test_compute_sample_grid_default():
    points = compute_sample_grid()
    assert len(points) == 36
    for point in points:
        assert isinstance(point, tuple)
        assert len(point) == 2


def test_compute_sample_grid_custom():
    points = compute_sample_grid(pan_range=(0.0, 90.0), tilt_layers=(-10.0, 0.0), pan_step=30.0)
    assert len(points) == 6
    for point in points:
        assert isinstance(point, tuple)
        assert len(point) == 2
        pan, tilt = point
        assert 0.0 <= pan < 90.0
        assert tilt in (-10.0, 0.0)