framework2

1

2

3

4

5
#include "ofMatrix4x4.h"
6

7
#include <limits>
8
#include <stdlib.h>
9
#include <iomanip>
10

11
#if (_MSC_VER)
12
#undef min
13
// see: http://stackoverflow.com/questions/1904635/warning-c4003-and-errors-c2589-and-c2059-on-x-stdnumericlimitsintmax
14
#endif
15

16
// FIXME: why not using std::numeric_limits<double>::epsilon()
17
inline bool equivalent(double lhs,double rhs,double epsilon=1e-6)
18
{ double delta = rhs-lhs; return delta<0.0?delta>=-epsilon:delta<=epsilon; }
19
template<typename T>
20
inline T square(T v) { return v*v; }
21

22
#define SET_ROW(row, v1, v2, v3, v4 )    \
23
_mat[(row)][0] = (v1); \
24
_mat[(row)][1] = (v2); \
25
_mat[(row)][2] = (v3); \
26
_mat[(row)][3] = (v4);
27

28
#define INNER_PRODUCT(a,b,r,c) \
29
((a)._mat[r][0] * (b)._mat[0][c]) \
30
+((a)._mat[r][1] * (b)._mat[1][c]) \
31
+((a)._mat[r][2] * (b)._mat[2][c]) \
32
+((a)._mat[r][3] * (b)._mat[3][c])
33

34
ofMatrix4x4::ofMatrix4x4( float a00, float a01, float a02, float a03,
35
											 float a10, float a11, float a12, float a13,
36
											 float a20, float a21, float a22, float a23,
37
											 float a30, float a31, float a32, float a33)
38
{
39
    SET_ROW(0, a00, a01, a02, a03 )
40
    SET_ROW(1, a10, a11, a12, a13 )
41
    SET_ROW(2, a20, a21, a22, a23 )
42
    SET_ROW(3, a30, a31, a32, a33 )
43
}
44

45
void ofMatrix4x4::set( float a00, float a01, float a02, float a03,
46
								float a10, float a11, float a12, float a13,
47
								float a20, float a21, float a22, float a23,
48
								float a30, float a31, float a32, float a33)
49
{
50
    SET_ROW(0, a00, a01, a02, a03 )
51
    SET_ROW(1, a10, a11, a12, a13 )
52
    SET_ROW(2, a20, a21, a22, a23 )
53
    SET_ROW(3, a30, a31, a32, a33 )
54
}
55

56
#define QX  q._v[0]
57
#define QY  q._v[1]
58
#define QZ  q._v[2]
59
#define QW  q._v[3]
60

61
void ofMatrix4x4::setRotate(const ofQuaternion& q)
62
{
63
    double length2 = q.length2();
64
    if (fabs(length2) <= std::numeric_limits<double>::min())
65
    {
66
        _mat[0][0] = 1.0; _mat[1][0] = 0.0; _mat[2][0] = 0.0;
67
        _mat[0][1] = 0.0; _mat[1][1] = 1.0; _mat[2][1] = 0.0;
68
        _mat[0][2] = 0.0; _mat[1][2] = 0.0; _mat[2][2] = 1.0;
69
    }
70
    else
71
    {
72
        double rlength2;
73
        // normalize quat if required.
74
        // We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
75
        // That is a square of a square root, so it is possible to avoid that
76
        if (length2 != 1.0)
77
        {
78
            rlength2 = 2.0/length2;
79
        }
80
        else
81
        {
82
            rlength2 = 2.0;
83
        }
84

85
        // Source: Gamasutra, Rotating Objects Using Quaternions
86
        //
87
        //http://www.gamasutra.com/features/19980703/quaternions_01.htm
88

89
        double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
90

91
        // calculate coefficients
92
        x2 = rlength2*QX;
93
        y2 = rlength2*QY;
94
        z2 = rlength2*QZ;
95

96
        xx = QX * x2;
97
        xy = QX * y2;
98
        xz = QX * z2;
99

100
        yy = QY * y2;
101
        yz = QY * z2;
102
        zz = QZ * z2;
103

104
        wx = QW * x2;
105
        wy = QW * y2;
106
        wz = QW * z2;
107

108
        // Note.  Gamasutra gets the matrix assignments inverted, resulting
109
        // in left-handed rotations, which is contrary to OpenGL and OSG's
110
        // methodology.  The matrix assignment has been altered in the next
111
        // few lines of code to do the right thing.
112
        // Don Burns - Oct 13, 2001
113
        _mat[0][0] = 1.0 - (yy + zz);
114
        _mat[1][0] = xy - wz;
115
        _mat[2][0] = xz + wy;
116

117

118
        _mat[0][1] = xy + wz;
119
        _mat[1][1] = 1.0 - (xx + zz);
120
        _mat[2][1] = yz - wx;
121

122
        _mat[0][2] = xz - wy;
123
        _mat[1][2] = yz + wx;
124
        _mat[2][2] = 1.0 - (xx + yy);
125
    }
126

127
#if 0
128
    _mat[0][3] = 0.0;
129
    _mat[1][3] = 0.0;
130
    _mat[2][3] = 0.0;
131

132
    _mat[3][0] = 0.0;
133
    _mat[3][1] = 0.0;
134
    _mat[3][2] = 0.0;
135
    _mat[3][3] = 1.0;
136
#endif
137
}
138
//
139
//ofQuaternion ofMatrix4x4::getRotate() const {
140
//	ofVec4f q;
141
//	float trace = _mat[0][0] + _mat[1][1] + _mat[2][2]; // I removed + 1.0f; see discussion with Ethan
142
//	if( trace > 0 ) {// I changed M_EPSILON to 0
143
//		float s = 0.5f / sqrtf(trace+ 1.0f);
144
//		q.w = 0.25f / s;
145
//		q.x = ( _mat[2][1] - _mat[1][2] ) * s;
146
//		q.y = ( _mat[0][2] - _mat[2][0] ) * s;
147
//		q.z = ( _mat[1][0] - _mat[0][1] ) * s;
148
//	} else {
149
//		if ( _mat[0][0] > _mat[1][1] && _mat[0][0] > _mat[2][2] ) {
150
//			float s = 2.0f * sqrtf( 1.0f + _mat[0][0] - _mat[1][1] - _mat[2][2]);
151
//			q.w = (_mat[2][1] - _mat[1][2] ) / s;
152
//			q.x = 0.25f * s;
153
//			q.y = (_mat[0][1] + _mat[1][0] ) / s;
154
//			q.z = (_mat[0][2] + _mat[2][0] ) / s;
155
//		} else if (_mat[1][1] > _mat[2][2]) {
156
//			float s = 2.0f * sqrtf( 1.0f + _mat[1][1] - _mat[0][0] - _mat[2][2]);
157
//			q.w = (_mat[0][2] - _mat[2][0] ) / s;
158
//			q.x = (_mat[0][1] + _mat[1][0] ) / s;
159
//			q.y = 0.25f * s;
160
//			q.z = (_mat[1][2] + _mat[2][1] ) / s;
161
//		} else {
162
//			float s = 2.0f * sqrtf( 1.0f + _mat[2][2] - _mat[0][0] - _mat[1][1] );
163
//			q.w = (_mat[1][0] - _mat[0][1] ) / s;
164
//			q.x = (_mat[0][2] + _mat[2][0] ) / s;
165
//			q.y = (_mat[1][2] + _mat[2][1] ) / s;
166
//			q.z = 0.25f * s;
167
//		}
168
//	}
169
//	return ofQuaternion(q.x, q.y, q.z, q.w);
170
//}
171

172
//#define COMPILE_getRotate_David_Spillings_Mk1
173
//#define COMPILE_getRotate_David_Spillings_Mk2
174
#define COMPILE_getRotate_Original
175

176
#ifdef COMPILE_getRotate_David_Spillings_Mk1
177
// David Spillings implementation Mk 1
178
ofQuaternion ofMatrix4x4::getRotate() const
179
{
180
	ofQuaternion q;
181

182
    float s;
183
    float tq[4];
184
    int    i, j;
185

186
    // Use tq to store the largest trace
187
    tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2];
188
    tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2];
189
    tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2];
190
    tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2];
191

192
    // Find the maximum (could also use stacked if's later)
193
    j = 0;
