用于轨迹规划的三次样条插值

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
deltaT = 0.001; %插补周期

n = length(t); % n >= 4
T = diff(t);
a = zeros(n - 2, 1);
b = zeros(n - 2, 1);
c = zeros(n - 2, 1);
d = zeros(n - 2, 1);

for i = 2 : n - 2
    a(i) = T(i + 1);
end
for i = 1 : n - 2
    b(i) = 2.0 * (T(i) + T(i + 1));
end
for i = 1 : n - 3
    c(i) = T(i);
end

d(1) = 3.0 * (T(1)^2 * (q(3) - q(2)) + T(2)^2 * (q(2) - q(1))) / (T(1) * T(2)) - T(2) * v0;
for i = 2 : n - 3
   d(i) = 3.0 * (T(i)^2 * (q(i + 2) - q(i + 1)) + T(i + 1)^2 * (q(i + 1) - q(i))) / (T(i) * T(i + 1)); 
end
d(n - 2) = 3.0 * (T(n - 2)^2 * (q(n) - q(n - 1)) + T(n - 1)^2 * (q(n - 1) - q(n - 2))) / (T(n - 2) * T(n - 1)) - T(n - 2) * vn;

[v, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n - 2);
if sta == 0
    error('三对角矩阵方程求解出错!');
end
v = [v0; v; vn];

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; 2.0 * (3.0 * (q(2) - q(1)) / T(1) - 2.0 * v(1) - v(2)) / T(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    b0 = q(i);
    b1 = v(i);
    b2 = (3.0 * (q(i + 1) - q(i)) / T(i) - 2.0 * v(i) - v(i + 1)) / T(i);
    b3 = (2.0 * (q(i) - q(i + 1)) / T(i) + v(i) + v(i + 1)) / T(i)^2;

    time = [time; tt];
    pos = [pos; b0 + (tt - t(i)) .* (b1 + (tt - t(i)) .* (b2 + b3 .* (tt - t(i))))];
    vel = [vel; b1 + (tt - t(i)) .* (2.0 * b2 + 3.0 * b3 .* (tt - t(i)))];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    b2 = (3.0 * (q(n) - q(n - 1)) / T(n - 1) - 2.0 * v(n - 1) - v(n)) / T(n - 1);
    b3 = (2.0 * (q(n - 1) - q(n)) / T(n - 1) + v(n - 1) + v(n)) / T(n - 1)^2;
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* T(n - 1)];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t, q, 'ro');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
hold on
plot([t(1), t(n)], [v0, vn], 'ro');
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
xlabel('t')
ylabel('acc')

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
deltaT = 0.001; %插补周期

n = length(t); % n >= 4
T = diff(t);
a = zeros(n, 1);
b = zeros(n, 1);
c = zeros(n, 1);
d = zeros(n, 1);

for i = 1 : n - 1
    a(i + 1) = T(i);
end

b(1) = 2.0 * T(1);
for i = 2 : n - 1
    b(i) = 2.0 * (T(i - 1) + T(i));
end
b(n) = 2.0 * T(n - 1);

for i = 1 : n - 1
    c(i) = T(i);
end

d(1) = 6.0 * ((q(2) - q(1)) / T(1) - v0);
for i = 2 : n - 1
   d(i) = 6.0 * ((q(i + 1) - q(i)) / T(i) - (q(i) - q(i - 1)) / T(i - 1)); 
end
d(n) = 6.0 * (vn - (q(n) - q(n - 1)) / T(n - 1));

[w, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n);
if sta == 0
    error('三对角矩阵方程求解出错!');
end

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; w(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    a0 = q(i);
    a1 = (q(i + 1) - q(i)) / T(i) - T(i) * (w(i + 1) + 2.0 * w(i)) / 6.0;
    a2 = 0.5 * w(i);
    a3 = (w(i + 1) - w(i)) / (6.0 * T(i));

    time = [time; tt];
    pos = [pos; a0 + (tt - t(i)) .* (a1 + (tt - t(i)) .* (a2 + a3 .* (tt - t(i))))];
    vel = [vel; a1 + (tt - t(i)) .* (2.0 * a2 + 3.0 * a3 .* (tt - t(i)))];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; w(n)];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t, q, 'ro');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
xlabel('t')
ylabel('acc')

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 3]'; %首尾数据一致
deltaT = 0.001; %插补周期
forwardPeriodCount = 2;
backwardPeriodCount = 1;

n = length(t);
T = diff(t);
a = zeros(n - 1, 1);
b = zeros(n - 1, 1);
c = zeros(n - 1, 1);
d = zeros(n - 1, 1);

a(1 : n - 1) = T(1 : n - 1);

b(1) = 2.0 * (T(n - 1) + T(1));
for i = 2 : n - 1
    b(i) = 2.0 * (T(i - 1) + T(i));
end

c(1) = T(n - 1);
for i = 2 : n - 2
    c(i) = T(i - 1);
end
c(n - 1) = T(n - 2);

d(1) = 3.0 * (T(n - 1)^2 * (q(2) - q(1)) + T(1)^2 * (q(n) - q(n - 1))) / (T(n - 1) * T(1));
for i = 2 : n - 1
    d(i) = 3.0 * (T(i - 1)^2 * (q(i + 1) - q(i)) + T(i)^2 * (q(i) - q(i - 1))) / (T(i - 1) * T(i));
end

[v, sta] = solve_cyclic_tridiagonal_matrix_equation(a, b, c, d, n - 1);
if sta == 0
    error('循环三对角矩阵方程求解出错!');
end
v = [v; v(1)];

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v(1)];
acc = [acc; 2.0 * (3.0 * (q(2) - q(1)) / T(1) - 2.0 * v(1) - v(2)) / T(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    a0 = q(i);
    a1 = v(i);
    a2 = (3.0 * (q(i + 1) - q(i)) / T(i) - 2.0 * v(i) - v(i + 1)) / T(i);
    a3 = (2.0 * (q(i) - q(i + 1)) / T(i) + v(i) + v(i + 1)) / T(i)^2;
    
    time = [time; tt];
    pos = [pos; a0 + (tt - t(i)) .* (a1 + (tt - t(i)) .* (a2 + a3 .* (tt - t(i))))];
    vel = [vel; a1 + (tt - t(i)) .* (2.0 * a2 + 3.0 * a3 .* (tt - t(i)))];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    a2 = (3.0 * (q(n) - q(n - 1)) / T(n - 1) - 2.0 * v(n - 1) - v(n)) / T(n - 1);
    a3 = (2.0 * (q(n - 1) - q(n)) / T(n - 1) + v(n - 1) + v(n)) / T(n - 1)^2;
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; v(n)];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* T(n - 1)];
end

