def facing_to_world(pos_root, quat_root, point_facing):
    """
    convert the points from the facing coordinate system to the world coordinate system
    :param pos_root: the root position in the world coordinate system
    :param quat_root: the root orientation (quaternion, xyzw) in the world coordinate system
    :param point_facing: positions of the points in the facing coordinate system
    :return: positions of the points in the world coordinate system
    """
    R_root_to_world = R.from_quat(quat_root).as_matrix()

    x_axis_world = R_root_to_world[:, 0]
    y_axis_world = R_root_to_world[:, 1]

    x_axis_ground = np.array([x_axis_world[0], x_axis_world[1], 0.0])
    y_axis_ground = np.array([y_axis_world[0], y_axis_world[1], 0.0])

    x_axis_facing = x_axis_ground / np.linalg.norm(x_axis_ground)
    z_axis_facing = np.array([0.0, 0.0, 1.0])
    y_axis_facing = np.cross(z_axis_facing, x_axis_facing)

    R_facing_to_root = np.vstack([x_axis_facing, y_axis_facing, z_axis_facing]).T

    point_root = R_facing_to_root @ point_facing

    point_world = R_root_to_world @ point_root + pos_root

    return point_world

    convert the points from the facing coordinate system to the world coordinate system
    :param pos_root: the root position in the world coordinate system
    :param quat_root: the root orientation (quaternion, xyzw) in the world coordinate system
    :param point_facing: positions of the points in the facing coordinate system
    :return: positions of the points in the world coordinate system
    
deploy/helpers/rotation_helper.py