194
    for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;
195

196
    // check the diagonal
197
    if (j==0)
198
    {
199
        /* perform instant calculation */
200
        QW = tq[0];
201
        QX = _mat[1][2]-_mat[2][1];
202
        QY = _mat[2][0]-_mat[0][2];
203
        QZ = _mat[0][1]-_mat[1][0];
204
    }
205
    else if (j==1)
206
    {
207
        QW = _mat[1][2]-_mat[2][1];
208
        QX = tq[1];
209
        QY = _mat[0][1]+_mat[1][0];
210
        QZ = _mat[2][0]+_mat[0][2];
211
    }
212
    else if (j==2)
213
    {
214
        QW = _mat[2][0]-_mat[0][2];
215
        QX = _mat[0][1]+_mat[1][0];
216
        QY = tq[2];
217
        QZ = _mat[1][2]+_mat[2][1];
218
    }
219
    else /* if (j==3) */
220
    {
221
        QW = _mat[0][1]-_mat[1][0];
222
        QX = _mat[2][0]+_mat[0][2];
223
        QY = _mat[1][2]+_mat[2][1];
224
        QZ = tq[3];
225
    }
226

227
    s = sqrt(0.25/tq[j]);
228
    QW *= s;
229
    QX *= s;
230
    QY *= s;
231
    QZ *= s;
232

233
    return q;
234

235
}
236
#endif
237

238

239
#ifdef COMPILE_getRotate_David_Spillings_Mk2
240
// David Spillings implementation Mk 2
241
ofQuaternion ofMatrix4x4::getRotate() const
242
{
243
    ofQuaternion q;
244

245
    // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
246
    QW = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] + _mat[1][1] + _mat[2][2] ) );
247
    QX = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] - _mat[1][1] - _mat[2][2] ) );
248
    QY = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] + _mat[1][1] - _mat[2][2] ) );
249
    QZ = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] - _mat[1][1] + _mat[2][2] ) );
250

251
#if 0
252
    // Robert Osfield, June 7th 2007, arggg this new implementation produces many many errors, so have to revert to sign(..) orignal below.
253
    QX = QX * osg::signOrZero(  _mat[1][2] - _mat[2][1]) ;
254
    QY = QY * osg::signOrZero(  _mat[2][0] - _mat[0][2]) ;
255
    QZ = QZ * osg::signOrZero(  _mat[0][1] - _mat[1][0]) ;
256
#else
257
    QX = QX * osg::sign(  _mat[1][2] - _mat[2][1]) ;
258
    QY = QY * osg::sign(  _mat[2][0] - _mat[0][2]) ;
259
    QZ = QZ * osg::sign(  _mat[0][1] - _mat[1][0]) ;
260
#endif
261

262
    return q;
263
}
264
#endif
265

266
#ifdef COMPILE_getRotate_Original
267
// Original implementation
268
ofQuaternion ofMatrix4x4::getRotate() const
269
{
270
    ofQuaternion q;
271

272
    ofMatrix4x4 mat = *this;
273
    ofVec3f vs = mat.getScale();
274
    mat.scale(1./vs.x,1./vs.y,1./vs.z);
275

276
    ofVec4f* m = mat._mat;
277

278
    // Source: Gamasutra, Rotating Objects Using Quaternions
279
    //
280
    //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
281

282
    float  tr, s;
283
    float tq[4];
284
    int    i, j, k;
285

286
    int nxt[3] = {1, 2, 0};
287

288
    tr = m[0][0] + m[1][1] + m[2][2]+1.0;
289

290
    // check the diagonal
291
    if (tr > 0.0)
292
    {
293
        s = (float)sqrt (tr);
294
        QW = s / 2.0;
295
        s = 0.5 / s;
296
        QX = (m[1][2] - m[2][1]) * s;
297
        QY = (m[2][0] - m[0][2]) * s;
298
        QZ = (m[0][1] - m[1][0]) * s;
299
    }
300
    else
301
    {
302
        // diagonal is negative
303
        i = 0;
304
        if (m[1][1] > m[0][0])
305
            i = 1;
306
        if (m[2][2] > m[i][i])
307
            i = 2;
308
        j = nxt[i];
309
        k = nxt[j];
310

311
        s = (float)sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
312

313
        tq[i] = s * 0.5;
314

315
        if (s != 0.0)
316
            s = 0.5 / s;
317

318
        tq[3] = (m[j][k] - m[k][j]) * s;
319
        tq[j] = (m[i][j] + m[j][i]) * s;
320
        tq[k] = (m[i][k] + m[k][i]) * s;
321

322
        QX = tq[0];
323
        QY = tq[1];
324
        QZ = tq[2];
325
        QW = tq[3];
326
    }
327

328
    return q;
329
}
330
#endif
331

332

333
//int ofMatrix4x4::compare(const ofMatrix4x4& m) const
334
//{
335
//    const ofMatrix4x4::float* lhs = reinterpret_cast<const ofMatrix4x4::float*>(_mat);
336
//    const ofMatrix4x4::float* end_lhs = lhs+16;
337
//    const ofMatrix4x4::float* rhs = reinterpret_cast<const ofMatrix4x4::float*>(m._mat);
338
//    for(;lhs!=end_lhs;++lhs,++rhs)
339
//    {
340
//        if (*lhs < *rhs) return -1;
341
//        if (*rhs < *lhs) return 1;
342
//    }
343
//    return 0;
344
//}
345

346
void ofMatrix4x4::setTranslation( float tx, float ty, float tz )
347
{
348
    _mat[3][0] = tx;
349
    _mat[3][1] = ty;
350
    _mat[3][2] = tz;
351
}
352

353

354
void ofMatrix4x4::setTranslation( const ofVec3f& v )
355
{
356
    _mat[3][0] = v.getPtr()[0];
357
    _mat[3][1] = v.getPtr()[1];
358
    _mat[3][2] = v.getPtr()[2];
359
}
360

361
void ofMatrix4x4::makeIdentityMatrix()
362
{
363
    SET_ROW(0,    1, 0, 0, 0 )
364
    SET_ROW(1,    0, 1, 0, 0 )
365
    SET_ROW(2,    0, 0, 1, 0 )
366
    SET_ROW(3,    0, 0, 0, 1 )
367
}
368

369
void ofMatrix4x4::makeScaleMatrix( const ofVec3f& v )
370
{
371
    makeScaleMatrix(v.getPtr()[0], v.getPtr()[1], v.getPtr()[2] );
372
}
373

374
void ofMatrix4x4::makeScaleMatrix( float x, float y, float z )
375
{
376
    SET_ROW(0,    x, 0, 0, 0 )
377
    SET_ROW(1,    0, y, 0, 0 )
378
    SET_ROW(2,    0, 0, z, 0 )
379
    SET_ROW(3,    0, 0, 0, 1 )
380
}
381

382
void ofMatrix4x4::makeTranslationMatrix( const ofVec3f& v )
383
{
384
    makeTranslationMatrix( v.getPtr()[0], v.getPtr()[1], v.getPtr()[2] );
385
}
386
void ofMatrix4x4::makeTranslationMatrix( float x, float y, float z )
387
{
388
    SET_ROW(0,    1, 0, 0, 0 )
389
    SET_ROW(1,    0, 1, 0, 0 )
390
    SET_ROW(2,    0, 0, 1, 0 )
391
    SET_ROW(3,    x, y, z, 1 )
392
}
393

394
void ofMatrix4x4::makeRotationMatrix( const ofVec3f& from, const ofVec3f& to )
395
{
396
    makeIdentityMatrix();
397

398
    ofQuaternion quat;
399
    quat.makeRotate(from,to);
400
    setRotate(quat);
401
}
402
void ofMatrix4x4::makeRotationMatrix( float angle, const ofVec3f& axis )
403
{
404
    makeIdentityMatrix();
405

406
    ofQuaternion quat;
407
    quat.makeRotate( angle, axis);
408
    setRotate(quat);
409
}
410

411
void ofMatrix4x4::makeRotationMatrix( float angle, float x, float y, float z )
412
{
413
    makeIdentityMatrix();
414

415
    ofQuaternion quat;
416
    quat.makeRotate( angle, x, y, z);
417
    setRotate(quat);
418
}
419