period = time(end) - time(1);

figure(1)
subplot(3, 1, 1)
plot(time, pos, 'k');
hold on
plot(t, q, 'ko');
for i = 1 : forwardPeriodCount
    plot(time + i * period, pos, '--');
    plot(t + i * period, q, 'bo');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, pos, '--');
    plot(t - i * period, q, 'bo');
end
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel, 'k');
hold on
for i = 1 : forwardPeriodCount
    plot(time + i * period, vel, '--');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, vel, '--');
end
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc, 'k');
hold on
for i = 1 : forwardPeriodCount
    plot(time + i * period, acc, '--');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, acc, '--');
end
xlabel('t')
ylabel('acc')

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
a0 = 0; %起始加速度
an = 0; %结束加速度
deltaT = 0.001; %插补周期

%前两个时间之间、后两个时间之间插入时间
n = length(t); % n >= 4
t2 = 0.5 * (t(1) + t(2));
t_n_1 = 0.5 * (t(n) + t(n - 1));
t = [t(1); t2; t(2 : n - 1); t_n_1; t(n)];
q = [q(1); 0.0; q(2 : n - 1); 0.0; q(n)];

n = n + 2;
T = diff(t);
a = zeros(n - 2, 1);
b = zeros(n - 2, 1);
c = zeros(n - 2, 1);
d = zeros(n - 2, 1);

a(2) = T(2) - T(1)^2 / T(2);
for i = 3 : n - 2
    a(i) = T(i);
end

b(1) = 2.0 * T(2) + T(1) * (3.0 + T(1) / T(2));
for i = 2 : n - 3
    b(i) = 2.0 * (T(i) + T(i + 1));
end
b(n - 2) = 2.0 * T(n - 2) + T(n - 1) * (3.0 + T(n - 1) / T(n - 2));

for i = 1 : n - 4
    c(i) = T(i + 1);
end
c(n - 3) = T(n - 2) - T(n - 1)^2 / T(n - 2);

d(1) = 6.0 * ((q(3) - q(1)) / T(2) - v0 * (1.0 + T(1) / T(2)) - a0 * (0.5 + T(1) / (3.0 * T(2))) * T(1));
d(2) = 6.0 * ((q(4) - q(3)) / T(3) - (q(3) - q(1)) / T(2) + v0 * T(1) / T(2) + a0 * T(1)^2 / (3.0 * T(2)));
for i = 3 : n - 4
    d(i) = 6.0 * ((q(i + 2) - q(i + 1)) / T(i + 1) - (q(i + 1) - q(i)) / T(i));
end
d(n - 3) = 6.0 * ((q(n) - q(n - 2)) / T(n - 2) - (q(n - 2) - q(n - 3)) / T(n - 3) - vn * T(n - 1) / T(n - 2) + an * T(n - 1)^2 / (3.0 * T(n - 2)));
d(n - 2) = 6.0 * ((q(n - 2) - q(n)) / T(n - 2) + vn * (1.0 + T(n - 1) / T(n - 2)) - an * (0.5 + T(n - 1) / (3.0 * T(n - 2))) * T(n - 1));

[w, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n - 2);
if sta == 0
    error('三对角矩阵方程求解出错!');
end

w = [a0; w; an];

q(2) = q(1) + T(1) * v0 + T(1)^2 * a0 / 3.0 + T(1)^2 * w(2) / 6.0;
q(n - 1) = q(n) - T(n - 1) * vn + T(n - 1)^2 * an / 3.0 + T(n - 1)^2 * w(n - 1) / 6.0;

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; a0];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    b0 = q(i);
    b1 = (q(i + 1) - q(i)) / T(i) - T(i) * (w(i + 1) + 2.0 * w(i)) / 6.0;
    b2 = 0.5 * w(i);
    b3 = (w(i + 1) - w(i)) / (6.0 * T(i));
    
    time = [time; tt];
    pos = [pos; b0 + (tt - t(i)) .* (b1 + (tt - t(i)) .* (b2 + b3 .* (tt - t(i))))];
    vel = [vel; b1 + (tt - t(i)) .* (2.0 * b2 + 3.0 * b3 .* (tt - t(i)))];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; an];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t([1, 3 : n - 2, n]), q([1, 3 : n - 2, n]), 'ro');
plot(t([2, n - 1]), q([2, n - 1]), 'bs');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
hold on
plot(t([1, n]), [v0, vn], 'ro');
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
hold on
plot(t([1, n]), [a0, an], 'ro');
xlabel('t')
ylabel('acc')

#include <iostream>
#include <vector>
using namespace std;

// 利用 Thomas 算法求解三对角系统
// 系统形式： a[i]*x[i-1] + b[i]*x[i] + c[i]*x[i+1] = d[i] ，其中 i=0,...,n-1
// 注意：a[0] 和 c[n-1] 不使用（可以置为0）
vector<double> thomasSolver(const vector<double>& a, const vector<double>& b, const vector<double>& c, const vector<double>& d) {
    int n = d.size();
    vector<double> c_star(n, 0.0), d_star(n, 0.0), x(n, 0.0);
    vector<double> b_mod = b;  // 复制一份b，因为需要在前向消去过程中修改

    // 第一步：前向消去
    c_star[0] = c[0] / b_mod[0];
    d_star[0] = d[0] / b_mod[0];
    for (int i = 1; i < n; i++) {
        double m = b_mod[i] - a[i] * c_star[i - 1];
        // 如果不是最后一个元素，更新 c_star
        if (i < n - 1)
            c_star[i] = c[i] / m;
        d_star[i] = (d[i] - a[i] * d_star[i - 1]) / m;
    }

    // 第二步：回代求解
    x[n - 1] = d_star[n - 1];
    for (int i = n - 2; i >= 0; i--) {
        x[i] = d_star[i] - c_star[i] * x[i + 1];
    }
    return x;
}

