miniassimp: 879c81d94345 include/miniassimp/matrix4x4.inl

miniassimp

view include/miniassimp/matrix4x4.inl @ 0:879c81d94345

initial commit

author	John Tsiombikas <nuclear@member.fsf.org>
date	Mon, 28 Jan 2019 18:19:26 +0200
parents
children

line source

1 /*

2 ---------------------------------------------------------------------------

3 Open Asset Import Library (assimp)

4 ---------------------------------------------------------------------------

12 Redistribution and use of this software in source and binary forms,

13 with or without modification, are permitted provided that the following

14 conditions are met:

16 * Redistributions of source code must retain the above

17 copyright notice, this list of conditions and the

18 following disclaimer.

20 * Redistributions in binary form must reproduce the above

21 copyright notice, this list of conditions and the

22 following disclaimer in the documentation and/or other

23 materials provided with the distribution.

25 * Neither the name of the assimp team, nor the names of its

26 contributors may be used to endorse or promote products

27 derived from this software without specific prior

28 written permission of the assimp team.

30 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

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

32 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

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

34 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

35 SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

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

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

38 THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

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

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

41 ---------------------------------------------------------------------------

42 */

44 /** @file matrix4x4.inl

45 * @brief Inline implementation of the 4x4 matrix operators

46 */

47 #pragma once

48 #ifndef AI_MATRIX4X4_INL_INC

49 #define AI_MATRIX4X4_INL_INC

51 #ifdef __cplusplus

53 #include "matrix4x4.h"

54 #include "matrix3x3.h"

55 #include "quaternion.h"

57 #include <algorithm>

58 #include <limits>

59 #include <cmath>

61 // ----------------------------------------------------------------------------------------

62 template <typename TReal>

63 aiMatrix4x4t<TReal>::aiMatrix4x4t() AI_NO_EXCEPT :

64 a1(1.0f), a2(), a3(), a4(),

65 b1(), b2(1.0f), b3(), b4(),

66 c1(), c2(), c3(1.0f), c4(),

67 d1(), d2(), d3(), d4(1.0f)

68 {

70 }

72 // ----------------------------------------------------------------------------------------

73 template <typename TReal>

74 aiMatrix4x4t<TReal>::aiMatrix4x4t (TReal _a1, TReal _a2, TReal _a3, TReal _a4,

75 TReal _b1, TReal _b2, TReal _b3, TReal _b4,

76 TReal _c1, TReal _c2, TReal _c3, TReal _c4,

77 TReal _d1, TReal _d2, TReal _d3, TReal _d4) :

78 a1(_a1), a2(_a2), a3(_a3), a4(_a4),

79 b1(_b1), b2(_b2), b3(_b3), b4(_b4),

80 c1(_c1), c2(_c2), c3(_c3), c4(_c4),

81 d1(_d1), d2(_d2), d3(_d3), d4(_d4)

82 {

84 }

86 // ------------------------------------------------------------------------------------------------

87 template <typename TReal>

88 template <typename TOther>

89 aiMatrix4x4t<TReal>::operator aiMatrix4x4t<TOther> () const

90 {

91 return aiMatrix4x4t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3),static_cast<TOther>(a4),

92 static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3),static_cast<TOther>(b4),

93 static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3),static_cast<TOther>(c4),

94 static_cast<TOther>(d1),static_cast<TOther>(d2),static_cast<TOther>(d3),static_cast<TOther>(d4));

95 }

98 // ----------------------------------------------------------------------------------------

99 template <typename TReal>

100 inline aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiMatrix3x3t<TReal>& m)

101 {

102 a1 = m.a1; a2 = m.a2; a3 = m.a3; a4 = static_cast<TReal>(0.0);

103 b1 = m.b1; b2 = m.b2; b3 = m.b3; b4 = static_cast<TReal>(0.0);

104 c1 = m.c1; c2 = m.c2; c3 = m.c3; c4 = static_cast<TReal>(0.0);

105 d1 = static_cast<TReal>(0.0); d2 = static_cast<TReal>(0.0); d3 = static_cast<TReal>(0.0); d4 = static_cast<TReal>(1.0);

106 }

