// ============================================================
// 隐式梯形法则求解中等刚性 ODE / DAE
// 编译: g++ -O2 -std=c++17 ode23t.cpp -o ode23t
// 运行: ./ode23t
// =============================================================
#include <vector>
#include <functional>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <limits>
#include <iostream>
#include <iomanip>
// -------------------------------------------------------------
// 类型别名
// -------------------------------------------------------------
using Vec = std::vector<double>;
using OdeFunc = std::function<Vec(double, const Vec&)>;
using JacFunc = std::function<void(double, const Vec&, std::vector<Vec>&)>;
using MassFunc= std::function<void(double, const Vec&, std::vector<Vec>&)>;
// -------------------------------------------------------------
// 选项结构
// -------------------------------------------------------------
struct Ode23tOptions {
double RelTol = 1e-3;
Vec AbsTol; // 长度 n，默认 1e-6
double InitialStep = 0.0; // 0 表示自动选取
double MaxStep = 0.0; // 0 表示 = (tf - t0)/10
bool NonNegative = false;
bool UseMass = false; // 是否使用质量矩阵
Ode23tOptions() {}
explicit Ode23tOptions(int n) : AbsTol(n, 1e-6) {}
};
// -------------------------------------------------------------
// 结果结构
// -------------------------------------------------------------
struct Ode23tResult {
Vec t;
std::vector<Vec> y;
};
// -------------------------------------------------------------
// 线性方程组求解（带部分主元的高斯-约当消元）
// A 会被原地修改；返回解向量 x 满足 A x = b
// -------------------------------------------------------------
static Vec solve_linear(std::vector<Vec> A, Vec b)
{
const int n = static_cast<int>(b.size());
for (int col = 0; col < n; ++col) {
// 选主元
int piv = col;
for (int r = col + 1; r < n; ++r)
if (std::abs(A[r][col]) > std::abs(A[piv][col])) piv = r;
std::swap(A[col], A[piv]);
std::swap(b[col], b[piv]);
const double d = A[col][col];
if (std::abs(d) < 1e-300)
throw std::runtime_error("ode23t: singular Jacobian in Newton step");
for (int j = col; j < n; ++j) A[col][j] /= d;
b[col] /= d;
for (int r = 0; r < n; ++r) {
if (r == col) continue;
const double factor = A[r][col];
if (factor == 0.0) continue;
for (int j = col; j < n; ++j)
A[r][j] -= factor * A[col][j];
b[r] -= factor * b[col];
}
}
return b;
}
// -------------------------------------------------------------
// 单步隐式梯形法
// 求 z 满足: M (z - y_n) - (h/2) [ f(t_n, y_n) + f(t_n+h, z) ] = 0
// 若 M == nullptr 则退化为: z - y_n - (h/2)[f_n + f_{n+1}] = 0
// 使用牛顿迭代 + 数值 / 解析 Jacobian
// 返回值: true 表示收敛
// -------------------------------------------------------------
static bool trapezoidal_step(
const OdeFunc& f,
double t, double h,
const Vec& y_n,
const std::vector<Vec>* M, // 质量矩阵（行主序），可 nullptr
const JacFunc* jac, // 解析 Jacobian，可 nullptr（用数值）
Vec& y_new,
int max_iter = 25,
double newton_tol = 1e-10)
{
const int n = static_cast<int>(y_n.size());
const Vec f_n = f(t, y_n);
// 初值猜测：显式 Euler
y_new.resize(n);
for (int i = 0; i < n; ++i)
y_new[i] = y_n[i] + h * f_n[i];
for (int iter = 0; iter < max_iter; ++iter) {
const Vec f_new = f(t + h, y_new);
// 计算残差 G(z)
Vec G(n, 0.0);
if (M) {
for (int i = 0; i < n; ++i) {
double sum = 0.0;
for (int j = 0; j < n; ++j)
sum += (*M)[i][j] * (y_new[j] - y_n[j]);
G[i] = sum - 0.5 * h * (f_n[i] + f_new[i]);
}
} else {
for (int i = 0; i < n; ++i)
G[i] = y_new[i] - y_n[i] - 0.5 * h * (f_n[i] + f_new[i]);
}
// 收敛判断（无穷范数 + 缩放）
double gnorm = 0.0;
for (int i = 0; i < n; ++i)
gnorm = std::max(gnorm, std::abs(G[i]));
if (gnorm < newton_tol) return true;
// 组装 Jacobian J = df/dy (t+h, y_new)
std::vector<Vec> J(n, Vec(n, 0.0));
if (jac) {
(*jac)(t + h, y_new, J);
} else {
// 数值 Jacobian（前向差分）
const double eps = 1e-8;
Vec yp = y_new;
for (int j = 0; j < n; ++j) {
const double save = yp[j];
const double dj = eps * std::max(1.0, std::abs(save));
yp[j] = save + dj;
const Vec fp = f(t + h, yp);
for (int i = 0; i < n; ++i)
J[i][j] = (fp[i] - f_new[i]) / dj;
yp[j] = save;
}
}
// 牛顿系统: ( M - (h/2) J ) * delta = -G
std::vector<Vec> A(n, Vec(n, 0.0));
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
const double base = M ? (*M)[i][j] : (i == j ? 1.0 : 0.0);
A[i][j] = base - 0.5 * h * J[i][j];
}
}
Vec rhs(n);
for (int i = 0; i < n; ++i) rhs[i] = -G[i];
const Vec delta = solve_linear(A, rhs);
for (int i = 0; i < n; ++i) y_new[i] += delta[i];
}
return false; // 未收敛
}
// -------------------------------------------------------------
// 误差估计：步长加倍法
// 比较 1 步 h 与 2 步 h/2 的结果
// 返回误差范数估计（若失败返回 +inf），并输出更精确的 y_out
// -------------------------------------------------------------
static double estimate_error(
const OdeFunc& f,
double t, double h,
const Vec& y_n,
const std::vector<Vec>* M,
const JacFunc* jac,
Vec& y_out)
{
const int n = static_cast<int>(y_n.size());
// 一整步
Vec y_full;
if (!trapezoidal_step(f, t, h, y_n, M, jac, y_full))
return std::numeric_limits<double>::infinity();
// 两步 h/2
Vec y_half, y_half2;
if (!trapezoidal_step(f, t, h * 0.5, y_n, M, jac, y_half) ||
!trapezoidal_step(f, t + h * 0.5, h * 0.5, y_half, M, jac, y_half2))
return std::numeric_limits<double>::infinity();
// 局部误差 = |y_full - y_half2| / (2^2 - 1)
double err = 0.0;
for (int i = 0; i < n; ++i)
err = std::max(err, std::abs(y_full[i] - y_half2[i]) / 3.0);
// 步长加倍法的更精确解：采用两半步行结果
y_out = std::move(y_half2);
return err;
}
// -------------------------------------------------------------
// ode23t 自适应步长求解主函数
// - 误差估计：步长加倍法（estimate_error）
// - 初始步长：Hairer 公式（常数 0.01、阈值 1e-5）
// - 步长控制：I 控制器（safety=0.9, max_scale=5.0, min_scale=0.2）
// - 最大步长：opts.MaxStep，0 表示 (tf-t0)/10
// -------------------------------------------------------------
static Ode23tResult ode23t(
const OdeFunc& f,
const JacFunc* jac, // 解析 Jacobian，可 nullptr
double t0, double tf,
const Vec& y0,
const Ode23tOptions& opts,
const std::vector<Vec>* M = nullptr) // 质量矩阵，可 nullptr
{
const int n = static_cast<int>(y0.size());
// 容差向量（AbsTol 长度不足时用 1e-6 填充）
Vec atol = opts.AbsTol;
if (atol.size() != static_cast<size_t>(n)) atol.assign(n, 1e-6);
const double rtol = opts.RelTol;
const double span = tf - t0;
const double hmax = opts.MaxStep > 0.0 ? opts.MaxStep : span / 10.0;
Ode23tResult res;
res.t.reserve(400);
res.y.reserve(400);
res.t.push_back(t0);
res.y.push_back(y0);
if (span <= 0.0) return res;
// ---- 初始步长（Hairer 公式）----
double h;
if (opts.InitialStep > 0.0) {
h = std::min(opts.InitialStep, hmax);
} else {
const Vec f0 = f(t0, y0);
double d0 = 0.0, d1 = 0.0;
for (int i = 0; i < n; ++i) {
const double w = atol[i] + rtol * std::abs(y0[i]);
d0 = std::max(d0, std::abs(y0[i]) / w);
d1 = std::max(d1, std::abs(f0[i]) / w);
}
if (d0 < 1e-5 || d1 < 1e-5) h = 1e-6;
else h = 0.01 * d0 / d1;
if (h > hmax) h = hmax;
}
if (!(h > 0.0) || !std::isfinite(h)) h = 1e-6;
// ---- 步长控制参数 ----
const double safety = 0.9;
const double max_scale = 5.0;
const double min_scale = 0.2;
double t = t0;
Vec y = y0;
double prev_err = 1.0; // 上一步缩放误差（PI 项记忆）
while (t < tf) {
// 限制步长不超过 MaxStep，且不越过终点
if (h > hmax) h = hmax;
if (t + h > tf) h = tf - t;
if (!(h > 0.0)) break;
// 试探一步，得到误差估计与更精确的候选解
Vec y_next;
const double err = estimate_error(f, t, h, y, M, jac, y_next);
if (!std::isfinite(err)) {
// 牛顿迭代未收敛：缩小步长重试
h *= 0.5;
if (h < hmax * 1e-12) break;
continue;
}
// 加权误差范数（无穷范数类）：
// w_i = AbsTol_i + RelTol * max(|y_i|, |y_next_i|)
// err_scaled = err / w，取整体代表权重，避免单一分量权值过小
double w_norm = 0.0, w_min = std::numeric_limits<double>::max();
double maxy = 0.0;
for (int i = 0; i < n; ++i) {
const double wi = atol[i] + rtol * std::max(std::abs(y[i]), std::abs(y_next[i]));
w_norm += wi * wi;
w_min = std::min(w_min, wi);
maxy = std::max(maxy, std::abs(y_next[i]));
}
// 用误差主导分量的权重做缩放（此处以"LF 范数 / RMS 权重"折中）
const double w_rms = std::sqrt(w_norm / n);
const double err_scaled_raw = err / w_rms;
// 接受 / 拒绝
if (err_scaled_raw > 1.0) {
// 拒绝：缩小步长重试（极限缩放 min_scale=0.2 之外再加安全系数）
double scale = safety * std::pow(err_scaled_raw, -1.0 / 3.0);
scale = std::min(scale, 0.5 / std::pow(err_scaled_raw, 1.0 / 3.0));
if (scale < 1e-3) scale = 1e-3;
h *= scale;
if (h < hmax * 1e-12) break;
continue;
}
// 接受该步
res.t.push_back(t + h);
res.y.push_back(y_next);
t += h;
y = std::move(y_next);
// 更新下一候选步长（PI 型控制器）
double scale = safety * std::pow(err_scaled_raw, -1.0 / 3.0);
// PI 修正项：(prev/curr)^(1/6) 的常见形式，温和起见取 1/8
scale *= std::pow(prev_err / std::max(err_scaled_raw, 1e-300), 1.0 / 8.0);
if (scale > max_scale) scale = max_scale;
if (scale < min_scale) scale = min_scale;
h *= scale;
prev_err = err_scaled_raw;
(void)maxy; // 保留，便于后续调整权重策略
}
return res;
}
// -------------------------------------------------------------
// Van der Pol 演示问题
// y1' = y2
// y2' = mu * (1 - y1^2) * y2 - y1
// -------------------------------------------------------------
static Vec vdp_rhs(double /*t*/, const Vec& y)
{
const double mu = 10.0;
Vec f(2);
f[0] = y[1];
f[1] = mu * (1.0 - y[0] * y[0]) * y[1] - y[0];
return f;
}
int main()
{
using std::cout;
using std::fixed;
using std::setprecision;
using std::setw;
// ---- 标准演示配置：mu = 10, tspan [0, 20], RelTol = 1e-4, MaxStep = 0.5 ----
Ode23tOptions opts(2);
opts.RelTol = 1e-4;
opts.AbsTol = { 1e-6, 1e-6 };
opts.MaxStep = 0.5;
const double t0 = 0.0, tf = 20.0, mu = 10.0;
const Vec y0 = { 2.0, 0.0 };
const Ode23tResult result = ode23t(vdp_rhs, nullptr, t0, tf, y0, opts);
const int steps = static_cast<int>(result.t.size()) - 1;
cout << "============================================\n";
cout << " ode23t (implicit trapezoidal) demo\n";
cout << " Problem : Van der Pol, mu = " << fixed << setprecision(6) << mu << "\n";
cout << " tspan : [" << t0 << ", " << tf << "]\n";
cout << " RelTol : " << opts.RelTol << "\n";
cout << "============================================\n";
cout << " Steps taken : " << steps << "\n";
cout << " Final t : " << fixed << setprecision(6) << result.t.back() << "\n";
cout << " Final y : (" << result.y.back()[0] << ", " << result.y.back()[1] << ")\n";
cout << "--------------------------------------------\n";
cout << " First few steps:\n";
cout << " idx t y1 y2\n";
const int show = std::min(steps, 9);
for (int i = 0; i <= show; ++i)
cout << setw(5) << i << " " << fixed << setprecision(6)
<< setw(10) << result.t[i] << " "
<< setw(10) << result.y[i][0] << " "
<< setw(10) << result.y[i][1] << "\n";
return 0;
}