开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 游戏 > 游戏引擎 > 查看源码
llquaternion.cpp
资源名称：snowglobe-src-viewer-2.0.0-r0.tar.gz　[点击查看]
上传用户：king477883
上传日期：2021-03-01
资源大小：9553k
文件大小：23k
源码类别：

游戏引擎

开发平台：

C++ Builder

llquaternion.cpp：源码内容
  1. /** 
  2.  * @file llquaternion.cpp
  3.  * @brief LLQuaternion class implementation.
  4.  *
  5.  * $LicenseInfo:firstyear=2000&license=viewergpl$
  6.  * 
  7.  * Copyright (c) 2000-2010, Linden Research, Inc.
  8.  * 
  9.  * Second Life Viewer Source Code
  10.  * The source code in this file ("Source Code") is provided by Linden Lab
  11.  * to you under the terms of the GNU General Public License, version 2.0
  12.  * ("GPL"), unless you have obtained a separate licensing agreement
  13.  * ("Other License"), formally executed by you and Linden Lab.  Terms of
  14.  * the GPL can be found in doc/GPL-license.txt in this distribution, or
  15.  * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
  16.  * 
  17.  * There are special exceptions to the terms and conditions of the GPL as
  18.  * it is applied to this Source Code. View the full text of the exception
  19.  * in the file doc/FLOSS-exception.txt in this software distribution, or
  20.  * online at
  21.  * http://secondlifegrid.net/programs/open_source/licensing/flossexception
  22.  * 
  23.  * By copying, modifying or distributing this software, you acknowledge
  24.  * that you have read and understood your obligations described above,
  25.  * and agree to abide by those obligations.
  26.  * 
  27.  * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
  28.  * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
  29.  * COMPLETENESS OR PERFORMANCE.
  30.  * $/LicenseInfo$
  31.  */
  33. #include "linden_common.h"
  35. #include "llquaternion.h"
  37. #include "llmath.h" // for F_PI
  38. //#include "vmath.h"
  39. #include "v3math.h"
  40. #include "v3dmath.h"
  41. #include "v4math.h"
  42. #include "m4math.h"
  43. #include "m3math.h"
  44. #include "llquantize.h"
  46. // WARNING: Don't use this for global const definitions!  using this
  47. // at the top of a *.cpp file might not give you what you think.
  48. const LLQuaternion LLQuaternion::DEFAULT;
  49.  
  50. // Constructors
  52. LLQuaternion::LLQuaternion(const LLMatrix4 &mat)
  53. {
  54. *this = mat.quaternion();
  55. normalize();
  56. }
  58. LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
  59. {
  60. *this = mat.quaternion();
  61. normalize();
  62. }
  64. LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
  65. {
  66. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  67. v.normalize();
  69. F32 c, s;
  70. c = cosf(angle*0.5f);
  71. s = sinf(angle*0.5f);
  73. mQ[VX] = v.mV[VX] * s;
  74. mQ[VY] = v.mV[VY] * s;
  75. mQ[VZ] = v.mV[VZ] * s;
  76. mQ[VW] = c;
  77. normalize();
  78. }
  80. LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
  81. {
  82. LLVector3 v(vec);
  83. v.normalize();
  85. F32 c, s;
  86. c = cosf(angle*0.5f);
  87. s = sinf(angle*0.5f);
  89. mQ[VX] = v.mV[VX] * s;
  90. mQ[VY] = v.mV[VY] * s;
  91. mQ[VZ] = v.mV[VZ] * s;
  92. mQ[VW] = c;
  93. normalize();
  94. }
  96. LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
  97.    const LLVector3 &y_axis,
  98.    const LLVector3 &z_axis)
  99. {
  100. LLMatrix3 mat;
  101. mat.setRows(x_axis, y_axis, z_axis);
  102. *this = mat.quaternion();
  103. normalize();
  104. }
  106. // Quatizations
  107. void LLQuaternion::quantize16(F32 lower, F32 upper)
  108. {
  109. F32 x = mQ[VX];
  110. F32 y = mQ[VY];
  111. F32 z = mQ[VZ];
  112. F32 s = mQ[VS];
  114. x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper);
  115. y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper);
  116. z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper);
  117. s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper);
  119. mQ[VX] = x;
  120. mQ[VY] = y;
  121. mQ[VZ] = z;
  122. mQ[VS] = s;
  124. normalize();
  125. }
  127. void LLQuaternion::quantize8(F32 lower, F32 upper)
  128. {
  129. mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper);
  130. mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, upper);
  131. mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper);
  132. mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper);
  134. normalize();
  135. }
  137. // LLVector3 Magnitude and Normalization Functions
  140. // Set LLQuaternion routines
  142. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
  143. {
  144. LLVector3 vec(x, y, z);
  145. vec.normalize();
  147. angle *= 0.5f;
  148. F32 c, s;
  149. c = cosf(angle);
  150. s = sinf(angle);
  152. mQ[VX] = vec.mV[VX]*s;
  153. mQ[VY] = vec.mV[VY]*s;
  154. mQ[VZ] = vec.mV[VZ]*s;
  155. mQ[VW] = c;
  157. normalize();
  158. return (*this);
  159. }
  161. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
  162. {
  163. LLVector3 v(vec);
  164. v.normalize();
  166. angle *= 0.5f;
  167. F32 c, s;
  168. c = cosf(angle);
  169. s = sinf(angle);
  171. mQ[VX] = v.mV[VX]*s;
  172. mQ[VY] = v.mV[VY]*s;
  173. mQ[VZ] = v.mV[VZ]*s;
  174. mQ[VW] = c;
  176. normalize();
  177. return (*this);
  178. }
  180. const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
  181. {
  182. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  183. v.normalize();
  185. F32 c, s;
  186. c = cosf(angle*0.5f);
  187. s = sinf(angle*0.5f);
  189. mQ[VX] = v.mV[VX]*s;
  190. mQ[VY] = v.mV[VY]*s;
  191. mQ[VZ] = v.mV[VZ]*s;
  192. mQ[VW] = c;
  194. normalize();
  195. return (*this);
  196. }
  198. const LLQuaternion& LLQuaternion::setEulerAngles(F32 roll, F32 pitch, F32 yaw)
  199. {
  200. LLMatrix3 rot_mat(roll, pitch, yaw);
  201. rot_mat.orthogonalize();
  202. *this = rot_mat.quaternion();
  204. normalize();
  205. return (*this);
  206. }
  208. // deprecated
  209. const LLQuaternion& LLQuaternion::set(const LLMatrix3 &mat)
  210. {
  211. *this = mat.quaternion();
  212. normalize();
  213. return (*this);
  214. }
  216. // deprecated
  217. const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat)
  218. {
  219. *this = mat.quaternion();
  220. normalize();
  221. return (*this);
  222. }
  224. // deprecated
  225. const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
  226. {
  227. LLVector3 vec(x, y, z);
  228. vec.normalize();
  230. angle *= 0.5f;
  231. F32 c, s;
  232. c = cosf(angle);
  233. s = sinf(angle);
  235. mQ[VX] = vec.mV[VX]*s;
  236. mQ[VY] = vec.mV[VY]*s;
  237. mQ[VZ] = vec.mV[VZ]*s;
  238. mQ[VW] = c;
  240. normalize();
  241. return (*this);
  242. }
  244. // deprecated
  245. const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
  246. {
  247. LLVector3 v(vec);
  248. v.normalize();
  250. angle *= 0.5f;
  251. F32 c, s;
  252. c = cosf(angle);
  253. s = sinf(angle);
  255. mQ[VX] = v.mV[VX]*s;
  256. mQ[VY] = v.mV[VY]*s;
  257. mQ[VZ] = v.mV[VZ]*s;
  258. mQ[VW] = c;
  260. normalize();
  261. return (*this);
  262. }
  264. const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
  265. {
  266. LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
  267. v.normalize();
  269. F32 c, s;
  270. c = cosf(angle*0.5f);
  271. s = sinf(angle*0.5f);
  273. mQ[VX] = v.mV[VX]*s;
  274. mQ[VY] = v.mV[VY]*s;
  275. mQ[VZ] = v.mV[VZ]*s;
  276. mQ[VW] = c;
  278. normalize();
  279. return (*this);
  280. }
  282. const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
  283. {
  284. LLMatrix3 rot_mat(roll, pitch, yaw);
  285. rot_mat.orthogonalize();
  286. *this = rot_mat.quaternion();
  288. normalize();
  289. return (*this);
  290. }
  292. const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat)
  293. {
  294. *this = mat.quaternion();
  295. normalize();
  296. return (*this);
  297. }
  299. const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat)
  300. {
  301. *this = mat.quaternion();
  302. normalize();
  303. return (*this);
  304. //#if 1
  305. // // NOTE: LLQuaternion's are actually inverted with respect to
  306. // // the matrices, so this code also assumes inverted quaternions
  307. // // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
  308. // // in reverse order (yaw,pitch,roll).
  309. // F64 cosX = cos(roll);
  310. //    F64 cosY = cos(pitch);
  311. //    F64 cosZ = cos(yaw);
  312. //
  313. //    F64 sinX = sin(roll);
  314. //    F64 sinY = sin(pitch);
  315. //    F64 sinZ = sin(yaw);
  316. //
  317. //    mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5;
  318. // if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO)
  319. // {
  320. // // null rotation, any axis will do
  321. // mQ[VX] = 0.0f;
  322. // mQ[VY] = 1.0f;
  323. // mQ[VZ] = 0.0f;
  324. // }
  325. // else
  326. // {
  327. // F32 inv_s = 1.0f / (4.0f * mQ[VW]);
  328. // mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s;
  329. // mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s;
  330. // mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s;
  331. // }
  332. //
  333. //#else // This only works on a certain subset of roll/pitch/yaw
  334. //
  335. // F64 cosX = cosf(roll/2.0);
  336. //    F64 cosY = cosf(pitch/2.0);
  337. //    F64 cosZ = cosf(yaw/2.0);
  338. //
  339. //    F64 sinX = sinf(roll/2.0);
  340. //    F64 sinY = sinf(pitch/2.0);
  341. //    F64 sinZ = sinf(yaw/2.0);
  342. //
  343. //    mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ);
  344. //    mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ);
  345. //    mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ);
  346. //    mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ);
  347. //#endif
  348. //
  349. // normalize();
  350. // return (*this);
  351. }
  353. // SJB: This code is correct for a logicly stored (non-transposed) matrix;
  354. // Our matrices are stored transposed, OpenGL style, so this generates the
  355. // INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion.
  356. // Because we use similar logic in LLMatrix3::quaternion(),
  357. // we are internally consistant so everything works OK :)
  358. LLMatrix3 LLQuaternion::getMatrix3(void) const
  359. {
  360. LLMatrix3 mat;
  361. F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
  363.     xx      = mQ[VX] * mQ[VX];
  364.     xy      = mQ[VX] * mQ[VY];
  365.     xz      = mQ[VX] * mQ[VZ];
  366.     xw      = mQ[VX] * mQ[VW];
  368.     yy      = mQ[VY] * mQ[VY];
  369.     yz      = mQ[VY] * mQ[VZ];
  370.     yw      = mQ[VY] * mQ[VW];
  372.     zz      = mQ[VZ] * mQ[VZ];
  373.     zw      = mQ[VZ] * mQ[VW];
  375.     mat.mMatrix[0][0]  = 1.f - 2.f * ( yy + zz );
  376.     mat.mMatrix[0][1]  =    2.f * ( xy + zw );
  377.     mat.mMatrix[0][2]  =    2.f * ( xz - yw );
  379.     mat.mMatrix[1][0]  =    2.f * ( xy - zw );
  380.     mat.mMatrix[1][1]  = 1.f - 2.f * ( xx + zz );
  381.     mat.mMatrix[1][2]  =    2.f * ( yz + xw );
  383.     mat.mMatrix[2][0]  =    2.f * ( xz + yw );
  384.     mat.mMatrix[2][1]  =    2.f * ( yz - xw );
  385.     mat.mMatrix[2][2]  = 1.f - 2.f * ( xx + yy );
  387. return mat;
  388. }
  390. LLMatrix4 LLQuaternion::getMatrix4(void) const
  391. {
  392. LLMatrix4 mat;
  393. F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
  395.     xx      = mQ[VX] * mQ[VX];
  396.     xy      = mQ[VX] * mQ[VY];
  397.     xz      = mQ[VX] * mQ[VZ];
  398.     xw      = mQ[VX] * mQ[VW];
  400.     yy      = mQ[VY] * mQ[VY];
  401.     yz      = mQ[VY] * mQ[VZ];
  402.     yw      = mQ[VY] * mQ[VW];
  404.     zz      = mQ[VZ] * mQ[VZ];
  405.     zw      = mQ[VZ] * mQ[VW];
  407.     mat.mMatrix[0][0]  = 1.f - 2.f * ( yy + zz );
  408.     mat.mMatrix[0][1]  =    2.f * ( xy + zw );
  409.     mat.mMatrix[0][2]  =    2.f * ( xz - yw );
  411.     mat.mMatrix[1][0]  =    2.f * ( xy - zw );
  412.     mat.mMatrix[1][1]  = 1.f - 2.f * ( xx + zz );
  413.     mat.mMatrix[1][2]  =    2.f * ( yz + xw );
  415.     mat.mMatrix[2][0]  =    2.f * ( xz + yw );
  416.     mat.mMatrix[2][1]  =    2.f * ( yz - xw );
  417.     mat.mMatrix[2][2]  = 1.f - 2.f * ( xx + yy );
  419. // TODO -- should we set the translation portion to zero?
  421. return mat;
  422. }
  427. // Other useful methods
  430. // calculate the shortest rotation from a to b
  431. void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
  432. {
  433. // Make a local copy of both vectors.
  434. LLVector3 vec_a = a;
  435. LLVector3 vec_b = b;
  437. // Make sure neither vector is zero length.  Also normalize
  438. // the vectors while we are at it.
  439. F32 vec_a_mag = vec_a.normalize();
  440. F32 vec_b_mag = vec_b.normalize();
  441. if (vec_a_mag < F_APPROXIMATELY_ZERO ||
  442. vec_b_mag < F_APPROXIMATELY_ZERO)
  443. {
  444. // Can't calculate a rotation from this.
  445. // Just return ZERO_ROTATION instead.
  446. loadIdentity();
  447. return;
  448. }
  450. // Create an axis to rotate around, and the cos of the angle to rotate.
  451. LLVector3 axis = vec_a % vec_b;
  452. F32 cos_theta  = vec_a * vec_b;
  454. // Check the angle between the vectors to see if they are parallel or anti-parallel.
  455. if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
  456. {
  457. // a and b are parallel.  No rotation is necessary.
  458. loadIdentity();
  459. }
  460. else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
  461. {
  462. // a and b are anti-parallel.
  463. // Rotate 180 degrees around some orthogonal axis.
  464. // Find the projection of the x-axis onto a, and try
  465. // using the vector between the projection and the x-axis
  466. // as the orthogonal axis.
  467. LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
  468. LLVector3 ortho_axis(1.f, 0.f, 0.f);
  469. ortho_axis -= proj;
  471. // Turn this into an orthonormal axis.
  472. F32 ortho_length = ortho_axis.normalize();
  473. // If the axis' length is 0, then our guess at an orthogonal axis
  474. // was wrong (a is parallel to the x-axis).
  475. if (ortho_length < F_APPROXIMATELY_ZERO)
  476. {
  477. // Use the z-axis instead.
  478. ortho_axis.setVec(0.f, 0.f, 1.f);
  479. }
  481. // Construct a quaternion from this orthonormal axis.
  482. mQ[VX] = ortho_axis.mV[VX];
  483. mQ[VY] = ortho_axis.mV[VY];
  484. mQ[VZ] = ortho_axis.mV[VZ];
  485. mQ[VW] = 0.f;
  486. }
  487. else
  488. {
  489. // a and b are NOT parallel or anti-parallel.
  490. // Return the rotation between these vectors.
  491. F32 theta = (F32)acos(cos_theta);
  493. setAngleAxis(theta, axis);
  494. }
  495. }
  497. // constrains rotation to a cone angle specified in radians
  498. const LLQuaternion &LLQuaternion::constrain(F32 radians)
  499. {
  500. const F32 cos_angle_lim = cosf( radians/2 ); // mQ[VW] limit
  501. const F32 sin_angle_lim = sinf( radians/2 ); // rotation axis length limit
  503. if (mQ[VW] < 0.f)
  504. {
  505. mQ[VX] *= -1.f;
  506. mQ[VY] *= -1.f;
  507. mQ[VZ] *= -1.f;
  508. mQ[VW] *= -1.f;
  509. }
  511. // if rotation angle is greater than limit (cos is less than limit)
  512. if( mQ[VW] < cos_angle_lim )
  513. {
  514. mQ[VW] = cos_angle_lim;
  515. F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2)
  516. F32 axis_mult_fact = sin_angle_lim / axis_len;
  517. mQ[VX] *= axis_mult_fact;
  518. mQ[VY] *= axis_mult_fact;
  519. mQ[VZ] *= axis_mult_fact;
  520. }
  522. return *this;
  523. }
  525. // Operators
  527. std::ostream& operator<<(std::ostream &s, const LLQuaternion &a)
  528. {
  529. s << "{ " 
  530. << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW] 
  531. << " }";
  532. return s;
  533. }
  536. // Does NOT renormalize the result
  537. LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b)
  538. {
  539. // LLQuaternion::mMultCount++;
  541. LLQuaternion q(
  542. b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
  543. b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
  544. b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
  545. b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
  546. );
  547. return q;
  548. }
  550. /*
  551. LLMatrix4 operator*(const LLMatrix4 &m, const LLQuaternion &q)
  552. {
  553. LLMatrix4 qmat(q);
  554. return (m*qmat);
  555. }
  556. */
  560. LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot)
  561. {
  562.     F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
  563.     F32 rx =   rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
  564.     F32 ry =   rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
  565.     F32 rz =   rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
  567.     F32 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  568.     F32 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  569.     F32 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  571.     return LLVector4(nx, ny, nz, a.mV[VW]);
  572. }
  574. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot)
  575. {
  576.     F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
  577.     F32 rx =   rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
  578.     F32 ry =   rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
  579.     F32 rz =   rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
  581.     F32 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  582.     F32 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  583.     F32 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  585.     return LLVector3(nx, ny, nz);
  586. }
  588. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot)
  589. {
  590.     F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ];
  591.     F64 rx =   rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY];
  592.     F64 ry =   rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ];
  593.     F64 rz =   rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX];
  595.     F64 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
  596.     F64 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
  597.     F64 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
  599.     return LLVector3d(nx, ny, nz);
  600. }
  602. F32 dot(const LLQuaternion &a, const LLQuaternion &b)
  603. {
  604. return a.mQ[VX] * b.mQ[VX] + 
  605.    a.mQ[VY] * b.mQ[VY] + 
  606.    a.mQ[VZ] * b.mQ[VZ] + 
  607.    a.mQ[VW] * b.mQ[VW]; 
  608. }
  610. // DEMO HACK: This lerp is probably inocrrect now due intermediate normalization
  611. // it should look more like the lerp below
  612. #if 0
  613. // linear interpolation
  614. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
  615. {
  616. LLQuaternion r;
  617. r = t * (q - p) + p;
  618. r.normalize();
  619. return r;
  620. }
  621. #endif
  623. // lerp from identity to q
  624. LLQuaternion lerp(F32 t, const LLQuaternion &q)
  625. {
  626. LLQuaternion r;
  627. r.mQ[VX] = t * q.mQ[VX];
  628. r.mQ[VY] = t * q.mQ[VY];
  629. r.mQ[VZ] = t * q.mQ[VZ];
  630. r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f;
  631. r.normalize();
  632. return r;
  633. }
  635. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
  636. {
  637. LLQuaternion r;
  638. F32 inv_t;
  640. inv_t = 1.f - t;
  642. r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]);
  643. r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]);
  644. r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]);
  645. r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]);
  646. r.normalize();
  647. return r;
  648. }
  651. // spherical linear interpolation
  652. LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b )
  653. {
  654. // cosine theta = dot product of a and b
  655. 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];
  657. // if b is on opposite hemisphere from a, use -a instead
  658. int bflip;
  659.   if (cos_t < 0.0f)
  660. {
  661. cos_t = -cos_t;
  662. bflip = TRUE;
  663. }
  664. else
  665. bflip = FALSE;
  667. // if B is (within precision limits) the same as A,
  668. // just linear interpolate between A and B.
  669. F32 alpha; // interpolant
  670. F32 beta; // 1 - interpolant
  671. if (1.0f - cos_t < 0.00001f)
  672. {
  673. beta = 1.0f - u;
  674. alpha = u;
  675.   }
  676. else
  677. {
  678.   F32 theta = acosf(cos_t);
  679.   F32 sin_t = sinf(theta);
  680.   beta = sinf(theta - u*theta) / sin_t;
  681.   alpha = sinf(u*theta) / sin_t;
  682.   }
  684. if (bflip)
  685. beta = -beta;
  687. // interpolate
  688. LLQuaternion ret;
  689. ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0];
  690.   ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1];
  691.   ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2];
  692.   ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3];
  694. return ret;
  695. }
  697. // lerp whenever possible
  698. LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
  699. {
  700. if (dot(a, b) < 0.f)
  701. {
  702. return slerp(t, a, b);
  703. }
  704. else
  705. {
  706. return lerp(t, a, b);
  707. }
  708. }
  710. LLQuaternion nlerp(F32 t, const LLQuaternion &q)
  711. {
  712. if (q.mQ[VW] < 0.f)
  713. {
  714. return slerp(t, q);
  715. }
  716. else
  717. {
  718. return lerp(t, q);
  719. }
  720. }
  722. // slerp from identity quaternion to another quaternion
  723. LLQuaternion slerp(F32 t, const LLQuaternion &q)
  724. {
  725. F32 c = q.mQ[VW];
  726. if (1.0f == t  ||  1.0f == c)
  727. {
  728. // the trivial cases
  729. return q;
  730. }
  732. LLQuaternion r;
  733. F32 s, angle, stq, stp;
  735. s = (F32) sqrt(1.f - c*c);
  737.     if (c < 0.0f)
  738.     {
  739.         // when c < 0.0 then theta > PI/2 
  740.         // since quat and -quat are the same rotation we invert one of  
  741.         // p or q to reduce unecessary spins
  742.         // A equivalent way to do it is to convert acos(c) as if it had 
  743. // been negative, and to negate stp 
  744.         angle   = (F32) acos(-c); 
  745.         stp     = -(F32) sin(angle * (1.f - t));
  746.         stq     = (F32) sin(angle * t);
  747.     }   
  748.     else
  749.     {
  750. angle  = (F32) acos(c);
  751.         stp     = (F32) sin(angle * (1.f - t));
  752.         stq     = (F32) sin(angle * t);
  753.     }
  755. r.mQ[VX] = (q.mQ[VX] * stq) / s;
  756. r.mQ[VY] = (q.mQ[VY] * stq) / s;
  757. r.mQ[VZ] = (q.mQ[VZ] * stq) / s;
  758. r.mQ[VW] = (stp + q.mQ[VW] * stq) / s;
  760. return r;
  761. }
  763. LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order)
  764. {
  765. LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) );
  766. LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) );
  767. LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) );
  768. LLQuaternion ret;
  769. switch( order )
  770. {
  771. case LLQuaternion::XYZ:
  772. ret = xQ * yQ * zQ;
  773. break;
  774. case LLQuaternion::YZX:
  775. ret = yQ * zQ * xQ;
  776. break;
  777. case LLQuaternion::ZXY:
  778. ret = zQ * xQ * yQ;
  779. break;
  780. case LLQuaternion::XZY:
  781. ret = xQ * zQ * yQ;
  782. break;
  783. case LLQuaternion::YXZ:
  784. ret = yQ * xQ * zQ;
  785. break;
  786. case LLQuaternion::ZYX:
  787. ret = zQ * yQ * xQ;
  788. break;
  789. }
  790. return ret;
  791. }
  793. const char *OrderToString( const LLQuaternion::Order order )
  794. {
  795. const char *p = NULL;
  796. switch( order )
  797. {
  798. default:
  799. case LLQuaternion::XYZ:
  800. p = "XYZ";
  801. break;
  802. case LLQuaternion::YZX:
  803. p = "YZX";
  804. break;
  805. case LLQuaternion::ZXY:
  806. p = "ZXY";
  807. break;
  808. case LLQuaternion::XZY:
  809. p = "XZY";
  810. break;
  811. case LLQuaternion::YXZ:
  812. p = "YXZ";
  813. break;
  814. case LLQuaternion::ZYX:
  815. p = "ZYX";
  816. break;
  817. }
  818. return p;
  819. }
  821. LLQuaternion::Order StringToOrder( const char *str )
  822. {
  823. if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0)
  824. return LLQuaternion::XYZ;
  826. if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0)
  827. return LLQuaternion::YZX;
  829. if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0)
  830. return LLQuaternion::ZXY;
  832. if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0)
  833. return LLQuaternion::XZY;
  835. if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0)
  836. return LLQuaternion::YXZ;
  838. if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0)
  839. return LLQuaternion::ZYX;
  841. return LLQuaternion::XYZ;
  842. }
  844. void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
  845. {
  846. F32 cos_a = mQ[VW];
  847. if (cos_a > 1.0f) cos_a = 1.0f;
  848. if (cos_a < -1.0f) cos_a = -1.0f;
  850.     F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
  852.     if ( fabs( sin_a ) < 0.0005f )
  853. sin_a = 1.0f;
  854. else
  855. sin_a = 1.f/sin_a;
  857.     F32 temp_angle = 2.0f * (F32) acos( cos_a );
  858. if (temp_angle > F_PI)
  859. {
  860. // The (angle,axis) pair should never have angles outside [PI, -PI]
  861. // since we want the _shortest_ (angle,axis) solution.
  862. // Since acos is defined for [0, PI], and we multiply by 2.0, we
  863. // can push the angle outside the acceptible range.
  864. // When this happens we set the angle to the other portion of a 
  865. // full 2PI rotation, and negate the axis, which reverses the 
  866. // direction of the rotation (by the right-hand rule).
  867. *angle = 2.f * F_PI - temp_angle;
  868.      vec.mV[VX] = - mQ[VX] * sin_a;
  869.      vec.mV[VY] = - mQ[VY] * sin_a;
  870.      vec.mV[VZ] = - mQ[VZ] * sin_a;
  871. }
  872. else
  873. {
  874. *angle = temp_angle;
  875.      vec.mV[VX] = mQ[VX] * sin_a;
  876.      vec.mV[VY] = mQ[VY] * sin_a;
  877.      vec.mV[VZ] = mQ[VZ] * sin_a;
  878. }
  879. }
  882. // quaternion does not need to be normalized
  883. void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
  884. {
  885. LLMatrix3 rot_mat(*this);
  886. rot_mat.orthogonalize();
  887. rot_mat.getEulerAngles(roll, pitch, yaw);
  889. // // NOTE: LLQuaternion's are actually inverted with respect to
  890. // // the matrices, so this code also assumes inverted quaternions
  891. // // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
  892. // // in reverse order (yaw,pitch,roll).
  893. // F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
  894. // F64 m20 = 2.0*(x*z-y*w);
  895. // if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
  896. // {
  897. // *roll = 0.0f;
  898. // *pitch = (F32)asin(m20);
  899. // *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
  900. // }
  901. // else
  902. // {
  903. // *roll  = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
  904. // *pitch = (F32)asin(m20);
  905. // *yaw   = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
  906. // }
  907. }
  909. // Saves space by using the fact that our quaternions are normalized
  910. LLVector3 LLQuaternion::packToVector3() const
  911. {
  912. if( mQ[VW] >= 0 )
  913. {
  914. return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
  915. }
  916. else
  917. {
  918. return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
  919. }
  920. }
  922. // Saves space by using the fact that our quaternions are normalized
  923. void LLQuaternion::unpackFromVector3( const LLVector3& vec )
  924. {
  925. mQ[VX] = vec.mV[VX];
  926. mQ[VY] = vec.mV[VY];
  927. mQ[VZ] = vec.mV[VZ];
  928. F32 t = 1.f - vec.magVecSquared();
  929. if( t > 0 )
  930. {
  931. mQ[VW] = sqrt( t );
  932. }
  933. else
  934. {
  935. // Need this to avoid trying to find the square root of a negative number due
  936. // to floating point error.
  937. mQ[VW] = 0;
  938. }
  939. }
  941. BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value)
  942. {
  943. if( buf.empty() || value == NULL)
  944. {
  945. return FALSE;
  946. }
  948. LLQuaternion quat;
  949. S32 count = sscanf( buf.c_str(), "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 );
  950. if( 4 == count )
  951. {
  952. value->set( quat );
  953. return TRUE;
  954. }
  956. return FALSE;
  957. }
  960. // End