# ===== Open3D helpers =====
import numpy as np
import open3d as o3d

def o3d_sphere(center, radius=0.015, color=(1.0, 0.0, 0.0)):
    sp = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
    sp.compute_vertex_normals()
    sp.paint_uniform_color(color)
    T = np.eye(4)
    T[:3, 3] = center
    sp.transform(T)
    return sp

def o3d_lineset(points, color=(1.0, 0.0, 0.0)):
    # points: shape (N,3) — we’ll connect 0-1, 1-2, 2-3
    pts = o3d.utility.Vector3dVector(points)
    lines = o3d.utility.Vector2iVector([[0,1],[1,2],[2,3]])
    ls = o3d.geometry.LineSet(points=pts, lines=lines)
    cols = [color, color, color]
    ls.colors = o3d.utility.Vector3dVector(cols)
    return ls

def o3d_frame(R=np.eye(3), t=np.zeros(3), size=0.08):
    fr = o3d.geometry.TriangleMesh.create_coordinate_frame(size=size)
    T = np.eye(4)
    T[:3,:3] = R
    T[:3, 3] = t
    fr.transform(T)
    return fr