JOGL v2.6.0-rc-20250712
JOGL, High-Performance Graphics Binding for Java™ (public API).
Quaternion.java
Go to the documentation of this file.
1/**
2 * Copyright 2010-2023 JogAmp Community. All rights reserved.
3 *
4 * Redistribution and use in source and binary forms, with or without modification, are
5 * permitted provided that the following conditions are met:
6 *
7 * 1. Redistributions of source code must retain the above copyright notice, this list of
8 * conditions and the following disclaimer.
9 *
10 * 2. Redistributions in binary form must reproduce the above copyright notice, this list
11 * of conditions and the following disclaimer in the documentation and/or other materials
12 * provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
15 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
17 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 *
24 * The views and conclusions contained in the software and documentation are those of the
25 * authors and should not be interpreted as representing official policies, either expressed
26 * or implied, of JogAmp Community.
27 */
28package com.jogamp.math;
29
30/**
31 * Quaternion implementation supporting
32 * <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q34">Gimbal-Lock</a> free rotations.
33 * <p>
34 * All matrix operation provided are in column-major order,
35 * as specified in the OpenGL fixed function pipeline, i.e. compatibility profile.
36 * See {@link FloatUtil}.
37 * </p>
38 * <p>
39 * See <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html">Matrix-FAQ</a>
40 * </p>
41 * <p>
42 * See <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm">euclideanspace.com-Quaternion</a>
43 * </p>
44 */
45public final class Quaternion {
46 private float x, y, z, w;
47
48 /**
49 * Quaternion Epsilon, used with equals method to determine if two Quaternions are close enough to be considered equal.
50 * <p>
51 * Using {@value}, which is ~10 times {@link FloatUtil#EPSILON}.
52 * </p>
53 */
54 public static final float ALLOWED_DEVIANCE = 1.0E-6f; // FloatUtil.EPSILON == 1.1920929E-7f; double ALLOWED_DEVIANCE: 1.0E-8f
55
56 public Quaternion() {
57 x = y = z = 0; w = 1;
58 }
59
60 public Quaternion(final Quaternion q) {
61 set(q);
62 }
63
64 public Quaternion(final float x, final float y, final float z, final float w) {
65 set(x, y, z, w);
66 }
67
68 public Quaternion copy() { return new Quaternion(this); }
69
70 /**
71 * See {@link #magnitude()} for special handling of {@link FloatUtil#EPSILON epsilon},
72 * which is not applied here.
73 * @return the squared magnitude of this quaternion.
74 */
75 public final float magnitudeSquared() {
76 return w*w + x*x + y*y + z*z;
77 }
78
79 /**
80 * Return the magnitude of this quaternion, i.e. sqrt({@link #magnitudeSquared()})
81 * <p>
82 * A magnitude of zero shall equal {@link #isIdentity() identity},
83 * as performed by {@link #normalize()}.
84 * </p>
85 * <p>
86 * Implementation Details:
87 * <ul>
88 * <li> returns 0f if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
89 * <li> returns 1f if {@link #magnitudeSquared()} is {@link FloatUtil#isEqual(float, float, float) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li>
90 * </ul>
91 * </p>
92 */
93 public final float magnitude() {
94 final float magnitudeSQ = magnitudeSquared();
95 if ( FloatUtil.isZero(magnitudeSQ) ) {
96 return 0f;
97 }
98 if ( FloatUtil.isEqual(1f, magnitudeSQ) ) {
99 return 1f;
100 }
101 return FloatUtil.sqrt(magnitudeSQ);
102 }
103
104 public final float w() {
105 return w;
106 }
107
108 public final void setW(final float w) {
109 this.w = w;
110 }
111
112 public final float x() {
113 return x;
114 }
115
116 public final void setX(final float x) {
117 this.x = x;
118 }
119
120 public final float y() {
121 return y;
122 }
123
124 public final void setY(final float y) {
125 this.y = y;
126 }
127
128 public final float z() {
129 return z;
130 }
131
132 public final void setZ(final float z) {
133 this.z = z;
134 }
135
136 /**
137 * Returns the dot product of this quaternion with the given x,y,z and w components.
138 */
139 public final float dot(final float x, final float y, final float z, final float w) {
140 return this.x * x + this.y * y + this.z * z + this.w * w;
141 }
142
143 /**
144 * Returns the dot product of this quaternion with the given quaternion
145 */
146 public final float dot(final Quaternion quat) {
147 return dot(quat.x(), quat.y(), quat.z(), quat.w());
148 }
149
150 /**
151 * Returns <code>true</code> if this quaternion has identity.
152 * <p>
153 * Implementation uses {@link FloatUtil#EPSILON epsilon} to compare
154 * {@link #w() W} {@link FloatUtil#isEqual(float, float) against 1f} and
155 * {@link #x() X}, {@link #y() Y} and {@link #z() Z}
156 * {@link FloatUtil#isZero(float) against zero}.
157 * </p>
158 */
159 public final boolean isIdentity() {
160 return FloatUtil.isEqual(1f, w) && VectorUtil.isZero(x, y, z);
161 // return w == 1f && x == 0f && y == 0f && z == 0f;
162 }
163
164 /***
165 * Set this quaternion to identity (x=0,y=0,z=0,w=1)
166 * @return this quaternion for chaining.
167 */
168 public final Quaternion setIdentity() {
169 x = y = z = 0f; w = 1f;
170 return this;
171 }
172
173 /**
174 * Normalize a quaternion required if to be used as a rotational quaternion.
175 * <p>
176 * Implementation Details:
177 * <ul>
178 * <li> {@link #setIdentity()} if {@link #magnitude()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
179 * </ul>
180 * </p>
181 * @return this quaternion for chaining.
182 */
183 public final Quaternion normalize() {
184 final float norm = magnitude();
185 if ( FloatUtil.isZero(norm) ) {
186 setIdentity();
187 } else {
188 final float invNorm = 1f/norm;
189 w *= invNorm;
190 x *= invNorm;
191 y *= invNorm;
192 z *= invNorm;
193 }
194 return this;
195 }
196
197 /**
198 * Conjugates this quaternion <code>[-x, -y, -z, w]</code>.
199 * @return this quaternion for chaining.
200 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q49">Matrix-FAQ Q49</a>
201 */
203 x = -x;
204 y = -y;
205 z = -z;
206 return this;
207 }
208
209 /**
210 * Invert the quaternion If rotational, will produce a the inverse rotation
211 * <p>
212 * Implementation Details:
213 * <ul>
214 * <li> {@link #conjugate() conjugates} if {@link #magnitudeSquared()} is is {@link FloatUtil#isEqual(float, float, float) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li>
215 * </ul>
216 * </p>
217 * @return this quaternion for chaining.
218 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q50">Matrix-FAQ Q50</a>
219 */
220 public final Quaternion invert() {
221 final float magnitudeSQ = magnitudeSquared();
222 if ( FloatUtil.isEqual(1.0f, magnitudeSQ) ) {
223 conjugate();
224 } else {
225 final float invmsq = 1f/magnitudeSQ;
226 w *= invmsq;
227 x = -x * invmsq;
228 y = -y * invmsq;
229 z = -z * invmsq;
230 }
231 return this;
232 }
233
234 /**
235 * Set all values of this quaternion using the given src.
236 * @return this quaternion for chaining.
237 */
238 public final Quaternion set(final Quaternion src) {
239 this.x = src.x;
240 this.y = src.y;
241 this.z = src.z;
242 this.w = src.w;
243 return this;
244 }
245
246 /**
247 * Set all values of this quaternion using the given components.
248 * @return this quaternion for chaining.
249 */
250 public final Quaternion set(final float x, final float y, final float z, final float w) {
251 this.x = x;
252 this.y = y;
253 this.z = z;
254 this.w = w;
255 return this;
256 }
257
258 /**
259 * Add a quaternion
260 *
261 * @param q quaternion
262 * @return this quaternion for chaining.
263 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
264 */
265 public final Quaternion add(final Quaternion q) {
266 x += q.x;
267 y += q.y;
268 z += q.z;
269 w += q.w;
270 return this;
271 }
272
273 /**
274 * Subtract a quaternion
275 *
276 * @param q quaternion
277 * @return this quaternion for chaining.
278 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
279 */
280 public final Quaternion subtract(final Quaternion q) {
281 x -= q.x;
282 y -= q.y;
283 z -= q.z;
284 w -= q.w;
285 return this;
286 }
287
288 /**
289 * Multiply this quaternion by the param quaternion
290 *
291 * @param q a quaternion to multiply with
292 * @return this quaternion for chaining.
293 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q53">Matrix-FAQ Q53</a>
294 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#mul">euclideanspace.com-QuaternionMul</a>
295 */
296 public final Quaternion mult(final Quaternion q) {
297 return set( w * q.x + x * q.w + y * q.z - z * q.y,
298 w * q.y - x * q.z + y * q.w + z * q.x,
299 w * q.z + x * q.y - y * q.x + z * q.w,
300 w * q.w - x * q.x - y * q.y - z * q.z );
301 }
302
303 /**
304 * Scale this quaternion by a constant
305 *
306 * @param n a float constant
307 * @return this quaternion for chaining.
308 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#scale">euclideanspace.com-QuaternionScale</a>
309 */
310 public final Quaternion scale(final float n) {
311 x *= n;
312 y *= n;
313 z *= n;
314 w *= n;
315 return this;
316 }
317
318 /**
319 * Rotate this quaternion by the given angle and axis.
320 * <p>
321 * The axis must be a normalized vector.
322 * </p>
323 * <p>
324 * A rotational quaternion is made from the given angle and axis.
325 * </p>
326 *
327 * @param angle in radians
328 * @param axisX x-coord of rotation axis
329 * @param axisY y-coord of rotation axis
330 * @param axisZ z-coord of rotation axis
331 * @return this quaternion for chaining.
332 */
333 public Quaternion rotateByAngleNormalAxis(final float angle, final float axisX, final float axisY, final float axisZ) {
334 if( VectorUtil.isZero(axisX, axisY, axisZ) ) {
335 // no change
336 return this;
337 }
338 final float halfAngle = 0.5f * angle;
339 final float sin = FloatUtil.sin(halfAngle);
340 final float qw = FloatUtil.cos(halfAngle);
341 final float qx = sin * axisX;
342 final float qy = sin * axisY;
343 final float qz = sin * axisZ;
344 return set( x * qw + y * qz - z * qy + w * qx,
345 -x * qz + y * qw + z * qx + w * qy,
346 x * qy - y * qx + z * qw + w * qz,
347 -x * qx - y * qy - z * qz + w * qw);
348 }
349
350 /**
351 * Rotate this quaternion by the given angle and axis.
352 * <p>
353 * The axis must be a normalized vector.
354 * </p>
355 * <p>
356 * A rotational quaternion is made from the given angle and axis.
357 * </p>
358 *
359 * @param angle in radians
360 * @param axis Vec3f coord of rotation axis
361 * @return this quaternion for chaining.
362 */
363 public Quaternion rotateByAngleNormalAxis(final float angle, final Vec3f axis) {
364 return rotateByAngleNormalAxis(angle, axis.x(), axis.y(), axis.z());
365 }
366
367 /**
368 * Rotate this quaternion around X axis with the given angle in radians
369 *
370 * @param angle in radians
371 * @return this quaternion for chaining.
372 */
373 public Quaternion rotateByAngleX(final float angle) {
374 final float halfAngle = 0.5f * angle;
375 final float sin = FloatUtil.sin(halfAngle);
376 final float cos = FloatUtil.cos(halfAngle);
377 return set( x * cos + w * sin,
378 y * cos + z * sin,
379 -y * sin + z * cos,
380 -x * sin + w * cos);
381 }
382
383 /**
384 * Rotate this quaternion around Y axis with the given angle in radians
385 *
386 * @param angle in radians
387 * @return this quaternion for chaining.
388 */
389 public Quaternion rotateByAngleY(final float angle) {
390 final float halfAngle = 0.5f * angle;
391 final float sin = FloatUtil.sin(halfAngle);
392 final float cos = FloatUtil.cos(halfAngle);
393 return set( x * cos - z * sin,
394 y * cos + w * sin,
395 x * sin + z * cos,
396 -y * sin + w * cos);
397 }
398
399 /**
400 * Rotate this quaternion around Z axis with the given angle in radians
401 *
402 * @param angle in radians
403 * @return this quaternion for chaining.
404 */
405 public Quaternion rotateByAngleZ(final float angle) {
406 final float halfAngle = 0.5f * angle;
407 final float sin = FloatUtil.sin(halfAngle);
408 final float cos = FloatUtil.cos(halfAngle);
409 return set( x * cos + y * sin,
410 -x * sin + y * cos,
411 z * cos + w * sin,
412 -z * sin + w * cos);
413 }
414
415 /**
416 * Rotates this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians.
417 * <p>
418 * The <code>angradXYZ</code> array is laid out in natural order:
419 * <ul>
420 * <li>x - bank</li>
421 * <li>y - heading</li>
422 * <li>z - attitude</li>
423 * </ul>
424 * </p>
425 * For details see {@link #rotateByEuler(float, float, float)}.
426 * @param angradXYZ euler angle array in radians
427 * @return this quaternion for chaining.
428 * @see #rotateByEuler(float, float, float)
429 */
430 public final Quaternion rotateByEuler(final Vec3f angradXYZ) {
431 return rotateByEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z());
432 }
433
434 /**
435 * Rotates this quaternion from the given Euler rotation angles in radians.
436 * <p>
437 * The rotations are applied in the given order and using chained rotation per axis:
438 * <ul>
439 * <li>y - heading - {@link #rotateByAngleY(float)}</li>
440 * <li>z - attitude - {@link #rotateByAngleZ(float)}</li>
441 * <li>x - bank - {@link #rotateByAngleX(float)}</li>
442 * </ul>
443 * </p>
444 * <p>
445 * Implementation Details:
446 * <ul>
447 * <li> NOP if all angles are {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
448 * <li> result is {@link #normalize()}ed</li>
449 * </ul>
450 * </p>
451 * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
452 * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
453 * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
454 * @return this quaternion for chaining.
455 * @see #rotateByAngleY(float)
456 * @see #rotateByAngleZ(float)
457 * @see #rotateByAngleX(float)
458 * @see #setFromEuler(float, float, float)
459 */
460 public final Quaternion rotateByEuler(final float bankX, final float headingY, final float attitudeZ) {
461 if ( VectorUtil.isZero(bankX, headingY, attitudeZ) ) {
462 return this;
463 } else {
464 // setFromEuler muls: ( 8 + 4 ) , + quat muls 24 = 36
465 // this: 8 + 8 + 8 + 4 = 28 muls
466 return rotateByAngleY(headingY).rotateByAngleZ(attitudeZ).rotateByAngleX(bankX).normalize();
467 }
468 }
469
470 /***
471 * Rotate the given vector by this quaternion
472 * @param vecIn vector to be rotated
473 * @param vecOut result storage for rotated vector, maybe equal to vecIn for in-place rotation
474 *
475 * @return the given vecOut store for chaining
476 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q63">Matrix-FAQ Q63</a>
477 */
478 public final Vec3f rotateVector(final Vec3f vecIn, final Vec3f vecOut) {
479 if( vecIn.isZero() ) {
480 vecOut.set(0, 0, 0);
481 } else {
482 final float vecX = vecIn.x();
483 final float vecY = vecIn.y();
484 final float vecZ = vecIn.z();
485 final float x_x = x*x;
486 final float y_y = y*y;
487 final float z_z = z*z;
488 final float w_w = w*w;
489
490 vecOut.setX( w_w * vecX
491 + x_x * vecX
492 - z_z * vecX
493 - y_y * vecX
494 + 2f * ( y*w*vecZ - z*w*vecY + y*x*vecY + z*x*vecZ ) );
495 ;
496
497 vecOut.setY( y_y * vecY
498 - z_z * vecY
499 + w_w * vecY
500 - x_x * vecY
501 + 2f * ( x*y*vecX + z*y*vecZ + w*z*vecX - x*w*vecZ ) );;
502
503 vecOut.setZ( z_z * vecZ
504 - y_y * vecZ
505 - x_x * vecZ
506 + w_w * vecZ
507 + 2f * ( x*z*vecX + y*z*vecY - w*y*vecX + w*x*vecY ) );
508 }
509 return vecOut;
510 }
511
512 /**
513 * Set this quaternion to a spherical linear interpolation
514 * between the given start and end quaternions by the given change amount.
515 * <p>
516 * Note: Method <i>does not</i> normalize this quaternion!
517 * </p>
518 *
519 * @param a start quaternion
520 * @param b end quaternion
521 * @param changeAmnt float between 0 and 1 representing interpolation.
522 * @return this quaternion for chaining.
523 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/">euclideanspace.com-QuaternionSlerp</a>
524 */
525 public final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt) {
526 // System.err.println("Slerp.0: A "+a+", B "+b+", t "+changeAmnt);
527 if (changeAmnt == 0.0f) {
528 set(a);
529 } else if (changeAmnt == 1.0f) {
530 set(b);
531 } else {
532 float bx = b.x;
533 float by = b.y;
534 float bz = b.z;
535 float bw = b.w;
536
537 // Calculate angle between them (quat dot product)
538 float cosHalfTheta = a.x * bx + a.y * by + a.z * bz + a.w * bw;
539
540 final float scale0, scale1;
541
542 if( cosHalfTheta >= 0.95f ) {
543 // quaternions are close, just use linear interpolation
544 scale0 = 1.0f - changeAmnt;
545 scale1 = changeAmnt;
546 // System.err.println("Slerp.1: Linear Interpol; cosHalfTheta "+cosHalfTheta);
547 } else if ( cosHalfTheta <= -0.99f ) {
548 // the quaternions are nearly opposite,
549 // we can pick any axis normal to a,b to do the rotation
550 scale0 = 0.5f;
551 scale1 = 0.5f;
552 // System.err.println("Slerp.2: Any; cosHalfTheta "+cosHalfTheta);
553 } else {
554 // System.err.println("Slerp.3: cosHalfTheta "+cosHalfTheta);
555 if( cosHalfTheta <= -FloatUtil.EPSILON ) { // FIXME: .. or shall we use the upper bound 'cosHalfTheta < FloatUtil.EPSILON' ?
556 // Negate the second quaternion and the result of the dot product (Inversion)
557 bx *= -1f;
558 by *= -1f;
559 bz *= -1f;
560 bw *= -1f;
561 cosHalfTheta *= -1f;
562 // System.err.println("Slerp.4: Inverted cosHalfTheta "+cosHalfTheta);
563 }
564 final float halfTheta = FloatUtil.acos(cosHalfTheta);
565 final float sinHalfTheta = FloatUtil.sqrt(1.0f - cosHalfTheta*cosHalfTheta);
566 // if theta = 180 degrees then result is not fully defined
567 // we could rotate around any axis normal to qa or qb
568 if ( Math.abs(sinHalfTheta) < 0.001f ){ // fabs is floating point absolute
569 scale0 = 0.5f;
570 scale1 = 0.5f;
571 // throw new InternalError("XXX"); // FIXME should not be reached due to above inversion ?
572 } else {
573 // Calculate the scale for q1 and q2, according to the angle and
574 // it's sine value
575 scale0 = FloatUtil.sin((1f - changeAmnt) * halfTheta) / sinHalfTheta;
576 scale1 = FloatUtil.sin(changeAmnt * halfTheta) / sinHalfTheta;
577 }
578 }
579
580 x = a.x * scale0 + bx * scale1;
581 y = a.y * scale0 + by * scale1;
582 z = a.z * scale0 + bz * scale1;
583 w = a.w * scale0 + bw * scale1;
584 }
585 // System.err.println("Slerp.X: Result "+this);
586 return this;
587 }
588
589 /**
590 * Set this quaternion to equal the rotation required
591 * to point the z-axis at <i>direction</i> and the y-axis to <i>up</i>.
592 * <p>
593 * Implementation generates a 3x3 matrix
594 * and is equal with ProjectFloat's lookAt(..).<br/>
595 * </p>
596 * Implementation Details:
597 * <ul>
598 * <li> result is {@link #normalize()}ed</li>
599 * </ul>
600 * </p>
601 * @param directionIn where to <i>look</i> at
602 * @param upIn a vector indicating the local <i>up</i> direction.
603 * @param xAxisOut vector storing the <i>orthogonal</i> x-axis of the coordinate system.
604 * @param yAxisOut vector storing the <i>orthogonal</i> y-axis of the coordinate system.
605 * @param zAxisOut vector storing the <i>orthogonal</i> z-axis of the coordinate system.
606 * @return this quaternion for chaining.
607 * @see <a href="http://www.euclideanspace.com/maths/algebra/vectors/lookat/index.htm">euclideanspace.com-LookUp</a>
608 */
609 public Quaternion setLookAt(final Vec3f directionIn, final Vec3f upIn,
610 final Vec3f xAxisOut, final Vec3f yAxisOut, final Vec3f zAxisOut) {
611 // Z = norm(dir)
612 zAxisOut.set(directionIn).normalize();
613
614 // X = upIn x Z
615 // (borrow yAxisOut for upNorm)
616 yAxisOut.set(upIn).normalize();
617 xAxisOut.cross(yAxisOut, zAxisOut).normalize();
618
619 // Y = Z x X
620 //
621 yAxisOut.cross(zAxisOut, xAxisOut).normalize();
622
623 /**
624 final float m00 = xAxisOut[0];
625 final float m01 = yAxisOut[0];
626 final float m02 = zAxisOut[0];
627 final float m10 = xAxisOut[1];
628 final float m11 = yAxisOut[1];
629 final float m12 = zAxisOut[1];
630 final float m20 = xAxisOut[2];
631 final float m21 = yAxisOut[2];
632 final float m22 = zAxisOut[2];
633 */
634 return setFromAxes(xAxisOut, yAxisOut, zAxisOut).normalize();
635 }
636
637 //
638 // Conversions
639 //
640
641 /**
642 * Initialize this quaternion from two vectors
643 * <pre>
644 * q = (s,v) = (v1•v2 , v1 × v2),
645 * angle = angle(v1, v2) = v1•v2
646 * axis = normal(v1 x v2)
647 * </pre>
648 * <p>
649 * Implementation Details:
650 * <ul>
651 * <li> {@link #setIdentity()} if square vector-length is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
652 * </ul>
653 * </p>
654 * @param v1 not normalized
655 * @param v2 not normalized
656 * @param tmpPivotVec temp storage for cross product
657 * @param tmpNormalVec temp storage to normalize vector
658 * @return this quaternion for chaining.
659 */
660 public final Quaternion setFromVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec, final Vec3f tmpNormalVec) {
661 final float factor = v1.length() * v2.length();
662 if ( FloatUtil.isZero(factor) ) {
663 return setIdentity();
664 } else {
665 final float dot = v1.dot(v2) / factor; // normalize
666 final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1]
667
668 tmpPivotVec.cross(v1, v2);
669
670 if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length() ) ) {
671 // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
672 // perpendicular to this vector will rotate vector a onto vector b.
673 //
674 // The following guarantees the dot-product will be 0.0.
675 int dominantIndex;
676 if (Math.abs(v1.x()) > Math.abs(v1.y())) {
677 if (Math.abs(v1.x()) > Math.abs(v1.z())) {
678 dominantIndex = 0;
679 } else {
680 dominantIndex = 2;
681 }
682 } else {
683 if (Math.abs(v1.y()) > Math.abs(v1.z())) {
684 dominantIndex = 1;
685 } else {
686 dominantIndex = 2;
687 }
688 }
689 tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) );
690 tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) );
691 tmpPivotVec.set( (dominantIndex + 2) % 3, 0f );
692 }
693 return setFromAngleAxis(theta, tmpPivotVec, tmpNormalVec);
694 }
695 }
696
697 /**
698 * Initialize this quaternion from two normalized vectors
699 * <pre>
700 * q = (s,v) = (v1•v2 , v1 × v2),
701 * angle = angle(v1, v2) = v1•v2
702 * axis = v1 x v2
703 * </pre>
704 * <p>
705 * Implementation Details:
706 * <ul>
707 * <li> {@link #setIdentity()} if square vector-length is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
708 * </ul>
709 * </p>
710 * @param v1 normalized
711 * @param v2 normalized
712 * @param tmpPivotVec temp storage for cross product
713 * @return this quaternion for chaining.
714 */
715 public final Quaternion setFromNormalVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec) {
716 final float factor = v1.length() * v2.length();
717 if ( FloatUtil.isZero(factor) ) {
718 return setIdentity();
719 } else {
720 final float dot = v1.dot(v2) / factor; // normalize
721 final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1]
722
723 tmpPivotVec.cross(v1, v2);
724
725 if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length() ) ) {
726 // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
727 // perpendicular to this vector will rotate vector a onto vector b.
728 //
729 // The following guarantees the dot-product will be 0.0.
730 int dominantIndex;
731 if (Math.abs(v1.x()) > Math.abs(v1.y())) {
732 if (Math.abs(v1.x()) > Math.abs(v1.z())) {
733 dominantIndex = 0;
734 } else {
735 dominantIndex = 2;
736 }
737 } else {
738 if (Math.abs(v1.y()) > Math.abs(v1.z())) {
739 dominantIndex = 1;
740 } else {
741 dominantIndex = 2;
742 }
743 }
744 tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) );
745 tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) );
746 tmpPivotVec.set( (dominantIndex + 2) % 3, 0f );
747 }
748 return setFromAngleNormalAxis(theta, tmpPivotVec);
749 }
750 }
751
752 /***
753 * Initialize this quaternion with given non-normalized axis vector and rotation angle
754 * <p>
755 * Implementation Details:
756 * <ul>
757 * <li> {@link #setIdentity()} if axis is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
758 * </ul>
759 * </p>
760 * @param angle rotation angle (rads)
761 * @param vector axis vector not normalized
762 * @param tmpV3f temp storage to normalize vector
763 * @return this quaternion for chaining.
764 *
765 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
766 * @see #toAngleAxis(Vec3f)
767 */
768 public final Quaternion setFromAngleAxis(final float angle, final Vec3f vector, final Vec3f tmpV3f) {
769 tmpV3f.set(vector).normalize();
770 return setFromAngleNormalAxis(angle, tmpV3f);
771 }
772
773 /***
774 * Initialize this quaternion with given normalized axis vector and rotation angle
775 * <p>
776 * Implementation Details:
777 * <ul>
778 * <li> {@link #setIdentity()} if axis is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
779 * </ul>
780 * </p>
781 * @param angle rotation angle (rads)
782 * @param vector axis vector normalized
783 * @return this quaternion for chaining.
784 *
785 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
786 * @see #toAngleAxis(Vec3f)
787 */
788 public final Quaternion setFromAngleNormalAxis(final float angle, final Vec3f vector) {
789 if( vector.isZero() ) {
790 setIdentity();
791 } else {
792 final float halfangle = angle * 0.5f;
793 final float sin = FloatUtil.sin(halfangle);
794 x = vector.x() * sin;
795 y = vector.y() * sin;
796 z = vector.z() * sin;
797 w = FloatUtil.cos(halfangle);
798 }
799 return this;
800 }
801
802 /**
803 * Transform the rotational quaternion to axis based rotation angles
804 *
805 * @param axis storage for computed axis
806 * @return the rotation angle in radians
807 * @see #setFromAngleAxis(float, Vec3f, Vec3f)
808 */
809 public final float toAngleAxis(final Vec3f axis) {
810 final float sqrLength = x*x + y*y + z*z;
811 float angle;
812 if ( FloatUtil.isZero(sqrLength) ) { // length is ~0
813 angle = 0.0f;
814 axis.set( 1.0f, 0.0f, 0.0f );
815 } else {
816 angle = FloatUtil.acos(w) * 2.0f;
817 final float invLength = 1.0f / FloatUtil.sqrt(sqrLength);
818 axis.set( x * invLength,
819 y * invLength,
820 z * invLength );
821 }
822 return angle;
823 }
824
825 /**
826 * Initializes this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians.
827 * <p>
828 * The <code>angradXYZ</code> vector is laid out in natural order:
829 * <ul>
830 * <li>x - bank</li>
831 * <li>y - heading</li>
832 * <li>z - attitude</li>
833 * </ul>
834 * </p>
835 * For details see {@link #setFromEuler(float, float, float)}.
836 * @param angradXYZ euler angle vector in radians holding x-bank, y-heading and z-attitude
837 * @return this quaternion for chaining.
838 * @see #setFromEuler(float, float, float)
839 */
840 public final Quaternion setFromEuler(final Vec3f angradXYZ) {
841 return setFromEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z());
842 }
843
844 /**
845 * Initializes this quaternion from the given Euler rotation angles in radians.
846 * <p>
847 * The rotations are applied in the given order:
848 * <ul>
849 * <li>y - heading</li>
850 * <li>z - attitude</li>
851 * <li>x - bank</li>
852 * </ul>
853 * </p>
854 * <p>
855 * Implementation Details:
856 * <ul>
857 * <li> {@link #setIdentity()} if all angles are {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
858 * <li> result is {@link #normalize()}ed</li>
859 * </ul>
860 * </p>
861 * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
862 * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
863 * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
864 * @return this quaternion for chaining.
865 *
866 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60">Matrix-FAQ Q60</a>
867 * @see <a href="http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html">Gems</a>
868 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">euclideanspace.com-eulerToQuaternion</a>
869 * @see #toEuler(Vec3f)
870 */
871 public final Quaternion setFromEuler(final float bankX, final float headingY, final float attitudeZ) {
872 if ( VectorUtil.isZero(bankX, headingY, attitudeZ) ) {
873 return setIdentity();
874 } else {
875 float angle = headingY * 0.5f;
876 final float sinHeadingY = FloatUtil.sin(angle);
877 final float cosHeadingY = FloatUtil.cos(angle);
878 angle = attitudeZ * 0.5f;
879 final float sinAttitudeZ = FloatUtil.sin(angle);
880 final float cosAttitudeZ = FloatUtil.cos(angle);
881 angle = bankX * 0.5f;
882 final float sinBankX = FloatUtil.sin(angle);
883 final float cosBankX = FloatUtil.cos(angle);
884
885 // variables used to reduce multiplication calls.
886 final float cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
887 final float sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
888 final float cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
889 final float sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
890
891 w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
892 x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
893 y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
894 z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
895 return normalize();
896 }
897 }
898
899 /**
900 * Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
901 * <p>
902 * The <code>result</code> array is laid out in natural order:
903 * <ul>
904 * <li>x - bank</li>
905 * <li>y - heading</li>
906 * <li>z - attitude</li>
907 * </ul>
908 * </p>
909 *
910 * @param result euler angle result vector for radians x-bank, y-heading and z-attitude
911 * @return the Vec3f `result` filled with x-bank, y-heading and z-attitude
912 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">euclideanspace.com-quaternionToEuler</a>
913 * @see #setFromEuler(float, float, float)
914 */
915 public Vec3f toEuler(final Vec3f result) {
916 final float sqw = w*w;
917 final float sqx = x*x;
918 final float sqy = y*y;
919 final float sqz = z*z;
920 final float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise, is correction factor
921 final float test = x*y + z*w;
922
923 if (test > 0.499f * unit) { // singularity at north pole
924 result.set( 0f, // x-bank
925 2f * FloatUtil.atan2(x, w), // y-heading
926 FloatUtil.HALF_PI ); // z-attitude
927 } else if (test < -0.499f * unit) { // singularity at south pole
928 result.set( 0f, // x-bank
929 -2 * FloatUtil.atan2(x, w), // y-heading
930 -FloatUtil.HALF_PI ); // z-attitude
931 } else {
932 result.set( FloatUtil.atan2(2f * x * w - 2 * y * z, -sqx + sqy - sqz + sqw), // x-bank
933 FloatUtil.atan2(2f * y * w - 2 * x * z, sqx - sqy - sqz + sqw), // y-heading
934 FloatUtil.asin( 2f * test / unit) ); // z-attitude
935 }
936 return result;
937 }
938
939 /**
940 * Compute the quaternion from a 3x3 column rotation matrix
941 * <p>
942 * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
943 * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
944 * </p>
945 * <p>
946 * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
947 * </p>
948 *
949 * @return this quaternion for chaining.
950 * @see #setFromMat(Matrix4f)
951 */
952 public Quaternion setFromMat(final float m00, final float m01, final float m02,
953 final float m10, final float m11, final float m12,
954 final float m20, final float m21, final float m22) {
955 // Note: Other implementations uses 'T' w/o '+1f' and compares 'T >= 0' while adding missing 1f in sqrt expr.
956 // However .. this causes setLookAt(..) to fail and actually violates the 'trace definition'.
957
958 // The trace T is the sum of the diagonal elements; see
959 // http://mathworld.wolfram.com/MatrixTrace.html
960 final float T = m00 + m11 + m22 + 1f;
961 // System.err.println("setFromMatrix.0 T "+T+", m00 "+m00+", m11 "+m11+", m22 "+m22);
962 if ( T > 0f ) {
963 // System.err.println("setFromMatrix.1");
964 final float S = 0.5f / FloatUtil.sqrt(T); // S = 1 / ( 2 t )
965 w = 0.25f / S; // w = 1 / ( 4 S ) = t / 2
966 x = ( m21 - m12 ) * S;
967 y = ( m02 - m20 ) * S;
968 z = ( m10 - m01 ) * S;
969 } else if ( m00 > m11 && m00 > m22) {
970 // System.err.println("setFromMatrix.2");
971 final float S = 0.5f / FloatUtil.sqrt(1.0f + m00 - m11 - m22); // S=4*qx
972 w = ( m21 - m12 ) * S;
973 x = 0.25f / S;
974 y = ( m10 + m01 ) * S;
975 z = ( m02 + m20 ) * S;
976 } else if ( m11 > m22 ) {
977 // System.err.println("setFromMatrix.3");
978 final float S = 0.5f / FloatUtil.sqrt(1.0f + m11 - m00 - m22); // S=4*qy
979 w = ( m02 - m20 ) * S;
980 x = ( m20 + m01 ) * S;
981 y = 0.25f / S;
982 z = ( m21 + m12 ) * S;
983 } else {
984 // System.err.println("setFromMatrix.3");
985 final float S = 0.5f / FloatUtil.sqrt(1.0f + m22 - m00 - m11); // S=4*qz
986 w = ( m10 - m01 ) * S;
987 x = ( m02 + m20 ) * S;
988 y = ( m21 + m12 ) * S;
989 z = 0.25f / S;
990 }
991 return this;
992 }
993
994 /**
995 * Compute the quaternion from a 3x3 column rotation matrix from {@link Matrix4f} instance
996 * <p>
997 * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
998 * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
999 * </p>
1000 * <p>
1001 * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
1002 * </p>
1003 *
1004 * @return this quaternion for chaining.
1005 * @see Matrix4f#getRotation(Quaternion)
1006 * @see #setFromMat(float, float, float, float, float, float, float, float, float)
1007 */
1009 return m.getRotation(this);
1010 }
1011
1012 /**
1013 * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
1014 * <p>
1015 * Implementation Details:
1016 * <ul>
1017 * <li> makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
1018 * </ul>
1019 * </p>
1020 *
1021 * @param matrix float[16] store for the resulting normalized column matrix 4x4
1022 * @return the given matrix store
1023 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54">Matrix-FAQ Q54</a>
1024 * @see #setFromMat(Matrix4f)
1025 * @see #setFromMat(float, float, float, float, float, float, float, float, float)
1026 */
1027 public final float[] toMatrix(final float[] matrix) {
1028 // pre-multiply scaled-reciprocal-magnitude to reduce multiplications
1029 final float norm = magnitudeSquared();
1030 if ( FloatUtil.isZero(norm) ) {
1031 // identity matrix -> srecip = 0f
1032 return FloatUtil.makeIdentity(matrix);
1033 }
1034 final float srecip;
1035 if ( FloatUtil.isEqual(1f, norm) ) {
1036 srecip = 2f;
1037 } else {
1038 srecip = 2.0f / norm;
1039 }
1040
1041 final float xs = srecip * x;
1042 final float ys = srecip * y;
1043 final float zs = srecip * z;
1044
1045 final float xx = x * xs;
1046 final float xy = x * ys;
1047 final float xz = x * zs;
1048 final float xw = xs * w;
1049 final float yy = y * ys;
1050 final float yz = y * zs;
1051 final float yw = ys * w;
1052 final float zz = z * zs;
1053 final float zw = zs * w;
1054
1055 matrix[0+0*4] = 1f - ( yy + zz );
1056 matrix[0+1*4] = ( xy - zw );
1057 matrix[0+2*4] = ( xz + yw );
1058 matrix[0+3*4] = 0f;
1059
1060 matrix[1+0*4] = ( xy + zw );
1061 matrix[1+1*4] = 1f - ( xx + zz );
1062 matrix[1+2*4] = ( yz - xw );
1063 matrix[1+3*4] = 0f;
1064
1065 matrix[2+0*4] = ( xz - yw );
1066 matrix[2+1*4] = ( yz + xw );
1067 matrix[2+2*4] = 1f - ( xx + yy );
1068 matrix[2+3*4] = 0f;
1069
1070 matrix[3+0*4] = 0f;
1071 matrix[3+1*4] = 0f;
1072 matrix[3+2*4] = 0f;
1073 matrix[3+3*4] = 1f;
1074 return matrix;
1075 }
1076
1077 /**
1078 * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
1079 * <p>
1080 * Implementation Details:
1081 * <ul>
1082 * <li> makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
1083 * </ul>
1084 * </p>
1085 *
1086 * @param matrix store for the resulting normalized column matrix 4x4
1087 * @return the given matrix store
1088 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54">Matrix-FAQ Q54</a>
1089 * @see #setFromMat(float, float, float, float, float, float, float, float, float)
1090 * @see Matrix4f#setToRotation(Quaternion)
1091 */
1092 public final Matrix4f toMatrix(final Matrix4f matrix) {
1093 return matrix.setToRotation(this);
1094 }
1095
1096 /**
1097 * Initializes this quaternion to represent a rotation formed by the given three <i>orthogonal</i> axes.
1098 * <p>
1099 * No validation whether the axes are <i>orthogonal</i> is performed.
1100 * </p>
1101 *
1102 * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
1103 * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
1104 * @param zAxis vector representing the <i>orthogonal</i> z-axis of the coordinate system.
1105 * @return this quaternion for chaining.
1106 */
1107 public final Quaternion setFromAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis) {
1108 return setFromMat(xAxis.x(), yAxis.x(), zAxis.x(),
1109 xAxis.y(), yAxis.y(), zAxis.y(),
1110 xAxis.z(), yAxis.z(), zAxis.z());
1111 }
1112
1113 /**
1114 * Extracts this quaternion's <i>orthogonal</i> rotation axes.
1115 *
1116 * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
1117 * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
1118 * @param zAxis vector representing the <i>orthogonal</i> z-axis of the coordinate system.
1119 * @param tmpMat4 temporary float[4] matrix, used to transform this quaternion to a matrix.
1120 */
1121 public void toAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis, final Matrix4f tmpMat4) {
1122 tmpMat4.setToRotation(this);
1123 tmpMat4.getColumn(2, zAxis);
1124 tmpMat4.getColumn(1, yAxis);
1125 tmpMat4.getColumn(0, xAxis);
1126 }
1127
1128 /**
1129 * Check if the the 3x3 matrix (param) is in fact an affine rotational
1130 * matrix
1131 *
1132 * @param m 3x3 column matrix
1133 * @return true if representing a rotational matrix, false otherwise
1134 */
1135 @Deprecated
1136 public final boolean isRotationMatrix3f(final float[] m) {
1137 final float epsilon = 0.01f; // margin to allow for rounding errors
1138 if (Math.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon)
1139 return false;
1140 if (Math.abs(m[0] * m[2] + m[3] * m[5] + m[6] * m[8]) > epsilon)
1141 return false;
1142 if (Math.abs(m[1] * m[2] + m[4] * m[5] + m[7] * m[8]) > epsilon)
1143 return false;
1144 if (Math.abs(m[0] * m[0] + m[3] * m[3] + m[6] * m[6] - 1) > epsilon)
1145 return false;
1146 if (Math.abs(m[1] * m[1] + m[4] * m[4] + m[7] * m[7] - 1) > epsilon)
1147 return false;
1148 if (Math.abs(m[2] * m[2] + m[5] * m[5] + m[8] * m[8] - 1) > epsilon)
1149 return false;
1150 return (Math.abs(determinant3f(m) - 1) < epsilon);
1151 }
1152
1153 @Deprecated
1154 private final float determinant3f(final float[] m) {
1155 return m[0] * m[4] * m[8] + m[3] * m[7] * m[2] + m[6] * m[1] * m[5]
1156 - m[0] * m[7] * m[5] - m[3] * m[1] * m[8] - m[6] * m[4] * m[2];
1157 }
1158
1159 //
1160 // std java overrides
1161 //
1162
1163 /**
1164 * @param o the object to compare for equality
1165 * @return true if this quaternion and the provided quaternion have roughly the same x, y, z and w values.
1166 */
1167 @Override
1168 public boolean equals(final Object o) {
1169 if (this == o) {
1170 return true;
1171 }
1172 if (!(o instanceof Quaternion)) {
1173 return false;
1174 }
1175 final Quaternion comp = (Quaternion) o;
1176 return Math.abs(x - comp.x()) <= ALLOWED_DEVIANCE &&
1177 Math.abs(y - comp.y()) <= ALLOWED_DEVIANCE &&
1178 Math.abs(z - comp.z()) <= ALLOWED_DEVIANCE &&
1179 Math.abs(w - comp.w()) <= ALLOWED_DEVIANCE;
1180 }
1181 @Override
1182 public final int hashCode() {
1183 throw new InternalError("hashCode not designed");
1184 }
1185
1186 @Override
1187 public String toString() {
1188 return "Quat[x "+x+", y "+y+", z "+z+", w "+w+"]";
1189 }
1190}
Basic Float math utility functions.
Definition: FloatUtil.java:83
static float acos(final float a)
static float sin(final float a)
static float atan2(final float y, final float x)
static final float EPSILON
Epsilon for floating point {@value}, as once computed via getMachineEpsilon() on an AMD-64 CPU.
static float cos(final float a)
static float asin(final float a)
static boolean isZero(final float a, final float epsilon)
Returns true if value is zero, i.e.
static boolean isEqual(final float a, final float b, final float epsilon)
Returns true if both values are equal, i.e.
static float[] makeIdentity(final float[] m)
Make matrix an identity matrix.
Definition: FloatUtil.java:96
static final float HALF_PI
The value PI/2, i.e.
static float sqrt(final float a)
Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations).
Definition: Matrix4f.java:89
final Matrix4f setToRotation(final Quaternion q)
Set this matrix to rotation using the given Quaternion.
Definition: Matrix4f.java:1124
final Quaternion getRotation(final Quaternion res)
Returns the rotation [m00 .
Definition: Matrix4f.java:1185
Vec4f getColumn(final int column, final Vec4f v_out)
Get the named column of the given column-major matrix to v_out.
Definition: Matrix4f.java:311
Quaternion implementation supporting Gimbal-Lock free rotations.
Definition: Quaternion.java:45
Quaternion setFromMat(final Matrix4f m)
Compute the quaternion from a 3x3 column rotation matrix from Matrix4f instance.
final float dot(final float x, final float y, final float z, final float w)
Returns the dot product of this quaternion with the given x,y,z and w components.
final Vec3f rotateVector(final Vec3f vecIn, final Vec3f vecOut)
final Quaternion rotateByEuler(final float bankX, final float headingY, final float attitudeZ)
Rotates this quaternion from the given Euler rotation angles in radians.
Quaternion conjugate()
Conjugates this quaternion [-x, -y, -z, w].
final Quaternion invert()
Invert the quaternion If rotational, will produce a the inverse rotation.
final Quaternion setFromNormalVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec)
Initialize this quaternion from two normalized vectors.
final Matrix4f toMatrix(final Matrix4f matrix)
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
final Quaternion rotateByEuler(final Vec3f angradXYZ)
Rotates this quaternion from the given Euler rotation array angradXYZ in radians.
final Quaternion scale(final float n)
Scale this quaternion by a constant.
Vec3f toEuler(final Vec3f result)
Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
boolean equals(final Object o)
final void setX(final float x)
final Quaternion setFromEuler(final Vec3f angradXYZ)
Initializes this quaternion from the given Euler rotation array angradXYZ in radians.
final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt)
Set this quaternion to a spherical linear interpolation between the given start and end quaternions b...
Quaternion rotateByAngleNormalAxis(final float angle, final float axisX, final float axisY, final float axisZ)
Rotate this quaternion by the given angle and axis.
final float magnitudeSquared()
See magnitude() for special handling of epsilon, which is not applied here.
Definition: Quaternion.java:75
final Quaternion setFromAngleNormalAxis(final float angle, final Vec3f vector)
final void setY(final float y)
final void setZ(final float z)
final Quaternion setFromAngleAxis(final float angle, final Vec3f vector, final Vec3f tmpV3f)
final boolean isRotationMatrix3f(final float[] m)
Check if the the 3x3 matrix (param) is in fact an affine rotational matrix.
static final float ALLOWED_DEVIANCE
Quaternion Epsilon, used with equals method to determine if two Quaternions are close enough to be co...
Definition: Quaternion.java:54
final void setW(final float w)
final Quaternion setFromEuler(final float bankX, final float headingY, final float attitudeZ)
Initializes this quaternion from the given Euler rotation angles in radians.
Quaternion rotateByAngleY(final float angle)
Rotate this quaternion around Y axis with the given angle in radians.
Quaternion rotateByAngleZ(final float angle)
Rotate this quaternion around Z axis with the given angle in radians.
final Quaternion setFromAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis)
Initializes this quaternion to represent a rotation formed by the given three orthogonal axes.
final float toAngleAxis(final Vec3f axis)
Transform the rotational quaternion to axis based rotation angles.
final Quaternion subtract(final Quaternion q)
Subtract a quaternion.
Quaternion(final float x, final float y, final float z, final float w)
Definition: Quaternion.java:64
Quaternion setFromMat(final float m00, final float m01, final float m02, final float m10, final float m11, final float m12, final float m20, final float m21, final float m22)
Compute the quaternion from a 3x3 column rotation matrix.
final float dot(final Quaternion quat)
Returns the dot product of this quaternion with the given quaternion.
final Quaternion setFromVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec, final Vec3f tmpNormalVec)
Initialize this quaternion from two vectors.
final float[] toMatrix(final float[] matrix)
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
void toAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis, final Matrix4f tmpMat4)
Extracts this quaternion's orthogonal rotation axes.
Quaternion rotateByAngleNormalAxis(final float angle, final Vec3f axis)
Rotate this quaternion by the given angle and axis.
Quaternion setLookAt(final Vec3f directionIn, final Vec3f upIn, final Vec3f xAxisOut, final Vec3f yAxisOut, final Vec3f zAxisOut)
Set this quaternion to equal the rotation required to point the z-axis at direction and the y-axis to...
final float magnitude()
Return the magnitude of this quaternion, i.e.
Definition: Quaternion.java:93
final Quaternion mult(final Quaternion q)
Multiply this quaternion by the param quaternion.
final Quaternion setIdentity()
final Quaternion normalize()
Normalize a quaternion required if to be used as a rotational quaternion.
final Quaternion add(final Quaternion q)
Add a quaternion.
final boolean isIdentity()
Returns true if this quaternion has identity.
Quaternion rotateByAngleX(final float angle)
Rotate this quaternion around X axis with the given angle in radians.
Quaternion(final Quaternion q)
Definition: Quaternion.java:60
3D Vector based upon three float components.
Definition: Vec3f.java:37
void setX(final float x)
Definition: Vec3f.java:158
boolean isZero()
Return true if all components are zero, i.e.
Definition: Vec3f.java:276
Vec3f normalize()
Normalize this vector in place.
Definition: Vec3f.java:297
float dot(final Vec3f o)
Return the dot product of this vector and the given one.
Definition: Vec3f.java:338
void setZ(final float z)
Definition: Vec3f.java:160
float length()
Return the length of this vector, a.k.a the norm or magnitude
Definition: Vec3f.java:283
float[] get(final float[] xyz)
xyz = this, returns xyz.
Definition: Vec3f.java:137
Vec3f cross(final Vec3f arg)
Returns this cross arg; creates new vector.
Definition: Vec3f.java:343
void setY(final float y)
Definition: Vec3f.java:159
Vec3f set(final Vec3f o)
this = o, returns this.
Definition: Vec3f.java:79
static boolean isZero(final float x, final float y, final float z, final float epsilon)
Return true if all three vector components are zero, i.e.
Definition: VectorUtil.java:49