107

108 // ----------------------------------------------------------------------------------------

109 template <typename TReal>

110 inline aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiVector3t<TReal>& scaling, const aiQuaterniont<TReal>& rotation, const aiVector3t<TReal>& position)

111 {

112 // build a 3x3 rotation matrix

113 aiMatrix3x3t<TReal> m = rotation.GetMatrix();

114

115 a1 = m.a1 * scaling.x;

116 a2 = m.a2 * scaling.x;

117 a3 = m.a3 * scaling.x;

118 a4 = position.x;

119

120 b1 = m.b1 * scaling.y;

121 b2 = m.b2 * scaling.y;

122 b3 = m.b3 * scaling.y;

123 b4 = position.y;

124

125 c1 = m.c1 * scaling.z;

126 c2 = m.c2 * scaling.z;

127 c3 = m.c3 * scaling.z;

128 c4= position.z;

129

130 d1 = static_cast<TReal>(0.0);

131 d2 = static_cast<TReal>(0.0);

132 d3 = static_cast<TReal>(0.0);

133 d4 = static_cast<TReal>(1.0);

134 }

135

136 // ----------------------------------------------------------------------------------------

137 template <typename TReal>

138 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::operator *= (const aiMatrix4x4t<TReal>& m)

139 {

140 *this = aiMatrix4x4t<TReal>(

141 m.a1 * a1 + m.b1 * a2 + m.c1 * a3 + m.d1 * a4,

142 m.a2 * a1 + m.b2 * a2 + m.c2 * a3 + m.d2 * a4,

143 m.a3 * a1 + m.b3 * a2 + m.c3 * a3 + m.d3 * a4,

144 m.a4 * a1 + m.b4 * a2 + m.c4 * a3 + m.d4 * a4,

145 m.a1 * b1 + m.b1 * b2 + m.c1 * b3 + m.d1 * b4,

146 m.a2 * b1 + m.b2 * b2 + m.c2 * b3 + m.d2 * b4,

147 m.a3 * b1 + m.b3 * b2 + m.c3 * b3 + m.d3 * b4,

148 m.a4 * b1 + m.b4 * b2 + m.c4 * b3 + m.d4 * b4,

149 m.a1 * c1 + m.b1 * c2 + m.c1 * c3 + m.d1 * c4,

150 m.a2 * c1 + m.b2 * c2 + m.c2 * c3 + m.d2 * c4,

151 m.a3 * c1 + m.b3 * c2 + m.c3 * c3 + m.d3 * c4,

152 m.a4 * c1 + m.b4 * c2 + m.c4 * c3 + m.d4 * c4,

153 m.a1 * d1 + m.b1 * d2 + m.c1 * d3 + m.d1 * d4,

154 m.a2 * d1 + m.b2 * d2 + m.c2 * d3 + m.d2 * d4,

155 m.a3 * d1 + m.b3 * d2 + m.c3 * d3 + m.d3 * d4,

156 m.a4 * d1 + m.b4 * d2 + m.c4 * d3 + m.d4 * d4);

157 return *this;

158 }

159

160 // ----------------------------------------------------------------------------------------

161 template <typename TReal>

162 inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const TReal& aFloat) const

163 {

164 aiMatrix4x4t<TReal> temp(

165 a1 * aFloat,

166 a2 * aFloat,

167 a3 * aFloat,

168 a4 * aFloat,

169 b1 * aFloat,

170 b2 * aFloat,

171 b3 * aFloat,

172 b4 * aFloat,

173 c1 * aFloat,

174 c2 * aFloat,

175 c3 * aFloat,

176 c4 * aFloat,

177 d1 * aFloat,

178 d2 * aFloat,

179 d3 * aFloat,

180 d4 * aFloat);

181 return temp;

182 }

183

184 // ----------------------------------------------------------------------------------------

185 template <typename TReal>

186 inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator+ (const aiMatrix4x4t<TReal>& m) const

187 {

188 aiMatrix4x4t<TReal> temp(

189 m.a1 + a1,

190 m.a2 + a2,

191 m.a3 + a3,

192 m.a4 + a4,

193 m.b1 + b1,

194 m.b2 + b2,

195 m.b3 + b3,

196 m.b4 + b4,

197 m.c1 + c1,

198 m.c2 + c2,

199 m.c3 + c3,

200 m.c4 + c4,

201 m.d1 + d1,

202 m.d2 + d2,

203 m.d3 + d3,

204 m.d4 + d4);

205 return temp;

206 }

207

208 // ----------------------------------------------------------------------------------------

209 template <typename TReal>

210 inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const aiMatrix4x4t<TReal>& m) const

211 {

212 aiMatrix4x4t<TReal> temp( *this);

213 temp *= m;

214 return temp;

215 }

216

217

218 // ----------------------------------------------------------------------------------------

219 template <typename TReal>

220 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Transpose()

221 {

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

223 std::swap( (TReal&)b1, (TReal&)a2);

224 std::swap( (TReal&)c1, (TReal&)a3);

225 std::swap( (TReal&)c2, (TReal&)b3);

226 std::swap( (TReal&)d1, (TReal&)a4);

227 std::swap( (TReal&)d2, (TReal&)b4);

228 std::swap( (TReal&)d3, (TReal&)c4);

229 return *this;

230 }

231

232

233 // ----------------------------------------------------------------------------------------

234 template <typename TReal>

235 inline TReal aiMatrix4x4t<TReal>::Determinant() const

236 {

237 return a1*b2*c3*d4 - a1*b2*c4*d3 + a1*b3*c4*d2 - a1*b3*c2*d4

238 + a1*b4*c2*d3 - a1*b4*c3*d2 - a2*b3*c4*d1 + a2*b3*c1*d4

239 - a2*b4*c1*d3 + a2*b4*c3*d1 - a2*b1*c3*d4 + a2*b1*c4*d3

240 + a3*b4*c1*d2 - a3*b4*c2*d1 + a3*b1*c2*d4 - a3*b1*c4*d2

241 + a3*b2*c4*d1 - a3*b2*c1*d4 - a4*b1*c2*d3 + a4*b1*c3*d2

242 - a4*b2*c3*d1 + a4*b2*c1*d3 - a4*b3*c1*d2 + a4*b3*c2*d1;

243 }

244

245 // ----------------------------------------------------------------------------------------

246 template <typename TReal>

247 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Inverse()

248 {

249 // Compute the reciprocal determinant

250 const TReal det = Determinant();

251 if(det == static_cast<TReal>(0.0))

252 {

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

254 // correct in a mathematical sense but it is easy to debug for the

255 // programmer.

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

257 *this = aiMatrix4x4t<TReal>(

258 nan,nan,nan,nan,

259 nan,nan,nan,nan,

260 nan,nan,nan,nan,

261 nan,nan,nan,nan);

262

263 return *this;

264 }

265

266 const TReal invdet = static_cast<TReal>(1.0) / det;

267

268 aiMatrix4x4t<TReal> res;

269 res.a1 = invdet * (b2 * (c3 * d4 - c4 * d3) + b3 * (c4 * d2 - c2 * d4) + b4 * (c2 * d3 - c3 * d2));

270 res.a2 = -invdet * (a2 * (c3 * d4 - c4 * d3) + a3 * (c4 * d2 - c2 * d4) + a4 * (c2 * d3 - c3 * d2));

271 res.a3 = invdet * (a2 * (b3 * d4 - b4 * d3) + a3 * (b4 * d2 - b2 * d4) + a4 * (b2 * d3 - b3 * d2));

272 res.a4 = -invdet * (a2 * (b3 * c4 - b4 * c3) + a3 * (b4 * c2 - b2 * c4) + a4 * (b2 * c3 - b3 * c2));

273 res.b1 = -invdet * (b1 * (c3 * d4 - c4 * d3) + b3 * (c4 * d1 - c1 * d4) + b4 * (c1 * d3 - c3 * d1));

274 res.b2 = invdet * (a1 * (c3 * d4 - c4 * d3) + a3 * (c4 * d1 - c1 * d4) + a4 * (c1 * d3 - c3 * d1));

275 res.b3 = -invdet * (a1 * (b3 * d4 - b4 * d3) + a3 * (b4 * d1 - b1 * d4) + a4 * (b1 * d3 - b3 * d1));

276 res.b4 = invdet * (a1 * (b3 * c4 - b4 * c3) + a3 * (b4 * c1 - b1 * c4) + a4 * (b1 * c3 - b3 * c1));

277 res.c1 = invdet * (b1 * (c2 * d4 - c4 * d2) + b2 * (c4 * d1 - c1 * d4) + b4 * (c1 * d2 - c2 * d1));

278 res.c2 = -invdet * (a1 * (c2 * d4 - c4 * d2) + a2 * (c4 * d1 - c1 * d4) + a4 * (c1 * d2 - c2 * d1));

279 res.c3 = invdet * (a1 * (b2 * d4 - b4 * d2) + a2 * (b4 * d1 - b1 * d4) + a4 * (b1 * d2 - b2 * d1));

280 res.c4 = -invdet * (a1 * (b2 * c4 - b4 * c2) + a2 * (b4 * c1 - b1 * c4) + a4 * (b1 * c2 - b2 * c1));

281 res.d1 = -invdet * (b1 * (c2 * d3 - c3 * d2) + b2 * (c3 * d1 - c1 * d3) + b3 * (c1 * d2 - c2 * d1));

282 res.d2 = invdet * (a1 * (c2 * d3 - c3 * d2) + a2 * (c3 * d1 - c1 * d3) + a3 * (c1 * d2 - c2 * d1));

283 res.d3 = -invdet * (a1 * (b2 * d3 - b3 * d2) + a2 * (b3 * d1 - b1 * d3) + a3 * (b1 * d2 - b2 * d1));

284 res.d4 = invdet * (a1 * (b2 * c3 - b3 * c2) + a2 * (b3 * c1 - b1 * c3) + a3 * (b1 * c2 - b2 * c1));

285 *this = res;

286

287 return *this;

288 }

289

290 // ----------------------------------------------------------------------------------------

291 template <typename TReal>

292 inline TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) {

293 if (p_iIndex > 3) {

294 return NULL;

295 }

296 switch ( p_iIndex ) {

297 case 0:

298 return &a1;

299 case 1:

300 return &b1;

301 case 2:

302 return &c1;

303 case 3:

304 return &d1;

305 default:

306 break;

307 }

308 return &a1;

309 }

310

311 // ----------------------------------------------------------------------------------------

312 template <typename TReal>

313 inline const TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) const {

314 if (p_iIndex > 3) {

315 return NULL;

316 }

317

318 switch ( p_iIndex ) {

319 case 0:

320 return &a1;

321 case 1:

322 return &b1;

323 case 2:

324 return &c1;

325 case 3:

326 return &d1;

327 default:

328 break;

329 }

330 return &a1;

331 }

332

333 // ----------------------------------------------------------------------------------------

334 template <typename TReal>

335 inline bool aiMatrix4x4t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const

336 {

337 return (a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && a4 == m.a4 &&

338 b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && b4 == m.b4 &&

339 c1 == m.c1 && c2 == m.c2 && c3 == m.c3 && c4 == m.c4 &&

340 d1 == m.d1 && d2 == m.d2 && d3 == m.d3 && d4 == m.d4);

341 }

342

343 // ----------------------------------------------------------------------------------------

344 template <typename TReal>

345 inline bool aiMatrix4x4t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const

346 {

347 return !(*this == m);

348 }

349

350 // ---------------------------------------------------------------------------

351 template<typename TReal>

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

353 return

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

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

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

357 std::abs(a4 - m.a4) <= epsilon &&

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

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

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

361 std::abs(b4 - m.b4) <= epsilon &&

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

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

364 std::abs(c3 - m.c3) <= epsilon &&

365 std::abs(c4 - m.c4) <= epsilon &&

366 std::abs(d1 - m.d1) <= epsilon &&

367 std::abs(d2 - m.d2) <= epsilon &&

368 std::abs(d3 - m.d3) <= epsilon &&

