From 1fb1bcafd5c32f4992a36ead5d497c5416badb2e Mon Sep 17 00:00:00 2001 From: ponchio Date: Tue, 9 Mar 2004 13:57:29 +0000 Subject: [PATCH] Variuos errors and minor changes. --- vcg/math/matrix44.h | 84 ++++++++++++++++++++++++++++++++++++++----- vcg/math/quaternion.h | 17 +++++++-- vcg/math/similarity.h | 39 +++++++++++++++++--- 3 files changed, 125 insertions(+), 15 deletions(-) diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h index 8c72f51c..d3a408b0 100644 --- a/vcg/math/matrix44.h +++ b/vcg/math/matrix44.h @@ -91,7 +91,7 @@ public: 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; + const T &operator[](const int i) const; Matrix44 operator+(const Matrix44 &m) const; Matrix44 operator-(const Matrix44 &m) const; @@ -155,9 +155,10 @@ template Point3 operator*(const Matrix44 &m, const Point3 &p) template Matrix44 &Transpose(Matrix44 &m); //return NULL matrix if not invertible template Matrix44 &Invert(Matrix44 &m); +template Matrix44 Inverse(const Matrix44 &m); typedef Matrix44 Matrix44s; -typedef Matrix44 Matrix44i; +typedef Matrix44 Matrix44i; typedef Matrix44 Matrix44f; typedef Matrix44 Matrix44d; @@ -184,7 +185,7 @@ template T &Matrix44::operator[](const int i) { return ((T *)_a)[i]; } -template T Matrix44::operator[](const int i) const { +template const T &Matrix44::operator[](const int i) const { assert(i >= 0 && i < 16); return ((T *)_a)[i]; } @@ -398,6 +399,19 @@ template Matrix44 &Invert(Matrix44 &m) { return m; } +template Matrix44 Inverse(const Matrix44 &m) { + LinearSolve solve(m); + Matrix44 res; + 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++) + res.element(i, j) = col[i]; + } + return res; +} + /* LINEAR SOLVE IMPLEMENTATION */ @@ -423,7 +437,60 @@ template T LinearSolve::Determinant() const { d is +or -1 depeneing of row permutation even or odd.*/ #define TINY 1e-100 -template bool LinearSolve::Decompose() { +template bool LinearSolve::Decompose() { + + /* Matrix44 A; + for(int i = 0; i < 16; i++) + A[i] = operator[](i); + SetIdentity(); + Point4 scale; + //* Set scale factor, scale(i) = max( |a(i,j)| ), for each row + for(int i = 0; i < 4; i++ ) { + index[i] = i; // Initialize row index list + T scalemax = (T)0.0; + for(int j = 0; j < 4; j++) + scalemax = (scalemax > math::Abs(A.element(i,j))) ? scalemax : math::Abs(A.element(i,j)); + scale[i] = scalemax; + } + + //* Loop over rows k = 1, ..., (N-1) + int signDet = 1; + for(int k = 0; k < 3; k++ ) { + //* Select pivot row from max( |a(j,k)/s(j)| ) + T ratiomax = (T)0.0; + int jPivot = k; + for(int i = k; i < 4; i++ ) { + T ratio = math::Abs(A.element(index[i], k))/scale[index[i]]; + if(ratio > ratiomax) { + jPivot = i; + ratiomax = ratio; + } + } + //* Perform pivoting using row index list + int indexJ = index[k]; + if( jPivot != k ) { // Pivot + indexJ = index[jPivot]; + index[jPivot] = index[k]; // Swap index jPivot and k + index[k] = indexJ; + signDet *= -1; // Flip sign of determinant + } + //* Perform forward elimination + for(int i=k+1; i < 4; i++ ) { + T coeff = A.element(index[i],k)/A.element(indexJ,k); + for(int j=k+1; j < 4; j++ ) + A.element(index[i],j) -= coeff*A.element(indexJ,j); + A.element(index[i],k) = coeff; + //for( j=1; j< 4; j++ ) + // element(index[i],j) -= A.element(index[i], k)*element(indexJ, j); + } + } + for(int i = 0; i < 16; i++) + operator[](i) = A[i]; + + d = signDet; + //*this = A; + return true; */ + d = 1; //no permutation still T scaling[4]; @@ -479,22 +546,23 @@ template bool LinearSolve::Decompose() { element(i,j) *= dum; } } - return true; + return true; } template Point4 LinearSolve::Solve(const Point4 &b) { Point4 x(b); - int first = 0, ip; + int first = -1, ip; for(int i = 0; i < 4; i++) { ip = index[i]; T sum = x[ip]; x[ip] = x[i]; - if(first) + if(first!= -1) for(int j = first; j <= i-1; j++) sum -= element(i,j) * x[j]; else - if (sum) first = i; + if(sum) first = i; + x[i] = sum; } for (int i = 3; i >= 0; i--) { T sum = x[i]; diff --git a/vcg/math/quaternion.h b/vcg/math/quaternion.h index 8bbf610d..385f318d 100644 --- a/vcg/math/quaternion.h +++ b/vcg/math/quaternion.h @@ -38,7 +38,9 @@ public: Point3 Rotate(const Point3 vec) const; }; -template Quaternion interpolate(const Quaternion a, const Quaternion b, double t); +template Quaternion Interpolate(const Quaternion a, const Quaternion b, double t); +template Quaternion &Invert(Quaternion &q); +template Quaternion Inverse(const Quaternion &q); //Implementation @@ -192,7 +194,18 @@ template void Quaternion::FromMatrix(Matrix44 &m) { } } } -template Quaternion interpolate(const Quaternion a, const Quaternion b, double t) { +template Quaternion &Invert(Quaternion &m) { + m.Invert(); + return m; +} + +template Quaternion Inverse(const Quaternion &m) { + Quaternion a = m; + a.Invert(); + return a; +} + +template Quaternion Interpolate(const Quaternion a, const Quaternion b, double t) { double v = a.V(0) * b.V(0) + a.V(1) * b.V(1) + a.V(2) * b.V(2) + a.V(3) * b.V(3); double phi = Acos(v); if(phi > 0.01) { diff --git a/vcg/math/similarity.h b/vcg/math/similarity.h index bff0a5f5..3ddba055 100644 --- a/vcg/math/similarity.h +++ b/vcg/math/similarity.h @@ -48,15 +48,21 @@ public: Similarity &SetScale(const S s); Similarity &SetTranslate(const Point3 &t); ///use radiants for angle. - void SetRotate(S angle, const Point3 & axis); + Similarity &SetRotate(S angle, const Point3 & axis); + Similarity &SetRotate(const Quaternion &q); Matrix44 Matrix() const; + void FromMatrix(const Matrix44 &m); Quaternion rot; Point3 tra; S sca; }; +template Similarity &Invert(Similarity &m); +template Similarity Inverse(const Similarity &m); + + template Similarity Similarity::operator*(const Similarity &a) const { Similarity r; r.rot = rot * a.rot; @@ -98,11 +104,19 @@ template Similarity &Similarity::SetTranslate(const Point3 &t return *this; } -template void Similarity::SetRotate(S angle, const Point3 &axis) { +template Similarity &Similarity::SetRotate(S angle, const Point3 &axis) { SetIdentity(); rot.FromAxis(angle, axis); + return *this; } +template Similarity &Similarity::SetRotate(const Quaternion &q) { + SetIdentity(); + rot = q; + return *this; +} + + template Matrix44 Similarity::Matrix() const { Matrix44 r; rot.ToMatrix(r); @@ -112,15 +126,30 @@ template Matrix44 Similarity::Matrix() const { return r; } -template Similarity &invert(Similarity &a) { - //WARNING:: TEST THIS!!! +template void Similarity::FromMatrix(const Matrix44 &m) { + sca = pow(m.Determinant(), 1/3); + assert(sca != 0); + Matrix44 t = m * Matrix44().SetScale(1/sca, 1/sca, 1/sca); + rot.FromMatrix(t); + tra[0] = t.element(3, 0); + tra[1] = t.element(3, 1); + tra[2] = t.element(3, 2); +} + +template Similarity &Invert(Similarity &a) { a.rot.Invert(); a.sca = 1/a.sca; a.tra = a.rot.Rotate(-a.tra)*a.sca; return a; } -template Similarity interpolate(const Similarity &a, const Similarity &b, const S t) { +template Similarity Inverse(const Similarity &m) { + Similarity a = m; + return Invert(a); +} + + +template Similarity Interpolate(const Similarity &a, const Similarity &b, const S t) { Similarity r; r.rot = interpolate(a.rot, b.rot, t); r.tra = t * a.tra + (1-t) * b.tra;