manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Variuos errors and minor changes.

This commit is contained in:
Federico Ponchio 2004-03-09 13:57:29 +00:00
parent 43f74f4627
commit 1fb1bcafd5
3 changed files with 125 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
vcg/math/matrix44.h
View File

@ -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 <class T> Point3<T> operator*(const Matrix44<T> &m, const Point3<T> &p)
template <class T> Matrix44<T> &Transpose(Matrix44<T> &m);
//return NULL matrix if not invertible
template <class T> Matrix44<T> &Invert(Matrix44<T> &m);
template <class T> Matrix44<T> Inverse(const Matrix44<T> &m);
typedef Matrix44<short> Matrix44s;
typedef Matrix44<int> Matrix44i;
typedef Matrix44<int> Matrix44i;
typedef Matrix44<float> Matrix44f;
typedef Matrix44<double> Matrix44d;
@ -184,7 +185,7 @@ template <class T> T &Matrix44<T>::operator[](const int i) {
return ((T *)_a)[i];
}
template <class T> T Matrix44<T>::operator[](const int i) const {
template <class T> const T &Matrix44<T>::operator[](const int i) const {
assert(i >= 0 && i < 16);
return ((T *)_a)[i];
}
@ -398,6 +399,19 @@ template <class T> Matrix44<T> &Invert(Matrix44<T> &m) {
return m;
}
template <class T> Matrix44<T> Inverse(const Matrix44<T> &m) {
LinearSolve<T> solve(m);
Matrix44<T> res;
for(int j = 0; j < 4; j++) { //Find inverse by columns.
Point4<T> 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 <class T> T LinearSolve<T>::Determinant() const {
d is +or -1 depeneing of row permutation even or odd.*/
#define TINY 1e-100
template <class T> bool LinearSolve<T>::Decompose() {
template <class T> bool LinearSolve<T>::Decompose() {
/* Matrix44<T> A;
for(int i = 0; i < 16; i++)
A[i] = operator[](i);
SetIdentity();
Point4<T> 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 <class T> bool LinearSolve<T>::Decompose() {
element(i,j) *= dum;
}
}
return true;
return true;
}
template <class T> Point4<T> LinearSolve<T>::Solve(const Point4<T> &b) {
Point4<T> 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];

17
vcg/math/quaternion.h
View File

@ -38,7 +38,9 @@ public:
Point3<S> Rotate(const Point3<S> vec) const;
};
template <class S> Quaternion<S> interpolate(const Quaternion<S> a, const Quaternion<S> b, double t);
template <class S> Quaternion<S> Interpolate(const Quaternion<S> a, const Quaternion<S> b, double t);
template <class S> Quaternion<S> &Invert(Quaternion<S> &q);
template <class S> Quaternion<S> Inverse(const Quaternion<S> &q);
//Implementation
@ -192,7 +194,18 @@ template <class S> void Quaternion<S>::FromMatrix(Matrix44<S> &m) {
}
}
}
template <class S> Quaternion<S> interpolate(const Quaternion<S> a, const Quaternion<S> b, double t) {
template <class S> Quaternion<S> &Invert(Quaternion<S> &m) {
m.Invert();
return m;
}
template <class S> Quaternion<S> Inverse(const Quaternion<S> &m) {
Quaternion<S> a = m;
a.Invert();
return a;
}
template <class S> Quaternion<S> Interpolate(const Quaternion<S> a, const Quaternion<S> 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) {

39
vcg/math/similarity.h
View File

@ -48,15 +48,21 @@ public:
Similarity &SetScale(const S s);
Similarity &SetTranslate(const Point3<S> &t);
///use radiants for angle.
void SetRotate(S angle, const Point3<S> & axis);
Similarity &SetRotate(S angle, const Point3<S> & axis);
Similarity &SetRotate(const Quaternion<S> &q);
Matrix44<S> Matrix() const;
void FromMatrix(const Matrix44<S> &m);
Quaternion<S> rot;
Point3<S> tra;
S sca;
};
template <class S> Similarity<S> &Invert(Similarity<S> &m);
template <class S> Similarity<S> Inverse(const Similarity<S> &m);
template <class S> Similarity<S> Similarity<S>::operator*(const Similarity &a) const {
Similarity<S> r;
r.rot = rot * a.rot;
@ -98,11 +104,19 @@ template <class S> Similarity<S> &Similarity<S>::SetTranslate(const Point3<S> &t
return *this;
}
template <class S> void Similarity<S>::SetRotate(S angle, const Point3<S> &axis) {
template <class S> Similarity<S> &Similarity<S>::SetRotate(S angle, const Point3<S> &axis) {
SetIdentity();
rot.FromAxis(angle, axis);
return *this;
}
template <class S> Similarity<S> &Similarity<S>::SetRotate(const Quaternion<S> &q) {
SetIdentity();
rot = q;
return *this;
}
template <class S> Matrix44<S> Similarity<S>::Matrix() const {
Matrix44<S> r;
rot.ToMatrix(r);
@ -112,15 +126,30 @@ template <class S> Matrix44<S> Similarity<S>::Matrix() const {
return r;
}
template <class S> Similarity<S> &invert(Similarity<S> &a) {
//WARNING:: TEST THIS!!!
template <class S> void Similarity<S>::FromMatrix(const Matrix44<S> &m) {
sca = pow(m.Determinant(), 1/3);
assert(sca != 0);
Matrix44<S> t = m * Matrix44<S>().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 <class S> Similarity<S> &Invert(Similarity<S> &a) {
a.rot.Invert();
a.sca = 1/a.sca;
a.tra = a.rot.Rotate(-a.tra)*a.sca;
return a;
}
template <class S> Similarity<S> interpolate(const Similarity<S> &a, const Similarity<S> &b, const S t) {
template <class S> Similarity<S> Inverse(const Similarity<S> &m) {
Similarity<S> a = m;
return Invert(a);
}
template <class S> Similarity<S> Interpolate(const Similarity<S> &a, const Similarity<S> &b, const S t) {
Similarity<S> r;
r.rot = interpolate(a.rot, b.rot, t);
r.tra = t * a.tra + (1-t) * b.tra;