给我一份复杂的空间圆弧轨迹算法代码

以下是一个复杂的空间圆弧轨迹算法代码，它使用了三次贝塞尔曲线和插值法来生成平滑的圆弧轨迹：

// 计算三次贝塞尔曲线上的点
Point3D bezier(Point3D p0, Point3D p1, Point3D p2, Point3D p3, double t) {
    double t2 = t * t;
    double t3 = t2 * t;
    double u = 1 - t;
    double u2 = u * u;
    double u3 = u2 * u;
    double x = u3 * p0.x + 3 * u2 * t * p1.x + 3 * u * t2 * p2.x + t3 * p3.x;
    double y = u3 * p0.y + 3 * u2 * t * p1.y + 3 * u * t2 * p2.y + t3 * p3.y;
    double z = u3 * p0.z + 3 * u2 * t * p1.z + 3 * u * t2 * p2.z + t3 * p3.z;
    return Point3D(x, y, z);
}

// 计算圆弧插值点
Point3D interpolateArc(Point3D start, Point3D end, Point3D center, double radius, bool clockwise, double t) {
    // 计算起点和终点相对于圆心的向量
    Vector3D startToCenter = center - start;
    Vector3D endToCenter = center - end;
    
    // 计算圆心角度
    double startAngle = atan2(startToCenter.y, startToCenter.x);
    double endAngle = atan2(endToCenter.y, endToCenter.x);
    double angle = clockwise ? (startAngle - endAngle) : (endAngle - startAngle);
    
    // 确保角度是正值
    if (angle < 0) {
        angle += 2 * M_PI;
    }
    
    // 计算圆心角度对应的参数值
    double t0 = 0;
    double t1 = angle / (2 * M_PI);
    
    // 计算起点和终点对应的参数值
    double tStart = t0 + (t1 - t0) * 0.1;
    double tEnd = t1 - (t1 - t0) * 0.1;
    
    // 计算起点、终点和圆心对应的贝塞尔控制点
    Point3D startControl = start + startToCenter * 0.33;
    Point3D endControl = end + endToCenter * 0.33;
    Vector3D startToEnd = end - start;
    Vector3D startToCenterNormalized = startToCenter.normalized();
    Vector3D startToEndNormalized = startToEnd.normalized();
    Point3D centerControl = center + startToCenterNormalized.cross(startToEndNormalized) * radius * 0.33;
    
    // 计算三次贝塞尔曲线上的点
    Point3D p0 = start;
    Point3D p1 = startControl;
    Point3D p2 = centerControl;
    Point3D p3 = endControl;
    Point3D p4 = end;
    if (t < tStart) {
        return p0;
    } else if (t < tEnd) {
        double tNormalized = (t - tStart) / (tEnd - tStart);
        return bezier(p0, p1, p2, p3, tNormalized);
    } else {
        return p4;
    }
}

// 生成圆弧轨迹
void generateArcTrajectory(Point3D start, Point3D end, Point3D center, double radius, bool clockwise, double step) {
    // 计算圆心角度
    Vector3D startToCenter = center - start;
    Vector3D endToCenter = center - end;
    double startAngle = atan2(startToCenter.y, startToCenter.x);
    double endAngle = atan2(endToCenter.y, endToCenter.x);
    double angle = clockwise ? (startAngle - endAngle) : (endAngle - startAngle);
    if (angle < 0) {
        angle += 2 * M_PI;
    }
    
    // 计算插补步长
    if (step < 0.01) {
        step = 0.01;
    }
    
    // 从起点开始插补
    for (double t = 0; t <= 1; t += step / angle) {
        // 计算插值点
        Point3D p = interpolateArc(start, end, center, radius, clockwise, t);
        
        // 输出当前点的坐标
        printf("(%lf, %lf, %lf)\n", p.x, p.y, p.z);
    }
}

在这个代码中，我们首先定义了一个计算三次贝塞尔曲线上的点的函数 `bezier`。然后，我们定义了一个计算圆弧插值点的函数 `interpolateArc`，该函数根据输入的起点、终点、圆心和半径，计算出圆弧上对应参数值为 `t` 的点。该函数首先计算起点和终点相对于圆心的向量，然后计算圆心角度。接下来，我们计算圆心角度对应的参数范围，并计算起点、终点和圆心对应的贝塞尔控制点。最后，我们根据三次贝塞尔曲线计算出插值点。

最后，我们定义了一个生成圆弧轨迹的函数 `generateArcTrajectory`。该函数首先计算圆心角度和插补步长。然后，我们从起点开始插补，逐步计算圆弧上的点的坐标，并输出每个点的坐标。在计算每个点的坐标时，我们使用 `interpolateArc` 函数来插值计算。