Source: quat.js

  1. import * as glMatrix from "./common.js"
  2. import * as mat3 from "./mat3.js"
  3. import * as vec3 from "./vec3.js"
  4. import * as vec4 from "./vec4.js"
  5. /**
  6. * Quaternion
  7. * @module quat
  8. */
  9. /**
  10. * Creates a new identity quat
  11. *
  12. * @returns {quat} a new quaternion
  13. */
  14. export function create() {
  15. let out = new glMatrix.ARRAY_TYPE(4);
  16. if(glMatrix.ARRAY_TYPE != Float32Array) {
  17. out[0] = 0;
  18. out[1] = 0;
  19. out[2] = 0;
  20. }
  21. out[3] = 1;
  22. return out;
  23. }
  24. /**
  25. * Set a quat to the identity quaternion
  26. *
  27. * @param {quat} out the receiving quaternion
  28. * @returns {quat} out
  29. */
  30. export function identity(out) {
  31. out[0] = 0;
  32. out[1] = 0;
  33. out[2] = 0;
  34. out[3] = 1;
  35. return out;
  36. }
  37. /**
  38. * Sets a quat from the given angle and rotation axis,
  39. * then returns it.
  40. *
  41. * @param {quat} out the receiving quaternion
  42. * @param {vec3} axis the axis around which to rotate
  43. * @param {Number} rad the angle in radians
  44. * @returns {quat} out
  45. **/
  46. export function setAxisAngle(out, axis, rad) {
  47. rad = rad * 0.5;
  48. let s = Math.sin(rad);
  49. out[0] = s * axis[0];
  50. out[1] = s * axis[1];
  51. out[2] = s * axis[2];
  52. out[3] = Math.cos(rad);
  53. return out;
  54. }
  55. /**
  56. * Gets the rotation axis and angle for a given
  57. * quaternion. If a quaternion is created with
  58. * setAxisAngle, this method will return the same
  59. * values as providied in the original parameter list
  60. * OR functionally equivalent values.
  61. * Example: The quaternion formed by axis [0, 0, 1] and
  62. * angle -90 is the same as the quaternion formed by
  63. * [0, 0, 1] and 270. This method favors the latter.
  64. * @param {vec3} out_axis Vector receiving the axis of rotation
  65. * @param {quat} q Quaternion to be decomposed
  66. * @return {Number} Angle, in radians, of the rotation
  67. */
  68. export function getAxisAngle(out_axis, q) {
  69. let rad = Math.acos(q[3]) * 2.0;
  70. let s = Math.sin(rad / 2.0);
  71. if (s > glMatrix.EPSILON) {
  72. out_axis[0] = q[0] / s;
  73. out_axis[1] = q[1] / s;
  74. out_axis[2] = q[2] / s;
  75. } else {
  76. // If s is zero, return any axis (no rotation - axis does not matter)
  77. out_axis[0] = 1;
  78. out_axis[1] = 0;
  79. out_axis[2] = 0;
  80. }
  81. return rad;
  82. }
  83. /**
  84. * Multiplies two quat's
  85. *
  86. * @param {quat} out the receiving quaternion
  87. * @param {quat} a the first operand
  88. * @param {quat} b the second operand
  89. * @returns {quat} out
  90. */
  91. export function multiply(out, a, b) {
  92. let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  93. let bx = b[0], by = b[1], bz = b[2], bw = b[3];
  94. out[0] = ax * bw + aw * bx + ay * bz - az * by;
  95. out[1] = ay * bw + aw * by + az * bx - ax * bz;
  96. out[2] = az * bw + aw * bz + ax * by - ay * bx;
  97. out[3] = aw * bw - ax * bx - ay * by - az * bz;
  98. return out;
  99. }
  100. /**
  101. * Rotates a quaternion by the given angle about the X axis
  102. *
  103. * @param {quat} out quat receiving operation result
  104. * @param {quat} a quat to rotate
  105. * @param {number} rad angle (in radians) to rotate
  106. * @returns {quat} out
  107. */
  108. export function rotateX(out, a, rad) {
  109. rad *= 0.5;
  110. let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  111. let bx = Math.sin(rad), bw = Math.cos(rad);
  112. out[0] = ax * bw + aw * bx;
  113. out[1] = ay * bw + az * bx;
  114. out[2] = az * bw - ay * bx;
  115. out[3] = aw * bw - ax * bx;
  116. return out;
  117. }
  118. /**
  119. * Rotates a quaternion by the given angle about the Y axis
  120. *
  121. * @param {quat} out quat receiving operation result
  122. * @param {quat} a quat to rotate
  123. * @param {number} rad angle (in radians) to rotate
  124. * @returns {quat} out
  125. */
  126. export function rotateY(out, a, rad) {
  127. rad *= 0.5;
  128. let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  129. let by = Math.sin(rad), bw = Math.cos(rad);
  130. out[0] = ax * bw - az * by;
  131. out[1] = ay * bw + aw * by;
  132. out[2] = az * bw + ax * by;
  133. out[3] = aw * bw - ay * by;
  134. return out;
  135. }
  136. /**
  137. * Rotates a quaternion by the given angle about the Z axis
  138. *
  139. * @param {quat} out quat receiving operation result
  140. * @param {quat} a quat to rotate
  141. * @param {number} rad angle (in radians) to rotate
  142. * @returns {quat} out
  143. */
  144. export function rotateZ(out, a, rad) {
  145. rad *= 0.5;
  146. let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  147. let bz = Math.sin(rad), bw = Math.cos(rad);
  148. out[0] = ax * bw + ay * bz;
  149. out[1] = ay * bw - ax * bz;
  150. out[2] = az * bw + aw * bz;
  151. out[3] = aw * bw - az * bz;
  152. return out;
  153. }
  154. /**
  155. * Calculates the W component of a quat from the X, Y, and Z components.
  156. * Assumes that quaternion is 1 unit in length.
  157. * Any existing W component will be ignored.
  158. *
  159. * @param {quat} out the receiving quaternion
  160. * @param {quat} a quat to calculate W component of
  161. * @returns {quat} out
  162. */
  163. export function calculateW(out, a) {
  164. let x = a[0], y = a[1], z = a[2];
  165. out[0] = x;
  166. out[1] = y;
  167. out[2] = z;
  168. out[3] = Math.sqrt(Math.abs(1.0 - x * x - y * y - z * z));
  169. return out;
  170. }
  171. /**
  172. * Performs a spherical linear interpolation between two quat
  173. *
  174. * @param {quat} out the receiving quaternion
  175. * @param {quat} a the first operand
  176. * @param {quat} b the second operand
  177. * @param {Number} t interpolation amount, in the range [0-1], between the two inputs
  178. * @returns {quat} out
  179. */
  180. export function slerp(out, a, b, t) {
  181. // benchmarks:
  182. // http://jsperf.com/quaternion-slerp-implementations
  183. let ax = a[0], ay = a[1], az = a[2], aw = a[3];
  184. let bx = b[0], by = b[1], bz = b[2], bw = b[3];
  185. let omega, cosom, sinom, scale0, scale1;
  186. // calc cosine
  187. cosom = ax * bx + ay * by + az * bz + aw * bw;
  188. // adjust signs (if necessary)
  189. if ( cosom < 0.0 ) {
  190. cosom = -cosom;
  191. bx = - bx;
  192. by = - by;
  193. bz = - bz;
  194. bw = - bw;
  195. }
  196. // calculate coefficients
  197. if ( (1.0 - cosom) > glMatrix.EPSILON ) {
  198. // standard case (slerp)
  199. omega = Math.acos(cosom);
  200. sinom = Math.sin(omega);
  201. scale0 = Math.sin((1.0 - t) * omega) / sinom;
  202. scale1 = Math.sin(t * omega) / sinom;
  203. } else {
  204. // "from" and "to" quaternions are very close
  205. // ... so we can do a linear interpolation
  206. scale0 = 1.0 - t;
  207. scale1 = t;
  208. }
  209. // calculate final values
  210. out[0] = scale0 * ax + scale1 * bx;
  211. out[1] = scale0 * ay + scale1 * by;
  212. out[2] = scale0 * az + scale1 * bz;
  213. out[3] = scale0 * aw + scale1 * bw;
  214. return out;
  215. }
  216. /**
  217. * Generates a random quaternion
  218. *
  219. * @param {quat} out the receiving quaternion
  220. * @returns {quat} out
  221. */
  222. export function random(out) {
  223. // Implementation of http://planning.cs.uiuc.edu/node198.html
  224. // TODO: Calling random 3 times is probably not the fastest solution
  225. let u1 = glMatrix.RANDOM();
  226. let u2 = glMatrix.RANDOM();
  227. let u3 = glMatrix.RANDOM();
  228. let sqrt1MinusU1 = Math.sqrt(1 - u1);
  229. let sqrtU1 = Math.sqrt(u1);
  230. out[0] = sqrt1MinusU1 * Math.sin(2.0 * Math.PI * u2);
  231. out[1] = sqrt1MinusU1 * Math.cos(2.0 * Math.PI * u2);
  232. out[2] = sqrtU1 * Math.sin(2.0 * Math.PI * u3);
  233. out[3] = sqrtU1 * Math.cos(2.0 * Math.PI * u3);
  234. return out;
  235. }
  236. /**
  237. * Calculates the inverse of a quat
  238. *
  239. * @param {quat} out the receiving quaternion
  240. * @param {quat} a quat to calculate inverse of
  241. * @returns {quat} out
  242. */
  243. export function invert(out, a) {
  244. let a0 = a[0], a1 = a[1], a2 = a[2], a3 = a[3];
  245. let dot = a0*a0 + a1*a1 + a2*a2 + a3*a3;
  246. let invDot = dot ? 1.0/dot : 0;
  247. // TODO: Would be faster to return [0,0,0,0] immediately if dot == 0
  248. out[0] = -a0*invDot;
  249. out[1] = -a1*invDot;
  250. out[2] = -a2*invDot;
  251. out[3] = a3*invDot;
  252. return out;
  253. }
  254. /**
  255. * Calculates the conjugate of a quat
  256. * If the quaternion is normalized, this function is faster than quat.inverse and produces the same result.
  257. *
  258. * @param {quat} out the receiving quaternion
  259. * @param {quat} a quat to calculate conjugate of
  260. * @returns {quat} out
  261. */
  262. export function conjugate(out, a) {
  263. out[0] = -a[0];
  264. out[1] = -a[1];
  265. out[2] = -a[2];
  266. out[3] = a[3];
  267. return out;
  268. }
  269. /**
  270. * Creates a quaternion from the given 3x3 rotation matrix.
  271. *
  272. * NOTE: The resultant quaternion is not normalized, so you should be sure
  273. * to renormalize the quaternion yourself where necessary.
  274. *
  275. * @param {quat} out the receiving quaternion
  276. * @param {mat3} m rotation matrix
  277. * @returns {quat} out
  278. * @function
  279. */
  280. export function fromMat3(out, m) {
  281. // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  282. // article "Quaternion Calculus and Fast Animation".
  283. let fTrace = m[0] + m[4] + m[8];
  284. let fRoot;
  285. if ( fTrace > 0.0 ) {
  286. // |w| > 1/2, may as well choose w > 1/2
  287. fRoot = Math.sqrt(fTrace + 1.0); // 2w
  288. out[3] = 0.5 * fRoot;
  289. fRoot = 0.5/fRoot; // 1/(4w)
  290. out[0] = (m[5]-m[7])*fRoot;
  291. out[1] = (m[6]-m[2])*fRoot;
  292. out[2] = (m[1]-m[3])*fRoot;
  293. } else {
  294. // |w| <= 1/2
  295. let i = 0;
  296. if ( m[4] > m[0] )
  297. i = 1;
  298. if ( m[8] > m[i*3+i] )
  299. i = 2;
  300. let j = (i+1)%3;
  301. let k = (i+2)%3;
  302. fRoot = Math.sqrt(m[i*3+i]-m[j*3+j]-m[k*3+k] + 1.0);
  303. out[i] = 0.5 * fRoot;
  304. fRoot = 0.5 / fRoot;
  305. out[3] = (m[j*3+k] - m[k*3+j]) * fRoot;
  306. out[j] = (m[j*3+i] + m[i*3+j]) * fRoot;
  307. out[k] = (m[k*3+i] + m[i*3+k]) * fRoot;
  308. }
  309. return out;
  310. }
  311. /**
  312. * Creates a quaternion from the given euler angle x, y, z.
  313. *
  314. * @param {quat} out the receiving quaternion
  315. * @param {x} Angle to rotate around X axis in degrees.
  316. * @param {y} Angle to rotate around Y axis in degrees.
  317. * @param {z} Angle to rotate around Z axis in degrees.
  318. * @returns {quat} out
  319. * @function
  320. */
  321. export function fromEuler(out, x, y, z) {
  322. let halfToRad = 0.5 * Math.PI / 180.0;
  323. x *= halfToRad;
  324. y *= halfToRad;
  325. z *= halfToRad;
  326. let sx = Math.sin(x);
  327. let cx = Math.cos(x);
  328. let sy = Math.sin(y);
  329. let cy = Math.cos(y);
  330. let sz = Math.sin(z);
  331. let cz = Math.cos(z);
  332. out[0] = sx * cy * cz - cx * sy * sz;
  333. out[1] = cx * sy * cz + sx * cy * sz;
  334. out[2] = cx * cy * sz - sx * sy * cz;
  335. out[3] = cx * cy * cz + sx * sy * sz;
  336. return out;
  337. }
  338. /**
  339. * Returns a string representation of a quatenion
  340. *
  341. * @param {quat} a vector to represent as a string
  342. * @returns {String} string representation of the vector
  343. */
  344. export function str(a) {
  345. return 'quat(' + a[0] + ', ' + a[1] + ', ' + a[2] + ', ' + a[3] + ')';
  346. }
  347. /**
  348. * Creates a new quat initialized with values from an existing quaternion
  349. *
  350. * @param {quat} a quaternion to clone
  351. * @returns {quat} a new quaternion
  352. * @function
  353. */
  354. export const clone = vec4.clone;
  355. /**
  356. * Creates a new quat initialized with the given values
  357. *
  358. * @param {Number} x X component
  359. * @param {Number} y Y component
  360. * @param {Number} z Z component
  361. * @param {Number} w W component
  362. * @returns {quat} a new quaternion
  363. * @function
  364. */
  365. export const fromValues = vec4.fromValues;
  366. /**
  367. * Copy the values from one quat to another
  368. *
  369. * @param {quat} out the receiving quaternion
  370. * @param {quat} a the source quaternion
  371. * @returns {quat} out
  372. * @function
  373. */
  374. export const copy = vec4.copy;
  375. /**
  376. * Set the components of a quat to the given values
  377. *
  378. * @param {quat} out the receiving quaternion
  379. * @param {Number} x X component
  380. * @param {Number} y Y component
  381. * @param {Number} z Z component
  382. * @param {Number} w W component
  383. * @returns {quat} out
  384. * @function
  385. */
  386. export const set = vec4.set;
  387. /**
  388. * Adds two quat's
  389. *
  390. * @param {quat} out the receiving quaternion
  391. * @param {quat} a the first operand
  392. * @param {quat} b the second operand
  393. * @returns {quat} out
  394. * @function
  395. */
  396. export const add = vec4.add;
  397. /**
  398. * Alias for {@link quat.multiply}
  399. * @function
  400. */
  401. export const mul = multiply;
  402. /**
  403. * Scales a quat by a scalar number
  404. *
  405. * @param {quat} out the receiving vector
  406. * @param {quat} a the vector to scale
  407. * @param {Number} b amount to scale the vector by
  408. * @returns {quat} out
  409. * @function
  410. */
  411. export const scale = vec4.scale;
  412. /**
  413. * Calculates the dot product of two quat's
  414. *
  415. * @param {quat} a the first operand
  416. * @param {quat} b the second operand
  417. * @returns {Number} dot product of a and b
  418. * @function
  419. */
  420. export const dot = vec4.dot;
  421. /**
  422. * Performs a linear interpolation between two quat's
  423. *
  424. * @param {quat} out the receiving quaternion
  425. * @param {quat} a the first operand
  426. * @param {quat} b the second operand
  427. * @param {Number} t interpolation amount, in the range [0-1], between the two inputs
  428. * @returns {quat} out
  429. * @function
  430. */
  431. export const lerp = vec4.lerp;
  432. /**
  433. * Calculates the length of a quat
  434. *
  435. * @param {quat} a vector to calculate length of
  436. * @returns {Number} length of a
  437. */
  438. export const length = vec4.length;
  439. /**
  440. * Alias for {@link quat.length}
  441. * @function
  442. */
  443. export const len = length;
  444. /**
  445. * Calculates the squared length of a quat
  446. *
  447. * @param {quat} a vector to calculate squared length of
  448. * @returns {Number} squared length of a
  449. * @function
  450. */
  451. export const squaredLength = vec4.squaredLength;
  452. /**
  453. * Alias for {@link quat.squaredLength}
  454. * @function
  455. */
  456. export const sqrLen = squaredLength;
  457. /**
  458. * Normalize a quat
  459. *
  460. * @param {quat} out the receiving quaternion
  461. * @param {quat} a quaternion to normalize
  462. * @returns {quat} out
  463. * @function
  464. */
  465. export const normalize = vec4.normalize;
  466. /**
  467. * Returns whether or not the quaternions have exactly the same elements in the same position (when compared with ===)
  468. *
  469. * @param {quat} a The first quaternion.
  470. * @param {quat} b The second quaternion.
  471. * @returns {Boolean} True if the vectors are equal, false otherwise.
  472. */
  473. export const exactEquals = vec4.exactEquals;
  474. /**
  475. * Returns whether or not the quaternions have approximately the same elements in the same position.
  476. *
  477. * @param {quat} a The first vector.
  478. * @param {quat} b The second vector.
  479. * @returns {Boolean} True if the vectors are equal, false otherwise.
  480. */
  481. export const equals = vec4.equals;
  482. /**
  483. * Sets a quaternion to represent the shortest rotation from one
  484. * vector to another.
  485. *
  486. * Both vectors are assumed to be unit length.
  487. *
  488. * @param {quat} out the receiving quaternion.
  489. * @param {vec3} a the initial vector
  490. * @param {vec3} b the destination vector
  491. * @returns {quat} out
  492. */
  493. export const rotationTo = (function() {
  494. let tmpvec3 = vec3.create();
  495. let xUnitVec3 = vec3.fromValues(1,0,0);
  496. let yUnitVec3 = vec3.fromValues(0,1,0);
  497. return function(out, a, b) {
  498. let dot = vec3.dot(a, b);
  499. if (dot < -0.999999) {
  500. vec3.cross(tmpvec3, xUnitVec3, a);
  501. if (vec3.len(tmpvec3) < 0.000001)
  502. vec3.cross(tmpvec3, yUnitVec3, a);
  503. vec3.normalize(tmpvec3, tmpvec3);
  504. setAxisAngle(out, tmpvec3, Math.PI);
  505. return out;
  506. } else if (dot > 0.999999) {
  507. out[0] = 0;
  508. out[1] = 0;
  509. out[2] = 0;
  510. out[3] = 1;
  511. return out;
  512. } else {
  513. vec3.cross(tmpvec3, a, b);
  514. out[0] = tmpvec3[0];
  515. out[1] = tmpvec3[1];
  516. out[2] = tmpvec3[2];
  517. out[3] = 1 + dot;
  518. return normalize(out, out);
  519. }
  520. };
  521. })();
  522. /**
  523. * Performs a spherical linear interpolation with two control points
  524. *
  525. * @param {quat} out the receiving quaternion
  526. * @param {quat} a the first operand
  527. * @param {quat} b the second operand
  528. * @param {quat} c the third operand
  529. * @param {quat} d the fourth operand
  530. * @param {Number} t interpolation amount, in the range [0-1], between the two inputs
  531. * @returns {quat} out
  532. */
  533. export const sqlerp = (function () {
  534. let temp1 = create();
  535. let temp2 = create();
  536. return function (out, a, b, c, d, t) {
  537. slerp(temp1, a, d, t);
  538. slerp(temp2, b, c, t);
  539. slerp(out, temp1, temp2, 2 * t * (1 - t));
  540. return out;
  541. };
  542. }());
  543. /**
  544. * Sets the specified quaternion with values corresponding to the given
  545. * axes. Each axis is a vec3 and is expected to be unit length and
  546. * perpendicular to all other specified axes.
  547. *
  548. * @param {vec3} view the vector representing the viewing direction
  549. * @param {vec3} right the vector representing the local "right" direction
  550. * @param {vec3} up the vector representing the local "up" direction
  551. * @returns {quat} out
  552. */
  553. export const setAxes = (function() {
  554. let matr = mat3.create();
  555. return function(out, view, right, up) {
  556. matr[0] = right[0];
  557. matr[3] = right[1];
  558. matr[6] = right[2];
  559. matr[1] = up[0];
  560. matr[4] = up[1];
  561. matr[7] = up[2];
  562. matr[2] = -view[0];
  563. matr[5] = -view[1];
  564. matr[8] = -view[2];
  565. return normalize(out, fromMat3(out, matr));
  566. };
  567. })();