| 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-2006 Torus Knot Software Ltd |
|---|
| 8 | Also see acknowledgements in Readme.html |
|---|
| 9 | |
|---|
| 10 | This program is free software; you can redistribute it and/or modify it under |
|---|
| 11 | the terms of the GNU Lesser General Public License as published by the Free Software |
|---|
| 12 | Foundation; either version 2 of the License, or (at your option) any later |
|---|
| 13 | version. |
|---|
| 14 | |
|---|
| 15 | This program is distributed in the hope that it will be useful, but WITHOUT |
|---|
| 16 | ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS |
|---|
| 17 | FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. |
|---|
| 18 | |
|---|
| 19 | You should have received a copy of the GNU Lesser General Public License along with |
|---|
| 20 | this program; if not, write to the Free Software Foundation, Inc., 59 Temple |
|---|
| 21 | Place - Suite 330, Boston, MA 02111-1307, USA, or go to |
|---|
| 22 | http://www.gnu.org/copyleft/lesser.txt. |
|---|
| 23 | |
|---|
| 24 | You may alternatively use this source under the terms of a specific version of |
|---|
| 25 | the OGRE Unrestricted License provided you have obtained such a license from |
|---|
| 26 | Torus Knot Software Ltd. |
|---|
| 27 | ----------------------------------------------------------------------------- |
|---|
| 28 | */ |
|---|
| 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::IDENTITY( |
|---|
| 44 | 1, 0, 0, 0, |
|---|
| 45 | 0, 1, 0, 0, |
|---|
| 46 | 0, 0, 1, 0, |
|---|
| 47 | 0, 0, 0, 1 ); |
|---|
| 48 | |
|---|
| 49 | const Matrix4 Matrix4::CLIPSPACE2DTOIMAGESPACE( |
|---|
| 50 | 0.5, 0, 0, 0.5, |
|---|
| 51 | 0, -0.5, 0, 0.5, |
|---|
| 52 | 0, 0, 1, 0, |
|---|
| 53 | 0, 0, 0, 1); |
|---|
| 54 | |
|---|
| 55 | //----------------------------------------------------------------------- |
|---|
| 56 | inline static Real |
|---|
| 57 | MINOR(const Matrix4& m, const size_t r0, const size_t r1, const size_t r2, |
|---|
| 58 | const size_t c0, const size_t c1, const size_t c2) |
|---|
| 59 | { |
|---|
| 60 | return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) - |
|---|
| 61 | m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) + |
|---|
| 62 | m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]); |
|---|
| 63 | } |
|---|
| 64 | //----------------------------------------------------------------------- |
|---|
| 65 | Matrix4 Matrix4::adjoint() const |
|---|
| 66 | { |
|---|
| 67 | return Matrix4( MINOR(*this, 1, 2, 3, 1, 2, 3), |
|---|
| 68 | -MINOR(*this, 0, 2, 3, 1, 2, 3), |
|---|
| 69 | MINOR(*this, 0, 1, 3, 1, 2, 3), |
|---|
| 70 | -MINOR(*this, 0, 1, 2, 1, 2, 3), |
|---|
| 71 | |
|---|
| 72 | -MINOR(*this, 1, 2, 3, 0, 2, 3), |
|---|
| 73 | MINOR(*this, 0, 2, 3, 0, 2, 3), |
|---|
| 74 | -MINOR(*this, 0, 1, 3, 0, 2, 3), |
|---|
| 75 | MINOR(*this, 0, 1, 2, 0, 2, 3), |
|---|
| 76 | |
|---|
| 77 | MINOR(*this, 1, 2, 3, 0, 1, 3), |
|---|
| 78 | -MINOR(*this, 0, 2, 3, 0, 1, 3), |
|---|
| 79 | MINOR(*this, 0, 1, 3, 0, 1, 3), |
|---|
| 80 | -MINOR(*this, 0, 1, 2, 0, 1, 3), |
|---|
| 81 | |
|---|
| 82 | -MINOR(*this, 1, 2, 3, 0, 1, 2), |
|---|
| 83 | MINOR(*this, 0, 2, 3, 0, 1, 2), |
|---|
| 84 | -MINOR(*this, 0, 1, 3, 0, 1, 2), |
|---|
| 85 | MINOR(*this, 0, 1, 2, 0, 1, 2)); |
|---|
| 86 | } |
|---|
| 87 | //----------------------------------------------------------------------- |
|---|
| 88 | Real Matrix4::determinant() const |
|---|
| 89 | { |
|---|
| 90 | return m[0][0] * MINOR(*this, 1, 2, 3, 1, 2, 3) - |
|---|
| 91 | m[0][1] * MINOR(*this, 1, 2, 3, 0, 2, 3) + |
|---|
| 92 | m[0][2] * MINOR(*this, 1, 2, 3, 0, 1, 3) - |
|---|
| 93 | m[0][3] * MINOR(*this, 1, 2, 3, 0, 1, 2); |
|---|
| 94 | } |
|---|
| 95 | //----------------------------------------------------------------------- |
|---|
| 96 | Matrix4 Matrix4::inverse() const |
|---|
| 97 | { |
|---|
| 98 | Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3]; |
|---|
| 99 | Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3]; |
|---|
| 100 | Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3]; |
|---|
| 101 | Real m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3]; |
|---|
| 102 | |
|---|
| 103 | Real v0 = m20 * m31 - m21 * m30; |
|---|
| 104 | Real v1 = m20 * m32 - m22 * m30; |
|---|
| 105 | Real v2 = m20 * m33 - m23 * m30; |
|---|
| 106 | Real v3 = m21 * m32 - m22 * m31; |
|---|
| 107 | Real v4 = m21 * m33 - m23 * m31; |
|---|
| 108 | Real v5 = m22 * m33 - m23 * m32; |
|---|
| 109 | |
|---|
| 110 | Real t00 = + (v5 * m11 - v4 * m12 + v3 * m13); |
|---|
| 111 | Real t10 = - (v5 * m10 - v2 * m12 + v1 * m13); |
|---|
| 112 | Real t20 = + (v4 * m10 - v2 * m11 + v0 * m13); |
|---|
| 113 | Real t30 = - (v3 * m10 - v1 * m11 + v0 * m12); |
|---|
| 114 | |
|---|
| 115 | Real invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03); |
|---|
| 116 | |
|---|
| 117 | Real d00 = t00 * invDet; |
|---|
| 118 | Real d10 = t10 * invDet; |
|---|
| 119 | Real d20 = t20 * invDet; |
|---|
| 120 | Real d30 = t30 * invDet; |
|---|
| 121 | |
|---|
| 122 | Real d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet; |
|---|
| 123 | Real d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet; |
|---|
| 124 | Real d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet; |
|---|
| 125 | Real d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet; |
|---|
| 126 | |
|---|
| 127 | v0 = m10 * m31 - m11 * m30; |
|---|
| 128 | v1 = m10 * m32 - m12 * m30; |
|---|
| 129 | v2 = m10 * m33 - m13 * m30; |
|---|
| 130 | v3 = m11 * m32 - m12 * m31; |
|---|
| 131 | v4 = m11 * m33 - m13 * m31; |
|---|
| 132 | v5 = m12 * m33 - m13 * m32; |
|---|
| 133 | |
|---|
| 134 | Real d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet; |
|---|
| 135 | Real d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet; |
|---|
| 136 | Real d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet; |
|---|
| 137 | Real d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet; |
|---|
| 138 | |
|---|
| 139 | v0 = m21 * m10 - m20 * m11; |
|---|
| 140 | v1 = m22 * m10 - m20 * m12; |
|---|
| 141 | v2 = m23 * m10 - m20 * m13; |
|---|
| 142 | v3 = m22 * m11 - m21 * m12; |
|---|
| 143 | v4 = m23 * m11 - m21 * m13; |
|---|
| 144 | v5 = m23 * m12 - m22 * m13; |
|---|
| 145 | |
|---|
| 146 | Real d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet; |
|---|
| 147 | Real d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet; |
|---|
| 148 | Real d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet; |
|---|
| 149 | Real d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet; |
|---|
| 150 | |
|---|
| 151 | return Matrix4( |
|---|
| 152 | d00, d01, d02, d03, |
|---|
| 153 | d10, d11, d12, d13, |
|---|
| 154 | d20, d21, d22, d23, |
|---|
| 155 | d30, d31, d32, d33); |
|---|
| 156 | } |
|---|
| 157 | //----------------------------------------------------------------------- |
|---|
| 158 | Matrix4 Matrix4::inverseAffine(void) const |
|---|
| 159 | { |
|---|
| 160 | assert(isAffine()); |
|---|
| 161 | |
|---|
| 162 | Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2]; |
|---|
| 163 | Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2]; |
|---|
| 164 | |
|---|
| 165 | Real t00 = m22 * m11 - m21 * m12; |
|---|
| 166 | Real t10 = m20 * m12 - m22 * m10; |
|---|
| 167 | Real t20 = m21 * m10 - m20 * m11; |
|---|
| 168 | |
|---|
| 169 | Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2]; |
|---|
| 170 | |
|---|
| 171 | Real invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20); |
|---|
| 172 | |
|---|
| 173 | t00 *= invDet; t10 *= invDet; t20 *= invDet; |
|---|
| 174 | |
|---|
| 175 | m00 *= invDet; m01 *= invDet; m02 *= invDet; |
|---|
| 176 | |
|---|
| 177 | Real r00 = t00; |
|---|
| 178 | Real r01 = m02 * m21 - m01 * m22; |
|---|
| 179 | Real r02 = m01 * m12 - m02 * m11; |
|---|
| 180 | |
|---|
| 181 | Real r10 = t10; |
|---|
| 182 | Real r11 = m00 * m22 - m02 * m20; |
|---|
| 183 | Real r12 = m02 * m10 - m00 * m12; |
|---|
| 184 | |
|---|
| 185 | Real r20 = t20; |
|---|
| 186 | Real r21 = m01 * m20 - m00 * m21; |
|---|
| 187 | Real r22 = m00 * m11 - m01 * m10; |
|---|
| 188 | |
|---|
| 189 | Real m03 = m[0][3], m13 = m[1][3], m23 = m[2][3]; |
|---|
| 190 | |
|---|
| 191 | Real r03 = - (r00 * m03 + r01 * m13 + r02 * m23); |
|---|
| 192 | Real r13 = - (r10 * m03 + r11 * m13 + r12 * m23); |
|---|
| 193 | Real r23 = - (r20 * m03 + r21 * m13 + r22 * m23); |
|---|
| 194 | |
|---|
| 195 | return Matrix4( |
|---|
| 196 | r00, r01, r02, r03, |
|---|
| 197 | r10, r11, r12, r13, |
|---|
| 198 | r20, r21, r22, r23, |
|---|
| 199 | 0, 0, 0, 1); |
|---|
| 200 | } |
|---|
| 201 | //----------------------------------------------------------------------- |
|---|
| 202 | void Matrix4::makeTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation) |
|---|
| 203 | { |
|---|
| 204 | // Ordering: |
|---|
| 205 | // 1. Scale |
|---|
| 206 | // 2. Rotate |
|---|
| 207 | // 3. Translate |
|---|
| 208 | |
|---|
| 209 | Matrix3 rot3x3, scale3x3; |
|---|
| 210 | orientation.ToRotationMatrix(rot3x3); |
|---|
| 211 | scale3x3 = Matrix3::ZERO; |
|---|
| 212 | scale3x3[0][0] = scale.x; |
|---|
| 213 | scale3x3[1][1] = scale.y; |
|---|
| 214 | scale3x3[2][2] = scale.z; |
|---|
| 215 | |
|---|
| 216 | // Set up final matrix with scale, rotation and translation |
|---|
| 217 | *this = rot3x3 * scale3x3; |
|---|
| 218 | this->setTrans(position); |
|---|
| 219 | |
|---|
| 220 | // No projection term |
|---|
| 221 | m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; |
|---|
| 222 | } |
|---|
| 223 | //----------------------------------------------------------------------- |
|---|
| 224 | void Matrix4::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 *= invScale; // scale |
|---|
| 234 | invTranslate = invRot * invTranslate; // rotate |
|---|
| 235 | |
|---|
| 236 | // Next, make a 3x3 rotation matrix and apply inverse scale |
|---|
| 237 | Matrix3 rot3x3, scale3x3; |
|---|
| 238 | invRot.ToRotationMatrix(rot3x3); |
|---|
| 239 | scale3x3 = Matrix3::ZERO; |
|---|
| 240 | scale3x3[0][0] = invScale.x; |
|---|
| 241 | scale3x3[1][1] = invScale.y; |
|---|
| 242 | scale3x3[2][2] = invScale.z; |
|---|
| 243 | |
|---|
| 244 | // Set up final matrix with scale, rotation and translation |
|---|
| 245 | *this = scale3x3 * rot3x3; |
|---|
| 246 | this->setTrans(invTranslate); |
|---|
| 247 | |
|---|
| 248 | // No projection term |
|---|
| 249 | m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; |
|---|
| 250 | } |
|---|
| 251 | |
|---|
| 252 | } |
|---|
