goat3d: a5c5cec3cb88 libs/vmath/matrix.cc

goat3d

view libs/vmath/matrix.cc @ 40:a5c5cec3cb88

- added mesh attribute and face append functions - added Int4 constructor - continued the blender exporter - fixed a bug in clean_filename which made it produce unterminated strings - renamed clean_filename to goat3d_clean_filename and made it extern - added call to goat3d_clean_filename in the mesh XML export code to cleanup ctm filenames

author	John Tsiombikas <nuclear@member.fsf.org>
date	Sun, 13 Oct 2013 10:14:19 +0300
parents
children

line source

1 #include <cstdio>

2 #include <cmath>

3 #include "matrix.h"

4 #include "vector.h"

5 #include "quat.h"

7 using namespace std;

9 // ----------- Matrix3x3 --------------

11 Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);

13 Matrix3x3::Matrix3x3()

15 *this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);

18 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,

19 scalar_t m21, scalar_t m22, scalar_t m23,

20 scalar_t m31, scalar_t m32, scalar_t m33)

22 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;

23 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;

24 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;

27 Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)

29 set_row_vector(ivec, 0);

30 set_row_vector(jvec, 1);

31 set_row_vector(kvec, 2);

34 Matrix3x3::Matrix3x3(const mat3_t cmat)

36 memcpy(m, cmat, sizeof(mat3_t));

39 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)

41 for(int i=0; i<3; i++) {

42 for(int j=0; j<3; j++) {

43 m[i][j] = mat4x4[i][j];

48 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)

50 Matrix3x3 res;

51 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];

52 scalar_t *dest = res.m[0];

54 for(int i=0; i<9; i++) {

55 *dest++ = *op1++ + *op2++;

57 return res;

60 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)

62 Matrix3x3 res;

63 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];

64 scalar_t *dest = res.m[0];

66 for(int i=0; i<9; i++) {

67 *dest++ = *op1++ - *op2++;

69 return res;

72 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)

74 Matrix3x3 res;

75 for(int i=0; i<3; i++) {

76 for(int j=0; j<3; j++) {

77 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];

80 return res;

83 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)

85 scalar_t *op1 = m1.m[0];

86 const scalar_t *op2 = m2.m[0];

88 for(int i=0; i<9; i++) {

89 *op1++ += *op2++;

93 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)

95 scalar_t *op1 = m1.m[0];

96 const scalar_t *op2 = m2.m[0];

98 for(int i=0; i<9; i++) {

99 *op1++ -= *op2++;

103 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)

105 Matrix3x3 res;

106 for(int i=0; i<3; i++) {

107 for(int j=0; j<3; j++) {

108 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];

111 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));

114 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)

116 Matrix3x3 res;

117 const scalar_t *mptr = mat.m[0];

118 scalar_t *dptr = res.m[0];

120 for(int i=0; i<9; i++) {

121 *dptr++ = *mptr++ * scalar;

123 return res;

126 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)

128 Matrix3x3 res;

129 const scalar_t *mptr = mat.m[0];

130 scalar_t *dptr = res.m[0];

132 for(int i=0; i<9; i++) {

133 *dptr++ = *mptr++ * scalar;

135 return res;

138 void operator *=(Matrix3x3 &mat, scalar_t scalar)

140 scalar_t *mptr = mat.m[0];

142 for(int i=0; i<9; i++) {

143 *mptr++ *= scalar;

147 void Matrix3x3::translate(const Vector2 &trans)

149 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);

150 *this *= tmat;

153 void Matrix3x3::set_translation(const Vector2 &trans)

155 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);

158 void Matrix3x3::rotate(scalar_t angle)

160 scalar_t cos_a = cos(angle);

161 scalar_t sin_a = sin(angle);

162 Matrix3x3 rmat( cos_a, -sin_a, 0,

163 sin_a, cos_a, 0,

164 0, 0, 1);

165 *this *= rmat;

168 void Matrix3x3::set_rotation(scalar_t angle)

170 scalar_t cos_a = cos(angle);

171 scalar_t sin_a = sin(angle);

172 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);

175 void Matrix3x3::rotate(const Vector3 &euler_angles)

177 Matrix3x3 xrot, yrot, zrot;

179 xrot = Matrix3x3( 1, 0, 0,

180 0, cos(euler_angles.x), -sin(euler_angles.x),

181 0, sin(euler_angles.x), cos(euler_angles.x));

183 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),

184 0, 1, 0,

185 -sin(euler_angles.y), 0, cos(euler_angles.y));

187 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,

188 sin(euler_angles.z), cos(euler_angles.z), 0,

189 0, 0, 1);

191 *this *= xrot * yrot * zrot;

194 void Matrix3x3::set_rotation(const Vector3 &euler_angles)

196 Matrix3x3 xrot, yrot, zrot;

