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

// ============================================================================
//  TR-BDF2 ODE Solver (类似 MATLAB 的 ode23tb)
//  单文件实现，包含示例主函数
//  编译: g++ -O2 -std=c++17 trbdf2.cpp -o trbdf2
// ============================================================================

#include <chrono>
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <functional>
#include <algorithm>
#include <limits>
#include <stdexcept>

using Vector = std::vector<double>;
using Matrix = std::vector<Vector>;   // 行优先存储

// ============================================================================
//  线性方程组求解 (带部分主元的高斯消元)
// ============================================================================
static Vector solveLinearSystem(Matrix A, Vector b) {
    const size_t n = b.size();
    for (size_t k = 0; k < n; ++k) {
        // 部分主元
        size_t pivot = k;
        double maxv = std::abs(A[k][k]);
        for (size_t i = k + 1; i < n; ++i) {
            if (std::abs(A[i][k]) > maxv) { maxv = std::abs(A[i][k]); pivot = i; }
        }
        if (maxv < 1e-300)
            throw std::runtime_error("线性方程组奇异");
        if (pivot != k) {
            std::swap(A[k], A[pivot]);
            std::swap(b[k], b[pivot]);
        }
        // 消元
        for (size_t i = k + 1; i < n; ++i) {
            double factor = A[i][k] / A[k][k];
            A[i][k] = 0.0;
            for (size_t j = k + 1; j < n; ++j)
                A[i][j] -= factor * A[k][j];
            b[i] -= factor * b[k];
        }
    }
    // 回代
    Vector x(n);
    for (int i = static_cast<int>(n) - 1; i >= 0; --i) {
        double s = b[i];
        for (size_t j = i + 1; j < n; ++j)
            s -= A[i][j] * x[j];
        x[i] = s / A[i][i];
    }
    return x;
}

// ============================================================================
//  数值雅可比: J(i,j) = ∂f_i / ∂y_j
// ============================================================================
static Matrix numericalJacobian(
    const std::function<Vector(double, const Vector&)>& f,
    double t, const Vector& y, const Vector& f0)
{
    const size_t n = y.size();
    Matrix J(n, Vector(n, 0.0));
    Vector yp = y;
    for (size_t j = 0; j < n; ++j) {
        double eps = std::sqrt(std::numeric_limits<double>::epsilon())
                     * std::max(1.0, std::abs(y[j]));
        yp[j] += eps;
        Vector fp = f(t, yp);
        for (size_t i = 0; i < n; ++i)
            J[i][j] = (fp[i] - f0[i]) / eps;
        yp[j] = y[j];
    }
    return J;
}

// ============================================================================
//  TR-BDF2 求解器
// ============================================================================
class TRBDF2Solver {
public:
    using OdeFunction = std::function<Vector(double, const Vector&)>;

    explicit TRBDF2Solver(OdeFunction ode) : f_(std::move(ode)) {
        // γ = 2 - √2  ≈ 0.5857864376
        gamma_ = 2.0 - std::sqrt(2.0);

        // 梯形法则系数: y_g = y_n + h*γ/2 * (f_n + f_g)
        trap_factor_ = 0.5 * gamma_;

        // BDF2 系数
        // y_{n+1} = c1*y_g - c2*y_n + c3*h*f_{n+1}
        double d = gamma_ * (2.0 - gamma_);
        c1_ = 1.0 / d;
        c2_ = (1.0 - gamma_) * (1.0 - gamma_) / d;
        c3_ = (1.0 - gamma_) / (2.0 - gamma_);

        // 误差估计系数: e = err_coef * h * (f_n - 2*f_g + f_{n+1})
        err_coef_ = (2.0 - gamma_) / (6.0 - 3.0 * gamma_);
    }

    void setTolerances(double rtol, double atol) { rtol_ = rtol; atol_ = atol; }
    void setInitialStep(double h0) { h_init_ = h0; }
    void setMinMaxStep(double hmin, double hmax) { h_min_ = hmin; h_max_ = hmax; }

