236 lines
6.4 KiB
C++
236 lines
6.4 KiB
C++
#pragma once
|
||
|
||
#ifndef Matrix_h
|
||
#define Matrix_h
|
||
|
||
#include <iostream>
|
||
#define swap2(x, y) ((x) = (x) ^ (y), (y) = (x) ^ (y), (x) = (x) ^ (y))
|
||
|
||
class matrix {
|
||
public:
|
||
matrix(int n, int m);
|
||
matrix(const matrix&);
|
||
~matrix();
|
||
|
||
void Input(double* p);
|
||
void Output();
|
||
int rows() const { return n; };
|
||
int cols() const { return m; };
|
||
|
||
double& operator()(int i, int j) const;
|
||
matrix& operator=(const matrix&);
|
||
matrix operator+(const matrix&) const;
|
||
matrix operator*(const matrix&) const;
|
||
matrix operator+(const double&) const;
|
||
matrix operator*(const double&) const;
|
||
matrix operator/(const double&) const;
|
||
|
||
matrix I();
|
||
matrix T();
|
||
|
||
protected:
|
||
int n, m;
|
||
double** element;
|
||
};
|
||
|
||
matrix::matrix(int n, int m) {
|
||
if (m < 0 || n < 0) std::cout << "Rows and Cols must be >= 0 " << std::endl;
|
||
if ((m == 0 || n == 0) && (m != 0 || n != 0))
|
||
std::cout << "Either both or neither rows and columns should be zero " << std::endl;
|
||
this->m = m, this->n = n;
|
||
element = new double*[n];
|
||
for (int i = 0; i < n; i++) element[i] = new double[m];
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) element[i][j] = 0;
|
||
}
|
||
|
||
matrix::matrix(const matrix& A) {
|
||
n = A.n, m = A.m;
|
||
element = new double*[n];
|
||
for (int i = 0; i < n; i++) element[i] = new double[m];
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) element[i][j] = A.element[i][j];
|
||
}
|
||
|
||
matrix::~matrix() {}
|
||
|
||
matrix& matrix::operator=(const matrix& A) {
|
||
if (this != &A) {
|
||
delete[] element;
|
||
n = A.n, m = A.m;
|
||
element = new double*[n];
|
||
for (int i = 0; i < n; i++) element[i] = new double[m];
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) element[i][j] = A.element[i][j];
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
void matrix::Input(double* p) {
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) this->element[i][j] = *(p++);
|
||
}
|
||
|
||
double& matrix::operator()(int i, int j) const {
|
||
if (i < 0 || i > n - 1 || j < 0 || j > m - 1)
|
||
std::cout << "Matrix Index Out Of Bounds " << std::endl;
|
||
return element[i][j];
|
||
}
|
||
|
||
matrix matrix::operator+(const matrix& A) const {
|
||
if (n != A.n || m != A.m) std::cout << "Matrix Size is Out of batch " << std::endl;
|
||
matrix w(n, m);
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] + A.element[i][j];
|
||
return w;
|
||
}
|
||
|
||
matrix matrix::operator+(const double& C) const {
|
||
matrix w(n, m);
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] + C;
|
||
return w;
|
||
}
|
||
|
||
matrix matrix::operator*(const matrix& A) const {
|
||
if (m != A.n) std::cout << "Matrix Style is Out of batch " << std::endl;
|
||
matrix w(n, A.m);
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < A.m; j++) {
|
||
w.element[i][j] = 0;
|
||
for (int k = 0; k < m; k++) w.element[i][j] += element[i][k] * A.element[k][j];
|
||
}
|
||
return w;
|
||
}
|
||
|
||
matrix matrix::operator*(const double& C) const {
|
||
matrix w(n, m);
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] * C;
|
||
return w;
|
||
}
|
||
|
||
matrix matrix::operator/(const double& C) const {
|
||
matrix w(n, m);
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] / C;
|
||
return w;
|
||
}
|
||
|
||
std::ostream& operator<<(std::ostream& cout, matrix& mA) {
|
||
for (int i = 0; i < mA.rows(); i++) {
|
||
for (int j = 0; j < mA.cols(); j++) cout << mA(i, j) << " ";
|
||
if (i < mA.rows() - 1) cout << std::endl;
|
||
}
|
||
return cout;
|
||
}
|
||
|
||
bool LUPDescomposition(matrix mA, matrix& mL, matrix& mU, int* P) {
|
||
int n = mA.rows();
|
||
int row = 0, tmp;
|
||
double L, U, tmp2;
|
||
for (int i = 0; i < n; i++) P[i] = i;
|
||
for (int i = 0; i < n - 1; i++) {
|
||
tmp2 = 0.;
|
||
for (int j = i; j < n; j++)
|
||
if (std::abs(mA(j, i)) > tmp2) {
|
||
tmp2 = std::abs(mA(j, i));
|
||
row = j;
|
||
}
|
||
|
||
if (tmp2 == 0) {
|
||
AfxMessageBox((CString)"<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>죬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>");
|
||
return FALSE;
|
||
}
|
||
|
||
tmp = P[i], P[i] = P[row], P[row] = tmp;
|
||
|
||
for (int j = 0; j < n; j++) tmp2 = mA(i, j), mA(i, j) = mA(row, j), mA(row, j) = tmp2;
|
||
|
||
U = mA(i, i), L = 0;
|
||
for (int j = i + 1; j < n; j++) {
|
||
L = mA(j, i) / U;
|
||
mA(j, i) = L;
|
||
for (int k = i + 1; k < n; k++) mA(j, k) = mA(j, k) - mA(i, k) * L;
|
||
}
|
||
}
|
||
for (int i = 0; i < n; i++) {
|
||
mL(i, i) = 1;
|
||
for (int j = 0; j < i; j++) mL(i, j) = mA(i, j);
|
||
for (int j = i; j < n; j++) mU(i, j) = mA(i, j);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
matrix LUPSolve(matrix& mL, matrix& mU, int* P, double* b) {
|
||
int n = mL.rows();
|
||
matrix mX(n, 1);
|
||
double* y = new double[n]();
|
||
|
||
for (int i = 0; i < n; i++) {
|
||
y[i] = b[P[i]];
|
||
for (int j = 0; j < i; j++) y[i] -= mL(i, j) * y[j];
|
||
}
|
||
|
||
for (int i = n - 1; i >= 0; i--) {
|
||
mX(i, 0) = y[i];
|
||
for (int j = n - 1; j > i; j--) mX(i, 0) -= mU(i, j) * mX(j, 0);
|
||
mX(i, 0) /= mU(i, i);
|
||
}
|
||
|
||
return mX;
|
||
}
|
||
|
||
// Inverse
|
||
matrix matrix::I() {
|
||
matrix mX(n, 1);
|
||
matrix mL(n, m);
|
||
matrix mU(n, m);
|
||
matrix mMir(n, m);
|
||
matrix mInv(n, m);
|
||
int* P = new int[n]();
|
||
double* b = new double[n]();
|
||
|
||
if (n != m) {
|
||
AfxMessageBox((CString)"<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>");
|
||
return mInv;
|
||
}
|
||
|
||
for (int i = 0; i < n; i++)
|
||
for (int j = 0; j < m; j++) mMir.element[i][j] = element[i][j];
|
||
LUPDescomposition(mMir, mL, mU, P);
|
||
|
||
for (int i = 0; i < n; i++) {
|
||
for (int j = 0; j < n; j++) b[j] = 0;
|
||
b[i] = 1;
|
||
|
||
mX = LUPSolve(mL, mU, P, b);
|
||
for (int j = 0; j < n; j++) mInv(j, i) = mX(j, 0);
|
||
}
|
||
return mInv;
|
||
}
|
||
|
||
// Transposition
|
||
matrix matrix::T() {
|
||
matrix w(m, n);
|
||
for (int i = 0; i < m; i++)
|
||
for (int j = 0; j < n; j++) w.element[i][j] = element[j][i];
|
||
return w;
|
||
}
|
||
|
||
double Gaussian_(double x, double A, double mean, double sigma) {
|
||
return A * exp(-(x - mean) * (x - mean) / (2 * sigma * sigma));
|
||
}
|
||
|
||
matrix GaussianJacobian_(int n, double* X, double A, double mean, double sigma) {
|
||
matrix J(n, 3);
|
||
for (int i = 0; i < n; i++) {
|
||
J(i, 0) = -Gaussian_(X[i], A, mean, sigma) / A;
|
||
J(i, 1) = -(X[i] - mean) * Gaussian_(X[i], A, mean, sigma) / (sigma * sigma);
|
||
J(i, 2) = -(X[i] - mean) * (X[i] - mean) * Gaussian_(X[i], A, mean, sigma) /
|
||
(sigma * sigma * sigma);
|
||
}
|
||
return J;
|
||
}
|
||
|
||
#endif |