198 xrot = Matrix3x3( 1, 0, 0,

199 0, cos(euler_angles.x), -sin(euler_angles.x),

200 0, sin(euler_angles.x), cos(euler_angles.x));

202 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),

203 0, 1, 0,

204 -sin(euler_angles.y), 0, cos(euler_angles.y));

206 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,

207 sin(euler_angles.z), cos(euler_angles.z), 0,

208 0, 0, 1);

210 *this = xrot * yrot * zrot;

213 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)

215 scalar_t sina = (scalar_t)sin(angle);

216 scalar_t cosa = (scalar_t)cos(angle);

217 scalar_t invcosa = 1-cosa;

218 scalar_t nxsq = axis.x * axis.x;

219 scalar_t nysq = axis.y * axis.y;

220 scalar_t nzsq = axis.z * axis.z;

222 Matrix3x3 xform;

223 xform.m[0][0] = nxsq + (1-nxsq) * cosa;

224 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;

225 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;

227 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;

228 xform.m[1][1] = nysq + (1-nysq) * cosa;

229 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;

231 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;

232 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;

233 xform.m[2][2] = nzsq + (1-nzsq) * cosa;

235 *this *= xform;

238 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)

240 scalar_t sina = (scalar_t)sin(angle);

241 scalar_t cosa = (scalar_t)cos(angle);

242 scalar_t invcosa = 1-cosa;

243 scalar_t nxsq = axis.x * axis.x;

244 scalar_t nysq = axis.y * axis.y;

245 scalar_t nzsq = axis.z * axis.z;

247 reset_identity();

248 m[0][0] = nxsq + (1-nxsq) * cosa;

249 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;

250 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;

251 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;

252 m[1][1] = nysq + (1-nysq) * cosa;

253 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;

254 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;

255 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;

256 m[2][2] = nzsq + (1-nzsq) * cosa;

259 // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes

260 // article "Quaternion Calculus and Fast Animation".

261 // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl

262 Quaternion Matrix3x3::get_rotation_quat() const

264 static const int next[3] = {1, 2, 0};

266 float quat[4];

268 scalar_t trace = m[0][0] + m[1][1] + m[2][2];

269 scalar_t root;

271 if(trace > 0.0f) {

272 // |w| > 1/2

273 root = sqrt(trace + 1.0f); // 2w

274 quat[0] = 0.5f * root;

275 root = 0.5f / root; // 1 / 4w

276 quat[1] = (m[2][1] - m[1][2]) * root;

277 quat[2] = (m[0][2] - m[2][0]) * root;

278 quat[3] = (m[1][0] - m[0][1]) * root;

279 } else {

280 // |w| <= 1/2

281 int i = 0;

282 if(m[1][1] > m[0][0]) {

283 i = 1;

285 if(m[2][2] > m[i][i]) {

286 i = 2;

288 int j = next[i];

289 int k = next[j];

291 root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);

292 quat[i + 1] = 0.5f * root;

293 root = 0.5f / root;

294 quat[0] = (m[k][j] - m[j][k]) * root;

295 quat[j + 1] = (m[j][i] - m[i][j]) * root;

296 quat[k + 1] = (m[k][i] - m[i][k]) * root;

298 return Quaternion(quat[0], quat[1], quat[2], quat[3]);

301 void Matrix3x3::scale(const Vector3 &scale_vec)

303 Matrix3x3 smat( scale_vec.x, 0, 0,

304 0, scale_vec.y, 0,

305 0, 0, scale_vec.z);

306 *this *= smat;

309 void Matrix3x3::set_scaling(const Vector3 &scale_vec)

311 *this = Matrix3x3( scale_vec.x, 0, 0,

312 0, scale_vec.y, 0,

313 0, 0, scale_vec.z);

316 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)

318 m[0][col_index] = vec.x;

319 m[1][col_index] = vec.y;

320 m[2][col_index] = vec.z;

323 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)

325 m[row_index][0] = vec.x;

326 m[row_index][1] = vec.y;

327 m[row_index][2] = vec.z;

330 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const

332 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);

335 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const

337 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);

340 void Matrix3x3::transpose()

342 Matrix3x3 tmp = *this;