    // 返回 { t 数组, y 数组 }
    std::pair<Vector, std::vector<Vector>> solve(
        double t0, double tf, const Vector& y0)
    {
        if (tf <= t0) throw std::runtime_error("tspan 必须满足 tf > t0");

        double t = t0;
        Vector y = y0;
        Vector t_out; t_out.push_back(t);
        std::vector<Vector> y_out; y_out.push_back(y);

        // 初始步长
        double h = (h_init_ > 0.0) ? h_init_ : estimateInitialStep(t0, y0, tf);
        h = std::min(std::max(h, h_min_), h_max_);

        // 保存上一成功步的导数，避免重复计算
        Vector f_cur = f_(t, y);
        int reject_count = 0;

        while (t < tf - 1e-14 * std::abs(tf)) {
            if (t + h > tf) h = tf - t;

            StepResult sr = doStep(t, y, f_cur, h);

            if (sr.success) {
                t += h;
                y = sr.y_new;
                f_cur = sr.f_new;
                t_out.push_back(t);
                y_out.push_back(y);
                h = adjustStepSize(h, sr.error_norm);
                reject_count = 0;
            } else {
                h *= 0.5;
                ++reject_count;
                if (h < h_min_) {
                    std::cerr << "警告: 步长降到最小值仍无法收敛，提前终止。\n";
                    break;
                }
                if (reject_count > 50) {
                    std::cerr << "警告: 步长反复被拒绝，提前终止。\n";
                    break;
                }
            }
        }

        return {t_out, y_out};
    }

private:
    OdeFunction f_;
    double gamma_;
    double trap_factor_;
    double c1_, c2_, c3_;
    double err_coef_;
    double rtol_ = 1e-3;
    double atol_ = 1e-6;
    double h_init_ = 0.0;
    double h_min_ = 1e-12;
    double h_max_ = 1.0;
    int    max_newton_iter_ = 12;
    double newton_rtol_ = 1e-10;
    double newton_atol_ = 1e-12;

    struct StepResult {
        bool success = false;
        Vector y_new;
        Vector f_new;
        double error_norm = 1e30;
    };

    // ---------- 初始步长估计 ----------
    double estimateInitialStep(double t, const Vector& y, double tf) {
        Vector f0 = f_(t, y);
        double d0 = 0.0, d1 = 0.0;
        for (size_t i = 0; i < y.size(); ++i) {
            double sc = atol_ + std::abs(y[i]) * rtol_;
            d0 += (y[i] / sc) * (y[i] / sc);
            d1 += (f0[i] / sc) * (f0[i] / sc);
        }
        d0 = std::sqrt(d0 / y.size());
        d1 = std::sqrt(d1 / y.size());
        double h;
        if (d0 < 1e-5 || d1 < 1e-5) h = 1e-6;
        else                          h = 0.01 * (d0 / d1);
        h = std::min(h, tf - t);
        return std::max(h, h_min_);
    }

    // ---------- 牛顿迭代辅助 ----------
    // 用简化牛顿法求解 G(z) = 0，其中 G(z) = z - RHS - a*h*f(t_z, z)
    // 迭代矩阵 M = I - a*h*J
    bool newtonSolve(
        double t_z, const Vector& rhs, double a_h,
        Vector& z, Vector& f_z)
    {
        const size_t n = z.size();
        for (int iter = 0; iter < max_newton_iter_; ++iter) {
            f_z = f_(t_z, z);

            // 残差 r = z - rhs - a_h * f_z
            Vector r(n);
            double rnorm = 0.0, znorm = 0.0;
            for (size_t i = 0; i < n; ++i) {
                r[i] = z[i] - rhs[i] - a_h * f_z[i];
                double sc = newton_atol_ + newton_rtol_ * std::max(std::abs(z[i]), std::abs(rhs[i]));
                rnorm += (r[i] / sc) * (r[i] / sc);
                znorm += (z[i] / sc) * (z[i] / sc);
            }
            rnorm = std::sqrt(rnorm / n);
            if (rnorm < 1.0) return true;   // 收敛

            // 雅可比
            Matrix J = numericalJacobian(f_, t_z, z, f_z);

            // 构造 M = I - a_h*J
            Matrix M(n, Vector(n));
            for (size_t i = 0; i < n; ++i)
                for (size_t j = 0; j < n; ++j)
                    M[i][j] = ((i == j) ? 1.0 : 0.0) - a_h * J[i][j];

            // 解 M*delta = -r
            Vector negr(n);
            for (size_t i = 0; i < n; ++i) negr[i] = -r[i];

            Vector delta;
            try {
                delta = solveLinearSystem(M, negr);
            } catch (...) {
                return false;
            }

            // 阻尼: 限制更新幅度
            double dnorm = 0.0;
            for (size_t i = 0; i < n; ++i) dnorm += delta[i] * delta[i];
            dnorm = std::sqrt(dnorm);
            double max_upd = 10.0 * (1.0 + std::sqrt(znorm / n));
            if (dnorm > max_upd && dnorm > 0.0) {
                double s = max_upd / dnorm;
                for (size_t i = 0; i < n; ++i) delta[i] *= s;
            }
            for (size_t i = 0; i < n; ++i) z[i] += delta[i];
        }
        return false;
    }

