3dMath/GMath.cpp

#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::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;
	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 MijMatrix(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 Mij(const GMatrix & m, int r, int c)
{
	GMatrix M = MijMatrix(m, r, c);
	return Det(M);
}

float Cij(const GMatrix & m, int r, int c)
{
	GMatrix M = MijMatrix(m, r, c);
	int t = pow(-1, r + c);
	return t*Det(M);
}

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 = MijMatrix(m, 0, i);
			ret += pow(-1, 0+ i) * m[0][i] * Det(t);
		}
	}

	return ret;
}

GMatrix Inverse(const GMatrix & m)
{
	GMatrix ret(m);
	ret.SetInverse();
	return ret;
}

GMatrix Transpose(const GMatrix & m)
{
	GMatrix ret(m);
	ret.SetTranspose();
	return ret;
}

GMatrix Adjoint(const GMatrix & m)
{
	GMatrix tM = Transpose(m);
	GMatrix ret(tM.r, tM.c);

	int i, j = 0;
	for (i = 0; i < tM.r; i++) {
		for (j = 0; j < tM.c; j++) {
			ret[i][j] = Cij(tM, i, j);
		}
	}
	return ret;
}

GLine::GLine(const GPoint3 & _p, const GVector3 & _v)
{
	p = _p;
	v = _v;
}

GLine::GLine(const GLine & copy)
{
	p = copy.p;
	v = copy.v;
}

GLine & GLine::operator=(const GLine & rhs)
{
	p = rhs.p;
	v = rhs.v;
	return *this;
}

GPoint3 GLine::operator()(const float t) const
{
	return p + v*t;
}

GLine & GLine::SetPt(const GPoint3 & _p)
{
	p = _p;
	return *this;
}

GLine & GLine::SetDir(const GVector3 & _v)
{
	v = _v;
	return *this;
}

GVector3 GLine::GetPt() const
{
	return p;
}

GVector3 GLine::GetDir() const
{
	return v;
}

bool GLine::IsOnLine(const GPoint3 & q)
{
	return EQ_ZERO(dist(q, *this), PRECISION);
}

float dist(const GPoint3 & q, const GLine & l)
{
	GVector3 u = q - l.p;
	GVector3 p = proj(u, l.v);
	return norm(u - p);
}

GPlane::GPlane(const GVector3 & _n, const GPoint3 & _p)
{
	n = _n;
	d = -n * _p;
}

GPlane::GPlane(const GPoint3 & p1, const GPoint3 & p2, const GPoint3 & p3)
{
	n = (p2 - p1) ^ (p3 - p1);
	d = -n * p1; //是平面上的一点点乘法线再取负号
}

GPlane::GPlane(const float & a, const float & b, const float & c, const float & d)
{
	n = GVector3(a, b, c);
	this->d = d;
}

GPlane::GPlane(const GPlane & copy)
{
	n = copy.n;
	d = copy.d;
}

GPlane& GPlane::operator=(const GPlane & rhs)
{
	n = rhs.n;
	d = rhs.d;
	return *this;
}

GVector3 GPlane::GetNormal() const
{
	return n;
}

float dist(const GPlane & pi, const GPoint3 & p)
{
	float D;
	D = (p*pi.n + pi.d) / norm(pi.n);
	return D;
}

bool intersect_line_plane(GPoint3 & p, const GLine & l, const GPlane & pi)
{
	//如果直线的方向和平面的法线垂直，则直线和平面平行，没有交点
	if (EQ_ZERO((l.v*pi.n), PRECISION)) {
		return false;
	}

	float t = -(l.p*pi.n + pi.d) / (l.v * pi.n);

	p = l(t);

	return true;
}

bool intersect_line_triangle(GPoint3 & p, const GLine & l, const GPoint3 & p1, const GPoint3 & p2, const GPoint3 & p3)
{
	GVector3 e1, e2, u, v, w;
	float a, b, t, det;

	e1 = p2 - p1;
	e2 = p3 - p1;
	u = l.v ^ e2;
	det = e1 * u;

	if (EQ_ZERO(det, PRECISION)) {
		return false;
	}

	w = l.p - p1;

	a = w * u / det;
	if (a < 0.0f || a > 1.0f) {
		return false;
	}

	v = w ^ e1;
	b = l.v * v / det;
	if (b < 0.0f || b > 1.0f) {
		return false;
	}

	t = e2 * v / det;
	p = l(t);

	return true; 
}

GQuater::GQuater(float w, float x, float y, float z)
{
	this->w = w;
	this->x = x;
	this->y = y;
	this->z = z;
}

GQuater::GQuater(const GQuater & copy)
{
	w = copy.w;
	x = copy.x;
	y = copy.y;
	z = copy.z;
}

GQuater::GQuater(GVector3 axis, float theta, bool radian)
{
	//q=(cosX, sinX(a,b,c))

	float rad, sn, cs;
	axis.normalize();

	if (!radian) {
		rad = theta * M_PI / 360; //因为旋转的是2倍X，所以这里除以360
	}

	sn = sin(rad);
	cs = cos(rad);

	sn = (abs(sn) < PRECISION) ? 0.0f : sn;
	cs = (abs(cs) < PRECISION) ? 0.0f : cs;

	w = cs;
	x = sn * axis[0];
	y = sn * axis[1];
	z = sn * axis[2];

}

GQuater & GQuater::operator=(const GQuater & rhs)
{
	w = rhs.w;
	x = rhs.x;
	y = rhs.y;
	z = rhs.z;

	return *this;
}

GQuater & GQuater::operator+=(const GQuater & rhs)
{
	w += rhs.w;
	x += rhs.x;
	y += rhs.y;
	z += rhs.z;

	return *this;
}

GQuater & GQuater::operator-=(const GQuater & rhs)
{
	w -= rhs.w;
	x -= rhs.x;
	y -= rhs.y;
	z -= rhs.z;

	return *this;
}

GQuater & GQuater::operator*=(const GQuater & rhs)
{
	float nw, nx, ny, nz = 0;
	nw = w * rhs.w - x*rhs.x - y * rhs.y - z * rhs.z;
	nx = w * rhs.x + rhs.w*x + y * rhs.z - z * rhs.y;
	ny = w * rhs.y + rhs.w*y + z * rhs.x - x * rhs.z;
	nz = w * rhs.z + rhs.w*z + x * rhs.y - y * rhs.x;
	w = nw;
	x = nx;
	y = ny;
	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;
}