343 for(int i=0; i<3; i++) {

344 for(int j=0; j<3; j++) {

345 m[i][j] = tmp[j][i];

350 Matrix3x3 Matrix3x3::transposed() const

352 Matrix3x3 res;

353 for(int i=0; i<3; i++) {

354 for(int j=0; j<3; j++) {

355 res[i][j] = m[j][i];

358 return res;

361 scalar_t Matrix3x3::determinant() const

363 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -

364 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +

365 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);

368 Matrix3x3 Matrix3x3::inverse() const

370 // TODO: implement 3x3 inverse

371 return *this;

374 ostream &operator <<(ostream &out, const Matrix3x3 &mat)

376 for(int i=0; i<3; i++) {

377 char str[100];

378 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);

379 out << str;

381 return out;

386 /* ----------------- Matrix4x4 implementation --------------- */

388 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

390 Matrix4x4::Matrix4x4()

392 *this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

395 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,

396 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,

397 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,

398 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)

400 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;

401 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;

402 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;

403 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;

406 Matrix4x4::Matrix4x4(const mat4_t cmat)

408 memcpy(m, cmat, sizeof(mat4_t));

411 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)

413 reset_identity();

414 for(int i=0; i<3; i++) {

415 for(int j=0; j<3; j++) {

416 m[i][j] = mat3x3[i][j];

421 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)

423 Matrix4x4 res;

424 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];

425 scalar_t *dest = res.m[0];

427 for(int i=0; i<16; i++) {

428 *dest++ = *op1++ + *op2++;

430 return res;

433 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)

435 Matrix4x4 res;

436 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];

437 scalar_t *dest = res.m[0];

439 for(int i=0; i<16; i++) {

440 *dest++ = *op1++ - *op2++;

442 return res;

445 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)

447 scalar_t *op1 = m1.m[0];

448 const scalar_t *op2 = m2.m[0];

450 for(int i=0; i<16; i++) {

451 *op1++ += *op2++;

455 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)

457 scalar_t *op1 = m1.m[0];

458 const scalar_t *op2 = m2.m[0];

460 for(int i=0; i<16; i++) {

461 *op1++ -= *op2++;

465 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)

467 Matrix4x4 res;

468 const scalar_t *mptr = mat.m[0];

469 scalar_t *dptr = res.m[0];

471 for(int i=0; i<16; i++) {

472 *dptr++ = *mptr++ * scalar;

474 return res;

477 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)

479 Matrix4x4 res;

480 const scalar_t *mptr = mat.m[0];

481 scalar_t *dptr = res.m[0];

483 for(int i=0; i<16; i++) {

484 *dptr++ = *mptr++ * scalar;

486 return res;

489 void operator *=(Matrix4x4 &mat, scalar_t scalar)

491 scalar_t *mptr = mat.m[0];

493 for(int i=0; i<16; i++) {

494 *mptr++ *= scalar;

498 void Matrix4x4::translate(const Vector3 &trans)

500 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);

501 *this *= tmat;

504 void Matrix4x4::set_translation(const Vector3 &trans)

506 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);

509 Vector3 Matrix4x4::get_translation() const

511 return Vector3(m[0][3], m[1][3], m[2][3]);

514 void Matrix4x4::rotate(const Vector3 &euler_angles)

516 Matrix3x3 xrot, yrot, zrot;

518 xrot = Matrix3x3( 1, 0, 0,

519 0, cos(euler_angles.x), -sin(euler_angles.x),

520 0, sin(euler_angles.x), cos(euler_angles.x));

522 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),

523 0, 1, 0,

524 -sin(euler_angles.y), 0, cos(euler_angles.y));

526 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,

527 sin(euler_angles.z), cos(euler_angles.z), 0,

528 0, 0, 1);

530 *this *= Matrix4x4(xrot * yrot * zrot);

533 void Matrix4x4::set_rotation(const Vector3 &euler_angles)

535 Matrix3x3 xrot, yrot, zrot;

537 xrot = Matrix3x3( 1, 0, 0,

538 0, cos(euler_angles.x), -sin(euler_angles.x),

539 0, sin(euler_angles.x), cos(euler_angles.x));

541 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),

542 0, 1, 0,

543 -sin(euler_angles.y), 0, cos(euler_angles.y));

545 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,

546 sin(euler_angles.z), cos(euler_angles.z), 0,

547 0, 0, 1);

549 *this = Matrix4x4(xrot * yrot * zrot);

552 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)

554 scalar_t sina = (scalar_t)sin(angle);

555 scalar_t cosa = (scalar_t)cos(angle);

556 scalar_t invcosa = 1-cosa;

557 scalar_t nxsq = axis.x * axis.x;

558 scalar_t nysq = axis.y * axis.y;

559 scalar_t nzsq = axis.z * axis.z;

561 Matrix4x4 xform;

562 xform[0][0] = nxsq + (1-nxsq) * cosa;

563 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;

564 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;

565 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;

566 xform[1][1] = nysq + (1-nysq) * cosa;

567 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;

568 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;

569 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;

570 xform[2][2] = nzsq + (1-nzsq) * cosa;

572 *this *= xform;

575 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)

577 scalar_t sina = (scalar_t)sin(angle);

578 scalar_t cosa = (scalar_t)cos(angle);

579 scalar_t invcosa = 1-cosa;

580 scalar_t nxsq = axis.x * axis.x;

581 scalar_t nysq = axis.y * axis.y;

