1/*
2 * Copyright 2011 Google Inc.
3 *
4 * Use of this source code is governed by a BSD-style license that can be
5 * found in the LICENSE file.
6 */
7
8#include "SkMatrix44.h"
9
10static inline bool eq4(const SkMScalar* SK_RESTRICT a,
11                      const SkMScalar* SK_RESTRICT b) {
12    return (a[0] == b[0]) & (a[1] == b[1]) & (a[2] == b[2]) & (a[3] == b[3]);
13}
14
15bool SkMatrix44::operator==(const SkMatrix44& other) const {
16    if (this == &other) {
17        return true;
18    }
19
20    if (this->isTriviallyIdentity() && other.isTriviallyIdentity()) {
21        return true;
22    }
23
24    const SkMScalar* SK_RESTRICT a = &fMat[0][0];
25    const SkMScalar* SK_RESTRICT b = &other.fMat[0][0];
26
27#if 0
28    for (int i = 0; i < 16; ++i) {
29        if (a[i] != b[i]) {
30            return false;
31        }
32    }
33    return true;
34#else
35    // to reduce branch instructions, we compare 4 at a time.
36    // see bench/Matrix44Bench.cpp for test.
37    if (!eq4(&a[0], &b[0])) {
38        return false;
39    }
40    if (!eq4(&a[4], &b[4])) {
41        return false;
42    }
43    if (!eq4(&a[8], &b[8])) {
44        return false;
45    }
46    return eq4(&a[12], &b[12]);
47#endif
48}
49
50///////////////////////////////////////////////////////////////////////////////
51
52int SkMatrix44::computeTypeMask() const {
53    unsigned mask = 0;
54
55    if (0 != perspX() || 0 != perspY() || 0 != perspZ() || 1 != fMat[3][3]) {
56        return kTranslate_Mask | kScale_Mask | kAffine_Mask | kPerspective_Mask;
57    }
58
59    if (0 != transX() || 0 != transY() || 0 != transZ()) {
60        mask |= kTranslate_Mask;
61    }
62
63    if (1 != scaleX() || 1 != scaleY() || 1 != scaleZ()) {
64        mask |= kScale_Mask;
65    }
66
67    if (0 != fMat[1][0] || 0 != fMat[0][1] || 0 != fMat[0][2] ||
68        0 != fMat[2][0] || 0 != fMat[1][2] || 0 != fMat[2][1]) {
69            mask |= kAffine_Mask;
70    }
71
72    return mask;
73}
74
75///////////////////////////////////////////////////////////////////////////////
76
77void SkMatrix44::asColMajorf(float dst[]) const {
78    const SkMScalar* src = &fMat[0][0];
79#ifdef SK_MSCALAR_IS_DOUBLE
80    for (int i = 0; i < 16; ++i) {
81        dst[i] = SkMScalarToFloat(src[i]);
82    }
83#elif defined SK_MSCALAR_IS_FLOAT
84    memcpy(dst, src, 16 * sizeof(float));
85#endif
86}
87
88void SkMatrix44::asColMajord(double dst[]) const {
89    const SkMScalar* src = &fMat[0][0];
90#ifdef SK_MSCALAR_IS_DOUBLE
91    memcpy(dst, src, 16 * sizeof(double));
92#elif defined SK_MSCALAR_IS_FLOAT
93    for (int i = 0; i < 16; ++i) {
94        dst[i] = SkMScalarToDouble(src[i]);
95    }
96#endif
97}
98
99void SkMatrix44::asRowMajorf(float dst[]) const {
100    const SkMScalar* src = &fMat[0][0];
101    for (int i = 0; i < 4; ++i) {
102        dst[0] = SkMScalarToFloat(src[0]);
103        dst[4] = SkMScalarToFloat(src[1]);
104        dst[8] = SkMScalarToFloat(src[2]);
105        dst[12] = SkMScalarToFloat(src[3]);
106        src += 4;
107        dst += 1;
108    }
109}
110
111void SkMatrix44::asRowMajord(double dst[]) const {
112    const SkMScalar* src = &fMat[0][0];
113    for (int i = 0; i < 4; ++i) {
114        dst[0] = SkMScalarToDouble(src[0]);
115        dst[4] = SkMScalarToDouble(src[1]);
116        dst[8] = SkMScalarToDouble(src[2]);
117        dst[12] = SkMScalarToDouble(src[3]);
118        src += 4;
119        dst += 1;
120    }
121}
122
123void SkMatrix44::setColMajorf(const float src[]) {
124    SkMScalar* dst = &fMat[0][0];
125#ifdef SK_MSCALAR_IS_DOUBLE
126    for (int i = 0; i < 16; ++i) {
127        dst[i] = SkMScalarToFloat(src[i]);
128    }
129#elif defined SK_MSCALAR_IS_FLOAT
130    memcpy(dst, src, 16 * sizeof(float));
131#endif
132
133    this->dirtyTypeMask();
134}
135
136void SkMatrix44::setColMajord(const double src[]) {
137    SkMScalar* dst = &fMat[0][0];
138#ifdef SK_MSCALAR_IS_DOUBLE
139    memcpy(dst, src, 16 * sizeof(double));
140#elif defined SK_MSCALAR_IS_FLOAT
141    for (int i = 0; i < 16; ++i) {
142        dst[i] = SkDoubleToMScalar(src[i]);
143    }
144#endif
145
146    this->dirtyTypeMask();
147}
148
149void SkMatrix44::setRowMajorf(const float src[]) {
150    SkMScalar* dst = &fMat[0][0];
151    for (int i = 0; i < 4; ++i) {
152        dst[0] = SkMScalarToFloat(src[0]);
153        dst[4] = SkMScalarToFloat(src[1]);
154        dst[8] = SkMScalarToFloat(src[2]);
155        dst[12] = SkMScalarToFloat(src[3]);
156        src += 4;
157        dst += 1;
158    }
159    this->dirtyTypeMask();
160}
161
162void SkMatrix44::setRowMajord(const double src[]) {
163    SkMScalar* dst = &fMat[0][0];
164    for (int i = 0; i < 4; ++i) {
165        dst[0] = SkDoubleToMScalar(src[0]);
166        dst[4] = SkDoubleToMScalar(src[1]);
167        dst[8] = SkDoubleToMScalar(src[2]);
168        dst[12] = SkDoubleToMScalar(src[3]);
169        src += 4;
170        dst += 1;
171    }
172    this->dirtyTypeMask();
173}
174
175///////////////////////////////////////////////////////////////////////////////
176
177const SkMatrix44& SkMatrix44::I() {
178    static const SkMatrix44 gIdentity44(kIdentity_Constructor);
179    return gIdentity44;
180}
181
182void SkMatrix44::setIdentity() {
183    fMat[0][0] = 1;
184    fMat[0][1] = 0;
185    fMat[0][2] = 0;
186    fMat[0][3] = 0;
187    fMat[1][0] = 0;
188    fMat[1][1] = 1;
189    fMat[1][2] = 0;
190    fMat[1][3] = 0;
191    fMat[2][0] = 0;
192    fMat[2][1] = 0;
193    fMat[2][2] = 1;
194    fMat[2][3] = 0;
195    fMat[3][0] = 0;
196    fMat[3][1] = 0;
197    fMat[3][2] = 0;
198    fMat[3][3] = 1;
199    this->setTypeMask(kIdentity_Mask);
200}
201
202void SkMatrix44::set3x3(SkMScalar m00, SkMScalar m01, SkMScalar m02,
203                        SkMScalar m10, SkMScalar m11, SkMScalar m12,
204                        SkMScalar m20, SkMScalar m21, SkMScalar m22) {
205    fMat[0][0] = m00; fMat[0][1] = m01; fMat[0][2] = m02; fMat[0][3] = 0;
206    fMat[1][0] = m10; fMat[1][1] = m11; fMat[1][2] = m12; fMat[1][3] = 0;
207    fMat[2][0] = m20; fMat[2][1] = m21; fMat[2][2] = m22; fMat[2][3] = 0;
208    fMat[3][0] = 0;   fMat[3][1] = 0;   fMat[3][2] = 0;   fMat[3][3] = 1;
209    this->dirtyTypeMask();
210}
211
212///////////////////////////////////////////////////////////////////////////////
213
214void SkMatrix44::setTranslate(SkMScalar dx, SkMScalar dy, SkMScalar dz) {
215    this->setIdentity();
216
217    if (!dx && !dy && !dz) {
218        return;
219    }
220
221    fMat[3][0] = dx;
222    fMat[3][1] = dy;
223    fMat[3][2] = dz;
224    this->setTypeMask(kTranslate_Mask);
225}
226
227void SkMatrix44::preTranslate(SkMScalar dx, SkMScalar dy, SkMScalar dz) {
228    if (!dx && !dy && !dz) {
229        return;
230    }
231
232    for (int i = 0; i < 4; ++i) {
233        fMat[3][i] = fMat[0][i] * dx + fMat[1][i] * dy + fMat[2][i] * dz + fMat[3][i];
234    }
235    this->dirtyTypeMask();
236}
237
238void SkMatrix44::postTranslate(SkMScalar dx, SkMScalar dy, SkMScalar dz) {
239    if (!dx && !dy && !dz) {
240        return;
241    }
242
243    if (this->getType() & kPerspective_Mask) {
244        for (int i = 0; i < 4; ++i) {
245            fMat[i][0] += fMat[i][3] * dx;
246            fMat[i][1] += fMat[i][3] * dy;
247            fMat[i][2] += fMat[i][3] * dz;
248        }
249    } else {
250        fMat[3][0] += dx;
251        fMat[3][1] += dy;
252        fMat[3][2] += dz;
253        this->dirtyTypeMask();
254    }
255}
256
257///////////////////////////////////////////////////////////////////////////////
258
259void SkMatrix44::setScale(SkMScalar sx, SkMScalar sy, SkMScalar sz) {
260    this->setIdentity();
261
262    if (1 == sx && 1 == sy && 1 == sz) {
263        return;
264    }
265
266    fMat[0][0] = sx;
267    fMat[1][1] = sy;
268    fMat[2][2] = sz;
269    this->setTypeMask(kScale_Mask);
270}
271
272void SkMatrix44::preScale(SkMScalar sx, SkMScalar sy, SkMScalar sz) {
273    if (1 == sx && 1 == sy && 1 == sz) {
274        return;
275    }
276
277    // The implementation matrix * pureScale can be shortcut
278    // by knowing that pureScale components effectively scale
279    // the columns of the original matrix.
280    for (int i = 0; i < 4; i++) {
281        fMat[0][i] *= sx;
282        fMat[1][i] *= sy;
283        fMat[2][i] *= sz;
284    }
285    this->dirtyTypeMask();
286}
287
288void SkMatrix44::postScale(SkMScalar sx, SkMScalar sy, SkMScalar sz) {
289    if (1 == sx && 1 == sy && 1 == sz) {
290        return;
291    }
292
293    for (int i = 0; i < 4; i++) {
294        fMat[i][0] *= sx;
295        fMat[i][1] *= sy;
296        fMat[i][2] *= sz;
297    }
298    this->dirtyTypeMask();
299}
300
301///////////////////////////////////////////////////////////////////////////////
302
303void SkMatrix44::setRotateAbout(SkMScalar x, SkMScalar y, SkMScalar z,
304                                SkMScalar radians) {
305    double len2 = (double)x * x + (double)y * y + (double)z * z;
306    if (1 != len2) {
307        if (0 == len2) {
308            this->setIdentity();
309            return;
310        }
311        double scale = 1 / sqrt(len2);
312        x = SkDoubleToMScalar(x * scale);
313        y = SkDoubleToMScalar(y * scale);
314        z = SkDoubleToMScalar(z * scale);
315    }
316    this->setRotateAboutUnit(x, y, z, radians);
317}
318
319void SkMatrix44::setRotateAboutUnit(SkMScalar x, SkMScalar y, SkMScalar z,
320                                    SkMScalar radians) {
321    double c = cos(radians);
322    double s = sin(radians);
323    double C = 1 - c;
324    double xs = x * s;
325    double ys = y * s;
326    double zs = z * s;
327    double xC = x * C;
328    double yC = y * C;
329    double zC = z * C;
330    double xyC = x * yC;
331    double yzC = y * zC;
332    double zxC = z * xC;
333
334    // if you're looking at wikipedia, remember that we're column major.
335    this->set3x3(SkDoubleToMScalar(x * xC + c),     // scale x
336                 SkDoubleToMScalar(xyC + zs),       // skew x
337                 SkDoubleToMScalar(zxC - ys),       // trans x
338
339                 SkDoubleToMScalar(xyC - zs),       // skew y
340                 SkDoubleToMScalar(y * yC + c),     // scale y
341                 SkDoubleToMScalar(yzC + xs),       // trans y
342
343                 SkDoubleToMScalar(zxC + ys),       // persp x
344                 SkDoubleToMScalar(yzC - xs),       // persp y
345                 SkDoubleToMScalar(z * zC + c));    // persp 2
346}
347
348///////////////////////////////////////////////////////////////////////////////
349
350static bool bits_isonly(int value, int mask) {
351    return 0 == (value & ~mask);
352}
353
354void SkMatrix44::setConcat(const SkMatrix44& a, const SkMatrix44& b) {
355    const SkMatrix44::TypeMask a_mask = a.getType();
356    const SkMatrix44::TypeMask b_mask = b.getType();
357
358    if (kIdentity_Mask == a_mask) {
359        *this = b;
360        return;
361    }
362    if (kIdentity_Mask == b_mask) {
363        *this = a;
364        return;
365    }
366
367    bool useStorage = (this == &a || this == &b);
368    SkMScalar storage[16];
369    SkMScalar* result = useStorage ? storage : &fMat[0][0];
370
371    // Both matrices are at most scale+translate
372    if (bits_isonly(a_mask | b_mask, kScale_Mask | kTranslate_Mask)) {
373        result[0] = a.fMat[0][0] * b.fMat[0][0];
374        result[1] = result[2] = result[3] = result[4] = 0;
375        result[5] = a.fMat[1][1] * b.fMat[1][1];
376        result[6] = result[7] = result[8] = result[9] = 0;
377        result[10] = a.fMat[2][2] * b.fMat[2][2];
378        result[11] = 0;
379        result[12] = a.fMat[0][0] * b.fMat[3][0] + a.fMat[3][0];
380        result[13] = a.fMat[1][1] * b.fMat[3][1] + a.fMat[3][1];
381        result[14] = a.fMat[2][2] * b.fMat[3][2] + a.fMat[3][2];
382        result[15] = 1;
383    } else {
384        for (int j = 0; j < 4; j++) {
385            for (int i = 0; i < 4; i++) {
386                double value = 0;
387                for (int k = 0; k < 4; k++) {
388                    value += SkMScalarToDouble(a.fMat[k][i]) * b.fMat[j][k];
389                }
390                *result++ = SkDoubleToMScalar(value);
391            }
392        }
393    }
394
395    if (useStorage) {
396        memcpy(fMat, storage, sizeof(storage));
397    }
398    this->dirtyTypeMask();
399}
400
401///////////////////////////////////////////////////////////////////////////////
402
403/** We always perform the calculation in doubles, to avoid prematurely losing
404    precision along the way. This relies on the compiler automatically
405    promoting our SkMScalar values to double (if needed).
406 */
407double SkMatrix44::determinant() const {
408    if (this->isIdentity()) {
409        return 1;
410    }
411    if (this->isScaleTranslate()) {
412        return fMat[0][0] * fMat[1][1] * fMat[2][2] * fMat[3][3];
413    }
414
415    double a00 = fMat[0][0];
416    double a01 = fMat[0][1];
417    double a02 = fMat[0][2];
418    double a03 = fMat[0][3];
419    double a10 = fMat[1][0];
420    double a11 = fMat[1][1];
421    double a12 = fMat[1][2];
422    double a13 = fMat[1][3];
423    double a20 = fMat[2][0];
424    double a21 = fMat[2][1];
425    double a22 = fMat[2][2];
426    double a23 = fMat[2][3];
427    double a30 = fMat[3][0];
428    double a31 = fMat[3][1];
429    double a32 = fMat[3][2];
430    double a33 = fMat[3][3];
431
432    double b00 = a00 * a11 - a01 * a10;
433    double b01 = a00 * a12 - a02 * a10;
434    double b02 = a00 * a13 - a03 * a10;
435    double b03 = a01 * a12 - a02 * a11;
436    double b04 = a01 * a13 - a03 * a11;
437    double b05 = a02 * a13 - a03 * a12;
438    double b06 = a20 * a31 - a21 * a30;
439    double b07 = a20 * a32 - a22 * a30;
440    double b08 = a20 * a33 - a23 * a30;
441    double b09 = a21 * a32 - a22 * a31;
442    double b10 = a21 * a33 - a23 * a31;
443    double b11 = a22 * a33 - a23 * a32;
444
445    // Calculate the determinant
446    return b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
447}
448
449///////////////////////////////////////////////////////////////////////////////
450
451bool SkMatrix44::invert(SkMatrix44* inverse) const {
452    if (this->isIdentity()) {
453        if (inverse) {
454            inverse->setIdentity();
455        }
456        return true;
457    }
458
459    if (this->isTranslate()) {
460        if (inverse) {
461            inverse->setTranslate(-fMat[3][0], -fMat[3][1], -fMat[3][2]);
462        }
463        return true;
464    }
465
466    if (this->isScaleTranslate()) {
467        if (0 == fMat[0][0] * fMat[1][1] * fMat[2][2]) {
468            return false;
469        }
470
471        if (inverse) {
472            double invXScale = 1 / fMat[0][0];
473            double invYScale = 1 / fMat[1][1];
474            double invZScale = 1 / fMat[2][2];
475
476            inverse->fMat[0][0] = invXScale;
477            inverse->fMat[0][1] = 0;
478            inverse->fMat[0][2] = 0;
479            inverse->fMat[0][3] = 0;
480
481            inverse->fMat[1][0] = 0;
482            inverse->fMat[1][1] = invYScale;
483            inverse->fMat[1][2] = 0;
484            inverse->fMat[1][3] = 0;
485
486            inverse->fMat[2][0] = 0;
487            inverse->fMat[2][1] = 0;
488            inverse->fMat[2][2] = invZScale;
489            inverse->fMat[2][3] = 0;
490
491            inverse->fMat[3][0] = -fMat[3][0] * invXScale;
492            inverse->fMat[3][1] = -fMat[3][1] * invYScale;
493            inverse->fMat[3][2] = -fMat[3][2] * invZScale;
494            inverse->fMat[3][3] = 1;
495
496            inverse->setTypeMask(this->getType());
497        }
498
499        return true;
500    }
501
502    double a00 = fMat[0][0];
503    double a01 = fMat[0][1];
504    double a02 = fMat[0][2];
505    double a03 = fMat[0][3];
506    double a10 = fMat[1][0];
507    double a11 = fMat[1][1];
508    double a12 = fMat[1][2];
509    double a13 = fMat[1][3];
510    double a20 = fMat[2][0];
511    double a21 = fMat[2][1];
512    double a22 = fMat[2][2];
513    double a23 = fMat[2][3];
514    double a30 = fMat[3][0];
515    double a31 = fMat[3][1];
516    double a32 = fMat[3][2];
517    double a33 = fMat[3][3];
518
519    if (!(this->getType() & kPerspective_Mask)) {
520        // If we know the matrix has no perspective, then the perspective
521        // component is (0, 0, 0, 1). We can use this information to save a lot
522        // of arithmetic that would otherwise be spent to compute the inverse
523        // of a general matrix.
524
525        SkASSERT(a03 == 0);
526        SkASSERT(a13 == 0);
527        SkASSERT(a23 == 0);
528        SkASSERT(a33 == 1);
529
530        double b00 = a00 * a11 - a01 * a10;
531        double b01 = a00 * a12 - a02 * a10;
532        double b03 = a01 * a12 - a02 * a11;
533        double b06 = a20 * a31 - a21 * a30;
534        double b07 = a20 * a32 - a22 * a30;
535        double b08 = a20;
536        double b09 = a21 * a32 - a22 * a31;
537        double b10 = a21;
538        double b11 = a22;
539
540        // Calculate the determinant
541        double det = b00 * b11 - b01 * b10 + b03 * b08;
542
543        double invdet = 1.0 / det;
544        // If det is zero, we want to return false. However, we also want to return false
545        // if 1/det overflows to infinity (i.e. det is denormalized). Both of these are
546        // handled by checking that 1/det is finite.
547        if (!sk_float_isfinite(invdet)) {
548            return false;
549        }
550        if (NULL == inverse) {
551            return true;
552        }
553
554        b00 *= invdet;
555        b01 *= invdet;
556        b03 *= invdet;
557        b06 *= invdet;
558        b07 *= invdet;
559        b08 *= invdet;
560        b09 *= invdet;
561        b10 *= invdet;
562        b11 *= invdet;
563
564        inverse->fMat[0][0] = SkDoubleToMScalar(a11 * b11 - a12 * b10);
565        inverse->fMat[0][1] = SkDoubleToMScalar(a02 * b10 - a01 * b11);
566        inverse->fMat[0][2] = SkDoubleToMScalar(b03);
567        inverse->fMat[0][3] = 0;
568        inverse->fMat[1][0] = SkDoubleToMScalar(a12 * b08 - a10 * b11);
569        inverse->fMat[1][1] = SkDoubleToMScalar(a00 * b11 - a02 * b08);
570        inverse->fMat[1][2] = SkDoubleToMScalar(-b01);
571        inverse->fMat[1][3] = 0;
572        inverse->fMat[2][0] = SkDoubleToMScalar(a10 * b10 - a11 * b08);
573        inverse->fMat[2][1] = SkDoubleToMScalar(a01 * b08 - a00 * b10);
574        inverse->fMat[2][2] = SkDoubleToMScalar(b00);
575        inverse->fMat[2][3] = 0;
576        inverse->fMat[3][0] = SkDoubleToMScalar(a11 * b07 - a10 * b09 - a12 * b06);
577        inverse->fMat[3][1] = SkDoubleToMScalar(a00 * b09 - a01 * b07 + a02 * b06);
578        inverse->fMat[3][2] = SkDoubleToMScalar(a31 * b01 - a30 * b03 - a32 * b00);
579        inverse->fMat[3][3] = 1;
580
581        inverse->setTypeMask(this->getType());
582        return true;
583    }
584
585    double b00 = a00 * a11 - a01 * a10;
586    double b01 = a00 * a12 - a02 * a10;
587    double b02 = a00 * a13 - a03 * a10;
588    double b03 = a01 * a12 - a02 * a11;
589    double b04 = a01 * a13 - a03 * a11;
590    double b05 = a02 * a13 - a03 * a12;
591    double b06 = a20 * a31 - a21 * a30;
592    double b07 = a20 * a32 - a22 * a30;
593    double b08 = a20 * a33 - a23 * a30;
594    double b09 = a21 * a32 - a22 * a31;
595    double b10 = a21 * a33 - a23 * a31;
596    double b11 = a22 * a33 - a23 * a32;
597
598    // Calculate the determinant
599    double det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
600
601    double invdet = 1.0 / det;
602    // If det is zero, we want to return false. However, we also want to return false
603    // if 1/det overflows to infinity (i.e. det is denormalized). Both of these are
604    // handled by checking that 1/det is finite.
605    if (!sk_float_isfinite(invdet)) {
606        return false;
607    }
608    if (NULL == inverse) {
609        return true;
610    }
611
612    b00 *= invdet;
613    b01 *= invdet;
614    b02 *= invdet;
615    b03 *= invdet;
616    b04 *= invdet;
617    b05 *= invdet;
618    b06 *= invdet;
619    b07 *= invdet;
620    b08 *= invdet;
621    b09 *= invdet;
622    b10 *= invdet;
623    b11 *= invdet;
624
625    inverse->fMat[0][0] = SkDoubleToMScalar(a11 * b11 - a12 * b10 + a13 * b09);
626    inverse->fMat[0][1] = SkDoubleToMScalar(a02 * b10 - a01 * b11 - a03 * b09);
627    inverse->fMat[0][2] = SkDoubleToMScalar(a31 * b05 - a32 * b04 + a33 * b03);
628    inverse->fMat[0][3] = SkDoubleToMScalar(a22 * b04 - a21 * b05 - a23 * b03);
629    inverse->fMat[1][0] = SkDoubleToMScalar(a12 * b08 - a10 * b11 - a13 * b07);
630    inverse->fMat[1][1] = SkDoubleToMScalar(a00 * b11 - a02 * b08 + a03 * b07);
631    inverse->fMat[1][2] = SkDoubleToMScalar(a32 * b02 - a30 * b05 - a33 * b01);
632    inverse->fMat[1][3] = SkDoubleToMScalar(a20 * b05 - a22 * b02 + a23 * b01);
633    inverse->fMat[2][0] = SkDoubleToMScalar(a10 * b10 - a11 * b08 + a13 * b06);
634    inverse->fMat[2][1] = SkDoubleToMScalar(a01 * b08 - a00 * b10 - a03 * b06);
635    inverse->fMat[2][2] = SkDoubleToMScalar(a30 * b04 - a31 * b02 + a33 * b00);
636    inverse->fMat[2][3] = SkDoubleToMScalar(a21 * b02 - a20 * b04 - a23 * b00);
637    inverse->fMat[3][0] = SkDoubleToMScalar(a11 * b07 - a10 * b09 - a12 * b06);
638    inverse->fMat[3][1] = SkDoubleToMScalar(a00 * b09 - a01 * b07 + a02 * b06);
639    inverse->fMat[3][2] = SkDoubleToMScalar(a31 * b01 - a30 * b03 - a32 * b00);
640    inverse->fMat[3][3] = SkDoubleToMScalar(a20 * b03 - a21 * b01 + a22 * b00);
641    inverse->dirtyTypeMask();
642
643    return true;
644}
645
646///////////////////////////////////////////////////////////////////////////////
647
648void SkMatrix44::transpose() {
649    SkTSwap(fMat[0][1], fMat[1][0]);
650    SkTSwap(fMat[0][2], fMat[2][0]);
651    SkTSwap(fMat[0][3], fMat[3][0]);
652    SkTSwap(fMat[1][2], fMat[2][1]);
653    SkTSwap(fMat[1][3], fMat[3][1]);
654    SkTSwap(fMat[2][3], fMat[3][2]);
655
656    if (!this->isTriviallyIdentity()) {
657        this->dirtyTypeMask();
658    }
659}
660
661///////////////////////////////////////////////////////////////////////////////
662
663void SkMatrix44::mapScalars(const SkScalar src[4], SkScalar dst[4]) const {
664    SkScalar storage[4];
665    SkScalar* result = (src == dst) ? storage : dst;
666
667    for (int i = 0; i < 4; i++) {
668        SkMScalar value = 0;
669        for (int j = 0; j < 4; j++) {
670            value += fMat[j][i] * src[j];
671        }
672        result[i] = SkMScalarToScalar(value);
673    }
674
675    if (storage == result) {
676        memcpy(dst, storage, sizeof(storage));
677    }
678}
679
680#ifdef SK_MSCALAR_IS_DOUBLE
681
682void SkMatrix44::mapMScalars(const SkMScalar src[4], SkMScalar dst[4]) const {
683    SkMScalar storage[4];
684    SkMScalar* result = (src == dst) ? storage : dst;
685
686    for (int i = 0; i < 4; i++) {
687        SkMScalar value = 0;
688        for (int j = 0; j < 4; j++) {
689            value += fMat[j][i] * src[j];
690        }
691        result[i] = value;
692    }
693
694    if (storage == result) {
695        memcpy(dst, storage, sizeof(storage));
696    }
697}
698
699#endif
700
701typedef void (*Map2Procf)(const SkMScalar mat[][4], const float src2[], int count, float dst4[]);
702typedef void (*Map2Procd)(const SkMScalar mat[][4], const double src2[], int count, double dst4[]);
703
704static void map2_if(const SkMScalar mat[][4], const float* SK_RESTRICT src2,
705                    int count, float* SK_RESTRICT dst4) {
706    for (int i = 0; i < count; ++i) {
707        dst4[0] = src2[0];
708        dst4[1] = src2[1];
709        dst4[2] = 0;
710        dst4[3] = 1;
711        src2 += 2;
712        dst4 += 4;
713    }
714}
715
716static void map2_id(const SkMScalar mat[][4], const double* SK_RESTRICT src2,
717                    int count, double* SK_RESTRICT dst4) {
718    for (int i = 0; i < count; ++i) {
719        dst4[0] = src2[0];
720        dst4[1] = src2[1];
721        dst4[2] = 0;
722        dst4[3] = 1;
723        src2 += 2;
724        dst4 += 4;
725    }
726}
727
728static void map2_tf(const SkMScalar mat[][4], const float* SK_RESTRICT src2,
729                    int count, float* SK_RESTRICT dst4) {
730    const float mat30 = SkMScalarToFloat(mat[3][0]);
731    const float mat31 = SkMScalarToFloat(mat[3][1]);
732    const float mat32 = SkMScalarToFloat(mat[3][2]);
733    for (int n = 0; n < count; ++n) {
734        dst4[0] = src2[0] + mat30;
735        dst4[1] = src2[1] + mat31;
736        dst4[2] = mat32;
737        dst4[3] = 1;
738        src2 += 2;
739        dst4 += 4;
740    }
741}
742
743static void map2_td(const SkMScalar mat[][4], const double* SK_RESTRICT src2,
744                    int count, double* SK_RESTRICT dst4) {
745    for (int n = 0; n < count; ++n) {
746        dst4[0] = src2[0] + mat[3][0];
747        dst4[1] = src2[1] + mat[3][1];
748        dst4[2] = mat[3][2];
749        dst4[3] = 1;
750        src2 += 2;
751        dst4 += 4;
752    }
753}
754
755static void map2_sf(const SkMScalar mat[][4], const float* SK_RESTRICT src2,
756                    int count, float* SK_RESTRICT dst4) {
757    const float mat32 = SkMScalarToFloat(mat[3][2]);
758    for (int n = 0; n < count; ++n) {
759        dst4[0] = SkMScalarToFloat(mat[0][0] * src2[0] + mat[3][0]);
760        dst4[1] = SkMScalarToFloat(mat[1][1] * src2[1] + mat[3][1]);
761        dst4[2] = mat32;
762        dst4[3] = 1;
763        src2 += 2;
764        dst4 += 4;
765    }
766}
767
768static void map2_sd(const SkMScalar mat[][4], const double* SK_RESTRICT src2,
769                    int count, double* SK_RESTRICT dst4) {
770    for (int n = 0; n < count; ++n) {
771        dst4[0] = mat[0][0] * src2[0] + mat[3][0];
772        dst4[1] = mat[1][1] * src2[1] + mat[3][1];
773        dst4[2] = mat[3][2];
774        dst4[3] = 1;
775        src2 += 2;
776        dst4 += 4;
777    }
778}
779
780static void map2_af(const SkMScalar mat[][4], const float* SK_RESTRICT src2,
781                    int count, float* SK_RESTRICT dst4) {
782    SkMScalar r;
783    for (int n = 0; n < count; ++n) {
784        SkMScalar sx = SkFloatToMScalar(src2[0]);
785        SkMScalar sy = SkFloatToMScalar(src2[1]);
786        r = mat[0][0] * sx + mat[1][0] * sy + mat[3][0];
787        dst4[0] = SkMScalarToFloat(r);
788        r = mat[0][1] * sx + mat[1][1] * sy + mat[3][1];
789        dst4[1] = SkMScalarToFloat(r);
790        r = mat[0][2] * sx + mat[1][2] * sy + mat[3][2];
791        dst4[2] = SkMScalarToFloat(r);
792        dst4[3] = 1;
793        src2 += 2;
794        dst4 += 4;
795    }
796}
797
798static void map2_ad(const SkMScalar mat[][4], const double* SK_RESTRICT src2,
799                    int count, double* SK_RESTRICT dst4) {
800    for (int n = 0; n < count; ++n) {
801        double sx = src2[0];
802        double sy = src2[1];
803        dst4[0] = mat[0][0] * sx + mat[1][0] * sy + mat[3][0];
804        dst4[1] = mat[0][1] * sx + mat[1][1] * sy + mat[3][1];
805        dst4[2] = mat[0][2] * sx + mat[1][2] * sy + mat[3][2];
806        dst4[3] = 1;
807        src2 += 2;
808        dst4 += 4;
809    }
810}
811
812static void map2_pf(const SkMScalar mat[][4], const float* SK_RESTRICT src2,
813                    int count, float* SK_RESTRICT dst4) {
814    SkMScalar r;
815    for (int n = 0; n < count; ++n) {
816        SkMScalar sx = SkFloatToMScalar(src2[0]);
817        SkMScalar sy = SkFloatToMScalar(src2[1]);
818        for (int i = 0; i < 4; i++) {
819            r = mat[0][i] * sx + mat[1][i] * sy + mat[3][i];
820            dst4[i] = SkMScalarToFloat(r);
821        }
822        src2 += 2;
823        dst4 += 4;
824    }
825}
826
827static void map2_pd(const SkMScalar mat[][4], const double* SK_RESTRICT src2,
828                    int count, double* SK_RESTRICT dst4) {
829    for (int n = 0; n < count; ++n) {
830        double sx = src2[0];
831        double sy = src2[1];
832        for (int i = 0; i < 4; i++) {
833            dst4[i] = mat[0][i] * sx + mat[1][i] * sy + mat[3][i];
834        }
835        src2 += 2;
836        dst4 += 4;
837    }
838}
839
840void SkMatrix44::map2(const float src2[], int count, float dst4[]) const {
841    static const Map2Procf gProc[] = {
842        map2_if, map2_tf, map2_sf, map2_sf, map2_af, map2_af, map2_af, map2_af
843    };
844
845    TypeMask mask = this->getType();
846    Map2Procf proc = (mask & kPerspective_Mask) ? map2_pf : gProc[mask];
847    proc(fMat, src2, count, dst4);
848}
849
850void SkMatrix44::map2(const double src2[], int count, double dst4[]) const {
851    static const Map2Procd gProc[] = {
852        map2_id, map2_td, map2_sd, map2_sd, map2_ad, map2_ad, map2_ad, map2_ad
853    };
854
855    TypeMask mask = this->getType();
856    Map2Procd proc = (mask & kPerspective_Mask) ? map2_pd : gProc[mask];
857    proc(fMat, src2, count, dst4);
858}
859
860///////////////////////////////////////////////////////////////////////////////
861
862void SkMatrix44::dump() const {
863    static const char* format =
864        "[%g %g %g %g][%g %g %g %g][%g %g %g %g][%g %g %g %g]\n";
865#if 0
866    SkDebugf(format,
867             fMat[0][0], fMat[1][0], fMat[2][0], fMat[3][0],
868             fMat[0][1], fMat[1][1], fMat[2][1], fMat[3][1],
869             fMat[0][2], fMat[1][2], fMat[2][2], fMat[3][2],
870             fMat[0][3], fMat[1][3], fMat[2][3], fMat[3][3]);
871#else
872    SkDebugf(format,
873             fMat[0][0], fMat[0][1], fMat[0][2], fMat[0][3],
874             fMat[1][0], fMat[1][1], fMat[1][2], fMat[1][3],
875             fMat[2][0], fMat[2][1], fMat[2][2], fMat[2][3],
876             fMat[3][0], fMat[3][1], fMat[3][2], fMat[3][3]);
877#endif
878}
879
880///////////////////////////////////////////////////////////////////////////////
881
882static void initFromMatrix(SkMScalar dst[4][4], const SkMatrix& src) {
883    dst[0][0] = SkScalarToMScalar(src[SkMatrix::kMScaleX]);
884    dst[1][0] = SkScalarToMScalar(src[SkMatrix::kMSkewX]);
885    dst[2][0] = 0;
886    dst[3][0] = SkScalarToMScalar(src[SkMatrix::kMTransX]);
887    dst[0][1] = SkScalarToMScalar(src[SkMatrix::kMSkewY]);
888    dst[1][1] = SkScalarToMScalar(src[SkMatrix::kMScaleY]);
889    dst[2][1] = 0;
890    dst[3][1] = SkScalarToMScalar(src[SkMatrix::kMTransY]);
891    dst[0][2] = 0;
892    dst[1][2] = 0;
893    dst[2][2] = 1;
894    dst[3][2] = 0;
895    dst[0][3] = SkScalarToMScalar(src[SkMatrix::kMPersp0]);
896    dst[1][3] = SkScalarToMScalar(src[SkMatrix::kMPersp1]);
897    dst[2][3] = 0;
898    dst[3][3] = SkScalarToMScalar(src[SkMatrix::kMPersp2]);
899}
900
901SkMatrix44::SkMatrix44(const SkMatrix& src) {
902    initFromMatrix(fMat, src);
903}
904
905SkMatrix44& SkMatrix44::operator=(const SkMatrix& src) {
906    initFromMatrix(fMat, src);
907
908    if (src.isIdentity()) {
909        this->setTypeMask(kIdentity_Mask);
910    } else {
911        this->dirtyTypeMask();
912    }
913    return *this;
914}
915
916SkMatrix44::operator SkMatrix() const {
917    SkMatrix dst;
918
919    dst[SkMatrix::kMScaleX]  = SkMScalarToScalar(fMat[0][0]);
920    dst[SkMatrix::kMSkewX]  = SkMScalarToScalar(fMat[1][0]);
921    dst[SkMatrix::kMTransX] = SkMScalarToScalar(fMat[3][0]);
922
923    dst[SkMatrix::kMSkewY]  = SkMScalarToScalar(fMat[0][1]);
924    dst[SkMatrix::kMScaleY] = SkMScalarToScalar(fMat[1][1]);
925    dst[SkMatrix::kMTransY] = SkMScalarToScalar(fMat[3][1]);
926
927    dst[SkMatrix::kMPersp0] = SkMScalarToScalar(fMat[0][3]);
928    dst[SkMatrix::kMPersp1] = SkMScalarToScalar(fMat[1][3]);
929    dst[SkMatrix::kMPersp2] = SkMScalarToScalar(fMat[3][3]);
930
931    return dst;
932}
933