匀速二阶贝塞尔曲线（二）

根据上一篇文章介绍的方法进行匀速贝塞尔曲线运动时，实际使用时会发现两个问题。

计算失效问题

当\(A=0\)，此时不能用\(L(t)\)计算二阶贝塞尔曲线的长度：

\[\begin{align} L(t) =& \frac{1}{8A^\frac{3}{2}} \big(T_1T_0-T_2ln(T_1+T_0) \\ &- BT_3+T_2ln(B+T_3) \big) \\ T_0 =& 2\sqrt{A}\sqrt{C+Bt+At^2} \\ T_1 =& B+2At \\ T_2 =& B^2-4AC \\ T_3 =& 2\sqrt{AC} \end{align} \]

同时当\(P_0\) \(P_1\) \(P_2\)三点共线时，\(L(1.0)=Nan\)，需要使用匀速直线运动方法进行计算。

加速度问题

当把Python代码中三个点的坐标替换为：

python 复制代码

P0 = np.array([636.593750,684.976562])  # 起点
P1 = np.array([636.593750,685.218750])  # 控制点
P2 = np.array([672.511719,682.589844])  # 终点

会得到如下结果：

通过打印\(t_n\)的值发现其部分\(>1\)，通过回顾\(t_n\)的计算公式：

\[\begin{align} t_n=t_{n-1}+\frac{\frac{n}{N}L(1.0)-L(t_{n-1})}{s(t_{n-1})} \quad,\quad n\in[1, N] \end{align} \]

我们只考虑了速度\(s(t)\)，并没有考虑加速度，通过对\(V(t)\)求导可以得到加速度：

\[\begin{align} F(t)=&V'(t) \\ =&2 \begin{bmatrix} x_0 \\ y_0 \\ \end{bmatrix} -4 \begin{bmatrix} x_1 \\ y_1 \\ \end{bmatrix} +2 \begin{bmatrix} x_2 \\ y_2 \\ \end{bmatrix} \end{align} \]

同样对其取模，舍弃方向：

\[\begin{align} f =& \Vert F(t) \Vert \\ =& \sqrt{a_x^2+a_y^2} \\ =& \sqrt{A} \end{align} \]

加速度为固定值，也就是恒加速度运动。最终\(t_n\)公式修改为：

\[\begin{align} t_n =& t_{n-1} + \Delta{t} \\ \Delta{t} =& \frac{-s(t_{n-1})+\sqrt{s^2(t_{n-1})+2f\Big(\frac{n}{N}L(1.0)-L(t_{n-1})\Big)}}{f} \end{align} \]

将之前的Python进行修改如下：

python 复制代码

import numpy as np
import matplotlib.pyplot as plt
import sys


def bezier_length(A, B, C, t):  # 二阶贝塞尔曲线长度
    T0 = 2 * np.sqrt(A) * np.sqrt(C + B * t + A * t * t)
    T1 = B + 2 * A * t
    T2 = B * B - 4 * A * C
    T3 = 2 * np.sqrt(A * C)
    return 1 / (8 * np.pow(A, 1.5)) * (T0 * T1 - T2 * np.log(T1 + T0) - B * T3 + T2 * np.log(B + T3))


def bezier_velocity(A, B, C, t):  # 二阶贝塞尔曲线速度
    return np.sqrt(C + B * t + A * t * t)


def bezier_movement(p0, p1, p2, N):  # 二阶贝塞尔曲线等距移动
    a = 2 * p0 - 4 * p1 + 2 * p2
    b = -2 * p0 + 2 * p1
    A = a[0] * a[0] + a[1] * a[1]
    B = 2 * a[0] * b[0] + 2 * a[1] * b[1]
    C = b[0] * b[0] + b[1] * b[1]
    f = np.sqrt(A)
    L1 = bezier_length(A, B, C, 1)
    print(A, B, C, L1)
    tn = 0
    res = np.zeros((N + 1, 2))
    for n in range(N + 1):
        print(tn)
        res[n] = (1 - tn) * (1 - tn) * p0 + 2 * tn * (1 - tn) * p1 + tn * tn * p2
        v = bezier_velocity(A, B, C, tn)
        l = ((n + 1) / N * L1 - bezier_length(A, B, C, tn))
        delta_t = (-v + np.sqrt(v * v + 2 * f * l)) / f
        tn = tn + delta_t
    return res


P0 = np.array([636.593750, 684.976562])  # 起点
P1 = np.array([636.593750, 685.218750])  # 控制点
P2 = np.array([672.511719, 682.589844])  # 终点

N = 10
if len(sys.argv) > 1:
    N = int(sys.argv[1])

curve = bezier_movement(P0, P1, P2, N)

plt.figure(figsize=(10, 6))

plt.plot([P0[0], P1[0], P2[0]], [P0[1], P1[1], P2[1]], 'ro--', label='Control-Line')
plt.plot(curve[:, 0], curve[:, 1], 'o', linewidth=2, label='Bezier movement')

plt.text(P0[0], P0[1], 'P0', fontsize=12, ha='right', va='top')
plt.text(P1[0], P1[1], 'P1', fontsize=12, ha='center', va='bottom')
plt.text(P2[0], P2[1], 'P2', fontsize=12, ha='left', va='top')

plt.title('Bezier Curve(N=' + str(N) + ')', fontsize=14)
plt.xlabel('X-axis', fontsize=12)
plt.ylabel('Y-axis', fontsize=12)
plt.grid(True, linestyle='--', alpha=0.7)
plt.legend()
plt.axis('equal')
plt.show()

最终运行结果如下图所示：