// 利用 Sherman-Morrison 公式求解循环三对角系统
// 系统形式：
//    b[0]*x[0] + c[0]*x[1] + r*x[n-1] = d[0]
//    a[i]*x[i-1] + b[i]*x[i] + c[i]*x[i+1] = d[i], i = 1,...,n-2
//    p*x[0] + a[n-1]*x[n-2] + b[n-1]*x[n-1] = d[n-1]
//
// 其中 r 和 p 分别为第一行和最后一行中额外的循环项
vector<double> cyclicThomasSolver(const vector<double>& a, const vector<double>& b, const vector<double>& c,
                                    const vector<double>& d, double r, double p) {
    int n = d.size();
    
    // 求解 T*z = d，其中 T 为去除循环项后的三对角矩阵
    vector<double> z = thomasSolver(a, b, c, d);
    
    // 构造向量 u 对应循环项 u = [r, 0, ..., 0, p]^T
    vector<double> u(n, 0.0);
    u[0]     = r;
    u[n - 1] = p;
    
    // 求解 T*y = u
    vector<double> y = thomasSolver(a, b, c, u);
    
    // 计算修正系数 alpha = (z[0] + z[n-1]) / (1 + y[0] + y[n-1])
    double numerator   = z[0] + z[n - 1];
    double denominator = 1.0 + y[0] + y[n - 1];
    double alpha       = numerator / denominator;
    
    // 最终解： x = z - alpha * y
    vector<double> x(n, 0.0);
    for (int i = 0; i < n; i++) {
        x[i] = z[i] - alpha * y[i];
    }
    
    return x;
}

int main() {
    // 示例：求解一个三对角系统
    // 系数数组要求：a[0] 和 c[n-1]置0，不参与计算
    // 例如，对于方程组：
    //   4*x0 + 1*x1             = 7
    //   1*x0 + 4*x1 + 1*x2        = 8
    //           1*x1 + 4*x2      = 7
    vector<double> a = {0, 1, 1};      // 下对角（a[0]不使用）
    vector<double> b = {4, 4, 4};        // 主对角
    vector<double> c = {1, 1, 0};        // 上对角（c[n-1]不使用）
    vector<double> d = {7, 8, 7};        // 右侧常数

    // 利用 Thomas 算法求解
    vector<double> sol = thomasSolver(a, b, c, d);
    cout << "三对角系统的解:" << endl;
    for (double x : sol) {
        cout << x << " ";
    }
    cout << "\n" << endl;
    
    // 示例：求解一个循环三对角系统
    // 在上例的基础上，假设第一行多了一个 r 项，最后一行多了一个 p 项
    double r = 1.0;  // 第一行末尾的循环项
    double p = 1.0;  // 最后一行首部的循环项
    
    vector<double> cyclicSol = cyclicThomasSolver(a, b, c, d, r, p);
    cout << "循环三对角系统的解:" << endl;
    for (double x : cyclicSol) {
        cout << x << " ";
    }
    cout << endl;
    
    return 0;
}

#include <iostream>
#include <vector>
#include <stdexcept>
#include <algorithm>
#include <cmath>

using namespace std;

//======================
// Thomas 算法：求解三对角矩阵方程
// 求解方程组： a[i]*x[i-1] + b[i]*x[i] + c[i]*x[i+1] = d[i] , i = 0,...,n-1
// 注意：a[0] 与 c[n-1]不使用（应置 0）
//======================
vector<double> solveTridiagonal(const vector<double>& a, const vector<double>& b,
                                const vector<double>& c, const vector<double>& d) {
    int n = d.size();
    vector<double> cp(n, 0.0), dp(n, 0.0), x(n, 0.0);
    if(n == 0) return x;
    cp[0] = c[0] / b[0];
    dp[0] = d[0] / b[0];
    for (int i = 1; i < n; ++i) {
        double m = b[i] - a[i] * cp[i - 1];
        if (i < n - 1)
            cp[i] = c[i] / m;
        dp[i] = (d[i] - a[i] * dp[i - 1]) / m;
    }
    x[n - 1] = dp[n - 1];
    for (int i = n - 2; i >= 0; --i)
        x[i] = dp[i] - cp[i] * x[i + 1];
    return x;
}

//======================
// CubicSpline 类：实现基于速度约束的三次样条插值
// 给定节点 (t_i, q_i) 以及端点速度 v0 与 vn 后，计算每段样条的系数
// 支持在任意 t 内查询样条值、速度（1阶导）与加速度（2阶导）
//======================
class CubicSpline {
public:
    // 设置节点数据（要求 t 为严格递增序列，q 与 t 数量相同）
    void setPoints(const vector<double>& t, const vector<double>& q) {
        if(t.size() != q.size() || t.size() < 2)
            throw invalid_argument("点数不匹配或少于两个点!");
        t_ = t;
        q_ = q;
        n_ = t_.size();
    }
    
    // 设置起始与终止速度
    void setBoundaryVelocities(double v0, double vn) {
        v0_ = v0;
        vn_ = vn;
    }
    
