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