1 /* 2 Copyright (c) 2008-2009 NetAllied Systems GmbH 3 4 This file is part of COLLADABaseUtils. 5 6 Licensed under the MIT Open Source License, 7 for details please see LICENSE file or the website 8 http://www.opensource.org/licenses/mit-license.php 9 */ 10 11 #include "COLLADABUStableHeaders.h" 12 #include "Math/COLLADABUMathMatrix4.h" 13 14 namespace COLLADABU 15 { 16 17 namespace Math 18 { 19 20 const Matrix4 Matrix4::ZERO( 21 ISNOTIDENTITY, 22 0, 0, 0, 0, 23 0, 0, 0, 0, 24 0, 0, 0, 0, 25 0, 0, 0, 0 ); 26 27 const Matrix4 Matrix4::IDENTITY( 28 ISIDENTITY, 29 1, 0, 0, 0, 30 0, 1, 0, 0, 31 0, 0, 1, 0, 32 0, 0, 0, 1 ); 33 34 const Matrix4 Matrix4::CLIPSPACE2DTOIMAGESPACE( 35 ISNOTIDENTITY, 36 0.5, 0, 0, 0.5, 37 0, -0.5, 0, 0.5, 38 0, 0, 1, 0, 39 0, 0, 0, 1 ); 40 41 inline Real MINOR(const Matrix4 & m,const size_t r0,const size_t r1,const size_t r2,const size_t c0,const size_t c1,const size_t c2)42 MINOR( const Matrix4& m, const size_t r0, const size_t r1, const size_t r2, 43 const size_t c0, const size_t c1, const size_t c2 ) 44 { 45 return m[ r0 ][ c0 ] * ( m[ r1 ][ c1 ] * m[ r2 ][ c2 ] - m[ r2 ][ c1 ] * m[ r1 ][ c2 ] ) - 46 m[ r0 ][ c1 ] * ( m[ r1 ][ c0 ] * m[ r2 ][ c2 ] - m[ r2 ][ c0 ] * m[ r1 ][ c2 ] ) + 47 m[ r0 ][ c2 ] * ( m[ r1 ][ c0 ] * m[ r2 ][ c1 ] - m[ r2 ][ c0 ] * m[ r1 ][ c1 ] ); 48 } 49 50 adjoint() const51 Matrix4 Matrix4::adjoint() const 52 { 53 return Matrix4( MINOR( *this, 1, 2, 3, 1, 2, 3 ), 54 -MINOR( *this, 0, 2, 3, 1, 2, 3 ), 55 MINOR( *this, 0, 1, 3, 1, 2, 3 ), 56 -MINOR( *this, 0, 1, 2, 1, 2, 3 ), 57 58 -MINOR( *this, 1, 2, 3, 0, 2, 3 ), 59 MINOR( *this, 0, 2, 3, 0, 2, 3 ), 60 -MINOR( *this, 0, 1, 3, 0, 2, 3 ), 61 MINOR( *this, 0, 1, 2, 0, 2, 3 ), 62 63 MINOR( *this, 1, 2, 3, 0, 1, 3 ), 64 -MINOR( *this, 0, 2, 3, 0, 1, 3 ), 65 MINOR( *this, 0, 1, 3, 0, 1, 3 ), 66 -MINOR( *this, 0, 1, 2, 0, 1, 3 ), 67 68 -MINOR( *this, 1, 2, 3, 0, 1, 2 ), 69 MINOR( *this, 0, 2, 3, 0, 1, 2 ), 70 -MINOR( *this, 0, 1, 3, 0, 1, 2 ), 71 MINOR( *this, 0, 1, 2, 0, 1, 2 ) ); 72 } 73 74 determinant() const75 Real Matrix4::determinant() const 76 { 77 if ( mState == ISIDENTITY ) 78 return 1; 79 80 return m[ 0 ][ 0 ] * MINOR( *this, 1, 2, 3, 1, 2, 3 ) - 81 m[ 0 ][ 1 ] * MINOR( *this, 1, 2, 3, 0, 2, 3 ) + 82 m[ 0 ][ 2 ] * MINOR( *this, 1, 2, 3, 0, 1, 3 ) - 83 m[ 0 ][ 3 ] * MINOR( *this, 1, 2, 3, 0, 1, 2 ); 84 } 85 inverse() const86 Matrix4 Matrix4::inverse() const 87 { 88 if ( mState == ISIDENTITY ) 89 return * this; 90 91 return adjoint() * ( 1.0f / determinant() ); 92 } 93 setRotate(Vector3 point,Real theta,Real phi,Real alpha)94 void Matrix4::setRotate( Vector3 point, Real theta, Real phi, Real alpha ) 95 //void setRotate(Point3d A, // rotate about a line centred on A 96 { 97 mState = UNKNOWN; 98 Real cosAlpha, sinAlpha, cosPhi, sinPhi, 99 cosTheta, sinTheta, cosPhi2, sinPhi2, 100 cosTheta2, sinTheta2, c, a1, a2, a3; 101 a1 = point.x; 102 a2 = point.y; 103 a3 = point.z; 104 105 cosPhi = cos( phi ); 106 sinPhi = sin( phi ); 107 cosPhi2 = cosPhi * cosPhi; 108 sinPhi2 = sinPhi * sinPhi; 109 cosTheta = cos( theta ); 110 sinTheta = sin( theta ); 111 cosTheta2 = cosTheta * cosTheta; 112 sinTheta2 = sinTheta * sinTheta; 113 cosAlpha = cos( alpha ); 114 sinAlpha = sin( alpha ); 115 c = 1.0 - cosAlpha; 116 m[ 0 ][ 0 ] = cosTheta2 * ( cosAlpha * cosPhi2 + sinPhi2 ) 117 + cosAlpha * sinTheta2; 118 m[ 1 ][ 0 ] = sinAlpha * cosPhi + c * sinPhi2 * cosTheta * sinTheta; 119 m[ 2 ][ 0 ] = sinPhi * ( cosPhi * cosTheta * c - sinAlpha * sinTheta ); 120 m[ 0 ][ 1 ] = sinPhi2 * cosTheta * sinTheta * c - sinAlpha * cosPhi; 121 m[ 1 ][ 1 ] = sinTheta2 * ( cosAlpha * cosPhi2 + sinPhi2 ) 122 + cosAlpha * cosTheta2; 123 m[ 2 ][ 1 ] = sinPhi * ( cosPhi * sinTheta * c + sinAlpha * cosTheta ); 124 m[ 0 ][ 2 ] = sinPhi * ( cosPhi * cosTheta * c + sinAlpha * sinTheta ); 125 m[ 1 ][ 2 ] = sinPhi * ( cosPhi * sinTheta * c - sinAlpha * cosTheta ); 126 m[ 2 ][ 2 ] = cosAlpha * sinPhi2 + cosPhi2; 127 m[ 0 ][ 3 ] = a1 - a1 * m[ 0 ][ 0 ] - a2 * m[ 0 ][ 1 ] - a3 * m[ 0 ][ 2 ]; 128 m[ 1 ][ 3 ] = a2 - a1 * m[ 1 ][ 0 ] - a2 * m[ 1 ][ 1 ] - a3 * m[ 1 ][ 2 ]; 129 m[ 2 ][ 3 ] = a3 - a1 * m[ 2 ][ 0 ] - a2 * m[ 2 ][ 1 ] - a3 * m[ 2 ][ 2 ]; 130 } 131 132 133 //----------------------------------------------------------------------- makeTransform(const Vector3 & position,const Vector3 & scale,const Quaternion & orientation)134 void Matrix4::makeTransform( const Vector3& position, const Vector3& scale, const Quaternion& orientation ) 135 { 136 // Ordering: 137 // 1. Scale 138 // 2. Rotate 139 // 3. Translate 140 mState = UNKNOWN; 141 Matrix3 rot3x3, scale3x3; 142 orientation.toRotationMatrix( rot3x3 ); 143 scale3x3 = Matrix3::ZERO; 144 scale3x3[ 0 ][ 0 ] = scale.x; 145 scale3x3[ 1 ][ 1 ] = scale.y; 146 scale3x3[ 2 ][ 2 ] = scale.z; 147 148 // Set up final matrix with scale, rotation and translation 149 *this = rot3x3 * scale3x3; 150 this->setTrans( position ); 151 152 // No projection term 153 m[ 3 ][ 0 ] = 0; 154 m[ 3 ][ 1 ] = 0; 155 m[ 3 ][ 2 ] = 0; 156 m[ 3 ][ 3 ] = 1; 157 } 158 159 } 160 } 161