420
void ofMatrix4x4::makeRotationMatrix( const ofQuaternion& quat )
421
{
422
    makeIdentityMatrix();
423

424
    setRotate(quat);
425
}
426

427
void ofMatrix4x4::makeRotationMatrix( float angle1, const ofVec3f& axis1,
428
									   float angle2, const ofVec3f& axis2,
429
									   float angle3, const ofVec3f& axis3)
430
{
431
    makeIdentityMatrix();
432

433
    ofQuaternion quat;
434
    quat.makeRotate(angle1, axis1,
435
                    angle2, axis2,
436
                    angle3, axis3);
437
    setRotate(quat);
438
}
439

440
void ofMatrix4x4::makeFromMultiplicationOf( const ofMatrix4x4& lhs, const ofMatrix4x4& rhs )
441
{
442
    if (&lhs==this)
443
    {
444
        postMult(rhs);
445
        return;
446
    }
447
    if (&rhs==this)
448
    {
449
        preMult(lhs);
450
        return;
451
    }
452

453
	// PRECONDITION: We assume neither &lhs nor &rhs == this
454
	// if it did, use preMult or postMult instead
455
    _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
456
    _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
457
    _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
458
    _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
459
    _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
460
    _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
461
    _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
462
    _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
463
    _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
464
    _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
465
    _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
466
    _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
467
    _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
468
    _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
469
    _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
470
    _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
471
}
472

473
void ofMatrix4x4::preMult( const ofMatrix4x4& other )
474
{
475
    // brute force method requiring a copy
476
    //ofMatrix4x4 tmp(other* *this);
477
    // *this = tmp;
478

479
    // more efficient method just use a float[4] for temporary storage.
480
    float t[4];
481
    for(int col=0; col<4; ++col) {
482
        t[0] = INNER_PRODUCT( other, *this, 0, col );
483
        t[1] = INNER_PRODUCT( other, *this, 1, col );
484
        t[2] = INNER_PRODUCT( other, *this, 2, col );
485
        t[3] = INNER_PRODUCT( other, *this, 3, col );
486
        _mat[0][col] = t[0];
487
        _mat[1][col] = t[1];
488
        _mat[2][col] = t[2];
489
        _mat[3][col] = t[3];
490
    }
491

492
}
493

494
void ofMatrix4x4::postMult( const ofMatrix4x4& other )
495
{
496
    // brute force method requiring a copy
497
    //ofMatrix4x4 tmp(*this * other);
498
    // *this = tmp;
499

500
    // more efficient method just use a float[4] for temporary storage.
501
    float t[4];
502
    for(int row=0; row<4; ++row)
503
    {
504
        t[0] = INNER_PRODUCT( *this, other, row, 0 );
505
        t[1] = INNER_PRODUCT( *this, other, row, 1 );
506
        t[2] = INNER_PRODUCT( *this, other, row, 2 );
507
        t[3] = INNER_PRODUCT( *this, other, row, 3 );
508
        SET_ROW(row, t[0], t[1], t[2], t[3] )
509
    }
510
}
511

512
#undef INNER_PRODUCT
513

514
// orthoNormalize the 3x3 rotation matrix
515
void ofMatrix4x4::makeOrthoNormalOf(const ofMatrix4x4& rhs)
516
{
517
    float x_colMag = (rhs._mat[0][0] * rhs._mat[0][0]) + (rhs._mat[1][0] * rhs._mat[1][0]) + (rhs._mat[2][0] * rhs._mat[2][0]);
518
    float y_colMag = (rhs._mat[0][1] * rhs._mat[0][1]) + (rhs._mat[1][1] * rhs._mat[1][1]) + (rhs._mat[2][1] * rhs._mat[2][1]);
519
    float z_colMag = (rhs._mat[0][2] * rhs._mat[0][2]) + (rhs._mat[1][2] * rhs._mat[1][2]) + (rhs._mat[2][2] * rhs._mat[2][2]);
520

521
    if(!equivalent((double)x_colMag, 1.0) && !equivalent((double)x_colMag, 0.0))
522
    {
523
		x_colMag = sqrt(x_colMag);
524
		_mat[0][0] = rhs._mat[0][0] / x_colMag;
525
		_mat[1][0] = rhs._mat[1][0] / x_colMag;
526
		_mat[2][0] = rhs._mat[2][0] / x_colMag;
527
    }
528
    else
529
    {
530
		_mat[0][0] = rhs._mat[0][0];
531
		_mat[1][0] = rhs._mat[1][0];
532
		_mat[2][0] = rhs._mat[2][0];
533
    }
534

535
    if(!equivalent((double)y_colMag, 1.0) && !equivalent((double)y_colMag, 0.0))
536
    {
537
		y_colMag = sqrt(y_colMag);
538
		_mat[0][1] = rhs._mat[0][1] / y_colMag;
539
		_mat[1][1] = rhs._mat[1][1] / y_colMag;
540
		_mat[2][1] = rhs._mat[2][1] / y_colMag;
541
    }
542
    else
543
    {
544
		_mat[0][1] = rhs._mat[0][1];
545
		_mat[1][1] = rhs._mat[1][1];
546
		_mat[2][1] = rhs._mat[2][1];
547
    }
548

549
    if(!equivalent((double)z_colMag, 1.0) && !equivalent((double)z_colMag, 0.0))
550
    {
551
		z_colMag = sqrt(z_colMag);
552
		_mat[0][2] = rhs._mat[0][2] / z_colMag;
553
		_mat[1][2] = rhs._mat[1][2] / z_colMag;
554
		_mat[2][2] = rhs._mat[2][2] / z_colMag;
555
    }
556
    else
557
    {
558
		_mat[0][2] = rhs._mat[0][2];
559
		_mat[1][2] = rhs._mat[1][2];
560
		_mat[2][2] = rhs._mat[2][2];
561
    }
562

563
    _mat[3][0] = rhs._mat[3][0];
564
    _mat[3][1] = rhs._mat[3][1];
565
    _mat[3][2] = rhs._mat[3][2];
566

567
    _mat[0][3] = rhs._mat[0][3];
568
    _mat[1][3] = rhs._mat[1][3];
569
    _mat[2][3] = rhs._mat[2][3];
570
    _mat[3][3] = rhs._mat[3][3];
571

572
}
573

574

575
/** 4x3 matrix invert, not right hand column is assumed to be 0,0,0,1. */
576
bool invert_4x3( const ofMatrix4x4& src, ofMatrix4x4 & dst);
577

578
/** full 4x4 matrix invert. */
579
bool invert_4x4( const ofMatrix4x4& rhs, ofMatrix4x4 & dst);
580

581
bool ofMatrix4x4::makeInvertOf(const ofMatrix4x4 & rhs){
582
	bool is_4x3 = (rhs._mat[0][3] == 0.0f && rhs._mat[1][3] == 0.0f &&  rhs._mat[2][3] == 0.0f && rhs._mat[3][3] == 1.0f);
583
	return is_4x3 ? invert_4x3(rhs,*this) :  invert_4x4(rhs,*this);
584
}
585

586
ofMatrix4x4 ofMatrix4x4::getInverse() const
587
{
588
    ofMatrix4x4 inverse;
589
    inverse.makeInvertOf(*this);
590
    return inverse;
591
}
592

593

594

595

596

597
/******************************************
598
 Matrix inversion technique:
599
 Given a matrix mat, we want to invert it.
600
 mat = [ r00 r01 r02 a
601
 r10 r11 r12 b
602
 r20 r21 r22 c
603
 tx  ty  tz  d ]
604
 We note that this matrix can be split into three matrices.
605
 mat = rot * trans * corr, where rot is rotation part, trans is translation part, and corr is the correction due to perspective (if any).
606
 rot = [ r00 r01 r02 0
607
 r10 r11 r12 0
608
 r20 r21 r22 0
609
 0   0   0   1 ]
610
 trans = [ 1  0  0  0
611
 0  1  0  0
612
 0  0  1  0
613
 tx ty tz 1 ]
614
 corr = [ 1 0 0 px
615
 0 1 0 py
616
 0 0 1 pz
617
 0 0 0 s ]
618
 where the elements of corr are obtained from linear combinations of the elements of rot, trans, and mat.
619
 So the inverse is mat' = (trans * corr)' * rot', where rot' must be computed the traditional way, which is easy since it is only a 3x3 matrix.
620
 This problem is simplified if [px py pz s] = [0 0 0 1], which will happen if mat was composed only of rotations, scales, and translations (which is common).  In this case, we can ignore corr entirely which saves on a lot of computations.
621
 ******************************************/
