From f1a82f35b23ac383e869f0440560f76b27ce620f Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Sat, 10 Feb 2007 18:01:37 +0000 Subject: [PATCH] From Vivek Rajan, MatrixDecomposition implementaion, adapted by Robert Osfield to be part of osg::Matrixf and osg::Matrixd classes. --- VisualStudio/osg/osg.dsp | 4 + include/osg/Matrixd | 20 +- include/osg/Matrixf | 14 +- src/osg/GNUmakefile | 1 + src/osg/MatrixDecomposition.cpp | 682 ++++++++++++++++++++++++++++++++ 5 files changed, 716 insertions(+), 5 deletions(-) create mode 100644 src/osg/MatrixDecomposition.cpp diff --git a/VisualStudio/osg/osg.dsp b/VisualStudio/osg/osg.dsp index ca8e60822..196b99f06 100755 --- a/VisualStudio/osg/osg.dsp +++ b/VisualStudio/osg/osg.dsp @@ -400,6 +400,10 @@ SOURCE=..\..\src\osg\Matrixf.cpp # End Source File # Begin Source File +SOURCE=..\..\src\osg\MatrixDecomposition.cpp +# End Source File +# Begin Source File + SOURCE=..\..\src\osg\MatrixTransform.cpp # End Source File # Begin Source File diff --git a/include/osg/Matrixd b/include/osg/Matrixd index 020140467..bc1167f3c 100644 --- a/include/osg/Matrixd +++ b/include/osg/Matrixd @@ -91,8 +91,6 @@ class OSG_EXPORT Matrixd value_type * ptr() { return (value_type*)_mat; } const value_type * ptr() const { return (const value_type *)_mat; } - void makeIdentity(); - bool isIdentity() const { return _mat[0][0]==1.0 && _mat[0][1]==0.0 && _mat[0][2]==0.0 && _mat[0][3]==0.0 && @@ -100,6 +98,8 @@ class OSG_EXPORT Matrixd _mat[2][0]==0.0 && _mat[2][1]==0.0 && _mat[2][2]==1.0 && _mat[2][3]==0.0 && _mat[3][0]==0.0 && _mat[3][1]==0.0 && _mat[3][2]==0.0 && _mat[3][3]==1.0; } + + void makeIdentity(); void makeScale( const Vec3f& ); void makeScale( const Vec3d& ); @@ -122,8 +122,20 @@ class OSG_EXPORT Matrixd value_type angle2, const Vec3d& axis2, value_type angle3, const Vec3d& axis3); - - + + /** decompose the matrix into translation, rotation, scale and scale orietation.*/ + void decompose( osg::Vec3f& translation, + osg::Quat& rotation, + osg::Vec3f& scale, + osg::Quat& so ) const; + + /** decompose the matrix into translation, rotation, scale and scale orietation.*/ + void decompose( osg::Vec3d& translation, + osg::Quat& rotation, + osg::Vec3d& scale, + osg::Quat& so ) const; + + /** Set to an orthographic projection. * See glOrtho for further details. */ diff --git a/include/osg/Matrixf b/include/osg/Matrixf index 199968168..819260184 100644 --- a/include/osg/Matrixf +++ b/include/osg/Matrixf @@ -123,7 +123,19 @@ class OSG_EXPORT Matrixf value_type angle3, const Vec3d& axis3); - + /** decompose the matrix into translation, rotation, scale and scale orietation.*/ + void decompose( osg::Vec3f& translation, + osg::Quat& rotation, + osg::Vec3f& scale, + osg::Quat& so ) const; + + /** decompose the matrix into translation, rotation, scale and scale orietation.*/ + void decompose( osg::Vec3d& translation, + osg::Quat& rotation, + osg::Vec3d& scale, + osg::Quat& so ) const; + + /** Set to an orthographic projection. * See glOrtho for further details. */ diff --git a/src/osg/GNUmakefile b/src/osg/GNUmakefile index e90882133..384f7560d 100644 --- a/src/osg/GNUmakefile +++ b/src/osg/GNUmakefile @@ -63,6 +63,7 @@ CXXFILES =\ Material.cpp\ Matrixd.cpp\ Matrixf.cpp\ + MatrixDecomposition.cpp\ MatrixTransform.cpp\ Multisample.cpp\ NodeCallback.cpp\ diff --git a/src/osg/MatrixDecomposition.cpp b/src/osg/MatrixDecomposition.cpp new file mode 100644 index 000000000..2ac3b044b --- /dev/null +++ b/src/osg/MatrixDecomposition.cpp @@ -0,0 +1,682 @@ +/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield + * + * This library is open source and may be redistributed and/or modified under + * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or + * (at your option) any later version. The full license is in LICENSE file + * included with this distribution, and on the openscenegraph.org website. + * + * This library 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 + * OpenSceneGraph Public License for more details. +*/ +//osgManipulator - Copyright (C) 2007 Fugro-Jason B.V. + +// Matrix decomposition code taken from Graphics Gems IV +// http://www.acm.org/pubs/tog/GraphicsGems/gemsiv/polar_decomp +// Copyright (C) 1993 Ken Shoemake + +#include +#include + +/** Copy nxn matrix A to C using "gets" for assignment **/ +#define matrixCopy(C, gets, A, n) {int i, j; for (i=0;imax) {max = abs; col = j;} + } + return col; + } + + /** Setup u for Household reflection to zero all v components but first **/ + void make_reflector(double *v, double *u) + { + double s = sqrt(vdot(v, v)); + u[0] = v[0]; u[1] = v[1]; + u[2] = v[2] + ((v[2]<0.0) ? -s : s); + s = sqrt(2.0/vdot(u, u)); + u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s; + } + + /** Apply Householder reflection represented by u to column vectors of M **/ + void reflect_cols(_HMatrix M, double *u) + { + int i, j; + for (i=0; i<3; i++) { + double s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i]; + for (j=0; j<3; j++) M[j][i] -= u[j]*s; + } + } + + /** Apply Householder reflection represented by u to row vectors of M **/ + void reflect_rows(_HMatrix M, double *u) + { + int i, j; + for (i=0; i<3; i++) { + double s = vdot(u, M[i]); + for (j=0; j<3; j++) M[i][j] -= u[j]*s; + } + } + + /** Find orthogonal factor Q of rank 1 (or less) M **/ + void do_rank1(_HMatrix M, _HMatrix Q) + { + double v1[3], v2[3], s; + int col; + mat_copy(Q,=,mat_id,4); + /* If rank(M) is 1, we should find a non-zero column in M */ + col = find_max_col(M); + if (col<0) return; /* Rank is 0 */ + v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2]; + make_reflector(v2, v2); reflect_rows(M, v2); + s = M[2][2]; + if (s<0.0) Q[2][2] = -1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); + } + + + /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/ + void do_rank2(_HMatrix M, _HMatrix MadjT, _HMatrix Q) + { + double v1[3], v2[3]; + double w, x, y, z, c, s, d; + int col; + /* If rank(M) is 2, we should find a non-zero column in MadjT */ + col = find_max_col(MadjT); + if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */ + v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + vcross(M[0], M[1], v2); + make_reflector(v2, v2); reflect_rows(M, v2); + w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1]; + if (w*z>x*y) { + c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s); + } else { + c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s; + } + Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); + } + + + double mat_norm(_HMatrix M, int tpose) + { + int i; + double sum, max; + max = 0.0; + for (i=0; i<3; i++) { + if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]); + else sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]); + if (max= 0.0) + { + s = sqrt(tr + mat[W][W]); + qu.w = s*0.5; + s = 0.5 / s; + qu.x = (mat[Z][Y] - mat[Y][Z]) * s; + qu.y = (mat[X][Z] - mat[Z][X]) * s; + qu.z = (mat[Y][X] - mat[X][Y]) * s; + } + else + { + int h = X; + if (mat[Y][Y] > mat[X][X]) h = Y; + if (mat[Z][Z] > mat[h][h]) h = Z; + switch (h) { +#define caseMacro(i,j,k,I,J,K) \ + case I:\ + s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\ + qu.i = s*0.5;\ + s = 0.5 / s;\ + qu.j = (mat[I][J] + mat[J][I]) * s;\ + qu.k = (mat[K][I] + mat[I][K]) * s;\ + qu.w = (mat[K][J] - mat[J][K]) * s;\ + break + caseMacro(x,y,z,X,Y,Z); + caseMacro(y,z,x,Y,Z,X); + caseMacro(z,x,y,Z,X,Y); + } + } + if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W])); + return (qu); + } + + + /******* Decompose Affine Matrix *******/ + + /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the + * translation components, q contains the rotation R, u contains U, k contains + * scale factors, and f contains the sign of the determinant. + * Assumes A transforms column vectors in right-handed coordinates. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. + */ + void decompAffine(_HMatrix A, _affineParts * parts) + { + _HMatrix Q, S, U; + Quat p; + + //Translation component. + parts->t = osg::Vec4d(A[X][W], A[Y][W], A[Z][W], 0); + double det = polarDecomp(A, Q, S); + if (det<0.0) + { + matrixCopy(Q, =, -Q, 3); + parts->f = -1; + } + else + parts->f = 1; + + parts->q = quatFromMatrix(Q); + parts->k = spectDecomp(S, U); + parts->u = quatFromMatrix(U); + p = snuggle(parts->u, &parts->k); + parts->u = Qt_Mul(parts->u, p); + } + + + /******* Polar Decomposition *******/ + /* Polar Decomposition of 3x3 matrix in 4x4, + * M = QS. See Nicholas Higham and Robert S. Schreiber, + * Fast Polar Decomposition of An Arbitrary Matrix, + * Technical Report 88-942, October 1988, + * Department of Computer Science, Cornell University. + */ + + double polarDecomp( _HMatrix M, _HMatrix Q, _HMatrix S) + { + +#define TOL 1.0e-6 + _HMatrix Mk, MadjTk, Ek; + double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2; + int i, j; + + mat_tpose(Mk,=,M,3); + M_one = norm_one(Mk); M_inf = norm_inf(Mk); + + do + { + adjoint_transpose(Mk, MadjTk); + det = vdot(Mk[0], MadjTk[0]); + if (det==0.0) + { + do_rank2(Mk, MadjTk, Mk); + break; + } + + MadjT_one = norm_one(MadjTk); + MadjT_inf = norm_inf(MadjTk); + + gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)); + g1 = gamma*0.5; + g2 = 0.5/(gamma*det); + matrixCopy(Ek,=,Mk,3); + matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3); + mat_copy(Ek,-=,Mk,3); + E_one = norm_one(Ek); + M_one = norm_one(Mk); + M_inf = norm_inf(Mk); + + } while(E_one>(M_one*TOL)); + + mat_tpose(Q,=,Mk,3); mat_pad(Q); + mat_mult(Mk, M, S); mat_pad(S); + + for (i=0; i<3; i++) + for (j=i; j<3; j++) + S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]); + return (det); + } + + + /******* Spectral Decomposition *******/ + /* Compute the spectral decomposition of symmetric positive semi-definite S. + * Returns rotation in U and scale factors in result, so that if K is a diagonal + * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method. + * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983. + */ + HVect spectDecomp(_HMatrix S, _HMatrix U) + { + HVect kv; + double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */ + double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b; + static char nxt[] = {Y,Z,X}; + int sweep, i, j; + mat_copy(U,=,mat_id,4); + Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z]; + OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y]; + for (sweep=20; sweep>0; sweep--) { + double sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]); + if (sm==0.0) break; + for (i=Z; i>=X; i--) { + int p = nxt[i]; int q = nxt[p]; + fabsOffDi = fabs(OffD[i]); + g = 100.0*fabsOffDi; + if (fabsOffDi>0.0) { + h = Diag[q] - Diag[p]; + fabsh = fabs(h); + if (fabsh+g==fabsh) { + t = OffD[i]/h; + } else { + theta = 0.5*h/OffD[i]; + t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0)); + if (theta<0.0) t = -t; + } + c = 1.0/sqrt(t*t+1.0); s = t*c; + tau = s/(c+1.0); + ta = t*OffD[i]; OffD[i] = 0.0; + Diag[p] -= ta; Diag[q] += ta; + OffDq = OffD[q]; + OffD[q] -= s*(OffD[p] + tau*OffD[q]); + OffD[p] += s*(OffDq - tau*OffD[p]); + for (j=Z; j>=X; j--) { + a = U[j][p]; b = U[j][q]; + U[j][p] -= s*(b + tau*a); + U[j][q] += s*(a - tau*b); + } + } + } + } + kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0; + return (kv); + } + + + /******* Spectral Axis Adjustment *******/ + +#define SQRTHALF (0.7071067811865475244) + static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF}; + static Quat qytoz = {SQRTHALF,0,0,SQRTHALF}; + static Quat qppmm = { 0.5, 0.5,-0.5,-0.5}; + static Quat qpppp = { 0.5, 0.5, 0.5, 0.5}; + static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5}; + static Quat qpppm = { 0.5, 0.5, 0.5,-0.5}; + static Quat q0001 = { 0.0, 0.0, 0.0, 1.0}; + static Quat q1000 = { 1.0, 0.0, 0.0, 0.0}; + + /* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p, + * which permutes the axes and turns freely in the plane of duplicate scale + * factors, such that q p has the largest possible w component, i.e. the + * smallest possible angle. Permutes k's components to go with q p instead of q. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. Details on p. 262-263. + */ + Quat snuggle(Quat q, HVect *k) + { +#define sgn(n,v) ((n)?-(v):(v)) +#define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];} +#define cycle(a,p) if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\ + else {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];} + + Quat p; + double ka[4]; + int i, turn = -1; + ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z; + + if (ka[X]==ka[Y]) { + if (ka[X]==ka[Z]) + turn = W; + else turn = Z; + } + else { + if (ka[X]==ka[Z]) + turn = Y; + else if (ka[Y]==ka[Z]) + turn = X; + } + if (turn>=0) { + Quat qtoz, qp; + unsigned neg[3], win; + double mag[3], t; + switch (turn) { + default: return (Qt_Conj(q)); + case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break; + case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break; + case Z: qtoz = q0001; break; + } + q = Qt_Conj(q); + mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5; + mag[1] = (double)q.x*q.z-(double)q.y*q.w; + mag[2] = (double)q.y*q.z+(double)q.x*q.w; + + for (i=0; i<3; i++) + //JVK?????? + //if (neg[i] = (mag[i]<0.0)) + if (neg[i] == (mag[i]<0.0)) + mag[i] = -mag[i]; + + if (mag[0]>mag[1]) { + if (mag[0]>mag[2]) + win = 0; + else win = 2; + } + else { + if (mag[1]>mag[2]) win = 1; + else win = 2; + } + + switch (win) { + case 0: if (neg[0]) p = q1000; else p = q0001; break; + case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break; + case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break; + } + + qp = Qt_Mul(q, p); + t = sqrt(mag[win]+0.5); + p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t)); + p = Qt_Mul(qtoz, Qt_Conj(p)); + } + else { + double qa[4], pa[4]; + unsigned lo, hi, neg[4], par = 0; + double all, big, two; + qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w; + for (i=0; i<4; i++) { + pa[i] = 0.0; + //if (neg[i] = (qa[i]<0.0)) qa[i] = -qa[i]; + if (neg[i] == (qa[i]<0.0)) qa[i] = -qa[i]; + par ^= neg[i]; + } + + /* Find two largest components, indices in hi and lo */ + if (qa[0]>qa[1]) lo = 0; + else lo = 1; + + if (qa[2]>qa[3]) hi = 2; + else hi = 3; + + if (qa[lo]>qa[hi]) { + if (qa[lo^1]>qa[hi]) { + hi = lo; lo ^= 1; + } + else { + hi ^= lo; lo ^= hi; hi ^= lo; + } + } + else { + if (qa[hi^1]>qa[lo]) lo = hi^1; + } + + all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5; + two = (qa[hi]+qa[lo])*SQRTHALF; + big = qa[hi]; + if (all>two) { + if (all>big) {/*all*/ + {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);} + cycle(ka,par); + } + else {/*big*/ pa[hi] = sgn(neg[hi],1.0);} + } else { + if (two>big) { /*two*/ + pa[hi] = sgn(neg[hi],SQRTHALF); + pa[lo] = sgn(neg[lo], SQRTHALF); + if (lo>hi) { + hi ^= lo; lo ^= hi; hi ^= lo; + } + if (hi==W) { + hi = "\001\002\000"[lo]; + lo = 3-hi-lo; + } + swap(ka,hi,lo); + } + else {/*big*/ + pa[hi] = sgn(neg[hi],1.0); + } + } + p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3]; + } + k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z]; + return (p); + } + +} + +void osg::Matrixf::decompose(osg::Vec3f& t, + osg::Quat& r, + osg::Vec3f& s, + osg::Quat& so) const +{ + Vec3d temp_trans; + Vec3d temp_scale; + decompose(temp_trans, r, temp_scale, so); + t = temp_trans; + s = temp_scale; +} + + +void osg::Matrixf::decompose(osg::Vec3d& t, + osg::Quat& r, + osg::Vec3d& s, + osg::Quat& so) const +{ + MatrixDecomposition::_affineParts parts; + MatrixDecomposition::_HMatrix hmatrix; + + // Transpose copy of LTW + for ( int i =0; i<4; i++) + { + for ( int j=0; j<4; j++) + { + hmatrix[i][j] = (*this)(j,i); + } + } + + MatrixDecomposition::decompAffine(hmatrix, &parts); + + double mul = 1.0; + if (parts.t[MatrixDecomposition::W] != 0.0) + mul = 1.0 / parts.t[MatrixDecomposition::W]; + + t[0] = parts.t[MatrixDecomposition::X] * mul; + t[1] = parts.t[MatrixDecomposition::Y] * mul; + t[2] = parts.t[MatrixDecomposition::Z] * mul; + + r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w); + + mul = 1.0; + if (parts.k.w != 0.0) + mul = 1.0 / parts.k.w; + + // mul be sign of determinant to support negative scales. + mul *= parts.f; + s[0] = parts.k.x * mul; + s[1] = parts.k.y * mul; + s[2] = parts.k.z * mul; + + so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w); +} + +void osg::Matrixd::decompose(osg::Vec3f& t, + osg::Quat& r, + osg::Vec3f& s, + osg::Quat& so) const +{ + Vec3d temp_trans; + Vec3d temp_scale; + decompose(temp_trans, r, temp_scale, so); + t = temp_trans; + s = temp_scale; +} + +void osg::Matrixd::decompose(osg::Vec3d& t, + osg::Quat& r, + osg::Vec3d& s, + osg::Quat& so) const +{ + MatrixDecomposition::_affineParts parts; + MatrixDecomposition::_HMatrix hmatrix; + + // Transpose copy of LTW + for ( int i =0; i<4; i++) + { + for ( int j=0; j<4; j++) + { + hmatrix[i][j] = (*this)(j,i); + } + } + + MatrixDecomposition::decompAffine(hmatrix, &parts); + + double mul = 1.0; + if (parts.t[MatrixDecomposition::W] != 0.0) + mul = 1.0 / parts.t[MatrixDecomposition::W]; + + t[0] = parts.t[MatrixDecomposition::X] * mul; + t[1] = parts.t[MatrixDecomposition::Y] * mul; + t[2] = parts.t[MatrixDecomposition::Z] * mul; + + r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w); + + mul = 1.0; + if (parts.k.w != 0.0) + mul = 1.0 / parts.k.w; + + // mul be sign of determinant to support negative scales. + mul *= parts.f; + s[0] = parts.k.x * mul; + s[1] = parts.k.y * mul; + s[2] = parts.k.z * mul; + + so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w); +}