PageRenderTime 41ms CodeModel.GetById 16ms RepoModel.GetById 0ms app.codeStats 0ms

/indra/llmath/llcoordframe.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 778 lines | 555 code | 136 blank | 87 comment | 32 complexity | ae4a122f1d56c57d13b7dfa76250c06b MD5 | raw file
Possible License(s): LGPL-2.1
  1. /**
  2. * @file llcoordframe.cpp
  3. * @brief LLCoordFrame 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 "m3math.h"
  30. #include "v4math.h"
  31. #include "m4math.h"
  32. #include "llquaternion.h"
  33. #include "llcoordframe.h"
  34. #ifndef X_AXIS
  35. #define X_AXIS 1.0f,0.0f,0.0f
  36. #define Y_AXIS 0.0f,1.0f,0.0f
  37. #define Z_AXIS 0.0f,0.0f,1.0f
  38. #endif
  39. // Constructors
  40. LLCoordFrame::LLCoordFrame() :
  41. mOrigin(0.f, 0.f, 0.f),
  42. mXAxis(X_AXIS),
  43. mYAxis(Y_AXIS),
  44. mZAxis(Z_AXIS)
  45. {
  46. }
  47. LLCoordFrame::LLCoordFrame(const LLVector3 &origin) :
  48. mOrigin(origin),
  49. mXAxis(X_AXIS),
  50. mYAxis(Y_AXIS),
  51. mZAxis(Z_AXIS)
  52. {
  53. if( !mOrigin.isFinite() )
  54. {
  55. reset();
  56. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  57. }
  58. }
  59. LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLVector3 &direction) :
  60. mOrigin(origin)
  61. {
  62. lookDir(direction);
  63. if( !isFinite() )
  64. {
  65. reset();
  66. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  67. }
  68. }
  69. LLCoordFrame::LLCoordFrame(const LLVector3 &x_axis,
  70. const LLVector3 &y_axis,
  71. const LLVector3 &z_axis) :
  72. mOrigin(0.f, 0.f, 0.f),
  73. mXAxis(x_axis),
  74. mYAxis(y_axis),
  75. mZAxis(z_axis)
  76. {
  77. if( !isFinite() )
  78. {
  79. reset();
  80. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  81. }
  82. }
  83. LLCoordFrame::LLCoordFrame(const LLVector3 &origin,
  84. const LLVector3 &x_axis,
  85. const LLVector3 &y_axis,
  86. const LLVector3 &z_axis) :
  87. mOrigin(origin),
  88. mXAxis(x_axis),
  89. mYAxis(y_axis),
  90. mZAxis(z_axis)
  91. {
  92. if( !isFinite() )
  93. {
  94. reset();
  95. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  96. }
  97. }
  98. LLCoordFrame::LLCoordFrame(const LLVector3 &origin,
  99. const LLMatrix3 &rotation) :
  100. mOrigin(origin),
  101. mXAxis(rotation.mMatrix[VX]),
  102. mYAxis(rotation.mMatrix[VY]),
  103. mZAxis(rotation.mMatrix[VZ])
  104. {
  105. if( !isFinite() )
  106. {
  107. reset();
  108. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  109. }
  110. }
  111. LLCoordFrame::LLCoordFrame(const LLQuaternion &q) :
  112. mOrigin(0.f, 0.f, 0.f)
  113. {
  114. LLMatrix3 rotation_matrix(q);
  115. mXAxis.setVec(rotation_matrix.mMatrix[VX]);
  116. mYAxis.setVec(rotation_matrix.mMatrix[VY]);
  117. mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
  118. if( !isFinite() )
  119. {
  120. reset();
  121. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  122. }
  123. }
  124. LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLQuaternion &q) :
  125. mOrigin(origin)
  126. {
  127. LLMatrix3 rotation_matrix(q);
  128. mXAxis.setVec(rotation_matrix.mMatrix[VX]);
  129. mYAxis.setVec(rotation_matrix.mMatrix[VY]);
  130. mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
  131. if( !isFinite() )
  132. {
  133. reset();
  134. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  135. }
  136. }
  137. LLCoordFrame::LLCoordFrame(const LLMatrix4 &mat) :
  138. mOrigin(mat.mMatrix[VW]),
  139. mXAxis(mat.mMatrix[VX]),
  140. mYAxis(mat.mMatrix[VY]),
  141. mZAxis(mat.mMatrix[VZ])
  142. {
  143. if( !isFinite() )
  144. {
  145. reset();
  146. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  147. }
  148. }
  149. // The folowing two constructors are dangerous due to implicit casting and have been disabled - SJB
  150. /*
  151. LLCoordFrame::LLCoordFrame(const F32 *origin, const F32 *rotation) :
  152. mOrigin(origin),
  153. mXAxis(rotation+3*VX),
  154. mYAxis(rotation+3*VY),
  155. mZAxis(rotation+3*VZ)
  156. {
  157. if( !isFinite() )
  158. {
  159. reset();
  160. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  161. }
  162. }
  163. */
  164. /*
  165. LLCoordFrame::LLCoordFrame(const F32 *origin_and_rotation) :
  166. mOrigin(origin_and_rotation),
  167. mXAxis(origin_and_rotation + 3*(VX+1)),
  168. mYAxis(origin_and_rotation + 3*(VY+1)),
  169. mZAxis(origin_and_rotation + 3*(VZ+1))
  170. {
  171. if( !isFinite() )
  172. {
  173. reset();
  174. llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
  175. }
  176. }
  177. */
  178. void LLCoordFrame::reset()
  179. {
  180. mOrigin.setVec(0.0f, 0.0f, 0.0f);
  181. resetAxes();
  182. }
  183. void LLCoordFrame::resetAxes()
  184. {
  185. mXAxis.setVec(1.0f, 0.0f, 0.0f);
  186. mYAxis.setVec(0.0f, 1.0f, 0.0f);
  187. mZAxis.setVec(0.0f, 0.0f, 1.0f);
  188. }
  189. // setOrigin() member functions set mOrigin
  190. void LLCoordFrame::setOrigin(F32 x, F32 y, F32 z)
  191. {
  192. mOrigin.setVec(x, y, z);
  193. if( !mOrigin.isFinite() )
  194. {
  195. reset();
  196. llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
  197. }
  198. }
  199. void LLCoordFrame::setOrigin(const LLVector3 &new_origin)
  200. {
  201. mOrigin = new_origin;
  202. if( !mOrigin.isFinite() )
  203. {
  204. reset();
  205. llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
  206. }
  207. }
  208. void LLCoordFrame::setOrigin(const F32 *origin)
  209. {
  210. mOrigin.mV[VX] = *(origin + VX);
  211. mOrigin.mV[VY] = *(origin + VY);
  212. mOrigin.mV[VZ] = *(origin + VZ);
  213. if( !mOrigin.isFinite() )
  214. {
  215. reset();
  216. llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
  217. }
  218. }
  219. void LLCoordFrame::setOrigin(const LLCoordFrame &frame)
  220. {
  221. mOrigin = frame.getOrigin();
  222. if( !mOrigin.isFinite() )
  223. {
  224. reset();
  225. llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
  226. }
  227. }
  228. // setAxes() member functions set the axes, and assume that
  229. // the arguments are orthogonal and normalized.
  230. void LLCoordFrame::setAxes(const LLVector3 &x_axis,
  231. const LLVector3 &y_axis,
  232. const LLVector3 &z_axis)
  233. {
  234. mXAxis = x_axis;
  235. mYAxis = y_axis;
  236. mZAxis = z_axis;
  237. if( !isFinite() )
  238. {
  239. reset();
  240. llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
  241. }
  242. }
  243. void LLCoordFrame::setAxes(const LLMatrix3 &rotation_matrix)
  244. {
  245. mXAxis.setVec(rotation_matrix.mMatrix[VX]);
  246. mYAxis.setVec(rotation_matrix.mMatrix[VY]);
  247. mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
  248. if( !isFinite() )
  249. {
  250. reset();
  251. llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
  252. }
  253. }
  254. void LLCoordFrame::setAxes(const LLQuaternion &q )
  255. {
  256. LLMatrix3 rotation_matrix(q);
  257. setAxes(rotation_matrix);
  258. if( !isFinite() )
  259. {
  260. reset();
  261. llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
  262. }
  263. }
  264. void LLCoordFrame::setAxes( const F32 *rotation_matrix )
  265. {
  266. mXAxis.mV[VX] = *(rotation_matrix + 3*VX + VX);
  267. mXAxis.mV[VY] = *(rotation_matrix + 3*VX + VY);
  268. mXAxis.mV[VZ] = *(rotation_matrix + 3*VX + VZ);
  269. mYAxis.mV[VX] = *(rotation_matrix + 3*VY + VX);
  270. mYAxis.mV[VY] = *(rotation_matrix + 3*VY + VY);
  271. mYAxis.mV[VZ] = *(rotation_matrix + 3*VY + VZ);
  272. mZAxis.mV[VX] = *(rotation_matrix + 3*VZ + VX);
  273. mZAxis.mV[VY] = *(rotation_matrix + 3*VZ + VY);
  274. mZAxis.mV[VZ] = *(rotation_matrix + 3*VZ + VZ);
  275. if( !isFinite() )
  276. {
  277. reset();
  278. llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
  279. }
  280. }
  281. void LLCoordFrame::setAxes(const LLCoordFrame &frame)
  282. {
  283. mXAxis = frame.getXAxis();
  284. mYAxis = frame.getYAxis();
  285. mZAxis = frame.getZAxis();
  286. if( !isFinite() )
  287. {
  288. reset();
  289. llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
  290. }
  291. }
  292. // translate() member functions move mOrigin to a relative position
  293. void LLCoordFrame::translate(F32 x, F32 y, F32 z)
  294. {
  295. mOrigin.mV[VX] += x;
  296. mOrigin.mV[VY] += y;
  297. mOrigin.mV[VZ] += z;
  298. if( !mOrigin.isFinite() )
  299. {
  300. reset();
  301. llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
  302. }
  303. }
  304. void LLCoordFrame::translate(const LLVector3 &v)
  305. {
  306. mOrigin += v;
  307. if( !mOrigin.isFinite() )
  308. {
  309. reset();
  310. llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
  311. }
  312. }
  313. void LLCoordFrame::translate(const F32 *origin)
  314. {
  315. mOrigin.mV[VX] += *(origin + VX);
  316. mOrigin.mV[VY] += *(origin + VY);
  317. mOrigin.mV[VZ] += *(origin + VZ);
  318. if( !mOrigin.isFinite() )
  319. {
  320. reset();
  321. llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
  322. }
  323. }
  324. // Rotate move the axes to a relative rotation
  325. void LLCoordFrame::rotate(F32 angle, F32 x, F32 y, F32 z)
  326. {
  327. LLQuaternion q(angle, LLVector3(x,y,z));
  328. rotate(q);
  329. }
  330. void LLCoordFrame::rotate(F32 angle, const LLVector3 &rotation_axis)
  331. {
  332. LLQuaternion q(angle, rotation_axis);
  333. rotate(q);
  334. }
  335. void LLCoordFrame::rotate(const LLQuaternion &q)
  336. {
  337. LLMatrix3 rotation_matrix(q);
  338. rotate(rotation_matrix);
  339. }
  340. void LLCoordFrame::rotate(const LLMatrix3 &rotation_matrix)
  341. {
  342. mXAxis.rotVec(rotation_matrix);
  343. mYAxis.rotVec(rotation_matrix);
  344. orthonormalize();
  345. if( !isFinite() )
  346. {
  347. reset();
  348. llwarns << "Non Finite in LLCoordFrame::rotate()" << llendl;
  349. }
  350. }
  351. void LLCoordFrame::roll(F32 angle)
  352. {
  353. LLQuaternion q(angle, mXAxis);
  354. LLMatrix3 rotation_matrix(q);
  355. rotate(rotation_matrix);
  356. if( !mYAxis.isFinite() || !mZAxis.isFinite() )
  357. {
  358. reset();
  359. llwarns << "Non Finite in LLCoordFrame::roll()" << llendl;
  360. }
  361. }
  362. void LLCoordFrame::pitch(F32 angle)
  363. {
  364. LLQuaternion q(angle, mYAxis);
  365. LLMatrix3 rotation_matrix(q);
  366. rotate(rotation_matrix);
  367. if( !mXAxis.isFinite() || !mZAxis.isFinite() )
  368. {
  369. reset();
  370. llwarns << "Non Finite in LLCoordFrame::pitch()" << llendl;
  371. }
  372. }
  373. void LLCoordFrame::yaw(F32 angle)
  374. {
  375. LLQuaternion q(angle, mZAxis);
  376. LLMatrix3 rotation_matrix(q);
  377. rotate(rotation_matrix);
  378. if( !mXAxis.isFinite() || !mYAxis.isFinite() )
  379. {
  380. reset();
  381. llwarns << "Non Finite in LLCoordFrame::yaw()" << llendl;
  382. }
  383. }
  384. // get*() routines
  385. LLQuaternion LLCoordFrame::getQuaternion() const
  386. {
  387. LLQuaternion quat(mXAxis, mYAxis, mZAxis);
  388. return quat;
  389. }
  390. void LLCoordFrame::getMatrixToLocal(LLMatrix4& mat) const
  391. {
  392. mat.setFwdCol(mXAxis);
  393. mat.setLeftCol(mYAxis);
  394. mat.setUpCol(mZAxis);
  395. mat.mMatrix[3][0] = -(mOrigin * LLVector3(mat.mMatrix[0][0], mat.mMatrix[1][0], mat.mMatrix[2][0]));
  396. mat.mMatrix[3][1] = -(mOrigin * LLVector3(mat.mMatrix[0][1], mat.mMatrix[1][1], mat.mMatrix[2][1]));
  397. mat.mMatrix[3][2] = -(mOrigin * LLVector3(mat.mMatrix[0][2], mat.mMatrix[1][2], mat.mMatrix[2][2]));
  398. }
  399. void LLCoordFrame::getRotMatrixToParent(LLMatrix4& mat) const
  400. {
  401. // Note: moves into CFR
  402. mat.setFwdRow( -mYAxis );
  403. mat.setLeftRow( mZAxis );
  404. mat.setUpRow( -mXAxis );
  405. }
  406. size_t LLCoordFrame::writeOrientation(char *buffer) const
  407. {
  408. memcpy(buffer, mOrigin.mV, 3*sizeof(F32)); /*Flawfinder: ignore */
  409. buffer += 3*sizeof(F32);
  410. memcpy(buffer, mXAxis.mV, 3*sizeof(F32)); /*Flawfinder: ignore */
  411. buffer += 3*sizeof(F32);
  412. memcpy(buffer, mYAxis.mV, 3*sizeof(F32));/*Flawfinder: ignore */
  413. buffer += 3*sizeof(F32);
  414. memcpy(buffer, mZAxis.mV, 3*sizeof(F32)); /*Flawfinder: ignore */
  415. return 12*sizeof(F32);
  416. }
  417. size_t LLCoordFrame::readOrientation(const char *buffer)
  418. {
  419. memcpy(mOrigin.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
  420. buffer += 3*sizeof(F32);
  421. memcpy(mXAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
  422. buffer += 3*sizeof(F32);
  423. memcpy(mYAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
  424. buffer += 3*sizeof(F32);
  425. memcpy(mZAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
  426. if( !isFinite() )
  427. {
  428. reset();
  429. llwarns << "Non Finite in LLCoordFrame::readOrientation()" << llendl;
  430. }
  431. return 12*sizeof(F32);
  432. }
  433. // rotation and transform vectors between reference frames
  434. LLVector3 LLCoordFrame::rotateToLocal(const LLVector3 &absolute_vector) const
  435. {
  436. LLVector3 local_vector(mXAxis * absolute_vector,
  437. mYAxis * absolute_vector,
  438. mZAxis * absolute_vector);
  439. return local_vector;
  440. }
  441. LLVector4 LLCoordFrame::rotateToLocal(const LLVector4 &absolute_vector) const
  442. {
  443. LLVector4 local_vector;
  444. local_vector.mV[VX] = mXAxis.mV[VX] * absolute_vector.mV[VX] +
  445. mXAxis.mV[VY] * absolute_vector.mV[VY] +
  446. mXAxis.mV[VZ] * absolute_vector.mV[VZ];
  447. local_vector.mV[VY] = mYAxis.mV[VX] * absolute_vector.mV[VX] +
  448. mYAxis.mV[VY] * absolute_vector.mV[VY] +
  449. mYAxis.mV[VZ] * absolute_vector.mV[VZ];
  450. local_vector.mV[VZ] = mZAxis.mV[VX] * absolute_vector.mV[VX] +
  451. mZAxis.mV[VY] * absolute_vector.mV[VY] +
  452. mZAxis.mV[VZ] * absolute_vector.mV[VZ];
  453. local_vector.mV[VW] = absolute_vector.mV[VW];
  454. return local_vector;
  455. }
  456. LLVector3 LLCoordFrame::rotateToAbsolute(const LLVector3 &local_vector) const
  457. {
  458. LLVector3 absolute_vector;
  459. absolute_vector.mV[VX] = mXAxis.mV[VX] * local_vector.mV[VX] +
  460. mYAxis.mV[VX] * local_vector.mV[VY] +
  461. mZAxis.mV[VX] * local_vector.mV[VZ];
  462. absolute_vector.mV[VY] = mXAxis.mV[VY] * local_vector.mV[VX] +
  463. mYAxis.mV[VY] * local_vector.mV[VY] +
  464. mZAxis.mV[VY] * local_vector.mV[VZ];
  465. absolute_vector.mV[VZ] = mXAxis.mV[VZ] * local_vector.mV[VX] +
  466. mYAxis.mV[VZ] * local_vector.mV[VY] +
  467. mZAxis.mV[VZ] * local_vector.mV[VZ];
  468. return absolute_vector;
  469. }
  470. LLVector4 LLCoordFrame::rotateToAbsolute(const LLVector4 &local_vector) const
  471. {
  472. LLVector4 absolute_vector;
  473. absolute_vector.mV[VX] = mXAxis.mV[VX] * local_vector.mV[VX] +
  474. mYAxis.mV[VX] * local_vector.mV[VY] +
  475. mZAxis.mV[VX] * local_vector.mV[VZ];
  476. absolute_vector.mV[VY] = mXAxis.mV[VY] * local_vector.mV[VX] +
  477. mYAxis.mV[VY] * local_vector.mV[VY] +
  478. mZAxis.mV[VY] * local_vector.mV[VZ];
  479. absolute_vector.mV[VZ] = mXAxis.mV[VZ] * local_vector.mV[VX] +
  480. mYAxis.mV[VZ] * local_vector.mV[VY] +
  481. mZAxis.mV[VZ] * local_vector.mV[VZ];
  482. absolute_vector.mV[VW] = local_vector[VW];
  483. return absolute_vector;
  484. }
  485. void LLCoordFrame::orthonormalize()
  486. // Makes sure the axes are orthogonal and normalized.
  487. {
  488. mXAxis.normVec(); // X is renormalized
  489. mYAxis -= mXAxis * (mXAxis * mYAxis); // Y remains in X-Y plane
  490. mYAxis.normVec(); // Y is normalized
  491. mZAxis = mXAxis % mYAxis; // Z = X cross Y
  492. }
  493. LLVector3 LLCoordFrame::transformToLocal(const LLVector3 &absolute_vector) const
  494. {
  495. return rotateToLocal(absolute_vector - mOrigin);
  496. }
  497. LLVector4 LLCoordFrame::transformToLocal(const LLVector4 &absolute_vector) const
  498. {
  499. LLVector4 local_vector(absolute_vector);
  500. local_vector.mV[VX] -= mOrigin.mV[VX];
  501. local_vector.mV[VY] -= mOrigin.mV[VY];
  502. local_vector.mV[VZ] -= mOrigin.mV[VZ];
  503. return rotateToLocal(local_vector);
  504. }
  505. LLVector3 LLCoordFrame::transformToAbsolute(const LLVector3 &local_vector) const
  506. {
  507. return (rotateToAbsolute(local_vector) + mOrigin);
  508. }
  509. LLVector4 LLCoordFrame::transformToAbsolute(const LLVector4 &local_vector) const
  510. {
  511. LLVector4 absolute_vector;
  512. absolute_vector = rotateToAbsolute(local_vector);
  513. absolute_vector.mV[VX] += mOrigin.mV[VX];
  514. absolute_vector.mV[VY] += mOrigin.mV[VY];
  515. absolute_vector.mV[VZ] += mOrigin.mV[VZ];
  516. return absolute_vector;
  517. }
  518. // This is how you combine a translation and rotation of a
  519. // coordinate frame to get an OpenGL transformation matrix:
  520. //
  521. // translation * rotation = transformation matrix
  522. //
  523. // (i)->
  524. // (j)| 1 0 0 0 | | a d g 0 | | a d g 0 |
  525. // | | 0 1 0 0 | * | b e h 0 | = | b e h 0 |
  526. // V | 0 0 1 0 | | c f i 0 | | c f i 0 |
  527. // |-x -y -z 1 | | 0 0 0 1 | |-(ax+by+cz) -(dx+ey+fz) -(gx+hy+iz) 1 |
  528. //
  529. // where {a,b,c} = x-axis
  530. // {d,e,f} = y-axis
  531. // {g,h,i} = z-axis
  532. // {x,y,z} = origin
  533. void LLCoordFrame::getOpenGLTranslation(F32 *ogl_matrix) const
  534. {
  535. *(ogl_matrix + 0) = 1.0f;
  536. *(ogl_matrix + 1) = 0.0f;
  537. *(ogl_matrix + 2) = 0.0f;
  538. *(ogl_matrix + 3) = 0.0f;
  539. *(ogl_matrix + 4) = 0.0f;
  540. *(ogl_matrix + 5) = 1.0f;
  541. *(ogl_matrix + 6) = 0.0f;
  542. *(ogl_matrix + 7) = 0.0f;
  543. *(ogl_matrix + 8) = 0.0f;
  544. *(ogl_matrix + 9) = 0.0f;
  545. *(ogl_matrix + 10) = 1.0f;
  546. *(ogl_matrix + 11) = 0.0f;
  547. *(ogl_matrix + 12) = -mOrigin.mV[VX];
  548. *(ogl_matrix + 13) = -mOrigin.mV[VY];
  549. *(ogl_matrix + 14) = -mOrigin.mV[VZ];
  550. *(ogl_matrix + 15) = 1.0f;
  551. }
  552. void LLCoordFrame::getOpenGLRotation(F32 *ogl_matrix) const
  553. {
  554. *(ogl_matrix + 0) = mXAxis.mV[VX];
  555. *(ogl_matrix + 4) = mXAxis.mV[VY];
  556. *(ogl_matrix + 8) = mXAxis.mV[VZ];
  557. *(ogl_matrix + 1) = mYAxis.mV[VX];
  558. *(ogl_matrix + 5) = mYAxis.mV[VY];
  559. *(ogl_matrix + 9) = mYAxis.mV[VZ];
  560. *(ogl_matrix + 2) = mZAxis.mV[VX];
  561. *(ogl_matrix + 6) = mZAxis.mV[VY];
  562. *(ogl_matrix + 10) = mZAxis.mV[VZ];
  563. *(ogl_matrix + 3) = 0.0f;
  564. *(ogl_matrix + 7) = 0.0f;
  565. *(ogl_matrix + 11) = 0.0f;
  566. *(ogl_matrix + 12) = 0.0f;
  567. *(ogl_matrix + 13) = 0.0f;
  568. *(ogl_matrix + 14) = 0.0f;
  569. *(ogl_matrix + 15) = 1.0f;
  570. }
  571. void LLCoordFrame::getOpenGLTransform(F32 *ogl_matrix) const
  572. {
  573. *(ogl_matrix + 0) = mXAxis.mV[VX];
  574. *(ogl_matrix + 4) = mXAxis.mV[VY];
  575. *(ogl_matrix + 8) = mXAxis.mV[VZ];
  576. *(ogl_matrix + 12) = -mOrigin * mXAxis;
  577. *(ogl_matrix + 1) = mYAxis.mV[VX];
  578. *(ogl_matrix + 5) = mYAxis.mV[VY];
  579. *(ogl_matrix + 9) = mYAxis.mV[VZ];
  580. *(ogl_matrix + 13) = -mOrigin * mYAxis;
  581. *(ogl_matrix + 2) = mZAxis.mV[VX];
  582. *(ogl_matrix + 6) = mZAxis.mV[VY];
  583. *(ogl_matrix + 10) = mZAxis.mV[VZ];
  584. *(ogl_matrix + 14) = -mOrigin * mZAxis;
  585. *(ogl_matrix + 3) = 0.0f;
  586. *(ogl_matrix + 7) = 0.0f;
  587. *(ogl_matrix + 11) = 0.0f;
  588. *(ogl_matrix + 15) = 1.0f;
  589. }
  590. // at and up_direction are presumed to be normalized
  591. void LLCoordFrame::lookDir(const LLVector3 &at, const LLVector3 &up_direction)
  592. {
  593. // Make sure 'at' and 'up_direction' are not parallel
  594. // and that neither are zero-length vectors
  595. LLVector3 left(up_direction % at);
  596. if (left.isNull())
  597. {
  598. //tweak lookat pos so we don't get a degenerate matrix
  599. LLVector3 tempat(at[VX] + 0.01f, at[VY], at[VZ]);
  600. tempat.normVec();
  601. left = (up_direction % tempat);
  602. }
  603. left.normVec();
  604. LLVector3 up = at % left;
  605. if (at.isFinite() && left.isFinite() && up.isFinite())
  606. {
  607. setAxes(at, left, up);
  608. }
  609. }
  610. void LLCoordFrame::lookDir(const LLVector3 &xuv)
  611. {
  612. static LLVector3 up_direction(0.0f, 0.0f, 1.0f);
  613. lookDir(xuv, up_direction);
  614. }
  615. void LLCoordFrame::lookAt(const LLVector3 &origin, const LLVector3 &point_of_interest, const LLVector3 &up_direction)
  616. {
  617. setOrigin(origin);
  618. LLVector3 at(point_of_interest - origin);
  619. at.normVec();
  620. lookDir(at, up_direction);
  621. }
  622. void LLCoordFrame::lookAt(const LLVector3 &origin, const LLVector3 &point_of_interest)
  623. {
  624. static LLVector3 up_direction(0.0f, 0.0f, 1.0f);
  625. setOrigin(origin);
  626. LLVector3 at(point_of_interest - origin);
  627. at.normVec();
  628. lookDir(at, up_direction);
  629. }
  630. // Operators and friends
  631. std::ostream& operator<<(std::ostream &s, const LLCoordFrame &C)
  632. {
  633. s << "{ "
  634. << " origin = " << C.mOrigin
  635. << " x_axis = " << C.mXAxis
  636. << " y_axis = " << C.mYAxis
  637. << " z_axis = " << C.mZAxis
  638. << " }";
  639. return s;
  640. }
  641. // Private member functions
  642. //EOF