    // 根据公式（见说明）计算三次样条各段系数
    // 模型参考：给定起始速度与结束速度，利用位置、速度、加速度连续性条件
    void computeSpline() {
        if(t_.empty() || q_.empty() || n_ < 2)
            throw runtime_error("数据未设置或点数不足!");
        // 计算各段时间间隔： T[i] = t_[i+1] - t_[i], i = 0,...,n_-2
        T_.resize(n_ - 1);
        for (size_t i = 0; i < n_ - 1; ++i) {
            T_[i] = t_[i + 1] - t_[i];
            if(T_[i] <= 0)
                throw runtime_error("时间节点必须严格递增!");
        }
        
        // 根据加速度连续性条件可构造对角占优的三对角系统，未知量为中间各节点速度 v[1]...v[n_-2]
        // 对应方程（参见文献或说明）：对于 k = 0,..., n_-3，
        //   T_[k+1] * v_[k] + 2*(T_[k] + T_[k+1])* v_[k+1] + T_[k] * v_[k+2]
        //     = 3*(T_[k]^2*(q_[k+2]-q_[k+1]) + T_[k+1]^2*(q_[k+1]-q_[k]))/(T_[k]*T_[k+1])
        // 对于 k = 0 与 k = n_-2（对应首、尾方程）要引入边界已知量 v0_ 与 vn_
        size_t m = n_ - 2; // 中间未知速度个数
        vector<double> a_tridiag(m, 0.0), b_tridiag(m, 0.0), c_tridiag(m, 0.0), d_tridiag(m, 0.0);
        if(m > 0) {
            // 第 1 个方程 (i = 0，对应 k = 0)
            a_tridiag[0] = 0.0;  // 第一行无下对角
            b_tridiag[0] = 2.0 * (T_[0] + T_[1]);
            if(m > 1)
                c_tridiag[0] = T_[0];
            d_tridiag[0] = 3.0 * ( T_[0]*T_[0]*(q_[2] - q_[1]) + T_[1]*T_[1]*(q_[1] - q_[0]) )
                           / (T_[0] * T_[1]) - T_[1] * v0_;
            // 中间方程： i = 1, 2, ..., m-2
            for (size_t i = 1; i < m - 1; ++i) {
                a_tridiag[i] = T_[i + 1];
                b_tridiag[i] = 2.0 * (T_[i] + T_[i + 1]);
                c_tridiag[i] = T_[i];
                d_tridiag[i] = 3.0 * ( T_[i]*T_[i]*(q_[i + 2] - q_[i + 1]) + T_[i + 1]*T_[i + 1]*(q_[i + 1] - q_[i]) )
                               / (T_[i] * T_[i + 1]);
            }
            // 最后一个方程： i = m-1，对应 k = n_-2
            if(m >= 1) {
                size_t i = m - 1;
                a_tridiag[i] = T_[i + 1]; // 注意：i+1 = m = n_-1，正好是 T_ 的最后一个元素
                b_tridiag[i] = 2.0 * (T_[i] + T_[i + 1]);
                // 最后一行无上对角，故 c_tridiag[i] 保持为 0
                d_tridiag[i] = 3.0 * ( T_[i]*T_[i]*(q_[i + 2] - q_[i + 1]) + T_[i + 1]*T_[i + 1]*(q_[i + 1] - q_[i]) )
                               / (T_[i] * T_[i + 1]) - T_[i] * vn_;
            }
        }
        
        // 求解三对角方程（若 m==0 则表示只有两个点，不需插值）
        vector<double> v_interior;
        if(m > 0)
            v_interior = solveTridiagonal(a_tridiag, b_tridiag, c_tridiag, d_tridiag);
        
        // 构造完整的速度向量： v_[0] = v0_, v_[1..n_-2] 为求得的解， v_[n_-1] = vn_
        v_.resize(n_);
        v_[0] = v0_;
        for (size_t i = 0; i < m; ++i)
            v_[i + 1] = v_interior[i];
        v_[n_ - 1] = vn_;
        
        // 根据公式（见说明），计算各段样条的系数：
        // 设第 i 段 (i = 0,..., n_-2) 多项式为：
        //   q_i(t) = a0_[i] + a1_[i]*(t - t_[i]) + a2_[i]*(t - t_[i])^2 + a3_[i]*(t - t_[i])^3
        // 其中：
        //   a0_[i] = q_[i]
        //   a1_[i] = v_[i]
        //   a2_[i] = [3*(q_[i+1]-q_[i])/T_[i] - 2*v_[i] - v_[i+1]] / T_[i]
        //   a3_[i] = [2*(q_[i]-q_[i+1])/T_[i] + v_[i] + v_[i+1]] / (T_[i]*T_[i])
        a0_.resize(n_ - 1);
        a1_.resize(n_ - 1);
        a2_.resize(n_ - 1);
        a3_.resize(n_ - 1);
        for (size_t i = 0; i < n_ - 1; ++i) {
            a0_[i] = q_[i];
            a1_[i] = v_[i];
            a2_[i] = (3.0 * (q_[i + 1] - q_[i]) / T_[i] - 2.0 * v_[i] - v_[i + 1]) / T_[i];
            a3_[i] = (2.0 * (q_[i] - q_[i + 1]) / T_[i] + v_[i] + v_[i + 1]) / (T_[i] * T_[i]);
        }
        computed_ = true;
    }
    
    // 在任意 t_query 处计算样条插值值（位置）
    double evaluate(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return a0_[i] + a1_[i]*dt + a2_[i]*dt*dt + a3_[i]*dt*dt*dt;
    }
    
    // 计算样条在 t_query 处的一阶导数（速度）
    double evaluateDerivative(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return a1_[i] + 2.0*a2_[i]*dt + 3.0*a3_[i]*dt*dt;
    }
    
    // 计算样条在 t_query 处的二阶导数（加速度）
    double evaluateSecondDerivative(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return 2.0*a2_[i] + 6.0*a3_[i]*dt;
    }
    
private:
    // 二分查找：找到满足 t_[i] <= t_query <= t_[i+1] 的区间 i
    size_t findInterval(double t_query) const {
        auto it = std::upper_bound(t_.begin(), t_.end(), t_query);
        size_t idx = std::distance(t_.begin(), it);
        if(idx == 0)
            return 0;
        if(idx >= t_.size())
            return t_.size() - 2;
        return idx - 1;
    }
    
    vector<double> t_;     // 节点时间
    vector<double> q_;     // 节点位置
    vector<double> T_;     // 各段时间间隔 T[i] = t_[i+1] - t_[i]
    vector<double> v_;     // 各节点速度（v0 与 vn 为边界给定，其余由三对角方程求得）
    vector<double> a0_, a1_, a2_, a3_; // 每段多项式系数
    double v0_, vn_;       // 起始与终止速度
    size_t n_;             // 节点数
    bool computed_ = false;
};

//======================
// 主函数：演示如何使用 CubicSpline 类
//======================
int main() {
    // 示例数据，与正文中 MATLAB 示例一致
    vector<double> t = {0, 5, 7, 8, 10, 15, 18};
    vector<double> q = {3, -2, -5, 0, 6, 12, 8};
    double v0 = 2.0;   // 起始速度
    double vn = -3.0;  // 结束速度

    // 构造样条对象，设置数据、边界速度，并计算样条系数
    CubicSpline spline;
    try {
        spline.setPoints(t, q);
        spline.setBoundaryVelocities(v0, vn);
        spline.computeSpline();
        
        // 在 [t.front(), t.back()] 内以一定步长进行插值（此处 dt = 0.001）
        double dt = 0.001;
        for (double time = t.front(); time <= t.back(); time += dt) {
            double pos = spline.evaluate(time);
            double vel = spline.evaluateDerivative(time);
            double acc = spline.evaluateSecondDerivative(time);
            // 为了减少输出量，每隔 1 秒打印一次结果
            if (fabs(fmod(time, 1.0)) < 1e-6)
                cout << "t = " << time << ", pos = " << pos 
                     << ", vel = " << vel << ", acc = " << acc << endl;
        }
    }
    catch(const exception &e) {
        cerr << "错误: " << e.what() << endl;
        return -1;
    }
    
    return 0;
}

#include <iostream>
#include <vector>
#include <stdexcept>
#include <algorithm>
#include <cmath>

using namespace std;

//======================
// Thomas 算法：求解三对角矩阵方程
// 求解方程组： a[i]*x[i-1] + b[i]*x[i] + c[i]*x[i+1] = d[i] , i = 0,...,n-1
// 要求 a[0] = 0, c[n-1] = 0
//======================
vector<double> solveTridiagonal(const vector<double>& a, const vector<double>& b,
                                const vector<double>& c, const vector<double>& d) {
    int n = d.size();
    vector<double> cp(n, 0.0), dp(n, 0.0), x(n, 0.0);
    if(n == 0) return x;
    cp[0] = c[0] / b[0];
    dp[0] = d[0] / b[0];
    for (int i = 1; i < n; ++i) {
        double m = b[i] - a[i] * cp[i - 1];
        if (i < n - 1)
            cp[i] = c[i] / m;
        dp[i] = (d[i] - a[i] * dp[i - 1]) / m;
    }
    x[n - 1] = dp[n - 1];
    for (int i = n - 2; i >= 0; --i)
        x[i] = dp[i] - cp[i] * x[i + 1];
    return x;
}

//======================
// CubicSplineAcceleration 类：基于加速度方法的三次样条插值
// 给定节点 (t_i, q_i) 以及边界速度 v0 与 vn，先求加速度 ω_i (i=0,...,n-1)
// 再计算每段多项式系数：
//   a0 = q[i],
//   a1 = (q[i+1]-q[i])/T[i] - T[i]*(ω[i+1] + 2ω[i])/6,
//   a2 = ω[i] / 2,
//   a3 = (ω[i+1]-ω[i])/(6*T[i])
//======================
class CubicSplineAcceleration {
public:
    // 设置节点数据（要求 t 为严格递增序列，q 与 t 数量相同）
    void setPoints(const vector<double>& t, const vector<double>& q) {
        if(t.size() != q.size() || t.size() < 2)
            throw invalid_argument("点数不匹配或少于两个点!");
        t_ = t;
        q_ = q;
        n_ = t_.size();
    }
    
    // 设置边界速度
    void setBoundaryVelocities(double v0, double vn) {
        v0_ = v0;
        vn_ = vn;
    }
    
