You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

1546 lines
25 KiB

#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(int dim, const GVector3& copy)
{
n = dim;
v = new float[n];
for (int i = 0; i < n; i++) {
if (i < 3) {
v[i] = copy[i];
}
else {
v[i] = 0;
}
}
}
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] = rhs[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] = lhs[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;
}