2 * Copyright (c) 2015 Samsung Electronics Co., Ltd.
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
19 #include <dali/public-api/math/matrix.h>
23 #include <cstring> // for memcpy
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>
36 const float ROTATION_EPSILON = 0.003f; // Deliberately large
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 );
46 * Helper to convert to Quaternion to float16 array
48 void Convert( float*& m, const Dali::Quaternion& rotation )
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;
60 m[0] = 1.0f - 2.0f * (yy + zz);
61 m[1] = 2.0f * (xy + wz);
62 m[2] = 2.0f * (xz - wy);
65 m[4] = 2.0f * (xy - wz);
66 m[5] = 1.0f - 2.0f * (xx + zz);
67 m[6] = 2.0f * (yz + wx);
70 m[8] = 2.0f * (xz + wy);
71 m[9] = 2.0f * (yz - wx);
72 m[10]= 1.0f - 2.0f * (xx + yy);
85 using Internal::PerformanceMonitor;
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};
92 const Matrix Matrix::IDENTITY(identityArray);
96 memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
99 Matrix::Matrix( bool initialize )
103 memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
107 Matrix::Matrix(const float* array)
109 memcpy( mMatrix, array, NUM_BYTES_IN_MATRIX );
112 Matrix::Matrix( const Quaternion& rotation )
114 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,18);
116 float* matrixPtr = &mMatrix[0];
117 Convert( matrixPtr, rotation );
120 Matrix::Matrix( const Matrix& matrix )
122 memcpy( mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX );
125 Matrix& Matrix::operator=( const Matrix& matrix )
127 // no point copying if self assigning
128 if( this != &matrix )
130 memcpy( mMatrix, matrix.mMatrix, NUM_BYTES_IN_MATRIX );
135 void Matrix::InvertTransform(Matrix& result) const
137 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
139 float* m1 = result.AsFloat();
141 DALI_ASSERT_ALWAYS( EqualsZero( mMatrix[3] ) && EqualsZero( mMatrix[7] ) && EqualsZero( mMatrix[11] ) && Equals( mMatrix[15], 1.0f ) && "Must be a transform matrix" );
155 m1[10] = mMatrix[10];
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] ) );
164 static bool InvertMatrix(const float* m, float* out)
168 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,192); // 12 x 16 multiples
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];
187 float det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12];
189 // In the case where the determinant is exactly zero, the matrix is non-invertible
190 if ( EqualsZero( det ) )
197 for (int i = 0; i < 16; i++)
199 out[i] = inv[i] * det;
205 bool Matrix::Invert()
209 return InvertMatrix(temp.AsFloat(), mMatrix);
212 void Matrix::Transpose()
214 float temp = mMatrix[1];
215 mMatrix[1] = mMatrix[4];
219 mMatrix[2] = mMatrix[8];
223 mMatrix[3] = mMatrix[12];
227 mMatrix[6] = mMatrix[9];
231 mMatrix[7] = mMatrix[13];
235 mMatrix[11] = mMatrix[14];
239 void Matrix::SetIdentity()
241 memcpy( mMatrix, identityArray, NUM_BYTES_IN_MATRIX );
244 void Matrix::SetIdentityAndScale( const Vector3& scale )
246 // initialize to zeros
247 memset( mMatrix, 0, NUM_BYTES_IN_MATRIX );
249 // just apply scale on the diagonal
250 mMatrix[0] = scale.x;
251 mMatrix[5] = scale.y;
252 mMatrix[10] = scale.z;
256 void Matrix::SetTranslation(const Vector4& translation)
258 memcpy( mMatrix + ROW3_OFFSET, &translation, NUM_BYTES_IN_ROW );
260 void Matrix::SetTranslation(const Vector3& other)
262 memcpy( mMatrix + ROW3_OFFSET, &other, NUM_BYTES_IN_ROW_OF_3 );
266 void Matrix::Multiply( Matrix& result, const Matrix& lhs, const Matrix& rhs )
268 MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
269 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,64); // 64 = 16*4
271 float* temp = result.AsFloat();
272 const float* rhsPtr = rhs.AsFloat();
273 const float* lhsPtr = lhs.AsFloat();
277 for( int i=0; i < 4; i++ )
279 // i<<2 gives the first vector / column
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]);
293 temp[loc1] = (value0 * rhsPtr[1]) +
294 (value1 * rhsPtr[5]) +
295 (value2 * rhsPtr[9]) +
296 (value3 * rhsPtr[13]);
298 temp[loc2] = (value0 * rhsPtr[2]) +
299 (value1 * rhsPtr[6]) +
300 (value2 * rhsPtr[10])+
301 (value3 * rhsPtr[14]);
303 temp[loc3] = (value0 * rhsPtr[3]) +
304 (value1 * rhsPtr[7]) +
305 (value2 * rhsPtr[11])+
306 (value3 * rhsPtr[15]);
311 // 64 32bit registers,
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
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]
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]
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]
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)
342 : "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory" );
347 void Matrix::Multiply( Matrix& result, const Matrix& lhs, const Quaternion& rhs )
349 MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
350 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,54); // 54 = 36+18
353 float* rhsPtr = &matrix[0];
354 Convert( rhsPtr, rhs );
356 // quaternion contains just rotation so it really only needs 3x3 matrix
358 float* temp = result.AsFloat();
359 const float* lhsPtr = lhs.AsFloat();
363 for( int i=0; i < 4; i++ )
365 // i<<2 gives the first vector / column
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
379 temp[loc1] = (value0 * rhsPtr[1]) +
380 (value1 * rhsPtr[5]) +
381 (value2 * rhsPtr[9]) +
382 (0.0f); //value3 * rhsPtr[13] is 0.0f
384 temp[loc2] = (value0 * rhsPtr[2]) +
385 (value1 * rhsPtr[6]) +
386 (value2 * rhsPtr[10])+
387 (0.0f); //value3 * rhsPtr[14] is 0.0f
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
397 // 64 32bit registers,
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.
418 : "r"(temp), "r"(lhsPtr), "r" (rhsPtr)
419 : "%r0", "%q0", "%q1", "%q2", "%q4", "%q5", "%q6", "%q7", "memory" );
428 Vector4 Matrix::operator*(const Vector4& rhs) const
430 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,16);
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];
443 // 64 32bit registers,
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
461 : "r"(mMatrix), "r"(&rhs), "r"(&temp)
462 : "q0", "q9", "q10", "q11", "memory" );
467 bool Matrix::operator==(const Matrix& rhs) const
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] ) ) );
488 bool Matrix::operator!=(const Matrix& rhs) const
498 void Matrix::OrthoNormalize()
500 Vector4 vector0(GetXAxis());
501 Vector4 vector1(GetYAxis());
502 Vector4 vector2(GetZAxis());
506 vector2 = vector0.Cross( vector1 );
507 vector1 = vector2.Cross( vector0 );
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 );
514 Vector3 Matrix::GetXAxis() const
516 return Vector3(mMatrix[0], mMatrix[1], mMatrix[2]);
519 Vector3 Matrix::GetYAxis() const
521 return Vector3(mMatrix[4], mMatrix[5], mMatrix[6]);
524 Vector3 Matrix::GetZAxis() const
526 return Vector3(mMatrix[8], mMatrix[9], mMatrix[10]);
529 void Matrix::SetXAxis(const Vector3& axis)
536 void Matrix::SetYAxis(const Vector3& axis)
543 void Matrix::SetZAxis(const Vector3& axis)
547 mMatrix[10] = axis.z;
550 void Matrix::SetTransformComponents(const Vector3& scale,
551 const Quaternion& rotation,
552 const Vector3& translation )
554 if( rotation.IsIdentity() )
556 mMatrix[0] = scale.x;
562 mMatrix[5] = scale.y;
568 mMatrix[10]= scale.z;
573 MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
574 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,27); // 27 = 9+18
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;
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)));
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)));
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)));
602 mMatrix[12] = translation.x;
603 mMatrix[13] = translation.y;
604 mMatrix[14] = translation.z;
608 void Matrix::SetInverseTransformComponents(const Vector3& scale,
609 const Quaternion& rotation,
610 const Vector3& translation )
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();
617 // Order of application is translation, rotation, scale.
618 // Ensure translation is relative to scale & rotation:
622 inverseRotation.Invert();
623 inverseTranslation = inverseRotation.Rotate(inverseTranslation);
626 inverseTranslation *= inverseScale;
630 MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
631 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,27); // 27 = 9+18
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;
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)));
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)));
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)));
660 mMatrix[0] = inverseScale.x;
666 mMatrix[5] = inverseScale.y;
672 mMatrix[10]= inverseScale.z;
677 mMatrix[12] = inverseTranslation.x;
678 mMatrix[13] = inverseTranslation.y;
679 mMatrix[14] = inverseTranslation.z;
683 void Matrix::SetInverseTransformComponents(const Vector3& xAxis,
684 const Vector3& yAxis,
685 const Vector3& zAxis,
686 const Vector3& translation )
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
691 // Order of application is translation, rotation
693 mMatrix[0] = xAxis.x;
694 mMatrix[1] = yAxis.x;
695 mMatrix[2] = zAxis.x;
698 mMatrix[4] = xAxis.y;
699 mMatrix[5] = yAxis.y;
700 mMatrix[6] = zAxis.y;
703 mMatrix[8] = xAxis.z;
704 mMatrix[9] = yAxis.z;
705 mMatrix[10] = zAxis.z;
712 // Ensure translation is relative to scale & rotation:
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);
721 void Matrix::GetTransformComponents(Vector3& position,
722 Quaternion& rotation,
723 Vector3& scale) const
725 position = GetTranslation3();
727 // Derive scale from axis lengths.
728 Vector3 theScale(GetXAxis().Length(), GetYAxis().Length(), GetZAxis().Length());
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 ) )
735 MATH_INCREASE_COUNTER(PerformanceMonitor::MATRIX_MULTIPLYS);
736 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,9);
738 // Non-identity scale is embedded into rotation matrix. Remove it first:
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;
751 Quaternion theRotation(m);
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 )
758 theRotation = Quaternion();
760 rotation = theRotation;
764 Quaternion theRotation(*this);
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 )
771 theRotation = Quaternion();
773 rotation = theRotation;
779 std::ostream& operator<< (std::ostream& o, const Matrix& matrix)
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] << "] ]";