llquaternion.h
上传用户:king477883
上传日期:2021-03-01
资源大小:9553k
文件大小:17k
源码类别:

游戏引擎

开发平台:

C++ Builder

  1. /** 
  2.  * @file llquaternion.h
  3.  * @brief LLQuaternion class header file.
  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.  */
  32. #ifndef LLQUATERNION_H
  33. #define LLQUATERNION_H
  34. #include "llmath.h"
  35. class LLVector4;
  36. class LLVector3;
  37. class LLVector3d;
  38. class LLMatrix4;
  39. class LLMatrix3;
  40. // NOTA BENE: Quaternion code is written assuming Unit Quaternions!!!!
  41. //    Moreover, it is written assuming that all vectors and matricies
  42. //    passed as arguments are normalized and unitary respectively.
  43. //    VERY VERY VERY VERY BAD THINGS will happen if these assumptions fail.
  44. static const U32 LENGTHOFQUAT = 4;
  45. class LLQuaternion
  46. {
  47. public:
  48. F32 mQ[LENGTHOFQUAT];
  49. static const LLQuaternion DEFAULT;
  50. LLQuaternion(); // Initializes Quaternion to (0,0,0,1)
  51. explicit LLQuaternion(const LLMatrix4 &mat); // Initializes Quaternion from Matrix4
  52. explicit LLQuaternion(const LLMatrix3 &mat); // Initializes Quaternion from Matrix3
  53. LLQuaternion(F32 x, F32 y, F32 z, F32 w); // Initializes Quaternion to normalize(x, y, z, w)
  54. LLQuaternion(F32 angle, const LLVector4 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
  55. LLQuaternion(F32 angle, const LLVector3 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
  56. LLQuaternion(const F32 *q); // Initializes Quaternion to normalize(x, y, z, w)
  57. LLQuaternion(const LLVector3 &x_axis,
  58.  const LLVector3 &y_axis,
  59.  const LLVector3 &z_axis); // Initializes Quaternion from Matrix3 = [x_axis ; y_axis ; z_axis]
  60. BOOL isIdentity() const;
  61. BOOL isNotIdentity() const;
  62. BOOL isFinite() const; // checks to see if all values of LLQuaternion are finite
  63. void quantize16(F32 lower, F32 upper); // changes the vector to reflect quatization
  64. void quantize8(F32 lower, F32 upper); // changes the vector to reflect quatization
  65. void loadIdentity(); // Loads the quaternion that represents the identity rotation
  66. const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w); // Sets Quaternion to normalize(x, y, z, w)
  67. const LLQuaternion& set(const LLQuaternion &quat); // Copies Quaternion
  68. const LLQuaternion& set(const F32 *q); // Sets Quaternion to normalize(quat[VX], quat[VY], quat[VZ], quat[VW])
  69. const LLQuaternion& set(const LLMatrix3 &mat); // Sets Quaternion to mat2quat(mat)
  70. const LLQuaternion& set(const LLMatrix4 &mat); // Sets Quaternion to mat2quat(mat)
  71. const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z); // Sets Quaternion to axis_angle2quat(angle, x, y, z)
  72. const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
  73. const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
  74. const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw); // Sets Quaternion to euler2quat(pitch, yaw, roll)
  75. const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w); // deprecated
  76. const LLQuaternion& setQuat(const LLQuaternion &quat); // deprecated
  77. const LLQuaternion& setQuat(const F32 *q); // deprecated
  78. const LLQuaternion& setQuat(const LLMatrix3 &mat); // deprecated
  79. const LLQuaternion& setQuat(const LLMatrix4 &mat); // deprecated
  80. const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z); // deprecated
  81. const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec); // deprecated
  82. const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec); // deprecated
  83. const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw); // deprecated
  84. LLMatrix4 getMatrix4(void) const; // Returns the Matrix4 equivalent of Quaternion
  85. LLMatrix3 getMatrix3(void) const; // Returns the Matrix3 equivalent of Quaternion
  86. void getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const; // returns rotation in radians about axis x,y,z
  87. void getAngleAxis(F32* angle, LLVector3 &vec) const;
  88. void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
  89. F32 normalize(); // Normalizes Quaternion and returns magnitude
  90. F32 normQuat(); // deprecated
  91. const LLQuaternion& conjugate(void); // Conjugates Quaternion and returns result
  92. const LLQuaternion& conjQuat(void); // deprecated
  93. // Other useful methods
  94. const LLQuaternion& transpose(); // transpose (same as conjugate)
  95. const LLQuaternion& transQuat(); // deprecated
  96. void shortestArc(const LLVector3 &a, const LLVector3 &b); // shortest rotation from a to b
  97. const LLQuaternion& constrain(F32 radians); // constrains rotation to a cone angle specified in radians
  98. // Standard operators
  99. friend std::ostream& operator<<(std::ostream &s, const LLQuaternion &a); // Prints a
  100. friend LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b); // Addition
  101. friend LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b); // Subtraction
  102. friend LLQuaternion operator-(const LLQuaternion &a); // Negation
  103. friend LLQuaternion operator*(F32 a, const LLQuaternion &q); // Scale
  104. friend LLQuaternion operator*(const LLQuaternion &q, F32 b); // Scale
  105. friend LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b); // Returns a * b
  106. friend LLQuaternion operator~(const LLQuaternion &a); // Returns a* (Conjugate of a)
  107. bool operator==(const LLQuaternion &b) const; // Returns a == b
  108. bool operator!=(const LLQuaternion &b) const; // Returns a != b
  109. friend const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b); // Returns a * b
  110. friend LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot); // Rotates a by rot
  111. friend LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot); // Rotates a by rot
  112. friend LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot); // Rotates a by rot
  113. // Non-standard operators
  114. friend F32 dot(const LLQuaternion &a, const LLQuaternion &b);
  115. friend LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from p to q
  116. friend LLQuaternion lerp(F32 t, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from identity to q
  117. friend LLQuaternion slerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);  // spherical linear interpolation from p to q
  118. friend LLQuaternion slerp(F32 t, const LLQuaternion &q); // spherical linear interpolation from identity to q
  119. friend LLQuaternion nlerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);  // normalized linear interpolation from p to q
  120. friend LLQuaternion nlerp(F32 t, const LLQuaternion &q);  // normalized linear interpolation from p to q
  121. LLVector3 packToVector3() const; // Saves space by using the fact that our quaternions are normalized
  122. void unpackFromVector3(const LLVector3& vec); // Saves space by using the fact that our quaternions are normalized
  123. enum Order {
  124. XYZ = 0,
  125. YZX = 1,
  126. ZXY = 2,
  127. XZY = 3,
  128. YXZ = 4,
  129. ZYX = 5
  130. };
  131. // Creates a quaternions from maya's rotation representation,
  132. // which is 3 rotations (in DEGREES) in the specified order
  133. friend LLQuaternion mayaQ(F32 x, F32 y, F32 z, Order order);
  134. // Conversions between Order and strings like "xyz" or "ZYX"
  135. friend const char *OrderToString( const Order order );
  136. friend Order StringToOrder( const char *str );
  137. static BOOL parseQuat(const std::string& buf, LLQuaternion* value);
  138. // For debugging, only
  139. //static U32 mMultCount;
  140. };
  141. // checker
  142. inline BOOL LLQuaternion::isFinite() const
  143. {
  144. return (llfinite(mQ[VX]) && llfinite(mQ[VY]) && llfinite(mQ[VZ]) && llfinite(mQ[VS]));
  145. }
  146. inline BOOL LLQuaternion::isIdentity() const
  147. {
  148. return 
  149. ( mQ[VX] == 0.f ) &&
  150. ( mQ[VY] == 0.f ) &&
  151. ( mQ[VZ] == 0.f ) &&
  152. ( mQ[VS] == 1.f );
  153. }
  154. inline BOOL LLQuaternion::isNotIdentity() const
  155. {
  156. return 
  157. ( mQ[VX] != 0.f ) ||
  158. ( mQ[VY] != 0.f ) ||
  159. ( mQ[VZ] != 0.f ) ||
  160. ( mQ[VS] != 1.f );
  161. }
  162. inline LLQuaternion::LLQuaternion(void)
  163. {
  164. mQ[VX] = 0.f;
  165. mQ[VY] = 0.f;
  166. mQ[VZ] = 0.f;
  167. mQ[VS] = 1.f;
  168. }
  169. inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w)
  170. {
  171. mQ[VX] = x;
  172. mQ[VY] = y;
  173. mQ[VZ] = z;
  174. mQ[VS] = w;
  175. //RN: don't normalize this case as its used mainly for temporaries during calculations
  176. //normalize();
  177. /*
  178. F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
  179. mag -= 1.f;
  180. mag = fabs(mag);
  181. llassert(mag < 10.f*FP_MAG_THRESHOLD);
  182. */
  183. }
  184. inline LLQuaternion::LLQuaternion(const F32 *q)
  185. {
  186. mQ[VX] = q[VX];
  187. mQ[VY] = q[VY];
  188. mQ[VZ] = q[VZ];
  189. mQ[VS] = q[VW];
  190. normalize();
  191. /*
  192. F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
  193. mag -= 1.f;
  194. mag = fabs(mag);
  195. llassert(mag < FP_MAG_THRESHOLD);
  196. */
  197. }
  198. inline void LLQuaternion::loadIdentity()
  199. {
  200. mQ[VX] = 0.0f;
  201. mQ[VY] = 0.0f;
  202. mQ[VZ] = 0.0f;
  203. mQ[VW] = 1.0f;
  204. }
  205. inline const LLQuaternion& LLQuaternion::set(F32 x, F32 y, F32 z, F32 w)
  206. {
  207. mQ[VX] = x;
  208. mQ[VY] = y;
  209. mQ[VZ] = z;
  210. mQ[VS] = w;
  211. normalize();
  212. return (*this);
  213. }
  214. inline const LLQuaternion& LLQuaternion::set(const LLQuaternion &quat)
  215. {
  216. mQ[VX] = quat.mQ[VX];
  217. mQ[VY] = quat.mQ[VY];
  218. mQ[VZ] = quat.mQ[VZ];
  219. mQ[VW] = quat.mQ[VW];
  220. normalize();
  221. return (*this);
  222. }
  223. inline const LLQuaternion& LLQuaternion::set(const F32 *q)
  224. {
  225. mQ[VX] = q[VX];
  226. mQ[VY] = q[VY];
  227. mQ[VZ] = q[VZ];
  228. mQ[VS] = q[VW];
  229. normalize();
  230. return (*this);
  231. }
  232. // deprecated
  233. inline const LLQuaternion& LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w)
  234. {
  235. mQ[VX] = x;
  236. mQ[VY] = y;
  237. mQ[VZ] = z;
  238. mQ[VS] = w;
  239. normalize();
  240. return (*this);
  241. }
  242. // deprecated
  243. inline const LLQuaternion& LLQuaternion::setQuat(const LLQuaternion &quat)
  244. {
  245. mQ[VX] = quat.mQ[VX];
  246. mQ[VY] = quat.mQ[VY];
  247. mQ[VZ] = quat.mQ[VZ];
  248. mQ[VW] = quat.mQ[VW];
  249. normalize();
  250. return (*this);
  251. }
  252. // deprecated
  253. inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q)
  254. {
  255. mQ[VX] = q[VX];
  256. mQ[VY] = q[VY];
  257. mQ[VZ] = q[VZ];
  258. mQ[VS] = q[VW];
  259. normalize();
  260. return (*this);
  261. }
  262. // There may be a cheaper way that avoids the sqrt.
  263. // Does sin_a = VX*VX + VY*VY + VZ*VZ?
  264. // Copied from Matrix and Quaternion FAQ 1.12
  265. inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
  266. {
  267. F32 cos_a = mQ[VW];
  268. if (cos_a > 1.0f) cos_a = 1.0f;
  269. if (cos_a < -1.0f) cos_a = -1.0f;
  270.     F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
  271.     if ( fabs( sin_a ) < 0.0005f )
  272. sin_a = 1.0f;
  273. else
  274. sin_a = 1.f/sin_a;
  275.     F32 temp_angle = 2.0f * (F32) acos( cos_a );
  276. if (temp_angle > F_PI)
  277. {
  278. // The (angle,axis) pair should never have angles outside [PI, -PI]
  279. // since we want the _shortest_ (angle,axis) solution.
  280. // Since acos is defined for [0, PI], and we multiply by 2.0, we
  281. // can push the angle outside the acceptible range.
  282. // When this happens we set the angle to the other portion of a 
  283. // full 2PI rotation, and negate the axis, which reverses the 
  284. // direction of the rotation (by the right-hand rule).
  285. *angle = 2.f * F_PI - temp_angle;
  286.      *x = - mQ[VX] * sin_a;
  287.      *y = - mQ[VY] * sin_a;
  288.      *z = - mQ[VZ] * sin_a;
  289. }
  290. else
  291. {
  292. *angle = temp_angle;
  293.      *x = mQ[VX] * sin_a;
  294.      *y = mQ[VY] * sin_a;
  295.      *z = mQ[VZ] * sin_a;
  296. }
  297. }
  298. inline const LLQuaternion& LLQuaternion::conjugate()
  299. {
  300. mQ[VX] *= -1.f;
  301. mQ[VY] *= -1.f;
  302. mQ[VZ] *= -1.f;
  303. return (*this);
  304. }
  305. inline const LLQuaternion& LLQuaternion::conjQuat()
  306. {
  307. mQ[VX] *= -1.f;
  308. mQ[VY] *= -1.f;
  309. mQ[VZ] *= -1.f;
  310. return (*this);
  311. }
  312. // Transpose
  313. inline const LLQuaternion& LLQuaternion::transpose()
  314. {
  315. mQ[VX] *= -1.f;
  316. mQ[VY] *= -1.f;
  317. mQ[VZ] *= -1.f;
  318. return (*this);
  319. }
  320. // deprecated
  321. inline const LLQuaternion& LLQuaternion::transQuat()
  322. {
  323. mQ[VX] *= -1.f;
  324. mQ[VY] *= -1.f;
  325. mQ[VZ] *= -1.f;
  326. return (*this);
  327. }
  328. inline LLQuaternion  operator+(const LLQuaternion &a, const LLQuaternion &b)
  329. {
  330. return LLQuaternion( 
  331. a.mQ[VX] + b.mQ[VX],
  332. a.mQ[VY] + b.mQ[VY],
  333. a.mQ[VZ] + b.mQ[VZ],
  334. a.mQ[VW] + b.mQ[VW] );
  335. }
  336. inline LLQuaternion  operator-(const LLQuaternion &a, const LLQuaternion &b)
  337. {
  338. return LLQuaternion( 
  339. a.mQ[VX] - b.mQ[VX],
  340. a.mQ[VY] - b.mQ[VY],
  341. a.mQ[VZ] - b.mQ[VZ],
  342. a.mQ[VW] - b.mQ[VW] );
  343. }
  344. inline LLQuaternion  operator-(const LLQuaternion &a)
  345. {
  346. return LLQuaternion(
  347. -a.mQ[VX],
  348. -a.mQ[VY],
  349. -a.mQ[VZ],
  350. -a.mQ[VW] );
  351. }
  352. inline LLQuaternion  operator*(F32 a, const LLQuaternion &q)
  353. {
  354. return LLQuaternion(
  355. a * q.mQ[VX],
  356. a * q.mQ[VY],
  357. a * q.mQ[VZ],
  358. a * q.mQ[VW] );
  359. }
  360. inline LLQuaternion  operator*(const LLQuaternion &q, F32 a)
  361. {
  362. return LLQuaternion(
  363. a * q.mQ[VX],
  364. a * q.mQ[VY],
  365. a * q.mQ[VZ],
  366. a * q.mQ[VW] );
  367. }
  368. inline LLQuaternion operator~(const LLQuaternion &a)
  369. {
  370. LLQuaternion q(a);
  371. q.conjQuat();
  372. return q;
  373. }
  374. inline bool LLQuaternion::operator==(const LLQuaternion &b) const
  375. {
  376. return (  (mQ[VX] == b.mQ[VX])
  377. &&(mQ[VY] == b.mQ[VY])
  378. &&(mQ[VZ] == b.mQ[VZ])
  379. &&(mQ[VS] == b.mQ[VS]));
  380. }
  381. inline bool LLQuaternion::operator!=(const LLQuaternion &b) const
  382. {
  383. return (  (mQ[VX] != b.mQ[VX])
  384. ||(mQ[VY] != b.mQ[VY])
  385. ||(mQ[VZ] != b.mQ[VZ])
  386. ||(mQ[VS] != b.mQ[VS]));
  387. }
  388. inline const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b)
  389. {
  390. #if 1
  391. LLQuaternion q(
  392. b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
  393. b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
  394. b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
  395. b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
  396. );
  397. a = q;
  398. #else
  399. a = a * b;
  400. #endif
  401. return a;
  402. }
  403. const F32 ONE_PART_IN_A_MILLION = 0.000001f;
  404. inline F32 LLQuaternion::normalize()
  405. {
  406. F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
  407. if (mag > FP_MAG_THRESHOLD)
  408. {
  409. // Floating point error can prevent some quaternions from achieving
  410. // exact unity length.  When trying to renormalize such quaternions we
  411. // can oscillate between multiple quantized states.  To prevent such
  412. // drifts we only renomalize if the length is far enough from unity.
  413. if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
  414. {
  415. F32 oomag = 1.f/mag;
  416. mQ[VX] *= oomag;
  417. mQ[VY] *= oomag;
  418. mQ[VZ] *= oomag;
  419. mQ[VS] *= oomag;
  420. }
  421. }
  422. else
  423. {
  424. // we were given a very bad quaternion so we set it to identity
  425. mQ[VX] = 0.f;
  426. mQ[VY] = 0.f;
  427. mQ[VZ] = 0.f;
  428. mQ[VS] = 1.f;
  429. }
  430. return mag;
  431. }
  432. // deprecated
  433. inline F32 LLQuaternion::normQuat()
  434. {
  435. F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
  436. if (mag > FP_MAG_THRESHOLD)
  437. {
  438. if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
  439. {
  440. // only renormalize if length not close enough to 1.0 already
  441. F32 oomag = 1.f/mag;
  442. mQ[VX] *= oomag;
  443. mQ[VY] *= oomag;
  444. mQ[VZ] *= oomag;
  445. mQ[VS] *= oomag;
  446. }
  447. }
  448. else
  449. {
  450. mQ[VX] = 0.f;
  451. mQ[VY] = 0.f;
  452. mQ[VZ] = 0.f;
  453. mQ[VS] = 1.f;
  454. }
  455. return mag;
  456. }
  457. LLQuaternion::Order StringToOrder( const char *str );
  458. // Some notes about Quaternions
  459. // What is a Quaternion?
  460. // ---------------------
  461. // A quaternion is a point in 4-dimensional complex space.
  462. // Q = { Qx, Qy, Qz, Qw }
  463. // 
  464. //
  465. // Why Quaternions?
  466. // ----------------
  467. // The set of quaternions that make up the the 4-D unit sphere 
  468. // can be mapped to the set of all rotations in 3-D space.  Sometimes
  469. // it is easier to describe/manipulate rotations in quaternion space
  470. // than rotation-matrix space.
  471. //
  472. //
  473. // How Quaternions?
  474. // ----------------
  475. // In order to take advantage of quaternions we need to know how to
  476. // go from rotation-matricies to quaternions and back.  We also have
  477. // to agree what variety of rotations we're generating.
  478. // 
  479. // Consider the equation...   v' = v * R 
  480. //
  481. // There are two ways to think about rotations of vectors.
  482. // 1) v' is the same vector in a different reference frame
  483. // 2) v' is a new vector in the same reference frame
  484. //
  485. // bookmark -- which way are we using?
  486. // 
  487. // 
  488. // Quaternion from Angle-Axis:
  489. // ---------------------------
  490. // Suppose we wanted to represent a rotation of some angle (theta) 
  491. // about some axis ({Ax, Ay, Az})...
  492. //
  493. // axis of rotation = {Ax, Ay, Az} 
  494. // angle_of_rotation = theta
  495. //
  496. // s = sin(0.5 * theta)
  497. // c = cos(0.5 * theta)
  498. // Q = { s * Ax, s * Ay, s * Az, c }
  499. //
  500. //
  501. // 3x3 Matrix from Quaternion
  502. // --------------------------
  503. //
  504. //     |                                                                    |
  505. //     | 1 - 2 * (y^2 + z^2)   2 * (x * y + z * w)     2 * (y * w - x * z)  |
  506. //     |                                                                    |
  507. // M = | 2 * (x * y - z * w)   1 - 2 * (x^2 + z^2)     2 * (y * z + x * w)  |
  508. //     |                                                                    |
  509. //     | 2 * (x * z + y * w)   2 * (y * z - x * w)     1 - 2 * (x^2 + y^2)  |
  510. //     |                                                                    |
  511. #endif