    // 根据加速度方法计算三次样条各段系数
    // 过程：
    // 1. 计算各段时间间隔 T[i] = t[i+1]-t[i], i = 0,...,n_-2
    // 2. 构造三对角系统 A*ω = d, 其中 ω 的维数为 n_（节点个数）
    //    A 的构造（0-indexed）：
    //      row0:    A[0,0] = 2*T[0],    A[0,1] = T[0]
    //      row i (1<= i <= n_-2): A[i,i-1] = T[i-1], A[i,i] = 2*(T[i-1]+T[i]), A[i,i+1] = T[i]
    //      row n_-1: A[n_-1, n_-2] = T[n_-2], A[n_-1, n_-1] = 2*T[n_-2]
    //    d 的构造：
    //      d[0] = 6*((q[1]-q[0])/T[0] - v0)
    //      d[i] = 6*((q[i+1]-q[i])/T[i] - (q[i]-q[i-1])/T[i-1])  (i=1,...,n_-2)
    //      d[n_-1] = 6*(vn - (q[n_-1]-q[n_-2])/T[n_-2])
    // 3. 利用 Thomas 算法求解 ω
    // 4. 根据 ω 计算各段样条多项式系数（每段 [t[i], t[i+1]]）
    void computeSpline() {
        if(t_.empty() || q_.empty() || n_ < 2)
            throw runtime_error("数据未设置或点数不足!");
        
        // 1. 计算各段时间间隔 T
        T_.resize(n_ - 1);
        for (size_t i = 0; i < n_ - 1; ++i) {
            T_[i] = t_[i+1] - t_[i];
            if(T_[i] <= 0)
                throw runtime_error("时间节点必须严格递增!");
        }
        
        // 2. 构造三对角系统 A*ω = d，其中 ω 的维数为 n_
        // a, b, c 为三对角矩阵的下对角、主对角、上对角，均大小为 n_
        vector<double> a(n_, 0.0), b(n_, 0.0), c(n_, 0.0), d_vec(n_, 0.0);
        // 下对角：对于 i = 1,..., n_-1, a[i] = T_[i-1]
        a[0] = 0.0;
        for (size_t i = 1; i < n_; ++i)
            a[i] = T_[i-1];
        
        // 主对角：
        // row0: b[0] = 2*T_[0]
        // row i (1<=i<=n_-2): b[i] = 2*(T_[i-1] + T_[i])
        // row n_-1: b[n_-1] = 2*T_[n_-2]
        b[0] = 2.0 * T_[0];
        for (size_t i = 1; i < n_ - 1; ++i)
            b[i] = 2.0 * (T_[i-1] + T_[i]);
        b[n_ - 1] = 2.0 * T_[n_ - 2];
        
        // 上对角：
        // 对于 i = 0,..., n_-2, c[i] = T_[i]；最后一行无上对角，c[n_-1]=0
        for (size_t i = 0; i < n_ - 1; ++i)
            c[i] = T_[i];
        c[n_ - 1] = 0.0;
        
        // 右端项 d_vec:
        // d[0] = 6*((q[1]-q[0])/T_[0] - v0)
        d_vec[0] = 6.0 * ((q_[1] - q_[0]) / T_[0] - v0_);
        // 对于 i = 1,..., n_-2:
        for (size_t i = 1; i < n_ - 1; ++i) {
            d_vec[i] = 6.0 * ((q_[i+1] - q_[i]) / T_[i] - (q_[i] - q_[i-1]) / T_[i-1]);
        }
        // d[n_-1] = 6*(vn - (q[n_-1]-q[n_-2])/T_[n_-2])
        d_vec[n_ - 1] = 6.0 * (vn_ - (q_[n_ - 1] - q_[n_ - 2]) / T_[n_ - 2]);
        
        // 3. 求解三对角系统 A*ω = d_vec，得到 ω（加速度向量，大小为 n_）
        omega_ = solveTridiagonal(a, b, c, d_vec);
        
        // 4. 根据 ω 计算每段多项式系数（区间 i=0,..., n_-2）
        // 每段多项式形式：
        //   q_i(t) = a0[i] + a1[i]*(t-t[i]) + a2[i]*(t-t[i])^2 + a3[i]*(t-t[i])^3, t in [t[i], t[i+1]]
        // 其中：
        //   a0[i] = q_[i]
        //   a1[i] = (q_[i+1]-q_[i])/T_[i] - T_[i]*(omega_[i+1] + 2*omega_[i]) / 6
        //   a2[i] = omega_[i] / 2
        //   a3[i] = (omega_[i+1]-omega_[i]) / (6*T_[i])
        size_t segs = n_ - 1;
        a0_.resize(segs);
        a1_.resize(segs);
        a2_.resize(segs);
        a3_.resize(segs);
        for (size_t i = 0; i < segs; ++i) {
            a0_[i] = q_[i];
            a1_[i] = (q_[i+1] - q_[i]) / T_[i] - T_[i] * (omega_[i+1] + 2.0 * omega_[i]) / 6.0;
            a2_[i] = omega_[i] / 2.0;
            a3_[i] = (omega_[i+1] - omega_[i]) / (6.0 * T_[i]);
        }
        
        computed_ = true;
    }
    
    // 在任意 t_query 处计算样条插值值（位置）
    double evaluate(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return a0_[i] + a1_[i]*dt + a2_[i]*dt*dt + a3_[i]*dt*dt*dt;
    }
    
    // 计算样条在 t_query 处的一阶导数（速度）
    double evaluateDerivative(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return a1_[i] + 2.0*a2_[i]*dt + 3.0*a3_[i]*dt*dt;
    }
    
    // 计算样条在 t_query 处的二阶导数（加速度）
    double evaluateSecondDerivative(double t_query) const {
        if(!computed_)
            throw runtime_error("请先调用 computeSpline() 计算样条系数!");
        if(t_query < t_.front() || t_query > t_.back())
            throw out_of_range("查询时间超出样条插值范围!");
        size_t i = findInterval(t_query);
        double dt = t_query - t_[i];
        return 2.0*a2_[i] + 6.0*a3_[i]*dt;
    }
    
private:
    // 二分查找：找到满足 t_[i] <= t_query <= t_[i+1] 的区间 i
    size_t findInterval(double t_query) const {
        auto it = std::upper_bound(t_.begin(), t_.end(), t_query);
        size_t idx = std::distance(t_.begin(), it);
        if(idx == 0)
            return 0;
        if(idx >= t_.size())
            return t_.size() - 2;
        return idx - 1;
    }
    
    vector<double> t_;          // 节点时间
    vector<double> q_;          // 节点位置
    vector<double> T_;          // 各段时间间隔 T[i] = t[i+1] - t[i]
    vector<double> omega_;      // 各节点加速度（由三对角系统求得），大小为 n_
    vector<double> a0_, a1_, a2_, a3_; // 每段多项式系数
    double v0_, vn_;            // 边界（起始、终止）速度
    size_t n_;                  // 节点数
    bool computed_ = false;
};

//======================
// 主函数：演示如何使用 CubicSplineAcceleration 类
//======================
int main() {
    // 示例数据（与 MATLAB 示例类似）
    vector<double> t = {0, 5, 7, 8, 10, 15, 18};
    vector<double> q = {3, -2, -5, 0, 6, 12, 8};
    double v0 = 2.0;   // 起始速度
    double vn = -3.0;  // 结束速度

    // 构造样条对象，设置数据与边界速度，计算样条系数
    CubicSplineAcceleration spline;
    try {
        spline.setPoints(t, q);
        spline.setBoundaryVelocities(v0, vn);
        spline.computeSpline();
        
        // 在 [t.front(), t.back()] 内以一定步长进行插值（此处 dt = 0.001）
        double dt = 0.001;
        for (double time = t.front(); time <= t.back(); time += dt) {
            double pos = spline.evaluate(time);
            double vel = spline.evaluateDerivative(time);
            double acc = spline.evaluateSecondDerivative(time);
            // 为了减少输出量，每隔 1 秒打印一次结果
            if (fabs(fmod(time, 1.0)) < 1e-6)
                cout << "t = " << time << ", pos = " << pos 
                     << ", vel = " << vel << ", acc = " << acc << endl;
        }
    }
    catch(const exception &e) {
        cerr << "错误: " << e.what() << endl;
        return -1;
    }
    
    return 0;
}

#include <iostream>
#include <vector>
#include <stdexcept>
#include <cmath>
#include <algorithm>

using namespace std;

