From 073d9ba7706a5f57a637f2468d877461a5a42019 Mon Sep 17 00:00:00 2001 From: ponchio Date: Thu, 19 Feb 2004 14:42:05 +0000 Subject: [PATCH] Created. --- vcg/math/matrix44.h | 489 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 489 insertions(+) create mode 100644 vcg/math/matrix44.h diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h new file mode 100644 index 00000000..b82f832a --- /dev/null +++ b/vcg/math/matrix44.h @@ -0,0 +1,489 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +/**************************************************************************** + History + +$LOG$ + +****************************************************************************/ + +#ifndef __VCGLIB_MATRIX44 +#define __VCGLIB_MATRIX44 + +/* + Annotations: +Opengl stores matrix in column-major order. That is, the matrix is stored as: + + a0 a4 a8 a12 + a1 a5 a9 a13 + a2 a6 a10 a14 + a3 a7 a11 a15 + +e.g. glTranslate generate a matrix that is + + 1 0 0 tx + 0 1 0 ty + 0 0 1 tz + 0 0 0 1 + +Matrix44 stores matrix in row-major order i.e. + + a0 a1 a2 a3 + a4 a5 a6 a7 + a8 a9 a10 a11 + a12 a13 a14 a15 + +and the Translate Function generate: + + 1 0 0 0 + 0 1 0 0 + 0 0 1 0 + tx ty tz 1 + +Le funzioni Translate e Rotate vanno usate con la Apply, +che moltiplica vettore (inteso come riga) per la matrice + +*/ + +#include +#include +#include "../space/Point3.h" +#include "../space/Point4.h" + + +namespace vcg { + +template class Matrix44 { +protected: + T _a[16]; + +public: + typedef T scalar; + + Matrix44() {}; + ~Matrix44() {}; + Matrix44(const Matrix44 &m); + Matrix44(const T v[]); + + T &element(const int row, const int col); + T element(const int row, const int col) const; + T &operator[](const int i); + T operator[](const int i) const; + + Matrix44 operator+(const Matrix44 &m) const; + Matrix44 operator-(const Matrix44 &m) const; + Matrix44 operator*(const Matrix44 &m) const; + Point4 operator*(const Point4 &v) const; + + bool operator==(const Matrix44 &m) const; + bool operator!= (const Matrix44 &m) const; + + Matrix44 operator-() const; + Matrix44 operator*(const T k) const; + void operator+=(const Matrix44 &m); + void operator-=(const Matrix44 &m); + void operator*=( const Matrix44 & m ); + void operator*=( const T k ); + + void SetZero(); + void SetIdentity(); + void SetDiagonal(const T k); + void SetScale(const T sx, const T sy, const T sz); + void SetTranslate(const Point3 &t); + void SetTranslate(const T sx, const T sy, const T sz); + void SetRotate(T angle, const Point3 & axis); //use radiants for angle. + + T Determinant() const; +}; + + +template class LinearSolve: public Matrix44 { +public: + LinearSolve(const Matrix44 &m); + Point4 Solve(Point4 &b); // solve A ยท x = b + T Determinant() const; +protected: + int index[4]; //hold permutation + T d; //hold sign of permutation + void Decompose(); +}; + + +template Point4 operator*(const Matrix44 &m, const Point4 &p); +template Point4 operator*(const Point4 &p, const Matrix44 &m); + +template Matrix44 &Transpose(Matrix44 &m); +template Matrix44 &Invert(Matrix44 &m); + +typedef Matrix44 Matrix44s; +typedef Matrix44 Matrix44i; +typedef Matrix44 Matrix44f; +typedef Matrix44 Matrix44d; + +//Implementazione + +template Matrix44::Matrix44(const Matrix44 &m) { + memcpy((T *)_a, (T *)m._a, 16 * sizeof(T)); +} + +template T &Matrix44::element(const int row, const int col) { + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; +} + +template T Matrix44::element(const int row, const int col) const { + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; +} + +template T &Matrix44::operator[](const int i) { + assert(i >= 0 && i < 16); + return ((T *)_a)[i]; +} + +template T Matrix44::operator[](const int i) const { + assert(i >= 0 && i < 16); + return ((T *)_a)[i]; +} + +template Matrix44 Matrix44::operator+(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret[i] = operator[](i) + m[i]; +} + +template Matrix44 Matrix44::operator-(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret[i] = operator[](i) - m[i]; +} + +template Matrix44 Matrix44::operator*(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 4; i++) + for(int j = 0; j < 4; j++) { + T t = 0.0; + for(int k = 0; k < 4; k++) + t += element[i][k] * m.element[k][j]; + ret.element[i][j] = t; + } + return ret; +} + +template Point4 Matrix44::operator*(const Point4 &v) const { + Point4 ret; + for(int i = 0; i < 4; i++){ + T t = 0.0; + for(int k = 0; k < 4; k++) + t += element[i][k] * v[k]; + ret[i] = t; + } + return ret; +} + + +template bool Matrix44::operator==(const Matrix44 &m) const { + for(int i = 0 ; i < 16; i++) + if(operator[](i) != m[i]) + return false; + return true; +} +template bool Matrix44::operator!=(const Matrix44 &m) const { + for(int i = 0 ; i < 16; i++) + if(operator[](i) != m[i]) + return true; + return false; +} + +template Matrix44 Matrix44::operator-() const { + Matrix44 res; + for(int i = 0; i < 16; i++) + res[i] = -operator[](i); + return res; +} + +template Matrix44 Matrix44::operator*(const T k) const { + Matrix44 res; + for(int i = 0; i < 16; i++) + res[i] = operator[](i) * k; + return res; +} + +template void Matrix44::operator+=(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + operator[](i) += m[i]; +} +template void Matrix44::operator-=(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + operator[](i) -= m[i]; +} +template void Matrix44::operator*=( const Matrix44 & m ) { + for(int i = 0; i < 4; i++) { + Point4 t(0, 0, 0, 0); + for(int k = 0; k < 4; k++) { + for(int j = 0; j < 4; j++) { + t[k] += element(i, k) * m.element(k, j); + } + } + for(int l = 0; l < 4; l++) + element(i, l) = t[l]; + } +} + +template void Matrix44::operator*=( const T k ) { + for(int i = 0; i < 4; i++) + operator[](i) *= k; +} + + +template void Matrix44::SetZero() { + memset((T *)_a, 0, 16 * sizeof(T)); +} + +template void Matrix44::SetIdentity() { + SetDiagonal(1); +} + +template void Matrix44::SetDiagonal(const T k) { + SetZero(); + element(0, 0) = k; + element(1, 1) = k; + element(2, 2) = k; + element(3, 3) = 1; +} + +template void Matrix44::SetScale(const T sx, const T sy, const T sz) { + SetZero(); + element(0, 0) = sx; + element(1, 1) = sy; + element(2, 2) = sz; + element(3, 3) = 1; +} + +template void Matrix44::SetTranslate(const Point3 &t) { + SetTranslate(t[0], t[1], t[2]); +} +template void Matrix44::SetTranslate(const T sx, const T sy, const T sz) { + SetIdentity(); + element(3, 0) = sx; + element(3, 1) = sy; + element(3, 2) = sz; +} +template void Matrix44::SetRotate(T angle, const Point3 & axis) { + //angle = angle*(T)3.14159265358979323846/180; e' in radianti! + T c = Cos(angle); + T s = Sin(angle); + T q = 1-c; + Point3 t = axis; + t.Normalize(); + element(0,0) = t[0]*t[0]*q + c; + element(1,0) = t[0]*t[1]*q - t[2]*s; + element(2,0) = t[0]*t[2]*q + t[1]*s; + element(3,0) = 0; + element(0,1) = t[1]*t[0]*q + t[2]*s; + element(1,1) = t[1]*t[1]*q + c; + element(2,1) = t[1]*t[2]*q - t[0]*s; + element(3,1) = 0; + element(0,2) = t[2]*t[0]*q -t[1]*s; + element(1,2) = t[2]*t[1]*q +t[0]*s; + element(2,2) = t[2]*t[2]*q +c; + element(3,2) = 0; + element(0,3) = 0; + element(1,3) = 0; + element(2,3) = 0; + element(3,3) = 1; +} + + +template T Matrix44::Determinant() const { + LinearSolve solve(*this); + return solve.Determinant(); +} + + +template Point4 operator*(const Matrix44 &m, const Point4 &p) { + T w; + Point4 s; + s.x() = a[0][0]*p.x() + a[0][1]*p.y() + a[0][2]*p.z() + a[0][3]; + s.y() = a[1][0]*p.x() + a[1][1]*p.y() + a[1][2]*p.z() + a[1][3]; + s.z() = a[2][0]*p.x() + a[2][1]*p.y() + a[2][2]*p.z() + a[2][3]; + s.w() = a[3][0]*p.x() + a[3][1]*p.y() + a[3][2]*p.z() + a[3][3]; + if(s.w()!= 0) s /= s.w(); + return s; +} + +template Point4 operator*(const Point4 &p, const Matrix44 &m) { + T w; + Point4 s; + s.x() = a[0][0]*p.x() + a[0][1]*p.y() + a[0][2]*p.z() + a[0][3]; + s.y() = a[1][0]*p.x() + a[1][1]*p.y() + a[1][2]*p.z() + a[1][3]; + s.z() = a[2][0]*p.x() + a[2][1]*p.y() + a[2][2]*p.z() + a[2][3]; + s.w() = a[3][0]*p.x() + a[3][1]*p.y() + a[3][2]*p.z() + a[3][3]; + if(s.w() != 0) s /= s.w(); + return s; +} + +template Matrix44 &Transpose(Matrix44 &m) { + for(int i = 1; i < 4; i++) + for(int j = 0; j < i; j++) { + T t = element(i, j); + element(i, j) = element(j, i); + element(j, i) = t; + } + return *this; +} + + + + +template Matrix44 &Invert(Matrix44 &m) { + LinearSolve solve(m); + + for(int j = 0; j < 4; j++) { //Find inverse by columns. + Point4 col(0, 0, 0, 0); + col[j] = 1.0; + col = solve.Solve(col); + for(int i = 0; i < 4; i++) + m.element(i, j) = col[i]; + } +} + + + +/* LINEAR SOLVE IMPLEMENTATION */ + +template LinearSolve::LinearSolve(const Matrix44 &m): Matrix44(m) { + Decompose(); +} + + +template T LinearSolve::Determinant() const { + T det = d; + for(int j = 0; j < 4; j++) + det *= element(j, j); + return det; +} + + +/*replaces a matrix by its LU decomposition of a rowwise permutation. +d is +or -1 depeneing of row permutation even or odd.*/ +#define TINY 1e-100 + +template void LinearSolve::Decompose() { + d = 1; //no permutation still + + T scaling[4]; + + //Saving the scvaling information per row + for(int i = 0; i < 4; i++) { + T largest = 0.0; + for(int j = 0; j < 4; j++) { + T t = fabs(element(i, j)); + if (t > largest) largest = t; + } + + if (largest == 0.0) { //oooppps there is a zero row! + return; + } + scaling[i] = 1.0 / largest; + } + + int imax; + for(int j = 0; j < 4; j++) { + for(int i = 0; i < j; i++) { + T sum = element(i,j); + for(int k = 0; k < i; k++) + sum -= element(i,k)*element(k,j); + element(i,j) = sum; + } + T largest = 0.0; + for(int i = j; i < 4; i++) { + T sum = element(i,j); + for(int k = 0; k < j; k++) + sum -= element(i,k)*element(k,j); + element(i,j) = sum; + T t = scaling[i] * fabs(sum); + if(t >= largest) { + largest = t; + imax = i; + } + } + if (j != imax) { + for (int k = 0; k < 4; k++) { + T dum = element(imax,k); + element(imax,k) = element(j,k); + element(j,k) = dum; + } + d = -(d); + scaling[imax] = scaling[j]; + } + index[j]=imax; + if (element(j,j) == 0.0) element(j,j) = TINY; + if (j != 3) { + T dum = 1.0 / (element(j,j)); + for (i = j+1; i < 4; i++) + element(i,j) *= dum; + } + } +} + + +template Point4 LinearSolve::Solve(Point4 &b) { + Point4 x(b); + int first = 0, ip; + for(int i = 0; i < 4; i++) { + ip = index[i]; + T sum = x[ip]; + x[ip] = x[i]; + if(first) + for(int j = first; j <= i-1; j++) + sum -= element(i,j) * x[j]; + else + if (sum) first = i; + } + for (int i = 3; i >= 0; i--) { + T sum = x[i]; + for (int j = i+1; j < 4; j++) + sum -= element(i, j) * x[j]; + x[i] = sum / element(i, i); + } + return x; +} + +} //namespace +#endif + + + + +/*#ifdef __GL_H__ +// Applicano la trasformazione intesa secondo la Apply +void glMultMatrix(Matrix44 const & M) { glMultMatrixd(&(M.a[0][0]));} +void glMultMatrix(Matrix44 const & M) { glMultMatrixf(&(M.a[0][0]));} +void glLoadMatrix(Matrix44 const & M) { glLoadMatrixd(&(M.a[0][0]));} +void glLoadMatrix(Matrix44 const & M) { glLoadMatrixf(&(M.a[0][0]));} +#endif*/ + +