C++代码实现MATLAB中ode15s函数功能

// ode15s_robertson.cpp
// 变阶 BDF 求解器 + 精确雅可比 + 自适应步长
// 求解 Robertson 刚性 ODE 问题
// 编译: g++ -std=c++11 -O2 -o ode15s_robertson ode15s_robertson.cpp -lm
// 运行: ./ode15s_robertson

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <vector>
#include <deque>
#include <algorithm>

#define EPS 1e-14

// LU分解求解线性方程组
static int solve_linear(double *A, double *b, int n)
{
for (int k = 0; k < n; ++k)
{
int pivot = k;
double maxv = fabs(A[k * n + k]);
for (int i = k + 1; i < n; ++i)
{
double v = fabs(A[i * n + k]);
if (v > maxv)
{
maxv = v;
pivot = i;
}
}
if (maxv < 1e-15)
{
for (int i = 0; i < n; ++i)
A[i * n + i] += 1e-12;
maxv = fabs(A[k * n + k]);
if (maxv < 1e-15)
return –1;
}
if (pivot != k)
{
for (int j = 0; j < n; ++j)
std::swap(A[k * n + j], A[pivot * n + j]);
std::swap(b[k], b[pivot]);
}
for (int i = k + 1; i < n; ++i)
{
double f = A[i * n + k] / A[k * n + k];
for (int j = k + 1; j < n; ++j)
A[i * n + j] -= f * A[k * n + j];
b[i] -= f * b[k];
}
}
for (int i = n – 1; i >= 0; —i)
{
double sum = b[i];
for (int j = i + 1; j < n; ++j)
sum -= A[i * n + j] * b[j];
b[i] = sum / A[i * n + i];
}
return 0;
}

// =========================== ODE15s 接口 ===========================
typedef int (*ODEFunc)(double t, const double *y, double *dy, void *params);
typedef int (*JacobianFunc)(double t, const double *y, double *dfdy, void *params);

typedef struct
{
double rel_tol, abs_tol, max_step, initial_step;
int max_order;
JacobianFunc jacobian;
void *jac_params;
} ODE15sOptions;

static ODE15sOptions ode15s_default_options()
{
ODE15sOptions opts = {1e-3, 1e-6, 1e20, 0.0, 5, NULL, NULL};
return opts;
}

typedef struct ODE15sResult
{
double *t, *y;
int n_points, n_vars;
} ODE15sResult;

static void ode15s_result_free(ODE15sResult *res)
{
if (res)
{
free(res->t);
free(res->y);
free(res);
}
}

