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 "nr-matrix.h"
  15
  16
  17
  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];
  32
  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     }
  50
  51     return d;
  52 }
  53
  54
  55
  56
  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)) {
  66
  67             NR::Coord const idet = 1.0 / det;
  68             NR::Coord *dest = d->c;
  69
  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]);
  73
  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];
  80
  81         } else {
  82             nr_matrix_set_identity(d);
  83         }
  84     } else {
  85         nr_matrix_set_identity(d);
  86     }
  87
  88     return d;
  89 }
  90
  91
  92
  93
  94
  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)
 100 {
 101     NR::Coord *dest = m->c;
 102
 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
 109
 110     return m;
 111 }
 112
 113
 114
 115
 116
 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)
 122 {
 123     NR::Coord *dest = m->c;
 124
 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
 131
 132     return m;
 133 }
 134
 135
 136
 137
 138
 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)
 144 {
 145     NR::Coord const s = sin(theta);
 146     NR::Coord const c = cos(theta);
 147
 148     NR::Coord *dest = m->c;
 149
 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
 156
 157     return m;
 158 }
 159
 160
 161
 162
 163
 164
 165
 166
 167
 168 /**
 169  *  Implement NR functions and methods
 170  */
 171 namespace NR {
 172
 173
 174
 175
 176
 177 /**
 178  *  Constructor.  Assign to nr if not null, else identity
 179  */
 180 Matrix::Matrix(NRMatrix const *nr)
 181 {
 182     if (nr) {
 183         assign(nr->c);
 184     } else {
 185         set_identity();
 186     }
 187 }
 188
 189
 190
 191
 192
 193 /**
 194  *  Multiply two matrices together
 195  */
 196 Matrix operator*(Matrix const &m0, Matrix const &m1)
 197 {
 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];
 204
 205     Matrix ret( d0, d1, d2, d3, d4, d5 );
 206
 207     return ret;
 208 }
 209
 210
 211
 212
 213
 214 /**
 215  *  Multiply a matrix by another
 216  */
 217 Matrix &Matrix::operator*=(Matrix const &o)
 218 {
 219     *this = *this * o;
 220     return *this;
 221 }
 222
 223
 224
 225
 226
 227 /**
 228  *  Multiply by a scaling matrix
 229  */
 230 Matrix &Matrix::operator*=(scale const &other)
 231 {
 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      */
 243
 244     NR::Coord const xscale = other[0];
 245     NR::Coord const yscale = other[1];
 246     NR::Coord *dest = _c;
 247
 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;
 254
 255     return *this;
 256 }
 257
 258
 259
 260
 261
 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
 267 {
 268     Matrix d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 269
 270     NR::Coord const det = _c[0] * _c[3] - _c[1] * _c[2];
 271     if (!NR_DF_TEST_CLOSE(det, 0.0, NR_EPSILON)) {
 272
 273         NR::Coord const idet = 1.0 / det;
 274         NR::Coord *dest = d._c;
 275
 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];
 282
 283     } else {
 284         d.set_identity();
 285     }
 286
 287     return d;
 288 }
 289
 290
 291
 292
 293
 294 /**
 295  *  Set this matrix to Identity
 296  */
 297 void Matrix::set_identity()
 298 {
 299     NR::Coord *dest = _c;
 300
 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
 308 }
 309
 310
 311
 312
 313
 314 /**
 315  *  return an Identity matrix
 316  */
 317 Matrix identity()
 318 {
 319     Matrix ret(1.0, 0.0,
 320                0.0, 1.0,
 321                0.0, 0.0);
 322     return ret;
 323 }
 324
 325
 326
 327
 328
 329 /**
 330  *
 331  */
 332 Matrix from_basis(Point const x_basis, Point const y_basis, Point const offset)
 333 {
 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;
 338 }
 339
 340
 341
 342
 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))
 356 {
 357 }
 358
 359
 360
 361
 362
 363 /**
 364  *  Return the determinant of the Matrix
 365  */
 366 NR::Coord Matrix::det() const
 367 {
 368     return _c[0] * _c[3] - _c[1] * _c[2];
 369 }
 370
 371
 372
 373
 374
 375 /**
 376  * Return the scalar of the descriminant of the Matrix
 377  */
 378 NR::Coord Matrix::descrim2() const
 379 {
 380     return fabs(det());
 381 }
 382
 383
 384
 385
 386
 387 /**
 388  *  Return the descriminant of the Matrix
 389  */
 390 NR::Coord Matrix::descrim() const
 391 {
 392     return sqrt(descrim2());
 393 }
 394
 395
 396
 397
 398
 399 /**
 400  *  Assign a matrix to a given coordinate array
 401  */
 402 Matrix &Matrix::assign(Coord const *array)
 403 {
 404     assert(array != NULL);
 405
 406     Coord const *src = array;
 407     Coord *dest = _c;
 408
 409     *dest++ = *src++;  //0
 410     *dest++ = *src++;  //1
 411     *dest++ = *src++;  //2
 412     *dest++ = *src++;  //3
 413     *dest++ = *src++;  //4
 414     *dest   = *src  ;  //5
 415
 416     return *this;
 417 }
 418
 419
 420
 421
 422
 423 /**
 424  *  Copy this matrix's value to a NRMatrix
 425  */
 426 NRMatrix *Matrix::copyto(NRMatrix *nrm) const {
 427
 428     assert(nrm != NULL);
 429
 430     Coord const *src = _c;
 431     Coord *dest = nrm->c;
 432
 433     *dest++ = *src++;  //0
 434     *dest++ = *src++;  //1
 435     *dest++ = *src++;  //2
 436     *dest++ = *src++;  //3
 437     *dest++ = *src++;  //4
 438     *dest   = *src  ;  //5
 439
 440     return nrm;
 441 }
 442
 443
 444
 445
 446 /**
 447  *  Copy this matrix's values to an array
 448  */
 449 NR::Coord *Matrix::copyto(NR::Coord *array) const {
 450
 451     assert(array != NULL);
 452
 453     Coord const *src = _c;
 454     Coord *dest = array;
 455
 456     *dest++ = *src++;  //0
 457     *dest++ = *src++;  //1
 458     *dest++ = *src++;  //2
 459     *dest++ = *src++;  //3
 460     *dest++ = *src++;  //4
 461     *dest   = *src  ;  //5
 462
 463     return array;
 464 }
 465
 466
 467
 468
 469
 470 /**
 471  *
 472  */
 473 double expansion(Matrix const &m) {
 474     return sqrt(fabs(m.det()));
 475 }
 476
 477
 478
 479
 480
 481 /**
 482  *
 483  */
 484 double Matrix::expansion() const {
 485     return sqrt(fabs(det()));
 486 }
 487
 488
 489
 490
 491
 492 /**
 493  *
 494  */
 495 double Matrix::expansionX() const {
 496     return sqrt(_c[0] * _c[0] + _c[1] * _c[1]);
 497 }
 498
 499
 500
 501
 502
 503 /**
 504  *
 505  */
 506 double Matrix::expansionY() const {
 507     return sqrt(_c[2] * _c[2] + _c[3] * _c[3]);
 508 }
 509
 510
 511
 512
 513
 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   );
 522 }
 523
 524
 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   );
 532 }
 533
 534
 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);
 542 }
 543
 544
 545
 546
 547
 548 /**
 549  *
 550  */
 551 bool Matrix::test_identity() const {
 552     return NR_MATRIX_DF_TEST_CLOSE(this, &NR_MATRIX_IDENTITY, NR_EPSILON);
 553 }
 554
 555
 556
 557
 558
 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);
 564 }
 565
 566
 567
 568
 569
 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);
 575 }
 576
 577
 578
 579
 580
 581 /**
 582  *
 583  */
 584 bool matrix_equalp(Matrix const &m0, Matrix const &m1, NR::Coord const epsilon)
 585 {
 586     return ( NR_MATRIX_DF_TEST_TRANSFORM_CLOSE(&m0, &m1, epsilon) &&
 587              NR_MATRIX_DF_TEST_TRANSLATE_CLOSE(&m0, &m1, epsilon)   );
 588 }
 589
 590
 591
 592
 593
 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)
 599 {
 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     }
 610 }
 611
 612
 613
 614 }  //namespace NR
 615
 616
 617
 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 :