Repository: Genesis-Embodied-AI/Genesis
File: tests/test_utils.py (Python)

Code before:
    assert a.sqr_magnitude() == pytest.approx(1.0 + 4.0 + 9.0)
    assert a.magnitude() == pytest.approx(np.sqrt(a.sqr_magnitude()))
    na = a.normalized()
    assert na.v.dtype == np.float32
    assert na.magnitude() == pytest.approx(1.0, rel=1e-6, abs=1e-6)
    assert np.allclose(Vec3.zero().normalized().v, [0.0, 0.0, 0.0])

    # copy is deep for underlying array
    cp = a.copy()
    assert cp is not a
    cp.v[...] = 0.0
    assert np.allclose(a.v, [1.0, 2.0, 3.0])
    assert np.allclose(cp.v, [0.0, 0.0, 0.0])

    # repr and tensor conversion
    assert "Vec3(" in repr(a)
    t = a.as_tensor()
    assert isinstance(t, torch.Tensor)
    assert t.dtype == torch.float32
    assert np.allclose(t.cpu().numpy(), a.v)

    # constructor guards
    with pytest.raises(AssertionError):
        Vec3(np.array([1, 2, 3, 4], dtype=np.float32))
    with pytest.raises(AssertionError):
        Vec3(np.array([1, 2, 3], dtype=np.float64))

    # --- Quat tests ---
    q = Quat.from_wxyz(1.0, 0.0, 0.0, 0.0)  # identity
    assert q.v.shape == (4,)
    assert q.v.dtype == np.float32
    assert q.w == pytest.approx(1.0)
    assert q.x == pytest.approx(0.0)
    assert q.y == pytest.approx(0.0)
    assert q.z == pytest.approx(0.0)

    # from_array converts dtype and enforces shape
    q_arr = Quat.from_array(np.array([0.5, 0.5, -0.5, 0.5], dtype=np.float64))
    assert q_arr.v.dtype == np.float32
    assert np.allclose(q_arr.v, [0.5, 0.5, -0.5, 0.5])

    # from_tensor
    q_t = Quat.from_tensor(torch.tensor([0.0, 1.0, 0.0, 0.0], dtype=torch.float32))
    assert np.allclose(q_t.v, [0.0, 1.0, 0.0, 0.0])

    # inverse
    q_inv = q_arr.get_inverse()
    assert q_inv.w == pytest.approx(q_arr.w)
    assert np.allclose(q_inv.v[1:], -q_arr.v[1:])

    # quat * quat (identity)

Review comment (suggestion):
This restriction is useless. I would not assert it. It would be better to do `self.v = np.asarray(v, dtype=np.float32)`

Code after:
    cp = a.copy()
    assert cp is not a
    cp.v[...] = 0.0
    assert_allclose(a.v, np.array([1.0, 2.0, 3.0]), tol=gs.EPS)
    assert_allclose(cp.v, 0.0, tol=gs.EPS)

    # repr and tensor conversion
    t = a.as_tensor()
    assert isinstance(t, torch.Tensor)
    assert_allclose(t.cpu().numpy(), a.v, tol=gs.EPS)

    # --- Quat tests ---
    q = Quat.from_wxyz(1.0, 0.0, 0.0, 0.0)  # identity
    assert q.v.shape == (4,)
    assert_allclose(np.array([q.w, q.x, q.y, q.z]), np.array([1.0, 0.0, 0.0, 0.0]), tol=gs.EPS)

    # from_array converts dtype and enforces shape
    q_arr = Quat.from_array(np.array([0.5, 0.5, -0.5, 0.5], dtype=np.float64))
    assert_allclose(q_arr.v, np.array([0.5, 0.5, -0.5, 0.5]), tol=gs.EPS)

    # from_tensor
    q_t = Quat.from_tensor(torch.tensor([0.0, 1.0, 0.0, 0.0], dtype=torch.float32))
    assert_allclose(q_t.v, np.array([0.0, 1.0, 0.0, 0.0]), tol=gs.EPS)

    # inverse
    q_inv = q_arr.get_inverse()
    assert_allclose(q_inv.w, q_arr.w, tol=gs.EPS)
    assert_allclose(q_inv.v[1:], -q_arr.v[1:], tol=gs.EPS)

    # quat * quat (identity)
    qq = q * q_arr
    assert_allclose(qq.v, q_arr.v, tol=gs.EPS)

    # rotation of a vector by 90deg about z: (1,0,0) -> (0,1,0)
    theta = np.pi / 2.0
    qz = Quat.from_wxyz(np.cos(theta / 2.0), 0.0, 0.0, np.sin(theta / 2.0))
    v_x = Vec3.from_xyz(1.0, 0.0, 0.0)
    v_rot = qz * v_x
    assert_allclose(v_rot.v, np.array([0.0, 1.0, 0.0]), tol=tol)

    # quat * quat inverse -> identity
    q_unit = qz * qz.get_inverse()
    assert_allclose(q_unit.v, Quat.from_wxyz(1.0, 0.0, 0.0, 0.0).v, tol=tol)

    # copy independence
    q_cp = qz.copy()
    assert q_cp is not qz
    q_cp.v[...] = 0.0
    assert_allclose(qz.v, np.array([np.cos(theta / 2.0), 0.0, 0.0, np.sin(theta / 2.0)]), tol=tol)
    assert_allclose(q_cp.v, np.array([0.0, 0.0, 0.0, 0.0]), tol=gs.EPS)