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

	return ret;
}