622

623
bool invert_4x3( const ofMatrix4x4& src, ofMatrix4x4 & dst )
624
{
625
    if (&src==&dst)
626
    {
627
		ofMatrix4x4 tm(src);
628
		return invert_4x3(tm,dst);
629
    }
630

631
    float r00, r01, r02,
632
	r10, r11, r12,
633
	r20, r21, r22;
634
	// Copy rotation components directly into registers for speed
635
    r00 = src._mat[0][0]; r01 = src._mat[0][1]; r02 = src._mat[0][2];
636
    r10 = src._mat[1][0]; r11 = src._mat[1][1]; r12 = src._mat[1][2];
637
    r20 = src._mat[2][0]; r21 = src._mat[2][1]; r22 = src._mat[2][2];
638

639
	// Partially compute inverse of rot
640
    dst._mat[0][0] = r11*r22 - r12*r21;
641
    dst._mat[0][1] = r02*r21 - r01*r22;
642
    dst._mat[0][2] = r01*r12 - r02*r11;
643

644
	// Compute determinant of rot from 3 elements just computed
645
    float one_over_det = 1.0/(r00*dst._mat[0][0] + r10*dst._mat[0][1] + r20*dst._mat[0][2]);
646
    r00 *= one_over_det; r10 *= one_over_det; r20 *= one_over_det;  // Saves on later computations
647

648
	// Finish computing inverse of rot
649
    dst._mat[0][0] *= one_over_det;
650
    dst._mat[0][1] *= one_over_det;
651
    dst._mat[0][2] *= one_over_det;
652
    dst._mat[0][3] = 0.0;
653
    dst._mat[1][0] = r12*r20 - r10*r22; // Have already been divided by det
654
    dst._mat[1][1] = r00*r22 - r02*r20; // same
655
    dst._mat[1][2] = r02*r10 - r00*r12; // same
656
    dst._mat[1][3] = 0.0;
657
    dst._mat[2][0] = r10*r21 - r11*r20; // Have already been divided by det
658
    dst._mat[2][1] = r01*r20 - r00*r21; // same
659
    dst._mat[2][2] = r00*r11 - r01*r10; // same
660
    dst._mat[2][3] = 0.0;
661
    dst._mat[3][3] = 1.0;
662

663
	// We no longer need the rxx or det variables anymore, so we can reuse them for whatever we want.  But we will still rename them for the sake of clarity.
664

665
#define d r22
666
    d  = src._mat[3][3];
667

668
    if( square(d-1.0) > 1.0e-6 )  // Involves perspective, so we must
669
    {                       // compute the full inverse
670

671
        ofMatrix4x4 TPinv;
672
        dst._mat[3][0] = dst._mat[3][1] = dst._mat[3][2] = 0.0;
673

674
#define px r00
675
#define py r01
676
#define pz r02
677
#define one_over_s  one_over_det
678
#define a  r10
679
#define b  r11
680
#define c  r12
681

682
        a  = src._mat[0][3]; b  = src._mat[1][3]; c  = src._mat[2][3];
683
        px = dst._mat[0][0]*a + dst._mat[0][1]*b + dst._mat[0][2]*c;
684
        py = dst._mat[1][0]*a + dst._mat[1][1]*b + dst._mat[1][2]*c;
685
        pz = dst._mat[2][0]*a + dst._mat[2][1]*b + dst._mat[2][2]*c;
686

687
#undef a
688
#undef b
689
#undef c
690
#define tx r10
691
#define ty r11
692
#define tz r12
693

694
        tx = src._mat[3][0]; ty = src._mat[3][1]; tz = src._mat[3][2];
695
        one_over_s  = 1.0/(d - (tx*px + ty*py + tz*pz));
696

697
        tx *= one_over_s; ty *= one_over_s; tz *= one_over_s;  // Reduces number of calculations later on
698

699
        // Compute inverse of trans*corr
700
        TPinv._mat[0][0] = tx*px + 1.0;
701
        TPinv._mat[0][1] = ty*px;
702
        TPinv._mat[0][2] = tz*px;
703
        TPinv._mat[0][3] = -px * one_over_s;
704
        TPinv._mat[1][0] = tx*py;
705
        TPinv._mat[1][1] = ty*py + 1.0;
706
        TPinv._mat[1][2] = tz*py;
707
        TPinv._mat[1][3] = -py * one_over_s;
708
        TPinv._mat[2][0] = tx*pz;
709
        TPinv._mat[2][1] = ty*pz;
710
        TPinv._mat[2][2] = tz*pz + 1.0;
711
        TPinv._mat[2][3] = -pz * one_over_s;
712
        TPinv._mat[3][0] = -tx;
713
        TPinv._mat[3][1] = -ty;
714
        TPinv._mat[3][2] = -tz;
715
        TPinv._mat[3][3] = one_over_s;
716

717
        dst.preMult(TPinv); // Finish computing full inverse of mat
718

719
#undef px
720
#undef py
721
#undef pz
722
#undef one_over_s
723
#undef d
724
    }
725
    else // Rightmost column is [0; 0; 0; 1] so it can be ignored
726
    {
727
        tx = src._mat[3][0]; ty = src._mat[3][1]; tz = src._mat[3][2];
728

729
        // Compute translation components of mat'
730
        dst._mat[3][0] = -(tx*dst._mat[0][0] + ty*dst._mat[1][0] + tz*dst._mat[2][0]);
731
        dst._mat[3][1] = -(tx*dst._mat[0][1] + ty*dst._mat[1][1] + tz*dst._mat[2][1]);
732
        dst._mat[3][2] = -(tx*dst._mat[0][2] + ty*dst._mat[1][2] + tz*dst._mat[2][2]);
733

734
#undef tx
735
#undef ty
736
#undef tz
737
    }
738

739
    return true;
740
}
741

742

743
template <class T>
744
inline T SGL_ABS(T a)
745
{
746
	return (a >= 0 ? a : -a);
747
}
748

749
#ifndef SGL_SWAP
750
#define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
751
#endif
752

753
bool invert_4x4( const ofMatrix4x4& src, ofMatrix4x4&dst )
754
{
755
    if (&src==&dst) {
756
		ofMatrix4x4 tm(src);
757
		return invert_4x4(tm,dst);
758
    }
759

760
    unsigned int indxc[4], indxr[4], ipiv[4];
761
    unsigned int i,j,k,l,ll;
762
    unsigned int icol = 0;
763
    unsigned int irow = 0;
764
    double temp, pivinv, dum, big;
765

766
    // copy in place this may be unnecessary
767
    dst = src;
768

769
    for (j=0; j<4; j++) ipiv[j]=0;
770

771
    for(i=0;i<4;i++)
772
    {
773
		big=0.0;
774
		for (j=0; j<4; j++)
775
			if (ipiv[j] != 1)
776
				for (k=0; k<4; k++)
777
				{
778
					if (ipiv[k] == 0)
779
					{
780
						if (SGL_ABS(dst(j,k)) >= big)
781
						{
782
							big = SGL_ABS(dst(j,k));
783
							irow=j;
784
							icol=k;
785
						}
786
					}
787
					else if (ipiv[k] > 1)
788
						return false;
789
				}
790
		++(ipiv[icol]);
791
		if (irow != icol)
792
			for (l=0; l<4; l++) SGL_SWAP(dst(irow,l),
793
										 dst(icol,l),
794
										 temp);
795

796
		indxr[i]=irow;
797
		indxc[i]=icol;
798
		if (dst(icol,icol) == 0)
799
			return false;
800

801
		pivinv = 1.0/dst(icol,icol);
802
		dst(icol,icol) = 1;
803
		for (l=0; l<4; l++) dst(icol,l) *= pivinv;
804
		for (ll=0; ll<4; ll++)
805
			if (ll != icol)
806
			{
807
				dum=dst(ll,icol);
808
				dst(ll,icol) = 0;
809
				for (l=0; l<4; l++)dst(ll,l) -= dst(icol,l)*dum;
810
			}
811
    }
812
    for (int lx=4; lx>0; --lx)
813
    {
814
		if (indxr[lx-1] != indxc[lx-1])
815
			for (k=0; k<4; k++) SGL_SWAP(dst(k,indxr[lx-1]),
816
										 dst(k,indxc[lx-1]),temp);
817
    }
818

819
    return true;
820
}
821

