root/OpenSceneGraph/trunk/src/osg/MatrixDecomposition.cpp @ 13041

Revision 13041, 22.5 kB (checked in by robert, 14 months ago)

Ran script to remove trailing spaces and tabs

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 * OpenSceneGraph Public License for more details.
12*/
13//osgManipulator - Copyright (C) 2007 Fugro-Jason B.V.
14
15// Matrix decomposition code taken from Graphics Gems IV
16// http://www.acm.org/pubs/tog/GraphicsGems/gemsiv/polar_decomp
17// Copyright (C) 1993 Ken Shoemake <shoemake@graphics.cis.upenn.edu>
18
19#include <osg/Matrixf>
20#include <osg/Matrixd>
21
22/** Copy nxn matrix A to C using "gets" for assignment **/
23#define matrixCopy(C, gets, A, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\
24    C[i][j] gets (A[i][j]);}
25
26/** Copy transpose of nxn matrix A to C using "gets" for assignment **/
27#define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
28    AT[i][j] gets (A[j][i]);}
29
30/** Fill out 3x3 matrix to 4x4 **/
31#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)
32
33
34/** Assign nxn matrix C the element-wise combination of A and B using "op" **/
35#define matBinop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
36    C[i][j] gets (A[i][j]) op (B[i][j]);}
37
38/** Copy nxn matrix A to C using "gets" for assignment **/
39#define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
40    C[i][j] gets (A[i][j]);}
41
42namespace MatrixDecomposition
43{
44
45    typedef struct {double x, y, z, w;} Quat; // Quaternion
46    enum QuatPart {X, Y, Z, W};
47    typedef double _HMatrix[4][4];
48    typedef Quat HVect;   // Homogeneous 3D vector
49    typedef struct
50    {
51        osg::Vec4d t;     // Translation Component;
52        Quat q;           // Essential Rotation
53        Quat  u;          //Stretch rotation
54        HVect k;          //Sign of determinant
55        double f;          // Sign of determinant
56    } _affineParts;
57
58    HVect spectDecomp(_HMatrix S, _HMatrix U);
59    Quat snuggle(Quat q, HVect* k);
60    double polarDecomp(_HMatrix M, _HMatrix Q, _HMatrix S);
61
62    static _HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
63
64    #define SQRTHALF (0.7071067811865475244)
65    static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF};
66    static Quat qytoz = {SQRTHALF,0,0,SQRTHALF};
67    static Quat qppmm = { 0.5, 0.5,-0.5,-0.5};
68    static Quat qpppp = { 0.5, 0.5, 0.5, 0.5};
69    static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5};
70    static Quat qpppm = { 0.5, 0.5, 0.5,-0.5};
71    static Quat q0001 = { 0.0, 0.0, 0.0, 1.0};
72    static Quat q1000 = { 1.0, 0.0, 0.0, 0.0};
73
74
75    /* Return product of quaternion q by scalar w. */
76    Quat Qt_Scale(Quat q, double w)
77    {
78        Quat qq;
79        qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
80        return (qq);
81    }
82
83    /* Return quaternion product qL * qR.  Note: order is important!
84     * To combine rotations, use the product Mul(qSecond, qFirst),
85     * which gives the effect of rotating by qFirst then qSecond. */
86    Quat Qt_Mul(Quat qL, Quat qR)
87    {
88        Quat qq;
89        qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
90        qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
91        qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
92        qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
93        return (qq);
94    }
95
96    /* Return conjugate of quaternion. */
97    Quat Qt_Conj(Quat q)
98    {
99        Quat qq;
100        qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
101        return (qq);
102    }
103
104    /* Construct a (possibly non-unit) quaternion from real components. */
105    Quat Qt_(double x, double y, double z, double w)
106    {
107        Quat qq;
108        qq.x = x; qq.y = y; qq.z = z; qq.w = w;
109        return (qq);
110    }
111
112    /** Multiply the upper left 3x3 parts of A and B to get AB **/
113    void mat_mult(_HMatrix A, _HMatrix B, _HMatrix AB)
114    {
115        int i, j;
116        for (i=0; i<3; i++) for (j=0; j<3; j++)
117            AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
118    }
119
120    /** Set v to cross product of length 3 vectors va and vb **/
121    void vcross(double *va, double *vb, double *v)
122    {
123        v[0] = va[1]*vb[2] - va[2]*vb[1];
124        v[1] = va[2]*vb[0] - va[0]*vb[2];
125        v[2] = va[0]*vb[1] - va[1]*vb[0];
126    }
127
128    /** Return dot product of length 3 vectors va and vb **/
129    double vdot(double *va, double *vb)
130    {
131        return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
132    }
133
134
135    /** Set MadjT to transpose of inverse of M times determinant of M **/
136    void adjoint_transpose(_HMatrix M, _HMatrix MadjT)
137    {
138        vcross(M[1], M[2], MadjT[0]);
139        vcross(M[2], M[0], MadjT[1]);
140        vcross(M[0], M[1], MadjT[2]);
141    }
142
143    /** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
144    int find_max_col(_HMatrix M)
145    {
146        double abs, max;
147        int i, j, col;
148        max = 0.0; col = -1;
149        for (i=0; i<3; i++) for (j=0; j<3; j++) {
150            abs = M[i][j]; if (abs<0.0) abs = -abs;
151            if (abs>max) {max = abs; col = j;}
152        }
153        return col;
154    }
155
156    /** Setup u for Household reflection to zero all v components but first **/
157    void make_reflector(double *v, double *u)
158    {
159        double s = sqrt(vdot(v, v));
160        u[0] = v[0]; u[1] = v[1];
161        u[2] = v[2] + ((v[2]<0.0) ? -s : s);
162        s = sqrt(2.0/vdot(u, u));
163        u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
164    }
165
166    /** Apply Householder reflection represented by u to column vectors of M **/
167    void reflect_cols(_HMatrix M, double *u)
168    {
169        int i, j;
170        for (i=0; i<3; i++) {
171            double s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
172            for (j=0; j<3; j++) M[j][i] -= u[j]*s;
173        }
174    }
175
176    /** Apply Householder reflection represented by u to row vectors of M **/
177    void reflect_rows(_HMatrix M, double *u)
178    {
179        int i, j;
180        for (i=0; i<3; i++) {
181            double s = vdot(u, M[i]);
182            for (j=0; j<3; j++) M[i][j] -= u[j]*s;
183        }
184    }
185
186    /** Find orthogonal factor Q of rank 1 (or less) M **/
187    void do_rank1(_HMatrix M, _HMatrix Q)
188    {
189        double v1[3], v2[3], s;
190        int col;
191        mat_copy(Q,=,mat_id,4);
192        /* If rank(M) is 1, we should find a non-zero column in M */
193        col = find_max_col(M);
194        if (col<0) return; /* Rank is 0 */
195        v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
196        make_reflector(v1, v1); reflect_cols(M, v1);
197        v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
198        make_reflector(v2, v2); reflect_rows(M, v2);
199        s = M[2][2];
200        if (s<0.0) Q[2][2] = -1.0;
201        reflect_cols(Q, v1); reflect_rows(Q, v2);
202    }
203
204
205    /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
206    void do_rank2(_HMatrix M, _HMatrix MadjT, _HMatrix Q)
207    {
208        double v1[3], v2[3];
209        double w, x, y, z, c, s, d;
210        int col;
211        /* If rank(M) is 2, we should find a non-zero column in MadjT */
212        col = find_max_col(MadjT);
213        if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
214        v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
215        make_reflector(v1, v1); reflect_cols(M, v1);
216        vcross(M[0], M[1], v2);
217        make_reflector(v2, v2); reflect_rows(M, v2);
218        w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
219        if (w*z>x*y) {
220            c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
221            Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
222        } else {
223            c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
224            Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
225        }
226        Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
227        reflect_cols(Q, v1); reflect_rows(Q, v2);
228    }
229
230
231    double mat_norm(_HMatrix M, int tpose)
232    {
233        int i;
234        double sum, max;
235        max = 0.0;
236        for (i=0; i<3; i++) {
237            if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
238            else       sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
239            if (max<sum) max = sum;
240        }
241        return max;
242    }
243
244    double norm_inf(_HMatrix M) {return mat_norm(M, 0);}
245    double norm_one(_HMatrix M) {return mat_norm(M, 1);}
246
247    /* Construct a unit quaternion from rotation matrix.  Assumes matrix is
248     * used to multiply column vector on the left: vnew = mat vold.  Works
249     * correctly for right-handed coordinate system and right-handed rotations.
250     * Translation and perspective components ignored. */
251
252    Quat quatFromMatrix(_HMatrix mat)
253    {
254        /* This algorithm avoids near-zero divides by looking for a large component
255         * - first w, then x, y, or z.  When the trace is greater than zero,
256         * |w| is greater than 1/2, which is as small as a largest component can be.
257         * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
258         * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
259        Quat qu = q0001;
260        double tr, s;
261
262        tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
263        if (tr >= 0.0)
264        {
265            s = sqrt(tr + mat[W][W]);
266            qu.w = s*0.5;
267            s = 0.5 / s;
268            qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
269            qu.y = (mat[X][Z] - mat[Z][X]) * s;
270            qu.z = (mat[Y][X] - mat[X][Y]) * s;
271        }
272        else
273        {
274            int h = X;
275            if (mat[Y][Y] > mat[X][X]) h = Y;
276            if (mat[Z][Z] > mat[h][h]) h = Z;
277            switch (h) {
278#define caseMacro(i,j,k,I,J,K) \
279                case I:\
280                       s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
281                qu.i = s*0.5;\
282                s = 0.5 / s;\
283                qu.j = (mat[I][J] + mat[J][I]) * s;\
284                qu.k = (mat[K][I] + mat[I][K]) * s;\
285                qu.w = (mat[K][J] - mat[J][K]) * s;\
286                break
287                caseMacro(x,y,z,X,Y,Z);
288                caseMacro(y,z,x,Y,Z,X);
289                caseMacro(z,x,y,Z,X,Y);
290            }
291        }
292        if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
293        return (qu);
294    }
295
296
297    /******* Decompose Affine Matrix *******/
298
299    /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
300     * translation components, q contains the rotation R, u contains U, k contains
301     * scale factors, and f contains the sign of the determinant.
302     * Assumes A transforms column vectors in right-handed coordinates.
303     * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
304     * Proceedings of Graphics Interface 1992.
305     */
306    void decompAffine(_HMatrix A, _affineParts * parts)
307        {
308            _HMatrix Q, S, U;
309            Quat p;
310
311            //Translation component.
312            parts->t = osg::Vec4d(A[X][W], A[Y][W], A[Z][W], 0);
313            double det = polarDecomp(A, Q, S);
314            if (det<0.0)
315            {
316                matrixCopy(Q, =, -Q, 3);
317                parts->f = -1;
318            }
319            else
320                parts->f = 1;
321
322            parts->q = quatFromMatrix(Q);
323            parts->k = spectDecomp(S, U);
324            parts->u = quatFromMatrix(U);
325            p = snuggle(parts->u, &parts->k);
326            parts->u = Qt_Mul(parts->u, p);
327        }
328
329
330    /******* Polar Decomposition *******/
331    /* Polar Decomposition of 3x3 matrix in 4x4,
332     * M = QS.  See Nicholas Higham and Robert S. Schreiber,
333     * Fast Polar Decomposition of An Arbitrary Matrix,
334     * Technical Report 88-942, October 1988,
335     * Department of Computer Science, Cornell University.
336     */
337
338    double polarDecomp( _HMatrix M, _HMatrix Q, _HMatrix S)
339    {
340
341#define TOL 1.0e-6
342        _HMatrix Mk, MadjTk, Ek;
343        double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
344        int i, j;
345
346        mat_tpose(Mk,=,M,3);
347        M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
348
349        do
350        {
351            adjoint_transpose(Mk, MadjTk);
352            det = vdot(Mk[0], MadjTk[0]);
353            if (det==0.0)
354            {
355                do_rank2(Mk, MadjTk, Mk);
356                break;
357            }
358
359            MadjT_one = norm_one(MadjTk);
360            MadjT_inf = norm_inf(MadjTk);
361
362            gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
363            g1 = gamma*0.5;
364            g2 = 0.5/(gamma*det);
365            matrixCopy(Ek,=,Mk,3);
366            matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3);
367            mat_copy(Ek,-=,Mk,3);
368            E_one = norm_one(Ek);
369            M_one = norm_one(Mk);
370            M_inf = norm_inf(Mk);
371
372        } while(E_one>(M_one*TOL));
373
374        mat_tpose(Q,=,Mk,3); mat_pad(Q);
375        mat_mult(Mk, M, S);  mat_pad(S);
376
377        for (i=0; i<3; i++)
378            for (j=i; j<3; j++)
379                S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
380        return (det);
381    }
382
383
384    /******* Spectral Decomposition *******/
385    /* Compute the spectral decomposition of symmetric positive semi-definite S.
386     * Returns rotation in U and scale factors in result, so that if K is a diagonal
387     * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
388     * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
389     */
390    HVect spectDecomp(_HMatrix S, _HMatrix U)
391    {
392        HVect kv;
393        double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */
394        double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b;
395        static char nxt[] = {Y,Z,X};
396        int sweep, i, j;
397        mat_copy(U,=,mat_id,4);
398        Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z];
399        OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y];
400        for (sweep=20; sweep>0; sweep--) {
401            double sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]);
402            if (sm==0.0) break;
403            for (i=Z; i>=X; i--) {
404                int p = nxt[i]; int q = nxt[p];
405                fabsOffDi = fabs(OffD[i]);
406                g = 100.0*fabsOffDi;
407                if (fabsOffDi>0.0) {
408                    h = Diag[q] - Diag[p];
409                    fabsh = fabs(h);
410                    if (fabsh+g==fabsh) {
411                        t = OffD[i]/h;
412                    } else {
413                        theta = 0.5*h/OffD[i];
414                        t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0));
415                        if (theta<0.0) t = -t;
416                    }
417                    c = 1.0/sqrt(t*t+1.0); s = t*c;
418                    tau = s/(c+1.0);
419                    ta = t*OffD[i]; OffD[i] = 0.0;
420                    Diag[p] -= ta; Diag[q] += ta;
421                    OffDq = OffD[q];
422                    OffD[q] -= s*(OffD[p] + tau*OffD[q]);
423                    OffD[p] += s*(OffDq   - tau*OffD[p]);
424                    for (j=Z; j>=X; j--) {
425                        a = U[j][p]; b = U[j][q];
426                        U[j][p] -= s*(b + tau*a);
427                        U[j][q] += s*(a - tau*b);
428                    }
429                }
430            }
431        }
432        kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0;
433        return (kv);
434    }
435
436
437    /******* Spectral Axis Adjustment *******/
438
439    /* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
440     * which permutes the axes and turns freely in the plane of duplicate scale
441     * factors, such that q p has the largest possible w component, i.e. the
442     * smallest possible angle. Permutes k's components to go with q p instead of q.
443     * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
444     * Proceedings of Graphics Interface 1992. Details on p. 262-263.
445     */
446    Quat snuggle(Quat q, HVect *k)
447    {
448#define sgn(n,v)    ((n)?-(v):(v))
449#define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
450#define cycle(a,p)  if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
451        else   {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
452
453        Quat p = q0001;
454        double ka[4];
455        int i, turn = -1;
456        ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
457
458        if (ka[X]==ka[Y]) {
459            if (ka[X]==ka[Z])
460                turn = W;
461            else turn = Z;
462        }
463        else {
464            if (ka[X]==ka[Z])
465                turn = Y;
466            else if (ka[Y]==ka[Z])
467                turn = X;
468        }
469        if (turn>=0) {
470            Quat qtoz, qp;
471            unsigned int  win;
472            double mag[3], t;
473            switch (turn) {
474                default: return (Qt_Conj(q));
475                case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
476                case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
477                case Z: qtoz = q0001; break;
478            }
479            q = Qt_Conj(q);
480            mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5;
481            mag[1] = (double)q.x*q.z-(double)q.y*q.w;
482            mag[2] = (double)q.y*q.z+(double)q.x*q.w;
483
484            bool neg[3];
485            for (i=0; i<3; i++)
486            {
487                neg[i] = (mag[i]<0.0);
488                if (neg[i]) mag[i] = -mag[i];
489            }
490
491            if (mag[0]>mag[1]) {
492                if (mag[0]>mag[2])
493                    win = 0;
494                else win = 2;
495            }
496            else {
497                if (mag[1]>mag[2]) win = 1;
498                else win = 2;
499            }
500
501            switch (win) {
502                case 0: if (neg[0]) p = q1000; else p = q0001; break;
503                case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
504                case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
505            }
506
507            qp = Qt_Mul(q, p);
508            t = sqrt(mag[win]+0.5);
509            p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t));
510            p = Qt_Mul(qtoz, Qt_Conj(p));
511        }
512        else {
513            double qa[4], pa[4];
514            unsigned int lo, hi;
515            bool par = false;
516            bool neg[4];
517            double all, big, two;
518            qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w;
519            for (i=0; i<4; i++) {
520                pa[i] = 0.0;
521                neg[i] = (qa[i]<0.0);
522                if (neg[i]) qa[i] = -qa[i];
523                par ^= neg[i];
524            }
525
526            /* Find two largest components, indices in hi and lo */
527            if (qa[0]>qa[1]) lo = 0;
528            else lo = 1;
529
530            if (qa[2]>qa[3]) hi = 2;
531            else hi = 3;
532
533            if (qa[lo]>qa[hi]) {
534                if (qa[lo^1]>qa[hi]) {
535                    hi = lo; lo ^= 1;
536                }
537                else {
538                    hi ^= lo; lo ^= hi; hi ^= lo;
539                }
540            }
541            else {
542                if (qa[hi^1]>qa[lo]) lo = hi^1;
543            }
544
545            all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
546            two = (qa[hi]+qa[lo])*SQRTHALF;
547            big = qa[hi];
548            if (all>two) {
549                if (all>big) {/*all*/
550                    {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);}
551                    cycle(ka,par);
552                }
553                else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
554            } else {
555                if (two>big) { /*two*/
556                    pa[hi] = sgn(neg[hi],SQRTHALF);
557                    pa[lo] = sgn(neg[lo], SQRTHALF);
558                    if (lo>hi) {
559                        hi ^= lo; lo ^= hi; hi ^= lo;
560                    }
561                    if (hi==W) {
562                        hi = "\001\002\000"[lo];
563                        lo = 3-hi-lo;
564                    }
565                    swap(ka,hi,lo);
566                }
567                else {/*big*/
568                    pa[hi] = sgn(neg[hi],1.0);
569                }
570            }
571            p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3];
572        }
573        k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z];
574        return (p);
575    }
576
577}
578
579void osg::Matrixf::decompose(osg::Vec3f& t,
580                             osg::Quat& r,
581                             osg::Vec3f& s,
582                             osg::Quat& so) const
583{
584    Vec3d temp_trans;
585    Vec3d temp_scale;
586    decompose(temp_trans, r, temp_scale, so);
587    t = temp_trans;
588    s = temp_scale;
589}
590
591
592void osg::Matrixf::decompose(osg::Vec3d& t,
593                             osg::Quat& r,
594                             osg::Vec3d& s,
595                             osg::Quat& so) const
596{
597    MatrixDecomposition::_affineParts parts;
598    MatrixDecomposition::_HMatrix hmatrix;
599
600    // Transpose copy of LTW
601    for ( int i =0; i<4; i++)
602    {
603        for ( int j=0; j<4; j++)
604        {
605            hmatrix[i][j] = (*this)(j,i);
606        }
607    }
608
609    MatrixDecomposition::decompAffine(hmatrix, &parts);
610
611    double mul = 1.0;
612    if (parts.t[MatrixDecomposition::W] != 0.0)
613        mul = 1.0 / parts.t[MatrixDecomposition::W];
614
615    t[0] = parts.t[MatrixDecomposition::X] * mul;
616    t[1] = parts.t[MatrixDecomposition::Y] * mul;
617    t[2] = parts.t[MatrixDecomposition::Z] * mul;
618
619    r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w);
620
621    mul = 1.0;
622    if (parts.k.w != 0.0)
623        mul = 1.0 / parts.k.w;
624
625    // mul be sign of determinant to support negative scales.
626    mul *= parts.f;
627    s[0] = parts.k.x * mul;
628    s[1] = parts.k.y * mul;
629    s[2] = parts.k.z * mul;
630
631    so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w);
632}
633
634void osg::Matrixd::decompose(osg::Vec3f& t,
635                             osg::Quat& r,
636                             osg::Vec3f& s,
637                             osg::Quat& so) const
638{
639    Vec3d temp_trans;
640    Vec3d temp_scale;
641    decompose(temp_trans, r, temp_scale, so);
642    t = temp_trans;
643    s = temp_scale;
644}
645
646void osg::Matrixd::decompose(osg::Vec3d& t,
647                             osg::Quat& r,
648                             osg::Vec3d& s,
649                             osg::Quat& so) const
650{
651    MatrixDecomposition::_affineParts parts;
652    MatrixDecomposition::_HMatrix hmatrix;
653
654    // Transpose copy of LTW
655    for ( int i =0; i<4; i++)
656    {
657        for ( int j=0; j<4; j++)
658        {
659            hmatrix[i][j] = (*this)(j,i);
660        }
661    }
662
663    MatrixDecomposition::decompAffine(hmatrix, &parts);
664
665    double mul = 1.0;
666    if (parts.t[MatrixDecomposition::W] != 0.0)
667        mul = 1.0 / parts.t[MatrixDecomposition::W];
668
669    t[0] = parts.t[MatrixDecomposition::X] * mul;
670    t[1] = parts.t[MatrixDecomposition::Y] * mul;
671    t[2] = parts.t[MatrixDecomposition::Z] * mul;
672
673    r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w);
674
675    mul = 1.0;
676    if (parts.k.w != 0.0)
677        mul = 1.0 / parts.k.w;
678
679    // mul be sign of determinant to support negative scales.
680    mul *= parts.f;
681    s[0] = parts.k.x * mul;
682    s[1] = parts.k.y * mul;
683    s[2] = parts.k.z * mul;
684
685    so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w);
686}
Note: See TracBrowser for help on using the browser.