/indra/llmath/m3math.cpp

https://bitbucket.org/lindenlab/viewer-beta/ · C++ · 614 lines · 415 code · 106 blank · 93 comment · 25 complexity · 0359b68362c8a14692a15a3a5a383739 MD5 · raw file

  1. /**
  2. * @file m3math.cpp
  3. * @brief LLMatrix3 class implementation.
  4. *
  5. * $LicenseInfo:firstyear=2000&license=viewerlgpl$
  6. * Second Life Viewer Source Code
  7. * Copyright (C) 2010, Linden Research, Inc.
  8. *
  9. * This library is free software; you can redistribute it and/or
  10. * modify it under the terms of the GNU Lesser General Public
  11. * License as published by the Free Software Foundation;
  12. * version 2.1 of the License only.
  13. *
  14. * This library is distributed in the hope that it will be useful,
  15. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  17. * Lesser General Public License for more details.
  18. *
  19. * You should have received a copy of the GNU Lesser General Public
  20. * License along with this library; if not, write to the Free Software
  21. * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  22. *
  23. * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA
  24. * $/LicenseInfo$
  25. */
  26. #include "linden_common.h"
  27. //#include "vmath.h"
  28. #include "v3math.h"
  29. #include "v3dmath.h"
  30. #include "v4math.h"
  31. #include "m4math.h"
  32. #include "m3math.h"
  33. #include "llquaternion.h"
  34. // LLMatrix3
  35. // ji
  36. // LLMatrix3 = |00 01 02 |
  37. // |10 11 12 |
  38. // |20 21 22 |
  39. // LLMatrix3 = |fx fy fz | forward-axis
  40. // |lx ly lz | left-axis
  41. // |ux uy uz | up-axis
  42. // Constructors
  43. LLMatrix3::LLMatrix3(const LLQuaternion &q)
  44. {
  45. setRot(q);
  46. }
  47. LLMatrix3::LLMatrix3(const F32 angle, const LLVector3 &vec)
  48. {
  49. LLQuaternion quat(angle, vec);
  50. setRot(quat);
  51. }
  52. LLMatrix3::LLMatrix3(const F32 angle, const LLVector3d &vec)
  53. {
  54. LLVector3 vec_f;
  55. vec_f.setVec(vec);
  56. LLQuaternion quat(angle, vec_f);
  57. setRot(quat);
  58. }
  59. LLMatrix3::LLMatrix3(const F32 angle, const LLVector4 &vec)
  60. {
  61. LLQuaternion quat(angle, vec);
  62. setRot(quat);
  63. }
  64. LLMatrix3::LLMatrix3(const F32 angle, const F32 x, const F32 y, const F32 z)
  65. {
  66. LLVector3 vec(x, y, z);
  67. LLQuaternion quat(angle, vec);
  68. setRot(quat);
  69. }
  70. LLMatrix3::LLMatrix3(const F32 roll, const F32 pitch, const F32 yaw)
  71. {
  72. setRot(roll,pitch,yaw);
  73. }
  74. // From Matrix and Quaternion FAQ
  75. void LLMatrix3::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
  76. {
  77. F64 angle_x, angle_y, angle_z;
  78. F64 cx, cy, cz; // cosine of angle_x, angle_y, angle_z
  79. F64 sx, sz; // sine of angle_x, angle_y, angle_z
  80. angle_y = asin(llclamp(mMatrix[2][0], -1.f, 1.f));
  81. cy = cos(angle_y);
  82. if (fabs(cy) > 0.005) // non-zero
  83. {
  84. // no gimbal lock
  85. cx = mMatrix[2][2] / cy;
  86. sx = - mMatrix[2][1] / cy;
  87. angle_x = (F32) atan2(sx, cx);
  88. cz = mMatrix[0][0] / cy;
  89. sz = - mMatrix[1][0] / cy;
  90. angle_z = (F32) atan2(sz, cz);
  91. }
  92. else
  93. {
  94. // yup, gimbal lock
  95. angle_x = 0;
  96. // some tricky math thereby avoided, see article
  97. cz = mMatrix[1][1];
  98. sz = mMatrix[0][1];
  99. angle_z = atan2(sz, cz);
  100. }
  101. *roll = (F32)angle_x;
  102. *pitch = (F32)angle_y;
  103. *yaw = (F32)angle_z;
  104. }
  105. // Clear and Assignment Functions
  106. const LLMatrix3& LLMatrix3::setIdentity()
  107. {
  108. mMatrix[0][0] = 1.f;
  109. mMatrix[0][1] = 0.f;
  110. mMatrix[0][2] = 0.f;
  111. mMatrix[1][0] = 0.f;
  112. mMatrix[1][1] = 1.f;
  113. mMatrix[1][2] = 0.f;
  114. mMatrix[2][0] = 0.f;
  115. mMatrix[2][1] = 0.f;
  116. mMatrix[2][2] = 1.f;
  117. return (*this);
  118. }
  119. const LLMatrix3& LLMatrix3::clear()
  120. {
  121. mMatrix[0][0] = 0.f;
  122. mMatrix[0][1] = 0.f;
  123. mMatrix[0][2] = 0.f;
  124. mMatrix[1][0] = 0.f;
  125. mMatrix[1][1] = 0.f;
  126. mMatrix[1][2] = 0.f;
  127. mMatrix[2][0] = 0.f;
  128. mMatrix[2][1] = 0.f;
  129. mMatrix[2][2] = 0.f;
  130. return (*this);
  131. }
  132. const LLMatrix3& LLMatrix3::setZero()
  133. {
  134. mMatrix[0][0] = 0.f;
  135. mMatrix[0][1] = 0.f;
  136. mMatrix[0][2] = 0.f;
  137. mMatrix[1][0] = 0.f;
  138. mMatrix[1][1] = 0.f;
  139. mMatrix[1][2] = 0.f;
  140. mMatrix[2][0] = 0.f;
  141. mMatrix[2][1] = 0.f;
  142. mMatrix[2][2] = 0.f;
  143. return (*this);
  144. }
  145. // various useful mMatrix functions
  146. const LLMatrix3& LLMatrix3::transpose()
  147. {
  148. // transpose the matrix
  149. F32 temp;
  150. temp = mMatrix[VX][VY]; mMatrix[VX][VY] = mMatrix[VY][VX]; mMatrix[VY][VX] = temp;
  151. temp = mMatrix[VX][VZ]; mMatrix[VX][VZ] = mMatrix[VZ][VX]; mMatrix[VZ][VX] = temp;
  152. temp = mMatrix[VY][VZ]; mMatrix[VY][VZ] = mMatrix[VZ][VY]; mMatrix[VZ][VY] = temp;
  153. return *this;
  154. }
  155. F32 LLMatrix3::determinant() const
  156. {
  157. // Is this a useful method when we assume the matrices are valid rotation
  158. // matrices throughout this implementation?
  159. return mMatrix[0][0] * (mMatrix[1][1] * mMatrix[2][2] - mMatrix[1][2] * mMatrix[2][1]) +
  160. mMatrix[0][1] * (mMatrix[1][2] * mMatrix[2][0] - mMatrix[1][0] * mMatrix[2][2]) +
  161. mMatrix[0][2] * (mMatrix[1][0] * mMatrix[2][1] - mMatrix[1][1] * mMatrix[2][0]);
  162. }
  163. // inverts this matrix
  164. void LLMatrix3::invert()
  165. {
  166. // fails silently if determinant is zero too small
  167. F32 det = determinant();
  168. const F32 VERY_SMALL_DETERMINANT = 0.000001f;
  169. if (fabs(det) > VERY_SMALL_DETERMINANT)
  170. {
  171. // invertiable
  172. LLMatrix3 t(*this);
  173. mMatrix[VX][VX] = ( t.mMatrix[VY][VY] * t.mMatrix[VZ][VZ] - t.mMatrix[VY][VZ] * t.mMatrix[VZ][VY] ) / det;
  174. mMatrix[VY][VX] = ( t.mMatrix[VY][VZ] * t.mMatrix[VZ][VX] - t.mMatrix[VY][VX] * t.mMatrix[VZ][VZ] ) / det;
  175. mMatrix[VZ][VX] = ( t.mMatrix[VY][VX] * t.mMatrix[VZ][VY] - t.mMatrix[VY][VY] * t.mMatrix[VZ][VX] ) / det;
  176. mMatrix[VX][VY] = ( t.mMatrix[VZ][VY] * t.mMatrix[VX][VZ] - t.mMatrix[VZ][VZ] * t.mMatrix[VX][VY] ) / det;
  177. mMatrix[VY][VY] = ( t.mMatrix[VZ][VZ] * t.mMatrix[VX][VX] - t.mMatrix[VZ][VX] * t.mMatrix[VX][VZ] ) / det;
  178. mMatrix[VZ][VY] = ( t.mMatrix[VZ][VX] * t.mMatrix[VX][VY] - t.mMatrix[VZ][VY] * t.mMatrix[VX][VX] ) / det;
  179. mMatrix[VX][VZ] = ( t.mMatrix[VX][VY] * t.mMatrix[VY][VZ] - t.mMatrix[VX][VZ] * t.mMatrix[VY][VY] ) / det;
  180. mMatrix[VY][VZ] = ( t.mMatrix[VX][VZ] * t.mMatrix[VY][VX] - t.mMatrix[VX][VX] * t.mMatrix[VY][VZ] ) / det;
  181. mMatrix[VZ][VZ] = ( t.mMatrix[VX][VX] * t.mMatrix[VY][VY] - t.mMatrix[VX][VY] * t.mMatrix[VY][VX] ) / det;
  182. }
  183. }
  184. // does not assume a rotation matrix, and does not divide by determinant, assuming results will be renormalized
  185. const LLMatrix3& LLMatrix3::adjointTranspose()
  186. {
  187. LLMatrix3 adjoint_transpose;
  188. adjoint_transpose.mMatrix[VX][VX] = mMatrix[VY][VY] * mMatrix[VZ][VZ] - mMatrix[VY][VZ] * mMatrix[VZ][VY] ;
  189. adjoint_transpose.mMatrix[VY][VX] = mMatrix[VY][VZ] * mMatrix[VZ][VX] - mMatrix[VY][VX] * mMatrix[VZ][VZ] ;
  190. adjoint_transpose.mMatrix[VZ][VX] = mMatrix[VY][VX] * mMatrix[VZ][VY] - mMatrix[VY][VY] * mMatrix[VZ][VX] ;
  191. adjoint_transpose.mMatrix[VX][VY] = mMatrix[VZ][VY] * mMatrix[VX][VZ] - mMatrix[VZ][VZ] * mMatrix[VX][VY] ;
  192. adjoint_transpose.mMatrix[VY][VY] = mMatrix[VZ][VZ] * mMatrix[VX][VX] - mMatrix[VZ][VX] * mMatrix[VX][VZ] ;
  193. adjoint_transpose.mMatrix[VZ][VY] = mMatrix[VZ][VX] * mMatrix[VX][VY] - mMatrix[VZ][VY] * mMatrix[VX][VX] ;
  194. adjoint_transpose.mMatrix[VX][VZ] = mMatrix[VX][VY] * mMatrix[VY][VZ] - mMatrix[VX][VZ] * mMatrix[VY][VY] ;
  195. adjoint_transpose.mMatrix[VY][VZ] = mMatrix[VX][VZ] * mMatrix[VY][VX] - mMatrix[VX][VX] * mMatrix[VY][VZ] ;
  196. adjoint_transpose.mMatrix[VZ][VZ] = mMatrix[VX][VX] * mMatrix[VY][VY] - mMatrix[VX][VY] * mMatrix[VY][VX] ;
  197. *this = adjoint_transpose;
  198. return *this;
  199. }
  200. // SJB: This code is correct for a logicly stored (non-transposed) matrix;
  201. // Our matrices are stored transposed, OpenGL style, so this generates the
  202. // INVERSE quaternion (-x, -y, -z, w)!
  203. // Because we use similar logic in LLQuaternion::getMatrix3,
  204. // we are internally consistant so everything works OK :)
  205. LLQuaternion LLMatrix3::quaternion() const
  206. {
  207. LLQuaternion quat;
  208. F32 tr, s, q[4];
  209. U32 i, j, k;
  210. U32 nxt[3] = {1, 2, 0};
  211. tr = mMatrix[0][0] + mMatrix[1][1] + mMatrix[2][2];
  212. // check the diagonal
  213. if (tr > 0.f)
  214. {
  215. s = (F32)sqrt (tr + 1.f);
  216. quat.mQ[VS] = s / 2.f;
  217. s = 0.5f / s;
  218. quat.mQ[VX] = (mMatrix[1][2] - mMatrix[2][1]) * s;
  219. quat.mQ[VY] = (mMatrix[2][0] - mMatrix[0][2]) * s;
  220. quat.mQ[VZ] = (mMatrix[0][1] - mMatrix[1][0]) * s;
  221. }
  222. else
  223. {
  224. // diagonal is negative
  225. i = 0;
  226. if (mMatrix[1][1] > mMatrix[0][0])
  227. i = 1;
  228. if (mMatrix[2][2] > mMatrix[i][i])
  229. i = 2;
  230. j = nxt[i];
  231. k = nxt[j];
  232. s = (F32)sqrt ((mMatrix[i][i] - (mMatrix[j][j] + mMatrix[k][k])) + 1.f);
  233. q[i] = s * 0.5f;
  234. if (s != 0.f)
  235. s = 0.5f / s;
  236. q[3] = (mMatrix[j][k] - mMatrix[k][j]) * s;
  237. q[j] = (mMatrix[i][j] + mMatrix[j][i]) * s;
  238. q[k] = (mMatrix[i][k] + mMatrix[k][i]) * s;
  239. quat.setQuat(q);
  240. }
  241. return quat;
  242. }
  243. // These functions take Rotation arguments
  244. const LLMatrix3& LLMatrix3::setRot(const F32 angle, const F32 x, const F32 y, const F32 z)
  245. {
  246. setRot(LLQuaternion(angle,x,y,z));
  247. return *this;
  248. }
  249. const LLMatrix3& LLMatrix3::setRot(const F32 angle, const LLVector3 &vec)
  250. {
  251. setRot(LLQuaternion(angle, vec));
  252. return *this;
  253. }
  254. const LLMatrix3& LLMatrix3::setRot(const F32 roll, const F32 pitch, const F32 yaw)
  255. {
  256. // Rotates RH about x-axis by 'roll' then
  257. // rotates RH about the old y-axis by 'pitch' then
  258. // rotates RH about the original z-axis by 'yaw'.
  259. // .
  260. // /|\ yaw axis
  261. // | __.
  262. // ._ ___| /| pitch axis
  263. // _||\ \\ |-. /
  264. // \|| \_______\_|__\_/_______
  265. // | _ _ o o o_o_o_o o /_\_ ________\ roll axis
  266. // // /_______/ /__________> /
  267. // /_,-' // /
  268. // /__,-'
  269. F32 cx, sx, cy, sy, cz, sz;
  270. F32 cxsy, sxsy;
  271. cx = (F32)cos(roll); //A
  272. sx = (F32)sin(roll); //B
  273. cy = (F32)cos(pitch); //C
  274. sy = (F32)sin(pitch); //D
  275. cz = (F32)cos(yaw); //E
  276. sz = (F32)sin(yaw); //F
  277. cxsy = cx * sy; //AD
  278. sxsy = sx * sy; //BD
  279. mMatrix[0][0] = cy * cz;
  280. mMatrix[1][0] = -cy * sz;
  281. mMatrix[2][0] = sy;
  282. mMatrix[0][1] = sxsy * cz + cx * sz;
  283. mMatrix[1][1] = -sxsy * sz + cx * cz;
  284. mMatrix[2][1] = -sx * cy;
  285. mMatrix[0][2] = -cxsy * cz + sx * sz;
  286. mMatrix[1][2] = cxsy * sz + sx * cz;
  287. mMatrix[2][2] = cx * cy;
  288. return *this;
  289. }
  290. const LLMatrix3& LLMatrix3::setRot(const LLQuaternion &q)
  291. {
  292. *this = q.getMatrix3();
  293. return *this;
  294. }
  295. const LLMatrix3& LLMatrix3::setRows(const LLVector3 &fwd, const LLVector3 &left, const LLVector3 &up)
  296. {
  297. mMatrix[0][0] = fwd.mV[0];
  298. mMatrix[0][1] = fwd.mV[1];
  299. mMatrix[0][2] = fwd.mV[2];
  300. mMatrix[1][0] = left.mV[0];
  301. mMatrix[1][1] = left.mV[1];
  302. mMatrix[1][2] = left.mV[2];
  303. mMatrix[2][0] = up.mV[0];
  304. mMatrix[2][1] = up.mV[1];
  305. mMatrix[2][2] = up.mV[2];
  306. return *this;
  307. }
  308. const LLMatrix3& LLMatrix3::setRow( U32 rowIndex, const LLVector3& row )
  309. {
  310. llassert( rowIndex >= 0 && rowIndex < NUM_VALUES_IN_MAT3 );
  311. mMatrix[rowIndex][0] = row[0];
  312. mMatrix[rowIndex][1] = row[1];
  313. mMatrix[rowIndex][2] = row[2];
  314. return *this;
  315. }
  316. const LLMatrix3& LLMatrix3::setCol( U32 colIndex, const LLVector3& col )
  317. {
  318. llassert( colIndex >= 0 && colIndex < NUM_VALUES_IN_MAT3 );
  319. mMatrix[0][colIndex] = col[0];
  320. mMatrix[1][colIndex] = col[1];
  321. mMatrix[2][colIndex] = col[2];
  322. return *this;
  323. }
  324. // Rotate exisitng mMatrix
  325. const LLMatrix3& LLMatrix3::rotate(const F32 angle, const F32 x, const F32 y, const F32 z)
  326. {
  327. LLMatrix3 mat(angle, x, y, z);
  328. *this *= mat;
  329. return *this;
  330. }
  331. const LLMatrix3& LLMatrix3::rotate(const F32 angle, const LLVector3 &vec)
  332. {
  333. LLMatrix3 mat(angle, vec);
  334. *this *= mat;
  335. return *this;
  336. }
  337. const LLMatrix3& LLMatrix3::rotate(const F32 roll, const F32 pitch, const F32 yaw)
  338. {
  339. LLMatrix3 mat(roll, pitch, yaw);
  340. *this *= mat;
  341. return *this;
  342. }
  343. const LLMatrix3& LLMatrix3::rotate(const LLQuaternion &q)
  344. {
  345. LLMatrix3 mat(q);
  346. *this *= mat;
  347. return *this;
  348. }
  349. void LLMatrix3::add(const LLMatrix3& other_matrix)
  350. {
  351. for (S32 i = 0; i < 3; ++i)
  352. {
  353. for (S32 j = 0; j < 3; ++j)
  354. {
  355. mMatrix[i][j] += other_matrix.mMatrix[i][j];
  356. }
  357. }
  358. }
  359. LLVector3 LLMatrix3::getFwdRow() const
  360. {
  361. return LLVector3(mMatrix[VX]);
  362. }
  363. LLVector3 LLMatrix3::getLeftRow() const
  364. {
  365. return LLVector3(mMatrix[VY]);
  366. }
  367. LLVector3 LLMatrix3::getUpRow() const
  368. {
  369. return LLVector3(mMatrix[VZ]);
  370. }
  371. const LLMatrix3& LLMatrix3::orthogonalize()
  372. {
  373. LLVector3 x_axis(mMatrix[VX]);
  374. LLVector3 y_axis(mMatrix[VY]);
  375. LLVector3 z_axis(mMatrix[VZ]);
  376. x_axis.normVec();
  377. y_axis -= x_axis * (x_axis * y_axis);
  378. y_axis.normVec();
  379. z_axis = x_axis % y_axis;
  380. setRows(x_axis, y_axis, z_axis);
  381. return (*this);
  382. }
  383. // LLMatrix3 Operators
  384. LLMatrix3 operator*(const LLMatrix3 &a, const LLMatrix3 &b)
  385. {
  386. U32 i, j;
  387. LLMatrix3 mat;
  388. for (i = 0; i < NUM_VALUES_IN_MAT3; i++)
  389. {
  390. for (j = 0; j < NUM_VALUES_IN_MAT3; j++)
  391. {
  392. mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] +
  393. a.mMatrix[j][1] * b.mMatrix[1][i] +
  394. a.mMatrix[j][2] * b.mMatrix[2][i];
  395. }
  396. }
  397. return mat;
  398. }
  399. /* Not implemented to help enforce code consistency with the syntax of
  400. row-major notation. This is a Good Thing.
  401. LLVector3 operator*(const LLMatrix3 &a, const LLVector3 &b)
  402. {
  403. LLVector3 vec;
  404. // matrix operates "from the left" on column vector
  405. vec.mV[VX] = a.mMatrix[VX][VX] * b.mV[VX] +
  406. a.mMatrix[VX][VY] * b.mV[VY] +
  407. a.mMatrix[VX][VZ] * b.mV[VZ];
  408. vec.mV[VY] = a.mMatrix[VY][VX] * b.mV[VX] +
  409. a.mMatrix[VY][VY] * b.mV[VY] +
  410. a.mMatrix[VY][VZ] * b.mV[VZ];
  411. vec.mV[VZ] = a.mMatrix[VZ][VX] * b.mV[VX] +
  412. a.mMatrix[VZ][VY] * b.mV[VY] +
  413. a.mMatrix[VZ][VZ] * b.mV[VZ];
  414. return vec;
  415. }
  416. */
  417. LLVector3 operator*(const LLVector3 &a, const LLMatrix3 &b)
  418. {
  419. // matrix operates "from the right" on row vector
  420. return LLVector3(
  421. a.mV[VX] * b.mMatrix[VX][VX] +
  422. a.mV[VY] * b.mMatrix[VY][VX] +
  423. a.mV[VZ] * b.mMatrix[VZ][VX],
  424. a.mV[VX] * b.mMatrix[VX][VY] +
  425. a.mV[VY] * b.mMatrix[VY][VY] +
  426. a.mV[VZ] * b.mMatrix[VZ][VY],
  427. a.mV[VX] * b.mMatrix[VX][VZ] +
  428. a.mV[VY] * b.mMatrix[VY][VZ] +
  429. a.mV[VZ] * b.mMatrix[VZ][VZ] );
  430. }
  431. LLVector3d operator*(const LLVector3d &a, const LLMatrix3 &b)
  432. {
  433. // matrix operates "from the right" on row vector
  434. return LLVector3d(
  435. a.mdV[VX] * b.mMatrix[VX][VX] +
  436. a.mdV[VY] * b.mMatrix[VY][VX] +
  437. a.mdV[VZ] * b.mMatrix[VZ][VX],
  438. a.mdV[VX] * b.mMatrix[VX][VY] +
  439. a.mdV[VY] * b.mMatrix[VY][VY] +
  440. a.mdV[VZ] * b.mMatrix[VZ][VY],
  441. a.mdV[VX] * b.mMatrix[VX][VZ] +
  442. a.mdV[VY] * b.mMatrix[VY][VZ] +
  443. a.mdV[VZ] * b.mMatrix[VZ][VZ] );
  444. }
  445. bool operator==(const LLMatrix3 &a, const LLMatrix3 &b)
  446. {
  447. U32 i, j;
  448. for (i = 0; i < NUM_VALUES_IN_MAT3; i++)
  449. {
  450. for (j = 0; j < NUM_VALUES_IN_MAT3; j++)
  451. {
  452. if (a.mMatrix[j][i] != b.mMatrix[j][i])
  453. return FALSE;
  454. }
  455. }
  456. return TRUE;
  457. }
  458. bool operator!=(const LLMatrix3 &a, const LLMatrix3 &b)
  459. {
  460. U32 i, j;
  461. for (i = 0; i < NUM_VALUES_IN_MAT3; i++)
  462. {
  463. for (j = 0; j < NUM_VALUES_IN_MAT3; j++)
  464. {
  465. if (a.mMatrix[j][i] != b.mMatrix[j][i])
  466. return TRUE;
  467. }
  468. }
  469. return FALSE;
  470. }
  471. const LLMatrix3& operator*=(LLMatrix3 &a, const LLMatrix3 &b)
  472. {
  473. U32 i, j;
  474. LLMatrix3 mat;
  475. for (i = 0; i < NUM_VALUES_IN_MAT3; i++)
  476. {
  477. for (j = 0; j < NUM_VALUES_IN_MAT3; j++)
  478. {
  479. mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] +
  480. a.mMatrix[j][1] * b.mMatrix[1][i] +
  481. a.mMatrix[j][2] * b.mMatrix[2][i];
  482. }
  483. }
  484. a = mat;
  485. return a;
  486. }
  487. const LLMatrix3& operator*=(LLMatrix3 &a, F32 scalar )
  488. {
  489. for( U32 i = 0; i < NUM_VALUES_IN_MAT3; ++i )
  490. {
  491. for( U32 j = 0; j < NUM_VALUES_IN_MAT3; ++j )
  492. {
  493. a.mMatrix[i][j] *= scalar;
  494. }
  495. }
  496. return a;
  497. }
  498. std::ostream& operator<<(std::ostream& s, const LLMatrix3 &a)
  499. {
  500. s << "{ "
  501. << a.mMatrix[VX][VX] << ", " << a.mMatrix[VX][VY] << ", " << a.mMatrix[VX][VZ] << "; "
  502. << a.mMatrix[VY][VX] << ", " << a.mMatrix[VY][VY] << ", " << a.mMatrix[VY][VZ] << "; "
  503. << a.mMatrix[VZ][VX] << ", " << a.mMatrix[VZ][VY] << ", " << a.mMatrix[VZ][VZ]
  504. << " }";
  505. return s;
  506. }