Warning, /include/assimp/matrix4x4.inl is written in an unsupported language. File is not indexed.
0001 /*
0002 ---------------------------------------------------------------------------
0003 Open Asset Import Library (assimp)
0004 ---------------------------------------------------------------------------
0005
0006 Copyright (c) 2006-2024, assimp team
0007
0008 All rights reserved.
0009
0010 Redistribution and use of this software in source and binary forms,
0011 with or without modification, are permitted provided that the following
0012 conditions are met:
0013
0014 * Redistributions of source code must retain the above
0015 copyright notice, this list of conditions and the
0016 following disclaimer.
0017
0018 * Redistributions in binary form must reproduce the above
0019 copyright notice, this list of conditions and the
0020 following disclaimer in the documentation and/or other
0021 materials provided with the distribution.
0022
0023 * Neither the name of the assimp team, nor the names of its
0024 contributors may be used to endorse or promote products
0025 derived from this software without specific prior
0026 written permission of the assimp team.
0027
0028 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
0029 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
0030 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
0031 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
0032 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
0033 SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
0034 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
0035 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
0036 THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
0037 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
0038 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
0039 ---------------------------------------------------------------------------
0040 */
0041
0042 /** @file matrix4x4.inl
0043 * @brief Inline implementation of the 4x4 matrix operators
0044 */
0045 #pragma once
0046 #ifndef AI_MATRIX4X4_INL_INC
0047 #define AI_MATRIX4X4_INL_INC
0048
0049 #ifdef __cplusplus
0050
0051 #include "matrix4x4.h"
0052 #include "matrix3x3.h"
0053 #include "quaternion.h"
0054 #include "MathFunctions.h"
0055
0056 #include <algorithm>
0057 #include <limits>
0058 #include <cmath>
0059
0060 // ----------------------------------------------------------------------------------------
0061 template <typename TReal>
0062 aiMatrix4x4t<TReal>::aiMatrix4x4t() AI_NO_EXCEPT :
0063 a1(1.0f), a2(), a3(), a4(),
0064 b1(), b2(1.0f), b3(), b4(),
0065 c1(), c2(), c3(1.0f), c4(),
0066 d1(), d2(), d3(), d4(1.0f) {
0067 // empty
0068 }
0069
0070 // ----------------------------------------------------------------------------------------
0071 template <typename TReal>
0072 aiMatrix4x4t<TReal>::aiMatrix4x4t (TReal _a1, TReal _a2, TReal _a3, TReal _a4,
0073 TReal _b1, TReal _b2, TReal _b3, TReal _b4,
0074 TReal _c1, TReal _c2, TReal _c3, TReal _c4,
0075 TReal _d1, TReal _d2, TReal _d3, TReal _d4) :
0076 a1(_a1), a2(_a2), a3(_a3), a4(_a4),
0077 b1(_b1), b2(_b2), b3(_b3), b4(_b4),
0078 c1(_c1), c2(_c2), c3(_c3), c4(_c4),
0079 d1(_d1), d2(_d2), d3(_d3), d4(_d4) {
0080 // empty
0081 }
0082
0083 // ------------------------------------------------------------------------------------------------
0084 template <typename TReal>
0085 template <typename TOther>
0086 aiMatrix4x4t<TReal>::operator aiMatrix4x4t<TOther> () const {
0087 return aiMatrix4x4t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3),static_cast<TOther>(a4),
0088 static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3),static_cast<TOther>(b4),
0089 static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3),static_cast<TOther>(c4),
0090 static_cast<TOther>(d1),static_cast<TOther>(d2),static_cast<TOther>(d3),static_cast<TOther>(d4));
0091 }
0092
0093
0094 // ----------------------------------------------------------------------------------------
0095 template <typename TReal>
0096 AI_FORCE_INLINE
0097 aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiMatrix3x3t<TReal>& m) {
0098 a1 = m.a1; a2 = m.a2; a3 = m.a3; a4 = static_cast<TReal>(0.0);
0099 b1 = m.b1; b2 = m.b2; b3 = m.b3; b4 = static_cast<TReal>(0.0);
0100 c1 = m.c1; c2 = m.c2; c3 = m.c3; c4 = static_cast<TReal>(0.0);
0101 d1 = static_cast<TReal>(0.0); d2 = static_cast<TReal>(0.0); d3 = static_cast<TReal>(0.0); d4 = static_cast<TReal>(1.0);
0102 }
0103
0104 // ----------------------------------------------------------------------------------------
0105 template <typename TReal>
0106 AI_FORCE_INLINE
0107 aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiVector3t<TReal>& scaling, const aiQuaterniont<TReal>& rotation, const aiVector3t<TReal>& position) {
0108 // build a 3x3 rotation matrix
0109 aiMatrix3x3t<TReal> m = rotation.GetMatrix();
0110
0111 a1 = m.a1 * scaling.x;
0112 a2 = m.a2 * scaling.x;
0113 a3 = m.a3 * scaling.x;
0114 a4 = position.x;
0115
0116 b1 = m.b1 * scaling.y;
0117 b2 = m.b2 * scaling.y;
0118 b3 = m.b3 * scaling.y;
0119 b4 = position.y;
0120
0121 c1 = m.c1 * scaling.z;
0122 c2 = m.c2 * scaling.z;
0123 c3 = m.c3 * scaling.z;
0124 c4= position.z;
0125
0126 d1 = static_cast<TReal>(0.0);
0127 d2 = static_cast<TReal>(0.0);
0128 d3 = static_cast<TReal>(0.0);
0129 d4 = static_cast<TReal>(1.0);
0130 }
0131
0132 // ----------------------------------------------------------------------------------------
0133 template <typename TReal>
0134 AI_FORCE_INLINE
0135 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::operator *= (const aiMatrix4x4t<TReal>& m) {
0136 *this = aiMatrix4x4t<TReal>(
0137 m.a1 * a1 + m.b1 * a2 + m.c1 * a3 + m.d1 * a4,
0138 m.a2 * a1 + m.b2 * a2 + m.c2 * a3 + m.d2 * a4,
0139 m.a3 * a1 + m.b3 * a2 + m.c3 * a3 + m.d3 * a4,
0140 m.a4 * a1 + m.b4 * a2 + m.c4 * a3 + m.d4 * a4,
0141 m.a1 * b1 + m.b1 * b2 + m.c1 * b3 + m.d1 * b4,
0142 m.a2 * b1 + m.b2 * b2 + m.c2 * b3 + m.d2 * b4,
0143 m.a3 * b1 + m.b3 * b2 + m.c3 * b3 + m.d3 * b4,
0144 m.a4 * b1 + m.b4 * b2 + m.c4 * b3 + m.d4 * b4,
0145 m.a1 * c1 + m.b1 * c2 + m.c1 * c3 + m.d1 * c4,
0146 m.a2 * c1 + m.b2 * c2 + m.c2 * c3 + m.d2 * c4,
0147 m.a3 * c1 + m.b3 * c2 + m.c3 * c3 + m.d3 * c4,
0148 m.a4 * c1 + m.b4 * c2 + m.c4 * c3 + m.d4 * c4,
0149 m.a1 * d1 + m.b1 * d2 + m.c1 * d3 + m.d1 * d4,
0150 m.a2 * d1 + m.b2 * d2 + m.c2 * d3 + m.d2 * d4,
0151 m.a3 * d1 + m.b3 * d2 + m.c3 * d3 + m.d3 * d4,
0152 m.a4 * d1 + m.b4 * d2 + m.c4 * d3 + m.d4 * d4);
0153 return *this;
0154 }
0155
0156 // ----------------------------------------------------------------------------------------
0157 template <typename TReal>
0158 AI_FORCE_INLINE aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const TReal& aFloat) const {
0159 aiMatrix4x4t<TReal> temp(
0160 a1 * aFloat,
0161 a2 * aFloat,
0162 a3 * aFloat,
0163 a4 * aFloat,
0164 b1 * aFloat,
0165 b2 * aFloat,
0166 b3 * aFloat,
0167 b4 * aFloat,
0168 c1 * aFloat,
0169 c2 * aFloat,
0170 c3 * aFloat,
0171 c4 * aFloat,
0172 d1 * aFloat,
0173 d2 * aFloat,
0174 d3 * aFloat,
0175 d4 * aFloat);
0176 return temp;
0177 }
0178
0179 // ----------------------------------------------------------------------------------------
0180 template <typename TReal>
0181 AI_FORCE_INLINE
0182 aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator+ (const aiMatrix4x4t<TReal>& m) const {
0183 aiMatrix4x4t<TReal> temp(
0184 m.a1 + a1,
0185 m.a2 + a2,
0186 m.a3 + a3,
0187 m.a4 + a4,
0188 m.b1 + b1,
0189 m.b2 + b2,
0190 m.b3 + b3,
0191 m.b4 + b4,
0192 m.c1 + c1,
0193 m.c2 + c2,
0194 m.c3 + c3,
0195 m.c4 + c4,
0196 m.d1 + d1,
0197 m.d2 + d2,
0198 m.d3 + d3,
0199 m.d4 + d4);
0200 return temp;
0201 }
0202
0203 // ----------------------------------------------------------------------------------------
0204 template <typename TReal>
0205 AI_FORCE_INLINE
0206 aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const aiMatrix4x4t<TReal>& m) const {
0207 aiMatrix4x4t<TReal> temp( *this);
0208 temp *= m;
0209 return temp;
0210 }
0211
0212 // ----------------------------------------------------------------------------------------
0213 template <typename TReal>
0214 AI_FORCE_INLINE aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Transpose() {
0215 // (TReal&) don't remove, GCC complains cause of packed fields
0216 std::swap( (TReal&)b1, (TReal&)a2);
0217 std::swap( (TReal&)c1, (TReal&)a3);
0218 std::swap( (TReal&)c2, (TReal&)b3);
0219 std::swap( (TReal&)d1, (TReal&)a4);
0220 std::swap( (TReal&)d2, (TReal&)b4);
0221 std::swap( (TReal&)d3, (TReal&)c4);
0222 return *this;
0223 }
0224
0225 // ----------------------------------------------------------------------------------------
0226 template <typename TReal>
0227 AI_FORCE_INLINE
0228 TReal aiMatrix4x4t<TReal>::Determinant() const {
0229 return a1*b2*c3*d4 - a1*b2*c4*d3 + a1*b3*c4*d2 - a1*b3*c2*d4
0230 + a1*b4*c2*d3 - a1*b4*c3*d2 - a2*b3*c4*d1 + a2*b3*c1*d4
0231 - a2*b4*c1*d3 + a2*b4*c3*d1 - a2*b1*c3*d4 + a2*b1*c4*d3
0232 + a3*b4*c1*d2 - a3*b4*c2*d1 + a3*b1*c2*d4 - a3*b1*c4*d2
0233 + a3*b2*c4*d1 - a3*b2*c1*d4 - a4*b1*c2*d3 + a4*b1*c3*d2
0234 - a4*b2*c3*d1 + a4*b2*c1*d3 - a4*b3*c1*d2 + a4*b3*c2*d1;
0235 }
0236
0237 // ----------------------------------------------------------------------------------------
0238 template <typename TReal>
0239 AI_FORCE_INLINE
0240 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Inverse() {
0241 // Compute the reciprocal determinant
0242 const TReal det = Determinant();
0243 if(det == static_cast<TReal>(0.0))
0244 {
0245 // Matrix is not invertible. Setting all elements to nan is not really
0246 // correct in a mathematical sense but it is easy to debug for the
0247 // programmer.
0248 const TReal nan = std::numeric_limits<TReal>::quiet_NaN();
0249 *this = aiMatrix4x4t<TReal>(
0250 nan,nan,nan,nan,
0251 nan,nan,nan,nan,
0252 nan,nan,nan,nan,
0253 nan,nan,nan,nan);
0254
0255 return *this;
0256 }
0257
0258 const TReal invdet = static_cast<TReal>(1.0) / det;
0259
0260 aiMatrix4x4t<TReal> res;
0261 res.a1 = invdet * (b2 * (c3 * d4 - c4 * d3) + b3 * (c4 * d2 - c2 * d4) + b4 * (c2 * d3 - c3 * d2));
0262 res.a2 = -invdet * (a2 * (c3 * d4 - c4 * d3) + a3 * (c4 * d2 - c2 * d4) + a4 * (c2 * d3 - c3 * d2));
0263 res.a3 = invdet * (a2 * (b3 * d4 - b4 * d3) + a3 * (b4 * d2 - b2 * d4) + a4 * (b2 * d3 - b3 * d2));
0264 res.a4 = -invdet * (a2 * (b3 * c4 - b4 * c3) + a3 * (b4 * c2 - b2 * c4) + a4 * (b2 * c3 - b3 * c2));
0265 res.b1 = -invdet * (b1 * (c3 * d4 - c4 * d3) + b3 * (c4 * d1 - c1 * d4) + b4 * (c1 * d3 - c3 * d1));
0266 res.b2 = invdet * (a1 * (c3 * d4 - c4 * d3) + a3 * (c4 * d1 - c1 * d4) + a4 * (c1 * d3 - c3 * d1));
0267 res.b3 = -invdet * (a1 * (b3 * d4 - b4 * d3) + a3 * (b4 * d1 - b1 * d4) + a4 * (b1 * d3 - b3 * d1));
0268 res.b4 = invdet * (a1 * (b3 * c4 - b4 * c3) + a3 * (b4 * c1 - b1 * c4) + a4 * (b1 * c3 - b3 * c1));
0269 res.c1 = invdet * (b1 * (c2 * d4 - c4 * d2) + b2 * (c4 * d1 - c1 * d4) + b4 * (c1 * d2 - c2 * d1));
0270 res.c2 = -invdet * (a1 * (c2 * d4 - c4 * d2) + a2 * (c4 * d1 - c1 * d4) + a4 * (c1 * d2 - c2 * d1));
0271 res.c3 = invdet * (a1 * (b2 * d4 - b4 * d2) + a2 * (b4 * d1 - b1 * d4) + a4 * (b1 * d2 - b2 * d1));
0272 res.c4 = -invdet * (a1 * (b2 * c4 - b4 * c2) + a2 * (b4 * c1 - b1 * c4) + a4 * (b1 * c2 - b2 * c1));
0273 res.d1 = -invdet * (b1 * (c2 * d3 - c3 * d2) + b2 * (c3 * d1 - c1 * d3) + b3 * (c1 * d2 - c2 * d1));
0274 res.d2 = invdet * (a1 * (c2 * d3 - c3 * d2) + a2 * (c3 * d1 - c1 * d3) + a3 * (c1 * d2 - c2 * d1));
0275 res.d3 = -invdet * (a1 * (b2 * d3 - b3 * d2) + a2 * (b3 * d1 - b1 * d3) + a3 * (b1 * d2 - b2 * d1));
0276 res.d4 = invdet * (a1 * (b2 * c3 - b3 * c2) + a2 * (b3 * c1 - b1 * c3) + a3 * (b1 * c2 - b2 * c1));
0277 *this = res;
0278
0279 return *this;
0280 }
0281
0282 // ----------------------------------------------------------------------------------------
0283 template <typename TReal>
0284 AI_FORCE_INLINE
0285 TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) {
0286 if (p_iIndex > 3) {
0287 return nullptr;
0288 }
0289 switch ( p_iIndex ) {
0290 case 0:
0291 return &a1;
0292 case 1:
0293 return &b1;
0294 case 2:
0295 return &c1;
0296 case 3:
0297 return &d1;
0298 default:
0299 break;
0300 }
0301 return &a1;
0302 }
0303
0304 // ----------------------------------------------------------------------------------------
0305 template <typename TReal>
0306 AI_FORCE_INLINE
0307 const TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) const {
0308 if (p_iIndex > 3) {
0309 return nullptr;
0310 }
0311
0312 switch ( p_iIndex ) {
0313 case 0:
0314 return &a1;
0315 case 1:
0316 return &b1;
0317 case 2:
0318 return &c1;
0319 case 3:
0320 return &d1;
0321 default:
0322 break;
0323 }
0324 return &a1;
0325 }
0326
0327 // ----------------------------------------------------------------------------------------
0328 template <typename TReal>
0329 AI_FORCE_INLINE
0330 bool aiMatrix4x4t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const {
0331 return (a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && a4 == m.a4 &&
0332 b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && b4 == m.b4 &&
0333 c1 == m.c1 && c2 == m.c2 && c3 == m.c3 && c4 == m.c4 &&
0334 d1 == m.d1 && d2 == m.d2 && d3 == m.d3 && d4 == m.d4);
0335 }
0336
0337 // ----------------------------------------------------------------------------------------
0338 template <typename TReal>
0339 AI_FORCE_INLINE
0340 bool aiMatrix4x4t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const {
0341 return !(*this == m);
0342 }
0343
0344 // ---------------------------------------------------------------------------
0345 template<typename TReal>
0346 AI_FORCE_INLINE
0347 bool aiMatrix4x4t<TReal>::Equal(const aiMatrix4x4t<TReal>& m, TReal epsilon) const {
0348 return
0349 std::abs(a1 - m.a1) <= epsilon &&
0350 std::abs(a2 - m.a2) <= epsilon &&
0351 std::abs(a3 - m.a3) <= epsilon &&
0352 std::abs(a4 - m.a4) <= epsilon &&
0353 std::abs(b1 - m.b1) <= epsilon &&
0354 std::abs(b2 - m.b2) <= epsilon &&
0355 std::abs(b3 - m.b3) <= epsilon &&
0356 std::abs(b4 - m.b4) <= epsilon &&
0357 std::abs(c1 - m.c1) <= epsilon &&
0358 std::abs(c2 - m.c2) <= epsilon &&
0359 std::abs(c3 - m.c3) <= epsilon &&
0360 std::abs(c4 - m.c4) <= epsilon &&
0361 std::abs(d1 - m.d1) <= epsilon &&
0362 std::abs(d2 - m.d2) <= epsilon &&
0363 std::abs(d3 - m.d3) <= epsilon &&
0364 std::abs(d4 - m.d4) <= epsilon;
0365 }
0366
0367 // ----------------------------------------------------------------------------------------
0368
0369 #define ASSIMP_MATRIX4_4_DECOMPOSE_PART \
0370 const aiMatrix4x4t<TReal>& _this = *this;/* Create alias for conveniance. */ \
0371 \
0372 /* extract translation */ \
0373 pPosition.x = _this[0][3]; \
0374 pPosition.y = _this[1][3]; \
0375 pPosition.z = _this[2][3]; \
0376 \
0377 /* extract the columns of the matrix. */ \
0378 aiVector3t<TReal> vCols[3] = { \
0379 aiVector3t<TReal>(_this[0][0],_this[1][0],_this[2][0]), \
0380 aiVector3t<TReal>(_this[0][1],_this[1][1],_this[2][1]), \
0381 aiVector3t<TReal>(_this[0][2],_this[1][2],_this[2][2]) \
0382 }; \
0383 \
0384 /* extract the scaling factors */ \
0385 pScaling.x = vCols[0].Length(); \
0386 pScaling.y = vCols[1].Length(); \
0387 pScaling.z = vCols[2].Length(); \
0388 \
0389 /* and the sign of the scaling */ \
0390 if (Determinant() < 0) pScaling = -pScaling; \
0391 \
0392 /* and remove all scaling from the matrix */ \
0393 if(pScaling.x) vCols[0] /= pScaling.x; \
0394 if(pScaling.y) vCols[1] /= pScaling.y; \
0395 if(pScaling.z) vCols[2] /= pScaling.z; \
0396 \
0397 do {} while(false)
0398
0399 template <typename TReal>
0400 AI_FORCE_INLINE
0401 void aiMatrix4x4t<TReal>::Decompose (aiVector3t<TReal>& pScaling, aiQuaterniont<TReal>& pRotation,
0402 aiVector3t<TReal>& pPosition) const {
0403 ASSIMP_MATRIX4_4_DECOMPOSE_PART;
0404
0405 // build a 3x3 rotation matrix
0406 aiMatrix3x3t<TReal> m(vCols[0].x,vCols[1].x,vCols[2].x,
0407 vCols[0].y,vCols[1].y,vCols[2].y,
0408 vCols[0].z,vCols[1].z,vCols[2].z);
0409
0410 // and generate the rotation quaternion from it
0411 pRotation = aiQuaterniont<TReal>(m);
0412 }
0413
0414 template <typename TReal>
0415 AI_FORCE_INLINE
0416 void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotation, aiVector3t<TReal>& pPosition) const {
0417 ASSIMP_MATRIX4_4_DECOMPOSE_PART;
0418
0419 /*
0420 assuming a right-handed coordinate system
0421 and post-multiplication of column vectors,
0422 the rotation matrix for an euler XYZ rotation is M = Rz * Ry * Rx.
0423 combining gives:
0424
0425 | CE BDE-AF ADE+BF 0 |
0426 M = | CF BDF+AE ADF-BE 0 |
0427 | -D CB AC 0 |
0428 | 0 0 0 1 |
0429
0430 where
0431 A = cos(angle_x), B = sin(angle_x);
0432 C = cos(angle_y), D = sin(angle_y);
0433 E = cos(angle_z), F = sin(angle_z);
0434 */
0435
0436 // Use a small epsilon to solve floating-point inaccuracies
0437 const TReal epsilon = Assimp::Math::getEpsilon<TReal>();
0438
0439 pRotation.y = std::asin(-vCols[0].z);// D. Angle around oY.
0440
0441 TReal C = std::cos(pRotation.y);
0442
0443 if(std::fabs(C) > epsilon)
0444 {
0445 // Finding angle around oX.
0446 TReal tan_x = vCols[2].z / C;// A
0447 TReal tan_y = vCols[1].z / C;// B
0448
0449 pRotation.x = std::atan2(tan_y, tan_x);
0450 // Finding angle around oZ.
0451 tan_x = vCols[0].x / C;// E
0452 tan_y = vCols[0].y / C;// F
0453 pRotation.z = std::atan2(tan_y, tan_x);
0454 }
0455 else
0456 {// oY is fixed.
0457 pRotation.x = 0;// Set angle around oX to 0. => A == 1, B == 0, C == 0, D == 1.
0458
0459 // And finding angle around oZ.
0460 TReal tan_x = vCols[1].y;// BDF+AE => E
0461 TReal tan_y = -vCols[1].x;// BDE-AF => F
0462
0463 pRotation.z = std::atan2(tan_y, tan_x);
0464 }
0465 }
0466
0467 #undef ASSIMP_MATRIX4_4_DECOMPOSE_PART
0468
0469 template <typename TReal>
0470 AI_FORCE_INLINE
0471 void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotationAxis, TReal& pRotationAngle,
0472 aiVector3t<TReal>& pPosition) const {
0473 aiQuaterniont<TReal> pRotation;
0474
0475 Decompose(pScaling, pRotation, pPosition);
0476 pRotation.Normalize();
0477
0478 TReal angle_cos = pRotation.w;
0479 TReal angle_sin = std::sqrt(1.0f - angle_cos * angle_cos);
0480
0481 pRotationAngle = std::acos(angle_cos) * 2;
0482
0483 // Use a small epsilon to solve floating-point inaccuracies
0484 const TReal epsilon = 10e-3f;
0485
0486 if(std::fabs(angle_sin) < epsilon) angle_sin = 1;
0487
0488 pRotationAxis.x = pRotation.x / angle_sin;
0489 pRotationAxis.y = pRotation.y / angle_sin;
0490 pRotationAxis.z = pRotation.z / angle_sin;
0491 }
0492
0493 // ----------------------------------------------------------------------------------------
0494 template <typename TReal>
0495 AI_FORCE_INLINE
0496 void aiMatrix4x4t<TReal>::DecomposeNoScaling (aiQuaterniont<TReal>& rotation,
0497 aiVector3t<TReal>& position) const {
0498 const aiMatrix4x4t<TReal>& _this = *this;
0499
0500 // extract translation
0501 position.x = _this[0][3];
0502 position.y = _this[1][3];
0503 position.z = _this[2][3];
0504
0505 // extract rotation
0506 rotation = aiQuaterniont<TReal>((aiMatrix3x3t<TReal>)_this);
0507 }
0508
0509 // ----------------------------------------------------------------------------------------
0510 template <typename TReal>
0511 AI_FORCE_INLINE
0512 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(const aiVector3t<TReal>& blubb) {
0513 return FromEulerAnglesXYZ(blubb.x,blubb.y,blubb.z);
0514 }
0515
0516 // ----------------------------------------------------------------------------------------
0517 template <typename TReal>
0518 AI_FORCE_INLINE
0519 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(TReal x, TReal y, TReal z) {
0520 aiMatrix4x4t<TReal>& _this = *this;
0521
0522 TReal cx = std::cos(x);
0523 TReal sx = std::sin(x);
0524 TReal cy = std::cos(y);
0525 TReal sy = std::sin(y);
0526 TReal cz = std::cos(z);
0527 TReal sz = std::sin(z);
0528
0529 // mz*my*mx
0530 _this.a1 = cz * cy;
0531 _this.a2 = cz * sy * sx - sz * cx;
0532 _this.a3 = sz * sx + cz * sy * cx;
0533
0534 _this.b1 = sz * cy;
0535 _this.b2 = cz * cx + sz * sy * sx;
0536 _this.b3 = sz * sy * cx - cz * sx;
0537
0538 _this.c1 = -sy;
0539 _this.c2 = cy * sx;
0540 _this.c3 = cy * cx;
0541
0542 return *this;
0543 }
0544
0545 // ----------------------------------------------------------------------------------------
0546 template <typename TReal>
0547 AI_FORCE_INLINE
0548 bool aiMatrix4x4t<TReal>::IsIdentity(const TReal epsilon) const {
0549 return (a2 <= epsilon && a2 >= -epsilon &&
0550 a3 <= epsilon && a3 >= -epsilon &&
0551 a4 <= epsilon && a4 >= -epsilon &&
0552 b1 <= epsilon && b1 >= -epsilon &&
0553 b3 <= epsilon && b3 >= -epsilon &&
0554 b4 <= epsilon && b4 >= -epsilon &&
0555 c1 <= epsilon && c1 >= -epsilon &&
0556 c2 <= epsilon && c2 >= -epsilon &&
0557 c4 <= epsilon && c4 >= -epsilon &&
0558 d1 <= epsilon && d1 >= -epsilon &&
0559 d2 <= epsilon && d2 >= -epsilon &&
0560 d3 <= epsilon && d3 >= -epsilon &&
0561 a1 <= 1.f+epsilon && a1 >= 1.f-epsilon &&
0562 b2 <= 1.f+epsilon && b2 >= 1.f-epsilon &&
0563 c3 <= 1.f+epsilon && c3 >= 1.f-epsilon &&
0564 d4 <= 1.f+epsilon && d4 >= 1.f-epsilon);
0565 }
0566
0567 // ----------------------------------------------------------------------------------------
0568 template <typename TReal>
0569 AI_FORCE_INLINE
0570 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationX(TReal a, aiMatrix4x4t<TReal>& out) {
0571 /*
0572 | 1 0 0 0 |
0573 M = | 0 cos(A) -sin(A) 0 |
0574 | 0 sin(A) cos(A) 0 |
0575 | 0 0 0 1 | */
0576 out = aiMatrix4x4t<TReal>();
0577 out.b2 = out.c3 = std::cos(a);
0578 out.b3 = -(out.c2 = std::sin(a));
0579 return out;
0580 }
0581
0582 // ----------------------------------------------------------------------------------------
0583 template <typename TReal>
0584 AI_FORCE_INLINE
0585 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationY(TReal a, aiMatrix4x4t<TReal>& out) {
0586 /*
0587 | cos(A) 0 sin(A) 0 |
0588 M = | 0 1 0 0 |
0589 | -sin(A) 0 cos(A) 0 |
0590 | 0 0 0 1 |
0591 */
0592 out = aiMatrix4x4t<TReal>();
0593 out.a1 = out.c3 = std::cos(a);
0594 out.c1 = -(out.a3 = std::sin(a));
0595 return out;
0596 }
0597
0598 // ----------------------------------------------------------------------------------------
0599 template <typename TReal>
0600 AI_FORCE_INLINE
0601 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationZ(TReal a, aiMatrix4x4t<TReal>& out) {
0602 /*
0603 | cos(A) -sin(A) 0 0 |
0604 M = | sin(A) cos(A) 0 0 |
0605 | 0 0 1 0 |
0606 | 0 0 0 1 | */
0607 out = aiMatrix4x4t<TReal>();
0608 out.a1 = out.b2 = std::cos(a);
0609 out.a2 = -(out.b1 = std::sin(a));
0610 return out;
0611 }
0612
0613 // ----------------------------------------------------------------------------------------
0614 // Returns a rotation matrix for a rotation around an arbitrary axis.
0615 template <typename TReal>
0616 AI_FORCE_INLINE
0617 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix4x4t<TReal>& out) {
0618 TReal c = std::cos( a), s = std::sin( a), t = 1 - c;
0619 TReal x = axis.x, y = axis.y, z = axis.z;
0620
0621 // Many thanks to MathWorld and Wikipedia
0622 out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y;
0623 out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x;
0624 out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c;
0625 out.a4 = out.b4 = out.c4 = static_cast<TReal>(0.0);
0626 out.d1 = out.d2 = out.d3 = static_cast<TReal>(0.0);
0627 out.d4 = static_cast<TReal>(1.0);
0628
0629 return out;
0630 }
0631
0632 // ----------------------------------------------------------------------------------------
0633 template <typename TReal>
0634 AI_FORCE_INLINE aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Translation( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out) {
0635 out = aiMatrix4x4t<TReal>();
0636 out.a4 = v.x;
0637 out.b4 = v.y;
0638 out.c4 = v.z;
0639 return out;
0640 }
0641
0642 // ----------------------------------------------------------------------------------------
0643 template <typename TReal>
0644 AI_FORCE_INLINE
0645 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Scaling( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out) {
0646 out = aiMatrix4x4t<TReal>();
0647 out.a1 = v.x;
0648 out.b2 = v.y;
0649 out.c3 = v.z;
0650 return out;
0651 }
0652
0653 // ----------------------------------------------------------------------------------------
0654 /** A function for creating a rotation matrix that rotates a vector called
0655 * "from" into another vector called "to".
0656 * Input : from[3], to[3] which both must be *normalized* non-zero vectors
0657 * Output: mtx[3][3] -- a 3x3 matrix in column-major form
0658 * Authors: Tomas Möller, John Hughes
0659 * "Efficiently Building a Matrix to Rotate One Vector to Another"
0660 * Journal of Graphics Tools, 4(4):1-4, 1999
0661 */
0662 // ----------------------------------------------------------------------------------------
0663 template <typename TReal>
0664 AI_FORCE_INLINE
0665 aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromToMatrix(const aiVector3t<TReal>& from,
0666 const aiVector3t<TReal>& to, aiMatrix4x4t<TReal>& mtx) {
0667 aiMatrix3x3t<TReal> m3;
0668 aiMatrix3x3t<TReal>::FromToMatrix(from,to,m3);
0669 mtx = aiMatrix4x4t<TReal>(m3);
0670 return mtx;
0671 }
0672
0673 #endif // __cplusplus
0674 #endif // AI_MATRIX4X4_INL_INC
