Code

add methods to identify scales and rotations
[inkscape.git] / src / libnr / nr-matrix.cpp
1 #define __NR_MATRIX_C__
3 /** \file
4  * Various matrix routines.  Currently includes some NR::rotate etc. routines too.
5  */
7 /*
8  * Authors:
9  *   Lauris Kaplinski <lauris@kaplinski.com>
10  *
11  * This code is in public domain
12  */
14 #include "nr-matrix.h"
18 /**
19  *  Multiply two NRMatrices together, storing the result in d.
20  */
21 NRMatrix *
22 nr_matrix_multiply(NRMatrix *d, NRMatrix const *m0, NRMatrix const *m1)
23 {
24     if (m0) {
25         if (m1) {
26             NR::Coord d0 = m0->c[0] * m1->c[0]  +  m0->c[1] * m1->c[2];
27             NR::Coord d1 = m0->c[0] * m1->c[1]  +  m0->c[1] * m1->c[3];
28             NR::Coord d2 = m0->c[2] * m1->c[0]  +  m0->c[3] * m1->c[2];
29             NR::Coord d3 = m0->c[2] * m1->c[1]  +  m0->c[3] * m1->c[3];
30             NR::Coord d4 = m0->c[4] * m1->c[0]  +  m0->c[5] * m1->c[2]  +  m1->c[4];
31             NR::Coord d5 = m0->c[4] * m1->c[1]  +  m0->c[5] * m1->c[3]  +  m1->c[5];
33             NR::Coord *dest = d->c;
34             *dest++ = d0;
35             *dest++ = d1;
36             *dest++ = d2;
37             *dest++ = d3;
38             *dest++ = d4;
39             *dest   = d5;
40         } else {
41             *d = *m0;
42         }
43     } else {
44         if (m1) {
45             *d = *m1;
46         } else {
47             nr_matrix_set_identity(d);
48         }
49     }
51     return d;
52 }
57 /**
58  *  Store the inverted value of Matrix m in d
59  */
60 NRMatrix *
61 nr_matrix_invert(NRMatrix *d, NRMatrix const *m)
62 {
63     if (m) {
64         NR::Coord const det = m->c[0] * m->c[3] - m->c[1] * m->c[2];
65         if (!NR_DF_TEST_CLOSE(det, 0.0, NR_EPSILON)) {
67             NR::Coord const idet = 1.0 / det;
68             NR::Coord *dest = d->c;
70             /* Cache m->c[0] and m->c[4] in case d == m. */
71             NR::Coord const m_c0(m->c[0]);
72             NR::Coord const m_c4(m->c[4]);
74             /*0*/ *dest++ =  m->c[3] * idet;
75             /*1*/ *dest++ = -m->c[1] * idet;
76             /*2*/ *dest++ = -m->c[2] * idet;
77             /*3*/ *dest++ =   m_c0   * idet;
78             /*4*/ *dest++ = -m_c4 * d->c[0] - m->c[5] * d->c[2];
79             /*5*/ *dest   = -m_c4 * d->c[1] - m->c[5] * d->c[3];
81         } else {
82             nr_matrix_set_identity(d);
83         }
84     } else {
85         nr_matrix_set_identity(d);
86     }
88     return d;
89 }
95 /**
96  *  Set this matrix to a translation of x and y
97  */
98 NRMatrix *
99 nr_matrix_set_translate(NRMatrix *m, NR::Coord const x, NR::Coord const y)
101     NR::Coord *dest = m->c;
103     *dest++ = 1.0;   //0
104     *dest++ = 0.0;   //1
105     *dest++ = 0.0;   //2
106     *dest++ = 1.0;   //3
107     *dest++ =   x;   //4
108     *dest   =   y;   //5
110     return m;
117 /**
118  *  Set this matrix to a scaling transform in sx and sy
119  */
120 NRMatrix *
121 nr_matrix_set_scale(NRMatrix *m, NR::Coord const sx, NR::Coord const sy)
123     NR::Coord *dest = m->c;
125     *dest++ =  sx;   //0
126     *dest++ = 0.0;   //1
127     *dest++ = 0.0;   //2
128     *dest++ =  sy;   //3
129     *dest++ = 0.0;   //4
130     *dest   = 0.0;   //5
132     return m;
139 /**
140  *  Set this matrix to a rotating transform of angle 'theta' radians
141  */
142 NRMatrix *
143 nr_matrix_set_rotate(NRMatrix *m, NR::Coord const theta)
145     NR::Coord const s = sin(theta);
146     NR::Coord const c = cos(theta);
148     NR::Coord *dest = m->c;
150     *dest++ =   c;   //0
151     *dest++ =   s;   //1
152     *dest++ =  -s;   //2
153     *dest++ =   c;   //3
154     *dest++ = 0.0;   //4
155     *dest   = 0.0;   //5
157     return m;
168 /**
169  *  Implement NR functions and methods
170  */
171 namespace NR {
177 /**
178  *  Constructor.  Assign to nr if not null, else identity
179  */
180 Matrix::Matrix(NRMatrix const *nr)
182     if (nr) {
183         assign(nr->c);
184     } else {
185         set_identity();
186     }
193 /**
194  *  Multiply two matrices together
195  */
196 Matrix operator*(Matrix const &m0, Matrix const &m1)
198     NR::Coord const d0 = m0[0] * m1[0]  +  m0[1] * m1[2];
199     NR::Coord const d1 = m0[0] * m1[1]  +  m0[1] * m1[3];
200     NR::Coord const d2 = m0[2] * m1[0]  +  m0[3] * m1[2];
201     NR::Coord const d3 = m0[2] * m1[1]  +  m0[3] * m1[3];
202     NR::Coord const d4 = m0[4] * m1[0]  +  m0[5] * m1[2]  +  m1[4];
203     NR::Coord const d5 = m0[4] * m1[1]  +  m0[5] * m1[3]  +  m1[5];
205     Matrix ret( d0, d1, d2, d3, d4, d5 );
207     return ret;
214 /**
215  *  Multiply a matrix by another
216  */
217 Matrix &Matrix::operator*=(Matrix const &o)
219     *this = *this * o;
220     return *this;
227 /**
228  *  Multiply by a scaling matrix
229  */
230 Matrix &Matrix::operator*=(scale const &other)
232     /* This loop is massive overkill.  Let's unroll.
233      *   o    _c[] goes from 0..5
234      *   o    other[] alternates between 0 and 1
235      */
236     /*
237      * for (unsigned i = 0; i < 3; ++i) {
238      *     for (unsigned j = 0; j < 2; ++j) {
239      *         this->_c[i * 2 + j] *= other[j];
240      *     }
241      * }
242      */
244     NR::Coord const xscale = other[0];
245     NR::Coord const yscale = other[1];
246     NR::Coord *dest = _c;
248     /*i=0 j=0*/  *dest++ *= xscale;
249     /*i=0 j=1*/  *dest++ *= yscale;
250     /*i=1 j=0*/  *dest++ *= xscale;
251     /*i=1 j=1*/  *dest++ *= yscale;
252     /*i=2 j=0*/  *dest++ *= xscale;
253     /*i=2 j=1*/  *dest   *= yscale;
255     return *this;
262 /**
263  *  Return the inverse of this matrix.  If an inverse is not defined,
264  *  then return the identity matrix.
265  */
266 Matrix Matrix::inverse() const
268     Matrix d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
270     NR::Coord const det = _c[0] * _c[3] - _c[1] * _c[2];
271     if (!NR_DF_TEST_CLOSE(det, 0.0, NR_EPSILON)) {
273         NR::Coord const idet = 1.0 / det;
274         NR::Coord *dest = d._c;
276         /*0*/ *dest++ =  _c[3] * idet;
277         /*1*/ *dest++ = -_c[1] * idet;
278         /*2*/ *dest++ = -_c[2] * idet;
279         /*3*/ *dest++ =  _c[0] * idet;
280         /*4*/ *dest++ = -_c[4] * d._c[0] - _c[5] * d._c[2];
281         /*5*/ *dest   = -_c[4] * d._c[1] - _c[5] * d._c[3];
283     } else {
284         d.set_identity();
285     }
287     return d;
294 /**
295  *  Set this matrix to Identity
296  */
297 void Matrix::set_identity()
299     NR::Coord *dest = _c;
301     *dest++ = 1.0; //0
302     *dest++ = 0.0; //1
303     *dest++ = 0.0; //2
304     *dest++ = 1.0; //3
305     // translation
306     *dest++ = 0.0; //4
307     *dest   = 0.0; //5
314 /**
315  *  return an Identity matrix
316  */
317 Matrix identity()
319     Matrix ret(1.0, 0.0,
320                0.0, 1.0,
321                0.0, 0.0);
322     return ret;
329 /**
330  *
331  */
332 Matrix from_basis(Point const x_basis, Point const y_basis, Point const offset)
334     Matrix const ret(x_basis[X], y_basis[X],
335                      x_basis[Y], y_basis[Y],
336                      offset[X], offset[Y]);
337     return ret;
343 /**
344  * Returns a rotation matrix corresponding by the specified angle (in radians) about the origin.
345  *
346  * \see NR::rotate_degrees
347  *
348  * Angle direction in Inkscape code: If you use the traditional mathematics convention that y
349  * increases upwards, then positive angles are anticlockwise as per the mathematics convention.  If
350  * you take the common non-mathematical convention that y increases downwards, then positive angles
351  * are clockwise, as is common outside of mathematics.
352  */
353 rotate::rotate(NR::Coord const theta) :
354     vec(cos(theta),
355         sin(theta))
363 /**
364  *  Return the determinant of the Matrix
365  */
366 NR::Coord Matrix::det() const
368     return _c[0] * _c[3] - _c[1] * _c[2];
375 /**
376  * Return the scalar of the descriminant of the Matrix
377  */
378 NR::Coord Matrix::descrim2() const
380     return fabs(det());
387 /**
388  *  Return the descriminant of the Matrix
389  */
390 NR::Coord Matrix::descrim() const
392     return sqrt(descrim2());
399 /**
400  *  Assign a matrix to a given coordinate array
401  */
402 Matrix &Matrix::assign(Coord const *array)
404     assert(array != NULL);
406     Coord const *src = array;
407     Coord *dest = _c;
409     *dest++ = *src++;  //0
410     *dest++ = *src++;  //1
411     *dest++ = *src++;  //2
412     *dest++ = *src++;  //3
413     *dest++ = *src++;  //4
414     *dest   = *src  ;  //5
416     return *this;
423 /**
424  *  Copy this matrix's value to a NRMatrix
425  */
426 NRMatrix *Matrix::copyto(NRMatrix *nrm) const {
428     assert(nrm != NULL);
430     Coord const *src = _c;
431     Coord *dest = nrm->c;
433     *dest++ = *src++;  //0
434     *dest++ = *src++;  //1
435     *dest++ = *src++;  //2
436     *dest++ = *src++;  //3
437     *dest++ = *src++;  //4
438     *dest   = *src  ;  //5
440     return nrm;
446 /**
447  *  Copy this matrix's values to an array
448  */
449 NR::Coord *Matrix::copyto(NR::Coord *array) const {
451     assert(array != NULL);
453     Coord const *src = _c;
454     Coord *dest = array;
456     *dest++ = *src++;  //0
457     *dest++ = *src++;  //1
458     *dest++ = *src++;  //2
459     *dest++ = *src++;  //3
460     *dest++ = *src++;  //4
461     *dest   = *src  ;  //5
463     return array;
470 /**
471  *
472  */
473 double expansion(Matrix const &m) {
474     return sqrt(fabs(m.det()));
481 /**
482  *
483  */
484 double Matrix::expansion() const {
485     return sqrt(fabs(det()));
492 /**
493  *
494  */
495 double Matrix::expansionX() const {
496     return sqrt(_c[0] * _c[0] + _c[1] * _c[1]);
503 /**
504  *
505  */
506 double Matrix::expansionY() const {
507     return sqrt(_c[2] * _c[2] + _c[3] * _c[3]);
514 /**
515  *
516  */
517 bool Matrix::is_translation(Coord const eps) const {
518     return ( fabs(_c[0] - 1.0) < eps &&
519              fabs(_c[3] - 1.0) < eps &&
520              fabs(_c[1])       < eps &&
521              fabs(_c[2])       < eps   );
525 /**
526  *
527  */
528 bool Matrix::is_scale(Coord const eps) const {
529     return ( (fabs(_c[0] - 1.0) > eps || fabs(_c[3] - 1.0) > eps) &&
530              fabs(_c[1])       < eps &&
531              fabs(_c[2])       < eps   );
535 /**
536  *
537  */
538 bool Matrix::is_rotation(Coord const eps) const {
539     return ( fabs(_c[1]) > eps &&
540              fabs(_c[2]) > eps &&
541              fabs(_c[1] + _c[2]) < 2 * eps);
548 /**
549  *
550  */
551 bool Matrix::test_identity() const {
552     return NR_MATRIX_DF_TEST_CLOSE(this, &NR_MATRIX_IDENTITY, NR_EPSILON);
559 /**
560  *
561  */
562 bool transform_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon) {
563     return NR_MATRIX_DF_TEST_TRANSFORM_CLOSE(&m0, &m1, epsilon);
570 /**
571  *
572  */
573 bool translate_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon) {
574     return NR_MATRIX_DF_TEST_TRANSLATE_CLOSE(&m0, &m1, epsilon);
581 /**
582  *
583  */
584 bool matrix_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon)
586     return ( NR_MATRIX_DF_TEST_TRANSFORM_CLOSE(&m0, &m1, epsilon) &&
587              NR_MATRIX_DF_TEST_TRANSLATE_CLOSE(&m0, &m1, epsilon)   );
594 /**
595  *  A home-made assertion.  Stop if the two matrixes are not 'close' to
596  *  each other.
597  */
598 void assert_close(Matrix const &a, Matrix const &b)
600     if (!matrix_equalp(a, b, 1e-3)) {
601         fprintf(stderr,
602                 "a = | %g %g |,\tb = | %g %g |\n"
603                 "    | %g %g | \t    | %g %g |\n"
604                 "    | %g %g | \t    | %g %g |\n",
605                 a[0], a[1], b[0], b[1],
606                 a[2], a[3], b[2], b[3],
607                 a[4], a[5], b[4], b[5]);
608         abort();
609     }
614 }  //namespace NR
618 /*
619   Local Variables:
620   mode:c++
621   c-file-style:"stroustrup"
622   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
623   indent-tabs-mode:nil
624   fill-column:99
625   End:
626 */
627 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :