1. 三阶 Bézier 曲线定义
B(t) = (1-t)^3P0 + 3 (1-t)^2t P1 + 3*(1-t)t^2P2 + t^3*P3, t ∈ [0,1]
其中： P0：起点 P3：终点 P1, P2：控制点
2. 输入条件
设： P0 = (x0, y0) // 起点坐标
P3 = (x3, y3) // 终点坐标
T0 = (cos θ0, sin θ0) // 起点切线方向单位向量
T3 = (cos θ3, sin θ3) // 终点切线方向单位向量
κ0：起点曲率（标量）
κ3：终点曲率（标量）
3. 控制点参数化
引入参数 s0 和 s3，分别表示起点和终点处的"速度幅值"：
P1 = P0 + (s0/3) * T0
P2 = P3 - (s3/3) * T3
目标：通过牛顿迭代法求解 s0 和 s3，使得 Bézier 曲线在端点处的曲率等于给定值 κ0 和 κ3。
4. 曲率公式（端点处）
在 t = 0 处的曲率近似公式为：
κ0 ≈ | B'(0) × B''(0) | / |B'(0)|^3
计算导数： B'(0) = 3*(P1 - P0) = s0 * T0
B''(0) = 6*(P2 - 2*P1 + P0)
代入 P1 和 P2：
令 ΔP = P3 - P0
则： P1 = P0 + (s0/3)*T0
P2 = P3 - (s3/3)*T3
B''(0) = 6*[ (P3 - (s3/3)T3) - 2 (P0 + (s0/3)T0) + P0 ]
= 6 [ P3 - P0 - (2*s0/3)T0 - (s3/3)T3 ]
= 6[ ΔP - (2s0/3)*T0 - (s3/3)*T3 ]
二维叉积（标量）： a × b = a_xb_y - a_yb_x
则： B'(0) × B''(0) = (s0T0) × {6 [ΔP - (2s0/3)T0 - (s3/3)T3]}
= 6s0 * [ T0 × ΔP - (2s0/3) (T0 × T0) - (s3/3)(T0 × T3) ]
= 6s0 * [ T0 × ΔP - (s3/3)*(T0 × T3) ] （因 T0 × T0 = 0）
|B'(0)|^3 = (s0)^3
所以： κ0 = |6s0 * [ T0 × ΔP - (s3/3) (T0 × T3) ]| / (s0)^3
= 6 * | T0 × ΔP - (s3/3)*(T0 × T3) | / (s0)^2
同理，在 t = 1 处：
κ3 = 6 * | T3 × (-ΔP) - (s0/3)*(T3 × T0) | / (s3)^2
注意：T3 × T0 = - (T0 × T3)
令： C03 = T0 × T3 = cosθ0sinθ3 - sinθ0 cosθ3 = sin(θ3 - θ0)
D0 = T0 × ΔP
D3 = T3 × (P0 - P3) = T3 × (-ΔP)
则曲率方程为：
(1) κ0 = 6 * | D0 - (s3/3)*C03 | / (s0)^2
(2) κ3 = 6 * | D3 + (s0/3)*C03 | / (s3)^2
5. 牛顿迭代法求解 s0 和 s3
定义变量： x = [s0, s3]^T
定义残差函数： F(x) = [ f1(s0,s3), f2(s0,s3) ]^T
其中： f1(s0,s3) = 6 * | D0 - (s3/3)*C03 | / (s0)^2 - κ0
f2(s0,s3) = 6 * | D3 + (s0/3)*C03 | / (s3)^2 - κ3
为简化，假设符号已知（例如曲率方向一致），可暂时去掉绝对值（迭代中根据符号调整）：
假设： D0 - (s3/3)*C03 ≥ 0
D3 + (s0/3)*C03 ≥ 0
则： f1 = 6*( D0 - (s3/3)C03 ) / (s0)^2 - κ0
f2 = 6( D3 + (s0/3)*C03 ) / (s3)^2 - κ3
雅可比矩阵 J = [ ∂f1/∂s0, ∂f1/∂s3 ; ∂f2/∂s0, ∂f2/∂s3 ]
计算偏导数：
∂f1/∂s0 = 6*( D0 - (s3/3)C03 ) * (-2) / (s0)^3
= -12( D0 - (s3/3)*C03 ) / (s0)^3
∂f1/∂s3 = 6 * (-C03/3) / (s0)^2 = -2*C03 / (s0)^2
∂f2/∂s0 = 6 * (C03/3) / (s3)^2 = 2*C03 / (s3)^2
∂f2/∂s3 = 6*( D3 + (s0/3)C03 ) * (-2) / (s3)^3
= -12( D3 + (s0/3)*C03 ) / (s3)^3
6. 牛顿迭代步骤
-
初始化：
选择初始值 s0^(0), s3^(0)（例如基于弦长：s0 = |P3-P0|, s3 = |P3-P0|）设置容差 ε（如 1e-6），最大迭代次数 N
-
对 k = 0,1,2,...,N-1： a. 计算当前残差： f1_k = 6*( D0 - (s3_k/3)C03 ) / (s0_k)^2 - κ0
f2_k = 6 ( D3 + (s0_k/3)C03 ) / (s3_k)^2 - κ3 b. 若 |f1_k| < ε 且 |f2_k| < ε，收敛，跳出 c. 计算雅可比矩阵元素： J11 = -12 ( D0 - (s3_k/3)C03 ) / (s0_k)^3
J12 = -2 C03 / (s0_k)^2J21 = 2C03 / (s3_k)^2
J22 = -12 ( D3 + (s0_k/3)*C03 ) / (s3_k)^3 d. 解线性方程组： [J11 J12] [Δs0] [-f1_k]J21 J22\] \[Δs3\] = \[-f2_k\] e. 更新： s0_{k+1} = s0_k + Δs0 s3_{k+1} = s3_k + Δs3 f. 若 Δs 很小，收敛
7. 计算控制点
P1 = P0 + (s0/3) * T0
P2 = P3 - (s3/3) * T3
8. 注意事项
- 若 κ0 = 0 或 κ3 = 0，表示直线段，可单独处理（如设 s0 较大）
- 叉积符号反映曲率方向，迭代中需检查绝对值内的表达式符号，必要时调整
- 初始值选择影响收敛性，建议基于几何意义初始化（如 s0 ≈ √(|D0|/κ0) 等）