3dMath/GMath.h

#pragma once
#include <cmath>
#include <stdio.h>
#include <cassert>
#include <iostream>
#include <iomanip>
#include <stdarg.h>

#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))<EPS)//通过误差去判断，两个数是否相等
#define EQ_ZERO(X,EPS) (ABS((X))<EPS)
#define ARR_ZERO(A, N) memset((A), 0, sizeof(A[0])*(N))
#define MIN(x,y) (x)<(y)?(x):(y)
#define SWAP(type, x, y) {type tmp=(x); (x)=(y); (y) = tmp;}
#define M_PI 3.141592

using namespace std;

class GVector3;
class GVector;
class GMatrix;
class GPlane;
class GQuater;
typedef GVector3 GPoint3;

class GVector3 {
private:
	float v[3];
public:
	GVector3(float x = 0.0f, float y = 0.0f, float z = 0.0f);
	GVector3(const GVector3& copy);
	GVector3& operator =(const GVector3& rhs);

	//向量加减
	GVector3 operator +(const GVector3& rhs) const; // u + v
	GVector3 operator -(const GVector3& rhs) const; // u - v

	//向量和常量乘除
	friend GVector3 operator *(const GVector3& lhs, const float& k); // u * k
	friend GVector3 operator *(const float& k, const GVector3& rhs); // k * u
	friend GVector3 operator /(const GVector3& lhs, const float& k); // u / k

	//求向量的模和方向
	friend float norm(const GVector3& v); //求向量大小或模或范数
	GVector3& normalize(); //求向量方向 单位向量

	//求向量的内积
	float operator *(const GVector3& rhs) const;

	//向量投影
	friend GVector3 proj(const GVector3& p, const GVector3& q);
	friend GVector3 perp(const GVector3& p, const GVector3& q);

	//求向量内积 叉乘
	GVector3 operator^(const GVector3& rhs) const;

	//完善向量类1
	GVector3& Set(const float& x, const float& y, const float& z);
	friend float distance(const GVector3& v, const GVector3 u);

	//完善向量类2
	GVector3& operator +=(const GVector3& rhs);
	GVector3& operator -=(const GVector3& rhs);
	friend ostream& operator<<(ostream& os, const GVector3& v);

	//完善向量类3
	GVector3& operator *=(const float& k);
	GVector3& operator /=(const float& k);
	GVector3& operator ^=(const GVector3& rhs);

	//完善向量类4
	bool operator ==(const GVector3 rhs) const;
	bool operator !=(const GVector3 rhs) const;

	//完善向量类5
	GVector3 operator+() const;
	GVector3 operator-() const;
	float& operator [](const int& idx); //下标赋值
	const float operator [](const int& idx) const;//下标获取分量
};

class GVector {
public:
	//GVector 1~2
	GVector(int dim = 3);
	GVector(int dim, double x, ...);
	GVector(const GVector3& copy);
	GVector(const GVector& copy);
	~GVector();
	
	//GVector 3
	GVector& Set(double x, ...);
	GVector& Set(float *p);

	//GVector 4
	GVector& operator =(const GVector& rhs);
	GVector& operator +=(const GVector& rhs);
	GVector& operator -=(const GVector& rhs);
	GVector& operator +=(const float& k);
	GVector& operator -=(const float& k);

	GVector &operator *=(const float &k);
	GVector	&operator /=(const float &k);

	bool operator ==(const GVector& rhs) const;
	bool operator !=(const GVector& rhs) const;

	GVector operator +() const;
	GVector operator -() const;

	GVector operator +(const GVector& rhs) const;
	GVector operator -(const GVector& rhs) const;
	float   operator *(const GVector& rhs) const;
	GVector operator /(const float& k) const;

	float& operator [](const int& idx);
	const float& operator [](const int& idx) const;

	GVector& Nornalize();

	int GetDim() const;

	friend GVector operator *(const float& k, const GVector& rhs);
	friend GVector operator *(const GVector& lhs, const float& k);
	friend GVector operator *(const GMatrix& m, const GVector& v);
	friend GMatrix operator *(const GVector& v, const GMatrix& m);
	friend float norm(const GVector& v);
	friend float distance(const GVector& v, const GVector& u);
	friend ostream& operator <<(ostream& os, const GVector& v);

	friend class GMatrix;
private:
	int n;
	float* v;
};

class GMatrix {
public:
	GMatrix(int row = 4, int col = 4, float *elem = NULL);
	GMatrix(const GMatrix& copy);
	~GMatrix();

	GMatrix& operator =(const GMatrix& rhs);

	GMatrix& operator +=(const GMatrix& rhs);
	GMatrix& operator -=(const GMatrix& rhs);
	GMatrix& operator *=(const float& k);
	GMatrix& operator *=(const GMatrix& rhs);
	GMatrix& operator /=(const float& k);

