src/libnr/nr-matrix.cpp

   1 #define __NR_MATRIX_C__
   2
   3 /** \file
   4  * Various matrix routines.  Currently includes some NR::rotate etc. routines too.
   5  */
   6
   7 /*
   8  * Authors:
   9  *   Lauris Kaplinski <lauris@kaplinski.com>
  10  *
  11  * This code is in public domain
  12  */
  13
  14 #include <cstdlib>
  15 #include "nr-matrix.h"
  16
  17
  18
  19 /**
  20  *  Multiply two NRMatrices together, storing the result in d.
  21  */
  22 NRMatrix *
  23 nr_matrix_multiply(NRMatrix *d, NRMatrix const *m0, NRMatrix const *m1)
  24 {
  25     if (m0) {
  26         if (m1) {
  27             NR::Coord d0 = m0->c[0] * m1->c[0]  +  m0->c[1] * m1->c[2];
  28             NR::Coord d1 = m0->c[0] * m1->c[1]  +  m0->c[1] * m1->c[3];
  29             NR::Coord d2 = m0->c[2] * m1->c[0]  +  m0->c[3] * m1->c[2];
  30             NR::Coord d3 = m0->c[2] * m1->c[1]  +  m0->c[3] * m1->c[3];
  31             NR::Coord d4 = m0->c[4] * m1->c[0]  +  m0->c[5] * m1->c[2]  +  m1->c[4];
  32             NR::Coord d5 = m0->c[4] * m1->c[1]  +  m0->c[5] * m1->c[3]  +  m1->c[5];
  33
  34             NR::Coord *dest = d->c;
  35             *dest++ = d0;
  36             *dest++ = d1;
  37             *dest++ = d2;
  38             *dest++ = d3;
  39             *dest++ = d4;
  40             *dest   = d5;
  41         } else {
  42             *d = *m0;
  43         }
  44     } else {
  45         if (m1) {
  46             *d = *m1;
  47         } else {
  48             nr_matrix_set_identity(d);
  49         }
  50     }
  51
  52     return d;
  53 }
  54
  55
  56
  57
  58 /**
  59  *  Store the inverted value of Matrix m in d
  60  */
  61 NRMatrix *
  62 nr_matrix_invert(NRMatrix *d, NRMatrix const *m)
  63 {
  64     if (m) {
  65         NR::Coord const det = m->c[0] * m->c[3] - m->c[1] * m->c[2];
  66         if (!NR_DF_TEST_CLOSE(det, 0.0, NR_EPSILON)) {
  67
  68             NR::Coord const idet = 1.0 / det;
  69             NR::Coord *dest = d->c;
  70
  71             /* Cache m->c[0] and m->c[4] in case d == m. */
  72             NR::Coord const m_c0(m->c[0]);
  73             NR::Coord const m_c4(m->c[4]);
  74
  75             /*0*/ *dest++ =  m->c[3] * idet;
  76             /*1*/ *dest++ = -m->c[1] * idet;
  77             /*2*/ *dest++ = -m->c[2] * idet;
  78             /*3*/ *dest++ =   m_c0   * idet;
  79             /*4*/ *dest++ = -m_c4 * d->c[0] - m->c[5] * d->c[2];
  80             /*5*/ *dest   = -m_c4 * d->c[1] - m->c[5] * d->c[3];
  81
  82         } else {
  83             nr_matrix_set_identity(d);
  84         }
  85     } else {
  86         nr_matrix_set_identity(d);
  87     }
  88
  89     return d;
  90 }
  91
  92
  93
  94
  95
  96 /**
  97  *  Set this matrix to a translation of x and y
  98  */
  99 NRMatrix *
 100 nr_matrix_set_translate(NRMatrix *m, NR::Coord const x, NR::Coord const y)
 101 {
 102     NR::Coord *dest = m->c;
 103
 104     *dest++ = 1.0;   //0
 105     *dest++ = 0.0;   //1
 106     *dest++ = 0.0;   //2
 107     *dest++ = 1.0;   //3
 108     *dest++ =   x;   //4
 109     *dest   =   y;   //5
 110
 111     return m;
 112 }
 113
 114
 115
 116
 117
 118 /**
 119  *  Set this matrix to a scaling transform in sx and sy
 120  */
 121 NRMatrix *
 122 nr_matrix_set_scale(NRMatrix *m, NR::Coord const sx, NR::Coord const sy)
 123 {
 124     NR::Coord *dest = m->c;
 125
 126     *dest++ =  sx;   //0
 127     *dest++ = 0.0;   //1
 128     *dest++ = 0.0;   //2
 129     *dest++ =  sy;   //3
 130     *dest++ = 0.0;   //4
 131     *dest   = 0.0;   //5
 132
 133     return m;
 134 }
 135
 136
 137
 138
 139
 140 /**
 141  *  Set this matrix to a rotating transform of angle 'theta' radians
 142  */
 143 NRMatrix *
 144 nr_matrix_set_rotate(NRMatrix *m, NR::Coord const theta)
 145 {
 146     NR::Coord const s = sin(theta);
 147     NR::Coord const c = cos(theta);
 148
 149     NR::Coord *dest = m->c;
 150
 151     *dest++ =   c;   //0
 152     *dest++ =   s;   //1
 153     *dest++ =  -s;   //2
 154     *dest++ =   c;   //3
 155     *dest++ = 0.0;   //4
 156     *dest   = 0.0;   //5
 157
 158     return m;
 159 }
 160
 161
 162
 163
 164
 165
 166
 167
 168
 169 /**
 170  *  Implement NR functions and methods
 171  */
 172 namespace NR {
 173
 174
 175
 176
 177
 178 /**
 179  *  Constructor.  Assign to nr if not null, else identity
 180  */
 181 Matrix::Matrix(NRMatrix const *nr)
 182 {
 183     if (nr) {
 184         assign(nr->c);
 185     } else {
 186         set_identity();
 187     }
 188 }
 189
 190
 191
 192
 193
 194 /**
 195  *  Multiply two matrices together
 196  */
 197 Matrix operator*(Matrix const &m0, Matrix const &m1)
 198 {
 199     NR::Coord const d0 = m0[0] * m1[0]  +  m0[1] * m1[2];
 200     NR::Coord const d1 = m0[0] * m1[1]  +  m0[1] * m1[3];
 201     NR::Coord const d2 = m0[2] * m1[0]  +  m0[3] * m1[2];
 202     NR::Coord const d3 = m0[2] * m1[1]  +  m0[3] * m1[3];
 203     NR::Coord const d4 = m0[4] * m1[0]  +  m0[5] * m1[2]  +  m1[4];
 204     NR::Coord const d5 = m0[4] * m1[1]  +  m0[5] * m1[3]  +  m1[5];
 205
 206     Matrix ret( d0, d1, d2, d3, d4, d5 );
 207
 208     return ret;
 209 }
 210
 211
 212
 213
 214
 215 /**
 216  *  Multiply a matrix by another
 217  */
 218 Matrix &Matrix::operator*=(Matrix const &o)
 219 {
 220     *this = *this * o;
 221     return *this;
 222 }
 223
 224
 225
 226
 227
 228 /**
 229  *  Multiply by a scaling matrix
 230  */
 231 Matrix &Matrix::operator*=(scale const &other)
 232 {
 233     /* This loop is massive overkill.  Let's unroll.
 234      *   o    _c[] goes from 0..5
 235      *   o    other[] alternates between 0 and 1
 236      */
 237     /*
 238      * for (unsigned i = 0; i < 3; ++i) {
 239      *     for (unsigned j = 0; j < 2; ++j) {
 240      *         this->_c[i * 2 + j] *= other[j];
 241      *     }
 242      * }
 243      */
 244
 245     NR::Coord const xscale = other[0];
 246     NR::Coord const yscale = other[1];
 247     NR::Coord *dest = _c;
 248
 249     /*i=0 j=0*/  *dest++ *= xscale;
 250     /*i=0 j=1*/  *dest++ *= yscale;
 251     /*i=1 j=0*/  *dest++ *= xscale;
 252     /*i=1 j=1*/  *dest++ *= yscale;
 253     /*i=2 j=0*/  *dest++ *= xscale;
 254     /*i=2 j=1*/  *dest   *= yscale;
 255
 256     return *this;
 257 }
 258
 259
 260
 261
 262
 263 /**
 264  *  Return the inverse of this matrix.  If an inverse is not defined,
 265  *  then return the identity matrix.
 266  */
 267 Matrix Matrix::inverse() const
 268 {
 269     Matrix d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 270
 271     NR::Coord const det = _c[0] * _c[3] - _c[1] * _c[2];
 272     if (!NR_DF_TEST_CLOSE(det, 0.0, NR_EPSILON)) {
 273
 274         NR::Coord const idet = 1.0 / det;
 275         NR::Coord *dest = d._c;
 276
 277         /*0*/ *dest++ =  _c[3] * idet;
 278         /*1*/ *dest++ = -_c[1] * idet;
 279         /*2*/ *dest++ = -_c[2] * idet;
 280         /*3*/ *dest++ =  _c[0] * idet;
 281         /*4*/ *dest++ = -_c[4] * d._c[0] - _c[5] * d._c[2];
 282         /*5*/ *dest   = -_c[4] * d._c[1] - _c[5] * d._c[3];
 283
 284     } else {
 285         d.set_identity();
 286     }
 287
 288     return d;
 289 }
 290
 291
 292
 293
 294
 295 /**
 296  *  Set this matrix to Identity
 297  */
 298 void Matrix::set_identity()
 299 {
 300     NR::Coord *dest = _c;
 301
 302     *dest++ = 1.0; //0
 303     *dest++ = 0.0; //1
 304     *dest++ = 0.0; //2
 305     *dest++ = 1.0; //3
 306     // translation
 307     *dest++ = 0.0; //4
 308     *dest   = 0.0; //5
 309 }
 310
 311
 312
 313
 314
 315 /**
 316  *  return an Identity matrix
 317  */
 318 Matrix identity()
 319 {
 320     Matrix ret(1.0, 0.0,
 321                0.0, 1.0,
 322                0.0, 0.0);
 323     return ret;
 324 }
 325
 326
 327
 328
 329
 330 /**
 331  *
 332  */
 333 Matrix from_basis(Point const x_basis, Point const y_basis, Point const offset)
 334 {
 335     Matrix const ret(x_basis[X], y_basis[X],
 336                      x_basis[Y], y_basis[Y],
 337                      offset[X], offset[Y]);
 338     return ret;
 339 }
 340
 341
 342
 343
 344 /**
 345  * Returns a rotation matrix corresponding by the specified angle (in radians) about the origin.
 346  *
 347  * \see NR::rotate_degrees
 348  *
 349  * Angle direction in Inkscape code: If you use the traditional mathematics convention that y
 350  * increases upwards, then positive angles are anticlockwise as per the mathematics convention.  If
 351  * you take the common non-mathematical convention that y increases downwards, then positive angles
 352  * are clockwise, as is common outside of mathematics.
 353  */
 354 rotate::rotate(NR::Coord const theta) :
 355     vec(cos(theta),
 356         sin(theta))
 357 {
 358 }
 359
 360
 361
 362
 363
 364 /**
 365  *  Return the determinant of the Matrix
 366  */
 367 NR::Coord Matrix::det() const
 368 {
 369     return _c[0] * _c[3] - _c[1] * _c[2];
 370 }
 371
 372
 373
 374
 375
 376 /**
 377  * Return the scalar of the descriminant of the Matrix
 378  */
 379 NR::Coord Matrix::descrim2() const
 380 {
 381     return fabs(det());
 382 }
 383
 384
 385
 386
 387
 388 /**
 389  *  Return the descriminant of the Matrix
 390  */
 391 NR::Coord Matrix::descrim() const
 392 {
 393     return sqrt(descrim2());
 394 }
 395
 396
 397
 398
 399
 400 /**
 401  *  Assign a matrix to a given coordinate array
 402  */
 403 Matrix &Matrix::assign(Coord const *array)
 404 {
 405     assert(array != NULL);
 406
 407     Coord const *src = array;
 408     Coord *dest = _c;
 409
 410     *dest++ = *src++;  //0
 411     *dest++ = *src++;  //1
 412     *dest++ = *src++;  //2
 413     *dest++ = *src++;  //3
 414     *dest++ = *src++;  //4
 415     *dest   = *src  ;  //5
 416
 417     return *this;
 418 }
 419
 420
 421
 422
 423
 424 /**
 425  *  Copy this matrix's value to a NRMatrix
 426  */
 427 NRMatrix *Matrix::copyto(NRMatrix *nrm) const {
 428
 429     assert(nrm != NULL);
 430
 431     Coord const *src = _c;
 432     Coord *dest = nrm->c;
 433
 434     *dest++ = *src++;  //0
 435     *dest++ = *src++;  //1
 436     *dest++ = *src++;  //2
 437     *dest++ = *src++;  //3
 438     *dest++ = *src++;  //4
 439     *dest   = *src  ;  //5
 440
 441     return nrm;
 442 }
 443
 444
 445
 446
 447 /**
 448  *  Copy this matrix's values to an array
 449  */
 450 NR::Coord *Matrix::copyto(NR::Coord *array) const {
 451
 452     assert(array != NULL);
 453
 454     Coord const *src = _c;
 455     Coord *dest = array;
 456
 457     *dest++ = *src++;  //0
 458     *dest++ = *src++;  //1
 459     *dest++ = *src++;  //2
 460     *dest++ = *src++;  //3
 461     *dest++ = *src++;  //4
 462     *dest   = *src  ;  //5
 463
 464     return array;
 465 }
 466
 467
 468
 469
 470
 471 /**
 472  *
 473  */
 474 double expansion(Matrix const &m) {
 475     return sqrt(fabs(m.det()));
 476 }
 477
 478
 479
 480
 481
 482 /**
 483  *
 484  */
 485 double Matrix::expansion() const {
 486     return sqrt(fabs(det()));
 487 }
 488
 489
 490
 491
 492
 493 /**
 494  *
 495  */
 496 double Matrix::expansionX() const {
 497     return sqrt(_c[0] * _c[0] + _c[1] * _c[1]);
 498 }
 499
 500
 501
 502
 503
 504 /**
 505  *
 506  */
 507 double Matrix::expansionY() const {
 508     return sqrt(_c[2] * _c[2] + _c[3] * _c[3]);
 509 }
 510
 511
 512
 513
 514
 515 /**
 516  *
 517  */
 518 bool Matrix::is_translation(Coord const eps) const {
 519     return ( fabs(_c[0] - 1.0) < eps &&
 520              fabs(_c[3] - 1.0) < eps &&
 521              fabs(_c[1])       < eps &&
 522              fabs(_c[2])       < eps   );
 523 }
 524
 525
 526 /**
 527  *
 528  */
 529 bool Matrix::is_scale(Coord const eps) const {
 530     return ( (fabs(_c[0] - 1.0) > eps || fabs(_c[3] - 1.0) > eps) &&
 531              fabs(_c[1])       < eps &&
 532              fabs(_c[2])       < eps   );
 533 }
 534
 535
 536 /**
 537  *
 538  */
 539 bool Matrix::is_rotation(Coord const eps) const {
 540     return ( fabs(_c[1]) > eps &&
 541              fabs(_c[2]) > eps &&
 542              fabs(_c[1] + _c[2]) < 2 * eps);
 543 }
 544
 545
 546
 547
 548
 549 /**
 550  *
 551  */
 552 bool Matrix::test_identity() const {
 553     return NR_MATRIX_DF_TEST_CLOSE(this, &NR_MATRIX_IDENTITY, NR_EPSILON);
 554 }
 555
 556
 557
 558
 559
 560 /**
 561  *
 562  */
 563 bool transform_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon) {
 564     return NR_MATRIX_DF_TEST_TRANSFORM_CLOSE(&m0, &m1, epsilon);
 565 }
 566
 567
 568
 569
 570
 571 /**
 572  *
 573  */
 574 bool translate_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon) {
 575     return NR_MATRIX_DF_TEST_TRANSLATE_CLOSE(&m0, &m1, epsilon);
 576 }
 577
 578
 579
 580
 581
 582 /**
 583  *
 584  */
 585 bool matrix_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon)
 586 {
 587     return ( NR_MATRIX_DF_TEST_TRANSFORM_CLOSE(&m0, &m1, epsilon) &&
 588              NR_MATRIX_DF_TEST_TRANSLATE_CLOSE(&m0, &m1, epsilon)   );
 589 }
 590
 591
 592
 593
 594
 595 /**
 596  *  A home-made assertion.  Stop if the two matrixes are not 'close' to
 597  *  each other.
 598  */
 599 void assert_close(Matrix const &a, Matrix const &b)
 600 {
 601     if (!matrix_equalp(a, b, 1e-3)) {
 602         fprintf(stderr,
 603                 "a = | %g %g |,\tb = | %g %g |\n"
 604                 "    | %g %g | \t    | %g %g |\n"
 605                 "    | %g %g | \t    | %g %g |\n",
 606                 a[0], a[1], b[0], b[1],
 607                 a[2], a[3], b[2], b[3],
 608                 a[4], a[5], b[4], b[5]);
 609         std::abort();
 610     }
 611 }
 612
 613
 614
 615 }  //namespace NR
 616
 617
 618
 619 /*
 620   Local Variables:
 621   mode:c++
 622   c-file-style:"stroustrup"
 623   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 624   indent-tabs-mode:nil
 625   fill-column:99
 626   End:
 627 */
 628 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :