dali/public-api/math/matrix.cpp

   1 /*
   2  * Copyright (c) 2018 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 <cstring> // for memcpy
  24 #include <ostream>
  25
  26 // INTERNAL INCLUDES
  27 #include <dali/public-api/common/dali-common.h>
  28 #include <dali/public-api/math/vector3.h>
  29 #include <dali/public-api/math/vector4.h>
  30 #include <dali/public-api/math/quaternion.h>
  31 #include <dali/public-api/math/math-utils.h>
  32 #include <dali/internal/render/common/performance-monitor.h>
  33
  34 namespace
  35 {
  36 const float ROTATION_EPSILON = 0.003f; // Deliberately large
  37
  38 const size_t NUM_BYTES_IN_ROW_OF_3( 3 * sizeof( float ) );
  39 const size_t NUM_BYTES_IN_ROW( 4 * sizeof( float ) );
  40 const size_t NUM_BYTES_IN_MATRIX( 16 * sizeof( float ) );
  41 const size_t ROW1_OFFSET( 4 );
  42 const size_t ROW2_OFFSET( 8 );
  43 const size_t ROW3_OFFSET( 12 );
  44
  45 /**
  46  * Helper to convert to Quaternion to float16 array
  47  */
  48 void Convert( float*& m, const Dali::Quaternion& rotation )
  49 {
  50   const float xx = rotation.mVector.x * rotation.mVector.x;
  51   const float yy = rotation.mVector.y * rotation.mVector.y;
  52   const float zz = rotation.mVector.z * rotation.mVector.z;
  53   const float xy = rotation.mVector.x * rotation.mVector.y;
  54   const float xz = rotation.mVector.x * rotation.mVector.z;
  55   const float wx = rotation.mVector.w * rotation.mVector.x;
  56   const float wy = rotation.mVector.w * rotation.mVector.y;
  57   const float wz = rotation.mVector.w * rotation.mVector.z;
  58   const float yz = rotation.mVector.y * rotation.mVector.z;
  59
  60   m[0] = 1.0f - 2.0f * (yy + zz);
  61   m[1] =        2.0f * (xy + wz);
  62   m[2] =        2.0f * (xz - wy);
  63   m[3] = 0.0f;
  64
  65   m[4] =        2.0f * (xy - wz);
  66   m[5] = 1.0f - 2.0f * (xx + zz);
  67   m[6] =        2.0f * (yz + wx);
  68   m[7] = 0.0f;
  69
  70   m[8] =        2.0f * (xz + wy);
  71   m[9] =        2.0f * (yz - wx);
  72   m[10]= 1.0f - 2.0f * (xx + yy);
  73   m[11]= 0.0f;
  74
  75   m[12]= 0.0f;
  76   m[13]= 0.0f;
  77   m[14]= 0.0f;
  78   m[15]= 1.0f;
  79 }
  80 }
  81
  82 namespace Dali
  83 {
  84
  85 using Internal::PerformanceMonitor;
  86
  87 const float identityArray[] = {1.0f, 0.0f, 0.0f, 0.0f,
  88                                0.0f, 1.0f, 0.0f, 0.0f,
  89                                0.0f, 0.0f, 1.0f, 0.0f,
  90                                0.0f, 0.0f, 0.0f, 1.0f};
  91
  92 const Matrix Matrix::IDENTITY(identityArray);
  93
  94 Matrix::Matrix()
  95 {
  96   memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
  97 }
  98
  99 Matrix::Matrix( bool initialize )
 100 {
 101   if( initialize )
 102   {
 103     memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
 104   }
 105 }
 106
 107 Matrix::Matrix(const float* array)
 108 {
 109   memcpy( mMatrix, array, NUM_BYTES_IN_MATRIX );
 110 }
 111
 112 Matrix::Matrix( const Quaternion& rotation )
 113 {
 114   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,18);
 115
 116   float* matrixPtr = &mMatrix[0];
 117   Convert( matrixPtr, rotation );
 118 }
 119
 120 Matrix::Matrix( const Matrix& matrix )
 121 {
 122   memcpy( mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX );
 123 }
 124
 125 Matrix& Matrix::operator=( const Matrix& matrix )
 126 {
 127   // no point copying if self assigning
 128   if( this != &matrix )
 129   {
 130     memcpy( mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX );
 131   }
 132   return *this;
 133 }
 134
 135 void Matrix::InvertTransform(Matrix& result) const
 136 {
 137   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 138
 139   float* m1 = result.AsFloat();
 140
 141   DALI_ASSERT_ALWAYS( EqualsZero( mMatrix[3] ) && EqualsZero( mMatrix[7] ) && EqualsZero( mMatrix[11] ) && Equals( mMatrix[15], 1.0f ) && "Must be a transform matrix" );
 142
 143   m1[0] = mMatrix[0];
 144   m1[1] = mMatrix[4];
 145   m1[2] = mMatrix[8];
 146   m1[3] = 0.0f;
 147
 148   m1[4] = mMatrix[1];
 149   m1[5] = mMatrix[5];
 150   m1[6] = mMatrix[9];
 151   m1[7] = 0.0f;
 152
 153   m1[8] = mMatrix[2];
 154   m1[9] = mMatrix[6];
 155   m1[10] = mMatrix[10];
 156   m1[11] = 0.0f;
 157
 158   m1[12] = -( ( mMatrix[0] * mMatrix[12] ) + ( mMatrix[1] * mMatrix[13] ) + ( mMatrix[2] * mMatrix[14] ) + ( mMatrix[3] * mMatrix[15] ) );
 159   m1[13] = -( ( mMatrix[4] * mMatrix[12] ) + ( mMatrix[5] * mMatrix[13] ) + ( mMatrix[6] * mMatrix[14] ) + ( mMatrix[7] * mMatrix[15] ) );
 160   m1[14] = -( ( mMatrix[8] * mMatrix[12] ) + ( mMatrix[9] * mMatrix[13] ) + ( mMatrix[10] * mMatrix[14] ) + ( mMatrix[11] * mMatrix[15] ) );
 161   m1[15] = 1.0f;
 162 }
 163
 164 static bool InvertMatrix(const float* m, float* out)
 165 {
 166   float inv[16];
 167
 168   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,192);  // 12 x 16 multiples
 169
 170   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];
 171   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];
 172   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];
 173   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];
 174   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];
 175   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];
 176   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];
 177   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];
 178   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];
 179   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];
 180   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];
 181   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];
 182   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];
 183   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];
 184   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];
 185   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];
 186
 187   float det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12];
 188
 189   // In the case where the determinant is exactly zero, the matrix is non-invertible
 190   if ( EqualsZero( det ) )
 191   {
 192     return false;
 193   }
 194
 195   det = 1.0f / det;
 196
 197   for( int32_t i = 0; i < 16; i++)
 198   {
 199     out[i] = inv[i] * det;
 200   }
 201
 202   return true;
 203 }
 204
 205 bool Matrix::Invert()
 206 {
 207   Matrix temp(*this);
 208
 209   return InvertMatrix(temp.AsFloat(), mMatrix);
 210 }
 211
 212 void Matrix::Transpose()
 213 {
 214   float temp = mMatrix[1];
 215   mMatrix[1] = mMatrix[4];
 216   mMatrix[4] = temp;
 217
 218   temp = mMatrix[2];
 219   mMatrix[2] = mMatrix[8];
 220   mMatrix[8] = temp;
 221
 222   temp = mMatrix[3];
 223   mMatrix[3] = mMatrix[12];
 224   mMatrix[12] = temp;
 225
 226   temp = mMatrix[6];
 227   mMatrix[6] = mMatrix[9];
 228   mMatrix[9] = temp;
 229
 230   temp = mMatrix[7];
 231   mMatrix[7] = mMatrix[13];
 232   mMatrix[13] = temp;
 233
 234   temp = mMatrix[11];
 235   mMatrix[11] = mMatrix[14];
 236   mMatrix[14] = temp;
 237 }
 238
 239 void Matrix::SetIdentity()
 240 {
 241   memcpy( mMatrix, identityArray, NUM_BYTES_IN_MATRIX );
 242 }
 243
 244 void Matrix::SetIdentityAndScale( const Vector3& scale )
 245 {
 246   // initialize to zeros
 247   memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
 248
 249   // just apply scale on the diagonal
 250   mMatrix[0]  = scale.x;
 251   mMatrix[5]  = scale.y;
 252   mMatrix[10] = scale.z;
 253   mMatrix[15] = 1.0f;
 254 }
 255
 256 void Matrix::SetTranslation(const Vector4& translation)
 257 {
 258   memcpy( mMatrix + ROW3_OFFSET, &translation, NUM_BYTES_IN_ROW );
 259 }
 260 void Matrix::SetTranslation(const Vector3& other)
 261 {
 262   memcpy( mMatrix + ROW3_OFFSET, &other, NUM_BYTES_IN_ROW_OF_3 );
 263   mMatrix[15] = 1.0f;
 264 }
 265
 266 void Matrix::Multiply( Matrix& result, const Matrix& lhs, const Matrix& rhs )
 267 {
 268   MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 269   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,64); // 64 = 16*4
 270
 271   float* temp = result.AsFloat();
 272   const float* rhsPtr  = rhs.AsFloat();
 273   const float* lhsPtr = lhs.AsFloat();
 274
 275 #ifndef  __ARM_NEON__
 276
 277   for( int32_t i=0; i < 4; i++ )
 278   {
 279     // i<<2 gives the first vector / column
 280     int32_t loc = i<<2;
 281     int32_t loc1 = loc + 1;
 282     int32_t loc2 = loc + 2;
 283     int32_t loc3 = loc + 3;
 284     float value0 = lhsPtr[loc];
 285     float value1 = lhsPtr[loc1];
 286     float value2 = lhsPtr[loc2];
 287     float value3 = lhsPtr[loc3];
 288     temp[loc]  = (value0 * rhsPtr[0]) +
 289                  (value1 * rhsPtr[4]) +
 290                  (value2 * rhsPtr[8]) +
 291                  (value3 * rhsPtr[12]);
 292
 293     temp[loc1] = (value0 * rhsPtr[1]) +
 294                  (value1 * rhsPtr[5]) +
 295                  (value2 * rhsPtr[9]) +
 296                  (value3 * rhsPtr[13]);
 297
 298     temp[loc2] = (value0 * rhsPtr[2]) +
 299                  (value1 * rhsPtr[6]) +
 300                  (value2 * rhsPtr[10])+
 301                  (value3 * rhsPtr[14]);
 302
 303     temp[loc3] = (value0 * rhsPtr[3]) +
 304                  (value1 * rhsPtr[7]) +
 305                  (value2 * rhsPtr[11])+
 306                  (value3 * rhsPtr[15]);
 307   }
 308
 309 #else
 310
 311   // 64 32bit registers,
 312   // aliased to
 313   // d = 64 bit double-word d0 -d31
 314   // q =128 bit quad-word   q0 -q15  (enough to handle a column of 4 floats in a matrix)
 315   // e.g. q0 = d0 and d1
 316
 317   // load and stores interleaved as NEON can load and store while calculating
 318   asm volatile ( "VLDM         %1,  {q0-q3}        \n\t"   // load matrix 1 (lhsPtr) q[0..q3]
 319                  "VLDM         %0,  {q8-q11}       \n\t"   // load matrix 2 (rhsPtr) q[q8-q11]
 320                  "VMUL.F32     q12, q8, d0[0]      \n\t"   // column 0 = rhsPtr[0..3] * lhsPtr[0..3]
 321                  "VMUL.F32     q13, q8, d2[0]      \n\t"   // column 1 = rhsPtr[0..3] * lhsPtr[4..7]
 322                  "VMUL.F32     q14, q8, d4[0]      \n\t"   // column 2 = rhsPtr[0..3] * lhsPtr[8..11]
 323                  "VMUL.F32     q15, q8, d6[0]      \n\t"   // column 3 = rhsPtr[0..3] * lhsPtr[12..15]
 324
 325                  "VMLA.F32     q12, q9, d0[1]      \n\t"   // column 0 += rhsPtr[4..7] * lhsPtr[0..3]
 326                  "VMLA.F32     q13, q9, d2[1]      \n\t"   // column 1 += rhsPtr[4..7] * lhsPtr[4..7]
 327                  "VMLA.F32     q14, q9, d4[1]      \n\t"   // column 2 += rhsPtr[4..7] * lhsPtr[8..11]
 328                  "VMLA.F32     q15, q9, d6[1]      \n\t"   // column 3 += rhsPtr[4..7] * lhsPtr[12..15]
 329
 330                  "VMLA.F32     q12, q10, d1[0]     \n\t"   // column 0 += rhsPtr[8..11] * lhsPtr[0..3]
 331                  "VMLA.F32     q13, q10, d3[0]     \n\t"   // column 1 += rhsPtr[8..11] * lhsPtr[4..7]
 332                  "VMLA.F32     q14, q10, d5[0]     \n\t"   // column 2 += rhsPtr[8..11] * lhsPtr[8..11]
 333                  "VMLA.F32     q15, q10, d7[0]     \n\t"   // column 3 += rhsPtr[8..11] * lhsPtr[12..15]
 334
 335                  "VMLA.F32     q12, q11, d1[1]     \n\t"   // column 0 += rhsPtr[12..15] * lhsPtr[0..3]
 336                  "VMLA.F32     q13, q11, d3[1]     \n\t"   // column 1 += rhsPtr[12..15] * lhsPtr[4..7]
 337                  "VMLA.F32     q14, q11, d5[1]     \n\t"   // column 2 += rhsPtr[12..15] * lhsPtr[8..11]
 338                  "VMLA.F32     q15, q11, d7[1]     \n\t"   // column 3 += rhsPtr[12..15] * lhsPtr[12..15]
 339                  "VSTM         %2,  {q12-q15}      \n\t"   // store entire output matrix.
 340                  : "+r"(rhsPtr), "+r"(lhsPtr), "+r"(temp)
 341                  :
 342                  : "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory" );
 343
 344 #endif
 345 }
 346
 347 void Matrix::Multiply( Matrix& result, const Matrix& lhs, const Quaternion& rhs )
 348 {
 349   MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 350   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,54); // 54 = 36+18
 351
 352   float matrix[16];
 353   float* rhsPtr = &matrix[0];
 354   Convert( rhsPtr, rhs );
 355
 356   // quaternion contains just rotation so it really only needs 3x3 matrix
 357
 358   float* temp = result.AsFloat();
 359   const float* lhsPtr = lhs.AsFloat();
 360
 361 #ifndef  __ARM_NEON__
 362
 363   for( int32_t i=0; i < 4; i++ )
 364   {
 365     // i<<2 gives the first vector / column
 366     int32_t loc = i<<2;
 367     int32_t loc1 = loc + 1;
 368     int32_t loc2 = loc + 2;
 369     int32_t loc3 = loc + 3;
 370     float value0 = lhsPtr[loc];
 371     float value1 = lhsPtr[loc1];
 372     float value2 = lhsPtr[loc2];
 373     float value3 = lhsPtr[loc3];
 374     temp[loc]  = (value0 * rhsPtr[0]) +
 375                  (value1 * rhsPtr[4]) +
 376                  (value2 * rhsPtr[8]) +
 377                  (0.0f); //value3 * rhsPtr[12] is 0.0f
 378
 379     temp[loc1] = (value0 * rhsPtr[1]) +
 380                  (value1 * rhsPtr[5]) +
 381                  (value2 * rhsPtr[9]) +
 382                  (0.0f); //value3 * rhsPtr[13] is 0.0f
 383
 384     temp[loc2] = (value0 * rhsPtr[2]) +
 385                  (value1 * rhsPtr[6]) +
 386                  (value2 * rhsPtr[10])+
 387                  (0.0f); //value3 * rhsPtr[14] is 0.0f
 388
 389     temp[loc3] = (0.0f) + //value0 * rhsPtr[3] is 0.0f
 390                  (0.0f) + //value1 * rhsPtr[7] is 0.0f
 391                  (0.0f) + //value2 * rhsPtr[11] is 0.0f
 392                  (value3); // rhsPtr[15] is 1.0f
 393   }
 394
 395 #else
 396
 397   // 64 32bit registers,
 398   // aliased to
 399   // d = 64 bit double-word d0 -d31
 400   // q =128 bit quad-word   q0 -q15  (enough to handle a column of 4 floats in a matrix)
 401   // e.g. q0 = d0 and d1
 402   // load and stores interleaved as NEON can load and store while calculating
 403   asm volatile ( "VLDM         %1,   {q4-q6}       \n\t" // load matrix 1 (lhsPtr)
 404                  "VLD1.F32     {q7}, [%2]!         \n\t" // load matrix 2 (rhsPtr) [0..3]
 405                  "VMUL.F32     q0,   q7,   d8[0]   \n\t" // column 0 = rhsPtr[0..3] * lhsPtr[0..3]
 406                  "VMUL.F32     q1,   q7,   d10[0]  \n\t" // column 1 = rhsPtr[0..3] * lhsPtr[4..7]
 407                  "VMUL.F32     q2,   q7,   d12[0]  \n\t" // column 2 = rhsPtr[0..3] * lhsPtr[8..11]
 408                  "VLD1.F32     {q7}, [%2]!         \n\t" // load matrix 2 (rhsPtr) [4..7]
 409                  "VMLA.F32     q0,   q7,   d8[1]   \n\t" // column 0+= rhsPtr[4..7] * lhsPtr[0..3]
 410                  "VMLA.F32     q1,   q7,   d10[1]  \n\t" // column 1+= rhsPtr[4..7] * lhsPtr[4..7]
 411                  "VMLA.F32     q2,   q7,   d12[1]  \n\t" // column 2+= rhsPtr[4..7] * lhsPtr[8..11]
 412                  "VLD1.F32     {q7}, [%2]!         \n\t" // load matrix 2 (rhsPtr) [8..11]
 413                  "VMLA.F32     q0,   q7,   d9[0]   \n\t" // column 0+= rhsPtr[8..11] * lhsPtr[0..3]
 414                  "VMLA.F32     q1,   q7,   d11[0]  \n\t" // column 1+= rhsPtr[8..11] * lhsPtr[4..7]
 415                  "VMLA.F32     q2,   q7,   d13[0]  \n\t" // column 2+= rhsPtr[8..11] * lhsPtr[8..11]
 416                  "VSTM         %0,   {q0-q2}       \n\t" // store entire output matrix.
 417                  :
 418                  : "r"(temp), "r"(lhsPtr), "r" (rhsPtr)
 419                  : "%r0", "%q0", "%q1", "%q2", "%q4", "%q5", "%q6", "%q7", "memory" );
 420
 421   temp[ 12 ] = 0.0f;
 422   temp[ 13 ] = 0.0f;
 423   temp[ 14 ] = 0.0f;
 424   temp[ 15 ] = 1.0f;
 425 #endif
 426 }
 427
 428 Vector4 Matrix::operator*(const Vector4& rhs) const
 429 {
 430   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,16);
 431
 432   Vector4 temp;
 433
 434 #ifndef  __ARM_NEON__
 435
 436   temp.x = rhs.x * mMatrix[0] + rhs.y * mMatrix[4] + rhs.z * mMatrix[8]  +  rhs.w * mMatrix[12];
 437   temp.y = rhs.x * mMatrix[1] + rhs.y * mMatrix[5] + rhs.z * mMatrix[9]  +  rhs.w * mMatrix[13];
 438   temp.z = rhs.x * mMatrix[2] + rhs.y * mMatrix[6] + rhs.z * mMatrix[10] +  rhs.w * mMatrix[14];
 439   temp.w = rhs.x * mMatrix[3] + rhs.y * mMatrix[7] + rhs.z * mMatrix[11] +  rhs.w * mMatrix[15];
 440
 441 #else
 442
 443   // 64 32bit registers,
 444   // aliased to
 445   // d = 64 bit double-word d0 -d31
 446   // q =128 bit quad-word   q0 -q15  (enough to handle a column of 4 floats in a matrix)
 447   // e.g. q0 = d0 and d1
 448   // load and stores interleaved as NEON can load and store while calculating
 449   asm volatile ( "VLD1.F32     {q0}, [%1]        \n\t"   //q0 = rhs
 450                  "VLD1.F32     {q9}, [%0]!       \n\t"
 451                  "VMUL.F32     q10,  q9,   d0[0] \n\t"
 452                  "VLD1.F32     {q9}, [%0]!       \n\t"
 453                  "VMLA.F32     q10,  q9,   d0[1] \n\t"   //q10 = mMatrix[0..3] * rhs + mMatrix[4..7] * rhs
 454                  "VLD1.F32     {q9}, [%0]!       \n\t"
 455                  "VMUL.F32     q11,  q9,   d1[0] \n\t"
 456                  "VLD1.F32     {q9}, [%0]!       \n\t"
 457                  "VMLA.F32     q11,  q9,   d1[1] \n\t"   //q11 = mMatrix[8..11] * rhs + mMatrix[12..15] * rhs
 458                  "VADD.F32     q10,  q10,  q11   \n\t"
 459                  "VST1.F32     {q10},[%2]        \n\t"   //temp = q10 + q11
 460                  :
 461                  : "r"(mMatrix), "r"(&rhs), "r"(&temp)
 462                  : "q0", "q9", "q10", "q11", "memory" );
 463 #endif
 464   return temp;
 465 }
 466
 467 bool Matrix::operator==(const Matrix& rhs) const
 468 {
 469   return (
 470   ( fabsf( mMatrix[0] - rhs.mMatrix[0] ) <= GetRangedEpsilon( mMatrix[0], rhs.mMatrix[0] ) ) &&
 471   ( fabsf( mMatrix[1] - rhs.mMatrix[1] ) <= GetRangedEpsilon( mMatrix[1], rhs.mMatrix[1] ) ) &&
 472   ( fabsf( mMatrix[2] - rhs.mMatrix[2] ) <= GetRangedEpsilon( mMatrix[2], rhs.mMatrix[2] ) ) &&
 473   ( fabsf( mMatrix[3] - rhs.mMatrix[3] ) <= GetRangedEpsilon( mMatrix[3], rhs.mMatrix[3] ) ) &&
 474   ( fabsf( mMatrix[4] - rhs.mMatrix[4] ) <= GetRangedEpsilon( mMatrix[4], rhs.mMatrix[4] ) ) &&
 475   ( fabsf( mMatrix[5] - rhs.mMatrix[5] ) <= GetRangedEpsilon( mMatrix[5], rhs.mMatrix[5] ) ) &&
 476   ( fabsf( mMatrix[6] - rhs.mMatrix[6] ) <= GetRangedEpsilon( mMatrix[6], rhs.mMatrix[6] ) ) &&
 477   ( fabsf( mMatrix[7] - rhs.mMatrix[7] ) <= GetRangedEpsilon( mMatrix[7], rhs.mMatrix[7] ) ) &&
 478   ( fabsf( mMatrix[8] - rhs.mMatrix[8] ) <= GetRangedEpsilon( mMatrix[8], rhs.mMatrix[8] ) ) &&
 479   ( fabsf( mMatrix[9] - rhs.mMatrix[9] ) <= GetRangedEpsilon( mMatrix[9], rhs.mMatrix[9] ) ) &&
 480   ( fabsf( mMatrix[10] - rhs.mMatrix[10] ) <= GetRangedEpsilon( mMatrix[10], rhs.mMatrix[10] ) ) &&
 481   ( fabsf( mMatrix[11] - rhs.mMatrix[11] ) <= GetRangedEpsilon( mMatrix[11], rhs.mMatrix[11] ) ) &&
 482   ( fabsf( mMatrix[12] - rhs.mMatrix[12] ) <= GetRangedEpsilon( mMatrix[12], rhs.mMatrix[12] ) ) &&
 483   ( fabsf( mMatrix[13] - rhs.mMatrix[13] ) <= GetRangedEpsilon( mMatrix[13], rhs.mMatrix[13] ) ) &&
 484   ( fabsf( mMatrix[14] - rhs.mMatrix[14] ) <= GetRangedEpsilon( mMatrix[14], rhs.mMatrix[14] ) ) &&
 485   ( fabsf( mMatrix[15] - rhs.mMatrix[15] ) <= GetRangedEpsilon( mMatrix[15], rhs.mMatrix[15] ) ) );
 486 }
 487
 488 bool Matrix::operator!=(const Matrix& rhs) const
 489 {
 490   if (*this == rhs)
 491   {
 492     return false;
 493   }
 494
 495   return true;
 496 }
 497
 498 void Matrix::OrthoNormalize()
 499 {
 500   Vector4 vector0(GetXAxis());
 501   Vector4 vector1(GetYAxis());
 502   Vector4 vector2(GetZAxis());
 503
 504   vector0.Normalize();
 505   vector1.Normalize();
 506   vector2 = vector0.Cross( vector1 );
 507   vector1 = vector2.Cross( vector0 );
 508
 509   memcpy( mMatrix, &vector0, NUM_BYTES_IN_ROW );
 510   memcpy( mMatrix + ROW1_OFFSET, &vector1, NUM_BYTES_IN_ROW );
 511   memcpy( mMatrix + ROW2_OFFSET, &vector2, NUM_BYTES_IN_ROW );
 512 }
 513
 514 Vector3 Matrix::GetXAxis() const
 515 {
 516   return Vector3(mMatrix[0], mMatrix[1], mMatrix[2]);
 517 }
 518
 519 Vector3 Matrix::GetYAxis() const
 520 {
 521   return Vector3(mMatrix[4], mMatrix[5], mMatrix[6]);
 522 }
 523
 524 Vector3 Matrix::GetZAxis() const
 525 {
 526   return Vector3(mMatrix[8], mMatrix[9], mMatrix[10]);
 527 }
 528
 529 void Matrix::SetXAxis(const Vector3& axis)
 530 {
 531   mMatrix[0] = axis.x;
 532   mMatrix[1] = axis.y;
 533   mMatrix[2] = axis.z;
 534 }
 535
 536 void Matrix::SetYAxis(const Vector3& axis)
 537 {
 538   mMatrix[4] = axis.x;
 539   mMatrix[5] = axis.y;
 540   mMatrix[6] = axis.z;
 541 }
 542
 543 void Matrix::SetZAxis(const Vector3& axis)
 544 {
 545   mMatrix[8] = axis.x;
 546   mMatrix[9] = axis.y;
 547   mMatrix[10] = axis.z;
 548 }
 549
 550 void Matrix::SetTransformComponents(const Vector3&    scale,
 551                                     const Quaternion& rotation,
 552                                     const Vector3&    translation )
 553 {
 554   if( rotation.IsIdentity() )
 555   {
 556     mMatrix[0] = scale.x;
 557     mMatrix[1] = 0.0f;
 558     mMatrix[2] = 0.0f;
 559     mMatrix[3] = 0.0f;
 560
 561     mMatrix[4] = 0.0f;
 562     mMatrix[5] = scale.y;
 563     mMatrix[6] = 0.0f;
 564     mMatrix[7] = 0.0f;
 565
 566     mMatrix[8] = 0.0f;
 567     mMatrix[9] = 0.0f;
 568     mMatrix[10]= scale.z;
 569     mMatrix[11]= 0.0f;
 570   }
 571   else
 572   {
 573     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 574     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,27); // 27 = 9+18
 575
 576     const float xx = rotation.mVector.x * rotation.mVector.x;
 577     const float yy = rotation.mVector.y * rotation.mVector.y;
 578     const float zz = rotation.mVector.z * rotation.mVector.z;
 579     const float xy = rotation.mVector.x * rotation.mVector.y;
 580     const float xz = rotation.mVector.x * rotation.mVector.z;
 581     const float wx = rotation.mVector.w * rotation.mVector.x;
 582     const float wy = rotation.mVector.w * rotation.mVector.y;
 583     const float wz = rotation.mVector.w * rotation.mVector.z;
 584     const float yz = rotation.mVector.y * rotation.mVector.z;
 585
 586     mMatrix[0] = (scale.x * (1.0f - 2.0f * (yy + zz)));
 587     mMatrix[1] = (scale.x * (       2.0f * (xy + wz)));
 588     mMatrix[2] = (scale.x * (       2.0f * (xz - wy)));
 589     mMatrix[3] = 0.0f;
 590
 591     mMatrix[4] = (scale.y * (       2.0f * (xy - wz)));
 592     mMatrix[5] = (scale.y * (1.0f - 2.0f * (xx + zz)));
 593     mMatrix[6] = (scale.y * (       2.0f * (yz + wx)));
 594     mMatrix[7] = 0.0f;
 595
 596     mMatrix[8] = (scale.z * (       2.0f * (xz + wy)));
 597     mMatrix[9] = (scale.z * (       2.0f * (yz - wx)));
 598     mMatrix[10]= (scale.z * (1.0f - 2.0f * (xx + yy)));
 599     mMatrix[11]= 0.0f;
 600   }
 601   // apply translation
 602   mMatrix[12] = translation.x;
 603   mMatrix[13] = translation.y;
 604   mMatrix[14] = translation.z;
 605   mMatrix[15] = 1.0f;
 606 }
 607
 608 void Matrix::SetInverseTransformComponents(const Vector3&    scale,
 609                                            const Quaternion& rotation,
 610                                            const Vector3&    translation )
 611 {
 612   Vector3 inverseTranslation = -translation;
 613   Vector3 inverseScale( 1.0f/scale.x, 1.0f/scale.y, 1.0f/scale.z);
 614   Quaternion inverseRotation(rotation);
 615   bool isRotated = ! inverseRotation.IsIdentity();
 616
 617   // Order of application is translation, rotation, scale.
 618   // Ensure translation is relative to scale & rotation:
 619
 620   if( isRotated )
 621   {
 622     inverseRotation.Invert();
 623     inverseTranslation = inverseRotation.Rotate(inverseTranslation);
 624   }
 625
 626   inverseTranslation *= inverseScale;
 627
 628   if( isRotated )
 629   {
 630     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 631     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,27); // 27 = 9+18
 632
 633     const float xx = inverseRotation.mVector.x * inverseRotation.mVector.x;
 634     const float yy = inverseRotation.mVector.y * inverseRotation.mVector.y;
 635     const float zz = inverseRotation.mVector.z * inverseRotation.mVector.z;
 636     const float xy = inverseRotation.mVector.x * inverseRotation.mVector.y;
 637     const float xz = inverseRotation.mVector.x * inverseRotation.mVector.z;
 638     const float wx = inverseRotation.mVector.w * inverseRotation.mVector.x;
 639     const float wy = inverseRotation.mVector.w * inverseRotation.mVector.y;
 640     const float wz = inverseRotation.mVector.w * inverseRotation.mVector.z;
 641     const float yz = inverseRotation.mVector.y * inverseRotation.mVector.z;
 642
 643     mMatrix[0] = (inverseScale.x * (1.0f - 2.0f * (yy + zz)));
 644     mMatrix[1] = (inverseScale.y * (2.0f * (xy + wz)));
 645     mMatrix[2] = (inverseScale.z * (2.0f * (xz - wy)));
 646     mMatrix[3] = 0.0f;
 647
 648     mMatrix[4] = (inverseScale.x * (2.0f * (xy - wz)));
 649     mMatrix[5] = (inverseScale.y * (1.0f - 2.0f * (xx + zz)));
 650     mMatrix[6] = (inverseScale.z * (2.0f * (yz + wx)));
 651     mMatrix[7] = 0.0f;
 652
 653     mMatrix[8] = (inverseScale.x * (2.0f * (xz + wy)));
 654     mMatrix[9] = (inverseScale.y * (2.0f * (yz - wx)));
 655     mMatrix[10]= (inverseScale.z * (1.0f - 2.0f * (xx + yy)));
 656     mMatrix[11]= 0.0f;
 657   }
 658   else
 659   {
 660     mMatrix[0] = inverseScale.x;
 661     mMatrix[1] = 0.0f;
 662     mMatrix[2] = 0.0f;
 663     mMatrix[3] = 0.0f;
 664
 665     mMatrix[4] = 0.0f;
 666     mMatrix[5] = inverseScale.y;
 667     mMatrix[6] = 0.0f;
 668     mMatrix[7] = 0.0f;
 669
 670     mMatrix[8] = 0.0f;
 671     mMatrix[9] = 0.0f;
 672     mMatrix[10]= inverseScale.z;
 673     mMatrix[11]= 0.0f;
 674   }
 675
 676   // apply translation
 677   mMatrix[12] = inverseTranslation.x;
 678   mMatrix[13] = inverseTranslation.y;
 679   mMatrix[14] = inverseTranslation.z;
 680   mMatrix[15] = 1.0f;
 681 }
 682
 683 void Matrix::SetInverseTransformComponents(const Vector3&    xAxis,
 684                                            const Vector3&    yAxis,
 685                                            const Vector3&    zAxis,
 686                                            const Vector3&    translation )
 687 {
 688   // x, y, z axis parameters represent a orthonormal basis with no scaling, i.e. a rotation matrix.
 689   // Invert rotation by transposing in place
 690
 691   // Order of application is translation, rotation
 692
 693   mMatrix[0]  = xAxis.x;
 694   mMatrix[1]  = yAxis.x;
 695   mMatrix[2]  = zAxis.x;
 696   mMatrix[3]  = 0.0f;
 697
 698   mMatrix[4]  = xAxis.y;
 699   mMatrix[5]  = yAxis.y;
 700   mMatrix[6]  = zAxis.y;
 701   mMatrix[7]  = 0.0f;
 702
 703   mMatrix[8]  = xAxis.z;
 704   mMatrix[9]  = yAxis.z;
 705   mMatrix[10] = zAxis.z;
 706   mMatrix[11] = 0.0f;
 707   mMatrix[12] = 0.0f;
 708   mMatrix[13] = 0.0f;
 709   mMatrix[14] = 0.0f;
 710   mMatrix[15] = 1.0f;
 711
 712   // Ensure translation is relative to scale & rotation:
 713
 714   Vector4 inverseTranslation( -translation.x, -translation.y, -translation.z, 1.0f);
 715   inverseTranslation = *this * inverseTranslation; // Rotate inverse translation
 716   inverseTranslation.w = 1.0f;
 717   SetTranslation(inverseTranslation);
 718 }
 719
 720
 721 void Matrix::GetTransformComponents(Vector3&     position,
 722                                     Quaternion&  rotation,
 723                                     Vector3&     scale) const
 724 {
 725   position = GetTranslation3();
 726
 727   // Derive scale from axis lengths.
 728   Vector3 theScale(GetXAxis().Length(), GetYAxis().Length(), GetZAxis().Length());
 729   scale = theScale;
 730
 731   if( ! ( fabs(theScale.x - Vector3::ONE.x) < ROTATION_EPSILON &&
 732           fabs(theScale.y - Vector3::ONE.y) < ROTATION_EPSILON &&
 733           fabs(theScale.z - Vector3::ONE.z) < ROTATION_EPSILON ) )
 734   {
 735     MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
 736     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,9);
 737
 738     // Non-identity scale is embedded into rotation matrix. Remove it first:
 739     Matrix m(*this);
 740     Vector3 inverseScale(1.0f/theScale.x, 1.0f/theScale.y, 1.0f/theScale.z);
 741     m.mMatrix[0] *= inverseScale.x;
 742     m.mMatrix[1] *= inverseScale.x;
 743     m.mMatrix[2] *= inverseScale.x;
 744     m.mMatrix[4] *= inverseScale.y;
 745     m.mMatrix[5] *= inverseScale.y;
 746     m.mMatrix[6] *= inverseScale.y;
 747     m.mMatrix[8] *= inverseScale.z;
 748     m.mMatrix[9] *= inverseScale.z;
 749     m.mMatrix[10] *= inverseScale.z;
 750
 751     Quaternion theRotation(m);
 752
 753     // If the imaginary components are close to zero, then use null quaternion instead.
 754     if( fabs(theRotation.mVector.x) < ROTATION_EPSILON &&
 755         fabs(theRotation.mVector.y) < ROTATION_EPSILON &&
 756         fabs(theRotation.mVector.z) < ROTATION_EPSILON )
 757     {
 758       theRotation = Quaternion();
 759     }
 760     rotation = theRotation;
 761   }
 762   else
 763   {
 764     Quaternion theRotation(*this);
 765
 766     // If the imaginary components are close to zero, then use null quaternion instead.
 767     if( fabs(theRotation.mVector.x) < ROTATION_EPSILON &&
 768         fabs(theRotation.mVector.y) < ROTATION_EPSILON &&
 769         fabs(theRotation.mVector.z) < ROTATION_EPSILON )
 770     {
 771       theRotation = Quaternion();
 772     }
 773     rotation = theRotation;
 774   }
 775 }
 776
 777
 778
 779 std::ostream& operator<< (std::ostream& o, const Matrix& matrix)
 780 {
 781   return o << "[ " << matrix.mMatrix[0]  << ", " << matrix.mMatrix[1]  << ", " << matrix.mMatrix[2]  << ", " << matrix.mMatrix[3]  << ", "
 782                    << matrix.mMatrix[4]  << ", " << matrix.mMatrix[5]  << ", " << matrix.mMatrix[6]  << ", " << matrix.mMatrix[7]  << ", "
 783                    << matrix.mMatrix[8]  << ", " << matrix.mMatrix[9]  << ", " << matrix.mMatrix[10] << ", " << matrix.mMatrix[11] << ", "
 784                    << matrix.mMatrix[12] << ", " << matrix.mMatrix[13] << ", " << matrix.mMatrix[14] << ", " << matrix.mMatrix[15] << " ]";
 785 }
 786
 787 } // namespace Dali