diff --git a/.vs/3dMath/v15/.suo b/.vs/3dMath/v15/.suo index 89e839a..33c6da6 100644 Binary files a/.vs/3dMath/v15/.suo and b/.vs/3dMath/v15/.suo differ 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