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-2014 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 30 namespace Ogre 31 { 32 33 const Matrix4 Matrix4::ZERO( 34 0, 0, 0, 0, 35 0, 0, 0, 0, 36 0, 0, 0, 0, 37 0, 0, 0, 0 ); 38 39 const Affine3 Affine3::ZERO( 40 0, 0, 0, 0, 41 0, 0, 0, 0, 42 0, 0, 0, 0); 43 44 const Affine3 Affine3::IDENTITY( 45 1, 0, 0, 0, 46 0, 1, 0, 0, 47 0, 0, 1, 0); 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 static Real MINOR(const TransformBase & 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 TransformBase& 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 TransformBase::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 //----------------------------------------------------------------------- inverse() const164 Affine3 Affine3::inverse() const 165 { 166 Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2]; 167 Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2]; 168 169 Real t00 = m22 * m11 - m21 * m12; 170 Real t10 = m20 * m12 - m22 * m10; 171 Real t20 = m21 * m10 - m20 * m11; 172 173 Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2]; 174 175 Real invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20); 176 177 t00 *= invDet; t10 *= invDet; t20 *= invDet; 178 179 m00 *= invDet; m01 *= invDet; m02 *= invDet; 180 181 Real r00 = t00; 182 Real r01 = m02 * m21 - m01 * m22; 183 Real r02 = m01 * m12 - m02 * m11; 184 185 Real r10 = t10; 186 Real r11 = m00 * m22 - m02 * m20; 187 Real r12 = m02 * m10 - m00 * m12; 188 189 Real r20 = t20; 190 Real r21 = m01 * m20 - m00 * m21; 191 Real r22 = m00 * m11 - m01 * m10; 192 193 Real m03 = m[0][3], m13 = m[1][3], m23 = m[2][3]; 194 195 Real r03 = - (r00 * m03 + r01 * m13 + r02 * m23); 196 Real r13 = - (r10 * m03 + r11 * m13 + r12 * m23); 197 Real r23 = - (r20 * m03 + r21 * m13 + r22 * m23); 198 199 return Affine3( 200 r00, r01, r02, r03, 201 r10, r11, r12, r13, 202 r20, r21, r22, r23); 203 } 204 //----------------------------------------------------------------------- makeTransform(const Vector3 & position,const Vector3 & scale,const Quaternion & orientation)205 void TransformBase::makeTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation) 206 { 207 // Ordering: 208 // 1. Scale 209 // 2. Rotate 210 // 3. Translate 211 212 Matrix3 rot3x3; 213 orientation.ToRotationMatrix(rot3x3); 214 215 // Set up final matrix with scale, rotation and translation 216 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; 217 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; 218 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; 219 220 // No projection term 221 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; 222 } 223 //----------------------------------------------------------------------- makeInverseTransform(const Vector3 & position,const Vector3 & scale,const Quaternion & orientation)224 void TransformBase::makeInverseTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation) 225 { 226 // Invert the parameters 227 Vector3 invTranslate = -position; 228 Vector3 invScale(1 / scale.x, 1 / scale.y, 1 / scale.z); 229 Quaternion invRot = orientation.Inverse(); 230 231 // Because we're inverting, order is translation, rotation, scale 232 // So make translation relative to scale & rotation 233 invTranslate = invRot * invTranslate; // rotate 234 invTranslate *= invScale; // scale 235 236 // Next, make a 3x3 rotation matrix 237 Matrix3 rot3x3; 238 invRot.ToRotationMatrix(rot3x3); 239 240 // Set up final matrix with scale, rotation and translation 241 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; 242 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; 243 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; 244 245 // No projection term 246 m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; 247 } 248 //----------------------------------------------------------------------- decomposition(Vector3 & position,Vector3 & scale,Quaternion & orientation) const249 void Affine3::decomposition(Vector3& position, Vector3& scale, Quaternion& orientation) const 250 { 251 Matrix3 matQ; 252 Vector3 vecU; 253 linear().QDUDecomposition( matQ, scale, vecU ); 254 255 orientation = Quaternion( matQ ); 256 position = Vector3( m[0][3], m[1][3], m[2][3] ); 257 } 258 259 } 260