822
void ofMatrix4x4::makeOrthoMatrix(double left, double right,
823
									  double bottom, double top,
824
									  double zNear, double zFar)
825
{
826
    // note transpose of ofMatrix4x4 wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
827
    double tx = -(right+left)/(right-left);
828
    double ty = -(top+bottom)/(top-bottom);
829
    double tz = -(zFar+zNear)/(zFar-zNear);
830
    SET_ROW(0, 2.0/(right-left),               0.0,               0.0, 0.0 )
831
    SET_ROW(1,              0.0,  2.0/(top-bottom),               0.0, 0.0 )
832
    SET_ROW(2,              0.0,               0.0,  -2.0/(zFar-zNear), 0.0 )
833
    SET_ROW(3,               tx,                ty,                 tz, 1.0 )
834
}
835

836
bool ofMatrix4x4::getOrtho(double& left, double& right,
837
									 double& bottom, double& top,
838
									 double& zNear, double& zFar) const
839
{
840
    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=0.0 || _mat[3][3]!=1.0) return false;
841

842
    zNear = (_mat[3][2]+1.0) / _mat[2][2];
843
    zFar = (_mat[3][2]-1.0) / _mat[2][2];
844

845
    left = -(1.0+_mat[3][0]) / _mat[0][0];
846
    right = (1.0-_mat[3][0]) / _mat[0][0];
847

848
    bottom = -(1.0+_mat[3][1]) / _mat[1][1];
849
    top = (1.0-_mat[3][1]) / _mat[1][1];
850

851
    return true;
852
}
853

854

855
void ofMatrix4x4::makeFrustumMatrix(double left, double right,
856
										double bottom, double top,
857
										double zNear, double zFar)
858
{
859
    // note transpose of ofMatrix4x4 wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
860
    double A = (right+left)/(right-left);
861
    double B = (top+bottom)/(top-bottom);
862
    double C = -(zFar+zNear)/(zFar-zNear);
863
    double D = -2.0*zFar*zNear/(zFar-zNear);
864
    SET_ROW(0, 2.0*zNear/(right-left),                    0.0, 0.0,  0.0 )
865
    SET_ROW(1,                    0.0, 2.0*zNear/(top-bottom), 0.0,  0.0 )
866
    SET_ROW(2,                      A,                      B,   C, -1.0 )
867
    SET_ROW(3,                    0.0,                    0.0,   D,  0.0 )
868
}
869

870
bool ofMatrix4x4::getFrustum(double& left, double& right,
871
                                       double& bottom, double& top,
872
                                       double& zNear, double& zFar) const
873
{
874
    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=-1.0 || _mat[3][3]!=0.0) return false;
875

876

877
    zNear = _mat[3][2] / (_mat[2][2]-1.0);
878
    zFar = _mat[3][2] / (1.0+_mat[2][2]);
879

880
    left = zNear * (_mat[2][0]-1.0) / _mat[0][0];
881
    right = zNear * (1.0+_mat[2][0]) / _mat[0][0];
882

883
    top = zNear * (1.0+_mat[2][1]) / _mat[1][1];
884
    bottom = zNear * (_mat[2][1]-1.0) / _mat[1][1];
885

886
    return true;
887
}
888

889

890
void ofMatrix4x4::makePerspectiveMatrix(double fovy,double aspectRatio,
891
                                            double zNear, double zFar)
892
{
893
    // calculate the appropriate left, right etc.
894
    double tan_fovy = tan(ofDegToRad(fovy*0.5));
895
    double right  =  tan_fovy * aspectRatio * zNear;
896
    double left   = -right;
897
    double top    =  tan_fovy * zNear;
898
    double bottom =  -top;
899
    makeFrustumMatrix(left,right,bottom,top,zNear,zFar);
900
}
901

902
bool ofMatrix4x4::getPerspective(double& fovy,double& aspectRatio,
903
                                           double& zNear, double& zFar) const
904
{
905
    double right  =  0.0;
906
    double left   =  0.0;
907
    double top    =  0.0;
908
    double bottom =  0.0;
909
    if (getFrustum(left,right,bottom,top,zNear,zFar))
910
    {
911
        fovy = ofRadToDeg(atan(top/zNear)-atan(bottom/zNear));
912
        aspectRatio = (right-left)/(top-bottom);
913
        return true;
914
    }
915
    return false;
916
}
917

918
void ofMatrix4x4::makeLookAtViewMatrix(const ofVec3f& eye,const ofVec3f& center,const ofVec3f& up)
919
{
920
	ofVec3f zaxis = (eye - center).getNormalized();
921
	ofVec3f xaxis = up.getCrossed(zaxis).getNormalized();
922
	ofVec3f yaxis = zaxis.getCrossed(xaxis);
923

924
	_mat[0].set(xaxis.x, yaxis.x, zaxis.x, 0);
925
	_mat[1].set(xaxis.y, yaxis.y, zaxis.y, 0);
926
	_mat[2].set(xaxis.z, yaxis.z, zaxis.z, 0);
927
	_mat[3].set(-xaxis.dot(eye), -yaxis.dot(eye), -zaxis.dot(eye), 1);
928
}
929

930
void ofMatrix4x4::makeLookAtMatrix(const ofVec3f& eye,const ofVec3f& center,const ofVec3f& up)
931
{
932
	ofVec3f zaxis = (eye - center).getNormalized();
933
	ofVec3f xaxis = up.getCrossed(zaxis).getNormalized();
934
	ofVec3f yaxis = zaxis.getCrossed(xaxis);
935

936
	_mat[0].set(xaxis.x, xaxis.y, xaxis.z, 0);
937
	_mat[1].set(yaxis.x, yaxis.y, yaxis.z, 0);
938
	_mat[2].set(zaxis.x, zaxis.y, zaxis.z, 0);
939
	_mat[3].set(eye.x, eye.y, eye.z, 1);
940
}
941

942
void ofMatrix4x4::getLookAt(ofVec3f& eye,ofVec3f& center,ofVec3f& up,float lookDistance) const
943
{
944
    ofMatrix4x4 inv;
945
    inv.makeInvertOf(*this);
946
    eye = ofVec3f(0.0,0.0,0.0)*inv;
947
    up = transform3x3(*this,ofVec3f(0.0,1.0,0.0));
948
    center = transform3x3(*this,ofVec3f(0.0,0.0,-1));
949
    center.normalize();
950
    center = eye + center*lookDistance;
951
}
952

953

954

955
typedef ofQuaternion HVect;
956
typedef double _HMatrix[4][4];
957
typedef struct
958
{
959
	ofVec4f t;     // Translation Component;
960
	ofQuaternion q;           // Essential Rotation
961
	ofQuaternion u;          //Stretch rotation
962
	HVect k;          //Sign of determinant
963
	double f;          // Sign of determinant
964
} _affineParts;
965

966

967
enum QuatPart {X, Y, Z, W};
968

969
#define SQRTHALF (0.7071067811865475244)
970
static ofQuaternion qxtoz(0,SQRTHALF,0,SQRTHALF);
971
static ofQuaternion qytoz(SQRTHALF,0,0,SQRTHALF);
972
static ofQuaternion qppmm( 0.5, 0.5,-0.5,-0.5);
973
static ofQuaternion qpppp( 0.5, 0.5, 0.5, 0.5);
974
static ofQuaternion qmpmm(-0.5, 0.5,-0.5,-0.5);
975
static ofQuaternion qpppm( 0.5, 0.5, 0.5,-0.5);
976
static ofQuaternion q0001( 0.0, 0.0, 0.0, 1.0);
977
static ofQuaternion q1000( 1.0, 0.0, 0.0, 0.0);
978

979
/** Copy nxn matrix A to C using "gets" for assignment **/
980
#define matrixCopy(C, gets, A, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\
981
    C[i][j] gets (A[i][j]);}
982

