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