请求实现AddAngleBetweenVectorsCost：IK场景下的代价项需求

可以实现类似AddAngleBetweenVectorsCost的代价项功能，核心是把角度约束的硬限制转化为软代价优化，具体方案如下：

• 核心逻辑：角度约束是强制两向量夹角等于目标值，代价项则是让夹角与目标值的偏差最小化。利用余弦值在[0,π]区间的单调性，通过计算当前夹角余弦值与目标夹角余弦值的平方差，来量化偏差作为代价输入。
• 实现步骤：
  1. 获取需要计算角度的两个目标向量（比如从骨骼节点导出的方向向量）；
  2. 计算当前两向量的余弦值：cos_current = dot(v1, v2) / (||v1|| * ||v2||)，注意处理零向量的边界情况；
  3. 计算目标夹角的余弦值cos_target = cos(desired_angle)；
  4. 将(cos_current - cos_target)²乘以权重后，加入到IK的优化目标中，权重用于调整该代价的优先级。
• 伪代码示例：

double AddAngleBetweenVectorsCost(Vector3d v1, Vector3d v2, double desired_angle_rad, double weight) {
    // 处理零向量避免除零错误
    double norm1 = v1.norm();
    double norm2 = v2.norm();
    if (norm1 < 1e-6 || norm2 < 1e-6) return 0.0;
    
    double cos_current = v1.dot(v2) / (norm1 * norm2);
    // 限制余弦值范围，避免数值误差导致的异常
    cos_current = std::clamp(cos_current, -1.0, 1.0);
    double cos_target = std::cos(desired_angle_rad);
    
    double cost = weight * std::pow(cos_current - cos_target, 2);
    return cost;
}

• 关键注意点：
  • 如果使用的IK框架支持自定义代价项，直接将上述逻辑封装后注册到优化器即可；
  • 权重参数需要根据场景调试，平衡该代价与其他IK目标（如位置跟踪）的优先级；
  • 针对周期性角度场景，需确保目标角度的取值范围合理，避免代价函数陷入局部最优。

内容的提问来源于stack exchange，提问作者Zev Minsky-Primus