369 std::abs(d4 - m.d4) <= epsilon;

370 }

371

372 // ----------------------------------------------------------------------------------------

373

374 #define ASSIMP_MATRIX4_4_DECOMPOSE_PART \

375 const aiMatrix4x4t<TReal>& _this = *this;/* Create alias for conveniance. */ \

376 \

377 /* extract translation */ \

378 pPosition.x = _this[0][3]; \

379 pPosition.y = _this[1][3]; \

380 pPosition.z = _this[2][3]; \

381 \

382 /* extract the columns of the matrix. */ \

383 aiVector3t<TReal> vCols[3] = { \

384 aiVector3t<TReal>(_this[0][0],_this[1][0],_this[2][0]), \

385 aiVector3t<TReal>(_this[0][1],_this[1][1],_this[2][1]), \

386 aiVector3t<TReal>(_this[0][2],_this[1][2],_this[2][2]) \

387 }; \

388 \

389 /* extract the scaling factors */ \

390 pScaling.x = vCols[0].Length(); \

391 pScaling.y = vCols[1].Length(); \

392 pScaling.z = vCols[2].Length(); \

393 \

394 /* and the sign of the scaling */ \

395 if (Determinant() < 0) pScaling = -pScaling; \

396 \

397 /* and remove all scaling from the matrix */ \

398 if(pScaling.x) vCols[0] /= pScaling.x; \

399 if(pScaling.y) vCols[1] /= pScaling.y; \

400 if(pScaling.z) vCols[2] /= pScaling.z; \

401 \

402 do {} while(false)

403

404

405

406

407 template <typename TReal>

408 inline void aiMatrix4x4t<TReal>::Decompose (aiVector3t<TReal>& pScaling, aiQuaterniont<TReal>& pRotation,

409 aiVector3t<TReal>& pPosition) const

410 {

411 ASSIMP_MATRIX4_4_DECOMPOSE_PART;

412

413 // build a 3x3 rotation matrix

414 aiMatrix3x3t<TReal> m(vCols[0].x,vCols[1].x,vCols[2].x,

415 vCols[0].y,vCols[1].y,vCols[2].y,

416 vCols[0].z,vCols[1].z,vCols[2].z);

417

418 // and generate the rotation quaternion from it

419 pRotation = aiQuaterniont<TReal>(m);

420 }

421

422 template <typename TReal>

423 inline void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotation, aiVector3t<TReal>& pPosition) const

424 {

425 ASSIMP_MATRIX4_4_DECOMPOSE_PART;

426

427 /*

428 assuming a right-handed coordinate system

429 and post-multiplication of column vectors,

430 the rotation matrix for an euler XYZ rotation is M = Rz * Ry * Rx.

431 combining gives:

432

433 | CE BDE-AF ADE+BF 0 |

434 M = | CF BDF+AE ADF-BE 0 |

435 | -D CB AC 0 |

436 | 0 0 0 1 |

437

438 where

439 A = cos(angle_x), B = sin(angle_x);

440 C = cos(angle_y), D = sin(angle_y);

441 E = cos(angle_z), F = sin(angle_z);

442 */

443

444 // Use a small epsilon to solve floating-point inaccuracies

445 const TReal epsilon = 10e-3f;

446

447 pRotation.y = std::asin(-vCols[0].z);// D. Angle around oY.

448

449 TReal C = std::cos(pRotation.y);

450

451 if(std::fabs(C) > epsilon)

452 {

453 // Finding angle around oX.

454 TReal tan_x = vCols[2].z / C;// A

455 TReal tan_y = vCols[1].z / C;// B

456

457 pRotation.x = std::atan2(tan_y, tan_x);

458 // Finding angle around oZ.

459 tan_x = vCols[0].x / C;// E

460 tan_y = vCols[0].y / C;// F

461 pRotation.z = std::atan2(tan_y, tan_x);

462 }

463 else

464 {// oY is fixed.

465 pRotation.x = 0;// Set angle around oX to 0. => A == 1, B == 0, C == 0, D == 1.

466

467 // And finding angle around oZ.

468 TReal tan_x = vCols[1].y;// BDF+AE => E

469 TReal tan_y = -vCols[1].x;// BDE-AF => F

470

471 pRotation.z = std::atan2(tan_y, tan_x);

472 }

473 }