// =========================== BDF 求解器核心 ===========================
struct BDFSolver
{
int n;
double h, t;
std::vector<double> y, f;
std::vector<double> y_prev, f_prev;
int step_count;
bool has_prev;
ODE15sOptions opts;
ODEFunc f_func;
void *f_params;

BDFSolver(int nv, ODEFunc f, void *p, const ODE15sOptions &opt)
: n(nv), h(1e-8), t(0), y(nv), f(nv), y_prev(nv), f_prev(nv),
step_count(0), has_prev(false), opts(opt), f_func(f), f_params(p) {}

int eval_deriv(double tt, const double *yy, double *ff)
{
return f_func(tt, yy, ff, f_params);
}

// 构建牛顿迭代矩阵: coef*I – J
bool build_iteration_matrix(double *W, double coef)
{
for (int i = 0; i < n * n; ++i)
W[i] = 0;
for (int i = 0; i < n; ++i)
W[i * n + i] = coef;

if (opts.jacobian)
{
std::vector<double> J(n * n, 0);
opts.jacobian(t, y.data(), J.data(), opts.jac_params);
for (int i = 0; i < n * n; ++i)
W[i] -= J[i];
}
else
{
std::vector<double> f0(n), f1(n);
eval_deriv(t, y.data(), f0.data());
double eps = std::sqrt(EPS);
for (int j = 0; j < n; ++j)
{
double delta = eps * std::max(1e-7, fabs(y[j]));
if (delta == 0)
delta = eps;
std::vector<double> y_pert(y);
y_pert[j] += delta;
eval_deriv(t, y_pert.data(), f1.data());
for (int i = 0; i < n; ++i)
W[i * n + j] -= (f1[i] – f0[i]) / delta;
}
}
return true;
}

// 缩放误差范数
double scaled_norm(const std::vector<double> &err, const std::vector<double> &y_val)
{
double max_err = 0;
for (int i = 0; i < n; ++i)
{
double scale = opts.abs_tol + opts.rel_tol * fabs(y_val[i]);
if (scale < 1e-20)
scale = 1e-20;
max_err = std::max(max_err, fabs(err[i]) / scale);
}
return max_err;
}

// BDF1: y_{n+1} – y_n = h*f(y_{n+1})
int bdf1_step(double t_next, std::vector<double> &y_next, double &error_est)
{
double coef = 1.0 / h;
std::vector<double> W(n * n, 0);
if (!build_iteration_matrix(W.data(), coef))
return –1;

std::vector<double> y_new(n);
for (int i = 0; i < n; ++i)
y_new[i] = y[i] + h * f[i];

std::vector<double> delta(n), resid(n), f_new(n);
int iter = 0, max_iter = 20;

for (; iter < max_iter; ++iter)
{
eval_deriv(t + h, y_new.data(), f_new.data());
for (int i = 0; i < n; ++i)
{
resid[i] = coef * (y_new[i] – y[i]) – f_new[i];
delta[i] = –resid[i];
}
if (solve_linear(W.data(), delta.data(), n) != 0)
return –2;

double norm_delta = 0;
for (int i = 0; i < n; ++i)
{
norm_delta = std::max(norm_delta, fabs(delta[i]));
y_new[i] += delta[i];
}
if (norm_delta < 1e-14)
break;
}
if (iter >= max_iter)
return –3;

// LTE = -h/2 * y''
eval_deriv(t + h, y_new.data(), f_new.data());
std::vector<double> lte(n);
for (int i = 0; i < n; ++i)
lte[i] = –0.5 * h * (f_new[i] – f[i]);
error_est = scaled_norm(lte, y_new);

y_next = y_new;
return 0;
}

// BDF2: (3*y_{n+1} – 4*y_n + y_{n-1})/(2h) = f(y_{n+1})
int bdf2_step(double t_next, std::vector<double> &y_next, double &error_est)
{
double coef = 3.0 / (2.0 * h);
std::vector<double> W(n * n, 0);
if (!build_iteration_matrix(W.data(), coef))
return –1;

std::vector<double> y_new(n);
for (int i = 0; i < n; ++i)
y_new[i] = y[i] + h * ((3.0 / 2.0) * f[i] – 0.5 * f_prev[i]);

std::vector<double> delta(n), resid(n), f_new(n);
int iter = 0, max_iter = 20;

for (; iter < max_iter; ++iter)
{
eval_deriv(t + h, y_new.data(), f_new.data());
for (int i = 0; i < n; ++i)
{
resid[i] = coef * y_new[i] – (2.0 / h) * y[i] + (0.5 / h) * y_prev[i] – f_new[i];
delta[i] = –resid[i];
}
if (solve_linear(W.data(), delta.data(), n) != 0)
return –2;

double norm_delta = 0;
for (int i = 0; i < n; ++i)
{
norm_delta = std::max(norm_delta, fabs(delta[i]));
y_new[i] += delta[i];
}
if (norm_delta < 1e-14)
break;
}
if (iter >= max_iter)
return –3;

std::vector<double> lte(n);
for (int i = 0; i < n; ++i)
lte[i] = –2.0 / 9.0 * h * (f[i] – f_prev[i]);
error_est = scaled_norm(lte, y_new);

y_next = y_new;
return 0;
}

int step(double t_next, std::vector<double> &y_next, double &error_est)
{
if (!has_prev || step_count < 2)
{
return bdf1_step(t_next, y_next, error_est);
}
else
{
return bdf2_step(t_next, y_next, error_est);
}
}

int adaptive_step(double t_target, std::deque<double> &t_out, std::deque<std::vector<double>> &y_out)
{
double t_curr = t;
std::vector<double> y_curr = y;
int fail_cnt = 0;
const int MAX_STEPS = 1000000;

while (t_curr < t_target – 1e-12)
{
if (step_count > MAX_STEPS)
return –3;

double h_remain = t_target – t_curr;
double h_try = std::min(h, h_remain);
if (opts.max_step < 1e19)
h_try = std::min(h_try, opts.max_step);
if (h_try < 1e-15)
h_try = 1e-15;

std::vector<double> y_try;
double error;
int stat = step(t_curr + h_try, y_try, error);

if (stat != 0)
{
h_try *= 0.25;
h = h_try;
if (++fail_cnt > 50)
return –1;
continue;
}

if (error <= 1.0)
{
fail_cnt = 0;
t_curr += h_try;
y_prev = y_curr;
f_prev = f;
y_curr = y_try;
t_out.push_back(t_curr);
y_out.push_back(y_curr);
step_count++;
has_prev = true;

y = y_curr;
eval_deriv(t_curr, y_curr.data(), f.data());

double factor = (error < 1e-10) ? 10.0 : std::pow(0.5 / error, 0.5);
factor = std::max(0.2, std::min(10.0, factor));
h = h_try * factor;
h = std::max(h, 1e-15);
}
else
{
h_try *= 0.25;
h = h_try;
if (++fail_cnt > 50)
return –2;
}
}
t = t_curr;
y = y_curr;
return 0;
}
};