	bool operator ==(const GMatrix& rhs) const;
	bool operator !=(const GMatrix& rhs) const;

	float* operator [](const int idx);
	const float* operator [](const int idx) const;

	GMatrix operator +() const;
	GMatrix operator -() const;

	GMatrix operator +(const GMatrix& rhs) const;
	GMatrix operator -(const GMatrix& rhs) const;
	GMatrix operator *(const GMatrix& rhs) const;
	GMatrix operator /(const float& k) const;

	GMatrix& SetInverse();
	GMatrix& SetTranspose();
	GMatrix& SetIdentity();
	GMatrix& SetZeros();

	GMatrix& SetRowVec(const int idx, const GVector& v);
	GMatrix& SetColVec(const int idx, const GVector& v);

	GVector GetRowVec(const int idx) const;
	GVector GetColVec(const int idx) const;

	GMatrix& ExchangeRows(const int idx0, const int idx1);
	GMatrix& ExchangeCols(const int idx0, const int idx1);

	int GetRowNum() const;
	int GetColNum() const;

	bool IsSquare() const;

	friend GMatrix operator *(const GMatrix& lhs, const float& k);
	friend GMatrix operator *(const float& k , const GMatrix& rhs);

	friend GVector operator *(const GMatrix& m, const GVector& v);
	friend GMatrix operator *(const GVector& v, const GMatrix& m);
	
	friend ostream& operator <<(ostream& os, const GMatrix& m);

	friend GMatrix RowEchelonForm(const GMatrix& m);
	friend GMatrix ReduceRowEchelonForm(const GMatrix& m);

	friend float* from_arr(const GMatrix& m);

	friend int Rank(const GMatrix& m);//获取矩阵的秩
	friend int Nullity(const GMatrix& m);//获取矩阵的零化度

	friend GMatrix MijMatrix(const GMatrix& m, int r, int c); //计算余子式矩阵
	friend float Mij(const GMatrix& m, int r, int c); //计算余子式
	friend float Cij(const GMatrix& m, int r, int c); //计算代数余子式矩阵的行列式值
	friend float Det(const GMatrix& m);
	friend GMatrix Inverse(const GMatrix& m);
	friend GMatrix Transpose(const GMatrix& m);
	friend GMatrix Adjoint(const GMatrix& m);//求伴随矩阵

private:
	int r;
	int c;
	float *m;
};

class GLine {
private:
	// l(t) = p + t*v;
	GPoint3 p;//端点
	GVector3 v;//方向
public:
	GLine(const GPoint3& _p = GPoint3(0.0f, 0.0f, 0.0f), const GVector3& _v = GVector3(0.0f, 0.0f, 0.0f));
	GLine(const GLine& copy);

	GLine& operator =(const GLine& rhs);
	GPoint3 operator ()(const float t) const;

	GLine& SetPt(const GPoint3& _p);
	GLine& SetDir(const GVector3& _v);

	GVector3 GetPt() const;
	GVector3 GetDir() const;

	bool IsOnLine(const GPoint3& q);
	friend float dist(const GPoint3& q, const GLine& l);

	friend bool intersect_line_plane(GPoint3& p, const GLine& l, const GPlane& pi);
	friend bool intersect_line_triangle(GPoint3& p, const GLine& l, const GPoint3& p1, const GPoint3& p2, const GPoint3& p3);
};

class GPlane {
private:
	//ax+by+cz+d=0 a b c 其实就是法线，d是平面上的一点点乘法线再取负号
	GVector3 n;
	float d;
public:
	GPlane(const GVector3& _n = GVector3(0.0f, 0.0f, 0.0f), const GPoint3& _p = GVector3(0.0f, 0.0f, 0.0f));
	GPlane(const GPoint3& p1, const GPoint3& p2, const GPoint3& p3);
	GPlane(const float& a, const float& b, const float&c, const float& d);
	GPlane(const GPlane& copy);

	GPlane& operator =(const GPlane& rhs);
	GVector3 GetNormal() const;

	friend float dist(const GPlane& pi, const GPoint3& p);

	friend bool intersect_line_plane(GPoint3& p, const GLine& l, const GPlane& pi);

	friend bool intersect_line_triangle(GPoint3& p, const GLine& l, const GPoint3& p1, const GPoint3& p2, const GPoint3& p3);
};

class GQuater {
private:
	float w, x, y, z;
public:
	GQuater(float w = 1.0f, float x = 0.0f, float y = 0.0f, float z = 0.0f);
	GQuater(const GQuater& copy);
	GQuater(GVector3 axis, float theta, bool radian = false);
	GQuater& operator=(const GQuater& rhs);

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