//======================
// Thomas 算法：求解三对角矩阵方程
// 求解方程组： a[i]*x[i-1] + b[i]*x[i] + c[i]*x[i+1] = d[i],  i=0,...,n-1
// 要求 a[0] = 0, c[n-1] = 0
//======================
vector<double> solveTridiagonal(const vector<double>& a, const vector<double>& b,
                                const vector<double>& c, const vector<double>& d) {
    int n = d.size();
    vector<double> cp(n, 0.0), dp(n, 0.0), x(n, 0.0);
    if(n == 0) return x;
    cp[0] = c[0] / b[0];
    dp[0] = d[0] / b[0];
    for (int i = 1; i < n; ++i) {
        double m = b[i] - a[i] * cp[i - 1];
        if(i < n - 1)
            cp[i] = c[i] / m;
        dp[i] = (d[i] - a[i] * dp[i - 1]) / m;
    }
    x[n - 1] = dp[n - 1];
    for (int i = n - 2; i >= 0; --i)
        x[i] = dp[i] - cp[i] * x[i + 1];
    return x;
}

//======================
// CubicSplineBoundary 类：在同时约束起始/结束速度与加速度条件下的三次样条插值
// 内部先在第一段与最后一段插入辅助点，构造扩展的节点序列，然后构造三对角系统求解辅助变量 ω，
// 最后计算各段三次多项式的系数，并提供在任意时刻处计算位置、速度、加速度的接口。
//======================
class CubicSplineBoundary {
public:
    // 设置原始节点数据（要求 t 为严格递增序列，q 与 t 数量相同）
    void setPoints(const vector<double>& t, const vector<double>& q) {
        if(t.size() != q.size() || t.size() < 2)
            throw invalid_argument("点数不匹配或少于两个点!");
        orig_t_ = t;
        orig_q_ = q;
    }
    
    // 设置边界条件：起始速度 v0 与加速度 a0，结束速度 vn 与加速度 an
    void setBoundaryConditions(double v0, double vn, double a0, double an) {
        v0_ = v0;
        vn_ = vn;
        a0_ = a0;
        an_ = an;
    }
    
    // 计算扩展节点、构造三对角系统求解 ω，再计算每段多项式系数
    void computeSpline() {
        // 1. 构造扩展节点
        size_t nOrig = orig_t_.size();
        // 扩展后节点数： N = nOrig + 2
        size_t N = nOrig + 2;
        ext_t_.resize(N);
        ext_q_.resize(N);
        // 第一个点不变
        ext_t_[0] = orig_t_[0];
        ext_q_[0] = orig_q_[0];
        // 插入辅助点 1：取原 t0 与 t1 的中点
        ext_t_[1] = 0.5 * (orig_t_[0] + orig_t_[1]);
        ext_q_[1] = 0.0; // 待求（后面更新）
        // 中间部分：原 t[1] ... t[nOrig-2]
        for (size_t i = 2; i < N - 2; ++i) {
            ext_t_[i] = orig_t_[i - 1]; // 注意下标转换
            ext_q_[i] = orig_q_[i - 1];
        }
        // 插入辅助点 2：取原 t[nOrig-2] 与 t[nOrig-1] 的中点
        ext_t_[N - 2] = 0.5 * (orig_t_[nOrig - 2] + orig_t_[nOrig - 1]);
        ext_q_[N - 2] = 0.0; // 待求
        // 最后一点
        ext_t_[N - 1] = orig_t_[nOrig - 1];
        ext_q_[N - 1] = orig_q_[nOrig - 1];
        
        // 2. 计算各段时间间隔 T, 共 N-1 个
        size_t M_T = N - 1;
        T_.resize(M_T);
        for (size_t i = 0; i < M_T; ++i) {
            T_[i] = ext_t_[i + 1] - ext_t_[i];
            if (T_[i] <= 0)
                throw runtime_error("时间节点必须严格递增!");
        }
        
        // 3. 构造三对角系统来求解内部 ω（辅助变量），系统规模 m = N - 2
        size_t m = N - 2;
        vector<double> a(m, 0.0), b(m, 0.0), c(m, 0.0), d(m, 0.0);
        // 下对角 a：
        a[0] = 0.0;
        if(m > 1)
            a[1] = T_[1] - (T_[0]*T_[0]) / T_[1];
        for (size_t i = 2; i < m; ++i)
            a[i] = T_[i];
        
        // 主对角 b：
        b[0] = 2.0 * T_[1] + T_[0] * (3.0 + T_[0] / T_[1]);
        for (size_t i = 1; i < m - 1; ++i)
            b[i] = 2.0 * (T_[i] + T_[i + 1]);
        b[m - 1] = 2.0 * T_[m - 1] + T_[m] * (3.0 + T_[m] / T_[m - 1]);
        
        // 上对角 c：
        for (size_t i = 0; i < m - 1; ++i)
            c[i] = T_[i + 1];
        c[m - 1] = T_[m] - (T_[m]*T_[m]) / T_[m - 1];
        
        // 右端项 d：
        // d[0] 对应辅助点 1的方程
        d[0] = 6.0 * ( (ext_q_[2] - ext_q_[0]) / T_[1]
                      - v0_ * (1.0 + T_[0] / T_[1])
                      - a0_ * (0.5 + T_[0] / (3.0 * T_[1])) * T_[0] );
        if(m > 1) {
            d[1] = 6.0 * ( (ext_q_[3] - ext_q_[2]) / T_[2]
                          - (ext_q_[2] - ext_q_[0]) / T_[1]
                          + v0_ * T_[0] / T_[1]
                          + a0_ * T_[0]*T_[0] / (3.0 * T_[1]) );
        }
        for (size_t i = 2; i < m - 2; ++i) {
            d[i] = 6.0 * ( (ext_q_[i+2] - ext_q_[i+1]) / T_[i+1]
                          - (ext_q_[i+1] - ext_q_[i]) / T_[i] );
        }
        if(m >= 4) {
            // 倒数第二方程：注意下标转换
            d[m - 2] = 6.0 * ( (ext_q_[N - 1] - ext_q_[N - 3]) / T_[N - 2]
                              - (ext_q_[N - 3] - ext_q_[N - 4]) / T_[N - 3]
                              - vn_ * T_[N - 2] / T_[N - 3]
                              + an_ * (T_[N - 2]*T_[N - 2]) / (3.0 * T_[N - 3]) );
            // 最后一方程
            d[m - 1] = 6.0 * ( (ext_q_[N - 3] - ext_q_[N - 1]) / T_[N - 2]
                              + vn_ * (1.0 + T_[N - 2] / T_[N - 3])
                              - an_ * (0.5 + T_[N - 2] / (3.0 * T_[N - 3])) * T_[N - 2] );
        }
        
        // 4. 求解三对角系统，得到内部 ω（记为 w_interior），再构造完整 ω 向量
        vector<double> w_interior = solveTridiagonal(a, b, c, d);
        w_.resize(N);
        w_[0] = a0_;
        for (size_t i = 0; i < m; ++i)
            w_[i + 1] = w_interior[i];
        w_[N - 1] = an_;
        
        // 5. 根据边界条件“修正”两个辅助点的 q 值
        ext_q_[1] = ext_q_[0] + T_[0]*v0_ + (T_[0]*T_[0]*a0_) / 3.0 + (T_[0]*T_[0]*w_[1]) / 6.0;
        ext_q_[N - 2] = ext_q_[N - 1] - T_[N - 2]*vn_ + (T_[N - 2]*T_[N - 2]*an_) / 3.0 + (T_[N - 2]*T_[N - 2]*w_[N - 2]) / 6.0;
        
        // 6. 对每一段 [ext_t_[i], ext_t_[i+1]] 计算三次多项式系数
        size_t segs = N - 1;
        a0_coef_.resize(segs);
        a1_coef_.resize(segs);
        a2_coef_.resize(segs);
        a3_coef_.resize(segs);
        for (size_t i = 0; i < segs; ++i) {
            a0_coef_[i] = ext_q_[i];
            a1_coef_[i] = (ext_q_[i+1] - ext_q_[i]) / T_[i] - T_[i]*(w_[i+1] + 2.0*w_[i]) / 6.0;
            a2_coef_[i] = w_[i] / 2.0;
            a3_coef_[i] = (w_[i+1] - w_[i]) / (6.0 * T_[i]);
        }
        
        computed_ = true;
    }
    
