1 /* 2 ----------------------------------------------------------------------------- 3 This source file is part of OGRE 4 (Object-oriented Graphics Rendering Engine) 5 For the latest info, see http://www.ogre3d.org/ 6 7 Copyright (c) 2000-2013 Torus Knot Software Ltd 8 9 Permission is hereby granted, free of charge, to any person obtaining a copy 10 of this software and associated documentation files (the "Software"), to deal 11 in the Software without restriction, including without limitation the rights 12 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 copies of the Software, and to permit persons to whom the Software is 14 furnished to do so, subject to the following conditions: 15 16 The above copyright notice and this permission notice shall be included in 17 all copies or substantial portions of the Software. 18 19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 25 THE SOFTWARE. 26 ----------------------------------------------------------------------------- 27 */ 28 #include "OgreStableHeaders.h" 29 #include "OgreMatrix4.h" 30 31 #include "OgreVector3.h" 32 #include "OgreMatrix3.h" 33 34 namespace Ogre 35 { 36 37 const Matrix4 Matrix4::ZERO( 38 0, 0, 0, 0, 39 0, 0, 0, 0, 40 0, 0, 0, 0, 41 0, 0, 0, 0 ); 42 43 const Matrix4 Matrix4::ZEROAFFINE( 44 0, 0, 0, 0, 45 0, 0, 0, 0, 46 0, 0, 0, 0, 47 0, 0, 0, 1 ); 48 49 const Matrix4 Matrix4::IDENTITY( 50 1, 0, 0, 0, 51 0, 1, 0, 0, 52 0, 0, 1, 0, 53 0, 0, 0, 1 ); 54 55 const Matrix4 Matrix4::CLIPSPACE2DTOIMAGESPACE( 56 0.5, 0, 0, 0.5, 57 0, -0.5, 0, 0.5, 58 0, 0, 1, 0, 59 0, 0, 0, 1); 60 61 //----------------------------------------------------------------------- 62 inline static 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)63 MINOR(const Matrix4& m, const size_t r0, const size_t r1, const size_t r2, 64 const size_t c0, const size_t c1, const size_t c2) 65 { 66 return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) - 67 m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) + 68 m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]); 69 } 70 //----------------------------------------------------------------------- adjoint() const71 Matrix4 Matrix4::adjoint() const 72 { 73 return Matrix4( MINOR(*this, 1, 2, 3, 1, 2, 3), 74 -MINOR(*this, 0, 2, 3, 1, 2, 3), 75 MINOR(*this, 0, 1, 3, 1, 2, 3), 76 -MINOR(*this, 0, 1, 2, 1, 2, 3), 77 78 -MINOR(*this, 1, 2, 3, 0, 2, 3), 79 MINOR(*this, 0, 2, 3, 0, 2, 3), 80 -MINOR(*this, 0, 1, 3, 0, 2, 3), 81 MINOR(*this, 0, 1, 2, 0, 2, 3), 82 83 MINOR(*this, 1, 2, 3, 0, 1, 3), 84 -MINOR(*this, 0, 2, 3, 0, 1, 3), 85 MINOR(*this, 0, 1, 3, 0, 1, 3), 86 -MINOR(*this, 0, 1, 2, 0, 1, 3), 87 88 -MINOR(*this, 1, 2, 3, 0, 1, 2), 89 MINOR(*this, 0, 2, 3, 0, 1, 2), 90 -MINOR(*this, 0, 1, 3, 0, 1, 2), 91 MINOR(*this, 0, 1, 2, 0, 1, 2)); 92 } 93 //----------------------------------------------------------------------- determinant() const94 Real Matrix4::determinant() const 95 { 96 return m[0][0] * MINOR(*this, 1, 2, 3, 1, 2, 3) - 97 m[0][1] * MINOR(*this, 1, 2, 3, 0, 2, 3) + 98 m[0][2] * MINOR(*this, 1, 2, 3, 0, 1, 3) - 99 m[0][3] * MINOR(*this, 1, 2, 3, 0, 1, 2); 100 } 101 //----------------------------------------------------------------------- inverse() const102 Matrix4 Matrix4::inverse() const 103 { 104 Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3]; 105 Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3]; 106 Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3]; 107 Real m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3]; 108 109 Real v0 = m20 * m31 - m21 * m30; 110 Real v1 = m20 * m32 - m22 * m30; 111 Real v2 = m20 * m33 - m23 * m30; 112 Real v3 = m21 * m32 - m22 * m31; 113 Real v4 = m21 * m33 - m23 * m31; 114 Real v5 = m22 * m33 - m23 * m32; 115 116 Real t00 = + (v5 * m11 - v4 * m12 + v3 * m13); 117 Real t10 = - (v5 * m10 - v2 * m12 + v1 * m13); 118 Real t20 = + (v4 * m10 - v2 * m11 + v0 * m13); 119 Real t30 = - (v3 * m10 - v1 * m11 + v0 * m12); 120 121 Real invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03); 122 123 Real d00 = t00 * invDet; 124 Real d10 = t10 * invDet; 125 Real d20 = t20 * invDet; 126 Real d30 = t30 * invDet; 127 128 Real d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet; 129 Real d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet; 130 Real d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet; 131 Real d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet; 132 133 v0 = m10 * m31 - m11 * m30; 134 v1 = m10 * m32 - m12 * m30; 135 v2 = m10 * m33 - m13 * m30; 136 v3 = m11 * m32 - m12 * m31; 137 v4 = m11 * m33 - m13 * m31; 138 v5 = m12 * m33 - m13 * m32; 139 140 Real d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet; 141 Real d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet; 142 Real d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet; 143 Real d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet; 144 145 v0 = m21 * m10 - m20 * m11; 146 v1 = m22 * m10 - m20 * m12; 147 v2 = m23 * m10 - m20 * m13; 148 v3 = m22 * m11 - m21 * m12; 149 v4 = m23 * m11 - m21 * m13; 150 v5 = m23 * m12 - m22 * m13; 151 152 Real d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet; 153 Real d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet; 154 Real d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet; 155 Real d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet; 156 157 return Matrix4( 158 d00, d01, d02, d03, 159 d10, d11, d12, d13, 160 d20, d21, d22, d23, 161 d30, d31, d32, d33); 162 } 163 //----------------------------------------------------------------------- inverseAffine(void) const164 Matrix4 Matrix4::inverseAffine(void) const 165 { 166 assert(isAffine()); 167 168 Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2]; 169 Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2]; 170 171 Real t00 = m22 * m11 - m21 * m12; 172 Real t10 = m20 * m12 - m22 * m10; 173 Real t20 = m21 * m10 - m20 * m11; 174 175 Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2]; 176 177 Real invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20); 178 179 t00 *= invDet; t10 *= invDet; t20 *= invDet; 180 181 m00 *= invDet; m01 *= invDet; m02 *= invDet; 182 183 Real r00 = t00; 184 Real r01 = m02 * m21 - m01 * m22; 185 Real r02 = m01 * m12 - m02 * m11; 186 187 Real r10 = t10; 188 Real r11 = m00 * m22 - m02 * m20; 189 Real r12 = m02 * m10 - m00 * m12; 190 191 Real r20 = t20; 192 Real r21 = m01 * m20 - m00 * m21; 193 Real r22 = m00 * m11 - m01 * m10; 194 195 Real m03 = m[0][3], m13 = m[1][3], m23 = m[2][3]; 196 197 Real r03 = - (r00 * m03 + r01 * m13 + r02 * m23); 198 Real r13 = - (r10 * m03 + r11 * m13 + r12 * m23); 199 Real r23 = - (r20 * m03 + r21 * m13 + r22 * m23); 200 201 return Matrix4( 202 r00, r01, r02, r03, 203 r10, r11, r12, r13, 204 r20, r21, r22, r23, 205 0, 0, 0, 1); 206 } 207 //----------------------------------------------------------------------- makeTransform(const Vector3 & position,const Vector3 & scale,const Quaternion & orientation)208 void Matrix4::makeTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation) 209 { 210 // Ordering: 211 // 1. Scale 212 // 2. Rotate 213 // 3. Translate 214 215 Matrix3 rot3x3; 216 orientation.ToRotationMatrix(rot3x3); 217 218 // Set up final matrix with scale, rotation and translation 219 m[0][0] = scale.x * rot3x3[0][0]; m[0][1] = scale.y * rot3x3[0][1]; m[0][2] = scale.z * rot3x3[0][2]; m[0][3] = position.x; 220 m[1][0] = scale.x * rot3x3[1][0]; m[1][1] = scale.y * rot3x3[1][1]; m[1][2] = scale.z * rot3x3[1][2]; m[1][3] = position.y; 221 m[2][0] = scale.x * rot3x3[2][0]; m[2][1] = scale.y * rot3x3[2][1]; m[2][2] = scale.z * rot3x3[2][2]; m[2][3] = position.z; 222 223 // No projection term 224 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; 225 } 226 //----------------------------------------------------------------------- makeInverseTransform(const Vector3 & position,const Vector3 & scale,const Quaternion & orientation)227 void Matrix4::makeInverseTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation) 228 { 229 // Invert the parameters 230 Vector3 invTranslate = -position; 231 Vector3 invScale(1 / scale.x, 1 / scale.y, 1 / scale.z); 232 Quaternion invRot = orientation.Inverse(); 233 234 // Because we're inverting, order is translation, rotation, scale 235 // So make translation relative to scale & rotation 236 invTranslate = invRot * invTranslate; // rotate 237 invTranslate *= invScale; // scale 238 239 // Next, make a 3x3 rotation matrix 240 Matrix3 rot3x3; 241 invRot.ToRotationMatrix(rot3x3); 242 243 // Set up final matrix with scale, rotation and translation 244 m[0][0] = invScale.x * rot3x3[0][0]; m[0][1] = invScale.x * rot3x3[0][1]; m[0][2] = invScale.x * rot3x3[0][2]; m[0][3] = invTranslate.x; 245 m[1][0] = invScale.y * rot3x3[1][0]; m[1][1] = invScale.y * rot3x3[1][1]; m[1][2] = invScale.y * rot3x3[1][2]; m[1][3] = invTranslate.y; 246 m[2][0] = invScale.z * rot3x3[2][0]; m[2][1] = invScale.z * rot3x3[2][1]; m[2][2] = invScale.z * rot3x3[2][2]; m[2][3] = invTranslate.z; 247 248 // No projection term 249 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; 250 } 251 //----------------------------------------------------------------------- decomposition(Vector3 & position,Vector3 & scale,Quaternion & orientation) const252 void Matrix4::decomposition(Vector3& position, Vector3& scale, Quaternion& orientation) const 253 { 254 assert(isAffine()); 255 256 Matrix3 m3x3; 257 extract3x3Matrix(m3x3); 258 259 Matrix3 matQ; 260 Vector3 vecU; 261 m3x3.QDUDecomposition( matQ, scale, vecU ); 262 263 orientation = Quaternion( matQ ); 264 position = Vector3( m[0][3], m[1][3], m[2][3] ); 265 } 266 267 } 268