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

// fgoalattain.cpp
// 零第三方依赖：仅需 C++17 标准库
// 编译: g++ -std=c++17 -O2 fgoalattain_standalone.cpp -o fgoalattain
#include <iostream>
#include <vector>
#include <cmath>
#include <functional>
#include <algorithm>
#include <stdexcept>
#include <string>
#include <iomanip>
using Vec = std::vector<double>;
using Mat = std::vector<Vec>;
// ================= 基础向量运算 =================
Vec operator+(const Vec& a, const Vec& b) {
Vec r(a.size());
for (size_t i = 0; i < a.size(); ++i) r[i] = a[i] + b[i];
return r;
}
Vec operator–(const Vec& a, const Vec& b) {
Vec r(a.size());
for (size_t i = 0; i < a.size(); ++i) r[i] = a[i] – b[i];
return r;
}
Vec operator*(double s, const Vec& a) {
Vec r(a.size());
for (size_t i = 0; i < a.size(); ++i) r[i] = s * a[i];
return r;
}
double dot(const Vec& a, const Vec& b) {
double s = 0;
for (size_t i = 0; i < a.size(); ++i) s += a[i] * b[i];
return s;
}
double norm2(const Vec& a) { return std::sqrt(dot(a, a)); }
// ================= 高斯消元解线性方程组 A x = b =================
Vec solve_lin(const Mat& A_, const Vec& b_) {
int n = (int)b_.size();
Mat A = A_;
Vec b = b_;
for (int k = 0; k < n; ++k) {
int piv = k;
double mx = std::abs(A[k][k]);
for (int i = k + 1; i < n; ++i)
if (std::abs(A[i][k]) > mx) { mx = std::abs(A[i][k]); piv = i; }
if (mx < 1e-14) throw std::runtime_error("Singular matrix");
std::swap(A[k], A[piv]);
std::swap(b[k], b[piv]);
for (int i = k + 1; i < n; ++i) {
double m = A[i][k] / A[k][k];
for (int j = k; j < n; ++j) A[i][j] -= m * A[k][j];
b[i] -= m * b[k];
}
}
Vec x(n);
for (int i = n – 1; i >= 0; —i) {
double s = b[i];
for (int j = i + 1; j < n; ++j) s -= A[i][j] * x[j];
x[i] = s / A[i][i];
}
return x;
}
// ================= 问题定义 =================
struct GoalAttainProblem {
int n_vars = 0; // 设计变量 x 的维度
int n_objectives = 0; // 目标函数 F 的维度
Vec goal; // 目标向量
Vec weight; // 权重向量
std::function<Vec(const Vec&)> F; // 目标函数 F(x)
std::function<Vec(const Vec&)> nonlin_ineq; // 非线性不等式 c(x) <= 0
std::function<Vec(const Vec&)> nonlin_eq; // 非线性等式 ceq(x) = 0
Vec lb, ub; // 变量上下界
};
// ================= 求解结果 =================
struct SolveResult {
Vec x; // 最优解
Vec fval; // F(x*)
double gamma; // 达到因子
bool success = false;
std::string msg;
};
// ================= 主求解函数：fgoalattain =================
SolveResult fgoalattain(const GoalAttainProblem& P, const Vec& x0,
bool verbose = false)
{
const int nv = P.n_vars;
const int no = P.n_objectives;
const int n = nv + 1; // 增广变量 z = [x; gamma]
// 探测用户约束的维度
Vec probe(nv, 0.0);
const int n_user_ineq = (int)P.nonlin_ineq(probe).size();
const int n_user_eq = (int)P.nonlin_eq(probe).size();
// 不等式约束函数（返回 c(z) <= 0）
auto eval_ineq = [&](const Vec& z) -> Vec {
Vec x(z.begin(), z.begin() + nv);
double g = z[nv];
Vec Fx = P.F(x);
Vec c(no + n_user_ineq);
for (int i = 0; i < no; ++i)
c[i] = Fx[i] – P.weight[i] * g – P.goal[i];
Vec u = P.nonlin_ineq(x);
for (int i = 0; i < n_user_ineq; ++i) c[no + i] = u[i];
return c;
};
// 等式约束函数
auto eval_eq = [&](const Vec& z) -> Vec {
Vec x(z.begin(), z.begin() + nv);
return P.nonlin_eq(x);
};
// ———- 构造初始点：保证严格可行 ———-
Vec z(n);
for (int i = 0; i < nv; ++i) z[i] = x0[i];
Vec F0 = P.F(x0);
double gmin = –1e300;
for (int i = 0; i < no; ++i) {
if (std::abs(P.weight[i]) > 1e-15) {
double gv = (F0[i] – P.goal[i]) / P.weight[i];
if (gv > gmin) gmin = gv;
}
}
if (gmin < –1e299) gmin = 0.0;
z[nv] = gmin + 1.0;
// 增大 gamma 直到目标达到约束全部满足
for (int t = 0; t < 200; ++t) {
Vec c = eval_ineq(z);
double mx = –1e300;
for (double v : c) mx = std::max(mx, v);
if (mx < –1e-3) break;
z[nv] += std::max(1.0, 2.0 * mx);
}
// 最终可行性检查
{
Vec c = eval_ineq(z);
bool feas = true;
for (double v : c) if (v >= –1e-10) { feas = false; break; }
if (!feas) {
SolveResult r;
r.x = x0; r.fval = F0; r.gamma = z[nv];
r.success = false;
r.msg = "Initial point not strictly feasible for user constraints";
return r;
}
}
// ———- 内点法主循环 ———-
double mu = 1.0;
const double mu_floor = 1e-9;
const int outer_max = 50;
const double inner_tol = 1e-8;
// 障碍目标函数 phi_mu(z) = gamma – mu*sum log(-c_i(z)) + 等式惩罚
auto phi = [&](const Vec& zz) -> double {
Vec c = eval_ineq(zz);
double v = zz[nv];
double pen = 0.0, infeas = 0.0;
for (double ci : c) {
if (ci >= 0) infeas += ci * ci;
else pen -= mu * std::log(–ci);
}
Vec eq = eval_eq(zz);
double eqpen = 0.0;
for (double h : eq) eqpen += h * h;
// 不可行时给一个大的光滑惩罚，把迭代拉回可行域
return v + pen + (1.0 / mu) * eqpen + 1e6 * infeas;
};
for (int outer = 0; outer < outer_max; ++outer) {
// — 内层 Newton 迭代 —
for (int it = 0; it < 200; ++it) {
double f0 = phi(z);
if (f0 > 1e100) break;
// 数值梯度（中心差分）
Vec g(n, 0.0);
const double eps = 1e-7;
for (int i = 0; i < n; ++i) {
double h = eps * std::max(1.0, std::abs(z[i]));
Vec zp = z, zm = z;
zp[i] += h;
zm[i] -= h;
g[i] = (phi(zp) – phi(zm)) / (2.0 * h);
}
if (norm2(g) < inner_tol * (1.0 + std::abs(f0))) break;
// 数值 Hessian（中心差分）
Mat H(n, Vec(n, 0.0));
for (int i = 0; i < n; ++i) {
for (int j = i; j < n; ++j) {
double hi = eps * std::max(1.0, std::abs(z[i]));
double hj = eps * std::max(1.0, std::abs(z[j]));
Vec zpp = z, zpm = z, zmp = z, zmm = z;
zpp[i] += hi; zpp[j] += hj;
zpm[i] += hi; zpm[j] -= hj;
zmp[i] -= hi; zmp[j] += hj;
zmm[i] -= hi; zmm[j] -= hj;
double val = (phi(zpp) – phi(zpm) – phi(zmp) + phi(zmm))
/ (4.0 * hi * hj);
H[i][j] = H[j][i] = val;
}
}
// 对角正则化（保证可解）
for (int i = 0; i < n; ++i) H[i][i] += 1e-7;
// 牛顿方向 H d = -g
Vec rhs(n);
for (int i = 0; i < n; ++i) rhs[i] = –g[i];
Vec d;
try {
d = solve_lin(H, rhs);
} catch (...) {
// 回退到最速下降
d = –1.0 * g;
double dn = norm2(d);
if (dn > 1e-12) d = (1.0 / dn) * d;
}
// 限制步长，防止数值溢出
double dn = norm2(d);
if (dn > 5.0) d = (5.0 / dn) * d;
// 回溯线搜索（保证可行 + 下降）
double alpha = 1.0;
bool ok = false;
for (int ls = 0; ls < 60; ++ls) {
Vec znew = z + alpha * d;
Vec c = eval_ineq(znew);
bool feas = true;
for (double ci : c) if (ci >= –1e-14) { feas = false; break; }
if (feas) {
double fn = phi(znew);
if (fn < f0) { z = znew; ok = true; break; }
}
alpha *= 0.5;
}
if (!ok) break;
}
if (verbose)
std::cout << " [outer " << outer << "] mu=" << mu
<< ", gamma=" << z[nv] << std::endl;
mu *= 0.25;
if (mu < mu_floor) break;
}
SolveResult r;
r.x.assign(z.begin(), z.begin() + nv);
r.fval = P.F(r.x);
r.gamma = z[nv];
r.success = true;
r.msg = "OK";
return r;
}
// ======================= 测试主程序 =======================
int main() {
std::cout << std::fixed << std::setprecision(6);
std::cout << "================================================\\n";
std::cout << " fgoalattain 独立实现（零外部依赖）\\n";
std::cout << "================================================\\n\\n";
// ———- 示例 1：MATLAB 文档示例 ———-
{
std::cout << "— Example 1: MATLAB 文档示例 —\\n";
GoalAttainProblem P;
P.n_vars = 1;
P.n_objectives = 2;
P.goal = {3.0, 6.0};
P.weight = {1.0, 1.0};
P.F = [](const Vec& x) -> Vec {
double t = x[0];
return { 2.0 + (t – 3.0) * (t – 3.0), 5.0 + t * t / 4.0 };
};
P.nonlin_ineq = [](const Vec&) { return Vec{}; };
P.nonlin_eq = [](const Vec&) { return Vec{}; };
P.lb = {–1e10};
P.ub = { 1e10};
Vec x0 = {1.0};
SolveResult r = fgoalattain(P, x0);
std::cout << " x* = " << r.x[0] << "\\n";
std::cout << " F(x*) = [" << r.fval[0]
<< ", " << r.fval[1] << "]\\n";
std::cout << " attainfactor = " << r.gamma << "\\n";
std::cout << " success = "
<< (r.success ? "true" : "false") << "\\n\\n";
std::cout << " 期望结果: x*=2, F*=[3,6], gamma=0\\n\\n";
}
// ———- 示例 2：带非线性不等式约束 ———-
{
std::cout << "— Example 2: 带约束 —\\n";
GoalAttainProblem P;
P.n_vars = 2;
P.n_objectives = 2;
P.goal = {0.0, 0.0};
P.weight = {1.0, 1.0};
P.F = [](const Vec& x) -> Vec {
return { x[0] * x[0] + x[1] * x[1],
(x[0] – 2.0) * (x[0] – 2.0) + x[1] * x[1] };
};
// x0 + x1 >= 1 -> 1 – x0 – x1 <= 0
P.nonlin_ineq = [](const Vec& x) -> Vec {
return { 1.0 – x[0] – x[1] };
};
P.nonlin_eq = [](const Vec&) { return Vec{}; };
P.lb = {–10.0, –10.0};
P.ub = { 10.0, 10.0};
Vec x0 = {1.0, 1.0};
SolveResult r = fgoalattain(P, x0);
std::cout << " x* = [" << r.x[0]
<< ", " << r.x[1] << "]\\n";
std::cout << " F(x*) = [" << r.fval[0]
<< ", " << r.fval[1] << "]\\n";
std::cout << " attainfactor = " << r.gamma << "\\n";
std::cout << " success = "
<< (r.success ? "true" : "false") << "\\n\\n";
}
// ———- 示例 3：不同权重 ———-
{
std::cout << "— Example 3: 权重 [1, 2] —\\n";
GoalAttainProblem P;
P.n_vars = 1;
P.n_objectives = 2;
P.goal = {3.0, 6.0};
P.weight = {1.0, 2.0}; // 对第二个目标赋予更大权重
P.F = [](const Vec& x) -> Vec {
double t = x[0];
return { 2.0 + (t – 3.0) * (t – 3.0), 5.0 + t * t / 4.0 };
};
P.nonlin_ineq = [](const Vec&) { return Vec{}; };
P.nonlin_eq = [](const Vec&) { return Vec{}; };
P.lb = {–1e10};
P.ub = { 1e10};
Vec x0 = {1.0};
SolveResult r = fgoalattain(P, x0);
std::cout << " x* = " << r.x[0] << "\\n";
std::cout << " F(x*) = [" << r.fval[0]
<< ", " << r.fval[1] << "]\\n";
std::cout << " attainfactor = " << r.gamma << "\\n";
std::cout << " success = "
<< (r.success ? "true" : "false") << "\\n\\n";
}
return 0;
}