1 /* 2 * Copyright (C) 2012 Open Source Robotics Foundation 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 * 16 */ 17 #ifndef IGNITION_MATH_MATRIX4_HH_ 18 #define IGNITION_MATH_MATRIX4_HH_ 19 20 #include <algorithm> 21 #include <utility> 22 #include <ignition/math/Helpers.hh> 23 #include <ignition/math/Matrix3.hh> 24 #include <ignition/math/Vector3.hh> 25 #include <ignition/math/Pose3.hh> 26 #include <ignition/math/config.hh> 27 28 namespace ignition 29 { 30 namespace math 31 { 32 // Inline bracket to help doxygen filtering. 33 inline namespace IGNITION_MATH_VERSION_NAMESPACE { 34 // 35 /// \class Matrix4 Matrix4.hh ignition/math/Matrix4.hh 36 /// \brief A 4x4 matrix class 37 template<typename T> 38 class Matrix4 39 { 40 /// \brief Identity matrix 41 public: static const Matrix4<T> Identity; 42 43 /// \brief Zero matrix 44 public: static const Matrix4<T> Zero; 45 46 /// \brief Constructor Matrix4()47 public: Matrix4() 48 { 49 memset(this->data, 0, sizeof(this->data[0][0])*16); 50 } 51 52 /// \brief Copy constructor 53 /// \param _m Matrix to copy Matrix4(const Matrix4<T> & _m)54 public: Matrix4(const Matrix4<T> &_m) 55 { 56 memcpy(this->data, _m.data, sizeof(this->data[0][0])*16); 57 } 58 59 /// \brief Constructor 60 /// \param[in] _v00 Row 0, Col 0 value 61 /// \param[in] _v01 Row 0, Col 1 value 62 /// \param[in] _v02 Row 0, Col 2 value 63 /// \param[in] _v03 Row 0, Col 3 value 64 /// \param[in] _v10 Row 1, Col 0 value 65 /// \param[in] _v11 Row 1, Col 1 value 66 /// \param[in] _v12 Row 1, Col 2 value 67 /// \param[in] _v13 Row 1, Col 3 value 68 /// \param[in] _v20 Row 2, Col 0 value 69 /// \param[in] _v21 Row 2, Col 1 value 70 /// \param[in] _v22 Row 2, Col 2 value 71 /// \param[in] _v23 Row 2, Col 3 value 72 /// \param[in] _v30 Row 3, Col 0 value 73 /// \param[in] _v31 Row 3, Col 1 value 74 /// \param[in] _v32 Row 3, Col 2 value 75 /// \param[in] _v33 Row 3, Col 3 value Matrix4(T _v00,T _v01,T _v02,T _v03,T _v10,T _v11,T _v12,T _v13,T _v20,T _v21,T _v22,T _v23,T _v30,T _v31,T _v32,T _v33)76 public: Matrix4(T _v00, T _v01, T _v02, T _v03, 77 T _v10, T _v11, T _v12, T _v13, 78 T _v20, T _v21, T _v22, T _v23, 79 T _v30, T _v31, T _v32, T _v33) 80 { 81 this->Set(_v00, _v01, _v02, _v03, 82 _v10, _v11, _v12, _v13, 83 _v20, _v21, _v22, _v23, 84 _v30, _v31, _v32, _v33); 85 } 86 87 /// \brief Construct Matrix4 from a quaternion. 88 /// \param[in] _q Quaternion. Matrix4(const Quaternion<T> & _q)89 public: explicit Matrix4(const Quaternion<T> &_q) 90 { 91 Quaternion<T> qt = _q; 92 qt.Normalize(); 93 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(), 94 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(), 95 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(), 96 0, 97 98 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(), 99 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(), 100 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(), 101 0, 102 103 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(), 104 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(), 105 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(), 106 0, 107 108 0, 0, 0, 1); 109 } 110 111 /// \brief Construct Matrix4 from a math::Pose3 112 /// \param[in] _pose Pose. Matrix4(const Pose3<T> & _pose)113 public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot()) 114 { 115 this->SetTranslation(_pose.Pos()); 116 } 117 118 /// \brief Destructor ~Matrix4()119 public: virtual ~Matrix4() {} 120 121 /// \brief Change the values 122 /// \param[in] _v00 Row 0, Col 0 value 123 /// \param[in] _v01 Row 0, Col 1 value 124 /// \param[in] _v02 Row 0, Col 2 value 125 /// \param[in] _v03 Row 0, Col 3 value 126 /// \param[in] _v10 Row 1, Col 0 value 127 /// \param[in] _v11 Row 1, Col 1 value 128 /// \param[in] _v12 Row 1, Col 2 value 129 /// \param[in] _v13 Row 1, Col 3 value 130 /// \param[in] _v20 Row 2, Col 0 value 131 /// \param[in] _v21 Row 2, Col 1 value 132 /// \param[in] _v22 Row 2, Col 2 value 133 /// \param[in] _v23 Row 2, Col 3 value 134 /// \param[in] _v30 Row 3, Col 0 value 135 /// \param[in] _v31 Row 3, Col 1 value 136 /// \param[in] _v32 Row 3, Col 2 value 137 /// \param[in] _v33 Row 3, Col 3 value Set(T _v00,T _v01,T _v02,T _v03,T _v10,T _v11,T _v12,T _v13,T _v20,T _v21,T _v22,T _v23,T _v30,T _v31,T _v32,T _v33)138 public: void Set( 139 T _v00, T _v01, T _v02, T _v03, 140 T _v10, T _v11, T _v12, T _v13, 141 T _v20, T _v21, T _v22, T _v23, 142 T _v30, T _v31, T _v32, T _v33) 143 { 144 this->data[0][0] = _v00; 145 this->data[0][1] = _v01; 146 this->data[0][2] = _v02; 147 this->data[0][3] = _v03; 148 149 this->data[1][0] = _v10; 150 this->data[1][1] = _v11; 151 this->data[1][2] = _v12; 152 this->data[1][3] = _v13; 153 154 this->data[2][0] = _v20; 155 this->data[2][1] = _v21; 156 this->data[2][2] = _v22; 157 this->data[2][3] = _v23; 158 159 this->data[3][0] = _v30; 160 this->data[3][1] = _v31; 161 this->data[3][2] = _v32; 162 this->data[3][3] = _v33; 163 } 164 165 /// \brief Set the upper-left 3x3 matrix from an axis and angle 166 /// \param[in] _axis the axis 167 /// \param[in] _angle ccw rotation around the axis in radians Axis(const Vector3<T> & _axis,T _angle)168 public: void Axis(const Vector3<T> &_axis, T _angle) 169 { 170 T c = cos(_angle); 171 T s = sin(_angle); 172 T C = 1-c; 173 174 this->data[0][0] = _axis.X()*_axis.X()*C + c; 175 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s; 176 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s; 177 178 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s; 179 this->data[1][1] = _axis.Y()*_axis.Y()*C + c; 180 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s; 181 182 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s; 183 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s; 184 this->data[2][2] = _axis.Z()*_axis.Z()*C + c; 185 } 186 187 /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] 188 /// \param[in] _t Values to set 189 /// \deprecated Use SetTranslation instead 190 public: void 191 IGN_DEPRECATED(4) Translate(const Vector3<T> & _t)192 Translate(const Vector3<T> &_t) 193 { 194 this->SetTranslation(_t); 195 } 196 197 /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] 198 /// \param[in] _t Values to set SetTranslation(const Vector3<T> & _t)199 public: void SetTranslation(const Vector3<T> &_t) 200 { 201 this->data[0][3] = _t.X(); 202 this->data[1][3] = _t.Y(); 203 this->data[2][3] = _t.Z(); 204 } 205 206 /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] 207 /// \param[in] _x X translation value. 208 /// \param[in] _y Y translation value. 209 /// \param[in] _z Z translation value. 210 /// \deprecated Use SetTranslation instead 211 public: void 212 IGN_DEPRECATED(4) Translate(T _x,T _y,T _z)213 Translate(T _x, T _y, T _z) 214 { 215 this->SetTranslation(_x, _y, _z); 216 } 217 218 /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] 219 /// \param[in] _x X translation value. 220 /// \param[in] _y Y translation value. 221 /// \param[in] _z Z translation value. SetTranslation(T _x,T _y,T _z)222 public: void SetTranslation(T _x, T _y, T _z) 223 { 224 this->data[0][3] = _x; 225 this->data[1][3] = _y; 226 this->data[2][3] = _z; 227 } 228 229 /// \brief Get the translational values as a Vector3 230 /// \return x,y,z translation values Translation() const231 public: Vector3<T> Translation() const 232 { 233 return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]); 234 } 235 236 /// \brief Get the scale values as a Vector3<T> 237 /// \return x,y,z scale values Scale() const238 public: Vector3<T> Scale() const 239 { 240 return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]); 241 } 242 243 /// \brief Get the rotation as a quaternion 244 /// \return the rotation Rotation() const245 public: Quaternion<T> Rotation() const 246 { 247 Quaternion<T> q; 248 /// algorithm from Ogre::Quaternion<T> source, which in turn is based on 249 /// Ken Shoemake's article "Quaternion<T> Calculus and Fast Animation". 250 T trace = this->data[0][0] + this->data[1][1] + this->data[2][2]; 251 T root; 252 if (trace > 0) 253 { 254 root = sqrt(trace + 1.0); 255 q.W(root / 2.0); 256 root = 1.0 / (2.0 * root); 257 q.X((this->data[2][1] - this->data[1][2]) * root); 258 q.Y((this->data[0][2] - this->data[2][0]) * root); 259 q.Z((this->data[1][0] - this->data[0][1]) * root); 260 } 261 else 262 { 263 static unsigned int s_iNext[3] = {1, 2, 0}; 264 unsigned int i = 0; 265 if (this->data[1][1] > this->data[0][0]) 266 i = 1; 267 if (this->data[2][2] > this->data[i][i]) 268 i = 2; 269 unsigned int j = s_iNext[i]; 270 unsigned int k = s_iNext[j]; 271 272 root = sqrt(this->data[i][i] - this->data[j][j] - 273 this->data[k][k] + 1.0); 274 275 T a, b, c; 276 a = root / 2.0; 277 root = 1.0 / (2.0 * root); 278 b = (this->data[j][i] + this->data[i][j]) * root; 279 c = (this->data[k][i] + this->data[i][k]) * root; 280 281 switch (i) 282 { 283 default: 284 case 0: q.X(a); break; 285 case 1: q.Y(a); break; 286 case 2: q.Z(a); break; 287 }; 288 switch (j) 289 { 290 default: 291 case 0: q.X(b); break; 292 case 1: q.Y(b); break; 293 case 2: q.Z(b); break; 294 }; 295 switch (k) 296 { 297 default: 298 case 0: q.X(c); break; 299 case 1: q.Y(c); break; 300 case 2: q.Z(c); break; 301 }; 302 303 q.W((this->data[k][j] - this->data[j][k]) * root); 304 } 305 306 return q; 307 } 308 309 /// \brief Get the rotation as a Euler angles 310 /// \param[in] _firstSolution True to get the first Euler solution, 311 /// false to get the second. 312 /// \return the rotation EulerRotation(bool _firstSolution) const313 public: Vector3<T> EulerRotation(bool _firstSolution) const 314 { 315 Vector3<T> euler; 316 Vector3<T> euler2; 317 318 T m31 = this->data[2][0]; 319 T m11 = this->data[0][0]; 320 T m12 = this->data[0][1]; 321 T m13 = this->data[0][2]; 322 T m32 = this->data[2][1]; 323 T m33 = this->data[2][2]; 324 T m21 = this->data[1][0]; 325 326 if (std::abs(m31) >= 1.0) 327 { 328 euler.Z(0.0); 329 euler2.Z(0.0); 330 331 if (m31 < 0.0) 332 { 333 euler.Y(IGN_PI / 2.0); 334 euler2.Y(IGN_PI / 2.0); 335 euler.X(atan2(m12, m13)); 336 euler2.X(atan2(m12, m13)); 337 } 338 else 339 { 340 euler.Y(-IGN_PI / 2.0); 341 euler2.Y(-IGN_PI / 2.0); 342 euler.X(atan2(-m12, -m13)); 343 euler2.X(atan2(-m12, -m13)); 344 } 345 } 346 else 347 { 348 euler.Y(-asin(m31)); 349 euler2.Y(IGN_PI - euler.Y()); 350 351 euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y()))); 352 euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y()))); 353 354 euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y()))); 355 euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y()))); 356 } 357 358 if (_firstSolution) 359 return euler; 360 else 361 return euler2; 362 } 363 364 /// \brief Get the transformation as math::Pose 365 /// \return the pose Pose() const366 public: Pose3<T> Pose() const 367 { 368 return Pose3<T>(this->Translation(), this->Rotation()); 369 } 370 371 /// \brief Set the scale 372 /// \param[in] _s scale Scale(const Vector3<T> & _s)373 public: void Scale(const Vector3<T> &_s) 374 { 375 this->data[0][0] = _s.X(); 376 this->data[1][1] = _s.Y(); 377 this->data[2][2] = _s.Z(); 378 this->data[3][3] = 1.0; 379 } 380 381 /// \brief Set the scale 382 /// \param[in] _x X scale value. 383 /// \param[in] _y Y scale value. 384 /// \param[in] _z Z scale value. Scale(T _x,T _y,T _z)385 public: void Scale(T _x, T _y, T _z) 386 { 387 this->data[0][0] = _x; 388 this->data[1][1] = _y; 389 this->data[2][2] = _z; 390 this->data[3][3] = 1.0; 391 } 392 393 /// \brief Return true if the matrix is affine 394 /// \return true if the matrix is affine, false otherwise IsAffine() const395 public: bool IsAffine() const 396 { 397 return equal(this->data[3][0], static_cast<T>(0)) && 398 equal(this->data[3][1], static_cast<T>(0)) && 399 equal(this->data[3][2], static_cast<T>(0)) && 400 equal(this->data[3][3], static_cast<T>(1)); 401 } 402 403 /// \brief Perform an affine transformation 404 /// \param[in] _v Vector3 value for the transformation 405 /// \return The result of the transformation. A default constructed 406 /// Vector3<T> is returned if this matrix is not affine. 407 /// \deprecated Use bool TransformAffine(const Vector3<T> &_v, 408 /// Vector3<T> &_result) const; 409 public: Vector3<T> 410 IGN_DEPRECATED(3.0) TransformAffine(const Vector3<T> & _v) const411 TransformAffine(const Vector3<T> &_v) const 412 { 413 if (this->IsAffine()) 414 { 415 return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + 416 this->data[0][2]*_v.Z() + this->data[0][3], 417 this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + 418 this->data[1][2]*_v.Z() + this->data[1][3], 419 this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + 420 this->data[2][2]*_v.Z() + this->data[2][3]); 421 } 422 else 423 { 424 return Vector3<T>(); 425 } 426 } 427 428 /// \brief Perform an affine transformation 429 /// \param[in] _v Vector3 value for the transformation 430 /// \param[out] _result The result of the transformation. _result is 431 /// not changed if this matrix is not affine. 432 /// \return True if this matrix is affine, false otherwise. TransformAffine(const Vector3<T> & _v,Vector3<T> & _result) const433 public: bool TransformAffine(const Vector3<T> &_v, 434 Vector3<T> &_result) const 435 { 436 if (!this->IsAffine()) 437 return false; 438 439 _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + 440 this->data[0][2]*_v.Z() + this->data[0][3], 441 this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + 442 this->data[1][2]*_v.Z() + this->data[1][3], 443 this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + 444 this->data[2][2]*_v.Z() + this->data[2][3]); 445 return true; 446 } 447 448 /// \brief Return the determinant of the matrix 449 /// \return Determinant of this matrix. Determinant() const450 public: T Determinant() const 451 { 452 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; 453 454 v0 = this->data[2][0]*this->data[3][1] 455 - this->data[2][1]*this->data[3][0]; 456 v1 = this->data[2][0]*this->data[3][2] 457 - this->data[2][2]*this->data[3][0]; 458 v2 = this->data[2][0]*this->data[3][3] 459 - this->data[2][3]*this->data[3][0]; 460 v3 = this->data[2][1]*this->data[3][2] 461 - this->data[2][2]*this->data[3][1]; 462 v4 = this->data[2][1]*this->data[3][3] 463 - this->data[2][3]*this->data[3][1]; 464 v5 = this->data[2][2]*this->data[3][3] 465 - this->data[2][3]*this->data[3][2]; 466 467 t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3]; 468 t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3]; 469 t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3]; 470 t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2]; 471 472 return t00 * this->data[0][0] 473 + t10 * this->data[0][1] 474 + t20 * this->data[0][2] 475 + t30 * this->data[0][3]; 476 } 477 478 /// \brief Return the inverse matrix. 479 /// This is a non-destructive operation. 480 /// \return Inverse of this matrix. Inverse() const481 public: Matrix4<T> Inverse() const 482 { 483 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; 484 Matrix4<T> r; 485 486 v0 = this->data[2][0]*this->data[3][1] - 487 this->data[2][1]*this->data[3][0]; 488 v1 = this->data[2][0]*this->data[3][2] - 489 this->data[2][2]*this->data[3][0]; 490 v2 = this->data[2][0]*this->data[3][3] - 491 this->data[2][3]*this->data[3][0]; 492 v3 = this->data[2][1]*this->data[3][2] - 493 this->data[2][2]*this->data[3][1]; 494 v4 = this->data[2][1]*this->data[3][3] - 495 this->data[2][3]*this->data[3][1]; 496 v5 = this->data[2][2]*this->data[3][3] - 497 this->data[2][3]*this->data[3][2]; 498 499 t00 = +(v5*this->data[1][1] - 500 v4*this->data[1][2] + v3*this->data[1][3]); 501 t10 = -(v5*this->data[1][0] - 502 v2*this->data[1][2] + v1*this->data[1][3]); 503 t20 = +(v4*this->data[1][0] - 504 v2*this->data[1][1] + v0*this->data[1][3]); 505 t30 = -(v3*this->data[1][0] - 506 v1*this->data[1][1] + v0*this->data[1][2]); 507 508 T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] + 509 t20 * this->data[0][2] + t30 * this->data[0][3]); 510 511 r(0, 0) = t00 * invDet; 512 r(1, 0) = t10 * invDet; 513 r(2, 0) = t20 * invDet; 514 r(3, 0) = t30 * invDet; 515 516 r(0, 1) = -(v5*this->data[0][1] - 517 v4*this->data[0][2] + v3*this->data[0][3]) * invDet; 518 r(1, 1) = +(v5*this->data[0][0] - 519 v2*this->data[0][2] + v1*this->data[0][3]) * invDet; 520 r(2, 1) = -(v4*this->data[0][0] - 521 v2*this->data[0][1] + v0*this->data[0][3]) * invDet; 522 r(3, 1) = +(v3*this->data[0][0] - 523 v1*this->data[0][1] + v0*this->data[0][2]) * invDet; 524 525 v0 = this->data[1][0]*this->data[3][1] - 526 this->data[1][1]*this->data[3][0]; 527 v1 = this->data[1][0]*this->data[3][2] - 528 this->data[1][2]*this->data[3][0]; 529 v2 = this->data[1][0]*this->data[3][3] - 530 this->data[1][3]*this->data[3][0]; 531 v3 = this->data[1][1]*this->data[3][2] - 532 this->data[1][2]*this->data[3][1]; 533 v4 = this->data[1][1]*this->data[3][3] - 534 this->data[1][3]*this->data[3][1]; 535 v5 = this->data[1][2]*this->data[3][3] - 536 this->data[1][3]*this->data[3][2]; 537 538 r(0, 2) = +(v5*this->data[0][1] - 539 v4*this->data[0][2] + v3*this->data[0][3]) * invDet; 540 r(1, 2) = -(v5*this->data[0][0] - 541 v2*this->data[0][2] + v1*this->data[0][3]) * invDet; 542 r(2, 2) = +(v4*this->data[0][0] - 543 v2*this->data[0][1] + v0*this->data[0][3]) * invDet; 544 r(3, 2) = -(v3*this->data[0][0] - 545 v1*this->data[0][1] + v0*this->data[0][2]) * invDet; 546 547 v0 = this->data[2][1]*this->data[1][0] - 548 this->data[2][0]*this->data[1][1]; 549 v1 = this->data[2][2]*this->data[1][0] - 550 this->data[2][0]*this->data[1][2]; 551 v2 = this->data[2][3]*this->data[1][0] - 552 this->data[2][0]*this->data[1][3]; 553 v3 = this->data[2][2]*this->data[1][1] - 554 this->data[2][1]*this->data[1][2]; 555 v4 = this->data[2][3]*this->data[1][1] - 556 this->data[2][1]*this->data[1][3]; 557 v5 = this->data[2][3]*this->data[1][2] - 558 this->data[2][2]*this->data[1][3]; 559 560 r(0, 3) = -(v5*this->data[0][1] - 561 v4*this->data[0][2] + v3*this->data[0][3]) * invDet; 562 r(1, 3) = +(v5*this->data[0][0] - 563 v2*this->data[0][2] + v1*this->data[0][3]) * invDet; 564 r(2, 3) = -(v4*this->data[0][0] - 565 v2*this->data[0][1] + v0*this->data[0][3]) * invDet; 566 r(3, 3) = +(v3*this->data[0][0] - 567 v1*this->data[0][1] + v0*this->data[0][2]) * invDet; 568 569 return r; 570 } 571 572 /// \brief Transpose this matrix. Transpose()573 public: void Transpose() 574 { 575 std::swap(this->data[0][1], this->data[1][0]); 576 std::swap(this->data[0][2], this->data[2][0]); 577 std::swap(this->data[0][3], this->data[3][0]); 578 std::swap(this->data[1][2], this->data[2][1]); 579 std::swap(this->data[1][3], this->data[3][1]); 580 std::swap(this->data[2][3], this->data[3][2]); 581 } 582 583 /// \brief Return the transpose of this matrix 584 /// \return Transpose of this matrix. Transposed() const585 public: Matrix4<T> Transposed() const 586 { 587 return Matrix4<T>( 588 this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0], 589 this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1], 590 this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2], 591 this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]); 592 } 593 594 /// \brief Equal operator. this = _mat 595 /// \param _mat Incoming matrix 596 /// \return itself operator =(const Matrix4<T> & _mat)597 public: Matrix4<T> &operator=(const Matrix4<T> &_mat) 598 { 599 memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16); 600 return *this; 601 } 602 603 /// \brief Equal operator for 3x3 matrix 604 /// \param _mat Incoming matrix 605 /// \return itself operator =(const Matrix3<T> & _mat)606 public: const Matrix4<T> &operator=(const Matrix3<T> &_mat) 607 { 608 this->data[0][0] = _mat(0, 0); 609 this->data[0][1] = _mat(0, 1); 610 this->data[0][2] = _mat(0, 2); 611 612 this->data[1][0] = _mat(1, 0); 613 this->data[1][1] = _mat(1, 1); 614 this->data[1][2] = _mat(1, 2); 615 616 this->data[2][0] = _mat(2, 0); 617 this->data[2][1] = _mat(2, 1); 618 this->data[2][2] = _mat(2, 2); 619 620 return *this; 621 } 622 623 /// \brief Multiplication assignment operator. This matrix will 624 /// become equal to this * _m2. 625 /// \param _mat Incoming matrix. 626 /// \return This matrix * _mat. operator *=(const Matrix4<T> & _m2)627 public: Matrix4<T> operator*=(const Matrix4<T> &_m2) 628 { 629 (*this) = (*this) * _m2; 630 return *this; 631 } 632 633 /// \brief Multiplication operator 634 /// \param _mat Incoming matrix 635 /// \return This matrix * _mat operator *(const Matrix4<T> & _m2) const636 public: Matrix4<T> operator*(const Matrix4<T> &_m2) const 637 { 638 return Matrix4<T>( 639 this->data[0][0] * _m2(0, 0) + 640 this->data[0][1] * _m2(1, 0) + 641 this->data[0][2] * _m2(2, 0) + 642 this->data[0][3] * _m2(3, 0), 643 644 this->data[0][0] * _m2(0, 1) + 645 this->data[0][1] * _m2(1, 1) + 646 this->data[0][2] * _m2(2, 1) + 647 this->data[0][3] * _m2(3, 1), 648 649 this->data[0][0] * _m2(0, 2) + 650 this->data[0][1] * _m2(1, 2) + 651 this->data[0][2] * _m2(2, 2) + 652 this->data[0][3] * _m2(3, 2), 653 654 this->data[0][0] * _m2(0, 3) + 655 this->data[0][1] * _m2(1, 3) + 656 this->data[0][2] * _m2(2, 3) + 657 this->data[0][3] * _m2(3, 3), 658 659 this->data[1][0] * _m2(0, 0) + 660 this->data[1][1] * _m2(1, 0) + 661 this->data[1][2] * _m2(2, 0) + 662 this->data[1][3] * _m2(3, 0), 663 664 this->data[1][0] * _m2(0, 1) + 665 this->data[1][1] * _m2(1, 1) + 666 this->data[1][2] * _m2(2, 1) + 667 this->data[1][3] * _m2(3, 1), 668 669 this->data[1][0] * _m2(0, 2) + 670 this->data[1][1] * _m2(1, 2) + 671 this->data[1][2] * _m2(2, 2) + 672 this->data[1][3] * _m2(3, 2), 673 674 this->data[1][0] * _m2(0, 3) + 675 this->data[1][1] * _m2(1, 3) + 676 this->data[1][2] * _m2(2, 3) + 677 this->data[1][3] * _m2(3, 3), 678 679 this->data[2][0] * _m2(0, 0) + 680 this->data[2][1] * _m2(1, 0) + 681 this->data[2][2] * _m2(2, 0) + 682 this->data[2][3] * _m2(3, 0), 683 684 this->data[2][0] * _m2(0, 1) + 685 this->data[2][1] * _m2(1, 1) + 686 this->data[2][2] * _m2(2, 1) + 687 this->data[2][3] * _m2(3, 1), 688 689 this->data[2][0] * _m2(0, 2) + 690 this->data[2][1] * _m2(1, 2) + 691 this->data[2][2] * _m2(2, 2) + 692 this->data[2][3] * _m2(3, 2), 693 694 this->data[2][0] * _m2(0, 3) + 695 this->data[2][1] * _m2(1, 3) + 696 this->data[2][2] * _m2(2, 3) + 697 this->data[2][3] * _m2(3, 3), 698 699 this->data[3][0] * _m2(0, 0) + 700 this->data[3][1] * _m2(1, 0) + 701 this->data[3][2] * _m2(2, 0) + 702 this->data[3][3] * _m2(3, 0), 703 704 this->data[3][0] * _m2(0, 1) + 705 this->data[3][1] * _m2(1, 1) + 706 this->data[3][2] * _m2(2, 1) + 707 this->data[3][3] * _m2(3, 1), 708 709 this->data[3][0] * _m2(0, 2) + 710 this->data[3][1] * _m2(1, 2) + 711 this->data[3][2] * _m2(2, 2) + 712 this->data[3][3] * _m2(3, 2), 713 714 this->data[3][0] * _m2(0, 3) + 715 this->data[3][1] * _m2(1, 3) + 716 this->data[3][2] * _m2(2, 3) + 717 this->data[3][3] * _m2(3, 3)); 718 } 719 720 /// \brief Multiplication operator 721 /// \param _vec Vector3 722 /// \return Resulting vector from multiplication operator *(const Vector3<T> & _vec) const723 public: Vector3<T> operator*(const Vector3<T> &_vec) const 724 { 725 return Vector3<T>( 726 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() + 727 this->data[0][2]*_vec.Z() + this->data[0][3], 728 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() + 729 this->data[1][2]*_vec.Z() + this->data[1][3], 730 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() + 731 this->data[2][2]*_vec.Z() + this->data[2][3]); 732 } 733 734 /// \brief Get the value at the specified row, column index 735 /// \param[in] _col The column index. Index values are clamped to a 736 /// range of [0, 3]. 737 /// \param[in] _row the row index. Index values are clamped to a 738 /// range of [0, 3]. 739 /// \return The value at the specified index operator ()(const size_t _row,const size_t _col) const740 public: inline const T &operator()(const size_t _row, 741 const size_t _col) const 742 { 743 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)][ 744 clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; 745 } 746 747 /// \brief Get a mutable version the value at the specified row, 748 /// column index 749 /// \param[in] _col The column index. Index values are clamped to a 750 /// range of [0, 3]. 751 /// \param[in] _row the row index. Index values are clamped to a 752 /// range of [0, 3]. 753 /// \return The value at the specified index operator ()(const size_t _row,const size_t _col)754 public: inline T &operator()(const size_t _row, const size_t _col) 755 { 756 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)] 757 [clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; 758 } 759 760 /// \brief Equality test with tolerance. 761 /// \param[in] _m the matrix to compare to 762 /// \param[in] _tol equality tolerance. 763 /// \return true if the elements of the matrices are equal within 764 /// the tolerence specified by _tol. Equal(const Matrix4 & _m,const T & _tol) const765 public: bool Equal(const Matrix4 &_m, const T &_tol) const 766 { 767 return equal<T>(this->data[0][0], _m(0, 0), _tol) 768 && equal<T>(this->data[0][1], _m(0, 1), _tol) 769 && equal<T>(this->data[0][2], _m(0, 2), _tol) 770 && equal<T>(this->data[0][3], _m(0, 3), _tol) 771 && equal<T>(this->data[1][0], _m(1, 0), _tol) 772 && equal<T>(this->data[1][1], _m(1, 1), _tol) 773 && equal<T>(this->data[1][2], _m(1, 2), _tol) 774 && equal<T>(this->data[1][3], _m(1, 3), _tol) 775 && equal<T>(this->data[2][0], _m(2, 0), _tol) 776 && equal<T>(this->data[2][1], _m(2, 1), _tol) 777 && equal<T>(this->data[2][2], _m(2, 2), _tol) 778 && equal<T>(this->data[2][3], _m(2, 3), _tol) 779 && equal<T>(this->data[3][0], _m(3, 0), _tol) 780 && equal<T>(this->data[3][1], _m(3, 1), _tol) 781 && equal<T>(this->data[3][2], _m(3, 2), _tol) 782 && equal<T>(this->data[3][3], _m(3, 3), _tol); 783 } 784 785 /// \brief Equality operator 786 /// \param[in] _m Matrix3 to test 787 /// \return true if the 2 matrices are equal (using the tolerance 1e-6), 788 /// false otherwise operator ==(const Matrix4<T> & _m) const789 public: bool operator==(const Matrix4<T> &_m) const 790 { 791 return this->Equal(_m, static_cast<T>(1e-6)); 792 } 793 794 /// \brief Inequality test operator 795 /// \param[in] _m Matrix4<T> to test 796 /// \return True if not equal (using the default tolerance of 1e-6) operator !=(const Matrix4<T> & _m) const797 public: bool operator!=(const Matrix4<T> &_m) const 798 { 799 return !(*this == _m); 800 } 801 802 /// \brief Stream insertion operator 803 /// \param _out output stream 804 /// \param _m Matrix to output 805 /// \return the stream operator <<(std::ostream & _out,const ignition::math::Matrix4<T> & _m)806 public: friend std::ostream &operator<<( 807 std::ostream &_out, const ignition::math::Matrix4<T> &_m) 808 { 809 _out << precision(_m(0, 0), 6) << " " 810 << precision(_m(0, 1), 6) << " " 811 << precision(_m(0, 2), 6) << " " 812 << precision(_m(0, 3), 6) << " " 813 << precision(_m(1, 0), 6) << " " 814 << precision(_m(1, 1), 6) << " " 815 << precision(_m(1, 2), 6) << " " 816 << precision(_m(1, 3), 6) << " " 817 << precision(_m(2, 0), 6) << " " 818 << precision(_m(2, 1), 6) << " " 819 << precision(_m(2, 2), 6) << " " 820 << precision(_m(2, 3), 6) << " " 821 << precision(_m(3, 0), 6) << " " 822 << precision(_m(3, 1), 6) << " " 823 << precision(_m(3, 2), 6) << " " 824 << precision(_m(3, 3), 6); 825 826 return _out; 827 } 828 829 /// \brief Stream extraction operator 830 /// \param _in input stream 831 /// \param _pt Matrix4<T> to read values into 832 /// \return the stream operator >>(std::istream & _in,ignition::math::Matrix4<T> & _m)833 public: friend std::istream &operator>>( 834 std::istream &_in, ignition::math::Matrix4<T> &_m) 835 { 836 // Skip white spaces 837 _in.setf(std::ios_base::skipws); 838 T d[16]; 839 _in >> d[0] >> d[1] >> d[2] >> d[3] 840 >> d[4] >> d[5] >> d[6] >> d[7] 841 >> d[8] >> d[9] >> d[10] >> d[11] 842 >> d[12] >> d[13] >> d[14] >> d[15]; 843 844 _m.Set(d[0], d[1], d[2], d[3], 845 d[4], d[5], d[6], d[7], 846 d[8], d[9], d[10], d[11], 847 d[12], d[13], d[14], d[15]); 848 return _in; 849 } 850 851 /// \brief Get transform which translates to _eye and rotates the X axis 852 /// so it faces the _target. The rotation is such that Z axis is in the 853 /// _up direction, if possible. The coordinate system is right-handed, 854 /// \param[in] _eye Coordinate frame translation. 855 /// \param[in] _target Point which the X axis should face. If _target is 856 /// equal to _eye, the X axis won't be rotated. 857 /// \param[in] _up Direction in which the Z axis should point. If _up is 858 /// zero or parallel to X, it will be set to +Z. 859 /// \return Transformation matrix. LookAt(const Vector3<T> & _eye,const Vector3<T> & _target,const Vector3<T> & _up=Vector3<T>::UnitZ)860 public: static Matrix4<T> LookAt(const Vector3<T> &_eye, 861 const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ) 862 { 863 // Most important constraint: direction to point X axis at 864 auto front = _target - _eye; 865 866 // Case when _eye == _target 867 if (front == Vector3<T>::Zero) 868 front = Vector3<T>::UnitX; 869 front.Normalize(); 870 871 // Desired direction to point Z axis at 872 auto up = _up; 873 874 // Case when _up == Zero 875 if (up == Vector3<T>::Zero) 876 up = Vector3<T>::UnitZ; 877 else 878 up.Normalize(); 879 880 // Case when _up is parallel to X 881 if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero) 882 up = Vector3<T>::UnitZ; 883 884 // Find direction to point Y axis at 885 auto left = up.Cross(front); 886 887 // Case when front is parallel to up 888 if (left == Vector3<T>::Zero) 889 left = Vector3<T>::UnitY; 890 else 891 left.Normalize(); 892 893 // Fix up direction so it's perpendicular to XY 894 up = (front.Cross(left)).Normalize(); 895 896 return Matrix4<T>( 897 front.X(), left.X(), up.X(), _eye.X(), 898 front.Y(), left.Y(), up.Y(), _eye.Y(), 899 front.Z(), left.Z(), up.Z(), _eye.Z(), 900 0, 0, 0, 1); 901 } 902 903 /// \brief The 4x4 matrix 904 private: T data[4][4]; 905 }; 906 907 template<typename T> 908 const Matrix4<T> Matrix4<T>::Identity( 909 1, 0, 0, 0, 910 0, 1, 0, 0, 911 0, 0, 1, 0, 912 0, 0, 0, 1); 913 914 template<typename T> 915 const Matrix4<T> Matrix4<T>::Zero( 916 0, 0, 0, 0, 917 0, 0, 0, 0, 918 0, 0, 0, 0, 919 0, 0, 0, 0); 920 921 typedef Matrix4<int> Matrix4i; 922 typedef Matrix4<double> Matrix4d; 923 typedef Matrix4<float> Matrix4f; 924 } 925 } 926 } 927 #endif 928