Hi Paul, I have just modified the osgunittests example to add the guts of your TestDecompose.cpp into the matrix testing section, so if you run:
osgunittests matrix It'll run your test and you'll get: Test - Matrix::decompos(), input rotation : 0.176777 0.306186 0.176777 0.918559 Matrixf::decomposef Translation : 0.000000 0.000000 0.000000 Rotation : 0.176777 0.306186 0.176777 0.918559 Scale : 1.000000 1.000000 1.000000 Scale Orientation: -0.087923 0.212264 -0.372446 0.899165 Matrixd::decompose Translation : 0.000000 0.000000 0.000000 Rotation : 0.176777 0.306186 0.176777 0.918559 Scale : 1.000000 1.000000 1.000000 Scale Orientation: 0.000000 -0.113641 -0.106971 0.987746 I tweaked the input values so that it converted the 30 values from degrees to radians as the OSG maths classes all use radians rather than degrees. I presume this is what you intended by your original code. The values reported all look pretty reasonable - the input and and output rotations, scales and translation are all correct. Then only item that one might debate as something that could do with improving upon is the scale orientation, but a scale of 1,1,1 itself is not a scale, so there isn't really an axis, so the orientation of no scale is really undefined, the above values are just as valid as any other value. Making these undefined value produces a consistent result is really just beatification. Added the clamping to zero in the MatrixDecomposition.cpp is not something I'm entirely comfortable with as it will almost certainly increase the errors in some valid combinations. Robert. On Mon, Jun 1, 2009 at 7:09 PM, p...@tcl3d<[email protected]> wrote: > Hello Robert, > > The attached test program TestDecompose.cpp decomposes both a Matrixf and a > Matrixd containing the same rotation matrix. > With version 2.8.1 and current trunk it produces the following output: > > Decompose Matrixf > Translation : 0.000000 0.000000 0.000000 > Rotation : 0.696550 0.054045 0.696550 -0.163445 > Scale : 1.000000 1.000000 1.000000 > Scale Orientation: -0.083980 -0.226097 -0.020295 0.970266 > Decompose Matrixd > Translation : 0.000000 0.000000 0.000000 > Rotation : 0.696550 0.054045 0.696550 -0.163445 > Scale : 1.000000 1.000000 1.000000 > Scale Orientation: -0.058612 0.229170 -0.000000 0.971620 > > The quat returned for "Scale Orientation" shows different values depending > on the type of the supplied Matrix. > This effect is reproducable on Windows and Linux. > > This is in my opinion due to a numerically instable algorithmn used in file > MatrixDecomposition.cpp in function polarDecomp. > > If applying the workaround as implemented in the attached > MatrixDecomposition.cpp I get the following consistent, and more reasonable > output: > > Decompose Matrixf > Translation : 0.000000 0.000000 0.000000 > Rotation : 0.696550 0.054045 0.696550 -0.163445 > Scale : 1.000000 1.000000 1.000000 > Scale Orientation: 0.000000 0.000000 0.000000 1.000000 > Decompose Matrixd > Translation : 0.000000 0.000000 0.000000 > Rotation : 0.696550 0.054045 0.696550 -0.163445 > Scale : 1.000000 1.000000 1.000000 > Scale Orientation: 0.000000 0.000000 0.000000 1.000000 > > I did not take an in-depth view into the implementation of the algorithms > used, so there may be a better, cleaner way to solve the inconsistencies. > > Regards, > > Paul Obermeier > > /* -*-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 <[email protected]> > > #include <osg/Matrixf> > #include <osg/Matrixd> > > #include <cfloat> > > /** Copy nxn matrix A to C using "gets" for assignment **/ > #define matrixCopy(C, gets, A, n) {int i, j; for (i=0;i<n;i++) for > (j=0;j<n;j++)\ > C[i][j] gets (A[i][j]);} > > /** Copy transpose of nxn matrix A to C using "gets" for assignment **/ > #define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\ > AT[i][j] gets (A[j][i]);} > > /** Fill out 3x3 matrix to 4x4 **/ > #define mat_pad(A) > (A[W][X]=A[X][W]=A[W][Y]=A[Y][W]=A[W][Z]=A[Z][W]=0,A[W][W]=1) > > > /** Assign nxn matrix C the element-wise combination of A and B using "op" > **/ > #define matBinop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) > for(j=0;j<n;j++)\ > C[i][j] gets (A[i][j]) op (B[i][j]);} > > /** Copy nxn matrix A to C using "gets" for assignment **/ > #define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\ > C[i][j] gets (A[i][j]);} > > namespace MatrixDecomposition > { > > typedef struct {double x, y, z, w;} Quat; // Quaternion > enum QuatPart {X, Y, Z, W}; > typedef double _HMatrix[4][4]; > typedef Quat HVect; // Homogeneous 3D vector > typedef struct > { > osg::Vec4d t; // Translation Component; > Quat q; // Essential Rotation > Quat u; //Stretch rotation > HVect k; //Sign of determinant > double f; // Sign of determinant > } _affineParts; > > HVect spectDecomp(_HMatrix S, _HMatrix U); > Quat snuggle(Quat q, HVect* k); > double polarDecomp(_HMatrix M, _HMatrix Q, _HMatrix S); > > static _HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; > > #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}; > > > /* Return product of quaternion q by scalar w. */ > Quat Qt_Scale(Quat q, double w) > { > Quat qq; > qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w; > return (qq); > } > > /* Return quaternion product qL * qR. Note: order is important! > * To combine rotations, use the product Mul(qSecond, qFirst), > * which gives the effect of rotating by qFirst then qSecond. */ > Quat Qt_Mul(Quat qL, Quat qR) > { > Quat qq; > qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z; > qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y; > qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z; > qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x; > return (qq); > } > > /* Return conjugate of quaternion. */ > Quat Qt_Conj(Quat q) > { > Quat qq; > qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w; > return (qq); > } > > /* Construct a (possibly non-unit) quaternion from real components. */ > Quat Qt_(double x, double y, double z, double w) > { > Quat qq; > qq.x = x; qq.y = y; qq.z = z; qq.w = w; > return (qq); > } > > /** Multiply the upper left 3x3 parts of A and B to get AB **/ > void mat_mult(_HMatrix A, _HMatrix B, _HMatrix AB) > { > int i, j; > for (i=0; i<3; i++) for (j=0; j<3; j++) > AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j]; > } > > /** Set v to cross product of length 3 vectors va and vb **/ > void vcross(double *va, double *vb, double *v) > { > v[0] = va[1]*vb[2] - va[2]*vb[1]; > v[1] = va[2]*vb[0] - va[0]*vb[2]; > v[2] = va[0]*vb[1] - va[1]*vb[0]; > } > > /** Return dot product of length 3 vectors va and vb **/ > double vdot(double *va, double *vb) > { > return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]); > } > > > /** Set MadjT to transpose of inverse of M times determinant of M **/ > void adjoint_transpose(_HMatrix M, _HMatrix MadjT) > { > vcross(M[1], M[2], MadjT[0]); > vcross(M[2], M[0], MadjT[1]); > vcross(M[0], M[1], MadjT[2]); > } > > /** Return index of column of M containing maximum abs entry, or -1 if > M=0 **/ > int find_max_col(_HMatrix M) > { > double abs, max; > int i, j, col; > max = 0.0; col = -1; > for (i=0; i<3; i++) for (j=0; j<3; j++) { > abs = M[i][j]; if (abs<0.0) abs = -abs; > if (abs>max) {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<sum) max = sum; > } > return max; > } > > double norm_inf(_HMatrix M) {return mat_norm(M, 0);} > double norm_one(_HMatrix M) {return mat_norm(M, 1);} > > /* Construct a unit quaternion from rotation matrix. Assumes matrix is > * used to multiply column vector on the left: vnew = mat vold. Works > * correctly for right-handed coordinate system and right-handed > rotations. > * Translation and perspective components ignored. */ > > Quat quatFromMatrix(_HMatrix mat) > { > /* This algorithm avoids near-zero divides by looking for a large > component > * - first w, then x, y, or z. When the trace is greater than zero, > * |w| is greater than 1/2, which is as small as a largest component > can be. > * Otherwise, the largest diagonal entry corresponds to the largest > of |x|, > * |y|, or |z|, one of which must be larger than |w|, and at least > 1/2. */ > Quat qu = q0001; > double tr, s; > > tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z]; > if (tr >= 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]); > > // Paul Obermeier: Workaround for numerical instability. > #ifndef FLT_EPSILON > #define FLT_EPSILON 1.192092896e-07 > #endif > for (i=0; i<3; i++) > for (j=0; j<3; j++) > if (fabs (S[i][j]) < FLT_EPSILON) > S[i][j] = 0.0; > > 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 *******/ > > /* 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 = q0001; > 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 int 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; > > bool neg[3]; > for (i=0; i<3; i++) > { > neg[i] = (mag[i]<0.0); > if (neg[i]) 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 int lo, hi; > bool par = false; > bool neg[4]; > 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; > neg[i] = (qa[i]<0.0); > if (neg[i]) 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); > } > > #include <cstdio> > > #include <osg/Matrixf> > #include <osg/Matrixd> > > int main (int argc, char *argv[]) { > double angx = 30.0; > double angy = 30.0; > double angz = 30.0; > > osg::Matrixf matf; > matf.makeRotate (angx, osg::Vec3f (1, 0, 0), > angy, osg::Vec3f (0, 1, 0), > angz, osg::Vec3f (0, 0, 1)); > > osg::Vec3f transf; > osg::Quat rotf; > osg::Vec3f sclf; > osg::Quat sof; > matf.decompose (transf, rotf, sclf, sof); > printf ("Decompose Matrixf\n"); > printf ("Translation : %f %f %f\n", transf.x(), transf.y(), > transf.z()); > printf ("Rotation : %f %f %f %f\n", rotf._v[0], rotf._v[1], > rotf._v[2], rotf._v[3]); > printf ("Scale : %f %f %f\n", sclf.x(), sclf.y(), sclf.z()); > printf ("Scale Orientation: %f %f %f %f\n", sof._v[0], sof._v[1], > sof._v[2], sof._v[3]); > > osg::Matrixd matd; > matd.makeRotate (angx, osg::Vec3f (1, 0, 0), > angy, osg::Vec3f (0, 1, 0), > angz, osg::Vec3f (0, 0, 1)); > > osg::Vec3f transd; > osg::Quat rotd; > osg::Vec3f scld; > osg::Quat sod; > matd.decompose (transd, rotd, scld, sod); > printf ("Decompose Matrixd\n"); > printf ("Translation : %f %f %f\n", transd.x(), transd.y(), > transd.z()); > printf ("Rotation : %f %f %f %f\n", rotd._v[0], rotd._v[1], > rotd._v[2], rotd._v[3]); > printf ("Scale : %f %f %f\n", scld.x(), scld.y(), scld.z()); > printf ("Scale Orientation: %f %f %f %f\n", sod._v[0], sod._v[1], > sod._v[2], sod._v[3]); > > return 0; > } > > _______________________________________________ > osg-submissions mailing list > [email protected] > http://lists.openscenegraph.org/listinfo.cgi/osg-submissions-openscenegraph.org > > _______________________________________________ osg-submissions mailing list [email protected] http://lists.openscenegraph.org/listinfo.cgi/osg-submissions-openscenegraph.org