474

475 #undef ASSIMP_MATRIX4_4_DECOMPOSE_PART

476

477 template <typename TReal>

478 inline void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotationAxis, TReal& pRotationAngle,

479 aiVector3t<TReal>& pPosition) const

480 {

481 aiQuaterniont<TReal> pRotation;

482

483 Decompose(pScaling, pRotation, pPosition);

484 pRotation.Normalize();

485

486 TReal angle_cos = pRotation.w;

487 TReal angle_sin = std::sqrt(1.0f - angle_cos * angle_cos);

488

489 pRotationAngle = std::acos(angle_cos) * 2;

490

491 // Use a small epsilon to solve floating-point inaccuracies

492 const TReal epsilon = 10e-3f;

493

494 if(std::fabs(angle_sin) < epsilon) angle_sin = 1;

495

496 pRotationAxis.x = pRotation.x / angle_sin;

497 pRotationAxis.y = pRotation.y / angle_sin;

498 pRotationAxis.z = pRotation.z / angle_sin;

499 }

500

501 // ----------------------------------------------------------------------------------------

502 template <typename TReal>

503 inline void aiMatrix4x4t<TReal>::DecomposeNoScaling (aiQuaterniont<TReal>& rotation,

504 aiVector3t<TReal>& position) const

505 {

506 const aiMatrix4x4t<TReal>& _this = *this;

507

508 // extract translation

509 position.x = _this[0][3];

510 position.y = _this[1][3];

511 position.z = _this[2][3];

512

513 // extract rotation

514 rotation = aiQuaterniont<TReal>((aiMatrix3x3t<TReal>)_this);

515 }

516

517 // ----------------------------------------------------------------------------------------

518 template <typename TReal>

519 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(const aiVector3t<TReal>& blubb)

520 {

521 return FromEulerAnglesXYZ(blubb.x,blubb.y,blubb.z);

522 }

523

524 // ----------------------------------------------------------------------------------------

525 template <typename TReal>

526 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(TReal x, TReal y, TReal z)

527 {

528 aiMatrix4x4t<TReal>& _this = *this;

529

530 TReal cx = std::cos(x);

531 TReal sx = std::sin(x);

532 TReal cy = std::cos(y);

533 TReal sy = std::sin(y);

534 TReal cz = std::cos(z);

535 TReal sz = std::sin(z);

536

537 // mz*my*mx

538 _this.a1 = cz * cy;

539 _this.a2 = cz * sy * sx - sz * cx;

540 _this.a3 = sz * sx + cz * sy * cx;

541

542 _this.b1 = sz * cy;

543 _this.b2 = cz * cx + sz * sy * sx;

544 _this.b3 = sz * sy * cx - cz * sx;

545

546 _this.c1 = -sy;

547 _this.c2 = cy * sx;

548 _this.c3 = cy * cx;

549

550 return *this;

551 }

552

553 // ----------------------------------------------------------------------------------------

554 template <typename TReal>

555 inline bool aiMatrix4x4t<TReal>::IsIdentity() const

556 {

557 // Use a small epsilon to solve floating-point inaccuracies

558 const static TReal epsilon = 10e-3f;

559

560 return (a2 <= epsilon && a2 >= -epsilon &&

561 a3 <= epsilon && a3 >= -epsilon &&

562 a4 <= epsilon && a4 >= -epsilon &&

563 b1 <= epsilon && b1 >= -epsilon &&

564 b3 <= epsilon && b3 >= -epsilon &&

565 b4 <= epsilon && b4 >= -epsilon &&

566 c1 <= epsilon && c1 >= -epsilon &&

567 c2 <= epsilon && c2 >= -epsilon &&

568 c4 <= epsilon && c4 >= -epsilon &&

569 d1 <= epsilon && d1 >= -epsilon &&

570 d2 <= epsilon && d2 >= -epsilon &&

571 d3 <= epsilon && d3 >= -epsilon &&

572 a1 <= 1.f+epsilon && a1 >= 1.f-epsilon &&

573 b2 <= 1.f+epsilon && b2 >= 1.f-epsilon &&

574 c3 <= 1.f+epsilon && c3 >= 1.f-epsilon &&

575 d4 <= 1.f+epsilon && d4 >= 1.f-epsilon);

576 }

