dali/public-api/math/matrix.cpp

   1 /*
   2  * Copyright (c) 2023 Samsung Electronics Co., Ltd.
   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
  18 // CLASS HEADERS
  19 #include <dali/public-api/math/matrix.h>
  20
  21 // EXTERNAL INCLUDES
  22 #include <cmath>
  23 #include <cstdint> // uint32_t
  24 #include <cstring> // memcpy
  25 #include <ostream>
  26
  27 // INTERNAL INCLUDES
  28 #include <dali/internal/common/matrix-utils.h>
  29 #include <dali/internal/render/common/performance-monitor.h>
  30 #include <dali/public-api/common/dali-common.h>
  31 #include <dali/public-api/math/math-utils.h>
  32 #include <dali/public-api/math/quaternion.h>
  33 #include <dali/public-api/math/vector3.h>
  34 #include <dali/public-api/math/vector4.h>
  35
  36 namespace
  37 {
  38 const float ROTATION_EPSILON = 0.003f; // Deliberately large
  39
  40 const uint32_t NUM_BYTES_IN_ROW_OF_3(3 * sizeof(float));
  41 const uint32_t NUM_BYTES_IN_ROW(4 * sizeof(float));
  42 const uint32_t NUM_BYTES_IN_MATRIX(16 * sizeof(float));
  43 const uint32_t ROW1_OFFSET(4);
  44 const uint32_t ROW2_OFFSET(8);
  45 const uint32_t ROW3_OFFSET(12);
  46 } // namespace
  47
  48 namespace Dali
  49 {
  50 using Internal::PerformanceMonitor;
  51
  52 // clang-format off
  53 const float identityArray[] = {
  54   1.0f, 0.0f, 0.0f, 0.0f,
  55   0.0f, 1.0f, 0.0f, 0.0f,
  56   0.0f, 0.0f, 1.0f, 0.0f,
  57   0.0f, 0.0f, 0.0f, 1.0f};
  58 // clang-format on
  59
  60 const Matrix Matrix::IDENTITY(identityArray);
  61
  62 Matrix::Matrix()
  63 {
  64   memset(mMatrix, 0, NUM_BYTES_IN_MATRIX);
  65 }
  66
  67 Matrix::Matrix(bool initialize)
  68 {
  69   if(initialize)
  70   {
  71     memset(mMatrix, 0, NUM_BYTES_IN_MATRIX);
  72   }
  73 }
  74
  75 Matrix::Matrix(const float* array)
  76 {
  77   memcpy(mMatrix, array, NUM_BYTES_IN_MATRIX);
  78 }
  79
  80 Matrix::Matrix(const Quaternion& rotation)
  81 {
  82   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 18);
  83
  84   float* matrixPtr = &mMatrix[0];
  85   Internal::MatrixUtils::ConvertQuaternion(matrixPtr, rotation);
  86 }
  87
  88 Matrix::Matrix(const Matrix& matrix)
  89 {
  90   memcpy(mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX);
  91 }
  92
  93 Matrix& Matrix::operator=(const Matrix& matrix)
  94 {
  95   // no point copying if self assigning
  96   if(this != &matrix)
  97   {
  98     memcpy(mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX);
  99   }
 100   return *this;
 101 }
 102
 103 Matrix::Matrix(Matrix&& matrix) noexcept
 104 {
 105   memcpy(mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX);
 106 }
 107
 108 Matrix& Matrix::operator=(Matrix&& matrix) noexcept
 109 {
 110   if(this != &matrix)
 111   {
 112     memcpy(mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX);
 113   }
 114   return *this;
 115 }
 116
 117 void Matrix::InvertTransform(Matrix& result) const
 118 {
 119   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
 120
 121   float* m1 = result.AsFloat();
 122
 123   DALI_ASSERT_ALWAYS(EqualsZero(mMatrix[3]) && EqualsZero(mMatrix[7]) && EqualsZero(mMatrix[11]) && Equals(mMatrix[15], 1.0f) && "Must be a transform matrix");
 124
 125   m1[0] = mMatrix[0];
 126   m1[1] = mMatrix[4];
 127   m1[2] = mMatrix[8];
 128   m1[3] = 0.0f;
 129
 130   m1[4] = mMatrix[1];
 131   m1[5] = mMatrix[5];
 132   m1[6] = mMatrix[9];
 133   m1[7] = 0.0f;
 134
 135   m1[8]  = mMatrix[2];
 136   m1[9]  = mMatrix[6];
 137   m1[10] = mMatrix[10];
 138   m1[11] = 0.0f;
 139
 140   m1[12] = -((mMatrix[0] * mMatrix[12]) + (mMatrix[1] * mMatrix[13]) + (mMatrix[2] * mMatrix[14]) + (mMatrix[3] * mMatrix[15]));
 141   m1[13] = -((mMatrix[4] * mMatrix[12]) + (mMatrix[5] * mMatrix[13]) + (mMatrix[6] * mMatrix[14]) + (mMatrix[7] * mMatrix[15]));
 142   m1[14] = -((mMatrix[8] * mMatrix[12]) + (mMatrix[9] * mMatrix[13]) + (mMatrix[10] * mMatrix[14]) + (mMatrix[11] * mMatrix[15]));
 143   m1[15] = 1.0f;
 144 }
 145
 146 static bool InvertMatrix(const float* m, float* out)
 147 {
 148   float inv[16];
 149
 150   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 192); // 12 x 16 multiples
 151
 152   inv[0]  = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
 153   inv[4]  = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
 154   inv[8]  = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
 155   inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
 156   inv[1]  = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
 157   inv[5]  = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
 158   inv[9]  = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
 159   inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
 160   inv[2]  = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
 161   inv[6]  = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
 162   inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
 163   inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
 164   inv[3]  = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
 165   inv[7]  = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
 166   inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
 167   inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
 168
 169   float det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
 170
 171   // In the case where the determinant is exactly zero, the matrix is non-invertible
 172   if(EqualsZero(det))
 173   {
 174     return false;
 175   }
 176
 177   det = 1.0f / det;
 178
 179   for(int32_t i = 0; i < 16; i++)
 180   {
 181     out[i] = inv[i] * det;
 182   }
 183
 184   return true;
 185 }
 186
 187 bool Matrix::Invert()
 188 {
 189   Matrix temp(*this);
 190
 191   return InvertMatrix(temp.AsFloat(), mMatrix);
 192 }
 193
 194 void Matrix::Transpose()
 195 {
 196   float temp = mMatrix[1];
 197   mMatrix[1] = mMatrix[4];
 198   mMatrix[4] = temp;
 199
 200   temp       = mMatrix[2];
 201   mMatrix[2] = mMatrix[8];
 202   mMatrix[8] = temp;
 203
 204   temp        = mMatrix[3];
 205   mMatrix[3]  = mMatrix[12];
 206   mMatrix[12] = temp;
 207
 208   temp       = mMatrix[6];
 209   mMatrix[6] = mMatrix[9];
 210   mMatrix[9] = temp;
 211
 212   temp        = mMatrix[7];
 213   mMatrix[7]  = mMatrix[13];
 214   mMatrix[13] = temp;
 215
 216   temp        = mMatrix[11];
 217   mMatrix[11] = mMatrix[14];
 218   mMatrix[14] = temp;
 219 }
 220
 221 void Matrix::SetIdentity()
 222 {
 223   memcpy(mMatrix, identityArray, NUM_BYTES_IN_MATRIX);
 224 }
 225
 226 void Matrix::SetIdentityAndScale(const Vector3& scale)
 227 {
 228   // initialize to zeros
 229   memset(mMatrix, 0, NUM_BYTES_IN_MATRIX);
 230
 231   // just apply scale on the diagonal
 232   mMatrix[0]  = scale.x;
 233   mMatrix[5]  = scale.y;
 234   mMatrix[10] = scale.z;
 235   mMatrix[15] = 1.0f;
 236 }
 237
 238 void Matrix::SetTranslation(const Vector4& translation)
 239 {
 240   memcpy(mMatrix + ROW3_OFFSET, &translation, NUM_BYTES_IN_ROW);
 241 }
 242 void Matrix::SetTranslation(const Vector3& other)
 243 {
 244   memcpy(mMatrix + ROW3_OFFSET, &other, NUM_BYTES_IN_ROW_OF_3);
 245   mMatrix[15] = 1.0f;
 246 }
 247
 248 void Matrix::Multiply(Matrix& result, const Matrix& lhs, const Matrix& rhs)
 249 {
 250   Internal::MatrixUtils::Multiply(result, lhs, rhs);
 251 }
 252
 253 void Matrix::Multiply(Matrix& result, const Matrix& lhs, const Quaternion& rhs)
 254 {
 255   Internal::MatrixUtils::Multiply(result, lhs, rhs);
 256 }
 257
 258 Matrix Matrix::operator*(const Matrix& rhs) const
 259 {
 260   Matrix result(false);
 261   Internal::MatrixUtils::Multiply(result, rhs, *this);
 262   return result;
 263 }
 264
 265 Matrix& Matrix::operator*=(const Matrix& rhs)
 266 {
 267   Internal::MatrixUtils::MultiplyAssign(*this, rhs);
 268   return *this;
 269 }
 270
 271 Vector4 Matrix::operator*(const Vector4& rhs) const
 272 {
 273   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 16);
 274
 275   Vector4 temp;
 276
 277 #ifndef __ARM_NEON__
 278
 279   temp.x = rhs.x * mMatrix[0] + rhs.y * mMatrix[4] + rhs.z * mMatrix[8] + rhs.w * mMatrix[12];
 280   temp.y = rhs.x * mMatrix[1] + rhs.y * mMatrix[5] + rhs.z * mMatrix[9] + rhs.w * mMatrix[13];
 281   temp.z = rhs.x * mMatrix[2] + rhs.y * mMatrix[6] + rhs.z * mMatrix[10] + rhs.w * mMatrix[14];
 282   temp.w = rhs.x * mMatrix[3] + rhs.y * mMatrix[7] + rhs.z * mMatrix[11] + rhs.w * mMatrix[15];
 283
 284 #else
 285
 286   // 64 32bit registers,
 287   // aliased to
 288   // d = 64 bit double-word d0 -d31
 289   // q =128 bit quad-word   q0 -q15  (enough to handle a column of 4 floats in a matrix)
 290   // e.g. q0 = d0 and d1
 291   // load and stores interleaved as NEON can load and store while calculating
 292   asm volatile(
 293     "VLD1.F32     {q0}, [%1]        \n\t" //q0 = rhs
 294     "VLD1.F32     {q9}, [%0]!       \n\t"
 295     "VMUL.F32     q10,  q9,   d0[0] \n\t"
 296     "VLD1.F32     {q9}, [%0]!       \n\t"
 297     "VMLA.F32     q10,  q9,   d0[1] \n\t" //q10 = mMatrix[0..3] * rhs + mMatrix[4..7] * rhs
 298     "VLD1.F32     {q9}, [%0]!       \n\t"
 299     "VMUL.F32     q11,  q9,   d1[0] \n\t"
 300     "VLD1.F32     {q9}, [%0]!       \n\t"
 301     "VMLA.F32     q11,  q9,   d1[1] \n\t" //q11 = mMatrix[8..11] * rhs + mMatrix[12..15] * rhs
 302     "VADD.F32     q10,  q10,  q11   \n\t"
 303     "VST1.F32     {q10},[%2]        \n\t" //temp = q10 + q11
 304     :
 305     : "r"(mMatrix), "r"(&rhs), "r"(&temp)
 306     : "q0", "q9", "q10", "q11", "memory");
 307 #endif
 308   return temp;
 309 }
 310
 311 bool Matrix::operator==(const Matrix& rhs) const
 312 {
 313   return (
 314     (fabsf(mMatrix[0] - rhs.mMatrix[0]) <= GetRangedEpsilon(mMatrix[0], rhs.mMatrix[0])) &&
 315     (fabsf(mMatrix[1] - rhs.mMatrix[1]) <= GetRangedEpsilon(mMatrix[1], rhs.mMatrix[1])) &&
 316     (fabsf(mMatrix[2] - rhs.mMatrix[2]) <= GetRangedEpsilon(mMatrix[2], rhs.mMatrix[2])) &&
 317     (fabsf(mMatrix[3] - rhs.mMatrix[3]) <= GetRangedEpsilon(mMatrix[3], rhs.mMatrix[3])) &&
 318     (fabsf(mMatrix[4] - rhs.mMatrix[4]) <= GetRangedEpsilon(mMatrix[4], rhs.mMatrix[4])) &&
 319     (fabsf(mMatrix[5] - rhs.mMatrix[5]) <= GetRangedEpsilon(mMatrix[5], rhs.mMatrix[5])) &&
 320     (fabsf(mMatrix[6] - rhs.mMatrix[6]) <= GetRangedEpsilon(mMatrix[6], rhs.mMatrix[6])) &&
 321     (fabsf(mMatrix[7] - rhs.mMatrix[7]) <= GetRangedEpsilon(mMatrix[7], rhs.mMatrix[7])) &&
 322     (fabsf(mMatrix[8] - rhs.mMatrix[8]) <= GetRangedEpsilon(mMatrix[8], rhs.mMatrix[8])) &&
 323     (fabsf(mMatrix[9] - rhs.mMatrix[9]) <= GetRangedEpsilon(mMatrix[9], rhs.mMatrix[9])) &&
 324     (fabsf(mMatrix[10] - rhs.mMatrix[10]) <= GetRangedEpsilon(mMatrix[10], rhs.mMatrix[10])) &&
 325     (fabsf(mMatrix[11] - rhs.mMatrix[11]) <= GetRangedEpsilon(mMatrix[11], rhs.mMatrix[11])) &&
 326     (fabsf(mMatrix[12] - rhs.mMatrix[12]) <= GetRangedEpsilon(mMatrix[12], rhs.mMatrix[12])) &&
 327     (fabsf(mMatrix[13] - rhs.mMatrix[13]) <= GetRangedEpsilon(mMatrix[13], rhs.mMatrix[13])) &&
 328     (fabsf(mMatrix[14] - rhs.mMatrix[14]) <= GetRangedEpsilon(mMatrix[14], rhs.mMatrix[14])) &&
 329     (fabsf(mMatrix[15] - rhs.mMatrix[15]) <= GetRangedEpsilon(mMatrix[15], rhs.mMatrix[15])));
 330 }
 331
 332 bool Matrix::operator!=(const Matrix& rhs) const
 333 {
 334   if(*this == rhs)
 335   {
 336     return false;
 337   }
 338
 339   return true;
 340 }
 341
 342 void Matrix::OrthoNormalize()
 343 {
 344   Vector4 vector0(GetXAxis());
 345   Vector4 vector1(GetYAxis());
 346   Vector4 vector2(GetZAxis());
 347
 348   vector0.Normalize();
 349   vector1.Normalize();
 350   vector2 = vector0.Cross(vector1);
 351   vector1 = vector2.Cross(vector0);
 352
 353   memcpy(mMatrix, &vector0, NUM_BYTES_IN_ROW);
 354   memcpy(mMatrix + ROW1_OFFSET, &vector1, NUM_BYTES_IN_ROW);
 355   memcpy(mMatrix + ROW2_OFFSET, &vector2, NUM_BYTES_IN_ROW);
 356 }
 357
 358 Vector3 Matrix::GetXAxis() const
 359 {
 360   return Vector3(mMatrix[0], mMatrix[1], mMatrix[2]);
 361 }
 362
 363 Vector3 Matrix::GetYAxis() const
 364 {
 365   return Vector3(mMatrix[4], mMatrix[5], mMatrix[6]);
 366 }
 367
 368 Vector3 Matrix::GetZAxis() const
 369 {
 370   return Vector3(mMatrix[8], mMatrix[9], mMatrix[10]);
 371 }
 372
 373 void Matrix::SetXAxis(const Vector3& axis)
 374 {
 375   mMatrix[0] = axis.x;
 376   mMatrix[1] = axis.y;
 377   mMatrix[2] = axis.z;
 378 }
 379
 380 void Matrix::SetYAxis(const Vector3& axis)
 381 {
 382   mMatrix[4] = axis.x;
 383   mMatrix[5] = axis.y;
 384   mMatrix[6] = axis.z;
 385 }
 386
 387 void Matrix::SetZAxis(const Vector3& axis)
 388 {
 389   mMatrix[8]  = axis.x;
 390   mMatrix[9]  = axis.y;
 391   mMatrix[10] = axis.z;
 392 }
 393
 394 void Matrix::SetTransformComponents(const Vector3&    scale,
 395                                     const Quaternion& rotation,
 396                                     const Vector3&    translation)
 397 {
 398   if(rotation.IsIdentity())
 399   {
 400     mMatrix[0] = scale.x;
 401     mMatrix[1] = 0.0f;
 402     mMatrix[2] = 0.0f;
 403     mMatrix[3] = 0.0f;
 404
 405     mMatrix[4] = 0.0f;
 406     mMatrix[5] = scale.y;
 407     mMatrix[6] = 0.0f;
 408     mMatrix[7] = 0.0f;
 409
 410     mMatrix[8]  = 0.0f;
 411     mMatrix[9]  = 0.0f;
 412     mMatrix[10] = scale.z;
 413     mMatrix[11] = 0.0f;
 414   }
 415   else
 416   {
 417     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 418     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 27); // 27 = 9+18
 419
 420     const float xx = rotation.mVector.x * rotation.mVector.x;
 421     const float yy = rotation.mVector.y * rotation.mVector.y;
 422     const float zz = rotation.mVector.z * rotation.mVector.z;
 423     const float xy = rotation.mVector.x * rotation.mVector.y;
 424     const float xz = rotation.mVector.x * rotation.mVector.z;
 425     const float wx = rotation.mVector.w * rotation.mVector.x;
 426     const float wy = rotation.mVector.w * rotation.mVector.y;
 427     const float wz = rotation.mVector.w * rotation.mVector.z;
 428     const float yz = rotation.mVector.y * rotation.mVector.z;
 429
 430     mMatrix[0] = (scale.x * (1.0f - 2.0f * (yy + zz)));
 431     mMatrix[1] = (scale.x * (2.0f * (xy + wz)));
 432     mMatrix[2] = (scale.x * (2.0f * (xz - wy)));
 433     mMatrix[3] = 0.0f;
 434
 435     mMatrix[4] = (scale.y * (2.0f * (xy - wz)));
 436     mMatrix[5] = (scale.y * (1.0f - 2.0f * (xx + zz)));
 437     mMatrix[6] = (scale.y * (2.0f * (yz + wx)));
 438     mMatrix[7] = 0.0f;
 439
 440     mMatrix[8]  = (scale.z * (2.0f * (xz + wy)));
 441     mMatrix[9]  = (scale.z * (2.0f * (yz - wx)));
 442     mMatrix[10] = (scale.z * (1.0f - 2.0f * (xx + yy)));
 443     mMatrix[11] = 0.0f;
 444   }
 445   // apply translation
 446   mMatrix[12] = translation.x;
 447   mMatrix[13] = translation.y;
 448   mMatrix[14] = translation.z;
 449   mMatrix[15] = 1.0f;
 450 }
 451
 452 void Matrix::SetInverseTransformComponents(const Vector3&    scale,
 453                                            const Quaternion& rotation,
 454                                            const Vector3&    translation)
 455 {
 456   Vector3    inverseTranslation = -translation;
 457   Vector3    inverseScale(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z);
 458   Quaternion inverseRotation(rotation);
 459   bool       isRotated = !inverseRotation.IsIdentity();
 460
 461   // Order of application is translation, rotation, scale.
 462   // Ensure translation is relative to scale & rotation:
 463
 464   if(isRotated)
 465   {
 466     inverseRotation.Invert();
 467     inverseTranslation = inverseRotation.Rotate(inverseTranslation);
 468   }
 469
 470   inverseTranslation *= inverseScale;
 471
 472   if(isRotated)
 473   {
 474     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 475     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 27); // 27 = 9+18
 476
 477     const float xx = inverseRotation.mVector.x * inverseRotation.mVector.x;
 478     const float yy = inverseRotation.mVector.y * inverseRotation.mVector.y;
 479     const float zz = inverseRotation.mVector.z * inverseRotation.mVector.z;
 480     const float xy = inverseRotation.mVector.x * inverseRotation.mVector.y;
 481     const float xz = inverseRotation.mVector.x * inverseRotation.mVector.z;
 482     const float wx = inverseRotation.mVector.w * inverseRotation.mVector.x;
 483     const float wy = inverseRotation.mVector.w * inverseRotation.mVector.y;
 484     const float wz = inverseRotation.mVector.w * inverseRotation.mVector.z;
 485     const float yz = inverseRotation.mVector.y * inverseRotation.mVector.z;
 486
 487     mMatrix[0] = (inverseScale.x * (1.0f - 2.0f * (yy + zz)));
 488     mMatrix[1] = (inverseScale.y * (2.0f * (xy + wz)));
 489     mMatrix[2] = (inverseScale.z * (2.0f * (xz - wy)));
 490     mMatrix[3] = 0.0f;
 491
 492     mMatrix[4] = (inverseScale.x * (2.0f * (xy - wz)));
 493     mMatrix[5] = (inverseScale.y * (1.0f - 2.0f * (xx + zz)));
 494     mMatrix[6] = (inverseScale.z * (2.0f * (yz + wx)));
 495     mMatrix[7] = 0.0f;
 496
 497     mMatrix[8]  = (inverseScale.x * (2.0f * (xz + wy)));
 498     mMatrix[9]  = (inverseScale.y * (2.0f * (yz - wx)));
 499     mMatrix[10] = (inverseScale.z * (1.0f - 2.0f * (xx + yy)));
 500     mMatrix[11] = 0.0f;
 501   }
 502   else
 503   {
 504     mMatrix[0] = inverseScale.x;
 505     mMatrix[1] = 0.0f;
 506     mMatrix[2] = 0.0f;
 507     mMatrix[3] = 0.0f;
 508
 509     mMatrix[4] = 0.0f;
 510     mMatrix[5] = inverseScale.y;
 511     mMatrix[6] = 0.0f;
 512     mMatrix[7] = 0.0f;
 513
 514     mMatrix[8]  = 0.0f;
 515     mMatrix[9]  = 0.0f;
 516     mMatrix[10] = inverseScale.z;
 517     mMatrix[11] = 0.0f;
 518   }
 519
 520   // apply translation
 521   mMatrix[12] = inverseTranslation.x;
 522   mMatrix[13] = inverseTranslation.y;
 523   mMatrix[14] = inverseTranslation.z;
 524   mMatrix[15] = 1.0f;
 525 }
 526
 527 void Matrix::SetInverseTransformComponents(const Vector3& xAxis,
 528                                            const Vector3& yAxis,
 529                                            const Vector3& zAxis,
 530                                            const Vector3& translation)
 531 {
 532   // x, y, z axis parameters represent a orthonormal basis with no scaling, i.e. a rotation matrix.
 533   // Invert rotation by transposing in place
 534
 535   // Order of application is translation, rotation
 536
 537   mMatrix[0] = xAxis.x;
 538   mMatrix[1] = yAxis.x;
 539   mMatrix[2] = zAxis.x;
 540   mMatrix[3] = 0.0f;
 541
 542   mMatrix[4] = xAxis.y;
 543   mMatrix[5] = yAxis.y;
 544   mMatrix[6] = zAxis.y;
 545   mMatrix[7] = 0.0f;
 546
 547   mMatrix[8]  = xAxis.z;
 548   mMatrix[9]  = yAxis.z;
 549   mMatrix[10] = zAxis.z;
 550   mMatrix[11] = 0.0f;
 551   mMatrix[12] = 0.0f;
 552   mMatrix[13] = 0.0f;
 553   mMatrix[14] = 0.0f;
 554   mMatrix[15] = 1.0f;
 555
 556   // Ensure translation is relative to scale & rotation:
 557
 558   Vector4 inverseTranslation(-translation.x, -translation.y, -translation.z, 1.0f);
 559   inverseTranslation   = *this * inverseTranslation; // Rotate inverse translation
 560   inverseTranslation.w = 1.0f;
 561   SetTranslation(inverseTranslation);
 562 }
 563
 564 Vector3 Matrix::GetScale() const
 565 {
 566   // Derive scale from axis lengths.
 567   return Vector3(GetXAxis().Length(), GetYAxis().Length(), GetZAxis().Length());
 568 }
 569
 570 void Matrix::GetTransformComponents(Vector3&    position,
 571                                     Quaternion& rotation,
 572                                     Vector3&    scale) const
 573 {
 574   position = GetTranslation3();
 575   scale    = GetScale();
 576
 577   if(!(fabs(scale.x - Vector3::ONE.x) < ROTATION_EPSILON &&
 578        fabs(scale.y - Vector3::ONE.y) < ROTATION_EPSILON &&
 579        fabs(scale.z - Vector3::ONE.z) < ROTATION_EPSILON))
 580   {
 581     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 582     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 9);
 583
 584     // Non-identity scale is embedded into rotation matrix. Remove it first:
 585     Matrix  m(*this);
 586     Vector3 inverseScale(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z);
 587     m.mMatrix[0] *= inverseScale.x;
 588     m.mMatrix[1] *= inverseScale.x;
 589     m.mMatrix[2] *= inverseScale.x;
 590     m.mMatrix[4] *= inverseScale.y;
 591     m.mMatrix[5] *= inverseScale.y;
 592     m.mMatrix[6] *= inverseScale.y;
 593     m.mMatrix[8] *= inverseScale.z;
 594     m.mMatrix[9] *= inverseScale.z;
 595     m.mMatrix[10] *= inverseScale.z;
 596
 597     Quaternion theRotation(m);
 598
 599     // If the imaginary components are close to zero, then use null quaternion instead.
 600     if(fabs(theRotation.mVector.x) < ROTATION_EPSILON &&
 601        fabs(theRotation.mVector.y) < ROTATION_EPSILON &&
 602        fabs(theRotation.mVector.z) < ROTATION_EPSILON)
 603     {
 604       theRotation = Quaternion();
 605     }
 606     rotation = theRotation;
 607   }
 608   else
 609   {
 610     Quaternion theRotation(*this);
 611
 612     // If the imaginary components are close to zero, then use null quaternion instead.
 613     if(fabs(theRotation.mVector.x) < ROTATION_EPSILON &&
 614        fabs(theRotation.mVector.y) < ROTATION_EPSILON &&
 615        fabs(theRotation.mVector.z) < ROTATION_EPSILON)
 616     {
 617       theRotation = Quaternion();
 618     }
 619     rotation = theRotation;
 620   }
 621 }
 622
 623 std::ostream& operator<<(std::ostream& o, const Matrix& matrix)
 624 {
 625   return o << "[ " << matrix.mMatrix[0] << ", " << matrix.mMatrix[1] << ", " << matrix.mMatrix[2] << ", " << matrix.mMatrix[3] << ", "
 626            << matrix.mMatrix[4] << ", " << matrix.mMatrix[5] << ", " << matrix.mMatrix[6] << ", " << matrix.mMatrix[7] << ", "
 627            << matrix.mMatrix[8] << ", " << matrix.mMatrix[9] << ", " << matrix.mMatrix[10] << ", " << matrix.mMatrix[11] << ", "
 628            << matrix.mMatrix[12] << ", " << matrix.mMatrix[13] << ", " << matrix.mMatrix[14] << ", " << matrix.mMatrix[15] << " ]";
 629 }
 630
 631 } // namespace Dali