    // ---------- 单步 TR-BDF2 ----------
    StepResult doStep(double t, const Vector& y_n, const Vector& f_n, double h) {
        const size_t n = y_n.size();
        StepResult sr;

        // ===== 阶段 1: 梯形规则 =====
        // y_g = y_n + (γh/2) * (f_n + f_g)
        double t_g = t + gamma_ * h;
        Vector y_g(n);
        for (size_t i = 0; i < n; ++i)
            y_g[i] = y_n[i] + trap_factor_ * h * f_n[i];  // 显式预测

        Vector rhs_g(n);
        for (size_t i = 0; i < n; ++i)
            rhs_g[i] = y_n[i] + trap_factor_ * h * f_n[i];

        Vector f_g;
        if (!newtonSolve(t_g, rhs_g, trap_factor_ * h, y_g, f_g)) {
            return sr;   // 失败
        }

        // ===== 阶段 2: BDF2 =====
        // y_{n+1} = c1*y_g - c2*y_n + c3*h*f_{n+1}
        double t_next = t + h;
        Vector y_next(n);
        Vector rhs_next(n);
        for (size_t i = 0; i < n; ++i) {
            rhs_next[i] = c1_ * y_g[i] - c2_ * y_n[i];
            y_next[i]  = rhs_next[i];  // 初值
        }

        Vector f_next;
        if (!newtonSolve(t_next, rhs_next, c3_ * h, y_next, f_next)) {
            return sr;   // 失败
        }

        // ===== 误差估计 =====
        // e_i = err_coef * h * (f_n - 2*f_g + f_{n+1})
        double err_norm = 0.0;
        for (size_t i = 0; i < n; ++i) {
            double e = err_coef_ * h * (f_n[i] - 2.0 * f_g[i] + f_next[i]);
            double sc = atol_ + rtol_ * std::max(std::abs(y_n[i]), std::abs(y_next[i]));
            err_norm += (e / sc) * (e / sc);
        }
        err_norm = std::sqrt(err_norm / n);

        sr.success    = true;
        sr.y_new      = std::move(y_next);
        sr.f_new      = std::move(f_next);
        sr.error_norm = err_norm;
        return sr;
    }

    // ---------- 步长调整 ----------
    double adjustStepSize(double h, double err) {
        const double safety  = 0.9;
        const double min_fac = 0.2;
        const double max_fac = 5.0;
        double fac;
        if (err <= 1e-16) fac = max_fac;
        else              fac = safety * std::pow(1.0 / err, 1.0 / 3.0);
        fac = std::min(std::max(fac, min_fac), max_fac);
        double h_new = h * fac;
        return std::min(std::max(h_new, h_min_), h_max_);
    }
};

// ============================================================================
//  示例: 求解范德波尔方程 (刚性问题)
//    y1' = y2
//    y2' = μ*(1 - y1^2)*y2 - y1,   μ = 1000
// ============================================================================
int main() {
    const double mu = 1000.0;
    auto vdp = [mu](double /*t*/, const Vector& y) -> Vector {
        Vector d(2);
        d[0] = y[1];
        d[1] = mu * (1.0 - y[0] * y[0]) * y[1] - y[0];
        return d;
    };

    TRBDF2Solver solver(vdp);
    solver.setTolerances(1e-4, 1e-7);
    solver.setMinMaxStep(1e-12, 0.1);

    Vector y0 = {2.0, 0.0};
    double t0 = 0.0;
    double tf = 3000.0;

    auto t_start = std::chrono::high_resolution_clock::now();
    auto [t_out, y_out] = solver.solve(t0, tf, y0);
    auto t_end = std::chrono::high_resolution_clock::now();

    double elapsed = std::chrono::duration<double>(t_end - t_start).count();

    std::cout << std::scientific << std::setprecision(6);
    std::cout << "==================== TR-BDF2 求解结果 ====================\n";
    std::cout << "时间区间: [" << t0 << ", " << tf << "]\n";
    std::cout << "初值    : y1=" << y0[0] << ", y2=" << y0[1] << "\n";
    std::cout << "容差    : rtol=1e-4, atol=1e-7\n";
    std::cout << "----------------------------------------------------------\n";
    std::cout << "输出点数  : " << t_out.size() << "\n";
    std::cout << "耗时      : " << elapsed << " 秒\n";
    std::cout << "最终解    : t = " << t_out.back()
              << ", y1 = " << y_out.back()[0]
              << ", y2 = " << y_out.back()[1] << "\n";
    std::cout << "==========================================================\n";

    // 打印前 5 个和最后 5 个输出点
    std::cout << "\n前 5 个输出点:\n";
    std::cout << std::setw(18) << "t"
              << std::setw(20) << "y1"
              << std::setw(20) << "y2" << "\n";
    size_t nshow = std::min<size_t>(5, t_out.size());
    for (size_t i = 0; i < nshow; ++i)
        std::cout << std::setw(18) << t_out[i]
                  << std::setw(20) << y_out[i][0]
                  << std::setw(20) << y_out[i][1] << "\n";

    std::cout << "\n最后 5 个输出点:\n";
    std::cout << std::setw(18) << "t"
              << std::setw(20) << "y1"
              << std::setw(20) << "y2" << "\n";
    size_t start = (t_out.size() > 5) ? t_out.size() - 5 : 0;
    for (size_t i = start; i < t_out.size(); ++i)
        std::cout << std::setw(18) << t_out[i]
                  << std::setw(20) << y_out[i][0]
                  << std::setw(20) << y_out[i][1] << "\n";

    return 0;
}