582 scalar_t nzsq = axis.z * axis.z;

584 reset_identity();

585 m[0][0] = nxsq + (1-nxsq) * cosa;

586 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;

587 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;

588 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;

589 m[1][1] = nysq + (1-nysq) * cosa;

590 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;

591 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;

592 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;

593 m[2][2] = nzsq + (1-nzsq) * cosa;

596 void Matrix4x4::rotate(const Quaternion &quat)

598 *this *= quat.get_rotation_matrix();

601 void Matrix4x4::set_rotation(const Quaternion &quat)

603 *this = quat.get_rotation_matrix();

606 Quaternion Matrix4x4::get_rotation_quat() const

608 Matrix3x3 mat3 = *this;

609 return mat3.get_rotation_quat();

612 void Matrix4x4::scale(const Vector4 &scale_vec)

614 Matrix4x4 smat( scale_vec.x, 0, 0, 0,

615 0, scale_vec.y, 0, 0,

616 0, 0, scale_vec.z, 0,

617 0, 0, 0, scale_vec.w);

618 *this *= smat;

621 void Matrix4x4::set_scaling(const Vector4 &scale_vec)

623 *this = Matrix4x4( scale_vec.x, 0, 0, 0,

624 0, scale_vec.y, 0, 0,

625 0, 0, scale_vec.z, 0,

626 0, 0, 0, scale_vec.w);

629 Vector3 Matrix4x4::get_scaling() const

631 Vector3 vi = get_row_vector(0);

632 Vector3 vj = get_row_vector(1);

633 Vector3 vk = get_row_vector(2);

635 return Vector3(vi.length(), vj.length(), vk.length());

638 void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)

640 float f = 1.0f / tan(vfov * 0.5f);

641 float dz = znear - zfar;

643 reset_identity();

645 m[0][0] = f / aspect;

646 m[1][1] = f;

647 m[2][2] = (zfar + znear) / dz;

648 m[3][2] = -1.0f;

649 m[2][3] = 2.0f * zfar * znear / dz;

650 m[3][3] = 0.0f;

653 void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)

655 float dx = right - left;

656 float dy = top - bottom;

657 float dz = zfar - znear;

659 reset_identity();

661 m[0][0] = 2.0 / dx;

662 m[1][1] = 2.0 / dy;

663 m[2][2] = -2.0 / dz;

664 m[0][3] = -(right + left) / dx;

665 m[1][3] = -(top + bottom) / dy;

666 m[2][3] = -(zfar + znear) / dz;

669 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)

671 m[0][col_index] = vec.x;

672 m[1][col_index] = vec.y;

673 m[2][col_index] = vec.z;

674 m[3][col_index] = vec.w;

677 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)

679 m[row_index][0] = vec.x;

680 m[row_index][1] = vec.y;

681 m[row_index][2] = vec.z;

682 m[row_index][3] = vec.w;

685 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const

687 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);

690 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const

692 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);

695 void Matrix4x4::transpose()

697 Matrix4x4 tmp = *this;

698 for(int i=0; i<4; i++) {

699 for(int j=0; j<4; j++) {

700 m[i][j] = tmp[j][i];

705 Matrix4x4 Matrix4x4::transposed() const

707 Matrix4x4 res;

708 for(int i=0; i<4; i++) {

709 for(int j=0; j<4; j++) {

710 res[i][j] = m[j][i];

713 return res;

716 scalar_t Matrix4x4::determinant() const

718 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

719 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +

720 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));

722 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

723 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

724 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));

726 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -

727 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

728 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

730 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -

731 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +

732 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

734 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;

738 Matrix4x4 Matrix4x4::adjoint() const

740 Matrix4x4 coef;

742 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

743 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +

744 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));

745 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

746 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

747 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));

748 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -

749 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

750 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

751 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -

752 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +

753 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

755 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

756 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +

757 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));

758 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -

759 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

760 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));

761 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -

762 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +

763 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

764 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -

765 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +

766 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));

768 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -

769 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +

770 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));

771 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -

772 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +

773 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));

774 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -

775 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +

776 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));

777 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -

778 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +

779 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));

781 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -

782 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +

783 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));

784 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -

785 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +

786 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));

787 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -

788 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +

789 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));

790 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -

791 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +

792 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));

794 coef.transpose();

796 for(int i=0; i<4; i++) {

797 for(int j=0; j<4; j++) {

798 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];

799 if(i%2) coef.m[i][j] = -coef.m[i][j];

803 return coef;

806 Matrix4x4 Matrix4x4::inverse() const

808 Matrix4x4 adj = adjoint();

810 return adj * (1.0f / determinant());

813 ostream &operator <<(ostream &out, const Matrix4x4 &mat)

815 for(int i=0; i<4; i++) {

816 char str[100];

817 sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);

818 out << str;

820 return out;