PageRenderTime 52ms CodeModel.GetById 20ms RepoModel.GetById 0ms app.codeStats 0ms

/indra/llmath/llquaternion.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 955 lines | 633 code | 150 blank | 172 comment | 40 complexity | 523f68e86d7a99745b9ba816cd8467bb MD5 | raw file
Possible License(s): LGPL-2.1
  1. /**
  2. * @file llquaternion.cpp
  3. * @brief LLQuaternion 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 "llmath.h" // for F_PI
  28. #include "llquaternion.h"
  29. //#include "vmath.h"
  30. #include "v3math.h"
  31. #include "v3dmath.h"
  32. #include "v4math.h"
  33. #include "m4math.h"
  34. #include "m3math.h"
  35. #include "llquantize.h"
  36. // WARNING: Don't use this for global const definitions! using this
  37. // at the top of a *.cpp file might not give you what you think.
  38. const LLQuaternion LLQuaternion::DEFAULT;
  39. // Constructors
  40. LLQuaternion::LLQuaternion(const LLMatrix4 &mat)
  41. {
  42. *this = mat.quaternion();
  43. normalize();
  44. }
  45. LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
  46. {
  47. *this = mat.quaternion();
  48. normalize();
  49. }
  50. LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
  51. {
  52. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  53. v.normalize();
  54. F32 c, s;
  55. c = cosf(angle*0.5f);
  56. s = sinf(angle*0.5f);
  57. mQ[VX] = v.mV[VX] * s;
  58. mQ[VY] = v.mV[VY] * s;
  59. mQ[VZ] = v.mV[VZ] * s;
  60. mQ[VW] = c;
  61. normalize();
  62. }
  63. LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
  64. {
  65. LLVector3 v(vec);
  66. v.normalize();
  67. F32 c, s;
  68. c = cosf(angle*0.5f);
  69. s = sinf(angle*0.5f);
  70. mQ[VX] = v.mV[VX] * s;
  71. mQ[VY] = v.mV[VY] * s;
  72. mQ[VZ] = v.mV[VZ] * s;
  73. mQ[VW] = c;
  74. normalize();
  75. }
  76. LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
  77. const LLVector3 &y_axis,
  78. const LLVector3 &z_axis)
  79. {
  80. LLMatrix3 mat;
  81. mat.setRows(x_axis, y_axis, z_axis);
  82. *this = mat.quaternion();
  83. normalize();
  84. }
  85. // Quatizations
  86. void LLQuaternion::quantize16(F32 lower, F32 upper)
  87. {
  88. F32 x = mQ[VX];
  89. F32 y = mQ[VY];
  90. F32 z = mQ[VZ];
  91. F32 s = mQ[VS];
  92. x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper);
  93. y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper);
  94. z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper);
  95. s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper);
  96. mQ[VX] = x;
  97. mQ[VY] = y;
  98. mQ[VZ] = z;
  99. mQ[VS] = s;
  100. normalize();
  101. }
  102. void LLQuaternion::quantize8(F32 lower, F32 upper)
  103. {
  104. mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper);
  105. mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, upper);
  106. mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper);
  107. mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper);
  108. normalize();
  109. }
  110. // LLVector3 Magnitude and Normalization Functions
  111. // Set LLQuaternion routines
  112. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
  113. {
  114. LLVector3 vec(x, y, z);
  115. vec.normalize();
  116. angle *= 0.5f;
  117. F32 c, s;
  118. c = cosf(angle);
  119. s = sinf(angle);
  120. mQ[VX] = vec.mV[VX]*s;
  121. mQ[VY] = vec.mV[VY]*s;
  122. mQ[VZ] = vec.mV[VZ]*s;
  123. mQ[VW] = c;
  124. normalize();
  125. return (*this);
  126. }
  127. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
  128. {
  129. LLVector3 v(vec);
  130. v.normalize();
  131. angle *= 0.5f;
  132. F32 c, s;
  133. c = cosf(angle);
  134. s = sinf(angle);
  135. mQ[VX] = v.mV[VX]*s;
  136. mQ[VY] = v.mV[VY]*s;
  137. mQ[VZ] = v.mV[VZ]*s;
  138. mQ[VW] = c;
  139. normalize();
  140. return (*this);
  141. }
  142. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
  143. {
  144. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  145. v.normalize();
  146. F32 c, s;
  147. c = cosf(angle*0.5f);
  148. s = sinf(angle*0.5f);
  149. mQ[VX] = v.mV[VX]*s;
  150. mQ[VY] = v.mV[VY]*s;
  151. mQ[VZ] = v.mV[VZ]*s;
  152. mQ[VW] = c;
  153. normalize();
  154. return (*this);
  155. }
  156. const LLQuaternion& LLQuaternion::setEulerAngles(F32 roll, F32 pitch, F32 yaw)
  157. {
  158. LLMatrix3 rot_mat(roll, pitch, yaw);
  159. rot_mat.orthogonalize();
  160. *this = rot_mat.quaternion();
  161. normalize();
  162. return (*this);
  163. }
  164. // deprecated
  165. const LLQuaternion& LLQuaternion::set(const LLMatrix3 &mat)
  166. {
  167. *this = mat.quaternion();
  168. normalize();
  169. return (*this);
  170. }
  171. // deprecated
  172. const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat)
  173. {
  174. *this = mat.quaternion();
  175. normalize();
  176. return (*this);
  177. }
  178. // deprecated
  179. const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
  180. {
  181. LLVector3 vec(x, y, z);
  182. vec.normalize();
  183. angle *= 0.5f;
  184. F32 c, s;
  185. c = cosf(angle);
  186. s = sinf(angle);
  187. mQ[VX] = vec.mV[VX]*s;
  188. mQ[VY] = vec.mV[VY]*s;
  189. mQ[VZ] = vec.mV[VZ]*s;
  190. mQ[VW] = c;
  191. normalize();
  192. return (*this);
  193. }
  194. // deprecated
  195. const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
  196. {
  197. LLVector3 v(vec);
  198. v.normalize();
  199. angle *= 0.5f;
  200. F32 c, s;
  201. c = cosf(angle);
  202. s = sinf(angle);
  203. mQ[VX] = v.mV[VX]*s;
  204. mQ[VY] = v.mV[VY]*s;
  205. mQ[VZ] = v.mV[VZ]*s;
  206. mQ[VW] = c;
  207. normalize();
  208. return (*this);
  209. }
  210. const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
  211. {
  212. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  213. v.normalize();
  214. F32 c, s;
  215. c = cosf(angle*0.5f);
  216. s = sinf(angle*0.5f);
  217. mQ[VX] = v.mV[VX]*s;
  218. mQ[VY] = v.mV[VY]*s;
  219. mQ[VZ] = v.mV[VZ]*s;
  220. mQ[VW] = c;
  221. normalize();
  222. return (*this);
  223. }
  224. const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
  225. {
  226. LLMatrix3 rot_mat(roll, pitch, yaw);
  227. rot_mat.orthogonalize();
  228. *this = rot_mat.quaternion();
  229. normalize();
  230. return (*this);
  231. }
  232. const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat)
  233. {
  234. *this = mat.quaternion();
  235. normalize();
  236. return (*this);
  237. }
  238. const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat)
  239. {
  240. *this = mat.quaternion();
  241. normalize();
  242. return (*this);
  243. //#if 1
  244. // // NOTE: LLQuaternion's are actually inverted with respect to
  245. // // the matrices, so this code also assumes inverted quaternions
  246. // // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
  247. // // in reverse order (yaw,pitch,roll).
  248. // F64 cosX = cos(roll);
  249. // F64 cosY = cos(pitch);
  250. // F64 cosZ = cos(yaw);
  251. //
  252. // F64 sinX = sin(roll);
  253. // F64 sinY = sin(pitch);
  254. // F64 sinZ = sin(yaw);
  255. //
  256. // mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5;
  257. // if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO)
  258. // {
  259. // // null rotation, any axis will do
  260. // mQ[VX] = 0.0f;
  261. // mQ[VY] = 1.0f;
  262. // mQ[VZ] = 0.0f;
  263. // }
  264. // else
  265. // {
  266. // F32 inv_s = 1.0f / (4.0f * mQ[VW]);
  267. // mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s;
  268. // mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s;
  269. // mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s;
  270. // }
  271. //
  272. //#else // This only works on a certain subset of roll/pitch/yaw
  273. //
  274. // F64 cosX = cosf(roll/2.0);
  275. // F64 cosY = cosf(pitch/2.0);
  276. // F64 cosZ = cosf(yaw/2.0);
  277. //
  278. // F64 sinX = sinf(roll/2.0);
  279. // F64 sinY = sinf(pitch/2.0);
  280. // F64 sinZ = sinf(yaw/2.0);
  281. //
  282. // mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ);
  283. // mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ);
  284. // mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ);
  285. // mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ);
  286. //#endif
  287. //
  288. // normalize();
  289. // return (*this);
  290. }
  291. // SJB: This code is correct for a logicly stored (non-transposed) matrix;
  292. // Our matrices are stored transposed, OpenGL style, so this generates the
  293. // INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion.
  294. // Because we use similar logic in LLMatrix3::quaternion(),
  295. // we are internally consistant so everything works OK :)
  296. LLMatrix3 LLQuaternion::getMatrix3(void) const
  297. {
  298. LLMatrix3 mat;
  299. F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
  300. xx = mQ[VX] * mQ[VX];
  301. xy = mQ[VX] * mQ[VY];
  302. xz = mQ[VX] * mQ[VZ];
  303. xw = mQ[VX] * mQ[VW];
  304. yy = mQ[VY] * mQ[VY];
  305. yz = mQ[VY] * mQ[VZ];
  306. yw = mQ[VY] * mQ[VW];
  307. zz = mQ[VZ] * mQ[VZ];
  308. zw = mQ[VZ] * mQ[VW];
  309. mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
  310. mat.mMatrix[0][1] = 2.f * ( xy + zw );
  311. mat.mMatrix[0][2] = 2.f * ( xz - yw );
  312. mat.mMatrix[1][0] = 2.f * ( xy - zw );
  313. mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
  314. mat.mMatrix[1][2] = 2.f * ( yz + xw );
  315. mat.mMatrix[2][0] = 2.f * ( xz + yw );
  316. mat.mMatrix[2][1] = 2.f * ( yz - xw );
  317. mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
  318. return mat;
  319. }
  320. LLMatrix4 LLQuaternion::getMatrix4(void) const
  321. {
  322. LLMatrix4 mat;
  323. F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
  324. xx = mQ[VX] * mQ[VX];
  325. xy = mQ[VX] * mQ[VY];
  326. xz = mQ[VX] * mQ[VZ];
  327. xw = mQ[VX] * mQ[VW];
  328. yy = mQ[VY] * mQ[VY];
  329. yz = mQ[VY] * mQ[VZ];
  330. yw = mQ[VY] * mQ[VW];
  331. zz = mQ[VZ] * mQ[VZ];
  332. zw = mQ[VZ] * mQ[VW];
  333. mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
  334. mat.mMatrix[0][1] = 2.f * ( xy + zw );
  335. mat.mMatrix[0][2] = 2.f * ( xz - yw );
  336. mat.mMatrix[1][0] = 2.f * ( xy - zw );
  337. mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
  338. mat.mMatrix[1][2] = 2.f * ( yz + xw );
  339. mat.mMatrix[2][0] = 2.f * ( xz + yw );
  340. mat.mMatrix[2][1] = 2.f * ( yz - xw );
  341. mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
  342. // TODO -- should we set the translation portion to zero?
  343. return mat;
  344. }
  345. // Other useful methods
  346. // calculate the shortest rotation from a to b
  347. void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
  348. {
  349. // Make a local copy of both vectors.
  350. LLVector3 vec_a = a;
  351. LLVector3 vec_b = b;
  352. // Make sure neither vector is zero length. Also normalize
  353. // the vectors while we are at it.
  354. F32 vec_a_mag = vec_a.normalize();
  355. F32 vec_b_mag = vec_b.normalize();
  356. if (vec_a_mag < F_APPROXIMATELY_ZERO ||
  357. vec_b_mag < F_APPROXIMATELY_ZERO)
  358. {
  359. // Can't calculate a rotation from this.
  360. // Just return ZERO_ROTATION instead.
  361. loadIdentity();
  362. return;
  363. }
  364. // Create an axis to rotate around, and the cos of the angle to rotate.
  365. LLVector3 axis = vec_a % vec_b;
  366. F32 cos_theta = vec_a * vec_b;
  367. // Check the angle between the vectors to see if they are parallel or anti-parallel.
  368. if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
  369. {
  370. // a and b are parallel. No rotation is necessary.
  371. loadIdentity();
  372. }
  373. else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
  374. {
  375. // a and b are anti-parallel.
  376. // Rotate 180 degrees around some orthogonal axis.
  377. // Find the projection of the x-axis onto a, and try
  378. // using the vector between the projection and the x-axis
  379. // as the orthogonal axis.
  380. LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
  381. LLVector3 ortho_axis(1.f, 0.f, 0.f);
  382. ortho_axis -= proj;
  383. // Turn this into an orthonormal axis.
  384. F32 ortho_length = ortho_axis.normalize();
  385. // If the axis' length is 0, then our guess at an orthogonal axis
  386. // was wrong (a is parallel to the x-axis).
  387. if (ortho_length < F_APPROXIMATELY_ZERO)
  388. {
  389. // Use the z-axis instead.
  390. ortho_axis.setVec(0.f, 0.f, 1.f);
  391. }
  392. // Construct a quaternion from this orthonormal axis.
  393. mQ[VX] = ortho_axis.mV[VX];
  394. mQ[VY] = ortho_axis.mV[VY];
  395. mQ[VZ] = ortho_axis.mV[VZ];
  396. mQ[VW] = 0.f;
  397. }
  398. else
  399. {
  400. // a and b are NOT parallel or anti-parallel.
  401. // Return the rotation between these vectors.
  402. F32 theta = (F32)acos(cos_theta);
  403. setAngleAxis(theta, axis);
  404. }
  405. }
  406. // constrains rotation to a cone angle specified in radians
  407. const LLQuaternion &LLQuaternion::constrain(F32 radians)
  408. {
  409. const F32 cos_angle_lim = cosf( radians/2 ); // mQ[VW] limit
  410. const F32 sin_angle_lim = sinf( radians/2 ); // rotation axis length limit
  411. if (mQ[VW] < 0.f)
  412. {
  413. mQ[VX] *= -1.f;
  414. mQ[VY] *= -1.f;
  415. mQ[VZ] *= -1.f;
  416. mQ[VW] *= -1.f;
  417. }
  418. // if rotation angle is greater than limit (cos is less than limit)
  419. if( mQ[VW] < cos_angle_lim )
  420. {
  421. mQ[VW] = cos_angle_lim;
  422. F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2)
  423. F32 axis_mult_fact = sin_angle_lim / axis_len;
  424. mQ[VX] *= axis_mult_fact;
  425. mQ[VY] *= axis_mult_fact;
  426. mQ[VZ] *= axis_mult_fact;
  427. }
  428. return *this;
  429. }
  430. // Operators
  431. std::ostream& operator<<(std::ostream &s, const LLQuaternion &a)
  432. {
  433. s << "{ "
  434. << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW]
  435. << " }";
  436. return s;
  437. }
  438. // Does NOT renormalize the result
  439. LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b)
  440. {
  441. // LLQuaternion::mMultCount++;
  442. LLQuaternion q(
  443. b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
  444. b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
  445. b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
  446. b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
  447. );
  448. return q;
  449. }
  450. /*
  451. LLMatrix4 operator*(const LLMatrix4 &m, const LLQuaternion &q)
  452. {
  453. LLMatrix4 qmat(q);
  454. return (m*qmat);
  455. }
  456. */
  457. LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot)
  458. {
  459. F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
  460. F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
  461. F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
  462. F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
  463. F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  464. F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  465. F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  466. return LLVector4(nx, ny, nz, a.mV[VW]);
  467. }
  468. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot)
  469. {
  470. F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
  471. F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
  472. F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
  473. F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
  474. F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  475. F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  476. F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  477. return LLVector3(nx, ny, nz);
  478. }
  479. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot)
  480. {
  481. F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ];
  482. F64 rx = rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY];
  483. F64 ry = rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ];
  484. F64 rz = rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX];
  485. F64 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  486. F64 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  487. F64 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  488. return LLVector3d(nx, ny, nz);
  489. }
  490. F32 dot(const LLQuaternion &a, const LLQuaternion &b)
  491. {
  492. return a.mQ[VX] * b.mQ[VX] +
  493. a.mQ[VY] * b.mQ[VY] +
  494. a.mQ[VZ] * b.mQ[VZ] +
  495. a.mQ[VW] * b.mQ[VW];
  496. }
  497. // DEMO HACK: This lerp is probably inocrrect now due intermediate normalization
  498. // it should look more like the lerp below
  499. #if 0
  500. // linear interpolation
  501. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
  502. {
  503. LLQuaternion r;
  504. r = t * (q - p) + p;
  505. r.normalize();
  506. return r;
  507. }
  508. #endif
  509. // lerp from identity to q
  510. LLQuaternion lerp(F32 t, const LLQuaternion &q)
  511. {
  512. LLQuaternion r;
  513. r.mQ[VX] = t * q.mQ[VX];
  514. r.mQ[VY] = t * q.mQ[VY];
  515. r.mQ[VZ] = t * q.mQ[VZ];
  516. r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f;
  517. r.normalize();
  518. return r;
  519. }
  520. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
  521. {
  522. LLQuaternion r;
  523. F32 inv_t;
  524. inv_t = 1.f - t;
  525. r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]);
  526. r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]);
  527. r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]);
  528. r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]);
  529. r.normalize();
  530. return r;
  531. }
  532. // spherical linear interpolation
  533. LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b )
  534. {
  535. // cosine theta = dot product of a and b
  536. F32 cos_t = a.mQ[0]*b.mQ[0] + a.mQ[1]*b.mQ[1] + a.mQ[2]*b.mQ[2] + a.mQ[3]*b.mQ[3];
  537. // if b is on opposite hemisphere from a, use -a instead
  538. int bflip;
  539. if (cos_t < 0.0f)
  540. {
  541. cos_t = -cos_t;
  542. bflip = TRUE;
  543. }
  544. else
  545. bflip = FALSE;
  546. // if B is (within precision limits) the same as A,
  547. // just linear interpolate between A and B.
  548. F32 alpha; // interpolant
  549. F32 beta; // 1 - interpolant
  550. if (1.0f - cos_t < 0.00001f)
  551. {
  552. beta = 1.0f - u;
  553. alpha = u;
  554. }
  555. else
  556. {
  557. F32 theta = acosf(cos_t);
  558. F32 sin_t = sinf(theta);
  559. beta = sinf(theta - u*theta) / sin_t;
  560. alpha = sinf(u*theta) / sin_t;
  561. }
  562. if (bflip)
  563. beta = -beta;
  564. // interpolate
  565. LLQuaternion ret;
  566. ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0];
  567. ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1];
  568. ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2];
  569. ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3];
  570. return ret;
  571. }
  572. // lerp whenever possible
  573. LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
  574. {
  575. if (dot(a, b) < 0.f)
  576. {
  577. return slerp(t, a, b);
  578. }
  579. else
  580. {
  581. return lerp(t, a, b);
  582. }
  583. }
  584. LLQuaternion nlerp(F32 t, const LLQuaternion &q)
  585. {
  586. if (q.mQ[VW] < 0.f)
  587. {
  588. return slerp(t, q);
  589. }
  590. else
  591. {
  592. return lerp(t, q);
  593. }
  594. }
  595. // slerp from identity quaternion to another quaternion
  596. LLQuaternion slerp(F32 t, const LLQuaternion &q)
  597. {
  598. F32 c = q.mQ[VW];
  599. if (1.0f == t || 1.0f == c)
  600. {
  601. // the trivial cases
  602. return q;
  603. }
  604. LLQuaternion r;
  605. F32 s, angle, stq, stp;
  606. s = (F32) sqrt(1.f - c*c);
  607. if (c < 0.0f)
  608. {
  609. // when c < 0.0 then theta > PI/2
  610. // since quat and -quat are the same rotation we invert one of
  611. // p or q to reduce unecessary spins
  612. // A equivalent way to do it is to convert acos(c) as if it had
  613. // been negative, and to negate stp
  614. angle = (F32) acos(-c);
  615. stp = -(F32) sin(angle * (1.f - t));
  616. stq = (F32) sin(angle * t);
  617. }
  618. else
  619. {
  620. angle = (F32) acos(c);
  621. stp = (F32) sin(angle * (1.f - t));
  622. stq = (F32) sin(angle * t);
  623. }
  624. r.mQ[VX] = (q.mQ[VX] * stq) / s;
  625. r.mQ[VY] = (q.mQ[VY] * stq) / s;
  626. r.mQ[VZ] = (q.mQ[VZ] * stq) / s;
  627. r.mQ[VW] = (stp + q.mQ[VW] * stq) / s;
  628. return r;
  629. }
  630. LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order)
  631. {
  632. LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) );
  633. LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) );
  634. LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) );
  635. LLQuaternion ret;
  636. switch( order )
  637. {
  638. case LLQuaternion::XYZ:
  639. ret = xQ * yQ * zQ;
  640. break;
  641. case LLQuaternion::YZX:
  642. ret = yQ * zQ * xQ;
  643. break;
  644. case LLQuaternion::ZXY:
  645. ret = zQ * xQ * yQ;
  646. break;
  647. case LLQuaternion::XZY:
  648. ret = xQ * zQ * yQ;
  649. break;
  650. case LLQuaternion::YXZ:
  651. ret = yQ * xQ * zQ;
  652. break;
  653. case LLQuaternion::ZYX:
  654. ret = zQ * yQ * xQ;
  655. break;
  656. }
  657. return ret;
  658. }
  659. const char *OrderToString( const LLQuaternion::Order order )
  660. {
  661. const char *p = NULL;
  662. switch( order )
  663. {
  664. default:
  665. case LLQuaternion::XYZ:
  666. p = "XYZ";
  667. break;
  668. case LLQuaternion::YZX:
  669. p = "YZX";
  670. break;
  671. case LLQuaternion::ZXY:
  672. p = "ZXY";
  673. break;
  674. case LLQuaternion::XZY:
  675. p = "XZY";
  676. break;
  677. case LLQuaternion::YXZ:
  678. p = "YXZ";
  679. break;
  680. case LLQuaternion::ZYX:
  681. p = "ZYX";
  682. break;
  683. }
  684. return p;
  685. }
  686. LLQuaternion::Order StringToOrder( const char *str )
  687. {
  688. if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0)
  689. return LLQuaternion::XYZ;
  690. if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0)
  691. return LLQuaternion::YZX;
  692. if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0)
  693. return LLQuaternion::ZXY;
  694. if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0)
  695. return LLQuaternion::XZY;
  696. if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0)
  697. return LLQuaternion::YXZ;
  698. if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0)
  699. return LLQuaternion::ZYX;
  700. return LLQuaternion::XYZ;
  701. }
  702. void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
  703. {
  704. F32 cos_a = mQ[VW];
  705. if (cos_a > 1.0f) cos_a = 1.0f;
  706. if (cos_a < -1.0f) cos_a = -1.0f;
  707. F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
  708. if ( fabs( sin_a ) < 0.0005f )
  709. sin_a = 1.0f;
  710. else
  711. sin_a = 1.f/sin_a;
  712. F32 temp_angle = 2.0f * (F32) acos( cos_a );
  713. if (temp_angle > F_PI)
  714. {
  715. // The (angle,axis) pair should never have angles outside [PI, -PI]
  716. // since we want the _shortest_ (angle,axis) solution.
  717. // Since acos is defined for [0, PI], and we multiply by 2.0, we
  718. // can push the angle outside the acceptible range.
  719. // When this happens we set the angle to the other portion of a
  720. // full 2PI rotation, and negate the axis, which reverses the
  721. // direction of the rotation (by the right-hand rule).
  722. *angle = 2.f * F_PI - temp_angle;
  723. vec.mV[VX] = - mQ[VX] * sin_a;
  724. vec.mV[VY] = - mQ[VY] * sin_a;
  725. vec.mV[VZ] = - mQ[VZ] * sin_a;
  726. }
  727. else
  728. {
  729. *angle = temp_angle;
  730. vec.mV[VX] = mQ[VX] * sin_a;
  731. vec.mV[VY] = mQ[VY] * sin_a;
  732. vec.mV[VZ] = mQ[VZ] * sin_a;
  733. }
  734. }
  735. // quaternion does not need to be normalized
  736. void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
  737. {
  738. LLMatrix3 rot_mat(*this);
  739. rot_mat.orthogonalize();
  740. rot_mat.getEulerAngles(roll, pitch, yaw);
  741. // // NOTE: LLQuaternion's are actually inverted with respect to
  742. // // the matrices, so this code also assumes inverted quaternions
  743. // // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
  744. // // in reverse order (yaw,pitch,roll).
  745. // F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
  746. // F64 m20 = 2.0*(x*z-y*w);
  747. // if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
  748. // {
  749. // *roll = 0.0f;
  750. // *pitch = (F32)asin(m20);
  751. // *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
  752. // }
  753. // else
  754. // {
  755. // *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
  756. // *pitch = (F32)asin(m20);
  757. // *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
  758. // }
  759. }
  760. // Saves space by using the fact that our quaternions are normalized
  761. LLVector3 LLQuaternion::packToVector3() const
  762. {
  763. if( mQ[VW] >= 0 )
  764. {
  765. return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
  766. }
  767. else
  768. {
  769. return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
  770. }
  771. }
  772. // Saves space by using the fact that our quaternions are normalized
  773. void LLQuaternion::unpackFromVector3( const LLVector3& vec )
  774. {
  775. mQ[VX] = vec.mV[VX];
  776. mQ[VY] = vec.mV[VY];
  777. mQ[VZ] = vec.mV[VZ];
  778. F32 t = 1.f - vec.magVecSquared();
  779. if( t > 0 )
  780. {
  781. mQ[VW] = sqrt( t );
  782. }
  783. else
  784. {
  785. // Need this to avoid trying to find the square root of a negative number due
  786. // to floating point error.
  787. mQ[VW] = 0;
  788. }
  789. }
  790. BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value)
  791. {
  792. if( buf.empty() || value == NULL)
  793. {
  794. return FALSE;
  795. }
  796. LLQuaternion quat;
  797. S32 count = sscanf( buf.c_str(), "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 );
  798. if( 4 == count )
  799. {
  800. value->set( quat );
  801. return TRUE;
  802. }
  803. return FALSE;
  804. }
  805. // End