// =========================== 求解接口 ===========================
static ODE15sResult *ode15s_solve_with_tspan(ODEFunc f, const double *tspan, int len,
const double *y0, int nv, ODE15sOptions *opts, void *f_params)
{
ODE15sOptions opt = opts ? *opts : ode15s_default_options();
BDFSolver solver(nv, f, f_params, opt);
solver.t = tspan[0];
for (int i = 0; i < nv; ++i)
solver.y[i] = y0[i];
solver.eval_deriv(solver.t, solver.y.data(), solver.f.data());

if (opt.initial_step > 0)
solver.h = opt.initial_step;
else
{
double norm_f = 0;
for (int i = 0; i < nv; ++i)
norm_f = std::max(norm_f, fabs(solver.f[i]));
if (norm_f < 1e-10)
norm_f = 1e-10;
solver.h = 1e-6 / norm_f;
}

std::deque<double> t_out;
std::deque<std::vector<double>> y_out;
t_out.push_back(solver.t);
y_out.push_back(solver.y);

for (int idx = 1; idx < len; ++idx)
{
double t_target = tspan[idx];
if (t_target <= solver.t)
continue;
int ret = solver.adaptive_step(t_target, t_out, y_out);
if (ret != 0)
{
printf("求解失败，返回码: %d (在t=%.6e, 步数=%d)\\n", ret, solver.t, solver.step_count);
break;
}
}

ODE15sResult *res = (ODE15sResult *)malloc(sizeof(ODE15sResult));
res->n_points = (int)t_out.size();
res->n_vars = nv;
res->t = (double *)malloc(res->n_points * sizeof(double));
res->y = (double *)malloc(res->n_points * nv * sizeof(double));
for (int i = 0; i < res->n_points; ++i)
{
res->t[i] = t_out[i];
for (int j = 0; j < nv; ++j)
res->y[i * nv + j] = y_out[i][j];
}
return res;
}

static ODE15sResult *ode15s_solve(ODEFunc f, double t0, double tf, const double *y0,
int nv, ODE15sOptions *opts, void *f_params)
{
double tspan[2] = {t0, tf};
return ode15s_solve_with_tspan(f, tspan, 2, y0, nv, opts, f_params);
}

// =========================== Robertson 问题 ===========================
static int robertson_ode(double t, const double *y, double *dy, void *params)
{
(void)t;
(void)params;
dy[0] = –0.04 * y[0] + 1e4 * y[1] * y[2];
dy[1] = 0.04 * y[0] – 1e4 * y[1] * y[2] – 3e7 * y[1] * y[1];
dy[2] = 3e7 * y[1] * y[1];
return 0;
}

// 雅可比矩阵 J(i,j) = dfi/dyj, 行主序
static int robertson_jac(double t, const double *y, double *J, void *params)
{
(void)t;
(void)params;
J[0 * 3 + 0] = –0.04;
J[0 * 3 + 1] = 1e4 * y[2];
J[0 * 3 + 2] = 1e4 * y[1];

J[1 * 3 + 0] = 0.04;
J[1 * 3 + 1] = –1e4 * y[2] – 6e7 * y[1];
J[1 * 3 + 2] = –1e4 * y[1];

J[2 * 3 + 0] = 0.0;
J[2 * 3 + 1] = 6e7 * y[1];
J[2 * 3 + 2] = 0.0;
return 0;
}

// =========================== 主函数 ===========================
int main()
{
printf("=== BDF求解器 (Robertson问题) ===\\n");
printf("与MATLAB ode15s等效: RelTol=1e-6, AbsTol=1e-9\\n\\n");

double tspan[] = {0, 1e6};
double y0[] = {1.0, 0.0, 0.0};
int nv = 3;

ODE15sOptions opts = ode15s_default_options();
opts.rel_tol = 1e-6;
opts.abs_tol = 1e-9;
opts.jacobian = robertson_jac;

// 1. 连续输出
printf("— [t,y] = ode15s(@robertson, [0 1e6], [1;0;0]) —\\n");
ODE15sResult *res = ode15s_solve(robertson_ode, tspan[0], tspan[1], y0, nv, &opts, NULL);
if (res)
{
printf("总步数: %d\\n", res->n_points);
printf("前10个时间点:\\n");
for (int i = 0; i < std::min(10, res->n_points); ++i)
{
printf(" t=%.2e: y=[%.6e, %.6e, %.6e]\\n", res->t[i],
res->y[i * nv + 0], res->y[i * nv + 1], res->y[i * nv + 2]);
}
printf("最终结果 (t=%.2e):\\n", res->t[res->n_points – 1]);
printf(" y=[%.6e, %.6e, %.6e]\\n",
res->y[(res->n_points – 1) * nv + 0],
res->y[(res->n_points – 1) * nv + 1],
res->y[(res->n_points – 1) * nv + 2]);
ode15s_result_free(res);
}

// 2. 离散时间点输出
double t_eval[] = {0, 1, 10, 100, 1000, 10000, 100000, 1000000};
int neval = sizeof(t_eval) / sizeof(t_eval[0]);
printf("\\n— y = ode15s(@robertson, t_eval, [1;0;0]) —\\n");
res = ode15s_solve_with_tspan(robertson_ode, t_eval, neval, y0, nv, &opts, NULL);
if (res)
{
printf("t\\t\\ty(1)\\t\\t\\ty(2)\\t\\t\\ty(3)\\n");
printf("———————————————————–\\n");
for (int i = 0; i < res->n_points; ++i)
{
printf("%.0f\\t%.6e\\t%.6e\\t%.6e\\n", res->t[i],
res->y[i * nv + 0], res->y[i * nv + 1], res->y[i * nv + 2]);
}
ode15s_result_free(res);
}

return 0;
}