983
/** Copy transpose of nxn matrix A to C using "gets" for assignment **/
984
#define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
985
    AT[i][j] gets (A[j][i]);}
986

987
/** Fill out 3x3 matrix to 4x4 **/
988
#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)
989

990

991
/** Assign nxn matrix C the element-wise combination of A and B using "op" **/
992
#define matBinop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
993
    C[i][j] gets (A[i][j]) op (B[i][j]);}
994

995
/** Copy nxn matrix A to C using "gets" for assignment **/
996
#define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
997
    C[i][j] gets (A[i][j]);}
998

999
/** Multiply the upper left 3x3 parts of A and B to get AB **/
1000
void mat_mult(_HMatrix A, _HMatrix B, _HMatrix AB)
1001
{
1002
    int i, j;
1003
    for (i=0; i<3; i++) for (j=0; j<3; j++)
1004
        AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
1005
}
1006

1007
double mat_norm(_HMatrix M, int tpose)
1008
{
1009
    int i;
1010
    double sum, max;
1011
    max = 0.0;
1012
    for (i=0; i<3; i++) {
1013
        if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
1014
        else       sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
1015
        if (max<sum) max = sum;
1016
    }
1017
    return max;
1018
}
1019

1020
double norm_inf(_HMatrix M) {return mat_norm(M, 0);}
1021
double norm_one(_HMatrix M) {return mat_norm(M, 1);}
1022

1023
static _HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
1024

1025
/** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
1026
int find_max_col(_HMatrix M)
1027
{
1028
    double abs, max;
1029
    int i, j, col;
1030
    max = 0.0; col = -1;
1031
    for (i=0; i<3; i++) for (j=0; j<3; j++) {
1032
        abs = M[i][j]; if (abs<0.0) abs = -abs;
1033
        if (abs>max) {max = abs; col = j;}
1034
    }
1035
    return col;
1036
}
1037

1038
void vcross(double *va, double *vb, double *v)
1039
{
1040
    v[0] = va[1]*vb[2] - va[2]*vb[1];
1041
    v[1] = va[2]*vb[0] - va[0]*vb[2];
1042
    v[2] = va[0]*vb[1] - va[1]*vb[0];
1043
}
1044

1045
/** Return dot product of length 3 vectors va and vb **/
1046
double vdot(double *va, double *vb)
1047
{
1048
    return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
1049
}
1050

1051

1052
/** Set MadjT to transpose of inverse of M times determinant of M **/
1053
void adjoint_transpose(_HMatrix M, _HMatrix MadjT)
1054
{
1055
    vcross(M[1], M[2], MadjT[0]);
1056
    vcross(M[2], M[0], MadjT[1]);
1057
    vcross(M[0], M[1], MadjT[2]);
1058
}
1059

1060
/** Setup u for Household reflection to zero all v components but first **/
1061
void make_reflector(double *v, double *u)
1062
{
1063
    double s = sqrt(vdot(v, v));
1064
    u[0] = v[0]; u[1] = v[1];
1065
    u[2] = v[2] + ((v[2]<0.0) ? -s : s);
1066
    s = sqrt(2.0/vdot(u, u));
1067
    u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
1068
}
1069

1070
/** Apply Householder reflection represented by u to column vectors of M **/
1071
void reflect_cols(_HMatrix M, double *u)
1072
{
1073
    int i, j;
1074
    for (i=0; i<3; i++) {
1075
        double s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
1076
        for (j=0; j<3; j++) M[j][i] -= u[j]*s;
1077
    }
1078
}
1079

1080
/** Apply Householder reflection represented by u to row vectors of M **/
1081
void reflect_rows(_HMatrix M, double *u)
1082
{
1083
    int i, j;
1084
    for (i=0; i<3; i++) {
1085
        double s = vdot(u, M[i]);
1086
        for (j=0; j<3; j++) M[i][j] -= u[j]*s;
1087
    }
1088
}
1089

1090
/** Find orthogonal factor Q of rank 1 (or less) M **/
1091
void do_rank1(_HMatrix M, _HMatrix Q)
1092
{
1093
    double v1[3], v2[3], s;
1094
    int col;
1095
    mat_copy(Q,=,mat_id,4);
1096
    /* If rank(M) is 1, we should find a non-zero column in M */
1097
    col = find_max_col(M);
1098
    if (col<0) return; /* Rank is 0 */
1099
    v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
1100
    make_reflector(v1, v1); reflect_cols(M, v1);
1101
    v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
1102
    make_reflector(v2, v2); reflect_rows(M, v2);
1103
    s = M[2][2];
1104
    if (s<0.0) Q[2][2] = -1.0;
1105
    reflect_cols(Q, v1); reflect_rows(Q, v2);
1106
}
1107

1108
/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
1109
void do_rank2(_HMatrix M, _HMatrix MadjT, _HMatrix Q)
1110
{
1111
    double v1[3], v2[3];
1112
    double w, x, y, z, c, s, d;
1113
    int col;
1114
    /* If rank(M) is 2, we should find a non-zero column in MadjT */
1115
    col = find_max_col(MadjT);
1116
    if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
1117
    v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
1118
    make_reflector(v1, v1); reflect_cols(M, v1);
1119
    vcross(M[0], M[1], v2);
1120
    make_reflector(v2, v2); reflect_rows(M, v2);
1121
    w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
1122
    if (w*z>x*y) {
1123
        c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
1124
        Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
1125
    } else {
1126
        c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
1127
        Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
1128
    }
1129
    Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
1130
    reflect_cols(Q, v1); reflect_rows(Q, v2);
1131
}
1132

1133
/* Return product of quaternion q by scalar w. */
1134
ofQuaternion Qt_Scale(ofQuaternion q, double w)
1135
{
1136
	ofQuaternion qq;
1137
	qq.w() = q.w()*w;
1138
	qq.x() = q.x()*w;
1139
	qq.y() = q.y()*w;
1140
	qq.z() = q.z()*w;
1141
	return (qq);
1142
}
1143

1144
/* Construct a unit quaternion from rotation matrix.  Assumes matrix is
1145
* used to multiply column vector on the left: vnew = mat vold.  Works
1146
* correctly for right-handed coordinate system and right-handed rotations.
1147
* Translation and perspective components ignored. */
1148

1149
ofQuaternion quatFromMatrix(_HMatrix mat)
1150
{
1151
   /* This algorithm avoids near-zero divides by looking for a large component
1152
	* - first w, then x, y, or z.  When the trace is greater than zero,
1153
	* |w| is greater than 1/2, which is as small as a largest component can be.
1154
	* Otherwise, the largest diagonal entry corresponds to the largest of |x|,
1155
	* |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
1156
   ofQuaternion qu = q0001;
1157
   double tr, s;
1158

1159
   tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
1160
   if (tr >= 0.0)
1161
   {
1162
	   s = sqrt(tr + mat[W][W]);
1163
	   qu.w() = s*0.5;
1164
	   s = 0.5 / s;
1165
	   qu.x() = (mat[Z][Y] - mat[Y][Z]) * s;
1166
	   qu.y() = (mat[X][Z] - mat[Z][X]) * s;
1167
	   qu.z() = (mat[Y][X] - mat[X][Y]) * s;
1168
   }
1169
   else
1170
   {
1171
	   int h = X;
1172
	   if (mat[Y][Y] > mat[X][X]) h = Y;
1173
	   if (mat[Z][Z] > mat[h][h]) h = Z;
1174
	   switch (h) {
1175
#define caseMacro(i,j,k,I,J,K) \
1176
		   case I:\
1177
				  s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
1178
		   qu.i() = s*0.5;\
1179
		   s = 0.5 / s;\
1180
		   qu.j() = (mat[I][J] + mat[J][I]) * s;\
1181
		   qu.k() = (mat[K][I] + mat[I][K]) * s;\
1182
		   qu.w() = (mat[K][J] - mat[J][K]) * s;\
1183
		   break
1184
		   caseMacro(x,y,z,X,Y,Z);
1185
		   caseMacro(y,z,x,Y,Z,X);
1186
		   caseMacro(z,x,y,Z,X,Y);
1187
	   }
1188
   }
1189
   if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
1190
   return (qu);
1191
}
1192

1193

1194
/******* Polar Decomposition *******/
1195
/* Polar Decomposition of 3x3 matrix in 4x4,
1196
 * M = QS.  See Nicholas Higham and Robert S. Schreiber,
1197
 * Fast Polar Decomposition of An Arbitrary Matrix,
1198
 * Technical Report 88-942, October 1988,
1199
 * Department of Computer Science, Cornell University.
1200
 */
1201

1202
double polarDecomp( _HMatrix M, _HMatrix Q, _HMatrix S)
1203
{
1204

1205
#define TOL 1.0e-6
1206
	_HMatrix Mk, MadjTk, Ek;
1207
	double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
1208
	int i, j;
1209

1210
	mat_tpose(Mk,=,M,3);
1211
	M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
1212

1213
	do
1214
	{
1215
		adjoint_transpose(Mk, MadjTk);
1216
		det = vdot(Mk[0], MadjTk[0]);
1217
		if (det==0.0)
1218
		{
1219
			do_rank2(Mk, MadjTk, Mk);
1220
			break;
1221
		}
1222

1223
		MadjT_one = norm_one(MadjTk);
1224
		MadjT_inf = norm_inf(MadjTk);
1225

1226
		gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
1227
		g1 = gamma*0.5;
1228
		g2 = 0.5/(gamma*det);
1229
		matrixCopy(Ek,=,Mk,3);
1230
		matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3);
1231
		mat_copy(Ek,-=,Mk,3);
1232
		E_one = norm_one(Ek);
1233
		M_one = norm_one(Mk);
1234
		M_inf = norm_inf(Mk);
1235

1236
	} while(E_one>(M_one*TOL));
1237

1238
	mat_tpose(Q,=,Mk,3); mat_pad(Q);
1239
	mat_mult(Mk, M, S);  mat_pad(S);
1240

1241
	for (i=0; i<3; i++)
1242
		for (j=i; j<3; j++)
1243
			S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
1244
	return (det);
1245
}
1246

