From b5b80a0b1f7bdef1b81eac29359ce9569bd6a415 Mon Sep 17 00:00:00 2001 From: blobt Date: Fri, 19 Feb 2021 12:34:04 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=844=E5=85=83=E6=95=B0=EF=BC=8C?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=9F=A9=E9=98=B5=E6=B1=82=E9=80=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vs/3dMath/v15/.suo | Bin 39424 -> 38400 bytes GMath.cpp | 236 +++++++++++++++++++++++++++++++++++++++++++- GMath.h | 29 ++++++ main.cpp | 17 +++- 4 files changed, 277 insertions(+), 5 deletions(-) diff --git a/.vs/3dMath/v15/.suo b/.vs/3dMath/v15/.suo index 89e839a31fa9246176dec8954fd08919367c3d76..33c6da62476dc47920193ff22e7a226da209f482 100644 GIT binary patch delta 1743 zcmd^9QEXFH7{2GUbZvJp-ge#E!B}ruF|dx&Zk-b{40Bf;*>HC7fEtuZI)Si-Xf-2> zEs1fW#lZa^XMDQ_B(Kcx3v!tjl+$Ilrv3*xnKh3y-mKg$n+z2z?}QG!3UWFF&||!D&@A) zS(y}VM9>~^Ivs~fjg}8O1d(p1FFR4e7)Fq6l25H3`+kJ|WmysI2M`U2l@7leV-up; z`Ho`L5HUmxq8$-OtV4O=jy2LbHX%9@OL-|SOlLZo;n$5|2=@37Oz+;tN~+G@*A-Sx z$vT7UHA#^R6nQDq#}r13T~4yalja(WM&vr^fuyKJWM&3=X;nXixzQF#a-Ya;9AYrj zTu>Mtt>!N)qeUa6%an1c0CvXEZf~M$;)=8Qkr($uHO%bif3xIqnXYKLWlgwD97yv^CFIMBIxFutlkK%Ou&B+VZyG$pvP_KlQd&4^-kq z3M2O|70RBzZU|;VXzD&yxV3p^KHw?o6AUIcykX|NFqFRE-`SRaW&6nC7qUBHQ+hRg zm~E}yoN7yLwn-?Yw}r_U4*ZWn5Upeh*AO zQGgSZMN$9c58OxoCFtjg3S)-4wJ4pZ7qE#IN^;-!=0yzv9%3X6QaJcE(kNw2^q z=J%sd++#hm#Owoc$t6L1x?f4By4zCDJq*C@Y}6R-R!@BY{7;|cD}$O@+S_m}*a7Db zG{D}`J?e{By1qGg>_T=xD}J_h;gK`WKp1`-?H!!DqrTU4x8v%qo5h}TD)bI1q4H6i zLY0Tm3{jsX{jvfh-s;zAQ6Ij{uW;;F{5RadXwSARk$>(uH*8mI|0h>$Et=1AczCc4 z$`7id=g@0R?OS>6+PgoFBwjr~f{!&>3+V%|UKSSV zO{nXIDe+PI4qOv2SgLS+3j((lVTZRBmURA<0|g?YMN?Riazk$9Mp}!gMwm328L3_4 zxefYLvJD~KfT%|_YO*Gzts2j5`QT39jIuUFJ0b8=jn5g>Y&=NAh)sywz^DIqE0wq1QfuV(#WWh(SaYO%}6~LQg|Pa=>nBKfSJgR5ioiP^@&V zP?}-ziAz?K_6*B5Dwa;%UWvR^j%4E~%!dQ|mr-$8U2Zf`P^4P0)d*@`O{Wg}+-LKC zg<5!7xQpEF^2QCB^&Hr2p!xbfYArR8Va)*znOrN&cj4h1QC{hV^5M5W2P|~nSSf0` zb3()VQaWKF)&ep!K{7lHAG4t8Mcls^L14x4sgZY9#Z7g|Wudt?mJJ-EOq~j(DD|kz z?ZXu2T>&-Oap6Xu$qb!?k^WY5FdUGN_fH%=nizra$DQ!um+}mm7X2gHj&)l^}vxMIiU&`eE!@3&2@`62sM$(-6>>4^Xm*2mclkwxFbpF37Bes zQVVQfIj|-J%RgLeFd2C=a2TN>8lh&mKl+W;gT4CQFU2CK_27$kn!C_KAqkQsen<^D z9S@O7a*TNKnRv+|(t+A)-06YB&}K}xnBQg+;i+BIVA_>d4Wqv?iPw{V3Q7u|*}0u( zMvaY$%yo%Bi_&N=TtS|{Q{^$uMpQq$*U3DDY9_>AkdWu^pZ^0o^^b(W^7knegz+s^ zgrSb-l@T4;t`$IpX#YlQSPle3p>k=4P}9U@IUDP-#b@55bv-8!FJD|Z#D1UM3a`ao zFdo}&Bm1fsUufTQYH;(Hnee6Q^P0j9S7VW(J*Th!aQEO@_XlIIq~1Ss{B`af?;<{? zF~|q8t(Gybzv_{qvllV=vi9$pXn9vta}mLIG;!|S$;%U+)2H4vJ{z-J;PVOD)>pMw zs8;X!f_lEj%e?$X(pC5o=SL9bWUL%;+yAR_ruF6%_{H+ySV#UUA;iY~@bu0UmO4Y% sNY&>@Cz({~t$!B%r&UG2hCH`hf6-U=4*Wj~4v$aX1+$a#zk!lJ0p%HZGynhq diff --git a/GMath.cpp b/GMath.cpp index e16020e..8c3566e 100644 --- a/GMath.cpp +++ b/GMath.cpp @@ -713,6 +713,46 @@ const float * GMatrix::operator[](const int idx) const return &m[idx*c]; } +GMatrix & GMatrix::SetInverse() +{ + //只有方阵才能求逆 + assert(r == c); + + //并不是所有矩阵都有逆矩阵，非奇异矩阵才有逆矩阵，也就是行列式不能为0 + float det = Det(*this); + assert(det != 0); + + //构建增广矩阵 + int exc = c * 2; + GMatrix exM(r, exc); + + int i, j = 0; + for (i = 0; i < exM.GetRowNum(); i++) { + for (j = 0; j < exM.GetColNum(); j++) { + if (j < c) { + exM[i][j] = m[i*c + j]; + } + else { + if (j - c == i) { + exM[i][j] = 1.0f; + } + } + } + } + + exM = ReduceRowEchelonForm(exM); + + for (i = 0; i < exM.GetRowNum(); i++) { + for (j = 0; j < exM.GetColNum(); j++) { + if (j >= c) { + m[i*c + (j - c)] = exM[i][j]; + } + } + } + + return *this; +} + GMatrix & GMatrix::SetTranspose() { int i, j; @@ -1015,6 +1055,18 @@ GMatrix Mij(const GMatrix & m, int rom, int col) return ret; } +GMatrix Cij(const GMatrix & m, int r, int c) +{ + GMatrix ret = Mij(m, r, c); + int t = pow(-1, r + c); + if (t == -1) { + for (int i = 0; i < ret.GetColNum(); i++) { + ret[0][i] = -ret[0][i];//行列式运算的-号只能影响一行 + } + } + return ret; +} + float Det(const GMatrix & m) { int i = 0; @@ -1031,8 +1083,11 @@ float Det(const GMatrix & m) } else { for (i = 0; i < c; i++) { - GMatrix t = Mij(m, 0, i); - ret += pow(-1, 0+ i) * m[0][i] * Det(t); + //GMatrix t = Mij(m, 0, i); + //ret += pow(-1, 0+ i) * m[0][i] * Det(t); + GMatrix t = Cij(m, 0, i); + //cout << t << endl; + ret += m[0][i] * Det(t); } } @@ -1270,3 +1325,180 @@ GQuater & GQuater::operator*=(const GQuater & rhs) z = nz; return *this; } + +GQuater & GQuater::operator*=(const float & s) +{ + w *= s; + x *= s; + y *= s; + z *= s; + return *this; +} + +GQuater & GQuater::operator/=(const float & s) +{ + w /= s; + x /= s; + y /= s; + z /= s; + return *this; +} + +GQuater GQuater::operator+() const +{ + return *this; +} + +GQuater GQuater::operator-() const +{ + //return *this*-1.0f; + return GQuater(); +} + +GQuater GQuater::operator+(const GQuater & rhs) const +{ + GQuater ret(*this); + + ret += rhs; + + return ret; +} + +GQuater GQuater::operator-(const GQuater & rhs) const +{ + GQuater ret(*this); + + ret -= rhs; + + return ret; +} + +GQuater GQuater::operator*(const GQuater & rhs) const +{ + GQuater ret(*this); + + ret *= rhs; + + return ret; +} + +GQuater GQuater::operator*(const float & s) const +{ + GQuater ret(*this); + + ret *= s; + + return ret; +} + +GQuater GQuater::operator/(const float & s) const +{ + GQuater ret(*this); + + ret /= s; + + return ret; +} + +GQuater & GQuater::Set(const float w, const float x, const float y, const float z) +{ + this->w = w; + this->x = x; + this->y = y; + this->z = z; + return *this; +} + +GQuater & GQuater::Set(float * q, bool invOrder) +{ + if (invOrder) { + w = q[1]; + x = q[2]; + y = q[3]; + z = q[0]; + } else { + w = q[0]; + x = q[1]; + y = q[2]; + z = q[3]; + } + return *this; +} + +bool GQuater::IsUnitQuater() const +{ + float len = SQR(w) + SQR(x) + SQR(y) + SQR(z); + return EQ(len, 1.0f, PRECISION) ? true : false; +} + +bool GQuater::IsIdentity() const +{ + return EQ(w,1.0f,PRECISION) && EQ(x, 0.0f, PRECISION) && EQ(y, 0.0f, PRECISION) && EQ(z, 0.0f, PRECISION); +} + +GQuater & GQuater::Normalize() +{ + float len = SQR(w) + SQR(x) + SQR(y) + SQR(z); + w /= len; + x /= len; + y /= len; + z /= len; + return *this; +} + +GQuater & GQuater::SetIdentitf() +{ + w /= 1.0f; + x /= 0.0f; + y /= 0.0f; + z /= 0.0f; + return *this; +} + +GQuater & GQuater::SetConjugate() +{ + x *= -1.0f; + y *= -1.0f; + z *= -1.0f; + return *this; +} + +GQuater & GQuater::SetInverse() +{ + if (!IsUnitQuater()) {//如果不是单位四元数要先单位化 + float len = SQR(w) + SQR(x) + SQR(y) + SQR(z); + *this /= len; + } + SetConjugate(); + return *this; +} + +GQuater operator*(const float & s, const GQuater & rhs) +{ + GQuater ret(rhs); + ret *= s; + return ret; +} + +ostream & operator<<(ostream & os, const GQuater & q) +{ + os << "(" << setw(2) << q.w << ", " << setw(2) << q.x << ", " << setw(2) << q.y << ", " << setw(2) << q.z << ")"; + return os; +} + +float norm(const GQuater & q) +{ + float norm = SQRT(SQR(q.w) + SQR(q.x) + SQR(q.y) + SQR(q.z)); + return norm; +} + +GQuater inv(const GQuater & q) +{ + GQuater ret(q); + if (!ret.IsUnitQuater()) {//如果不是单位四元数要先单位化 + float t = SQR(ret.w) + SQR(ret.x) + SQR(ret.y) + SQR(ret.z); + ret /= t; + } + ret.Set(ret.w, -ret.x, -ret.y, -ret.z); + return ret; +} diff --git a/GMath.h b/GMath.h index 2b0340f..0ba5737 100644 --- a/GMath.h +++ b/GMath.h @@ -165,6 +165,7 @@ public: GMatrix operator *(const GMatrix& rhs) const; GMatrix operator /(const float& k) const; + GMatrix& SetInverse(); GMatrix& SetTranspose(); GMatrix& SetIdentity(); GMatrix& SetZeros(); @@ -200,6 +201,7 @@ public: friend int Nullity(const GMatrix& m);//获取矩阵的零化度 friend GMatrix Mij(const GMatrix& m, int r, int c); //计算余子式矩阵 + friend GMatrix Cij(const GMatrix& m, int r, int c); //计算代数余子式矩阵 friend float Det(const GMatrix& m); private: @@ -266,4 +268,31 @@ public: GQuater& operator+=(const GQuater& rhs); GQuater& operator-=(const GQuater& rhs); GQuater& operator*=(const GQuater& rhs); + + GQuater& operator*=(const float& s); + GQuater& operator/=(const float& s); + GQuater operator+() const; + GQuater operator-() const; + + GQuater operator+(const GQuater& rhs) const; + GQuater operator-(const GQuater& rhs) const; + GQuater operator*(const GQuater& rhs) const; + GQuater operator*(const float& s) const; + GQuater operator/(const float& s) const; + + GQuater& Set(const float w, const float x, const float y, const float z); + GQuater& Set(float *q, bool invOrder = false); + + bool IsUnitQuater() const; + bool IsIdentity() const; + + friend GQuater operator*(const float &s, const GQuater &rhs); + friend ostream& operator<<(ostream &os, const GQuater &q); + friend float norm(const GQuater &q); + friend GQuater inv(const GQuater &q); + + GQuater& Normalize(); + GQuater& SetIdentitf(); + GQuater& SetConjugate(); + GQuater& SetInverse(); }; \ No newline at end of file diff --git a/main.cpp b/main.cpp index d040f47..c5e531f 100644 --- a/main.cpp +++ b/main.cpp @@ -2,9 +2,20 @@ int main() { - GQuater q1(1.0f, 1.0f, 2.0f, 1.0f); - GQuater q2(1.0f/7.0f, -1.0f/7.0f, -2.0f/7.0f, -1.0f/7.0f); + float m1[] = { + 2, 3, 8, 4, + 6, 0, -3, 8, + -1, 3, 2, 9, + 2, 0, 4, 10 + }; + + + GMatrix M1(4, 4, m1); + GMatrix M2(4, 4, m1); + + M1.SetInverse(); + cout << M1 << endl; + - q1 *= q2; return 0; } \ No newline at end of file