commit 1784db117eb603094aad404dc536bb4cdd97b33b Author: blobt Date: Thu Feb 11 09:22:35 2021 +0800 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d1b76c1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,43 @@ +# ---> C++ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + + +#vs2015 +Debug +Release +x64 +.git +ipch +*.VC.db +*.VC.VC.opendb diff --git a/.vs/3dMath/v15/.suo b/.vs/3dMath/v15/.suo new file mode 100644 index 0000000..c81a293 Binary files /dev/null and b/.vs/3dMath/v15/.suo differ diff --git a/.vs/3dMath/v15/Browse.VC.opendb b/.vs/3dMath/v15/Browse.VC.opendb new file mode 100644 index 0000000..f2a154f Binary files /dev/null and b/.vs/3dMath/v15/Browse.VC.opendb differ diff --git a/3dMath.sln b/3dMath.sln new file mode 100644 index 0000000..8f2d771 --- /dev/null +++ b/3dMath.sln @@ -0,0 +1,31 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.28307.1300 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "3dMath", "3dMath.vcxproj", "{BAB2F10C-2B89-463F-B785-9A18937F6238}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Debug|x64.ActiveCfg = Debug|x64 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Debug|x64.Build.0 = Debug|x64 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Debug|x86.ActiveCfg = Debug|Win32 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Debug|x86.Build.0 = Debug|Win32 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Release|x64.ActiveCfg = Release|x64 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Release|x64.Build.0 = Release|x64 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Release|x86.ActiveCfg = Release|Win32 + {BAB2F10C-2B89-463F-B785-9A18937F6238}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {B523576E-FA21-49B7-90A5-0F9CF9D239BD} + EndGlobalSection +EndGlobal diff --git a/3dMath.vcxproj b/3dMath.vcxproj new file mode 100644 index 0000000..b805f1c --- /dev/null +++ b/3dMath.vcxproj @@ -0,0 +1,135 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {BAB2F10C-2B89-463F-B785-9A18937F6238} + My3dMath + 10.0.17763.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + + + + + + + + + \ No newline at end of file diff --git a/3dMath.vcxproj.filters b/3dMath.vcxproj.filters new file mode 100644 index 0000000..7ed2a6c --- /dev/null +++ b/3dMath.vcxproj.filters @@ -0,0 +1,30 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + 头文件 + + + + + 源文件 + + + 源文件 + + + \ No newline at end of file diff --git a/3dMath.vcxproj.user b/3dMath.vcxproj.user new file mode 100644 index 0000000..6e2aec7 --- /dev/null +++ b/3dMath.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/GMath.cpp b/GMath.cpp new file mode 100644 index 0000000..e8f164f --- /dev/null +++ b/GMath.cpp @@ -0,0 +1,1041 @@ +#include "GMath.h" + +GVector3::GVector3(float x, float y, float z) +{ + v[0] = x; + v[1] = y; + v[2] = z; +} + +GVector3::GVector3(const GVector3 & copy) +{ + v[0] = copy.v[0]; + v[1] = copy.v[1]; + v[2] = copy.v[2]; +} + +GVector3 & GVector3::operator=(const GVector3 & rhs) +{ + v[0] = rhs.v[0]; + v[1] = rhs.v[1]; + v[2] = rhs.v[2]; + return *this; +} + +GVector3 GVector3::operator+(const GVector3 & rhs) const +{ + GVector3 ret; + + ret.v[0] = v[0] + rhs.v[0]; + ret.v[1] = v[1] + rhs.v[1]; + ret.v[2] = v[2] + rhs.v[2]; + + return ret; +} + +GVector3 GVector3::operator-(const GVector3 & rhs) const +{ + GVector3 ret; + + ret.v[0] = v[0] - rhs.v[0]; + ret.v[1] = v[1] - rhs.v[1]; + ret.v[2] = v[2] - rhs.v[2]; + + return ret; +} + +GVector3 operator*(const GVector3 & lhs, const float & k) +{ + GVector3 ret; + + ret.v[0] = lhs.v[0] * k; + ret.v[1] = lhs.v[1] * k; + ret.v[2] = lhs.v[2] * k; + + return ret; +} + +GVector3 operator*(const float & k, const GVector3& rhs) +{ + GVector3 ret; + + ret.v[0] = rhs.v[0] * k; + ret.v[1] = rhs.v[1] * k; + ret.v[2] = rhs.v[2] * k; + + return ret; +} + +GVector3 operator/(const GVector3 & lhs, const float & k) +{ + GVector3 ret; + assert(k != 0); + ret.v[0] = lhs.v[0] / k; + ret.v[1] = lhs.v[1] / k; + ret.v[2] = lhs.v[2] / k; + + return ret; +} + +float norm(const GVector3 & v) +{ + return SQRT(v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2]); +} + +GVector3 & GVector3::normalize() +{ + float len = norm(*this); + + if (len > PRECISION) { + v[0] /= len; + v[1] /= len; + v[2] /= len; + } + + return *this; +} + +float GVector3::operator*(const GVector3 & rhs) const +{ + float ret; + + ret = v[0] * rhs.v[0] + v[1] * rhs.v[1] +v[2] * rhs.v[2]; + + return ret; +} + +GVector3 proj(const GVector3 & p, const GVector3 & q) +{ + return (p*q) / SQR(norm(q)) * q; +} + +GVector3 perp(const GVector3 & p, const GVector3 & q) +{ + return p - proj(p,q); +} + +GVector3 GVector3::operator^(const GVector3 & rhs) const +{ + GVector3 ret; + + ret.v[0] = v[1] * rhs.v[2] - v[2] * rhs.v[1]; + ret.v[1] = v[2] * rhs.v[0] - v[0] * rhs.v[2]; + ret.v[2] = v[0] * rhs.v[1] - v[1] * rhs.v[0]; + + return ret; +} + +GVector3 & GVector3::Set(const float & x, const float & y, const float & z) +{ + v[0] = x; + v[1] = y; + v[2] = z; + + return *this; +} + +float distance(const GVector3 & v, const GVector3 u) +{ + float ret = 0.0; + + float a = v.v[0] - u.v[0]; + float b = v.v[1] - u.v[1]; + float c = v.v[2] - u.v[2]; + + ret = SQRT(SQR(a) + SQR(b) + SQR(c)); + + return ret; +} + + +GVector3 & GVector3::operator+=(const GVector3 & rhs) +{ + v[0] += rhs.v[0]; + v[1] += rhs.v[1]; + v[2] += rhs.v[2]; + + return *this; +} + + +GVector3 & GVector3::operator-=(const GVector3 & rhs) +{ + v[0] -= rhs.v[0]; + v[1] -= rhs.v[1]; + v[2] -= rhs.v[2]; + + return *this; +} + +ostream & operator<<(ostream & os, const GVector3 & v) +{ + os << "[" << setw(2) << v.v[0] << ", " << setw(2) << v.v[1] << ", " << setw(2) << v.v[2] << "]"; + return os; +} + +GVector3 & GVector3::operator*=(const float & k) +{ + v[0] *= k; + v[1] *= k; + v[2] *= k; + + return *this; +} + +GVector3 & GVector3::operator/=(const float & k) +{ + v[0] /= k; + v[1] /= k; + v[2] /= k; + + return *this; +} + +GVector3 & GVector3::operator^=(const GVector3 & rhs) +{ + v[0] = v[1] * rhs.v[2] - v[2] * rhs.v[1]; + v[1] = v[2] * rhs.v[0] - v[0] * rhs.v[2]; + v[2] = v[0] * rhs.v[1] - v[1] * rhs.v[0]; + + return *this; +} + +bool GVector3::operator==(const GVector3 rhs) const +{ + return !((*this) != rhs); +} + +bool GVector3::operator!=(const GVector3 rhs) const +{ + return (!EQ(v[0], rhs.v[0], PRECISION) || !EQ(v[1], rhs.v[1], PRECISION) || !EQ(v[2], rhs.v[2], PRECISION)); +} + +GVector3 GVector3::operator+() const +{ + return *this; +} + +GVector3 GVector3::operator-() const +{ + return *this * -1; +} + +float & GVector3::operator[](const int & idx) +{ + return v[idx]; +} + +const float GVector3::operator[](const int & idx) const +{ + return v[idx]; +} + + +//----GVector----// +GVector::GVector(int dim) +{ + n = dim; + v = new float[n]; + ARR_ZERO(v, n); +} + +GVector::GVector(int dim, double x, ...) +{ + n = dim; + v = new float[n]; + + va_list ap; + va_start(ap, dim); + for (int i = 0; i < n; i++) { + v[i] = (float)va_arg(ap, double); + } + va_end(ap); +} + +GVector::GVector(const GVector3 & copy) +{ + n = 3; + v = new float[3]; + v[0] = copy[0]; + v[1] = copy[1]; + v[2] = copy[2]; +} + +GVector::GVector(const GVector & copy) +{ + n = copy.n; + v = new float[n]; + memcpy(v, copy.v, n * sizeof(float)); +} + +GVector::~GVector() +{ + if (v) { + delete[] v; + } + v = NULL; +} + +GVector & GVector::Set(double x, ...) +{ + v[0] = (float)x; + + va_list ap; + va_start(ap, x); + for (int i = 1; i < n; i++) { + v[i] = (float)va_arg(ap, double); + } + va_end(ap); + return *this; +} + +GVector & GVector::Set(float * p) +{ + memcpy(v, p, sizeof(float) * n); + return *this; +} + +GVector & GVector::operator=(const GVector & rhs) +{ + if (v) { + delete[] v; + } + n = rhs.n; + v = new float[n]; + memcpy(v, rhs.v, n * sizeof(float)); + return *this; +} + +GVector & GVector::operator+=(const GVector & rhs) +{ + assert(n = rhs.n); + for (int i = 0; i < n; i++) { + v[i] += rhs.v[i]; + } + return *this; +} + +GVector & GVector::operator-=(const GVector & rhs) +{ + assert(n = rhs.n); + for (int i = 0; i < n; i++) { + v[i] -= rhs.v[i]; + } + return *this; +} + +GVector & GVector::operator+=(const float & k) +{ + for (int i = 0; i < n; i++) { + v[i] += k; + } + return *this; +} + +GVector & GVector::operator-=(const float & k) +{ + for (int i = 0; i < n; i++) { + v[i] -= k; + } + return *this; +} + +GVector & GVector::operator*=(const float & k) +{ + for (int i = 0; i < n; i++) { + v[i] *= k; + } + return *this; +} + +GVector & GVector::operator/=(const float & k) +{ + for (int i = 0; i < n; i++) { + v[i] /= k; + } + return *this; +} + +bool GVector::operator==(const GVector & rhs) const +{ + return ((*this) == rhs); +} + +bool GVector::operator!=(const GVector & rhs) const +{ + assert(n == rhs.n); + for (int i = 0; i < n; i++) { + if (!EQ(v[i], rhs.v[i], PRECISION)) { + return true; + } + } + return false; +} + +GVector GVector::operator+() const +{ + return *this; +} + +GVector GVector::operator-() const +{ + return *this * -1; +} + +GVector GVector::operator+(const GVector & rhs) const +{ + assert(n == rhs.n); + GVector ret = GVector(n); + for (int i = 0; i < n; i++) { + ret.v[i] = v[i] + rhs.v[i]; + } + return ret; +} + +GVector GVector::operator-(const GVector & rhs) const +{ + assert(n == rhs.n); + GVector ret = GVector(n); + for (int i = 0; i < n; i++) { + ret.v[i] = v[i] - rhs.v[i]; + } + return ret; +} + +float GVector::operator*(const GVector & rhs) const +{ + assert(n == rhs.n); + float ret = 0; + for (int i = 0; i < n; i++) { + ret += v[i] * rhs.v[i]; + } + return ret; +} + +GVector GVector::operator/(const float & k) const +{ + GVector ret = GVector(n); + for (int i = 0; i < n; i++) { + ret.v[i] = v[i]/k; + } + return ret; +} + +float & GVector::operator[](const int & idx) +{ + assert(idx >= 0 && idx <= n); + return v[idx]; +} + +const float & GVector::operator[](const int & idx) const +{ + assert(idx >= 0 && idx <= n); + return v[idx]; +} + +GVector & GVector::Nornalize() +{ + float m = norm(*this); + for (int i = 0; i < n; i++) { + v[i] /= m; + } + return *this; +} + +int GVector::GetDim() const +{ + return n; +} + + +GVector operator*(const float & k, const GVector & rhs) +{ + GVector ret(rhs.n); + for (int i = 0; i < ret.n; i++) { + ret.v[i] *= k; + } + return ret; +} + +GVector operator*(const GVector & lhs, const float & k) +{ + GVector ret(lhs.n); + for (int i = 0; i < ret.n; i++) { + ret.v[i] *= k; + } + return ret; +} + +float norm(const GVector & v) +{ + float ret = 0; + + for (int i = 0; i < v.n; i++) { + ret += SQR(v.v[i]); + } + ret = SQRT(ret); + return ret; +} + +float distance(const GVector & v, const GVector & u) +{ + return norm(v - u); +} + +ostream & operator<<(ostream & os, const GVector & v) +{ + os << "[ "; + for (int i = 0; i < v.n; i++) { + os << setw(5) << v.v[i]; + if (i != v.n - 1) { + os << ", "; + } + } + os << " ]" << endl; + return os; +} + +GMatrix::GMatrix(int row, int col, float * elem) +{ + r = row; + c = col; + m = new float[r*c]; + if (elem) { + memcpy(m, elem, sizeof(float)*r*c); + } + else { + ARR_ZERO(m, r*c); + } +} + +GMatrix::GMatrix(const GMatrix & copy) +{ + r = copy.r; + c = copy.c; + m = new float[r*c]; + memcpy(m, copy.m, sizeof(float)*r*c); +} + +GMatrix::~GMatrix() +{ + if (m) { + delete[] m; + } + m = NULL; +} + +GMatrix & GMatrix::operator=(const GMatrix & rhs) +{ + if (m) { + delete[] m; + } + r = rhs.r; + c = rhs.c; + m = new float[r*c]; + memcpy(m, rhs.m, sizeof(float)*r*c); + return *this; +} + +GMatrix & GMatrix::operator+=(const GMatrix & rhs) +{ + assert(r == rhs.r && c == rhs.c); + for (int i = 0; i < r*c; i++) { + m[i] += rhs.m[i]; + } + return *this; +} + +GMatrix & GMatrix::operator-=(const GMatrix & rhs) +{ + assert(r == rhs.r && c == rhs.c); + for (int i = 0; i < r*c; i++) { + m[i] -= rhs.m[i]; + } + return *this; +} + +GMatrix & GMatrix::operator*=(const float & k) +{ + for (int i = 0; i < r*c; i++) { + m[i] *= k; + } + return *this; +} + +GMatrix & GMatrix::operator*=(const GMatrix & rhs) +{ + assert(c == rhs.r); + c = rhs.c; + float* p = new float[r*c]; + ARR_ZERO(p, r*c) + ; + for (int i = 0; i < r; i++) { + for (int j = 0; j < c; j++) { + for (int k = 0; k < rhs.r; k++) { + p[i*c + j] += m[i*rhs.r + k] * rhs.m[k*c + j]; + } + } + } + + delete[] m; + m = p; + return *this; +} + +GMatrix & GMatrix::operator/=(const float & k) +{ + assert(k != 0); + for (int i = 0; i < r*c; i++) { + m[i] /= k; + } + return *this; +} + +GMatrix GMatrix::operator+() const +{ + return *this; +} + +GMatrix GMatrix::operator-() const +{ + return *this*-1; +} + +GMatrix GMatrix::operator+(const GMatrix & rhs) const +{ + assert(r == rhs.r && c == rhs.c); + GMatrix ret(*this); + ret += rhs; + return ret; +} + +GMatrix GMatrix::operator-(const GMatrix & rhs) const +{ + assert(r == rhs.r && c == rhs.c); + GMatrix ret(*this); + ret -= rhs; + return ret; +} + +GMatrix GMatrix::operator*(const GMatrix & rhs) const +{ + assert(c == rhs.r); + GMatrix ret(*this); + ret *= rhs; + return ret; +} + +GMatrix GMatrix::operator/(const float & k) const +{ + GMatrix ret(*this); + ret /= k; + return ret; +} + +GMatrix operator*(const GMatrix & lhs, const float & k) +{ + GMatrix ret(lhs); + ret *= k; + return ret; +} + +GMatrix operator*(const float& k, const GMatrix & rhs) +{ + GMatrix ret(rhs); + ret *= k; + return ret; + +} + +GVector operator*(const GMatrix & m, const GVector & v) +{ + assert(m.c == v.n); + GVector ret(m.r); + for (int i = 0; i < m.r; i++) { + for (int j = 0; j < m.c; j++) { + ret.v[i] += m.m[i*m.c + j] * v.v[j]; + } + } + return ret; +} + +GMatrix operator*(const GVector & v, const GMatrix & m) +{ + assert(m.r == 1); + GMatrix ret(v.n, m.c); + for (int i = 0; i < v.n; i++) { + for (int j = 0; j < m.c; j++) { + ret.m[i*m.c + j] = v.v[i] * m.m[j]; + } + } + return ret; +} + +bool GMatrix::operator==(const GMatrix & rhs) const +{ + if (r != rhs.r || c != rhs.c) { + return false; + } + + for (int i = 0; i < r*c; i++) { + if (abs(m[i] - rhs.m[i]) > PRECISION) { + return false; + } + } + + return true; +} + +bool GMatrix::operator!=(const GMatrix & rhs) const +{ + if (r != rhs.r || c != rhs.c) { + return true; + } + + for (int i = 0; i < r*c; i++) { + if (abs(m[i] - rhs.m[i]) > PRECISION) { + return true; + } + } + + return false; +} + +float * GMatrix::operator[](const int idx) +{ + assert(idx >= 0 && idx < r); + return &m[idx*c]; +} + +const float * GMatrix::operator[](const int idx) const +{ + assert(idx >= 0 && idx < r); + return &m[idx*c]; +} + +GMatrix & GMatrix::SetTranspose() +{ + int i, j; + if (r == c) { //Spuare matrix + for (i = 0; i < r; i++) { + for (j = i+1; j < c; j++) { + SWAP(float, m[i*c + j], m[j* r + i]) + } + } + } + else { + float *p = new float[r*c]; + memcpy(p, m, sizeof(float)*r*c); + SWAP(int, r, c); + for (i = 0; i < r; i++) { + for (j = 0; j < c; j++) { + m[i*c + j] = p[j*r + i]; + } + } + delete[] p; + p = NULL; + } + return *this; +} + +GMatrix & GMatrix::SetIdentity() +{ + int min = MIN(r, c); + SetZeros(); + for (int i = 0; i < min; i++) { + m[i*c + i] = 1.0f; + } + return *this; +} + +GMatrix & GMatrix::SetZeros() +{ + memset(m, 0, sizeof(float)*r*c); + return *this; +} + +ostream & operator<<(ostream & os, const GMatrix & m) +{ + float r = 0.0f; + for (int i = 0; i < m.r; i++) { + os << "|"; + for (int j = 0; j < m.c; j++) { + r = EQ_ZERO(m.m[i*m.c + j], PRECISION) ? 0 : m.m[i*m.c + j]; + os << setw(6) << r << " "; + } + os << " |" << endl; + } + + return os; +} + +GMatrix & GMatrix::SetRowVec(const int idx, const GVector & v) +{ + assert(idx < r && v.n == c); + for (int i = 0; i < c; i++) { + m[idx*c + i] = v.v[i]; + } + return *this; +} + +GMatrix & GMatrix::SetColVec(const int idx, const GVector & v) +{ + assert(idx < c && v.n == r); + for (int i = 0; i < r; i++) { + m[i*c + idx] = v.v[i]; + } + return *this; +} + +GVector GMatrix::GetRowVec(const int idx) const +{ + assert(idx < r); + GVector ret(c); + for (int i = 0; i < c; i++) { + ret.v[i] = m[idx*c + i]; + } + return ret; +} + +GVector GMatrix::GetColVec(const int idx) const +{ + assert(idx < c); + GVector ret(r); + for (int i = 0; i < r; i++) { + ret.v[i] = m[i*c + idx]; + } + return ret; +} + +GMatrix & GMatrix::ExchangeRows(const int idx0, const int idx1) +{ + GVector tmp(c); + tmp = GetRowVec(idx0); + SetRowVec(idx0, GetRowVec(idx1)); + SetRowVec(idx1, tmp); + return *this; +} + +GMatrix & GMatrix::ExchangeCols(const int idx0, const int idx1) +{ + GVector tmp(r); + tmp = GetColVec(idx0); + SetColVec(idx0, GetColVec(idx1)); + SetColVec(idx1, tmp); + return *this; +} + +int GMatrix::GetRowNum() const +{ + return r; +} + +int GMatrix::GetColNum() const +{ + return c; +} + +bool GMatrix::IsSquare() const +{ + return (r == c) ? true : false; +} + +GMatrix RowEchelonForm(const GMatrix & m) +{ + int i, j, k; + int r = m.GetRowNum(); + int c = m.GetColNum(); + + int n = MIN(r, c); + + GMatrix t(m); + + int shift = 0; + + + + for (i = 0; i < n; i++) { + float max = ABS(t[i][i + shift]); + int privot_idx = i;//к + + //ҳ ֵĵ + for (j = i+1; j < n; j++) { + if (max < ABS(t[j][i + shift])) { + max = ABS(t[j][i + shift]); + privot_idx = j; + } + } + + //ֵ0Ļôֱһ + if (EQ_ZERO(max, PRECISION)) { + shift++; + i--; //ǵҪһ + continue; + } + + //ֵвУǾҪ + if (i != privot_idx) { + t.ExchangeRows(i, privot_idx); + } + + //ȡֵ + float s = t[i][i + shift];//ΪһѾֵн + + //1 + for (j = i + shift; j < c; j++) { + t[i][j] = t[i][j] / s; + } + + //ѵǰУ1µֵ0 + for (j = i + 1; j < r; j++) { + s = t[j][i + shift]; + for (k = i + shift; k < c; k++) { + t[j][k] = t[j][k] - s * t[i][k]; + } + } + } + + return t; +} + +GMatrix ReduceRowEchelonForm(const GMatrix & m) +{ + int i, j, k; + int r = m.GetRowNum(); + int c = m.GetColNum(); + + int n = MIN(r, c); + + GMatrix t(m); + + int shift = 0; + for (i = 0; i < n; i++) { + float max = ABS(t[i][i + shift]); + int privot_idx = i;//к + + //ҳ ֵĵ + for (j = i + 1; j < n; j++) { + if (max < ABS(t[j][i + shift])) { + max = ABS(t[j][i + shift]); + privot_idx = j; + } + } + + //ֵ0Ļôֱһ + if (EQ_ZERO(max, PRECISION)) { + shift++; + i--; //ǵҪһ + continue; + } + + //ֵвУǾҪ + if (i != privot_idx) { + t.ExchangeRows(i, privot_idx); + } + + //ȡֵ + float s = t[i][i + shift];//ΪһѾֵн + + //1 + for (j = i + shift; j < c; j++) { + t[i][j] = t[i][j] / s; + } + + //ѵǰУ1ֵ0 + for (j = 0; j < r; j++) { + if (i == j) + continue; + s = t[j][i + shift]; + for (k = i + shift; k < c; k++) { + t[j][k] = t[j][k] - s * t[i][k]; + } + } + } + + return t; +} + +float * from_arr(const GMatrix & m) +{ + return m.m; +} + +int Rank(const GMatrix & m) +{ + int i, r, rank = 0; + r = m.GetRowNum(); + GMatrix t = RowEchelonForm(m); + + for (i = 0; i < r; i++) { + GVector rVec = t.GetRowVec(i); + if (!EQ_ZERO(norm(rVec), PRECISION)) { + rank++; + } + } + + return rank; +} + +int Nullity(const GMatrix & m) +{ + int rank = Rank(m); + return m.GetColNum() - rank; +} + +GMatrix Mij(const GMatrix & m, int rom, int col) +{ + int i,j = 0; + int r = m.GetRowNum(); + int c = m.GetColNum(); + + assert(r == c);// + assert(rom < r && col < c); + + //ʽ + int nR = r - 1; + int nC = c - 1; + + GMatrix ret(nR, nC); + + nR = 0; + for (i = 0; i < r; i++) { + nC = 0; + if (i == rom) { + continue; + } + for (j = 0; j < c; j++) { + if (j == col) { + continue; + } + ret[nR][nC] = m[i][j]; + nC++; + } + nR++; + } + return ret; +} + +float Det(const GMatrix & m) +{ + int i = 0; + int r = m.GetRowNum(); + int c = m.GetColNum(); + + assert(r == c);//ʽ + + float ret = 0.0f; + + + if (r == 1) { + ret = m[0][0]; + } + else { + for (i = 0; i < c; i++) { + GMatrix t = Mij(m, 0, i); + ret += pow(-1, 0+ i) * m[0][i] * Det(t); + } + } + + return ret; +} + diff --git a/GMath.h b/GMath.h new file mode 100644 index 0000000..f18bfa5 --- /dev/null +++ b/GMath.h @@ -0,0 +1,202 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#define PRECISION 0.000001 +#define SQRT(X) sqrt((X)) //ƽ +#define SQR(X) ((X)*(X)) //ƽ +#define ABS(X) abs((X)) +#define EQ(X,Y,EPS) (ABS((X)-(Y))