import numpy as np

def hat(w):
    wx, wy, wz = w
    return np.array([[0, -wz, wy],
                     [wz, 0, -wx],
                     [-wy, wx, 0]], dtype=float)


def rotvec_to_R(w):
    """Axis-angle (rotation vector) to rotation matrix."""
    th = np.linalg.norm(w)
    if th < 1e-12:
        return np.eye(3)
    k = w / th
    K = hat(k)
    return np.eye(3) + np.sin(th) * K + (1-np.cos(th)) * (K @ K)


def R_to_rotvec(R):
    """Rotation matrix to rotation vector (log map)."""
    tr = np.trace(R)
    cos_th = (tr - 1) / 2
    cos_th = np.clip(cos_th, -1.0, 1.0)
    th = np.arccos(cos_th)
    if th < 1e-12:
        return np.zeros(3)
    w_hat = (R - R.T) / (2*np.sin(th))
    return th * np.array([w_hat[2,1], w_hat[0,2], w_hat[1,0]])


def normalize(v, eps=1e-12):
    n = np.linalg.norm(v)
    if n < eps: 
        return v*0.0
    return v / n


def clip(x, a, b):
    return max(a, min(b, x))