quat2.js 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835
  1. import * as glMatrix from "./common.js";
  2. import * as quat from "./quat.js";
  3. import * as mat4 from "./mat4.js";
  4. /**
  5. * Dual Quaternion<br>
  6. * Format: [real, dual]<br>
  7. * Quaternion format: XYZW<br>
  8. * Make sure to have normalized dual quaternions, otherwise the functions may not work as intended.<br>
  9. * @module quat2
  10. */
  11. /**
  12. * Creates a new identity dual quat
  13. *
  14. * @returns {quat2} a new dual quaternion [real -> rotation, dual -> translation]
  15. */
  16. export function create() {
  17. var dq = new glMatrix.ARRAY_TYPE(8);
  18. if (glMatrix.ARRAY_TYPE != Float32Array) {
  19. dq[0] = 0;
  20. dq[1] = 0;
  21. dq[2] = 0;
  22. dq[4] = 0;
  23. dq[5] = 0;
  24. dq[6] = 0;
  25. dq[7] = 0;
  26. }
  27. dq[3] = 1;
  28. return dq;
  29. }
  30. /**
  31. * Creates a new quat initialized with values from an existing quaternion
  32. *
  33. * @param {ReadonlyQuat2} a dual quaternion to clone
  34. * @returns {quat2} new dual quaternion
  35. * @function
  36. */
  37. export function clone(a) {
  38. var dq = new glMatrix.ARRAY_TYPE(8);
  39. dq[0] = a[0];
  40. dq[1] = a[1];
  41. dq[2] = a[2];
  42. dq[3] = a[3];
  43. dq[4] = a[4];
  44. dq[5] = a[5];
  45. dq[6] = a[6];
  46. dq[7] = a[7];
  47. return dq;
  48. }
  49. /**
  50. * Creates a new dual quat initialized with the given values
  51. *
  52. * @param {Number} x1 X component
  53. * @param {Number} y1 Y component
  54. * @param {Number} z1 Z component
  55. * @param {Number} w1 W component
  56. * @param {Number} x2 X component
  57. * @param {Number} y2 Y component
  58. * @param {Number} z2 Z component
  59. * @param {Number} w2 W component
  60. * @returns {quat2} new dual quaternion
  61. * @function
  62. */
  63. export function fromValues(x1, y1, z1, w1, x2, y2, z2, w2) {
  64. var dq = new glMatrix.ARRAY_TYPE(8);
  65. dq[0] = x1;
  66. dq[1] = y1;
  67. dq[2] = z1;
  68. dq[3] = w1;
  69. dq[4] = x2;
  70. dq[5] = y2;
  71. dq[6] = z2;
  72. dq[7] = w2;
  73. return dq;
  74. }
  75. /**
  76. * Creates a new dual quat from the given values (quat and translation)
  77. *
  78. * @param {Number} x1 X component
  79. * @param {Number} y1 Y component
  80. * @param {Number} z1 Z component
  81. * @param {Number} w1 W component
  82. * @param {Number} x2 X component (translation)
  83. * @param {Number} y2 Y component (translation)
  84. * @param {Number} z2 Z component (translation)
  85. * @returns {quat2} new dual quaternion
  86. * @function
  87. */
  88. export function fromRotationTranslationValues(x1, y1, z1, w1, x2, y2, z2) {
  89. var dq = new glMatrix.ARRAY_TYPE(8);
  90. dq[0] = x1;
  91. dq[1] = y1;
  92. dq[2] = z1;
  93. dq[3] = w1;
  94. var ax = x2 * 0.5,
  95. ay = y2 * 0.5,
  96. az = z2 * 0.5;
  97. dq[4] = ax * w1 + ay * z1 - az * y1;
  98. dq[5] = ay * w1 + az * x1 - ax * z1;
  99. dq[6] = az * w1 + ax * y1 - ay * x1;
  100. dq[7] = -ax * x1 - ay * y1 - az * z1;
  101. return dq;
  102. }
  103. /**
  104. * Creates a dual quat from a quaternion and a translation
  105. *
  106. * @param {ReadonlyQuat2} dual quaternion receiving operation result
  107. * @param {ReadonlyQuat} q a normalized quaternion
  108. * @param {ReadonlyVec3} t tranlation vector
  109. * @returns {quat2} dual quaternion receiving operation result
  110. * @function
  111. */
  112. export function fromRotationTranslation(out, q, t) {
  113. var ax = t[0] * 0.5,
  114. ay = t[1] * 0.5,
  115. az = t[2] * 0.5,
  116. bx = q[0],
  117. by = q[1],
  118. bz = q[2],
  119. bw = q[3];
  120. out[0] = bx;
  121. out[1] = by;
  122. out[2] = bz;
  123. out[3] = bw;
  124. out[4] = ax * bw + ay * bz - az * by;
  125. out[5] = ay * bw + az * bx - ax * bz;
  126. out[6] = az * bw + ax * by - ay * bx;
  127. out[7] = -ax * bx - ay * by - az * bz;
  128. return out;
  129. }
  130. /**
  131. * Creates a dual quat from a translation
  132. *
  133. * @param {ReadonlyQuat2} dual quaternion receiving operation result
  134. * @param {ReadonlyVec3} t translation vector
  135. * @returns {quat2} dual quaternion receiving operation result
  136. * @function
  137. */
  138. export function fromTranslation(out, t) {
  139. out[0] = 0;
  140. out[1] = 0;
  141. out[2] = 0;
  142. out[3] = 1;
  143. out[4] = t[0] * 0.5;
  144. out[5] = t[1] * 0.5;
  145. out[6] = t[2] * 0.5;
  146. out[7] = 0;
  147. return out;
  148. }
  149. /**
  150. * Creates a dual quat from a quaternion
  151. *
  152. * @param {ReadonlyQuat2} dual quaternion receiving operation result
  153. * @param {ReadonlyQuat} q the quaternion
  154. * @returns {quat2} dual quaternion receiving operation result
  155. * @function
  156. */
  157. export function fromRotation(out, q) {
  158. out[0] = q[0];
  159. out[1] = q[1];
  160. out[2] = q[2];
  161. out[3] = q[3];
  162. out[4] = 0;
  163. out[5] = 0;
  164. out[6] = 0;
  165. out[7] = 0;
  166. return out;
  167. }
  168. /**
  169. * Creates a new dual quat from a matrix (4x4)
  170. *
  171. * @param {quat2} out the dual quaternion
  172. * @param {ReadonlyMat4} a the matrix
  173. * @returns {quat2} dual quat receiving operation result
  174. * @function
  175. */
  176. export function fromMat4(out, a) {
  177. //TODO Optimize this
  178. var outer = quat.create();
  179. mat4.getRotation(outer, a);
  180. var t = new glMatrix.ARRAY_TYPE(3);
  181. mat4.getTranslation(t, a);
  182. fromRotationTranslation(out, outer, t);
  183. return out;
  184. }
  185. /**
  186. * Copy the values from one dual quat to another
  187. *
  188. * @param {quat2} out the receiving dual quaternion
  189. * @param {ReadonlyQuat2} a the source dual quaternion
  190. * @returns {quat2} out
  191. * @function
  192. */
  193. export function copy(out, a) {
  194. out[0] = a[0];
  195. out[1] = a[1];
  196. out[2] = a[2];
  197. out[3] = a[3];
  198. out[4] = a[4];
  199. out[5] = a[5];
  200. out[6] = a[6];
  201. out[7] = a[7];
  202. return out;
  203. }
  204. /**
  205. * Set a dual quat to the identity dual quaternion
  206. *
  207. * @param {quat2} out the receiving quaternion
  208. * @returns {quat2} out
  209. */
  210. export function identity(out) {
  211. out[0] = 0;
  212. out[1] = 0;
  213. out[2] = 0;
  214. out[3] = 1;
  215. out[4] = 0;
  216. out[5] = 0;
  217. out[6] = 0;
  218. out[7] = 0;
  219. return out;
  220. }
  221. /**
  222. * Set the components of a dual quat to the given values
  223. *
  224. * @param {quat2} out the receiving quaternion
  225. * @param {Number} x1 X component
  226. * @param {Number} y1 Y component
  227. * @param {Number} z1 Z component
  228. * @param {Number} w1 W component
  229. * @param {Number} x2 X component
  230. * @param {Number} y2 Y component
  231. * @param {Number} z2 Z component
  232. * @param {Number} w2 W component
  233. * @returns {quat2} out
  234. * @function
  235. */
  236. export function set(out, x1, y1, z1, w1, x2, y2, z2, w2) {
  237. out[0] = x1;
  238. out[1] = y1;
  239. out[2] = z1;
  240. out[3] = w1;
  241. out[4] = x2;
  242. out[5] = y2;
  243. out[6] = z2;
  244. out[7] = w2;
  245. return out;
  246. }
  247. /**
  248. * Gets the real part of a dual quat
  249. * @param {quat} out real part
  250. * @param {ReadonlyQuat2} a Dual Quaternion
  251. * @return {quat} real part
  252. */
  253. export var getReal = quat.copy;
  254. /**
  255. * Gets the dual part of a dual quat
  256. * @param {quat} out dual part
  257. * @param {ReadonlyQuat2} a Dual Quaternion
  258. * @return {quat} dual part
  259. */
  260. export function getDual(out, a) {
  261. out[0] = a[4];
  262. out[1] = a[5];
  263. out[2] = a[6];
  264. out[3] = a[7];
  265. return out;
  266. }
  267. /**
  268. * Set the real component of a dual quat to the given quaternion
  269. *
  270. * @param {quat2} out the receiving quaternion
  271. * @param {ReadonlyQuat} q a quaternion representing the real part
  272. * @returns {quat2} out
  273. * @function
  274. */
  275. export var setReal = quat.copy;
  276. /**
  277. * Set the dual component of a dual quat to the given quaternion
  278. *
  279. * @param {quat2} out the receiving quaternion
  280. * @param {ReadonlyQuat} q a quaternion representing the dual part
  281. * @returns {quat2} out
  282. * @function
  283. */
  284. export function setDual(out, q) {
  285. out[4] = q[0];
  286. out[5] = q[1];
  287. out[6] = q[2];
  288. out[7] = q[3];
  289. return out;
  290. }
  291. /**
  292. * Gets the translation of a normalized dual quat
  293. * @param {vec3} out translation
  294. * @param {ReadonlyQuat2} a Dual Quaternion to be decomposed
  295. * @return {vec3} translation
  296. */
  297. export function getTranslation(out, a) {
  298. var ax = a[4],
  299. ay = a[5],
  300. az = a[6],
  301. aw = a[7],
  302. bx = -a[0],
  303. by = -a[1],
  304. bz = -a[2],
  305. bw = a[3];
  306. out[0] = (ax * bw + aw * bx + ay * bz - az * by) * 2;
  307. out[1] = (ay * bw + aw * by + az * bx - ax * bz) * 2;
  308. out[2] = (az * bw + aw * bz + ax * by - ay * bx) * 2;
  309. return out;
  310. }
  311. /**
  312. * Translates a dual quat by the given vector
  313. *
  314. * @param {quat2} out the receiving dual quaternion
  315. * @param {ReadonlyQuat2} a the dual quaternion to translate
  316. * @param {ReadonlyVec3} v vector to translate by
  317. * @returns {quat2} out
  318. */
  319. export function translate(out, a, v) {
  320. var ax1 = a[0],
  321. ay1 = a[1],
  322. az1 = a[2],
  323. aw1 = a[3],
  324. bx1 = v[0] * 0.5,
  325. by1 = v[1] * 0.5,
  326. bz1 = v[2] * 0.5,
  327. ax2 = a[4],
  328. ay2 = a[5],
  329. az2 = a[6],
  330. aw2 = a[7];
  331. out[0] = ax1;
  332. out[1] = ay1;
  333. out[2] = az1;
  334. out[3] = aw1;
  335. out[4] = aw1 * bx1 + ay1 * bz1 - az1 * by1 + ax2;
  336. out[5] = aw1 * by1 + az1 * bx1 - ax1 * bz1 + ay2;
  337. out[6] = aw1 * bz1 + ax1 * by1 - ay1 * bx1 + az2;
  338. out[7] = -ax1 * bx1 - ay1 * by1 - az1 * bz1 + aw2;
  339. return out;
  340. }
  341. /**
  342. * Rotates a dual quat around the X axis
  343. *
  344. * @param {quat2} out the receiving dual quaternion
  345. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  346. * @param {number} rad how far should the rotation be
  347. * @returns {quat2} out
  348. */
  349. export function rotateX(out, a, rad) {
  350. var bx = -a[0],
  351. by = -a[1],
  352. bz = -a[2],
  353. bw = a[3],
  354. ax = a[4],
  355. ay = a[5],
  356. az = a[6],
  357. aw = a[7],
  358. ax1 = ax * bw + aw * bx + ay * bz - az * by,
  359. ay1 = ay * bw + aw * by + az * bx - ax * bz,
  360. az1 = az * bw + aw * bz + ax * by - ay * bx,
  361. aw1 = aw * bw - ax * bx - ay * by - az * bz;
  362. quat.rotateX(out, a, rad);
  363. bx = out[0];
  364. by = out[1];
  365. bz = out[2];
  366. bw = out[3];
  367. out[4] = ax1 * bw + aw1 * bx + ay1 * bz - az1 * by;
  368. out[5] = ay1 * bw + aw1 * by + az1 * bx - ax1 * bz;
  369. out[6] = az1 * bw + aw1 * bz + ax1 * by - ay1 * bx;
  370. out[7] = aw1 * bw - ax1 * bx - ay1 * by - az1 * bz;
  371. return out;
  372. }
  373. /**
  374. * Rotates a dual quat around the Y axis
  375. *
  376. * @param {quat2} out the receiving dual quaternion
  377. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  378. * @param {number} rad how far should the rotation be
  379. * @returns {quat2} out
  380. */
  381. export function rotateY(out, a, rad) {
  382. var bx = -a[0],
  383. by = -a[1],
  384. bz = -a[2],
  385. bw = a[3],
  386. ax = a[4],
  387. ay = a[5],
  388. az = a[6],
  389. aw = a[7],
  390. ax1 = ax * bw + aw * bx + ay * bz - az * by,
  391. ay1 = ay * bw + aw * by + az * bx - ax * bz,
  392. az1 = az * bw + aw * bz + ax * by - ay * bx,
  393. aw1 = aw * bw - ax * bx - ay * by - az * bz;
  394. quat.rotateY(out, a, rad);
  395. bx = out[0];
  396. by = out[1];
  397. bz = out[2];
  398. bw = out[3];
  399. out[4] = ax1 * bw + aw1 * bx + ay1 * bz - az1 * by;
  400. out[5] = ay1 * bw + aw1 * by + az1 * bx - ax1 * bz;
  401. out[6] = az1 * bw + aw1 * bz + ax1 * by - ay1 * bx;
  402. out[7] = aw1 * bw - ax1 * bx - ay1 * by - az1 * bz;
  403. return out;
  404. }
  405. /**
  406. * Rotates a dual quat around the Z axis
  407. *
  408. * @param {quat2} out the receiving dual quaternion
  409. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  410. * @param {number} rad how far should the rotation be
  411. * @returns {quat2} out
  412. */
  413. export function rotateZ(out, a, rad) {
  414. var bx = -a[0],
  415. by = -a[1],
  416. bz = -a[2],
  417. bw = a[3],
  418. ax = a[4],
  419. ay = a[5],
  420. az = a[6],
  421. aw = a[7],
  422. ax1 = ax * bw + aw * bx + ay * bz - az * by,
  423. ay1 = ay * bw + aw * by + az * bx - ax * bz,
  424. az1 = az * bw + aw * bz + ax * by - ay * bx,
  425. aw1 = aw * bw - ax * bx - ay * by - az * bz;
  426. quat.rotateZ(out, a, rad);
  427. bx = out[0];
  428. by = out[1];
  429. bz = out[2];
  430. bw = out[3];
  431. out[4] = ax1 * bw + aw1 * bx + ay1 * bz - az1 * by;
  432. out[5] = ay1 * bw + aw1 * by + az1 * bx - ax1 * bz;
  433. out[6] = az1 * bw + aw1 * bz + ax1 * by - ay1 * bx;
  434. out[7] = aw1 * bw - ax1 * bx - ay1 * by - az1 * bz;
  435. return out;
  436. }
  437. /**
  438. * Rotates a dual quat by a given quaternion (a * q)
  439. *
  440. * @param {quat2} out the receiving dual quaternion
  441. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  442. * @param {ReadonlyQuat} q quaternion to rotate by
  443. * @returns {quat2} out
  444. */
  445. export function rotateByQuatAppend(out, a, q) {
  446. var qx = q[0],
  447. qy = q[1],
  448. qz = q[2],
  449. qw = q[3],
  450. ax = a[0],
  451. ay = a[1],
  452. az = a[2],
  453. aw = a[3];
  454. out[0] = ax * qw + aw * qx + ay * qz - az * qy;
  455. out[1] = ay * qw + aw * qy + az * qx - ax * qz;
  456. out[2] = az * qw + aw * qz + ax * qy - ay * qx;
  457. out[3] = aw * qw - ax * qx - ay * qy - az * qz;
  458. ax = a[4];
  459. ay = a[5];
  460. az = a[6];
  461. aw = a[7];
  462. out[4] = ax * qw + aw * qx + ay * qz - az * qy;
  463. out[5] = ay * qw + aw * qy + az * qx - ax * qz;
  464. out[6] = az * qw + aw * qz + ax * qy - ay * qx;
  465. out[7] = aw * qw - ax * qx - ay * qy - az * qz;
  466. return out;
  467. }
  468. /**
  469. * Rotates a dual quat by a given quaternion (q * a)
  470. *
  471. * @param {quat2} out the receiving dual quaternion
  472. * @param {ReadonlyQuat} q quaternion to rotate by
  473. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  474. * @returns {quat2} out
  475. */
  476. export function rotateByQuatPrepend(out, q, a) {
  477. var qx = q[0],
  478. qy = q[1],
  479. qz = q[2],
  480. qw = q[3],
  481. bx = a[0],
  482. by = a[1],
  483. bz = a[2],
  484. bw = a[3];
  485. out[0] = qx * bw + qw * bx + qy * bz - qz * by;
  486. out[1] = qy * bw + qw * by + qz * bx - qx * bz;
  487. out[2] = qz * bw + qw * bz + qx * by - qy * bx;
  488. out[3] = qw * bw - qx * bx - qy * by - qz * bz;
  489. bx = a[4];
  490. by = a[5];
  491. bz = a[6];
  492. bw = a[7];
  493. out[4] = qx * bw + qw * bx + qy * bz - qz * by;
  494. out[5] = qy * bw + qw * by + qz * bx - qx * bz;
  495. out[6] = qz * bw + qw * bz + qx * by - qy * bx;
  496. out[7] = qw * bw - qx * bx - qy * by - qz * bz;
  497. return out;
  498. }
  499. /**
  500. * Rotates a dual quat around a given axis. Does the normalisation automatically
  501. *
  502. * @param {quat2} out the receiving dual quaternion
  503. * @param {ReadonlyQuat2} a the dual quaternion to rotate
  504. * @param {ReadonlyVec3} axis the axis to rotate around
  505. * @param {Number} rad how far the rotation should be
  506. * @returns {quat2} out
  507. */
  508. export function rotateAroundAxis(out, a, axis, rad) {
  509. //Special case for rad = 0
  510. if (Math.abs(rad) < glMatrix.EPSILON) {
  511. return copy(out, a);
  512. }
  513. var axisLength = Math.hypot(axis[0], axis[1], axis[2]);
  514. rad = rad * 0.5;
  515. var s = Math.sin(rad);
  516. var bx = s * axis[0] / axisLength;
  517. var by = s * axis[1] / axisLength;
  518. var bz = s * axis[2] / axisLength;
  519. var bw = Math.cos(rad);
  520. var ax1 = a[0],
  521. ay1 = a[1],
  522. az1 = a[2],
  523. aw1 = a[3];
  524. out[0] = ax1 * bw + aw1 * bx + ay1 * bz - az1 * by;
  525. out[1] = ay1 * bw + aw1 * by + az1 * bx - ax1 * bz;
  526. out[2] = az1 * bw + aw1 * bz + ax1 * by - ay1 * bx;
  527. out[3] = aw1 * bw - ax1 * bx - ay1 * by - az1 * bz;
  528. var ax = a[4],
  529. ay = a[5],
  530. az = a[6],
  531. aw = a[7];
  532. out[4] = ax * bw + aw * bx + ay * bz - az * by;
  533. out[5] = ay * bw + aw * by + az * bx - ax * bz;
  534. out[6] = az * bw + aw * bz + ax * by - ay * bx;
  535. out[7] = aw * bw - ax * bx - ay * by - az * bz;
  536. return out;
  537. }
  538. /**
  539. * Adds two dual quat's
  540. *
  541. * @param {quat2} out the receiving dual quaternion
  542. * @param {ReadonlyQuat2} a the first operand
  543. * @param {ReadonlyQuat2} b the second operand
  544. * @returns {quat2} out
  545. * @function
  546. */
  547. export function add(out, a, b) {
  548. out[0] = a[0] + b[0];
  549. out[1] = a[1] + b[1];
  550. out[2] = a[2] + b[2];
  551. out[3] = a[3] + b[3];
  552. out[4] = a[4] + b[4];
  553. out[5] = a[5] + b[5];
  554. out[6] = a[6] + b[6];
  555. out[7] = a[7] + b[7];
  556. return out;
  557. }
  558. /**
  559. * Multiplies two dual quat's
  560. *
  561. * @param {quat2} out the receiving dual quaternion
  562. * @param {ReadonlyQuat2} a the first operand
  563. * @param {ReadonlyQuat2} b the second operand
  564. * @returns {quat2} out
  565. */
  566. export function multiply(out, a, b) {
  567. var ax0 = a[0],
  568. ay0 = a[1],
  569. az0 = a[2],
  570. aw0 = a[3],
  571. bx1 = b[4],
  572. by1 = b[5],
  573. bz1 = b[6],
  574. bw1 = b[7],
  575. ax1 = a[4],
  576. ay1 = a[5],
  577. az1 = a[6],
  578. aw1 = a[7],
  579. bx0 = b[0],
  580. by0 = b[1],
  581. bz0 = b[2],
  582. bw0 = b[3];
  583. out[0] = ax0 * bw0 + aw0 * bx0 + ay0 * bz0 - az0 * by0;
  584. out[1] = ay0 * bw0 + aw0 * by0 + az0 * bx0 - ax0 * bz0;
  585. out[2] = az0 * bw0 + aw0 * bz0 + ax0 * by0 - ay0 * bx0;
  586. out[3] = aw0 * bw0 - ax0 * bx0 - ay0 * by0 - az0 * bz0;
  587. out[4] = ax0 * bw1 + aw0 * bx1 + ay0 * bz1 - az0 * by1 + ax1 * bw0 + aw1 * bx0 + ay1 * bz0 - az1 * by0;
  588. out[5] = ay0 * bw1 + aw0 * by1 + az0 * bx1 - ax0 * bz1 + ay1 * bw0 + aw1 * by0 + az1 * bx0 - ax1 * bz0;
  589. out[6] = az0 * bw1 + aw0 * bz1 + ax0 * by1 - ay0 * bx1 + az1 * bw0 + aw1 * bz0 + ax1 * by0 - ay1 * bx0;
  590. out[7] = aw0 * bw1 - ax0 * bx1 - ay0 * by1 - az0 * bz1 + aw1 * bw0 - ax1 * bx0 - ay1 * by0 - az1 * bz0;
  591. return out;
  592. }
  593. /**
  594. * Alias for {@link quat2.multiply}
  595. * @function
  596. */
  597. export var mul = multiply;
  598. /**
  599. * Scales a dual quat by a scalar number
  600. *
  601. * @param {quat2} out the receiving dual quat
  602. * @param {ReadonlyQuat2} a the dual quat to scale
  603. * @param {Number} b amount to scale the dual quat by
  604. * @returns {quat2} out
  605. * @function
  606. */
  607. export function scale(out, a, b) {
  608. out[0] = a[0] * b;
  609. out[1] = a[1] * b;
  610. out[2] = a[2] * b;
  611. out[3] = a[3] * b;
  612. out[4] = a[4] * b;
  613. out[5] = a[5] * b;
  614. out[6] = a[6] * b;
  615. out[7] = a[7] * b;
  616. return out;
  617. }
  618. /**
  619. * Calculates the dot product of two dual quat's (The dot product of the real parts)
  620. *
  621. * @param {ReadonlyQuat2} a the first operand
  622. * @param {ReadonlyQuat2} b the second operand
  623. * @returns {Number} dot product of a and b
  624. * @function
  625. */
  626. export var dot = quat.dot;
  627. /**
  628. * Performs a linear interpolation between two dual quats's
  629. * NOTE: The resulting dual quaternions won't always be normalized (The error is most noticeable when t = 0.5)
  630. *
  631. * @param {quat2} out the receiving dual quat
  632. * @param {ReadonlyQuat2} a the first operand
  633. * @param {ReadonlyQuat2} b the second operand
  634. * @param {Number} t interpolation amount, in the range [0-1], between the two inputs
  635. * @returns {quat2} out
  636. */
  637. export function lerp(out, a, b, t) {
  638. var mt = 1 - t;
  639. if (dot(a, b) < 0) t = -t;
  640. out[0] = a[0] * mt + b[0] * t;
  641. out[1] = a[1] * mt + b[1] * t;
  642. out[2] = a[2] * mt + b[2] * t;
  643. out[3] = a[3] * mt + b[3] * t;
  644. out[4] = a[4] * mt + b[4] * t;
  645. out[5] = a[5] * mt + b[5] * t;
  646. out[6] = a[6] * mt + b[6] * t;
  647. out[7] = a[7] * mt + b[7] * t;
  648. return out;
  649. }
  650. /**
  651. * Calculates the inverse of a dual quat. If they are normalized, conjugate is cheaper
  652. *
  653. * @param {quat2} out the receiving dual quaternion
  654. * @param {ReadonlyQuat2} a dual quat to calculate inverse of
  655. * @returns {quat2} out
  656. */
  657. export function invert(out, a) {
  658. var sqlen = squaredLength(a);
  659. out[0] = -a[0] / sqlen;
  660. out[1] = -a[1] / sqlen;
  661. out[2] = -a[2] / sqlen;
  662. out[3] = a[3] / sqlen;
  663. out[4] = -a[4] / sqlen;
  664. out[5] = -a[5] / sqlen;
  665. out[6] = -a[6] / sqlen;
  666. out[7] = a[7] / sqlen;
  667. return out;
  668. }
  669. /**
  670. * Calculates the conjugate of a dual quat
  671. * If the dual quaternion is normalized, this function is faster than quat2.inverse and produces the same result.
  672. *
  673. * @param {quat2} out the receiving quaternion
  674. * @param {ReadonlyQuat2} a quat to calculate conjugate of
  675. * @returns {quat2} out
  676. */
  677. export function conjugate(out, a) {
  678. out[0] = -a[0];
  679. out[1] = -a[1];
  680. out[2] = -a[2];
  681. out[3] = a[3];
  682. out[4] = -a[4];
  683. out[5] = -a[5];
  684. out[6] = -a[6];
  685. out[7] = a[7];
  686. return out;
  687. }
  688. /**
  689. * Calculates the length of a dual quat
  690. *
  691. * @param {ReadonlyQuat2} a dual quat to calculate length of
  692. * @returns {Number} length of a
  693. * @function
  694. */
  695. export var length = quat.length;
  696. /**
  697. * Alias for {@link quat2.length}
  698. * @function
  699. */
  700. export var len = length;
  701. /**
  702. * Calculates the squared length of a dual quat
  703. *
  704. * @param {ReadonlyQuat2} a dual quat to calculate squared length of
  705. * @returns {Number} squared length of a
  706. * @function
  707. */
  708. export var squaredLength = quat.squaredLength;
  709. /**
  710. * Alias for {@link quat2.squaredLength}
  711. * @function
  712. */
  713. export var sqrLen = squaredLength;
  714. /**
  715. * Normalize a dual quat
  716. *
  717. * @param {quat2} out the receiving dual quaternion
  718. * @param {ReadonlyQuat2} a dual quaternion to normalize
  719. * @returns {quat2} out
  720. * @function
  721. */
  722. export function normalize(out, a) {
  723. var magnitude = squaredLength(a);
  724. if (magnitude > 0) {
  725. magnitude = Math.sqrt(magnitude);
  726. var a0 = a[0] / magnitude;
  727. var a1 = a[1] / magnitude;
  728. var a2 = a[2] / magnitude;
  729. var a3 = a[3] / magnitude;
  730. var b0 = a[4];
  731. var b1 = a[5];
  732. var b2 = a[6];
  733. var b3 = a[7];
  734. var a_dot_b = a0 * b0 + a1 * b1 + a2 * b2 + a3 * b3;
  735. out[0] = a0;
  736. out[1] = a1;
  737. out[2] = a2;
  738. out[3] = a3;
  739. out[4] = (b0 - a0 * a_dot_b) / magnitude;
  740. out[5] = (b1 - a1 * a_dot_b) / magnitude;
  741. out[6] = (b2 - a2 * a_dot_b) / magnitude;
  742. out[7] = (b3 - a3 * a_dot_b) / magnitude;
  743. }
  744. return out;
  745. }
  746. /**
  747. * Returns a string representation of a dual quatenion
  748. *
  749. * @param {ReadonlyQuat2} a dual quaternion to represent as a string
  750. * @returns {String} string representation of the dual quat
  751. */
  752. export function str(a) {
  753. return "quat2(" + a[0] + ", " + a[1] + ", " + a[2] + ", " + a[3] + ", " + a[4] + ", " + a[5] + ", " + a[6] + ", " + a[7] + ")";
  754. }
  755. /**
  756. * Returns whether or not the dual quaternions have exactly the same elements in the same position (when compared with ===)
  757. *
  758. * @param {ReadonlyQuat2} a the first dual quaternion.
  759. * @param {ReadonlyQuat2} b the second dual quaternion.
  760. * @returns {Boolean} true if the dual quaternions are equal, false otherwise.
  761. */
  762. export function exactEquals(a, b) {
  763. return a[0] === b[0] && a[1] === b[1] && a[2] === b[2] && a[3] === b[3] && a[4] === b[4] && a[5] === b[5] && a[6] === b[6] && a[7] === b[7];
  764. }
  765. /**
  766. * Returns whether or not the dual quaternions have approximately the same elements in the same position.
  767. *
  768. * @param {ReadonlyQuat2} a the first dual quat.
  769. * @param {ReadonlyQuat2} b the second dual quat.
  770. * @returns {Boolean} true if the dual quats are equal, false otherwise.
  771. */
  772. export function equals(a, b) {
  773. var a0 = a[0],
  774. a1 = a[1],
  775. a2 = a[2],
  776. a3 = a[3],
  777. a4 = a[4],
  778. a5 = a[5],
  779. a6 = a[6],
  780. a7 = a[7];
  781. var b0 = b[0],
  782. b1 = b[1],
  783. b2 = b[2],
  784. b3 = b[3],
  785. b4 = b[4],
  786. b5 = b[5],
  787. b6 = b[6],
  788. b7 = b[7];
  789. return Math.abs(a0 - b0) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a0), Math.abs(b0)) && Math.abs(a1 - b1) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a1), Math.abs(b1)) && Math.abs(a2 - b2) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a2), Math.abs(b2)) && Math.abs(a3 - b3) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a3), Math.abs(b3)) && Math.abs(a4 - b4) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a4), Math.abs(b4)) && Math.abs(a5 - b5) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a5), Math.abs(b5)) && Math.abs(a6 - b6) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a6), Math.abs(b6)) && Math.abs(a7 - b7) <= glMatrix.EPSILON * Math.max(1.0, Math.abs(a7), Math.abs(b7));
  790. }