1 | /* |
---|---|

2 | --------------------------------------------------------------------------- |

3 | Open Asset Import Library (assimp) |

4 | --------------------------------------------------------------------------- |

5 | |

6 | Copyright (c) 2006-2017, assimp team |

7 | |

8 | |

9 | All rights reserved. |

10 | |

11 | Redistribution and use of this software in source and binary forms, |

12 | with or without modification, are permitted provided that the following |

13 | conditions are met: |

14 | |

15 | * Redistributions of source code must retain the above |

16 | copyright notice, this list of conditions and the |

17 | following disclaimer. |

18 | |

19 | * Redistributions in binary form must reproduce the above |

20 | copyright notice, this list of conditions and the |

21 | following disclaimer in the documentation and/or other |

22 | materials provided with the distribution. |

23 | |

24 | * Neither the name of the assimp team, nor the names of its |

25 | contributors may be used to endorse or promote products |

26 | derived from this software without specific prior |

27 | written permission of the assimp team. |

28 | |

29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |

30 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |

31 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |

32 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |

33 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |

34 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |

35 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |

36 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |

37 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |

38 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |

39 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |

40 | --------------------------------------------------------------------------- |

41 | */ |

42 | |

43 | /** @file matrix3x3.inl |

44 | * @brief Inline implementation of the 3x3 matrix operators |

45 | */ |

46 | #pragma once |

47 | #ifndef AI_MATRIX3X3_INL_INC |

48 | #define AI_MATRIX3X3_INL_INC |

49 | |

50 | #ifdef __cplusplus |

51 | #include "matrix3x3.h" |

52 | |

53 | #include "matrix4x4.h" |

54 | #include <algorithm> |

55 | #include <cmath> |

56 | #include <limits> |

57 | |

58 | // ------------------------------------------------------------------------------------------------ |

59 | // Construction from a 4x4 matrix. The remaining parts of the matrix are ignored. |

60 | template <typename TReal> |

61 | inline aiMatrix3x3t<TReal>::aiMatrix3x3t( const aiMatrix4x4t<TReal>& pMatrix) |

62 | { |

63 | a1 = pMatrix.a1; a2 = pMatrix.a2; a3 = pMatrix.a3; |

64 | b1 = pMatrix.b1; b2 = pMatrix.b2; b3 = pMatrix.b3; |

65 | c1 = pMatrix.c1; c2 = pMatrix.c2; c3 = pMatrix.c3; |

66 | } |

67 | |

68 | // ------------------------------------------------------------------------------------------------ |

69 | template <typename TReal> |

70 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::operator *= (const aiMatrix3x3t<TReal>& m) |

71 | { |

72 | *this = aiMatrix3x3t<TReal>(m.a1 * a1 + m.b1 * a2 + m.c1 * a3, |

73 | m.a2 * a1 + m.b2 * a2 + m.c2 * a3, |

74 | m.a3 * a1 + m.b3 * a2 + m.c3 * a3, |

75 | m.a1 * b1 + m.b1 * b2 + m.c1 * b3, |

76 | m.a2 * b1 + m.b2 * b2 + m.c2 * b3, |

77 | m.a3 * b1 + m.b3 * b2 + m.c3 * b3, |

78 | m.a1 * c1 + m.b1 * c2 + m.c1 * c3, |

79 | m.a2 * c1 + m.b2 * c2 + m.c2 * c3, |

80 | m.a3 * c1 + m.b3 * c2 + m.c3 * c3); |

81 | return *this; |

82 | } |

83 | |

84 | // ------------------------------------------------------------------------------------------------ |

85 | template <typename TReal> |

86 | template <typename TOther> |

87 | aiMatrix3x3t<TReal>::operator aiMatrix3x3t<TOther> () const |

88 | { |

89 | return aiMatrix3x3t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3), |

90 | static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3), |

91 | static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3)); |

92 | } |

93 | |

94 | // ------------------------------------------------------------------------------------------------ |

95 | template <typename TReal> |

96 | inline aiMatrix3x3t<TReal> aiMatrix3x3t<TReal>::operator* (const aiMatrix3x3t<TReal>& m) const |

97 | { |

98 | aiMatrix3x3t<TReal> temp( *this); |

99 | temp *= m; |

100 | return temp; |

101 | } |

102 | |

103 | // ------------------------------------------------------------------------------------------------ |

104 | template <typename TReal> |

105 | inline TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) { |

106 | switch ( p_iIndex ) { |

107 | case 0: |

108 | return &a1; |

109 | case 1: |

110 | return &b1; |

111 | case 2: |

112 | return &c1; |

113 | default: |

114 | break; |

115 | } |

116 | return &a1; |

117 | } |

118 | |

119 | // ------------------------------------------------------------------------------------------------ |

120 | template <typename TReal> |

121 | inline const TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) const { |

122 | switch ( p_iIndex ) { |

123 | case 0: |

124 | return &a1; |

125 | case 1: |

126 | return &b1; |

127 | case 2: |

128 | return &c1; |

129 | default: |

130 | break; |

131 | } |

132 | return &a1; |

133 | } |

134 | |

135 | // ------------------------------------------------------------------------------------------------ |

136 | template <typename TReal> |

137 | inline bool aiMatrix3x3t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const |

138 | { |

139 | return a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && |

140 | b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && |

141 | c1 == m.c1 && c2 == m.c2 && c3 == m.c3; |

142 | } |

143 | |

144 | // ------------------------------------------------------------------------------------------------ |

145 | template <typename TReal> |

146 | inline bool aiMatrix3x3t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const |

147 | { |

148 | return !(*this == m); |

149 | } |

150 | |

151 | // --------------------------------------------------------------------------- |

152 | template<typename TReal> |

153 | inline bool aiMatrix3x3t<TReal>::Equal(const aiMatrix4x4t<TReal>& m, TReal epsilon) const { |

154 | return |

155 | std::abs(a1 - m.a1) <= epsilon && |

156 | std::abs(a2 - m.a2) <= epsilon && |

157 | std::abs(a3 - m.a3) <= epsilon && |

158 | std::abs(b1 - m.b1) <= epsilon && |

159 | std::abs(b2 - m.b2) <= epsilon && |

160 | std::abs(b3 - m.b3) <= epsilon && |

161 | std::abs(c1 - m.c1) <= epsilon && |

162 | std::abs(c2 - m.c2) <= epsilon && |

163 | std::abs(c3 - m.c3) <= epsilon; |

164 | } |

165 | |

166 | // ------------------------------------------------------------------------------------------------ |

167 | template <typename TReal> |

168 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Transpose() |

169 | { |

170 | // (TReal&) don't remove, GCC complains cause of packed fields |

171 | std::swap( (TReal&)a2, (TReal&)b1); |

172 | std::swap( (TReal&)a3, (TReal&)c1); |

173 | std::swap( (TReal&)b3, (TReal&)c2); |

174 | return *this; |

175 | } |

176 | |

177 | // ---------------------------------------------------------------------------------------- |

178 | template <typename TReal> |

179 | inline TReal aiMatrix3x3t<TReal>::Determinant() const |

180 | { |

181 | return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 + a3*b1*c2 - a3*b2*c1; |

182 | } |

183 | |

184 | // ---------------------------------------------------------------------------------------- |

185 | template <typename TReal> |

186 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Inverse() |

187 | { |

188 | // Compute the reciprocal determinant |

189 | TReal det = Determinant(); |

190 | if(det == static_cast<TReal>(0.0)) |

191 | { |

192 | // Matrix not invertible. Setting all elements to nan is not really |

193 | // correct in a mathematical sense; but at least qnans are easy to |

194 | // spot. XXX we might throw an exception instead, which would |

195 | // be even much better to spot :/. |

196 | const TReal nan = std::numeric_limits<TReal>::quiet_NaN(); |

197 | *this = aiMatrix3x3t<TReal>( nan,nan,nan,nan,nan,nan,nan,nan,nan); |

198 | |

199 | return *this; |

200 | } |

201 | |

202 | TReal invdet = static_cast<TReal>(1.0) / det; |

203 | |

204 | aiMatrix3x3t<TReal> res; |

205 | res.a1 = invdet * (b2 * c3 - b3 * c2); |

206 | res.a2 = -invdet * (a2 * c3 - a3 * c2); |

207 | res.a3 = invdet * (a2 * b3 - a3 * b2); |

208 | res.b1 = -invdet * (b1 * c3 - b3 * c1); |

209 | res.b2 = invdet * (a1 * c3 - a3 * c1); |

210 | res.b3 = -invdet * (a1 * b3 - a3 * b1); |

211 | res.c1 = invdet * (b1 * c2 - b2 * c1); |

212 | res.c2 = -invdet * (a1 * c2 - a2 * c1); |

213 | res.c3 = invdet * (a1 * b2 - a2 * b1); |

214 | *this = res; |

215 | |

216 | return *this; |

217 | } |

218 | |

219 | // ------------------------------------------------------------------------------------------------ |

220 | template <typename TReal> |

221 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::RotationZ(TReal a, aiMatrix3x3t<TReal>& out) |

222 | { |

223 | out.a1 = out.b2 = std::cos(a); |

224 | out.b1 = std::sin(a); |

225 | out.a2 = - out.b1; |

226 | |

227 | out.a3 = out.b3 = out.c1 = out.c2 = 0.f; |

228 | out.c3 = 1.f; |

229 | |

230 | return out; |

231 | } |

232 | |

233 | // ------------------------------------------------------------------------------------------------ |

234 | // Returns a rotation matrix for a rotation around an arbitrary axis. |

235 | template <typename TReal> |

236 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix3x3t<TReal>& out) |

237 | { |

238 | TReal c = std::cos( a), s = std::sin( a), t = 1 - c; |

239 | TReal x = axis.x, y = axis.y, z = axis.z; |

240 | |

241 | // Many thanks to MathWorld and Wikipedia |

242 | out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y; |

243 | out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x; |

244 | out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c; |

245 | |

246 | return out; |

247 | } |

248 | |

249 | // ------------------------------------------------------------------------------------------------ |

250 | template <typename TReal> |

251 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Translation( const aiVector2t<TReal>& v, aiMatrix3x3t<TReal>& out) |

252 | { |

253 | out = aiMatrix3x3t<TReal>(); |

254 | out.a3 = v.x; |

255 | out.b3 = v.y; |

256 | return out; |

257 | } |

258 | |

259 | // ---------------------------------------------------------------------------------------- |

260 | /** A function for creating a rotation matrix that rotates a vector called |

261 | * "from" into another vector called "to". |

262 | * Input : from[3], to[3] which both must be *normalized* non-zero vectors |

263 | * Output: mtx[3][3] -- a 3x3 matrix in colum-major form |

264 | * Authors: Tomas MÃ¶ller, John Hughes |

265 | * "Efficiently Building a Matrix to Rotate One Vector to Another" |

266 | * Journal of Graphics Tools, 4(4):1-4, 1999 |

267 | */ |

268 | // ---------------------------------------------------------------------------------------- |

269 | template <typename TReal> |

270 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::FromToMatrix(const aiVector3t<TReal>& from, |

271 | const aiVector3t<TReal>& to, aiMatrix3x3t<TReal>& mtx) |

272 | { |

273 | const TReal e = from * to; |

274 | const TReal f = (e < 0)? -e:e; |

275 | |

276 | if (f > static_cast<TReal>(1.0) - static_cast<TReal>(0.00001)) /* "from" and "to"-vector almost parallel */ |

277 | { |

278 | aiVector3D u,v; /* temporary storage vectors */ |

279 | aiVector3D x; /* vector most nearly orthogonal to "from" */ |

280 | |

281 | x.x = (from.x > 0.0)? from.x : -from.x; |

282 | x.y = (from.y > 0.0)? from.y : -from.y; |

283 | x.z = (from.z > 0.0)? from.z : -from.z; |

284 | |

285 | if (x.x < x.y) |

286 | { |

287 | if (x.x < x.z) |

288 | { |

289 | x.x = static_cast<TReal>(1.0); |

290 | x.y = x.z = static_cast<TReal>(0.0); |

291 | } |

292 | else |

293 | { |

294 | x.z = static_cast<TReal>(1.0); |

295 | x.x = x.y = static_cast<TReal>(0.0); |

296 | } |

297 | } |

298 | else |

299 | { |

300 | if (x.y < x.z) |

301 | { |

302 | x.y = static_cast<TReal>(1.0); |

303 | x.x = x.z = static_cast<TReal>(0.0); |

304 | } |

305 | else |

306 | { |

307 | x.z = static_cast<TReal>(1.0); |

308 | x.x = x.y = static_cast<TReal>(0.0); |

309 | } |

310 | } |

311 | |

312 | u.x = x.x - from.x; u.y = x.y - from.y; u.z = x.z - from.z; |

313 | v.x = x.x - to.x; v.y = x.y - to.y; v.z = x.z - to.z; |

314 | |

315 | const TReal c1 = static_cast<TReal>(2.0) / (u * u); |

316 | const TReal c2 = static_cast<TReal>(2.0) / (v * v); |

317 | const TReal c3 = c1 * c2 * (u * v); |

318 | |

319 | for (unsigned int i = 0; i < 3; i++) |

320 | { |

321 | for (unsigned int j = 0; j < 3; j++) |

322 | { |

323 | mtx[i][j] = - c1 * u[i] * u[j] - c2 * v[i] * v[j] |

324 | + c3 * v[i] * u[j]; |

325 | } |

326 | mtx[i][i] += static_cast<TReal>(1.0); |

327 | } |

328 | } |

329 | else /* the most common case, unless "from"="to", or "from"=-"to" */ |

330 | { |

331 | const aiVector3D v = from ^ to; |

332 | /* ... use this hand optimized version (9 mults less) */ |

333 | const TReal h = static_cast<TReal>(1.0)/(static_cast<TReal>(1.0) + e); /* optimization by Gottfried Chen */ |

334 | const TReal hvx = h * v.x; |

335 | const TReal hvz = h * v.z; |

336 | const TReal hvxy = hvx * v.y; |

337 | const TReal hvxz = hvx * v.z; |

338 | const TReal hvyz = hvz * v.y; |

339 | mtx[0][0] = e + hvx * v.x; |

340 | mtx[0][1] = hvxy - v.z; |

341 | mtx[0][2] = hvxz + v.y; |

342 | |

343 | mtx[1][0] = hvxy + v.z; |

344 | mtx[1][1] = e + h * v.y * v.y; |

345 | mtx[1][2] = hvyz - v.x; |

346 | |

347 | mtx[2][0] = hvxz - v.y; |

348 | mtx[2][1] = hvyz + v.x; |

349 | mtx[2][2] = e + hvz * v.z; |

350 | } |

351 | return mtx; |

352 | } |

353 | |

354 | |

355 | #endif // __cplusplus |

356 | #endif // AI_MATRIX3X3_INL_INC |

357 |