1247
/******* Spectral Decomposition *******/
1248
/* Compute the spectral decomposition of symmetric positive semi-definite S.
1249
* Returns rotation in U and scale factors in result, so that if K is a diagonal
1250
* matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
1251
* See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
1252
*/
1253
HVect spectDecomp(_HMatrix S, _HMatrix U)
1254
{
1255
   HVect kv;
1256
   double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */
1257
   double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b;
1258
   static char nxt[] = {Y,Z,X};
1259
   int sweep, i, j;
1260
   mat_copy(U,=,mat_id,4);
1261
   Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z];
1262
   OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y];
1263
   for (sweep=20; sweep>0; sweep--) {
1264
	   double sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]);
1265
	   if (sm==0.0) break;
1266
	   for (i=Z; i>=X; i--) {
1267
		   int p = nxt[i]; int q = nxt[p];
1268
		   fabsOffDi = fabs(OffD[i]);
1269
		   g = 100.0*fabsOffDi;
1270
		   if (fabsOffDi>0.0) {
1271
			   h = Diag[q] - Diag[p];
1272
			   fabsh = fabs(h);
1273
			   if (fabsh+g==fabsh) {
1274
				   t = OffD[i]/h;
1275
			   } else {
1276
				   theta = 0.5*h/OffD[i];
1277
				   t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0));
1278
				   if (theta<0.0) t = -t;
1279
			   }
1280
			   c = 1.0/sqrt(t*t+1.0); s = t*c;
1281
			   tau = s/(c+1.0);
1282
			   ta = t*OffD[i]; OffD[i] = 0.0;
1283
			   Diag[p] -= ta; Diag[q] += ta;
1284
			   OffDq = OffD[q];
1285
			   OffD[q] -= s*(OffD[p] + tau*OffD[q]);
1286
			   OffD[p] += s*(OffDq   - tau*OffD[p]);
1287
			   for (j=Z; j>=X; j--) {
1288
				   a = U[j][p]; b = U[j][q];
1289
				   U[j][p] -= s*(b + tau*a);
1290
				   U[j][q] += s*(a - tau*b);
1291
			   }
1292
		   }
1293
	   }
1294
   }
1295
   kv.x() = Diag[X]; kv.y() = Diag[Y]; kv.z() = Diag[Z]; kv.w() = 1.0;
1296
   return (kv);
1297
}
1298

1299
/* Return conjugate of quaternion. */
1300
ofQuaternion Qt_Conj(ofQuaternion q)
1301
{
1302
	ofQuaternion qq;
1303
    qq.x() = -q.x(); qq.y() = -q.y(); qq.z() = -q.z(); qq.w() = q.w();
1304
    return (qq);
1305
}
1306

1307
/* Return quaternion product qL * qR.  Note: order is important!
1308
 * To combine rotations, use the product Mul(qSecond, qFirst),
1309
 * which gives the effect of rotating by qFirst then qSecond. */
1310
ofQuaternion Qt_Mul(ofQuaternion qL, ofQuaternion qR)
1311
{
1312
	ofQuaternion qq;
1313
    qq.w() = qL.w()*qR.w() - qL.x()*qR.x() - qL.y()*qR.y() - qL.z()*qR.z();
1314
    qq.x() = qL.w()*qR.x() + qL.x()*qR.w() + qL.y()*qR.z() - qL.z()*qR.y();
1315
    qq.y() = qL.w()*qR.y() + qL.y()*qR.w() + qL.z()*qR.x() - qL.x()*qR.z();
1316
    qq.z() = qL.w()*qR.z() + qL.z()*qR.w() + qL.x()*qR.y() - qL.y()*qR.x();
1317
    return (qq);
1318
}
1319

1320
/* Construct a (possibly non-unit) quaternion from real components. */
1321
ofQuaternion Qt_(double x, double y, double z, double w)
1322
{
1323
	ofQuaternion qq;
1324
    qq.x() = x; qq.y() = y; qq.z() = z; qq.w() = w;
1325
    return (qq);
1326
}
1327

1328
/******* Spectral Axis Adjustment *******/
1329

1330
/* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
1331
 * which permutes the axes and turns freely in the plane of duplicate scale
1332
 * factors, such that q p has the largest possible w component, i.e. the
1333
 * smallest possible angle. Permutes k's components to go with q p instead of q.
1334
 * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
1335
 * Proceedings of Graphics Interface 1992. Details on p. 262-263.
1336
 */
1337
ofQuaternion snuggle(ofQuaternion q, HVect *k)
1338
{
1339
#define sgn(n,v)    ((n)?-(v):(v))
1340
#define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
1341
#define cycle(a,p)  if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
1342
	else   {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
1343

1344
	ofQuaternion p = q0001;
1345
	double ka[4];
1346
	int i, turn = -1;
1347
	ka[X] = k->x(); ka[Y] = k->y(); ka[Z] = k->z();
1348

1349
	if (ka[X]==ka[Y]) {
1350
		if (ka[X]==ka[Z])
1351
			turn = W;
1352
		else turn = Z;
1353
	}
1354
	else {
1355
		if (ka[X]==ka[Z])
1356
			turn = Y;
1357
		else if (ka[Y]==ka[Z])
1358
			turn = X;
1359
	}
1360
	if (turn>=0) {
1361
		ofQuaternion qtoz, qp;
1362
		unsigned int  win;
1363
		double mag[3], t;
1364
		switch (turn) {
1365
			default: return (Qt_Conj(q));
1366
			case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
1367
			case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
1368
			case Z: qtoz = q0001; break;
1369
		}
1370
		q = Qt_Conj(q);
1371
		mag[0] = (double)q.z()*q.z()+(double)q.w()*q.w()-0.5;
1372
		mag[1] = (double)q.x()*q.z()-(double)q.y()*q.w();
1373
		mag[2] = (double)q.y()*q.z()+(double)q.x()*q.w();
1374

1375
		bool neg[3];
1376
		for (i=0; i<3; i++)
1377
		{
1378
			neg[i] = (mag[i]<0.0);
1379
			if (neg[i]) mag[i] = -mag[i];
1380
		}
1381

1382
		if (mag[0]>mag[1]) {
1383
			if (mag[0]>mag[2])
1384
				win = 0;
1385
			else win = 2;
1386
		}
1387
		else {
1388
			if (mag[1]>mag[2]) win = 1;
1389
			else win = 2;
1390
		}
1391

1392
		switch (win) {
1393
			case 0: if (neg[0]) p = q1000; else p = q0001; break;
1394
			case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
1395
			case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
1396
		}
1397

1398
		qp = Qt_Mul(q, p);
1399
		t = sqrt(mag[win]+0.5);
1400
		p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z()/t,qp.w()/t));
1401
		p = Qt_Mul(qtoz, Qt_Conj(p));
1402
	}
