void LUdecomp(std::vector<std::vector<double> > A, std::vector<std::vector<double> > &L, std::vector<std::vector<double> > &U)
{
int n = A.size();
L.assign(n, std::vector<double>(n, 0));
U.assign(n, std::vector<double>(n, 0));
for (int i = 0; i < n; ++i)
U[i][i] = 1;
for (int i = 0; i < n; ++i)
{
for (int j = i; j < n; ++j)
{
L[j][i] = A[j][i];
for (int k = 0; k < i; ++k)
L[j][i] -= L[j][k] * U[k][i];
}
for (int j = i + 1; j < n; ++j)
{
U[i][j] = A[i][j];
for (int k = 0; k < i; ++k)
U[i][j] -= L[i][k] * U[k][j];
U[i][j] /= L[i][i];
}
}
}
void inverseMatrix(std::vector<std::vector<double> > A, std::vector<std::vector<double> > &X)
{
std::vector<std::vector<double> > L(A.size(), std::vector<double>(A.size(), 0));
std::vector<std::vector<double> > U(A.size(), std::vector<double>(A.size(), 0));
LUdecomp(A, L, U);
int n = L.size();
X.assign(n, std::vector<double>(n, 0));
for (int i = n - 1; i >= 0; --i)
{
X[i][i] = 1;
for (int k = i + 1; k < n; ++k)
X[i][i] -= L[k][i] * X[i][k];
X[i][i] /= L[i][i];
for (int j = i - 1; j >= 0; --j)
for (int k = j + 1; k < n; ++k)
X[j][i] -= U[j][k] * X[k][i];
for (int j = i - 1; j >= 0; --j){
for (int k = j + 1; k < n; ++k)
X[i][j] -= X[i][k] * L[k][j];
X[i][j] /= L[j][j];
}
}
}
double f1(double x, double y, double z)
{
return 2 * x - y - exp(-z);
}
double f2(double x, double y, double z)
{
return -x + 2 * y*y - exp(-z);
}
double f3(double x, double y, double z)
{
return exp(x) + y + z;
}
double f11(double x, double y, double z)
{
return 2;
}
double f12(double x, double y, double z)
{
return -1;
}
double f13(double x, double y, double z)
{
return exp(-z);
}
double f21(double x, double y, double z)
{
return -1;
}
double f22(double x, double y, double z)
{
return 4 * y;
}
double f23(double x, double y, double z)
{
return exp(-z);
}
double f31(double x, double y, double z)
{
return exp(x);
}
double f32(double x, double y, double z)
{
return 1;
}
double f33(double x, double y, double z)
{
return 1;
}
void newton(std::vector<double> &x, std::vector<std::vector<double> > &iters)
{
std::vector<double> xNext;
std::vector<std::vector<double> > J(3, std::vector<double>(3, 0));
double(*P[3][3])(double, double, double) = { { f11, f12, f13 }, { f21, f22, f23 }, {f31, f32, f33} };
double norm = 0;
do
{
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
J[i][j] = P[i][j](x[0], x[1], x[2]);
}
}
std::vector<std::vector<double> > invJ;
inverseMatrix(J, invJ);
std::vector < std::vector<double>> F(3, std::vector<double>(1, 0));
F[0][0] = f1(x[0], x[1], x[2]);
F[1][0] = f2(x[0], x[1], x[2]);
F[2][0] = f3(x[0], x[1], x[2]);
std::vector < std::vector<double>> temp(3, std::vector<double>(1, 0));
multiply(invJ, F, temp);
xNext = { temp[0][0], temp[1][0], temp[2][0] };
for (int i = 0; i < 3; ++i)
{
xNext[i] = x[i] - xNext[i];
}
norm = 0;
for (int i = 0; i < 3; ++i)
{
norm = std::max(fabs(xNext[i] - x[i]), norm);
}
x = xNext;
iters.push_back(x);
} while (norm > eps);
}