    // 在任意查询时间 t_query 内计算样条插值（位置）
    double evaluate(double t_query) const {
        if (!computed_)
            throw runtime_error("请先调用 computeSpline()!");
        if (t_query < ext_t_.front() || t_query > ext_t_.back())
            throw out_of_range("查询时间超出范围!");
        size_t i = findSegment(t_query);
        double dt = t_query - ext_t_[i];
        return a0_coef_[i] + a1_coef_[i]*dt + a2_coef_[i]*dt*dt + a3_coef_[i]*dt*dt*dt;
    }
    
    // 一阶导（速度）
    double evaluateDerivative(double t_query) const {
        if (!computed_)
            throw runtime_error("请先调用 computeSpline()!");
        if (t_query < ext_t_.front() || t_query > ext_t_.back())
            throw out_of_range("查询时间超出范围!");
        size_t i = findSegment(t_query);
        double dt = t_query - ext_t_[i];
        return a1_coef_[i] + 2.0*a2_coef_[i]*dt + 3.0*a3_coef_[i]*dt*dt;
    }
    
    // 二阶导（加速度）
    double evaluateSecondDerivative(double t_query) const {
        if (!computed_)
            throw runtime_error("请先调用 computeSpline()!");
        if (t_query < ext_t_.front() || t_query > ext_t_.back())
            throw out_of_range("查询时间超出范围!");
        size_t i = findSegment(t_query);
        double dt = t_query - ext_t_[i];
        return 2.0*a2_coef_[i] + 6.0*a3_coef_[i]*dt;
    }
    
private:
    // 二分查找：返回满足 ext_t_[i] <= t_query <= ext_t_[i+1] 的区间下标 i
    size_t findSegment(double t_query) const {
        auto it = std::upper_bound(ext_t_.begin(), ext_t_.end(), t_query);
        size_t idx = std::distance(ext_t_.begin(), it);
        if (idx == 0)
            return 0;
        if (idx >= ext_t_.size())
            return ext_t_.size() - 2;
        return idx - 1;
    }
    
    // 原始数据
    vector<double> orig_t_, orig_q_;
    // 扩展后的节点
    vector<double> ext_t_, ext_q_;
    // 各段间隔 T
    vector<double> T_;
    // 边界条件
    double v0_, vn_, a0_, an_;
    // 求解得到的辅助变量 ω（长度 = 扩展节点数）
    vector<double> w_;
    // 每段三次多项式系数
    vector<double> a0_coef_, a1_coef_, a2_coef_, a3_coef_;
    
    bool computed_ = false;
};

//======================
// 主函数：演示如何使用 CubicSplineBoundary 类
//======================
int main() {
    try {
        // 示例数据（与文中 MATLAB 示例类似）
        vector<double> t = {0, 5, 7, 8, 10, 15, 18};  // 原始时间节点
        vector<double> q = {3, -2, -5, 0, 6, 12, 8};   // 原始位置
        double v0 = 2.0;   // 起始速度
        double vn = -3.0;  // 结束速度
        double a0 = 0.0;   // 起始加速度
        double an = 0.0;   // 结束加速度
        double deltaT = 0.001;  // 插值步长
        
        CubicSplineBoundary spline;
        spline.setPoints(t, q);
        spline.setBoundaryConditions(v0, vn, a0, an);
        spline.computeSpline();
        
        // 示例：以 0.01 秒步长输出部分插值结果
        double t_start = spline.evaluate( t.front() ); // 仅为示例，实际应在 [ext_t[0], ext_t.back()]
        cout << "部分样条插值结果：" << endl;
        for (double time = t.front(); time <= t.back(); time += 0.01) {
            double pos = spline.evaluate(time);
            double vel = spline.evaluateDerivative(time);
            double acc = spline.evaluateSecondDerivative(time);
            cout << "t = " << time << ", pos = " << pos
                 << ", vel = " << vel << ", acc = " << acc << endl;
        }
    }
    catch (const exception &e) {
        cerr << "错误: " << e.what() << endl;
        return -1;
    }
    
    return 0;
}