1 module quaternion_d;
2 
3 import Math = math;
4 
5 import axis_angle_4d;
6 import matrix_3d;
7 import matrix_4d;
8 import matrix_4x3d;
9 import vector_3d;
10 import vector_4d;
11 
12 /*
13  * The MIT License
14  *
15  * Copyright (c) 2015-2021 Richard Greenlees
16  *
17  * Permission is hereby granted, free of charge, to any person obtaining a copy
18  * of this software and associated documentation files (the "Software"), to deal
19  * in the Software without restriction, including without limitation the rights
20  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
21  * copies of the Software, and to permit persons to whom the Software is
22  * furnished to do so, subject to the following conditions:
23  *
24  * The above copyright notice and this permission notice shall be included in
25  * all copies or substantial portions of the Software.
26  *
27  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
28  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
29  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
30  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
31  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
32  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
33  * THE SOFTWARE.
34  */
35 
36 /**
37  * Quaternion of 4 double-precision floats which can represent rotation and uniform scaling.
38  *
39  * @author Richard Greenlees
40  * @author Kai Burjack
41  */
42 struct Quaterniond {
43 
44     /**
45      * The first component of the vector part.
46      */
47     public double x = 0.0;
48     /**
49      * The second component of the vector part.
50      */
51     public double y = 0.0;
52     /**
53      * The third component of the vector part.
54      */
55     public double z = 0.0;
56     /**
57      * The real/scalar part of the quaternion.
58      */
59     public double w = 1.0;
60 
61     /**
62      * Create a new {@link Quaterniond} and initialize its components to the given values.
63      * 
64      * @param x
65      *          the first component of the imaginary part
66      * @param y
67      *          the second component of the imaginary part
68      * @param z
69      *          the third component of the imaginary part
70      * @param w
71      *          the real part
72      */
73     this(double x, double y, double z, double w) {
74         this.x = x;
75         this.y = y;
76         this.z = z;
77         this.w = w;
78     }
79 
80     /**
81      * Create a new {@link Quaterniond} and initialize its components to the same values as the given {@link Quaterniond}.
82      * 
83      * @param source
84      *          the {@link Quaterniond} to take the component values from
85      */
86     this(Quaterniond source) {
87         x = source.x;
88         y = source.y;
89         z = source.z;
90         w = source.w;
91     }
92 
93     /**
94      * Create a new {@link Quaterniond} and initialize it to represent the same rotation as the given {@link AxisAngle4d}.
95      * 
96      * @param axisAngle
97      *          the axis-angle to initialize this quaternion with
98      */
99     this(AxisAngle4d axisAngle) {
100         double s = Math.sin(axisAngle.angle * 0.5);
101         x = axisAngle.x * s;
102         y = axisAngle.y * s;
103         z = axisAngle.z * s;
104         w = Math.cosFromSin(s, axisAngle.angle * 0.5);
105     }
106 
107     /**
108      * Normalize this quaternion.
109      * 
110      * @return this
111      */
112     ref public Quaterniond normalize() return {
113         double invNorm = Math.invsqrt(lengthSquared());
114         x *= invNorm;
115         y *= invNorm;
116         z *= invNorm;
117         w *= invNorm;
118         return this;
119     }
120 
121     public Quaterniond normalize(ref Quaterniond dest) {
122         double invNorm = Math.invsqrt(lengthSquared());
123         dest.x = x * invNorm;
124         dest.y = y * invNorm;
125         dest.z = z * invNorm;
126         dest.w = w * invNorm;
127         return dest;
128     }
129 
130     /**
131      * Add the quaternion <code>(x, y, z, w)</code> to this quaternion.
132      * 
133      * @param x
134      *          the x component of the vector part
135      * @param y
136      *          the y component of the vector part
137      * @param z
138      *          the z component of the vector part
139      * @param w
140      *          the real/scalar component
141      * @return this
142      */
143     ref public Quaterniond add(double x, double y, double z, double w) return {
144         add(x, y, z, w, this);
145         return this;
146     }
147 
148     public Quaterniond add(double x, double y, double z, double w, ref Quaterniond dest) {
149         dest.x = this.x + x;
150         dest.y = this.y + y;
151         dest.z = this.z + z;
152         dest.w = this.w + w;
153         return dest;
154     }
155 
156     /**
157      * Add <code>q2</code> to this quaternion.
158      * 
159      * @param q2
160      *          the quaternion to add to this
161      * @return this
162      */
163     ref public Quaterniond add(Quaterniond q2) return {
164         x += q2.x;
165         y += q2.y;
166         z += q2.z;
167         w += q2.w;
168         return this;
169     }
170 
171     public Quaterniond add(Quaterniond q2, ref Quaterniond dest) {
172         dest.x = x + q2.x;
173         dest.y = y + q2.y;
174         dest.z = z + q2.z;
175         dest.w = w + q2.w;
176         return dest;
177     }
178 
179     public double dot(Quaterniond otherQuat) {
180         return this.x * otherQuat.x + this.y * otherQuat.y + this.z * otherQuat.z + this.w * otherQuat.w;
181     }
182 
183     public double angle() {
184         return 2.0 * Math.safeAcos(w);
185     }
186 
187     public Matrix3d get(ref Matrix3d dest) {
188         return dest.set(this);
189     }
190 
191 
192     public Matrix4d get(ref Matrix4d dest) {
193         return dest.set(this);
194     }
195 
196     public AxisAngle4d get(AxisAngle4d dest) {
197         double x = this.x;
198         double y = this.y;
199         double z = this.z;
200         double w = this.w;
201         if (w > 1.0) {
202             double invNorm = Math.invsqrt(lengthSquared());
203             x *= invNorm;
204             y *= invNorm;
205             z *= invNorm;
206             w *= invNorm;
207         }
208         dest.angle = 2.0 * Math.acos(w);
209         double s = Math.sqrt(1.0 - w * w);
210         if (s < 0.001) {
211             dest.x = x;
212             dest.y = y;
213             dest.z = z;
214         } else {
215             s = 1.0 / s;
216             dest.x = x * s;
217             dest.y = y * s;
218             dest.z = z * s;
219         }
220         return dest;
221     }
222 
223     /**
224      * Set the given {@link Quaterniond} to the values of <code>this</code>.
225      * 
226      * @see #set(Quaterniond)
227      * 
228      * @param dest
229      *          the {@link Quaterniond} to set
230      * @return the passed in destination
231      */
232     public Quaterniond get(ref Quaterniond dest) {
233         return dest.set(this);
234     }
235 
236 
237     /**
238      * Set this quaternion to the new values.
239      * 
240      * @param x
241      *          the new value of x
242      * @param y
243      *          the new value of y
244      * @param z
245      *          the new value of z
246      * @param w
247      *          the new value of w
248      * @return this
249      */
250     ref public Quaterniond set(double x, double y, double z, double w) return {
251         this.x = x;
252         this.y = y;
253         this.z = z;
254         this.w = w;
255         return this;
256     }
257 
258     /**
259      * Set this quaternion to be a copy of q.
260      * 
261      * @param q
262      *          the {@link Quaterniond} to copy
263      * @return this
264      */
265     ref public Quaterniond set(Quaterniond q) return {
266         x = q.x;
267         y = q.y;
268         z = q.z;
269         w = q.w;
270         return this;
271     }
272 
273     /**
274      * Set this {@link Quaterniond} to be equivalent to the given
275      * {@link AxisAngle4d}.
276      * 
277      * @param axisAngle
278      *            the {@link AxisAngle4d}
279      * @return this
280      */
281     ref public Quaterniond set(AxisAngle4d axisAngle) return {
282         return setAngleAxis(axisAngle.angle, axisAngle.x, axisAngle.y, axisAngle.z);
283     }
284 
285     /**
286      * Set this quaternion to a rotation equivalent to the supplied axis and
287      * angle (in radians).
288      * <p>
289      * This method assumes that the given rotation axis <code>(x, y, z)</code> is already normalized
290      * 
291      * @param angle
292      *          the angle in radians
293      * @param x
294      *          the x-component of the normalized rotation axis
295      * @param y
296      *          the y-component of the normalized rotation axis
297      * @param z
298      *          the z-component of the normalized rotation axis
299      * @return this
300      */
301     ref public Quaterniond setAngleAxis(double angle, double x, double y, double z) return {
302         double s = Math.sin(angle * 0.5);
303         this.x = x * s;
304         this.y = y * s;
305         this.z = z * s;
306         this.w = Math.cosFromSin(s, angle * 0.5);
307         return this;
308     }
309 
310     /**
311      * Set this quaternion to be a representation of the supplied axis and
312      * angle (in radians).
313      * 
314      * @param angle
315      *          the angle in radians
316      * @param axis
317      *          the rotation axis
318      * @return this
319      */
320     ref public Quaterniond setAngleAxis(double angle, Vector3d axis) return {
321         return setAngleAxis(angle, axis.x, axis.y, axis.z);
322     }
323 
324     private void setFromUnnormalized(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) {
325         double nm00 = m00, nm01 = m01, nm02 = m02;
326         double nm10 = m10, nm11 = m11, nm12 = m12;
327         double nm20 = m20, nm21 = m21, nm22 = m22;
328         double lenX = Math.invsqrt(m00 * m00 + m01 * m01 + m02 * m02);
329         double lenY = Math.invsqrt(m10 * m10 + m11 * m11 + m12 * m12);
330         double lenZ = Math.invsqrt(m20 * m20 + m21 * m21 + m22 * m22);
331         nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
332         nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
333         nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
334         setFromNormalized(nm00, nm01, nm02, nm10, nm11, nm12, nm20, nm21, nm22);
335     }
336 
337     private void setFromNormalized(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) {
338         double t;
339         double tr = m00 + m11 + m22;
340         if (tr >= 0.0) {
341             t = Math.sqrt(tr + 1.0);
342             w = t * 0.5;
343             t = 0.5 / t;
344             x = (m12 - m21) * t;
345             y = (m20 - m02) * t;
346             z = (m01 - m10) * t;
347         } else {
348             if (m00 >= m11 && m00 >= m22) {
349                 t = Math.sqrt(m00 - (m11 + m22) + 1.0);
350                 x = t * 0.5;
351                 t = 0.5 / t;
352                 y = (m10 + m01) * t;
353                 z = (m02 + m20) * t;
354                 w = (m12 - m21) * t;
355             } else if (m11 > m22) {
356                 t = Math.sqrt(m11 - (m22 + m00) + 1.0);
357                 y = t * 0.5;
358                 t = 0.5 / t;
359                 z = (m21 + m12) * t;
360                 x = (m10 + m01) * t;
361                 w = (m20 - m02) * t;
362             } else {
363                 t = Math.sqrt(m22 - (m00 + m11) + 1.0);
364                 z = t * 0.5;
365                 t = 0.5 / t;
366                 x = (m02 + m20) * t;
367                 y = (m21 + m12) * t;
368                 w = (m01 - m10) * t;
369             }
370         }
371     }
372 
373 
374     /**
375      * Set this quaternion to be a representation of the rotational component of the given matrix.
376      * <p>
377      * This method assumes that the first three columns of the upper left 3x3 submatrix are no unit vectors.
378      * 
379      * @param mat
380      *          the matrix whose rotational component is used to set this quaternion
381      * @return this
382      */
383     ref public Quaterniond setFromUnnormalized(Matrix4x3d mat) return {
384         setFromUnnormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
385         return this;
386     }
387 
388 
389     /**
390      * Set this quaternion to be a representation of the rotational component of the given matrix.
391      * <p>
392      * This method assumes that the first three columns of the upper left 3x3 submatrix are unit vectors.
393      * 
394      * @param mat
395      *          the matrix whose rotational component is used to set this quaternion
396      * @return this
397      */
398     ref public Quaterniond setFromNormalized(Matrix4x3d mat) return {
399         setFromNormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
400         return this;
401     }
402 
403     /**
404      * Set this quaternion to be a representation of the rotational component of the given matrix.
405      * <p>
406      * This method assumes that the first three columns of the upper left 3x3 submatrix are no unit vectors.
407      * 
408      * @param mat
409      *          the matrix whose rotational component is used to set this quaternion
410      * @return this
411      */
412     ref public Quaterniond setFromUnnormalized(Matrix4d mat) return {
413         setFromUnnormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
414         return this;
415     }
416 
417     /**
418      * Set this quaternion to be a representation of the rotational component of the given matrix.
419      * <p>
420      * This method assumes that the first three columns of the upper left 3x3 submatrix are unit vectors.
421      * 
422      * @param mat
423      *          the matrix whose rotational component is used to set this quaternion
424      * @return this
425      */
426     ref public Quaterniond setFromNormalized(Matrix4d mat) return {
427         setFromNormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
428         return this;
429     }
430 
431 
432 
433     /**
434      * Set this quaternion to be a representation of the rotational component of the given matrix.
435      * <p>
436      * This method assumes that the first three columns of the upper left 3x3 submatrix are no unit vectors.
437      * 
438      * @param mat
439      *          the matrix whose rotational component is used to set this quaternion
440      * @return this
441      */
442     ref public Quaterniond setFromUnnormalized(Matrix3d mat) return {
443         setFromUnnormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
444         return this;
445     }
446 
447     /**
448      * Set this quaternion to be a representation of the rotational component of the given matrix.
449      * 
450      * @param mat
451      *          the matrix whose rotational component is used to set this quaternion
452      * @return this
453      */
454     ref public Quaterniond setFromNormalized(Matrix3d mat) return {
455         setFromNormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
456         return this;
457     }
458 
459     /**
460      * Set this quaternion to be a representation of the supplied axis and
461      * angle (in radians).
462      * 
463      * @param axis
464      *          the rotation axis
465      * @param angle
466      *          the angle in radians
467      * @return this
468      */
469     ref public Quaterniond fromAxisAngleRad(Vector3d axis, double angle) return {
470         return fromAxisAngleRad(axis.x, axis.y, axis.z, angle);
471     }
472 
473     /**
474      * Set this quaternion to be a representation of the supplied axis and
475      * angle (in radians).
476      * 
477      * @param axisX
478      *          the x component of the rotation axis
479      * @param axisY
480      *          the y component of the rotation axis
481      * @param axisZ
482      *          the z component of the rotation axis         
483      * @param angle
484      *          the angle in radians
485      * @return this
486      */
487     ref public Quaterniond fromAxisAngleRad(double axisX, double axisY, double axisZ, double angle) return {
488         double hangle = angle / 2.0;
489         double sinAngle = Math.sin(hangle);
490         double vLength = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
491         x = axisX / vLength * sinAngle;
492         y = axisY / vLength * sinAngle;
493         z = axisZ / vLength * sinAngle;
494         w = Math.cosFromSin(sinAngle, hangle);
495         return this;
496     }
497 
498     /**
499      * Set this quaternion to be a representation of the supplied axis and
500      * angle (in degrees).
501      * 
502      * @param axis
503      *          the rotation axis
504      * @param angle
505      *          the angle in degrees
506      * @return this
507      */
508     ref public Quaterniond fromAxisAngleDeg(Vector3d axis, double angle) return {
509         return fromAxisAngleRad(axis.x, axis.y, axis.z, Math.toRadians(angle));
510     }
511 
512     /**
513      * Set this quaternion to be a representation of the supplied axis and
514      * angle (in degrees).
515      * 
516      * @param axisX
517      *          the x component of the rotation axis
518      * @param axisY
519      *          the y component of the rotation axis
520      * @param axisZ
521      *          the z component of the rotation axis         
522      * @param angle
523      *          the angle in radians
524      * @return this
525      */
526     ref public Quaterniond fromAxisAngleDeg(double axisX, double axisY, double axisZ, double angle) return {
527         return fromAxisAngleRad(axisX, axisY, axisZ, Math.toRadians(angle));
528     }
529 
530     /**
531      * Multiply this quaternion by <code>q</code>.
532      * <p>
533      * If <code>T</code> is <code>this</code> and <code>Q</code> is the given
534      * quaternion, then the resulting quaternion <code>R</code> is:
535      * <p>
536      * <code>R = T * Q</code>
537      * <p>
538      * So, this method uses post-multiplication like the matrix classes, resulting in a
539      * vector to be transformed by <code>Q</code> first, and then by <code>T</code>.
540      * 
541      * @param q
542      *          the quaternion to multiply <code>this</code> by
543      * @return this
544      */
545     ref public Quaterniond mul(Quaterniond q) return {
546         mul(q, this);
547         return this;
548     }
549 
550     public Quaterniond mul(Quaterniond q, ref Quaterniond dest) {
551         return mul(q.x, q.y, q.z, q.w, dest);
552     }
553 
554     /**
555      * Multiply this quaternion by the quaternion represented via <code>(qx, qy, qz, qw)</code>.
556      * <p>
557      * If <code>T</code> is <code>this</code> and <code>Q</code> is the given
558      * quaternion, then the resulting quaternion <code>R</code> is:
559      * <p>
560      * <code>R = T * Q</code>
561      * <p>
562      * So, this method uses post-multiplication like the matrix classes, resulting in a
563      * vector to be transformed by <code>Q</code> first, and then by <code>T</code>.
564      * 
565      * @param qx
566      *          the x component of the quaternion to multiply <code>this</code> by
567      * @param qy
568      *          the y component of the quaternion to multiply <code>this</code> by
569      * @param qz
570      *          the z component of the quaternion to multiply <code>this</code> by
571      * @param qw
572      *          the w component of the quaternion to multiply <code>this</code> by
573      * @return this
574      */
575     ref public Quaterniond mul(double qx, double qy, double qz, double qw) return {
576         mul(qx, qy, qz, qw, this);
577         return this;
578     }
579 
580     public Quaterniond mul(double qx, double qy, double qz, double qw, ref Quaterniond dest) {
581         return dest.set(Math.fma(w, qx, Math.fma(x, qw, Math.fma(y, qz, -z * qy))),
582                         Math.fma(w, qy, Math.fma(-x, qz, Math.fma(y, qw, z * qx))),
583                         Math.fma(w, qz, Math.fma(x, qy, Math.fma(-y, qx, z * qw))),
584                         Math.fma(w, qw, Math.fma(-x, qx, Math.fma(-y, qy, -z * qz))));
585     }
586 
587     /**
588      * Pre-multiply this quaternion by <code>q</code>.
589      * <p>
590      * If <code>T</code> is <code>this</code> and <code>Q</code> is the given quaternion, then the resulting quaternion <code>R</code> is:
591      * <p>
592      * <code>R = Q * T</code>
593      * <p>
594      * So, this method uses pre-multiplication, resulting in a vector to be transformed by <code>T</code> first, and then by <code>Q</code>.
595      * 
596      * @param q
597      *            the quaternion to pre-multiply <code>this</code> by
598      * @return this
599      */
600     ref public Quaterniond premul(Quaterniond q) return {
601         premul(q, this);
602         return this;
603     }
604 
605     public Quaterniond premul(Quaterniond q, ref Quaterniond dest) {
606         return premul(q.x, q.y, q.z, q.w, dest);
607     }
608 
609     /**
610      * Pre-multiply this quaternion by the quaternion represented via <code>(qx, qy, qz, qw)</code>.
611      * <p>
612      * If <code>T</code> is <code>this</code> and <code>Q</code> is the given quaternion, then the resulting quaternion <code>R</code> is:
613      * <p>
614      * <code>R = Q * T</code>
615      * <p>
616      * So, this method uses pre-multiplication, resulting in a vector to be transformed by <code>T</code> first, and then by <code>Q</code>.
617      * 
618      * @param qx
619      *          the x component of the quaternion to multiply <code>this</code> by
620      * @param qy
621      *          the y component of the quaternion to multiply <code>this</code> by
622      * @param qz
623      *          the z component of the quaternion to multiply <code>this</code> by
624      * @param qw
625      *          the w component of the quaternion to multiply <code>this</code> by
626      * @return this
627      */
628     ref public Quaterniond premul(double qx, double qy, double qz, double qw) return {
629         premul(qx, qy, qz, qw, this);
630         return this;
631     }
632 
633     public Quaterniond premul(double qx, double qy, double qz, double qw, ref Quaterniond dest) {
634         return dest.set(Math.fma(qw, x, Math.fma(qx, w, Math.fma(qy, z, -qz * y))),
635                         Math.fma(qw, y, Math.fma(-qx, z, Math.fma(qy, w, qz * x))),
636                         Math.fma(qw, z, Math.fma(qx, y, Math.fma(-qy, x, qz * w))),
637                         Math.fma(qw, w, Math.fma(-qx, x, Math.fma(-qy, y, -qz * z))));
638     }
639 
640     public Vector3d transform(Vector3d vec){
641         return transform(vec.x, vec.y, vec.z, vec);
642     }
643 
644     public Vector3d transformInverse(Vector3d vec){
645         return transformInverse(vec.x, vec.y, vec.z, vec);
646     }
647 
648     public Vector3d transformUnit(Vector3d vec){
649         return transformUnit(vec.x, vec.y, vec.z, vec);
650     }
651 
652     public Vector3d transformInverseUnit(Vector3d vec){
653         return transformInverseUnit(vec.x, vec.y, vec.z, vec);
654     }
655 
656     public Vector3d transformPositiveX(ref Vector3d dest) {
657         double ww = w * w;
658         double xx = x * x;
659         double yy = y * y;
660         double zz = z * z;
661         double zw = z * w;
662         double xy = x * y;
663         double xz = x * z;
664         double yw = y * w;
665         dest.x = ww + xx - zz - yy;
666         dest.y = xy + zw + zw + xy;
667         dest.z = xz - yw + xz - yw;
668         return dest;
669     }
670 
671     public Vector4d transformPositiveX(ref Vector4d dest) {
672         double ww = w * w;
673         double xx = x * x;
674         double yy = y * y;
675         double zz = z * z;
676         double zw = z * w;
677         double xy = x * y;
678         double xz = x * z;
679         double yw = y * w;
680         dest.x = ww + xx - zz - yy;
681         dest.y = xy + zw + zw + xy;
682         dest.z = xz - yw + xz - yw;
683         return dest;
684     }
685 
686     public Vector3d transformUnitPositiveX(ref Vector3d dest) {
687         double yy = y * y;
688         double zz = z * z;
689         double xy = x * y;
690         double xz = x * z;
691         double yw = y * w;
692         double zw = z * w;
693         dest.x = 1.0 - yy - yy - zz - zz;
694         dest.y = xy + zw + xy + zw;
695         dest.z = xz - yw + xz - yw;
696         return dest;
697     }
698 
699     public Vector4d transformUnitPositiveX(ref Vector4d dest) {
700         double yy = y * y;
701         double zz = z * z;
702         double xy = x * y;
703         double xz = x * z;
704         double yw = y * w;
705         double zw = z * w;
706         dest.x = 1.0 - yy - yy - zz - zz;
707         dest.y = xy + zw + xy + zw;
708         dest.z = xz - yw + xz - yw;
709         return dest;
710     }
711 
712     public Vector3d transformPositiveY(ref Vector3d dest) {
713         double ww = w * w;
714         double xx = x * x;
715         double yy = y * y;
716         double zz = z * z;
717         double zw = z * w;
718         double xy = x * y;
719         double yz = y * z;
720         double xw = x * w;
721         dest.x = -zw + xy - zw + xy;
722         dest.y = yy - zz + ww - xx;
723         dest.z = yz + yz + xw + xw;
724         return dest;
725     }
726 
727     public Vector4d transformPositiveY(ref Vector4d dest) {
728         double ww = w * w;
729         double xx = x * x;
730         double yy = y * y;
731         double zz = z * z;
732         double zw = z * w;
733         double xy = x * y;
734         double yz = y * z;
735         double xw = x * w;
736         dest.x = -zw + xy - zw + xy;
737         dest.y = yy - zz + ww - xx;
738         dest.z = yz + yz + xw + xw;
739         return dest;
740     }
741 
742     public Vector4d transformUnitPositiveY(ref Vector4d dest) {
743         double xx = x * x;
744         double zz = z * z;
745         double xy = x * y;
746         double yz = y * z;
747         double xw = x * w;
748         double zw = z * w;
749         dest.x = xy - zw + xy - zw;
750         dest.y = 1.0 - xx - xx - zz - zz;
751         dest.z = yz + yz + xw + xw;
752         return dest;
753     }
754 
755     public Vector3d transformUnitPositiveY(ref Vector3d dest) {
756         double xx = x * x;
757         double zz = z * z;
758         double xy = x * y;
759         double yz = y * z;
760         double xw = x * w;
761         double zw = z * w;
762         dest.x = xy - zw + xy - zw;
763         dest.y = 1.0 - xx - xx - zz - zz;
764         dest.z = yz + yz + xw + xw;
765         return dest;
766     }
767 
768     public Vector3d transformPositiveZ(ref Vector3d dest) {
769         double ww = w * w;
770         double xx = x * x;
771         double yy = y * y;
772         double zz = z * z;
773         double xz = x * z;
774         double yw = y * w;
775         double yz = y * z;
776         double xw = x * w;
777         dest.x = yw + xz + xz + yw;
778         dest.y = yz + yz - xw - xw;
779         dest.z = zz - yy - xx + ww;
780         return dest;
781     }
782 
783     public Vector4d transformPositiveZ(ref Vector4d dest) {
784         double ww = w * w;
785         double xx = x * x;
786         double yy = y * y;
787         double zz = z * z;
788         double xz = x * z;
789         double yw = y * w;
790         double yz = y * z;
791         double xw = x * w;
792         dest.x = yw + xz + xz + yw;
793         dest.y = yz + yz - xw - xw;
794         dest.z = zz - yy - xx + ww;
795         return dest;
796     }
797 
798     public Vector4d transformUnitPositiveZ(ref Vector4d dest) {
799         double xx = x * x;
800         double yy = y * y;
801         double xz = x * z;
802         double yz = y * z;
803         double xw = x * w;
804         double yw = y * w;
805         dest.x = xz + yw + xz + yw;
806         dest.y = yz + yz - xw - xw;
807         dest.z = 1.0 - xx - xx - yy - yy;
808         return dest;
809     }
810 
811     public Vector3d transformUnitPositiveZ(ref Vector3d dest) {
812         double xx = x * x;
813         double yy = y * y;
814         double xz = x * z;
815         double yz = y * z;
816         double xw = x * w;
817         double yw = y * w;
818         dest.x = xz + yw + xz + yw;
819         dest.y = yz + yz - xw - xw;
820         dest.z = 1.0 - xx - xx - yy - yy;
821         return dest;
822     }
823 
824     public Vector4d transform(Vector4d vec){
825         return transform(vec, vec);
826     }
827 
828     public Vector4d transformInverse(Vector4d vec){
829         return transformInverse(vec, vec);
830     }
831 
832     public Vector3d transform(Vector3d vec, ref Vector3d dest) {
833         return transform(vec.x, vec.y, vec.z, dest);
834     }
835 
836     public Vector3d transformInverse(Vector3d vec, ref Vector3d dest) {
837         return transformInverse(vec.x, vec.y, vec.z, dest);
838     }
839 
840     public Vector3d transform(double x, double y, double z, ref Vector3d dest) {
841         double xx = this.x * this.x, yy = this.y * this.y, zz = this.z * this.z, ww = this.w * this.w;
842         double xy = this.x * this.y, xz = this.x * this.z, yz = this.y * this.z, xw = this.x * this.w;
843         double zw = this.z * this.w, yw = this.y * this.w, k = 1 / (xx + yy + zz + ww);
844         return dest.set(Math.fma((xx - yy - zz + ww) * k, x, Math.fma(2 * (xy - zw) * k, y, (2 * (xz + yw) * k) * z)),
845                         Math.fma(2 * (xy + zw) * k, x, Math.fma((yy - xx - zz + ww) * k, y, (2 * (yz - xw) * k) * z)),
846                         Math.fma(2 * (xz - yw) * k, x, Math.fma(2 * (yz + xw) * k, y, ((zz - xx - yy + ww) * k) * z)));
847     }
848 
849     public Vector3d transformInverse(double x, double y, double z, ref Vector3d dest) {
850         double n = 1.0 / Math.fma(this.x, this.x, Math.fma(this.y, this.y, Math.fma(this.z, this.z, this.w * this.w)));
851         double qx = this.x * n, qy = this.y * n, qz = this.z * n, qw = this.w * n;
852         double xx = qx * qx, yy = qy * qy, zz = qz * qz, ww = qw * qw;
853         double xy = qx * qy, xz = qx * qz, yz = qy * qz, xw = qx * qw;
854         double zw = qz * qw, yw = qy * qw, k = 1 / (xx + yy + zz + ww);
855         return dest.set(Math.fma((xx - yy - zz + ww) * k, x, Math.fma(2 * (xy + zw) * k, y, (2 * (xz - yw) * k) * z)),
856                         Math.fma(2 * (xy - zw) * k, x, Math.fma((yy - xx - zz + ww) * k, y, (2 * (yz + xw) * k) * z)),
857                         Math.fma(2 * (xz + yw) * k, x, Math.fma(2 * (yz - xw) * k, y, ((zz - xx - yy + ww) * k) * z)));
858     }
859 
860     public Vector4d transform(Vector4d vec, ref Vector4d dest) {
861         return transform(vec.x, vec.y, vec.z, dest);
862     }
863 
864     public Vector4d transformInverse(Vector4d vec, ref Vector4d dest) {
865         return transformInverse(vec.x, vec.y, vec.z, dest);
866     }
867 
868     public Vector4d transform(double x, double y, double z, ref Vector4d dest) {
869         double xx = this.x * this.x, yy = this.y * this.y, zz = this.z * this.z, ww = this.w * this.w;
870         double xy = this.x * this.y, xz = this.x * this.z, yz = this.y * this.z, xw = this.x * this.w;
871         double zw = this.z * this.w, yw = this.y * this.w, k = 1 / (xx + yy + zz + ww);
872         return dest.set(Math.fma((xx - yy - zz + ww) * k, x, Math.fma(2 * (xy - zw) * k, y, (2 * (xz + yw) * k) * z)),
873                         Math.fma(2 * (xy + zw) * k, x, Math.fma((yy - xx - zz + ww) * k, y, (2 * (yz - xw) * k) * z)),
874                         Math.fma(2 * (xz - yw) * k, x, Math.fma(2 * (yz + xw) * k, y, ((zz - xx - yy + ww) * k) * z)), dest.w);
875     }
876 
877     public Vector4d transformInverse(double x, double y, double z, ref Vector4d dest) {
878         double n = 1.0 / Math.fma(this.x, this.x, Math.fma(this.y, this.y, Math.fma(this.z, this.z, this.w * this.w)));
879         double qx = this.x * n, qy = this.y * n, qz = this.z * n, qw = this.w * n;
880         double xx = qx * qx, yy = qy * qy, zz = qz * qz, ww = qw * qw;
881         double xy = qx * qy, xz = qx * qz, yz = qy * qz, xw = qx * qw;
882         double zw = qz * qw, yw = qy * qw, k = 1 / (xx + yy + zz + ww);
883         return dest.set(Math.fma((xx - yy - zz + ww) * k, x, Math.fma(2 * (xy + zw) * k, y, (2 * (xz - yw) * k) * z)),
884                         Math.fma(2 * (xy - zw) * k, x, Math.fma((yy - xx - zz + ww) * k, y, (2 * (yz + xw) * k) * z)),
885                         Math.fma(2 * (xz + yw) * k, x, Math.fma(2 * (yz - xw) * k, y, ((zz - xx - yy + ww) * k) * z)));
886     }
887 
888     public Vector4d transformUnit(Vector4d vec){
889         return transformUnit(vec, vec);
890     }
891 
892     public Vector4d transformInverseUnit(Vector4d vec){
893         return transformInverseUnit(vec, vec);
894     }
895 
896     public Vector3d transformUnit(Vector3d vec, ref Vector3d dest) {
897         return transformUnit(vec.x, vec.y, vec.z, dest);
898     }
899 
900     public Vector3d transformInverseUnit(Vector3d vec, ref Vector3d dest) {
901         return transformInverseUnit(vec.x, vec.y, vec.z, dest);
902     }
903 
904     public Vector3d transformUnit(double x, double y, double z, ref Vector3d dest) {
905         double xx = this.x * this.x, xy = this.x * this.y, xz = this.x * this.z;
906         double xw = this.x * this.w, yy = this.y * this.y, yz = this.y * this.z;
907         double yw = this.y * this.w, zz = this.z * this.z, zw = this.z * this.w;
908         return dest.set(Math.fma(Math.fma(-2, yy + zz, 1), x, Math.fma(2 * (xy - zw), y, (2 * (xz + yw)) * z)),
909                         Math.fma(2 * (xy + zw), x, Math.fma(Math.fma(-2, xx + zz, 1), y, (2 * (yz - xw)) * z)),
910                         Math.fma(2 * (xz - yw), x, Math.fma(2 * (yz + xw), y, Math.fma(-2, xx + yy, 1) * z)));
911     }
912 
913     public Vector3d transformInverseUnit(double x, double y, double z, ref Vector3d dest) {
914         double xx = this.x * this.x, xy = this.x * this.y, xz = this.x * this.z;
915         double xw = this.x * this.w, yy = this.y * this.y, yz = this.y * this.z;
916         double yw = this.y * this.w, zz = this.z * this.z, zw = this.z * this.w;
917         return dest.set(Math.fma(Math.fma(-2, yy + zz, 1), x, Math.fma(2 * (xy + zw), y, (2 * (xz - yw)) * z)),
918                         Math.fma(2 * (xy - zw), x, Math.fma(Math.fma(-2, xx + zz, 1), y, (2 * (yz + xw)) * z)),
919                         Math.fma(2 * (xz + yw), x, Math.fma(2 * (yz - xw), y, Math.fma(-2, xx + yy, 1) * z)));
920     }
921 
922     public Vector4d transformUnit(Vector4d vec, ref Vector4d dest) {
923         return transformUnit(vec.x, vec.y, vec.z, dest);
924     }
925 
926     public Vector4d transformInverseUnit(Vector4d vec, ref Vector4d dest) {
927         return transformInverseUnit(vec.x, vec.y, vec.z, dest);
928     }
929 
930     public Vector4d transformUnit(double x, double y, double z, ref Vector4d dest) {
931         double xx = this.x * this.x, xy = this.x * this.y, xz = this.x * this.z;
932         double xw = this.x * this.w, yy = this.y * this.y, yz = this.y * this.z;
933         double yw = this.y * this.w, zz = this.z * this.z, zw = this.z * this.w;
934         return dest.set(Math.fma(Math.fma(-2, yy + zz, 1), x, Math.fma(2 * (xy - zw), y, (2 * (xz + yw)) * z)),
935                         Math.fma(2 * (xy + zw), x, Math.fma(Math.fma(-2, xx + zz, 1), y, (2 * (yz - xw)) * z)),
936                         Math.fma(2 * (xz - yw), x, Math.fma(2 * (yz + xw), y, Math.fma(-2, xx + yy, 1) * z)),
937                         dest.w);
938     }
939 
940     public Vector4d transformInverseUnit(double x, double y, double z, ref Vector4d dest) {
941         double xx = this.x * this.x, xy = this.x * this.y, xz = this.x * this.z;
942         double xw = this.x * this.w, yy = this.y * this.y, yz = this.y * this.z;
943         double yw = this.y * this.w, zz = this.z * this.z, zw = this.z * this.w;
944         return dest.set(Math.fma(Math.fma(-2, yy + zz, 1), x, Math.fma(2 * (xy + zw), y, (2 * (xz - yw)) * z)),
945                         Math.fma(2 * (xy - zw), x, Math.fma(Math.fma(-2, xx + zz, 1), y, (2 * (yz + xw)) * z)),
946                         Math.fma(2 * (xz + yw), x, Math.fma(2 * (yz - xw), y, Math.fma(-2, xx + yy, 1) * z)),
947                         dest.w);
948     }
949 
950 
951     public Quaterniond invert(ref Quaterniond dest) {
952         double invNorm = 1.0 / lengthSquared();
953         dest.x = -x * invNorm;
954         dest.y = -y * invNorm;
955         dest.z = -z * invNorm;
956         dest.w = w * invNorm;
957         return dest;
958     }
959 
960     /**
961      * Invert this quaternion and {@link #normalize() normalize} it.
962      * <p>
963      * If this quaternion is already normalized, then {@link #conjugate()} should be used instead.
964      * 
965      * @see #conjugate()
966      * 
967      * @return this
968      */
969     ref public Quaterniond invert() return {
970         invert(this);
971         return this;
972     }
973 
974     public Quaterniond div(Quaterniond b, ref Quaterniond dest) {
975         double invNorm = 1.0 / Math.fma(b.x, b.x, Math.fma(b.y, b.y, Math.fma(b.z, b.z, b.w * b.w)));
976         double x = -b.x * invNorm;
977         double y = -b.y * invNorm;
978         double z = -b.z * invNorm;
979         double w = b.w * invNorm;
980         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
981                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
982                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
983                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
984     }
985 
986     /**
987      * Divide <code>this</code> quaternion by <code>b</code>.
988      * <p>
989      * The division expressed using the inverse is performed in the following way:
990      * <p>
991      * <code>this = this * b^-1</code>, where <code>b^-1</code> is the inverse of <code>b</code>.
992      * 
993      * @param b
994      *          the {@link Quaterniond} to divide this by
995      * @return this
996      */
997     ref public Quaterniond div(Quaterniond b) return {
998         div(b, this);
999         return this;
1000     }
1001 
1002     /**
1003      * Conjugate this quaternion.
1004      * 
1005      * @return this
1006      */
1007     ref public Quaterniond conjugate() return {
1008         x = -x;
1009         y = -y;
1010         z = -z;
1011         return this;
1012     }
1013 
1014     public Quaterniond conjugate(ref Quaterniond dest) {
1015         dest.x = -x;
1016         dest.y = -y;
1017         dest.z = -z;
1018         dest.w = w;
1019         return dest;
1020     }
1021 
1022     /**
1023      * Set this quaternion to the identity.
1024      * 
1025      * @return this
1026      */
1027     ref public Quaterniond identity() return {
1028         x = 0.0;
1029         y = 0.0;
1030         z = 0.0;
1031         w = 1.0;
1032         return this;
1033     }
1034 
1035     public double lengthSquared() {
1036         return Math.fma(x, x, Math.fma(y, y, Math.fma(z, z, w * w)));
1037     }
1038 
1039     /**
1040      * Set this quaternion from the supplied euler angles (in radians) with rotation order XYZ.
1041      * <p>
1042      * This method is equivalent to calling: <code>rotationX(angleX).rotateY(angleY).rotateZ(angleZ)</code>
1043      * <p>
1044      * Reference: <a href="http://gamedev.stackexchange.com/questions/13436/glm-euler-angles-to-quaternion#answer-13446">this stackexchange answer</a>
1045      * 
1046      * @param angleX
1047      *          the angle in radians to rotate about x
1048      * @param angleY
1049      *          the angle in radians to rotate about y
1050      * @param angleZ
1051      *          the angle in radians to rotate about z
1052      * @return this
1053      */
1054     ref public Quaterniond rotationXYZ(double angleX, double angleY, double angleZ) return {
1055         double sx = Math.sin(angleX * 0.5);
1056         double cx = Math.cosFromSin(sx, angleX * 0.5);
1057         double sy = Math.sin(angleY * 0.5);
1058         double cy = Math.cosFromSin(sy, angleY * 0.5);
1059         double sz = Math.sin(angleZ * 0.5);
1060         double cz = Math.cosFromSin(sz, angleZ * 0.5);
1061 
1062         double cycz = cy * cz;
1063         double sysz = sy * sz;
1064         double sycz = sy * cz;
1065         double cysz = cy * sz;
1066         w = cx*cycz - sx*sysz;
1067         x = sx*cycz + cx*sysz;
1068         y = cx*sycz - sx*cysz;
1069         z = cx*cysz + sx*sycz;
1070 
1071         return this;
1072     }
1073 
1074     /**
1075      * Set this quaternion from the supplied euler angles (in radians) with rotation order ZYX.
1076      * <p>
1077      * This method is equivalent to calling: <code>rotationZ(angleZ).rotateY(angleY).rotateX(angleX)</code>
1078      * <p>
1079      * Reference: <a href="http://gamedev.stackexchange.com/questions/13436/glm-euler-angles-to-quaternion#answer-13446">this stackexchange answer</a>
1080      * 
1081      * @param angleX
1082      *          the angle in radians to rotate about x
1083      * @param angleY
1084      *          the angle in radians to rotate about y
1085      * @param angleZ
1086      *          the angle in radians to rotate about z
1087      * @return this
1088      */
1089     ref public Quaterniond rotationZYX(double angleZ, double angleY, double angleX) return {
1090         double sx = Math.sin(angleX * 0.5);
1091         double cx = Math.cosFromSin(sx, angleX * 0.5);
1092         double sy = Math.sin(angleY * 0.5);
1093         double cy = Math.cosFromSin(sy, angleY * 0.5);
1094         double sz = Math.sin(angleZ * 0.5);
1095         double cz = Math.cosFromSin(sz, angleZ * 0.5);
1096 
1097         double cycz = cy * cz;
1098         double sysz = sy * sz;
1099         double sycz = sy * cz;
1100         double cysz = cy * sz;
1101         w = cx*cycz + sx*sysz;
1102         x = sx*cycz - cx*sysz;
1103         y = cx*sycz + sx*cysz;
1104         z = cx*cysz - sx*sycz;
1105 
1106         return this;
1107     }
1108 
1109     /**
1110      * Set this quaternion from the supplied euler angles (in radians) with rotation order YXZ.
1111      * <p>
1112      * This method is equivalent to calling: <code>rotationY(angleY).rotateX(angleX).rotateZ(angleZ)</code>
1113      * <p>
1114      * Reference: <a href="https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles">https://en.wikipedia.org</a>
1115      * 
1116      * @param angleY
1117      *          the angle in radians to rotate about y
1118      * @param angleX
1119      *          the angle in radians to rotate about x
1120      * @param angleZ
1121      *          the angle in radians to rotate about z
1122      * @return this
1123      */
1124     ref public Quaterniond rotationYXZ(double angleY, double angleX, double angleZ) return {
1125         double sx = Math.sin(angleX * 0.5);
1126         double cx = Math.cosFromSin(sx, angleX * 0.5);
1127         double sy = Math.sin(angleY * 0.5);
1128         double cy = Math.cosFromSin(sy, angleY * 0.5);
1129         double sz = Math.sin(angleZ * 0.5);
1130         double cz = Math.cosFromSin(sz, angleZ * 0.5);
1131 
1132         double x = cy * sx;
1133         double y = sy * cx;
1134         double z = sy * sx;
1135         double w = cy * cx;
1136         this.x = x * cz + y * sz;
1137         this.y = y * cz - x * sz;
1138         this.z = w * sz - z * cz;
1139         this.w = w * cz + z * sz;
1140 
1141         return this;
1142     }
1143 
1144     /**
1145      * Interpolate between <code>this</code> {@link #normalize() unit} quaternion and the specified
1146      * <code>target</code> {@link #normalize() unit} quaternion using spherical linear interpolation using the specified interpolation factor <code>alpha</code>.
1147      * <p>
1148      * This method resorts to non-spherical linear interpolation when the absolute dot product between <code>this</code> and <code>target</code> is
1149      * below <code>1E-6</code>.
1150      * 
1151      * @param target
1152      *          the target of the interpolation, which should be reached with <code>alpha = 1.0</code>
1153      * @param alpha
1154      *          the interpolation factor, within <code>[0..1]</code>
1155      * @return this
1156      */
1157     ref public Quaterniond slerp(Quaterniond target, double alpha) return {
1158         slerp(target, alpha, this);
1159         return this;
1160     }
1161 
1162     public Quaterniond slerp(Quaterniond target, double alpha, ref Quaterniond dest) {
1163         double cosom = Math.fma(x, target.x, Math.fma(y, target.y, Math.fma(z, target.z, w * target.w)));
1164         double absCosom = Math.abs(cosom);
1165         double scale0, scale1;
1166         if (1.0 - absCosom > 1E-6) {
1167             double sinSqr = 1.0 - absCosom * absCosom;
1168             double sinom = Math.invsqrt(sinSqr);
1169             double omega = Math.atan2(sinSqr * sinom, absCosom);
1170             scale0 = Math.sin((1.0 - alpha) * omega) * sinom;
1171             scale1 = Math.sin(alpha * omega) * sinom;
1172         } else {
1173             scale0 = 1.0 - alpha;
1174             scale1 = alpha;
1175         }
1176         scale1 = cosom >= 0.0 ? scale1 : -scale1;
1177         dest.x = Math.fma(scale0, x, scale1 * target.x);
1178         dest.y = Math.fma(scale0, y, scale1 * target.y);
1179         dest.z = Math.fma(scale0, z, scale1 * target.z);
1180         dest.w = Math.fma(scale0, w, scale1 * target.w);
1181         return dest;
1182     }
1183 
1184     /**
1185      * Interpolate between all of the quaternions given in <code>qs</code> via spherical linear interpolation using the specified interpolation factors <code>weights</code>,
1186      * and store the result in <code>dest</code>.
1187      * <p>
1188      * This method will interpolate between each two successive quaternions via {@link #slerp(Quaterniond, double)} using their relative interpolation weights.
1189      * <p>
1190      * This method resorts to non-spherical linear interpolation when the absolute dot product of any two interpolated quaternions is below <code>1E-6f</code>.
1191      * <p>
1192      * Reference: <a href="http://gamedev.stackexchange.com/questions/62354/method-for-interpolation-between-3-quaternions#answer-62356">http://gamedev.stackexchange.com/</a>
1193      * 
1194      * @param qs
1195      *          the quaternions to interpolate over
1196      * @param weights
1197      *          the weights of each individual quaternion in <code>qs</code>
1198      * @param dest
1199      *          will hold the result
1200      * @return dest
1201      */
1202     public static Quaterniond slerp(Quaterniond[] qs, double[] weights, ref Quaterniond dest) {
1203         dest.set(qs[0]);
1204         double w = weights[0];
1205         for (int i = 1; i < qs.length; i++) {
1206             double w0 = w;
1207             double w1 = weights[i];
1208             double rw1 = w1 / (w0 + w1);
1209             w += w1;
1210             dest.slerp(qs[i], rw1);
1211         }
1212         return dest;
1213     }
1214 
1215     /**
1216      * Apply scaling to this quaternion, which results in any vector transformed by this quaternion to change
1217      * its length by the given <code>factor</code>.
1218      * 
1219      * @param factor
1220      *          the scaling factor
1221      * @return this
1222      */
1223     ref public Quaterniond scale(double factor) return {
1224         scale(factor, this);
1225         return this;
1226     }
1227 
1228     public Quaterniond scale(double factor, ref Quaterniond dest) {
1229         double sqrt = Math.sqrt(factor);
1230         dest.x = sqrt * x;
1231         dest.y = sqrt * y;
1232         dest.z = sqrt * z;
1233         dest.w = sqrt * w;
1234         return dest;
1235     }
1236 
1237     /**
1238      * Set this quaternion to represent scaling, which results in a transformed vector to change
1239      * its length by the given <code>factor</code>.
1240      * 
1241      * @param factor
1242      *          the scaling factor
1243      * @return this
1244      */
1245     ref public Quaterniond scaling(double factor) return {
1246         double sqrt = Math.sqrt(factor);
1247         this.x = 0.0;
1248         this.y = 0.0;
1249         this.z = 0.0;
1250         this.w = sqrt;
1251         return this;
1252     }
1253 
1254     /**
1255      * Integrate the rotation given by the angular velocity <code>(vx, vy, vz)</code> around the x, y and z axis, respectively,
1256      * with respect to the given elapsed time delta <code>dt</code> and add the differentiate rotation to the rotation represented by this quaternion.
1257      * <p>
1258      * This method pre-multiplies the rotation given by <code>dt</code> and <code>(vx, vy, vz)</code> by <code>this</code>, so
1259      * the angular velocities are always relative to the local coordinate system of the rotation represented by <code>this</code> quaternion.
1260      * <p>
1261      * This method is equivalent to calling: <code>rotateLocal(dt * vx, dt * vy, dt * vz)</code>
1262      * <p>
1263      * Reference: <a href="http://physicsforgames.blogspot.de/2010/02/quaternions.html">http://physicsforgames.blogspot.de/</a>
1264      * 
1265      * @param dt
1266      *          the delta time
1267      * @param vx
1268      *          the angular velocity around the x axis
1269      * @param vy
1270      *          the angular velocity around the y axis
1271      * @param vz
1272      *          the angular velocity around the z axis
1273      * @return this
1274      */
1275     ref public Quaterniond integrate(double dt, double vx, double vy, double vz) return {
1276         integrate(dt, vx, vy, vz, this);
1277         return this;
1278     }
1279 
1280     public Quaterniond integrate(double dt, double vx, double vy, double vz, ref Quaterniond dest) {
1281         double thetaX = dt * vx * 0.5;
1282         double thetaY = dt * vy * 0.5;
1283         double thetaZ = dt * vz * 0.5;
1284         double thetaMagSq = thetaX * thetaX + thetaY * thetaY + thetaZ * thetaZ;
1285         double s;
1286         double dqX, dqY, dqZ, dqW;
1287         if (thetaMagSq * thetaMagSq / 24.0 < 1E-8) {
1288             dqW = 1.0 - thetaMagSq * 0.5;
1289             s = 1.0 - thetaMagSq / 6.0;
1290         } else {
1291             double thetaMag = Math.sqrt(thetaMagSq);
1292             double sin = Math.sin(thetaMag);
1293             s = sin / thetaMag;
1294             dqW = Math.cosFromSin(sin, thetaMag);
1295         }
1296         dqX = thetaX * s;
1297         dqY = thetaY * s;
1298         dqZ = thetaZ * s;
1299         /* Pre-multiplication */
1300         return dest.set(Math.fma(dqW, x, Math.fma(dqX, w, Math.fma(dqY, z, -dqZ * y))),
1301                         Math.fma(dqW, y, Math.fma(-dqX, z, Math.fma(dqY, w, dqZ * x))),
1302                         Math.fma(dqW, z, Math.fma(dqX, y, Math.fma(-dqY, x, dqZ * w))),
1303                         Math.fma(dqW, w, Math.fma(-dqX, x, Math.fma(-dqY, y, -dqZ * z))));
1304     }
1305 
1306     /**
1307      * Compute a linear (non-spherical) interpolation of <code>this</code> and the given quaternion <code>q</code>
1308      * and store the result in <code>this</code>.
1309      * 
1310      * @param q
1311      *          the other quaternion
1312      * @param factor
1313      *          the interpolation factor. It is between 0.0 and 1.0
1314      * @return this
1315      */
1316     ref public Quaterniond nlerp(Quaterniond q, double factor) return {
1317         nlerp(q, factor, this);
1318         return this;
1319     }
1320 
1321     public Quaterniond nlerp(Quaterniond q, double factor, ref Quaterniond dest) {
1322         double cosom = Math.fma(x, q.x, Math.fma(y, q.y, Math.fma(z, q.z, w * q.w)));
1323         double scale0 = 1.0 - factor;
1324         double scale1 = (cosom >= 0.0) ? factor : -factor;
1325         dest.x = Math.fma(scale0, x, scale1 * q.x);
1326         dest.y = Math.fma(scale0, y, scale1 * q.y);
1327         dest.z = Math.fma(scale0, z, scale1 * q.z);
1328         dest.w = Math.fma(scale0, w, scale1 * q.w);
1329         double s = Math.invsqrt(Math.fma(dest.x, dest.x, Math.fma(dest.y, dest.y, Math.fma(dest.z, dest.z, dest.w * dest.w))));
1330         dest.x *= s;
1331         dest.y *= s;
1332         dest.z *= s;
1333         dest.w *= s;
1334         return dest;
1335     }
1336 
1337     /**
1338      * Interpolate between all of the quaternions given in <code>qs</code> via non-spherical linear interpolation using the
1339      * specified interpolation factors <code>weights</code>, and store the result in <code>dest</code>.
1340      * <p>
1341      * This method will interpolate between each two successive quaternions via {@link #nlerp(Quaterniond, double)}
1342      * using their relative interpolation weights.
1343      * <p>
1344      * Reference: <a href="http://gamedev.stackexchange.com/questions/62354/method-for-interpolation-between-3-quaternions#answer-62356">http://gamedev.stackexchange.com/</a>
1345      * 
1346      * @param qs
1347      *          the quaternions to interpolate over
1348      * @param weights
1349      *          the weights of each individual quaternion in <code>qs</code>
1350      * @param dest
1351      *          will hold the result
1352      * @return dest
1353      */
1354     public static Quaterniond nlerp(Quaterniond[] qs, double[] weights, ref Quaterniond dest) {
1355         dest.set(qs[0]);
1356         double w = weights[0];
1357         for (int i = 1; i < qs.length; i++) {
1358             double w0 = w;
1359             double w1 = weights[i];
1360             double rw1 = w1 / (w0 + w1);
1361             w += w1;
1362             dest.nlerp(qs[i], rw1);
1363         }
1364         return dest;
1365     }
1366 
1367     public Quaterniond nlerpIterative(Quaterniond q, double alpha, double dotThreshold, ref Quaterniond dest) {
1368         double q1x = x, q1y = y, q1z = z, q1w = w;
1369         double q2x = q.x, q2y = q.y, q2z = q.z, q2w = q.w;
1370         double dot = Math.fma(q1x, q2x, Math.fma(q1y, q2y, Math.fma(q1z, q2z, q1w * q2w)));
1371         double absDot = Math.abs(dot);
1372         if (1.0 - 1E-6 < absDot) {
1373             return dest.set(this);
1374         }
1375         double alphaN = alpha;
1376         while (absDot < dotThreshold) {
1377             double scale0 = 0.5;
1378             double scale1 = dot >= 0.0 ? 0.5 : -0.5;
1379             if (alphaN < 0.5) {
1380                 q2x = Math.fma(scale0, q2x, scale1 * q1x);
1381                 q2y = Math.fma(scale0, q2y, scale1 * q1y);
1382                 q2z = Math.fma(scale0, q2z, scale1 * q1z);
1383                 q2w = Math.fma(scale0, q2w, scale1 * q1w);
1384                 float s = cast(float) Math.invsqrt(Math.fma(q2x, q2x, Math.fma(q2y, q2y, Math.fma(q2z, q2z, q2w * q2w))));
1385                 q2x *= s;
1386                 q2y *= s;
1387                 q2z *= s;
1388                 q2w *= s;
1389                 alphaN = alphaN + alphaN;
1390             } else {
1391                 q1x = Math.fma(scale0, q1x, scale1 * q2x);
1392                 q1y = Math.fma(scale0, q1y, scale1 * q2y);
1393                 q1z = Math.fma(scale0, q1z, scale1 * q2z);
1394                 q1w = Math.fma(scale0, q1w, scale1 * q2w);
1395                 float s = cast(float) Math.invsqrt(Math.fma(q1x, q1x, Math.fma(q1y, q1y, Math.fma(q1z, q1z, q1w * q1w))));
1396                 q1x *= s;
1397                 q1y *= s;
1398                 q1z *= s;
1399                 q1w *= s;
1400                 alphaN = alphaN + alphaN - 1.0;
1401             }
1402             dot = Math.fma(q1x, q2x, Math.fma(q1y, q2y, Math.fma(q1z, q2z, q1w * q2w)));
1403             absDot = Math.abs(dot);
1404         }
1405         double scale0 = 1.0 - alphaN;
1406         double scale1 = dot >= 0.0 ? alphaN : -alphaN;
1407         double resX = Math.fma(scale0, q1x, scale1 * q2x);
1408         double resY = Math.fma(scale0, q1y, scale1 * q2y);
1409         double resZ = Math.fma(scale0, q1z, scale1 * q2z);
1410         double resW = Math.fma(scale0, q1w, scale1 * q2w);
1411         double s = Math.invsqrt(Math.fma(resX, resX, Math.fma(resY, resY, Math.fma(resZ, resZ, resW * resW))));
1412         dest.x = resX * s;
1413         dest.y = resY * s;
1414         dest.z = resZ * s;
1415         dest.w = resW * s;
1416         return dest;
1417     }
1418 
1419     /**
1420      * Compute linear (non-spherical) interpolations of <code>this</code> and the given quaternion <code>q</code>
1421      * iteratively and store the result in <code>this</code>.
1422      * <p>
1423      * This method performs a series of small-step nlerp interpolations to avoid doing a costly spherical linear interpolation, like
1424      * {@link #slerp(Quaterniond, double, Quaterniond) slerp},
1425      * by subdividing the rotation arc between <code>this</code> and <code>q</code> via non-spherical linear interpolations as long as
1426      * the absolute dot product of <code>this</code> and <code>q</code> is greater than the given <code>dotThreshold</code> parameter.
1427      * <p>
1428      * Thanks to <code>@theagentd</code> at <a href="http://www.java-gaming.org/">http://www.java-gaming.org/</a> for providing the code.
1429      * 
1430      * @param q
1431      *          the other quaternion
1432      * @param alpha
1433      *          the interpolation factor, between 0.0 and 1.0
1434      * @param dotThreshold
1435      *          the threshold for the dot product of <code>this</code> and <code>q</code> above which this method performs another iteration
1436      *          of a small-step linear interpolation
1437      * @return this
1438      */
1439     ref public Quaterniond nlerpIterative(Quaterniond q, double alpha, double dotThreshold) return {
1440         nlerpIterative(q, alpha, dotThreshold, this);
1441         return this;
1442     }
1443 
1444     /**
1445      * Interpolate between all of the quaternions given in <code>qs</code> via iterative non-spherical linear interpolation using the
1446      * specified interpolation factors <code>weights</code>, and store the result in <code>dest</code>.
1447      * <p>
1448      * This method will interpolate between each two successive quaternions via {@link #nlerpIterative(Quaterniond, double, double)}
1449      * using their relative interpolation weights.
1450      * <p>
1451      * Reference: <a href="http://gamedev.stackexchange.com/questions/62354/method-for-interpolation-between-3-quaternions#answer-62356">http://gamedev.stackexchange.com/</a>
1452      * 
1453      * @param qs
1454      *          the quaternions to interpolate over
1455      * @param weights
1456      *          the weights of each individual quaternion in <code>qs</code>
1457      * @param dotThreshold
1458      *          the threshold for the dot product of each two interpolated quaternions above which {@link #nlerpIterative(Quaterniond, double, double)} performs another iteration
1459      *          of a small-step linear interpolation
1460      * @param dest
1461      *          will hold the result
1462      * @return dest
1463      */
1464     public static Quaterniond nlerpIterative(Quaterniond[] qs, double[] weights, double dotThreshold, ref Quaterniond dest) {
1465         dest.set(qs[0]);
1466         double w = weights[0];
1467         for (int i = 1; i < qs.length; i++) {
1468             double w0 = w;
1469             double w1 = weights[i];
1470             double rw1 = w1 / (w0 + w1);
1471             w += w1;
1472             dest.nlerpIterative(qs[i], rw1, dotThreshold);
1473         }
1474         return dest;
1475     }
1476 
1477     /**
1478      * Apply a rotation to this quaternion that maps the given direction to the positive Z axis.
1479      * <p>
1480      * Because there are multiple possibilities for such a rotation, this method will choose the one that ensures the given up direction to remain
1481      * parallel to the plane spanned by the <code>up</code> and <code>dir</code> vectors. 
1482      * <p>
1483      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1484      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1485      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1486      * rotation added by this method will be applied first!
1487      * <p>
1488      * Reference: <a href="http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html">http://answers.unity3d.com</a>
1489      * 
1490      * @see #lookAlong(double, double, double, double, double, double, Quaterniond)
1491      * 
1492      * @param dir
1493      *              the direction to map to the positive Z axis
1494      * @param up
1495      *              the vector which will be mapped to a vector parallel to the plane
1496      *              spanned by the given <code>dir</code> and <code>up</code>
1497      * @return this
1498      */
1499     ref public Quaterniond lookAlong(Vector3d dir, Vector3d up) return {
1500         lookAlong(dir.x, dir.y, dir.z, up.x, up.y, up.z, this);
1501         return this;
1502     }
1503 
1504     public Quaterniond lookAlong(Vector3d dir, Vector3d up, ref Quaterniond dest) {
1505         return lookAlong(dir.x, dir.y, dir.z, up.x, up.y, up.z, dest);
1506     }
1507 
1508     /**
1509      * Apply a rotation to this quaternion that maps the given direction to the positive Z axis.
1510      * <p>
1511      * Because there are multiple possibilities for such a rotation, this method will choose the one that ensures the given up direction to remain
1512      * parallel to the plane spanned by the <code>up</code> and <code>dir</code> vectors. 
1513      * <p>
1514      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1515      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1516      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1517      * rotation added by this method will be applied first!
1518      * <p>
1519      * Reference: <a href="http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html">http://answers.unity3d.com</a>
1520      * 
1521      * @see #lookAlong(double, double, double, double, double, double, Quaterniond)
1522      * 
1523      * @param dirX
1524      *              the x-coordinate of the direction to look along
1525      * @param dirY
1526      *              the y-coordinate of the direction to look along
1527      * @param dirZ
1528      *              the z-coordinate of the direction to look along
1529      * @param upX
1530      *              the x-coordinate of the up vector
1531      * @param upY
1532      *              the y-coordinate of the up vector
1533      * @param upZ
1534      *              the z-coordinate of the up vector
1535      * @return this
1536      */
1537     ref public Quaterniond lookAlong(double dirX, double dirY, double dirZ, double upX, double upY, double upZ) return {
1538         lookAlong(dirX, dirY, dirZ, upX, upY, upZ, this);
1539         return this;
1540     }
1541 
1542     public Quaterniond lookAlong(double dirX, double dirY, double dirZ, double upX, double upY, double upZ, ref Quaterniond dest) {
1543         // Normalize direction
1544         double invDirLength = Math.invsqrt(dirX * dirX + dirY * dirY + dirZ * dirZ);
1545         double dirnX = -dirX * invDirLength;
1546         double dirnY = -dirY * invDirLength;
1547         double dirnZ = -dirZ * invDirLength;
1548         // left = up x dir
1549         double leftX, leftY, leftZ;
1550         leftX = upY * dirnZ - upZ * dirnY;
1551         leftY = upZ * dirnX - upX * dirnZ;
1552         leftZ = upX * dirnY - upY * dirnX;
1553         // normalize left
1554         double invLeftLength = Math.invsqrt(leftX * leftX + leftY * leftY + leftZ * leftZ);
1555         leftX *= invLeftLength;
1556         leftY *= invLeftLength;
1557         leftZ *= invLeftLength;
1558         // up = direction x left
1559         double upnX = dirnY * leftZ - dirnZ * leftY;
1560         double upnY = dirnZ * leftX - dirnX * leftZ;
1561         double upnZ = dirnX * leftY - dirnY * leftX;
1562 
1563         /* Convert orthonormal basis vectors to quaternion */
1564         double x, y, z, w;
1565         double t;
1566         double tr = leftX + upnY + dirnZ;
1567         if (tr >= 0.0) {
1568             t = Math.sqrt(tr + 1.0);
1569             w = t * 0.5;
1570             t = 0.5 / t;
1571             x = (dirnY - upnZ) * t;
1572             y = (leftZ - dirnX) * t;
1573             z = (upnX - leftY) * t;
1574         } else {
1575             if (leftX > upnY && leftX > dirnZ) {
1576                 t = Math.sqrt(1.0 + leftX - upnY - dirnZ);
1577                 x = t * 0.5;
1578                 t = 0.5 / t;
1579                 y = (leftY + upnX) * t;
1580                 z = (dirnX + leftZ) * t;
1581                 w = (dirnY - upnZ) * t;
1582             } else if (upnY > dirnZ) {
1583                 t = Math.sqrt(1.0 + upnY - leftX - dirnZ);
1584                 y = t * 0.5;
1585                 t = 0.5 / t;
1586                 x = (leftY + upnX) * t;
1587                 z = (upnZ + dirnY) * t;
1588                 w = (leftZ - dirnX) * t;
1589             } else {
1590                 t = Math.sqrt(1.0 + dirnZ - leftX - upnY);
1591                 z = t * 0.5;
1592                 t = 0.5 / t;
1593                 x = (dirnX + leftZ) * t;
1594                 y = (upnZ + dirnY) * t;
1595                 w = (upnX - leftY) * t;
1596             }
1597         }
1598         /* Multiply */
1599         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
1600                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
1601                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
1602                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
1603     }
1604 
1605     public int hashCode() {
1606         immutable int prime = 31;
1607         int result = 1;
1608         long temp;
1609         temp = Math.doubleToLongBits(w);
1610         result = prime * result + cast(int) (temp ^ (temp >>> 32));
1611         temp = Math.doubleToLongBits(x);
1612         result = prime * result + cast(int) (temp ^ (temp >>> 32));
1613         temp = Math.doubleToLongBits(y);
1614         result = prime * result + cast(int) (temp ^ (temp >>> 32));
1615         temp = Math.doubleToLongBits(z);
1616         result = prime * result + cast(int) (temp ^ (temp >>> 32));
1617         return result;
1618     }
1619 
1620     public bool equals(Quaterniond other) {
1621         if (this == other)
1622             return true;
1623         if (Math.doubleToLongBits(w) != Math.doubleToLongBits(other.w))
1624             return false;
1625         if (Math.doubleToLongBits(x) != Math.doubleToLongBits(other.x))
1626             return false;
1627         if (Math.doubleToLongBits(y) != Math.doubleToLongBits(other.y))
1628             return false;
1629         if (Math.doubleToLongBits(z) != Math.doubleToLongBits(other.z))
1630             return false;
1631         return true;
1632     }
1633 
1634     /**
1635      * Compute the difference between <code>this</code> and the <code>other</code> quaternion
1636      * and store the result in <code>this</code>.
1637      * <p>
1638      * The difference is the rotation that has to be applied to get from
1639      * <code>this</code> rotation to <code>other</code>. If <code>T</code> is <code>this</code>, <code>Q</code>
1640      * is <code>other</code> and <code>D</code> is the computed difference, then the following equation holds:
1641      * <p>
1642      * <code>T * D = Q</code>
1643      * <p>
1644      * It is defined as: <code>D = T^-1 * Q</code>, where <code>T^-1</code> denotes the {@link #invert() inverse} of <code>T</code>.
1645      * 
1646      * @param other
1647      *          the other quaternion
1648      * @return this
1649      */
1650     ref public Quaterniond difference(Quaterniond other) return {
1651         difference(other, this);
1652         return this;
1653     }
1654 
1655     public Quaterniond difference(Quaterniond other, ref Quaterniond dest) {
1656         double invNorm = 1.0 / lengthSquared();
1657         double x = -this.x * invNorm;
1658         double y = -this.y * invNorm;
1659         double z = -this.z * invNorm;
1660         double w = this.w * invNorm;
1661         dest.set(Math.fma(w, other.x, Math.fma(x, other.w, Math.fma(y, other.z, -z * other.y))),
1662                  Math.fma(w, other.y, Math.fma(-x, other.z, Math.fma(y, other.w, z * other.x))),
1663                  Math.fma(w, other.z, Math.fma(x, other.y, Math.fma(-y, other.x, z * other.w))),
1664                  Math.fma(w, other.w, Math.fma(-x, other.x, Math.fma(-y, other.y, -z * other.z))));
1665         return dest;
1666     }
1667 
1668     /**
1669      * Set <code>this</code> quaternion to a rotation that rotates the <code>fromDir</code> vector to point along <code>toDir</code>.
1670      * <p>
1671      * Since there can be multiple possible rotations, this method chooses the one with the shortest arc.
1672      * <p>
1673      * Reference: <a href="http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another#answer-1171995">stackoverflow.com</a>
1674      * 
1675      * @param fromDirX
1676      *              the x-coordinate of the direction to rotate into the destination direction
1677      * @param fromDirY
1678      *              the y-coordinate of the direction to rotate into the destination direction
1679      * @param fromDirZ
1680      *              the z-coordinate of the direction to rotate into the destination direction
1681      * @param toDirX
1682      *              the x-coordinate of the direction to rotate to
1683      * @param toDirY
1684      *              the y-coordinate of the direction to rotate to
1685      * @param toDirZ
1686      *              the z-coordinate of the direction to rotate to
1687      * @return this
1688      */
1689     ref public Quaterniond rotationTo(double fromDirX, double fromDirY, double fromDirZ, double toDirX, double toDirY, double toDirZ) return {
1690         double fn = Math.invsqrt(Math.fma(fromDirX, fromDirX, Math.fma(fromDirY, fromDirY, fromDirZ * fromDirZ)));
1691         double tn = Math.invsqrt(Math.fma(toDirX, toDirX, Math.fma(toDirY, toDirY, toDirZ * toDirZ)));
1692         double fx = fromDirX * fn, fy = fromDirY * fn, fz = fromDirZ * fn;
1693         double tx = toDirX * tn, ty = toDirY * tn, tz = toDirZ * tn;
1694         double dot = fx * tx + fy * ty + fz * tz;
1695         double x, y, z, w;
1696         if (dot < -1.0 + 1E-6) {
1697             x = fy;
1698             y = -fx;
1699             z = 0.0;
1700             w = 0.0;
1701             if (x * x + y * y == 0.0) {
1702                 x = 0.0;
1703                 y = fz;
1704                 z = -fy;
1705                 w = 0.0;
1706             }
1707             this.x = x;
1708             this.y = y;
1709             this.z = z;
1710             this.w = 0;
1711         } else {
1712             double sd2 = Math.sqrt((1.0 + dot) * 2.0);
1713             double isd2 = 1.0 / sd2;
1714             double cx = fy * tz - fz * ty;
1715             double cy = fz * tx - fx * tz;
1716             double cz = fx * ty - fy * tx;
1717             x = cx * isd2;
1718             y = cy * isd2;
1719             z = cz * isd2;
1720             w = sd2 * 0.5;
1721             double n2 = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, Math.fma(z, z, w * w))));
1722             this.x = x * n2;
1723             this.y = y * n2;
1724             this.z = z * n2;
1725             this.w = w * n2;
1726         }
1727         return this;
1728     }
1729 
1730     /**
1731      * Set <code>this</code> quaternion to a rotation that rotates the <code>fromDir</code> vector to point along <code>toDir</code>.
1732      * <p>
1733      * Because there can be multiple possible rotations, this method chooses the one with the shortest arc.
1734      * 
1735      * @see #rotationTo(double, double, double, double, double, double)
1736      * 
1737      * @param fromDir
1738      *          the starting direction
1739      * @param toDir
1740      *          the destination direction
1741      * @return this
1742      */
1743     ref public Quaterniond rotationTo(Vector3d fromDir, Vector3d toDir) return {
1744         return rotationTo(fromDir.x, fromDir.y, fromDir.z, toDir.x, toDir.y, toDir.z);
1745     }
1746 
1747     public Quaterniond rotateTo(double fromDirX, double fromDirY, double fromDirZ,
1748                                 double toDirX, double toDirY, double toDirZ, ref Quaterniond dest) {
1749         double fn = Math.invsqrt(Math.fma(fromDirX, fromDirX, Math.fma(fromDirY, fromDirY, fromDirZ * fromDirZ)));
1750         double tn = Math.invsqrt(Math.fma(toDirX, toDirX, Math.fma(toDirY, toDirY, toDirZ * toDirZ)));
1751         double fx = fromDirX * fn, fy = fromDirY * fn, fz = fromDirZ * fn;
1752         double tx = toDirX * tn, ty = toDirY * tn, tz = toDirZ * tn;
1753         double dot = fx * tx + fy * ty + fz * tz;
1754         double x, y, z, w;
1755         if (dot < -1.0 + 1E-6) {
1756             x = fy;
1757             y = -fx;
1758             z = 0.0;
1759             w = 0.0;
1760             if (x * x + y * y == 0.0) {
1761                 x = 0.0;
1762                 y = fz;
1763                 z = -fy;
1764                 w = 0.0;
1765             }
1766         } else {
1767             double sd2 = Math.sqrt((1.0 + dot) * 2.0);
1768             double isd2 = 1.0 / sd2;
1769             double cx = fy * tz - fz * ty;
1770             double cy = fz * tx - fx * tz;
1771             double cz = fx * ty - fy * tx;
1772             x = cx * isd2;
1773             y = cy * isd2;
1774             z = cz * isd2;
1775             w = sd2 * 0.5;
1776             double n2 = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, Math.fma(z, z, w * w))));
1777             x *= n2;
1778             y *= n2;
1779             z *= n2;
1780             w *= n2;
1781         }
1782         /* Multiply */
1783         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
1784                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
1785                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
1786                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
1787     }
1788 
1789     /**
1790      * Set this quaternion to a rotation of the given angle in radians about the supplied axis.
1791      * 
1792      * @param angle
1793      *          the rotation angle in radians
1794      * @param axisX
1795      *          the x-coordinate of the rotation axis
1796      * @param axisY
1797      *          the y-coordinate of the rotation axis
1798      * @param axisZ
1799      *          the z-coordinate of the rotation axis
1800      * @return this
1801      */
1802     ref public Quaterniond rotationAxis(double angle, double axisX, double axisY, double axisZ) return {
1803         double hangle = angle / 2.0;
1804         double sinAngle = Math.sin(hangle);
1805         double invVLength = Math.invsqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
1806         return set(axisX * invVLength * sinAngle,
1807                    axisY * invVLength * sinAngle,
1808                    axisZ * invVLength * sinAngle,
1809                    Math.cosFromSin(sinAngle, hangle));
1810     }
1811 
1812     /**
1813      * Set this quaternion to represent a rotation of the given radians about the x axis.
1814      * 
1815      * @param angle
1816      *              the angle in radians to rotate about the x axis
1817      * @return this
1818      */
1819     ref public Quaterniond rotationX(double angle) return {
1820         double sin = Math.sin(angle * 0.5);
1821         double cos = Math.cosFromSin(sin, angle * 0.5);
1822         return set(sin, 0, cos, 0);
1823     }
1824 
1825     /**
1826      * Set this quaternion to represent a rotation of the given radians about the y axis.
1827      * 
1828      * @param angle
1829      *              the angle in radians to rotate about the y axis
1830      * @return this
1831      */
1832     ref public Quaterniond rotationY(double angle) return {
1833         double sin = Math.sin(angle * 0.5);
1834         double cos = Math.cosFromSin(sin, angle * 0.5);
1835         return set(0, sin, 0, cos);
1836     }
1837 
1838     /**
1839      * Set this quaternion to represent a rotation of the given radians about the z axis.
1840      * 
1841      * @param angle
1842      *              the angle in radians to rotate about the z axis
1843      * @return this
1844      */
1845     ref public Quaterniond rotationZ(double angle) return {
1846         double sin = Math.sin(angle * 0.5);
1847         double cos = Math.cosFromSin(sin, angle * 0.5);
1848         return set(0, 0, sin, cos);
1849     }
1850 
1851     /**
1852      * Apply a rotation to <code>this</code> that rotates the <code>fromDir</code> vector to point along <code>toDir</code>.
1853      * <p>
1854      * Since there can be multiple possible rotations, this method chooses the one with the shortest arc.
1855      * <p>
1856      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1857      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1858      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1859      * rotation added by this method will be applied first!
1860      * 
1861      * @see #rotateTo(double, double, double, double, double, double, Quaterniond)
1862      * 
1863      * @param fromDirX
1864      *              the x-coordinate of the direction to rotate into the destination direction
1865      * @param fromDirY
1866      *              the y-coordinate of the direction to rotate into the destination direction
1867      * @param fromDirZ
1868      *              the z-coordinate of the direction to rotate into the destination direction
1869      * @param toDirX
1870      *              the x-coordinate of the direction to rotate to
1871      * @param toDirY
1872      *              the y-coordinate of the direction to rotate to
1873      * @param toDirZ
1874      *              the z-coordinate of the direction to rotate to
1875      * @return this
1876      */
1877     ref public Quaterniond rotateTo(double fromDirX, double fromDirY, double fromDirZ, double toDirX, double toDirY, double toDirZ) return {
1878         rotateTo(fromDirX, fromDirY, fromDirZ, toDirX, toDirY, toDirZ, this);
1879         return this;
1880     }
1881 
1882     public Quaterniond rotateTo(Vector3d fromDir, Vector3d toDir, ref Quaterniond dest) {
1883         return rotateTo(fromDir.x, fromDir.y, fromDir.z, toDir.x, toDir.y, toDir.z, dest);
1884     }
1885 
1886     /**
1887      * Apply a rotation to <code>this</code> that rotates the <code>fromDir</code> vector to point along <code>toDir</code>.
1888      * <p>
1889      * Because there can be multiple possible rotations, this method chooses the one with the shortest arc.
1890      * <p>
1891      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1892      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1893      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1894      * rotation added by this method will be applied first!
1895      * 
1896      * @see #rotateTo(double, double, double, double, double, double, Quaterniond)
1897      * 
1898      * @param fromDir
1899      *          the starting direction
1900      * @param toDir
1901      *          the destination direction
1902      * @return this
1903      */
1904     ref public Quaterniond rotateTo(Vector3d fromDir, Vector3d toDir) return {
1905         rotateTo(fromDir.x, fromDir.y, fromDir.z, toDir.x, toDir.y, toDir.z, this);
1906         return this;
1907     }
1908 
1909     /**
1910      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the x axis.
1911      * <p>
1912      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1913      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1914      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1915      * rotation added by this method will be applied first!
1916      * 
1917      * @param angle
1918      *              the angle in radians to rotate about the x axis
1919      * @return this
1920      */
1921     ref public Quaterniond rotateX(double angle) return {
1922         rotateX(angle, this);
1923         return this;
1924     }
1925 
1926     public Quaterniond rotateX(double angle, ref Quaterniond dest) {
1927         double sin = Math.sin(angle * 0.5);
1928         double cos = Math.cosFromSin(sin, angle * 0.5);
1929         return dest.set(w * sin + x * cos,
1930                         y * cos + z * sin,
1931                         z * cos - y * sin,
1932                         w * cos - x * sin);
1933     }
1934 
1935     /**
1936      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the y axis.
1937      * <p>
1938      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1939      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1940      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1941      * rotation added by this method will be applied first!
1942      * 
1943      * @param angle
1944      *              the angle in radians to rotate about the y axis
1945      * @return this
1946      */
1947     ref public Quaterniond rotateY(double angle) return {
1948         rotateY(angle, this);
1949         return this;
1950     }
1951 
1952     public Quaterniond rotateY(double angle, ref Quaterniond dest) {
1953         double sin = Math.sin(angle * 0.5);
1954         double cos = Math.cosFromSin(sin, angle * 0.5);
1955         return dest.set(x * cos - z * sin,
1956                         w * sin + y * cos,
1957                         x * sin + z * cos,
1958                         w * cos - y * sin);
1959     }
1960 
1961     /**
1962      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the z axis.
1963      * <p>
1964      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1965      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
1966      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
1967      * rotation added by this method will be applied first!
1968      * 
1969      * @param angle
1970      *              the angle in radians to rotate about the z axis
1971      * @return this
1972      */
1973     ref public Quaterniond rotateZ(double angle) return {
1974         rotateZ(angle, this);
1975         return this;
1976     }
1977 
1978     public Quaterniond rotateZ(double angle, ref Quaterniond dest) {
1979         double sin = Math.sin(angle * 0.5);
1980         double cos = Math.cosFromSin(sin, angle * 0.5);
1981         return dest.set(x * cos + y * sin,
1982                         y * cos - x * sin,
1983                         w * sin + z * cos,
1984                         w * cos - z * sin);
1985     }
1986 
1987     /**
1988      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the local x axis.
1989      * <p>
1990      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
1991      * specified rotation, then the new quaternion will be <code>R * Q</code>. So when transforming a
1992      * vector <code>v</code> with the new quaternion by using <code>R * Q * v</code>, the
1993      * rotation represented by <code>this</code> will be applied first!
1994      * 
1995      * @param angle
1996      *              the angle in radians to rotate about the local x axis
1997      * @return this
1998      */
1999     ref public Quaterniond rotateLocalX(double angle) return {
2000         rotateLocalX(angle, this);
2001         return this;
2002     }
2003 
2004     public Quaterniond rotateLocalX(double angle, ref Quaterniond dest) {
2005         double hangle = angle * 0.5;
2006         double s = Math.sin(hangle);
2007         double c = Math.cosFromSin(s, hangle);
2008         dest.set(c * x + s * w,
2009                  c * y - s * z,
2010                  c * z + s * y,
2011                  c * w - s * x);
2012         return dest;
2013     }
2014 
2015     /**
2016      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the local y axis.
2017      * <p>
2018      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2019      * specified rotation, then the new quaternion will be <code>R * Q</code>. So when transforming a
2020      * vector <code>v</code> with the new quaternion by using <code>R * Q * v</code>, the
2021      * rotation represented by <code>this</code> will be applied first!
2022      * 
2023      * @param angle
2024      *              the angle in radians to rotate about the local y axis
2025      * @return this
2026      */
2027     ref public Quaterniond rotateLocalY(double angle) return {
2028         rotateLocalY(angle, this);
2029         return this;
2030     }
2031 
2032     public Quaterniond rotateLocalY(double angle, ref Quaterniond dest) {
2033         double hangle = angle * 0.5;
2034         double s = Math.sin(hangle);
2035         double c = Math.cosFromSin(s, hangle);
2036         dest.set(c * x + s * z,
2037                  c * y + s * w,
2038                  c * z - s * x,
2039                  c * w - s * y);
2040         return dest;
2041     }
2042 
2043     /**
2044      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the local z axis.
2045      * <p>
2046      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2047      * specified rotation, then the new quaternion will be <code>R * Q</code>. So when transforming a
2048      * vector <code>v</code> with the new quaternion by using <code>R * Q * v</code>, the
2049      * rotation represented by <code>this</code> will be applied first!
2050      * 
2051      * @param angle
2052      *              the angle in radians to rotate about the local z axis
2053      * @return this
2054      */
2055     ref public Quaterniond rotateLocalZ(double angle) return {
2056         rotateLocalZ(angle, this);
2057         return this;
2058     }
2059 
2060     public Quaterniond rotateLocalZ(double angle, ref Quaterniond dest) {
2061         double hangle = angle * 0.5;
2062         double s = Math.sin(hangle);
2063         double c = Math.cosFromSin(s, hangle);
2064         dest.set(c * x - s * y,
2065                  c * y + s * x,
2066                  c * z + s * w,
2067                  c * w - s * z);
2068         return dest;
2069     }
2070 
2071     /**
2072      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the cartesian base unit axes,
2073      * called the euler angles using rotation sequence <code>XYZ</code>.
2074      * <p>
2075      * This method is equivalent to calling: <code>rotateX(angleX).rotateY(angleY).rotateZ(angleZ)</code>
2076      * <p>
2077      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2078      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
2079      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
2080      * rotation added by this method will be applied first!
2081      * 
2082      * @param angleX
2083      *              the angle in radians to rotate about the x axis
2084      * @param angleY
2085      *              the angle in radians to rotate about the y axis
2086      * @param angleZ
2087      *              the angle in radians to rotate about the z axis
2088      * @return this
2089      */
2090     ref public Quaterniond rotateXYZ(double angleX, double angleY, double angleZ) return {
2091         rotateXYZ(angleX, angleY, angleZ, this);
2092         return this;
2093     }
2094 
2095     public Quaterniond rotateXYZ(double angleX, double angleY, double angleZ, ref Quaterniond dest) {
2096         double sx =  Math.sin(angleX * 0.5);
2097         double cx =  Math.cosFromSin(sx, angleX * 0.5);
2098         double sy =  Math.sin(angleY * 0.5);
2099         double cy =  Math.cosFromSin(sy, angleY * 0.5);
2100         double sz =  Math.sin(angleZ * 0.5);
2101         double cz =  Math.cosFromSin(sz, angleZ * 0.5);
2102 
2103         double cycz = cy * cz;
2104         double sysz = sy * sz;
2105         double sycz = sy * cz;
2106         double cysz = cy * sz;
2107         double w = cx*cycz - sx*sysz;
2108         double x = sx*cycz + cx*sysz;
2109         double y = cx*sycz - sx*cysz;
2110         double z = cx*cysz + sx*sycz;
2111         // right-multiply
2112         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
2113                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
2114                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
2115                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
2116     }
2117 
2118     /**
2119      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the cartesian base unit axes,
2120      * called the euler angles, using the rotation sequence <code>ZYX</code>.
2121      * <p>
2122      * This method is equivalent to calling: <code>rotateZ(angleZ).rotateY(angleY).rotateX(angleX)</code>
2123      * <p>
2124      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2125      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
2126      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
2127      * rotation added by this method will be applied first!
2128      * 
2129      * @param angleZ
2130      *              the angle in radians to rotate about the z axis
2131      * @param angleY
2132      *              the angle in radians to rotate about the y axis
2133      * @param angleX
2134      *              the angle in radians to rotate about the x axis
2135      * @return this
2136      */
2137     ref public Quaterniond rotateZYX(double angleZ, double angleY, double angleX) return {
2138         rotateZYX(angleZ, angleY, angleX, this);
2139         return this;
2140     }
2141 
2142     public Quaterniond rotateZYX(double angleZ, double angleY, double angleX, ref Quaterniond dest) {
2143         double sx =  Math.sin(angleX * 0.5);
2144         double cx =  Math.cosFromSin(sx, angleX * 0.5);
2145         double sy =  Math.sin(angleY * 0.5);
2146         double cy =  Math.cosFromSin(sy, angleY * 0.5);
2147         double sz =  Math.sin(angleZ * 0.5);
2148         double cz =  Math.cosFromSin(sz, angleZ * 0.5);
2149 
2150         double cycz = cy * cz;
2151         double sysz = sy * sz;
2152         double sycz = sy * cz;
2153         double cysz = cy * sz;
2154         double w = cx*cycz + sx*sysz;
2155         double x = sx*cycz - cx*sysz;
2156         double y = cx*sycz + sx*cysz;
2157         double z = cx*cysz - sx*sycz;
2158         // right-multiply
2159         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
2160                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
2161                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
2162                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
2163     }
2164 
2165     /**
2166      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the cartesian base unit axes,
2167      * called the euler angles, using the rotation sequence <code>YXZ</code>.
2168      * <p>
2169      * This method is equivalent to calling: <code>rotateY(angleY).rotateX(angleX).rotateZ(angleZ)</code>
2170      * <p>
2171      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2172      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
2173      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
2174      * rotation added by this method will be applied first!
2175      * 
2176      * @param angleY
2177      *              the angle in radians to rotate about the y axis
2178      * @param angleX
2179      *              the angle in radians to rotate about the x axis
2180      * @param angleZ
2181      *              the angle in radians to rotate about the z axis
2182      * @return this
2183      */
2184     ref public Quaterniond rotateYXZ(double angleY, double angleX, double angleZ) return {
2185         rotateYXZ(angleY, angleX, angleZ, this);
2186         return this;
2187     }
2188 
2189     public Quaterniond rotateYXZ(double angleY, double angleX, double angleZ, ref Quaterniond dest) {
2190         double sx =  Math.sin(angleX * 0.5);
2191         double cx =  Math.cosFromSin(sx, angleX * 0.5);
2192         double sy =  Math.sin(angleY * 0.5);
2193         double cy =  Math.cosFromSin(sy, angleY * 0.5);
2194         double sz =  Math.sin(angleZ * 0.5);
2195         double cz =  Math.cosFromSin(sz, angleZ * 0.5);
2196 
2197         double yx = cy * sx;
2198         double yy = sy * cx;
2199         double yz = sy * sx;
2200         double yw = cy * cx;
2201         double x = yx * cz + yy * sz;
2202         double y = yy * cz - yx * sz;
2203         double z = yw * sz - yz * cz;
2204         double w = yw * cz + yz * sz;
2205         // right-multiply
2206         return dest.set(Math.fma(this.w, x, Math.fma(this.x, w, Math.fma(this.y, z, -this.z * y))),
2207                         Math.fma(this.w, y, Math.fma(-this.x, z, Math.fma(this.y, w, this.z * x))),
2208                         Math.fma(this.w, z, Math.fma(this.x, y, Math.fma(-this.y, x, this.z * w))),
2209                         Math.fma(this.w, w, Math.fma(-this.x, x, Math.fma(-this.y, y, -this.z * z))));
2210     }
2211 
2212     public Vector3d getEulerAnglesXYZ(Vector3d eulerAngles) {
2213         eulerAngles.x = Math.atan2(x * w - y * z, 0.5 - x * x - y * y);
2214         eulerAngles.y = Math.safeAsin(2.0 * (x * z + y * w));
2215         eulerAngles.z = Math.atan2(z * w - x * y, 0.5 - y * y - z * z);
2216         return eulerAngles;
2217     }
2218 
2219     public Vector3d getEulerAnglesZYX(Vector3d eulerAngles) {
2220         eulerAngles.x = Math.atan2(y * z + w * x, 0.5 - x * x + y * y);
2221         eulerAngles.y = Math.safeAsin(-2.0 * (x * z - w * y));
2222         eulerAngles.z = Math.atan2(x * y + w * z, 0.5 - y * y - z * z);
2223         return eulerAngles;
2224     }
2225 
2226     public Vector3d getEulerAnglesZXY(Vector3d eulerAngles) {
2227         eulerAngles.x = Math.safeAsin(2.0 * (w * x + y * z));
2228         eulerAngles.y = Math.atan2(w * y - x * z, 0.5 - y * y - x * x);
2229         eulerAngles.z = Math.atan2(w * z - x * y, 0.5 - z * z - x * x);
2230         return eulerAngles;
2231     }
2232 
2233     public Vector3d getEulerAnglesYXZ(Vector3d eulerAngles) {
2234         eulerAngles.x = Math.safeAsin(-2.0 * (y * z - w * x));
2235         eulerAngles.y = Math.atan2(x * z + y * w, 0.5 - y * y - x * x);
2236         eulerAngles.z = Math.atan2(y * x + w * z, 0.5 - x * x - z * z);
2237         return eulerAngles;
2238     }
2239 
2240     public Quaterniond rotateAxis(double angle, double axisX, double axisY, double axisZ, ref Quaterniond dest) {
2241         double hangle = angle / 2.0;
2242         double sinAngle = Math.sin(hangle);
2243         double invVLength = Math.invsqrt(Math.fma(axisX, axisX, Math.fma(axisY, axisY, axisZ * axisZ)));
2244         double rx = axisX * invVLength * sinAngle;
2245         double ry = axisY * invVLength * sinAngle;
2246         double rz = axisZ * invVLength * sinAngle;
2247         double rw = Math.cosFromSin(sinAngle, hangle);
2248         return dest.set(Math.fma(this.w, rx, Math.fma(this.x, rw, Math.fma(this.y, rz, -this.z * ry))),
2249                         Math.fma(this.w, ry, Math.fma(-this.x, rz, Math.fma(this.y, rw, this.z * rx))),
2250                         Math.fma(this.w, rz, Math.fma(this.x, ry, Math.fma(-this.y, rx, this.z * rw))),
2251                         Math.fma(this.w, rw, Math.fma(-this.x, rx, Math.fma(-this.y, ry, -this.z * rz))));
2252     }
2253 
2254     public Quaterniond rotateAxis(double angle, Vector3d axis, ref Quaterniond dest) {
2255         return rotateAxis(angle, axis.x, axis.y, axis.z, dest);
2256     }
2257 
2258     /**
2259      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the specified axis.
2260      * <p>
2261      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2262      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
2263      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
2264      * rotation added by this method will be applied first!
2265      * 
2266      * @see #rotateAxis(double, double, double, double, Quaterniond)
2267      * 
2268      * @param angle
2269      *              the angle in radians to rotate about the specified axis
2270      * @param axis
2271      *              the rotation axis
2272      * @return this
2273      */
2274     ref public Quaterniond rotateAxis(double angle, Vector3d axis) return {
2275         rotateAxis(angle, axis.x, axis.y, axis.z, this);
2276         return this;
2277     }
2278 
2279     /**
2280      * Apply a rotation to <code>this</code> quaternion rotating the given radians about the specified axis.
2281      * <p>
2282      * If <code>Q</code> is <code>this</code> quaternion and <code>R</code> the quaternion representing the 
2283      * specified rotation, then the new quaternion will be <code>Q * R</code>. So when transforming a
2284      * vector <code>v</code> with the new quaternion by using <code>Q * R * v</code>, the
2285      * rotation added by this method will be applied first!
2286      * 
2287      * @see #rotateAxis(double, double, double, double, Quaterniond)
2288      * 
2289      * @param angle
2290      *              the angle in radians to rotate about the specified axis
2291      * @param axisX
2292      *              the x coordinate of the rotation axis
2293      * @param axisY
2294      *              the y coordinate of the rotation axis
2295      * @param axisZ
2296      *              the z coordinate of the rotation axis
2297      * @return this
2298      */
2299     ref public Quaterniond rotateAxis(double angle, double axisX, double axisY, double axisZ) return {
2300         rotateAxis(angle, axisX, axisY, axisZ, this);
2301         return this;
2302     }
2303 
2304     public Vector3d positiveX(Vector3d dir) {
2305         double invNorm = 1.0 / lengthSquared();
2306         double nx = -x * invNorm;
2307         double ny = -y * invNorm;
2308         double nz = -z * invNorm;
2309         double nw =  w * invNorm;
2310         double dy = ny + ny;
2311         double dz = nz + nz;
2312         dir.x = -ny * dy - nz * dz + 1.0;
2313         dir.y =  nx * dy + nw * dz;
2314         dir.z =  nx * dz - nw * dy;
2315         return dir;
2316     }
2317 
2318     public Vector3d normalizedPositiveX(Vector3d dir) {
2319         double dy = y + y;
2320         double dz = z + z;
2321         dir.x = -y * dy - z * dz + 1.0;
2322         dir.y =  x * dy - w * dz;
2323         dir.z =  x * dz + w * dy;
2324         return dir;
2325     }
2326 
2327     public Vector3d positiveY(Vector3d dir) {
2328         double invNorm = 1.0 / lengthSquared();
2329         double nx = -x * invNorm;
2330         double ny = -y * invNorm;
2331         double nz = -z * invNorm;
2332         double nw =  w * invNorm;
2333         double dx = nx + nx;
2334         double dy = ny + ny;
2335         double dz = nz + nz;
2336         dir.x =  nx * dy - nw * dz;
2337         dir.y = -nx * dx - nz * dz + 1.0;
2338         dir.z =  ny * dz + nw * dx;
2339         return dir;
2340     }
2341 
2342     public Vector3d normalizedPositiveY(Vector3d dir) {
2343         double dx = x + x;
2344         double dy = y + y;
2345         double dz = z + z;
2346         dir.x =  x * dy + w * dz;
2347         dir.y = -x * dx - z * dz + 1.0;
2348         dir.z =  y * dz - w * dx;
2349         return dir;
2350     }
2351 
2352     public Vector3d positiveZ(Vector3d dir) {
2353         double invNorm = 1.0 / lengthSquared();
2354         double nx = -x * invNorm;
2355         double ny = -y * invNorm;
2356         double nz = -z * invNorm;
2357         double nw =  w * invNorm;
2358         double dx = nx + nx;
2359         double dy = ny + ny;
2360         double dz = nz + nz;
2361         dir.x =  nx * dz + nw * dy;
2362         dir.y =  ny * dz - nw * dx;
2363         dir.z = -nx * dx - ny * dy + 1.0;
2364         return dir;
2365     }
2366 
2367     public Vector3d normalizedPositiveZ(Vector3d dir) {
2368         double dx = x + x;
2369         double dy = y + y;
2370         double dz = z + z;
2371         dir.x =  x * dz - w * dy;
2372         dir.y =  y * dz + w * dx;
2373         dir.z = -x * dx - y * dy + 1.0;
2374         return dir;
2375     }
2376 
2377     /**
2378      * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>.
2379      * 
2380      * @param q
2381      *          the {@link Quaterniond} to conjugate <code>this</code> by
2382      * @return this
2383      */
2384     ref public Quaterniond conjugateBy(Quaterniond q) return {
2385         conjugateBy(q, this);
2386         return this;
2387     }
2388 
2389     /**
2390      * Conjugate <code>this</code> by the given quaternion <code>q</code> by computing <code>q * this * q^-1</code>
2391      * and store the result into <code>dest</code>.
2392      * 
2393      * @param q
2394      *          the {@link Quaterniond} to conjugate <code>this</code> by
2395      * @param dest
2396      *          will hold the result
2397      * @return dest
2398      */
2399     public Quaterniond conjugateBy(Quaterniond q, ref Quaterniond dest) {
2400         double invNorm = 1.0 / q.lengthSquared();
2401         double qix = -q.x * invNorm, qiy = -q.y * invNorm, qiz = -q.z * invNorm, qiw = q.w * invNorm;
2402         double qpx = Math.fma(q.w, x, Math.fma(q.x, w, Math.fma(q.y, z, -q.z * y)));
2403         double qpy = Math.fma(q.w, y, Math.fma(-q.x, z, Math.fma(q.y, w, q.z * x)));
2404         double qpz = Math.fma(q.w, z, Math.fma(q.x, y, Math.fma(-q.y, x, q.z * w)));
2405         double qpw = Math.fma(q.w, w, Math.fma(-q.x, x, Math.fma(-q.y, y, -q.z * z)));
2406         return dest.set(Math.fma(qpw, qix, Math.fma(qpx, qiw, Math.fma(qpy, qiz, -qpz * qiy))),
2407                         Math.fma(qpw, qiy, Math.fma(-qpx, qiz, Math.fma(qpy, qiw, qpz * qix))),
2408                         Math.fma(qpw, qiz, Math.fma(qpx, qiy, Math.fma(-qpy, qix, qpz * qiw))),
2409                         Math.fma(qpw, qiw, Math.fma(-qpx, qix, Math.fma(-qpy, qiy, -qpz * qiz))));
2410     }
2411 
2412     public bool isFinite() {
2413         return Math.isFinite(x) && Math.isFinite(y) && Math.isFinite(z) && Math.isFinite(w);
2414     }
2415 
2416     public bool equals(Quaterniond q, double delta) {
2417         if (this == q)
2418             return true;
2419         if (!Math.equals(x, q.x, delta))
2420             return false;
2421         if (!Math.equals(y, q.y, delta))
2422             return false;
2423         if (!Math.equals(z, q.z, delta))
2424             return false;
2425         if (!Math.equals(w, q.w, delta))
2426             return false;
2427         return true;
2428     }
2429 
2430     public bool equals(double x, double y, double z, double w) {
2431         if (Math.doubleToLongBits(this.x) != Math.doubleToLongBits(x))
2432             return false;
2433         if (Math.doubleToLongBits(this.y) != Math.doubleToLongBits(y))
2434             return false;
2435         if (Math.doubleToLongBits(this.z) != Math.doubleToLongBits(z))
2436             return false;
2437         if (Math.doubleToLongBits(this.w) != Math.doubleToLongBits(w))
2438             return false;
2439         return true;
2440     }
2441 
2442 }