"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。

对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
（规格：docs/物理追踪思路.md §2.3）。

速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶
小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机
体系完成、旋转在输出端执行"不破坏 §8 限幅约束。

正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
  机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东；
  机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。
"""

import math


def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float,
                       yaw_rad: float) -> tuple[float, float, float]:
    """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。"""
    c = math.cos(yaw_rad)
    s = math.sin(yaw_rad)
    north = v_forward * c - v_right * s
    east = v_forward * s + v_right * c
    return north, east, v_down