1403
	else {
1404
		double qa[4], pa[4];
1405
		unsigned int lo, hi;
1406
		bool par = false;
1407
		bool neg[4];
1408
		double all, big, two;
1409
		qa[0] = q.x(); qa[1] = q.y(); qa[2] = q.z(); qa[3] = q.w();
1410
		for (i=0; i<4; i++) {
1411
			pa[i] = 0.0;
1412
			neg[i] = (qa[i]<0.0);
1413
			if (neg[i]) qa[i] = -qa[i];
1414
			par ^= neg[i];
1415
		}
1416

1417
		/* Find two largest components, indices in hi and lo */
1418
		if (qa[0]>qa[1]) lo = 0;
1419
		else lo = 1;
1420

1421
		if (qa[2]>qa[3]) hi = 2;
1422
		else hi = 3;
1423

1424
		if (qa[lo]>qa[hi]) {
1425
			if (qa[lo^1]>qa[hi]) {
1426
				hi = lo; lo ^= 1;
1427
			}
1428
			else {
1429
				hi ^= lo; lo ^= hi; hi ^= lo;
1430
			}
1431
		}
1432
		else {
1433
			if (qa[hi^1]>qa[lo]) lo = hi^1;
1434
		}
1435

1436
		all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
1437
		two = (qa[hi]+qa[lo])*SQRTHALF;
1438
		big = qa[hi];
1439
		if (all>two) {
1440
			if (all>big) {/*all*/
1441
				{int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);}
1442
				cycle(ka,par);
1443
			}
1444
			else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
1445
		} else {
1446
			if (two>big) { /*two*/
1447
				pa[hi] = sgn(neg[hi],SQRTHALF);
1448
				pa[lo] = sgn(neg[lo], SQRTHALF);
1449
				if (lo>hi) {
1450
					hi ^= lo; lo ^= hi; hi ^= lo;
1451
				}
1452
				if (hi==W) {
1453
					hi = "\001\002\000"[lo];
1454
					lo = 3-hi-lo;
1455
				}
1456
				swap(ka,hi,lo);
1457
			}
1458
			else {/*big*/
1459
				pa[hi] = sgn(neg[hi],1.0);
1460
			}
1461
		}
1462
		p.x() = -pa[0]; p.y() = -pa[1]; p.z() = -pa[2]; p.w() = pa[3];
1463
	}
1464
	k->x() = ka[X]; k->y() = ka[Y]; k->z() = ka[Z];
1465
	return (p);
1466
}
1467

1468
/******* Decompose Affine Matrix *******/
1469

1470
/* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
1471
 * translation components, q contains the rotation R, u contains U, k contains
1472
 * scale factors, and f contains the sign of the determinant.
1473
 * Assumes A transforms column vectors in right-handed coordinates.
1474
 * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
1475
 * Proceedings of Graphics Interface 1992.
1476
 */
1477
void decompAffine(_HMatrix A, _affineParts * parts)
1478
{
1479
	_HMatrix Q, S, U;
1480
	ofQuaternion p;
1481

1482
	//Translation component.
1483
	parts->t = ofVec4f(A[X][W], A[Y][W], A[Z][W], 0);
1484
	double det = polarDecomp(A, Q, S);
1485
	if (det<0.0)
1486
	{
1487
		matrixCopy(Q, =, -Q, 3);
1488
		parts->f = -1;
1489
	}
1490
	else
1491
		parts->f = 1;
1492

1493
	parts->q = quatFromMatrix(Q);
1494
	parts->k = spectDecomp(S, U);
1495
	parts->u = quatFromMatrix(U);
1496
	p = snuggle(parts->u, &parts->k);
1497
	parts->u = Qt_Mul(parts->u, p);
1498
}
1499

1500
void ofMatrix4x4::decompose( ofVec3f& t,
1501
		   ofQuaternion& r,
1502
		   ofVec3f& s,
1503
		   ofQuaternion& so ) const{
1504

1505
	_affineParts parts;
1506
    _HMatrix hmatrix;
1507

1508
    // Transpose copy of LTW
1509
    for ( int i =0; i<4; i++)
1510
    {
1511
        for ( int j=0; j<4; j++)
1512
        {
1513
            hmatrix[i][j] = (*this)(j,i);
1514
        }
1515
    }
1516

1517
    decompAffine(hmatrix, &parts);
1518

1519
    double mul = 1.0;
1520
    if (parts.t[W] != 0.0)
1521
        mul = 1.0 / parts.t[W];
1522

1523
    t.x = parts.t[X] * mul;
1524
    t.y = parts.t[Y] * mul;
1525
    t.z = parts.t[Z] * mul;
1526

1527
    r.set(parts.q.x(), parts.q.y(), parts.q.z(), parts.q.w());
1528

1529
    mul = 1.0;
1530
    if (parts.k.w() != 0.0)
1531
        mul = 1.0 / parts.k.w();
1532

1533
    // mul be sign of determinant to support negative scales.
1534
    mul *= parts.f;
1535
    s.x= parts.k.x() * mul;
1536
    s.y = parts.k.y() * mul;
1537
    s.z = parts.k.z() * mul;
1538

1539
    so.set(parts.u.x(), parts.u.y(), parts.u.z(), parts.u.w());
1540
}
1541

1542
#undef SET_ROW
1543

1544
std::ostream& operator<<(std::ostream& os, const ofMatrix4x4& M) {
1545
	int w = 8;
1546
	os	<< std::setw(w)
1547
		<< M._mat[0][0] << ", " << std::setw(w)
1548
		<< M._mat[0][1] << ", " << std::setw(w)
1549
		<< M._mat[0][2] << ", " << std::setw(w)
1550
		<< M._mat[0][3] << std::endl;
1551

1552
	os	<< std::setw(w)
1553
		<< M._mat[1][0] << ", " << std::setw(w)
1554
		<< M._mat[1][1] << ", " << std::setw(w)
1555
		<< M._mat[1][2] << ", " << std::setw(w)
1556
		<< M._mat[1][3] << std::endl;
1557

1558
	os	<< std::setw(w)
1559
		<< M._mat[2][0] << ", " << std::setw(w)
1560
		<< M._mat[2][1] << ", " << std::setw(w)
1561
		<< M._mat[2][2] << ", " << std::setw(w)
1562
		<< M._mat[2][3] << std::endl;
1563

1564
	os	<< std::setw(w)
1565
		<< M._mat[3][0] << ", " << std::setw(w)
1566
		<< M._mat[3][1] << ", " << std::setw(w)
1567
		<< M._mat[3][2] << ", " << std::setw(w)
1568
		<< M._mat[3][3];
1569

1570
	return os;
1571
}
1572

1573
std::istream& operator>>(std::istream& is, ofMatrix4x4& M) {
1574
	is >> M._mat[0][0]; is.ignore(2);
1575
	is >> M._mat[0][1]; is.ignore(2);
1576
	is >> M._mat[0][2]; is.ignore(2);
1577
	is >> M._mat[0][3]; is.ignore(1);
1578

1579
	is >> M._mat[1][0]; is.ignore(2);
1580
	is >> M._mat[1][1]; is.ignore(2);
1581
	is >> M._mat[1][2]; is.ignore(2);
1582
	is >> M._mat[1][3]; is.ignore(1);
1583

1584
	is >> M._mat[2][0]; is.ignore(2);
1585
	is >> M._mat[2][1]; is.ignore(2);
1586
	is >> M._mat[2][2]; is.ignore(2);
1587
	is >> M._mat[2][3]; is.ignore(1);
1588

1589
	is >> M._mat[3][0]; is.ignore(2);
1590
	is >> M._mat[3][1]; is.ignore(2);
1591
	is >> M._mat[3][2]; is.ignore(2);
1592
	is >> M._mat[3][3];
1593
	return is;
1594
}
1595