完善4元数，添加矩阵求逆

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;
+}

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();
 };

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;
 }