1 #include "Matrix3by3.h" 2 #include "mathutilities.h" 3 #include "Report.h" 4 using namespace ProtoMol::Report; 5 6 namespace ProtoMol { 7 //________________________________________________________________ Matrix3by3 8 9 10 const Matrix3by3 Matrix3by3::ourIdentity = Matrix3by3(1,0,0, 0,1,0, 0,0,1); 11 Matrix3by3()12 Matrix3by3::Matrix3by3() 13 { 14 // zeroMatrix(); 15 // for performance reasons, an empty Matrix3by3 object will contain 16 // anything left in memory (not zeroed any more). user should avoid 17 // using x += y type operations until the matrices (x) have been 18 // properly initialized. A direct assian operation, x = y, will 19 // replace whatever left in x with the new values. 20 } 21 Matrix3by3(Real x00,Real x01,Real x02,Real x10,Real x11,Real x12,Real x20,Real x21,Real x22)22 Matrix3by3::Matrix3by3(Real x00, Real x01, Real x02, 23 Real x10, Real x11, Real x12, 24 Real x20, Real x21, Real x22) 25 { 26 m00 = x00; m01 = x01; m02 = x02; 27 m10 = x10; m11 = x11; m12 = x12; 28 m20 = x20; m21 = x21; m22 = x22; 29 } 30 Matrix3by3(float mat[9])31 Matrix3by3::Matrix3by3(float mat[9]) 32 { 33 m00 = *mat++; m01 = *mat++; m02 = *mat++; 34 m10 = *mat++; m11 = *mat++; m12 = *mat++; 35 m20 = *mat++; m21 = *mat++; m22 = *mat++; 36 } 37 Matrix3by3(double mat[9])38 Matrix3by3::Matrix3by3(double mat[9]) 39 { 40 m00 = *mat++; m01 = *mat++; m02 = *mat++; 41 m10 = *mat++; m11 = *mat++; m12 = *mat++; 42 m20 = *mat++; m21 = *mat++; m22 = *mat++; 43 } 44 45 //construct a matrix that is the outer products of two coordinates Matrix3by3(const Vector3D & a,const Vector3D & b)46 Matrix3by3::Matrix3by3(const Vector3D &a, const Vector3D &b) 47 { 48 (*this)(a.x*b.x,a.x*b.y,a.x*b.z, 49 a.y*b.x,a.y*b.y,a.y*b.z, 50 a.z*b.x,a.z*b.y,a.z*b.z); 51 } 52 Matrix3by3(const Vector3D & v1,const Vector3D & v2,const Vector3D & v3)53 Matrix3by3::Matrix3by3(const Vector3D& v1, const Vector3D& v2, 54 const Vector3D& v3) 55 { 56 m00 = v1.x; m01 = v1.y; m02 = v1.z; 57 m10 = v2.x; m11 = v2.y; m12 = v2.z; 58 m20 = v3.x; m21 = v3.y; m22 = v3.z; 59 } 60 identity()61 void Matrix3by3::identity() 62 { 63 m00 = 1; m01 = 0; m02 = 0; 64 m10 = 0; m11 = 1; m12 = 0; 65 m20 = 0; m21 = 0; m22 = 1; 66 } 67 zeroMatrix()68 void Matrix3by3::zeroMatrix() 69 { 70 m00 = 0; m01 = 0; m02 = 0; 71 m10 = 0; m11 = 0; m12 = 0; 72 m20 = 0; m21 = 0; m22 = 0; 73 } 74 zero() const75 bool Matrix3by3::zero() const 76 { 77 return *this == Matrix3by3(0,0,0, 0,0,0, 0,0,0); 78 } 79 operator ()(int i,int j) const80 Real Matrix3by3::operator()(int i, int j) const 81 { 82 if (i == 0) { 83 if (j == 0) return m00; 84 else if (j == 1) return m01; 85 else if (j == 2) return m02; 86 } 87 else if (i == 1) { 88 if (j == 0) return m10; 89 else if (j == 1) return m11; 90 else if (j == 2) return m12; 91 } 92 else if (i == 2) { 93 if (j == 0) return m20; 94 else if (j == 1) return m21; 95 else if (j == 2) return m22; 96 } 97 report << recoverable << "[Matrix3by3::operator()] index out of range."<<endr; 98 return 0; 99 } operator ()(int i,int j,Real x)100 void Matrix3by3::operator()(int i, int j, Real x) 101 { 102 if (i == 0) { 103 if (j == 0) { m00 = x; return;} 104 else if (j == 1) { m01 = x; return;} 105 else if (j == 2) { m02 = x; return;} 106 } 107 else if (i == 1) { 108 if (j == 0) { m10 = x; return;} 109 else if (j == 1) { m11 = x; return;} 110 else if (j == 2) { m12 = x; return;} 111 } 112 else if (i == 2) { 113 if (j == 0) { m20 = x; return;} 114 else if (j == 1) { m21 = x; return;} 115 else if (j == 2) { m22 = x; return;} 116 } 117 report << recoverable << "[Matrix3by3::operator()] index out of range."<<endr; 118 } 119 120 operator ()(Real x00,Real x01,Real x02,Real x10,Real x11,Real x12,Real x20,Real x21,Real x22)121 void Matrix3by3::operator()(Real x00, Real x01, Real x02, 122 Real x10, Real x11, Real x12, 123 Real x20, Real x21, Real x22) 124 { 125 m00 = x00; m01 = x01; m02 = x02; 126 m10 = x10; m11 = x11; m12 = x12; 127 m20 = x20; m21 = x21; m22 = x22; 128 } 129 operator ==(const Matrix3by3 & tm) const130 bool Matrix3by3::operator==(const Matrix3by3& tm) const 131 { 132 return (equal(m00, tm.m00) && equal(m01, tm.m01) && equal(m02, tm.m02) && 133 equal(m10, tm.m10) && equal(m11, tm.m11) && equal(m12, tm.m12) && 134 equal(m20, tm.m20) && equal(m21, tm.m21) && equal(m22, tm.m22)); 135 } 136 operator !=(const Matrix3by3 & tm) const137 bool Matrix3by3::operator!=(const Matrix3by3& tm) const 138 { 139 return !(*this == tm); 140 } 141 operator *=(const Real tm)142 Matrix3by3& Matrix3by3::operator*=(const Real tm) 143 { 144 m00 *= tm; m01 *= tm; m02 *= tm; 145 m10 *= tm; m11 *= tm; m12 *= tm; 146 m20 *= tm; m21 *= tm; m22 *= tm; 147 return *this; 148 } 149 operator /=(const Real tm)150 Matrix3by3& Matrix3by3::operator/=(const Real tm) 151 { 152 m00 /= tm; m01 /= tm; m02 /= tm; 153 m10 /= tm; m11 /= tm; m12 /= tm; 154 m20 /= tm; m21 /= tm; m22 /= tm; 155 return *this; 156 } 157 158 operator *=(const Matrix3by3 & tm)159 Matrix3by3& Matrix3by3::operator*=(const Matrix3by3& tm) 160 { 161 Matrix3by3 temp(*this); 162 163 m00 = temp.m00*tm.m00 + temp.m01*tm.m10 + temp.m02*tm.m20; 164 m01 = temp.m00*tm.m01 + temp.m01*tm.m11 + temp.m02*tm.m21; 165 m02 = temp.m00*tm.m02 + temp.m01*tm.m12 + temp.m02*tm.m22; 166 167 m10 = temp.m10*tm.m00 + temp.m11*tm.m10 + temp.m12*tm.m20; 168 m11 = temp.m10*tm.m01 + temp.m11*tm.m11 + temp.m12*tm.m21; 169 m12 = temp.m10*tm.m02 + temp.m11*tm.m12 + temp.m12*tm.m22; 170 171 m20 = temp.m20*tm.m00 + temp.m21*tm.m10 + temp.m22*tm.m20; 172 m21 = temp.m20*tm.m01 + temp.m21*tm.m11 + temp.m22*tm.m21; 173 m22 = temp.m20*tm.m02 + temp.m21*tm.m12 + temp.m22*tm.m22; 174 175 return *this; 176 } 177 178 operator *(const Real tm) const179 Matrix3by3 Matrix3by3::operator*(const Real tm) const 180 { 181 Matrix3by3 res; 182 res.m00 = m00*tm; 183 res.m01 = m01*tm; 184 res.m02 = m02*tm; 185 186 res.m10 = m10*tm; 187 res.m11 = m11*tm; 188 res.m12 = m12*tm; 189 190 res.m20 = m20*tm; 191 res.m21 = m21*tm; 192 res.m22 = m22*tm; 193 194 return res; 195 } 196 197 operator /(const Real tm) const198 Matrix3by3 Matrix3by3::operator/(const Real tm) const 199 { 200 Matrix3by3 res; 201 res.m00 = m00/tm; 202 res.m01 = m01/tm; 203 res.m02 = m02/tm; 204 205 res.m10 = m10/tm; 206 res.m11 = m11/tm; 207 res.m12 = m12/tm; 208 209 res.m20 = m20/tm; 210 res.m21 = m21/tm; 211 res.m22 = m22/tm; 212 return res; 213 } 214 215 operator *(const Matrix3by3 & tm) const216 Matrix3by3 Matrix3by3::operator*(const Matrix3by3& tm) const 217 { 218 Matrix3by3 res; 219 res.m00 = m00*tm.m00 + m01*tm.m10 + m02*tm.m20; 220 res.m01 = m00*tm.m01 + m01*tm.m11 + m02*tm.m21; 221 res.m02 = m00*tm.m02 + m01*tm.m12 + m02*tm.m22; 222 223 res.m10 = m10*tm.m00 + m11*tm.m10 + m12*tm.m20; 224 res.m11 = m10*tm.m01 + m11*tm.m11 + m12*tm.m21; 225 res.m12 = m10*tm.m02 + m11*tm.m12 + m12*tm.m22; 226 227 res.m20 = m20*tm.m00 + m21*tm.m10 + m22*tm.m20; 228 res.m21 = m20*tm.m01 + m21*tm.m11 + m22*tm.m21; 229 res.m22 = m20*tm.m02 + m21*tm.m12 + m22*tm.m22; 230 return res; 231 } 232 233 operator *(const Vector3D & tm) const234 Vector3D Matrix3by3::operator*(const Vector3D& tm) const 235 { 236 Vector3D res; 237 res.x = m00*tm.x + m01*tm.y + m02*tm.z; 238 res.y = m10*tm.x + m11*tm.y + m12*tm.z; 239 res.z = m20*tm.x + m21*tm.y + m22*tm.z; 240 return res; 241 } 242 243 operator -=(const Matrix3by3 & tm)244 Matrix3by3& Matrix3by3::operator-=(const Matrix3by3& tm) 245 { 246 m00 -= tm.m00; 247 m01 -= tm.m01; 248 m02 -= tm.m02; 249 250 m10 -= tm.m10; 251 m11 -= tm.m11; 252 m12 -= tm.m12; 253 254 m20 -= tm.m20; 255 m21 -= tm.m21; 256 m22 -= tm.m22; 257 return *this; 258 } 259 operator -(const Matrix3by3 & tm) const260 Matrix3by3 Matrix3by3::operator-(const Matrix3by3& tm) const 261 { 262 Matrix3by3 res; 263 264 res.m00 = m00 - tm.m00; 265 res.m01 = m01 - tm.m01; 266 res.m02 = m02 - tm.m02; 267 268 res.m10 = m10 - tm.m10; 269 res.m11 = m11 - tm.m11; 270 res.m12 = m12 - tm.m12; 271 272 res.m20 = m20 - tm.m20; 273 res.m21 = m21 - tm.m21; 274 res.m22 = m22 - tm.m22; 275 return res; 276 277 } 278 279 operator -(void) const280 Matrix3by3 Matrix3by3::operator-(void) const 281 { 282 Matrix3by3 res; 283 284 res.m00 = -m00; 285 res.m01 = -m01; 286 res.m02 = -m02; 287 res.m10 = -m10; 288 res.m11 = -m11; 289 res.m12 = -m12; 290 res.m20 = -m20; 291 res.m21 = -m21; 292 res.m22 = -m22; 293 return res; 294 } 295 transposed() const296 Matrix3by3 Matrix3by3::transposed() const 297 { 298 Matrix3by3 res; 299 300 res.m00 = m00; 301 res.m01 = m10; 302 res.m02 = m20; 303 304 res.m10 = m01; 305 res.m11 = m11; 306 res.m12 = m21; 307 308 res.m20 = m02; 309 res.m21 = m12; 310 res.m22 = m22; 311 return res; 312 } 313 transpose(const Matrix3by3 & tm)314 void Matrix3by3::transpose(const Matrix3by3& tm) 315 { 316 317 // NB: make sure that *this == tm also works 318 Real a = tm.m10; 319 m10 = tm.m01; 320 m01 = a; 321 322 Real b = tm.m20; 323 m20 = tm.m02; 324 m02 = b; 325 326 Real c = tm.m21; 327 m21 = tm.m12; 328 m12 = c; 329 330 m00 = tm.m00; 331 m11 = tm.m11; 332 m22 = tm.m22; 333 } 334 transpose()335 void Matrix3by3::transpose() 336 { 337 Real a = m10; 338 m10 = m01; 339 m01 = a; 340 341 Real b = m20; 342 m20 = m02; 343 m02 = b; 344 345 Real c = m21; 346 m21 = m12; 347 m12 = c; 348 } 349 350 operator +=(const Matrix3by3 & tm)351 Matrix3by3& Matrix3by3::operator+=(const Matrix3by3& tm) 352 { 353 m00 += tm.m00; 354 m01 += tm.m01; 355 m02 += tm.m02; 356 357 m10 += tm.m10; 358 m11 += tm.m11; 359 m12 += tm.m12; 360 361 m20 += tm.m20; 362 m21 += tm.m21; 363 m22 += tm.m22; 364 return *this; 365 } 366 operator +(const Matrix3by3 & tm) const367 Matrix3by3 Matrix3by3::operator+(const Matrix3by3& tm) const 368 { 369 Matrix3by3 res; 370 371 res.m00 = m00 + tm.m00; 372 res.m01 = m01 + tm.m01; 373 res.m02 = m02 + tm.m02; 374 375 res.m10 = m10 + tm.m10; 376 res.m11 = m11 + tm.m11; 377 res.m12 = m12 + tm.m12; 378 379 res.m20 = m20 + tm.m20; 380 res.m21 = m21 + tm.m21; 381 res.m22 = m22 + tm.m22; 382 return res; 383 } 384 385 det()386 Real Matrix3by3::det() 387 { 388 Real a00 = m11*m22 - m12*m21; 389 Real a10 = m10*m22 - m12*m20; 390 Real a20 = m10*m21 - m11*m20; 391 return m00*a00 - m01*a10 + m02*a20; 392 } 393 invert()394 bool Matrix3by3::invert() 395 { 396 Matrix3by3 tmp; 397 Real det; 398 399 // 400 // Calculate the determinant of submatrix A (optimized version: 401 // don,t just compute the determinant of A) 402 // 403 tmp.m00 = m11*m22 - m12*m21; 404 tmp.m10 = m10*m22 - m12*m20; 405 tmp.m20 = m10*m21 - m11*m20; 406 407 tmp.m01 = m01*m22 - m02*m21; 408 tmp.m11 = m00*m22 - m02*m20; 409 tmp.m21 = m00*m21 - m01*m20; 410 411 tmp.m02 = m01*m12 - m02*m11; 412 tmp.m12 = m00*m12 - m02*m10; 413 tmp.m22 = m00*m11 - m01*m10; 414 415 det = m00*tmp.m00 - m01*tmp.m10 + m02*tmp.m20; 416 417 // 418 // singular matrix ? 419 // 420 if (fabs(det) < Constant::EPSILON*Constant::EPSILON) 421 return false; 422 423 det = 1/det; 424 425 // 426 // inverse(A) = adj(A)/det(A) 427 // 428 tmp.m00 *= det; 429 tmp.m02 *= det; 430 tmp.m11 *= det; 431 tmp.m20 *= det; 432 tmp.m22 *= det; 433 434 det = -det; 435 436 tmp.m01 *= det; 437 tmp.m10 *= det; 438 tmp.m12 *= det; 439 tmp.m21 *= det; 440 441 *this = tmp; 442 return true; 443 } 444 445 /* 446 * Concat scale matrix to the current transformation. 447 */ 448 scale(Real s)449 void Matrix3by3::scale(Real s) 450 { 451 m00 *= s; m01 *= s; m02 *= s; 452 m10 *= s; m11 *= s; m12 *= s; 453 m20 *= s; m21 *= s; m22 *= s; 454 } 455 456 scale(Real sx,Real sy,Real sz)457 void Matrix3by3::scale(Real sx, Real sy, Real sz) 458 { 459 m00 *= sx; m01 *= sy; m02 *= sz; 460 m10 *= sx; m11 *= sy; m12 *= sz; 461 m20 *= sx; m21 *= sy; m22 *= sz; 462 } 463 scale(const Vector3D & scaleFactor)464 void Matrix3by3::scale(const Vector3D& scaleFactor) 465 { 466 m00 *= scaleFactor.x; m01 *= scaleFactor.y; m02 *= scaleFactor.z; 467 m10 *= scaleFactor.x; m11 *= scaleFactor.y; m12 *= scaleFactor.z; 468 m20 *= scaleFactor.x; m21 *= scaleFactor.y; m22 *= scaleFactor.z; 469 } 470 rotate(const Vector3D & axis,Real sinAlpha,Real cosAlpha)471 void Matrix3by3::rotate(const Vector3D& axis, Real sinAlpha, Real cosAlpha) 472 { 473 if(axis.normSquared() > 0.0){ 474 Real n1, n2, n3; 475 476 Vector3D n(axis.normalized()); 477 478 n1 = n.x; n2 = n.y; n3 = n.z; 479 480 m00 = n1*n1 + (1. - n1*n1)*cosAlpha; 481 m01 = n1*n2*(1. - cosAlpha) + n3*sinAlpha; 482 m02 = n1*n3*(1. - cosAlpha) - n2*sinAlpha; 483 484 m10 = n1*n2*(1. - cosAlpha) - n3*sinAlpha; 485 m11 = n2*n2 + (1. - n2*n2)*cosAlpha; 486 m12 = n2*n3*(1. - cosAlpha) + n1*sinAlpha; 487 488 m20 = n1*n3*(1. - cosAlpha) + n2*sinAlpha; 489 m21 = n2*n3*(1. - cosAlpha) - n1*sinAlpha; 490 m22 = n3*n3 + (1. - n3*n3)*cosAlpha; 491 } 492 else { 493 identity(); 494 } 495 496 } 497 rotate(const Vector3D & axis,Real alpha)498 void Matrix3by3::rotate(const Vector3D& axis, Real alpha) 499 { 500 Real sinAlpha, cosAlpha; 501 sincos(alpha, sinAlpha, cosAlpha); 502 rotate(axis,sinAlpha, cosAlpha); 503 } 504 rotate(const Vector3D & from,const Vector3D & to)505 void Matrix3by3::rotate(const Vector3D& from, const Vector3D& to) 506 { 507 508 if(from.normSquared() > 0.0 && to.normSquared() > 0.0){ 509 Vector3D a(from.normalized()); 510 Vector3D b(to.normalized()); 511 512 // linear 513 if((a-b).normSquared() < Constant::EPSILON){ 514 // same direction 515 identity(); 516 } 517 else if((a+b).normSquared() < Constant::EPSILON){ 518 // opposite direction 519 Real n1 = fabs(a.x); 520 Real n2 = fabs(a.y); 521 Real n3 = fabs(a.z); 522 if(n1 >= max(n2,n3)){ 523 m00 =-1.0; 524 m01 = 0.0; 525 m02 = 0.0; 526 527 m10 = 0.0; 528 m11 =-1.0; 529 m12 = 0.0; 530 531 m20 = 0.0; 532 m21 = 0.0; 533 m22 = 1.0; 534 } 535 else if( n2 >= max(n1,n3)){ 536 m00 = 1.0; 537 m01 = 0.0; 538 m02 = 0.0; 539 540 m10 = 0.0; 541 m11 =-1.0; 542 m12 = 0.0; 543 544 m20 = 0.0; 545 m21 = 0.0; 546 m22 =-1.0; 547 548 } 549 else { 550 m00 =-1.0; 551 m01 = 0.0; 552 m02 = 0.0; 553 554 m10 = 0.0; 555 m11 = 1.0; 556 m12 = 0.0; 557 558 m20 = 0.0; 559 m21 = 0.0; 560 m22 =-1.0; 561 } 562 } 563 else { 564 Vector3D axis(b.cross(a)); 565 axis.normalize(); 566 rotate(axis,a.cross(b).norm(),a.dot(b)); 567 } 568 } 569 else { 570 identity(); 571 } 572 573 } 574 575 576 577 //______________________________________________________________________ friends 578 operator <<(std::ostream & os,const Matrix3by3 & tm)579 std::ostream& operator<<(std::ostream &os, const Matrix3by3& tm) 580 { 581 os << "[[" << tm.m00 << ", " << tm.m01 << ", " <<tm.m02 << "], "; 582 os << "[" << tm.m10 << ", " << tm.m11 << ", " <<tm.m12 << "], "; 583 os << "[" << tm.m20 << ", " << tm.m21 << ", " <<tm.m22 << "]]"; 584 return os; 585 } 586 operator *(const Vector3D & point,const Matrix3by3 & tm)587 Vector3D operator*(const Vector3D& point, const Matrix3by3& tm) 588 { 589 return Vector3D(tm.m00*point.x + tm.m10*point.y + tm.m20*point.z, 590 tm.m01*point.x + tm.m11*point.y + tm.m21*point.z, 591 tm.m02*point.x + tm.m12*point.y + tm.m22*point.z); 592 } 593 convert(const Matrix3by3 & from,float to[9])594 void convert(const Matrix3by3& from, float to[9]) 595 { 596 to[0] = static_cast<float>(from.m00); to[1] = static_cast<float>(from.m01); to[2] = static_cast<float>(from.m02); 597 to[3] = static_cast<float>(from.m10); to[4] = static_cast<float>(from.m11); to[5] = static_cast<float>(from.m12); 598 to[6] = static_cast<float>(from.m20); to[7] = static_cast<float>(from.m21); to[8] = static_cast<float>(from.m22); 599 } 600 convert(const Matrix3by3 & from,double to[16])601 void convert(const Matrix3by3& from, double to[16]) 602 { 603 to[0] = static_cast<double>(from.m00); to[1] = static_cast<double>(from.m01); to[2] = static_cast<double>(from.m02); 604 to[3] = static_cast<double>(from.m10); to[4] = static_cast<double>(from.m11); to[5] = static_cast<double>(from.m12); 605 to[6] = static_cast<double>(from.m20); to[7] = static_cast<double>(from.m21); to[8] = static_cast<double>(from.m22); 606 } 607 608 } 609