577

578 // ----------------------------------------------------------------------------------------

579 template <typename TReal>

580 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationX(TReal a, aiMatrix4x4t<TReal>& out)

581 {

582 /*

583 | 1 0 0 0 |

584 M = | 0 cos(A) -sin(A) 0 |

585 | 0 sin(A) cos(A) 0 |

586 | 0 0 0 1 | */

587 out = aiMatrix4x4t<TReal>();

588 out.b2 = out.c3 = std::cos(a);

589 out.b3 = -(out.c2 = std::sin(a));

590 return out;

591 }

592

593 // ----------------------------------------------------------------------------------------

594 template <typename TReal>

595 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationY(TReal a, aiMatrix4x4t<TReal>& out)

596 {

597 /*

598 | cos(A) 0 sin(A) 0 |

599 M = | 0 1 0 0 |

600 | -sin(A) 0 cos(A) 0 |

601 | 0 0 0 1 |

602 */

603 out = aiMatrix4x4t<TReal>();

604 out.a1 = out.c3 = std::cos(a);

605 out.c1 = -(out.a3 = std::sin(a));

606 return out;

607 }

608

609 // ----------------------------------------------------------------------------------------

610 template <typename TReal>

611 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationZ(TReal a, aiMatrix4x4t<TReal>& out)

612 {

613 /*

614 | cos(A) -sin(A) 0 0 |

615 M = | sin(A) cos(A) 0 0 |

616 | 0 0 1 0 |

617 | 0 0 0 1 | */

618 out = aiMatrix4x4t<TReal>();

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

620 out.a2 = -(out.b1 = std::sin(a));

621 return out;

622 }

623

624 // ----------------------------------------------------------------------------------------

625 // Returns a rotation matrix for a rotation around an arbitrary axis.

626 template <typename TReal>

627 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix4x4t<TReal>& out)

628 {

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

630 TReal x = axis.x, y = axis.y, z = axis.z;

631

632 // Many thanks to MathWorld and Wikipedia

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

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

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

636 out.a4 = out.b4 = out.c4 = static_cast<TReal>(0.0);

637 out.d1 = out.d2 = out.d3 = static_cast<TReal>(0.0);

638 out.d4 = static_cast<TReal>(1.0);

639

640 return out;

641 }

642

643 // ----------------------------------------------------------------------------------------

644 template <typename TReal>

645 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Translation( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out)

646 {

647 out = aiMatrix4x4t<TReal>();

648 out.a4 = v.x;

649 out.b4 = v.y;

650 out.c4 = v.z;

651 return out;

652 }

653

654 // ----------------------------------------------------------------------------------------

655 template <typename TReal>

656 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Scaling( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out)

657 {

658 out = aiMatrix4x4t<TReal>();

659 out.a1 = v.x;

660 out.b2 = v.y;

661 out.c3 = v.z;

662 return out;

663 }

664

665 // ----------------------------------------------------------------------------------------

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

667 * "from" into another vector called "to".

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

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

670 * Authors: Tomas Möller, John Hughes

671 * "Efficiently Building a Matrix to Rotate One Vector to Another"

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

673 */

674 // ----------------------------------------------------------------------------------------

675 template <typename TReal>

676 inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromToMatrix(const aiVector3t<TReal>& from,

677 const aiVector3t<TReal>& to, aiMatrix4x4t<TReal>& mtx)

678 {

679 aiMatrix3x3t<TReal> m3;

680 aiMatrix3x3t<TReal>::FromToMatrix(from,to,m3);

681 mtx = aiMatrix4x4t<TReal>(m3);

682 return mtx;

683 }

684

685 #endif // __cplusplus

686 #endif // AI_MATRIX4X4_INL_INC