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.
1064 lines
17 KiB
1064 lines
17 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()
|
|
{
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|