1/* 2--------------------------------------------------------------------------- 3Open Asset Import Library (assimp) 4--------------------------------------------------------------------------- 5 6Copyright (c) 2006-2012, assimp team 7 8All rights reserved. 9 10Redistribution and use of this software in source and binary forms, 11with or without modification, are permitted provided that the following 12conditions are met: 13 14* Redistributions of source code must retain the above 15 copyright notice, this list of conditions and the 16 following disclaimer. 17 18* Redistributions in binary form must reproduce the above 19 copyright notice, this list of conditions and the 20 following disclaimer in the documentation and/or other 21 materials provided with the distribution. 22 23* Neither the name of the assimp team, nor the names of its 24 contributors may be used to endorse or promote products 25 derived from this software without specific prior 26 written permission of the assimp team. 27 28THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 29"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 30LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 31A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 32OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 33SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 34LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 35DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 36THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 37(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 38OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 39--------------------------------------------------------------------------- 40*/ 41 42/** @file aiMatrix3x3.inl 43 * @brief Inline implementation of the 3x3 matrix operators 44 */ 45#ifndef AI_MATRIX3x3_INL_INC 46#define AI_MATRIX3x3_INL_INC 47 48#ifdef __cplusplus 49#include "matrix3x3.h" 50 51#include "matrix4x4.h" 52#include <algorithm> 53#include <limits> 54 55// ------------------------------------------------------------------------------------------------ 56// Construction from a 4x4 matrix. The remaining parts of the matrix are ignored. 57template <typename TReal> 58inline aiMatrix3x3t<TReal>::aiMatrix3x3t( const aiMatrix4x4t<TReal>& pMatrix) 59{ 60 a1 = pMatrix.a1; a2 = pMatrix.a2; a3 = pMatrix.a3; 61 b1 = pMatrix.b1; b2 = pMatrix.b2; b3 = pMatrix.b3; 62 c1 = pMatrix.c1; c2 = pMatrix.c2; c3 = pMatrix.c3; 63} 64 65// ------------------------------------------------------------------------------------------------ 66template <typename TReal> 67inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::operator *= (const aiMatrix3x3t<TReal>& m) 68{ 69 *this = aiMatrix3x3t<TReal>(m.a1 * a1 + m.b1 * a2 + m.c1 * a3, 70 m.a2 * a1 + m.b2 * a2 + m.c2 * a3, 71 m.a3 * a1 + m.b3 * a2 + m.c3 * a3, 72 m.a1 * b1 + m.b1 * b2 + m.c1 * b3, 73 m.a2 * b1 + m.b2 * b2 + m.c2 * b3, 74 m.a3 * b1 + m.b3 * b2 + m.c3 * b3, 75 m.a1 * c1 + m.b1 * c2 + m.c1 * c3, 76 m.a2 * c1 + m.b2 * c2 + m.c2 * c3, 77 m.a3 * c1 + m.b3 * c2 + m.c3 * c3); 78 return *this; 79} 80 81// ------------------------------------------------------------------------------------------------ 82template <typename TReal> 83template <typename TOther> 84aiMatrix3x3t<TReal>::operator aiMatrix3x3t<TOther> () const 85{ 86 return aiMatrix3x3t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3), 87 static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3), 88 static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3)); 89} 90 91// ------------------------------------------------------------------------------------------------ 92template <typename TReal> 93inline aiMatrix3x3t<TReal> aiMatrix3x3t<TReal>::operator* (const aiMatrix3x3t<TReal>& m) const 94{ 95 aiMatrix3x3t<TReal> temp( *this); 96 temp *= m; 97 return temp; 98} 99 100// ------------------------------------------------------------------------------------------------ 101template <typename TReal> 102inline TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) 103{ 104 return &this->a1 + p_iIndex * 3; 105} 106 107// ------------------------------------------------------------------------------------------------ 108template <typename TReal> 109inline const TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) const 110{ 111 return &this->a1 + p_iIndex * 3; 112} 113 114// ------------------------------------------------------------------------------------------------ 115template <typename TReal> 116inline bool aiMatrix3x3t<TReal>::operator== (const aiMatrix4x4t<TReal> m) const 117{ 118 return a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && 119 b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && 120 c1 == m.c1 && c2 == m.c2 && c3 == m.c3; 121} 122 123// ------------------------------------------------------------------------------------------------ 124template <typename TReal> 125inline bool aiMatrix3x3t<TReal>::operator!= (const aiMatrix4x4t<TReal> m) const 126{ 127 return !(*this == m); 128} 129 130// ------------------------------------------------------------------------------------------------ 131template <typename TReal> 132inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Transpose() 133{ 134 // (TReal&) don't remove, GCC complains cause of packed fields 135 std::swap( (TReal&)a2, (TReal&)b1); 136 std::swap( (TReal&)a3, (TReal&)c1); 137 std::swap( (TReal&)b3, (TReal&)c2); 138 return *this; 139} 140 141// ---------------------------------------------------------------------------------------- 142template <typename TReal> 143inline TReal aiMatrix3x3t<TReal>::Determinant() const 144{ 145 return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 + a3*b1*c2 - a3*b2*c1; 146} 147 148// ---------------------------------------------------------------------------------------- 149template <typename TReal> 150inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Inverse() 151{ 152 // Compute the reciprocal determinant 153 TReal det = Determinant(); 154 if(det == static_cast<TReal>(0.0)) 155 { 156 // Matrix not invertible. Setting all elements to nan is not really 157 // correct in a mathematical sense; but at least qnans are easy to 158 // spot. XXX we might throw an exception instead, which would 159 // be even much better to spot :/. 160 const TReal nan = std::numeric_limits<TReal>::quiet_NaN(); 161 *this = aiMatrix3x3t<TReal>( nan,nan,nan,nan,nan,nan,nan,nan,nan); 162 163 return *this; 164 } 165 166 TReal invdet = static_cast<TReal>(1.0) / det; 167 168 aiMatrix3x3t<TReal> res; 169 res.a1 = invdet * (b2 * c3 - b3 * c2); 170 res.a2 = -invdet * (a2 * c3 - a3 * c2); 171 res.a3 = invdet * (a2 * b3 - a3 * b2); 172 res.b1 = -invdet * (b1 * c3 - b3 * c1); 173 res.b2 = invdet * (a1 * c3 - a3 * c1); 174 res.b3 = -invdet * (a1 * b3 - a3 * b1); 175 res.c1 = invdet * (b1 * c2 - b2 * c1); 176 res.c2 = -invdet * (a1 * c2 - a2 * c1); 177 res.c3 = invdet * (a1 * b2 - a2 * b1); 178 *this = res; 179 180 return *this; 181} 182 183// ------------------------------------------------------------------------------------------------ 184template <typename TReal> 185inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::RotationZ(TReal a, aiMatrix3x3t<TReal>& out) 186{ 187 out.a1 = out.b2 = math::cos(a); 188 out.b1 = math::sin(a); 189 out.a2 = - out.b1; 190 191 out.a3 = out.b3 = out.c1 = out.c2 = 0.f; 192 out.c3 = 1.f; 193 194 return out; 195} 196 197// ------------------------------------------------------------------------------------------------ 198// Returns a rotation matrix for a rotation around an arbitrary axis. 199template <typename TReal> 200inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix3x3t<TReal>& out) 201{ 202 TReal c = math::cos( a), s = math::sin( a), t = 1 - c; 203 TReal x = axis.x, y = axis.y, z = axis.z; 204 205 // Many thanks to MathWorld and Wikipedia 206 out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y; 207 out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x; 208 out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c; 209 210 return out; 211} 212 213// ------------------------------------------------------------------------------------------------ 214template <typename TReal> 215inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Translation( const aiVector2t<TReal>& v, aiMatrix3x3t<TReal>& out) 216{ 217 out = aiMatrix3x3t<TReal>(); 218 out.a3 = v.x; 219 out.b3 = v.y; 220 return out; 221} 222 223// ---------------------------------------------------------------------------------------- 224/** A function for creating a rotation matrix that rotates a vector called 225 * "from" into another vector called "to". 226 * Input : from[3], to[3] which both must be *normalized* non-zero vectors 227 * Output: mtx[3][3] -- a 3x3 matrix in colum-major form 228 * Authors: Tomas M�ller, John Hughes 229 * "Efficiently Building a Matrix to Rotate One Vector to Another" 230 * Journal of Graphics Tools, 4(4):1-4, 1999 231 */ 232// ---------------------------------------------------------------------------------------- 233template <typename TReal> 234inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::FromToMatrix(const aiVector3t<TReal>& from, 235 const aiVector3t<TReal>& to, aiMatrix3x3t<TReal>& mtx) 236{ 237 const TReal e = from * to; 238 const TReal f = (e < 0)? -e:e; 239 240 if (f > static_cast<TReal>(1.0) - static_cast<TReal>(0.00001)) /* "from" and "to"-vector almost parallel */ 241 { 242 aiVector3D u,v; /* temporary storage vectors */ 243 aiVector3D x; /* vector most nearly orthogonal to "from" */ 244 245 x.x = (from.x > 0.0)? from.x : -from.x; 246 x.y = (from.y > 0.0)? from.y : -from.y; 247 x.z = (from.z > 0.0)? from.z : -from.z; 248 249 if (x.x < x.y) 250 { 251 if (x.x < x.z) 252 { 253 x.x = static_cast<TReal>(1.0); x.y = x.z = static_cast<TReal>(0.0); 254 } 255 else 256 { 257 x.z = static_cast<TReal>(1.0); x.y = x.z = static_cast<TReal>(0.0); 258 } 259 } 260 else 261 { 262 if (x.y < x.z) 263 { 264 x.y = static_cast<TReal>(1.0); x.x = x.z = static_cast<TReal>(0.0); 265 } 266 else 267 { 268 x.z = static_cast<TReal>(1.0); x.x = x.y = static_cast<TReal>(0.0); 269 } 270 } 271 272 u.x = x.x - from.x; u.y = x.y - from.y; u.z = x.z - from.z; 273 v.x = x.x - to.x; v.y = x.y - to.y; v.z = x.z - to.z; 274 275 const TReal c1 = static_cast<TReal>(2.0) / (u * u); 276 const TReal c2 = static_cast<TReal>(2.0) / (v * v); 277 const TReal c3 = c1 * c2 * (u * v); 278 279 for (unsigned int i = 0; i < 3; i++) 280 { 281 for (unsigned int j = 0; j < 3; j++) 282 { 283 mtx[i][j] = - c1 * u[i] * u[j] - c2 * v[i] * v[j] 284 + c3 * v[i] * u[j]; 285 } 286 mtx[i][i] += static_cast<TReal>(1.0); 287 } 288 } 289 else /* the most common case, unless "from"="to", or "from"=-"to" */ 290 { 291 const aiVector3D v = from ^ to; 292 /* ... use this hand optimized version (9 mults less) */ 293 const TReal h = static_cast<TReal>(1.0)/(static_cast<TReal>(1.0) + e); /* optimization by Gottfried Chen */ 294 const TReal hvx = h * v.x; 295 const TReal hvz = h * v.z; 296 const TReal hvxy = hvx * v.y; 297 const TReal hvxz = hvx * v.z; 298 const TReal hvyz = hvz * v.y; 299 mtx[0][0] = e + hvx * v.x; 300 mtx[0][1] = hvxy - v.z; 301 mtx[0][2] = hvxz + v.y; 302 303 mtx[1][0] = hvxy + v.z; 304 mtx[1][1] = e + h * v.y * v.y; 305 mtx[1][2] = hvyz - v.x; 306 307 mtx[2][0] = hvxz - v.y; 308 mtx[2][1] = hvyz + v.x; 309 mtx[2][2] = e + hvz * v.z; 310 } 311 return mtx; 312} 313 314 315#endif // __cplusplus 316#endif // AI_MATRIX3x3_INL_INC 317