1 /* 2 * Copyright (c) 2009-2010 jMonkeyEngine 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are 7 * met: 8 * 9 * * Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 12 * * Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 */ 32 package com.jme3.math; 33 34 import com.jme3.export.*; 35 import com.jme3.util.TempVars; 36 import java.io.Externalizable; 37 import java.io.IOException; 38 import java.io.ObjectInput; 39 import java.io.ObjectOutput; 40 import java.util.logging.Logger; 41 42 /** 43 * <code>Quaternion</code> defines a single example of a more general class of 44 * hypercomplex numbers. Quaternions extends a rotation in three dimensions to a 45 * rotation in four dimensions. This avoids "gimbal lock" and allows for smooth 46 * continuous rotation. 47 * 48 * <code>Quaternion</code> is defined by four floating point numbers: {x y z 49 * w}. 50 * 51 * @author Mark Powell 52 * @author Joshua Slack 53 */ 54 public final class Quaternion implements Savable, Cloneable, java.io.Serializable { 55 56 static final long serialVersionUID = 1; 57 58 private static final Logger logger = Logger.getLogger(Quaternion.class.getName()); 59 /** 60 * Represents the identity quaternion rotation (0, 0, 0, 1). 61 */ 62 public static final Quaternion IDENTITY = new Quaternion(); 63 public static final Quaternion DIRECTION_Z = new Quaternion(); 64 public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0); 65 66 static { DIRECTION_Z.fromAxes(Vector3f.UNIT_X, Vector3f.UNIT_Y, Vector3f.UNIT_Z)67 DIRECTION_Z.fromAxes(Vector3f.UNIT_X, Vector3f.UNIT_Y, Vector3f.UNIT_Z); 68 } 69 protected float x, y, z, w; 70 71 /** 72 * Constructor instantiates a new <code>Quaternion</code> object 73 * initializing all values to zero, except w which is initialized to 1. 74 * 75 */ Quaternion()76 public Quaternion() { 77 x = 0; 78 y = 0; 79 z = 0; 80 w = 1; 81 } 82 83 /** 84 * Constructor instantiates a new <code>Quaternion</code> object from the 85 * given list of parameters. 86 * 87 * @param x 88 * the x value of the quaternion. 89 * @param y 90 * the y value of the quaternion. 91 * @param z 92 * the z value of the quaternion. 93 * @param w 94 * the w value of the quaternion. 95 */ Quaternion(float x, float y, float z, float w)96 public Quaternion(float x, float y, float z, float w) { 97 this.x = x; 98 this.y = y; 99 this.z = z; 100 this.w = w; 101 } 102 getX()103 public float getX() { 104 return x; 105 } 106 getY()107 public float getY() { 108 return y; 109 } 110 getZ()111 public float getZ() { 112 return z; 113 } 114 getW()115 public float getW() { 116 return w; 117 } 118 119 /** 120 * sets the data in a <code>Quaternion</code> object from the given list 121 * of parameters. 122 * 123 * @param x 124 * the x value of the quaternion. 125 * @param y 126 * the y value of the quaternion. 127 * @param z 128 * the z value of the quaternion. 129 * @param w 130 * the w value of the quaternion. 131 * @return this 132 */ set(float x, float y, float z, float w)133 public Quaternion set(float x, float y, float z, float w) { 134 this.x = x; 135 this.y = y; 136 this.z = z; 137 this.w = w; 138 return this; 139 } 140 141 /** 142 * Sets the data in this <code>Quaternion</code> object to be equal to the 143 * passed <code>Quaternion</code> object. The values are copied producing 144 * a new object. 145 * 146 * @param q 147 * The Quaternion to copy values from. 148 * @return this 149 */ set(Quaternion q)150 public Quaternion set(Quaternion q) { 151 this.x = q.x; 152 this.y = q.y; 153 this.z = q.z; 154 this.w = q.w; 155 return this; 156 } 157 158 /** 159 * Constructor instantiates a new <code>Quaternion</code> object from a 160 * collection of rotation angles. 161 * 162 * @param angles 163 * the angles of rotation (x, y, z) that will define the 164 * <code>Quaternion</code>. 165 */ Quaternion(float[] angles)166 public Quaternion(float[] angles) { 167 fromAngles(angles); 168 } 169 170 /** 171 * Constructor instantiates a new <code>Quaternion</code> object from an 172 * interpolation between two other quaternions. 173 * 174 * @param q1 175 * the first quaternion. 176 * @param q2 177 * the second quaternion. 178 * @param interp 179 * the amount to interpolate between the two quaternions. 180 */ Quaternion(Quaternion q1, Quaternion q2, float interp)181 public Quaternion(Quaternion q1, Quaternion q2, float interp) { 182 slerp(q1, q2, interp); 183 } 184 185 /** 186 * Constructor instantiates a new <code>Quaternion</code> object from an 187 * existing quaternion, creating a copy. 188 * 189 * @param q 190 * the quaternion to copy. 191 */ Quaternion(Quaternion q)192 public Quaternion(Quaternion q) { 193 this.x = q.x; 194 this.y = q.y; 195 this.z = q.z; 196 this.w = q.w; 197 } 198 199 /** 200 * Sets this Quaternion to {0, 0, 0, 1}. Same as calling set(0,0,0,1). 201 */ loadIdentity()202 public void loadIdentity() { 203 x = y = z = 0; 204 w = 1; 205 } 206 207 /** 208 * @return true if this Quaternion is {0,0,0,1} 209 */ isIdentity()210 public boolean isIdentity() { 211 if (x == 0 && y == 0 && z == 0 && w == 1) { 212 return true; 213 } else { 214 return false; 215 } 216 } 217 218 /** 219 * <code>fromAngles</code> builds a quaternion from the Euler rotation 220 * angles (y,r,p). 221 * 222 * @param angles 223 * the Euler angles of rotation (in radians). 224 */ fromAngles(float[] angles)225 public Quaternion fromAngles(float[] angles) { 226 if (angles.length != 3) { 227 throw new IllegalArgumentException( 228 "Angles array must have three elements"); 229 } 230 231 return fromAngles(angles[0], angles[1], angles[2]); 232 } 233 234 /** 235 * <code>fromAngles</code> builds a Quaternion from the Euler rotation 236 * angles (y,r,p). Note that we are applying in order: roll, pitch, yaw but 237 * we've ordered them in x, y, and z for convenience. 238 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm</a> 239 * 240 * @param yaw 241 * the Euler yaw of rotation (in radians). (aka Bank, often rot 242 * around x) 243 * @param roll 244 * the Euler roll of rotation (in radians). (aka Heading, often 245 * rot around y) 246 * @param pitch 247 * the Euler pitch of rotation (in radians). (aka Attitude, often 248 * rot around z) 249 */ fromAngles(float yaw, float roll, float pitch)250 public Quaternion fromAngles(float yaw, float roll, float pitch) { 251 float angle; 252 float sinRoll, sinPitch, sinYaw, cosRoll, cosPitch, cosYaw; 253 angle = pitch * 0.5f; 254 sinPitch = FastMath.sin(angle); 255 cosPitch = FastMath.cos(angle); 256 angle = roll * 0.5f; 257 sinRoll = FastMath.sin(angle); 258 cosRoll = FastMath.cos(angle); 259 angle = yaw * 0.5f; 260 sinYaw = FastMath.sin(angle); 261 cosYaw = FastMath.cos(angle); 262 263 // variables used to reduce multiplication calls. 264 float cosRollXcosPitch = cosRoll * cosPitch; 265 float sinRollXsinPitch = sinRoll * sinPitch; 266 float cosRollXsinPitch = cosRoll * sinPitch; 267 float sinRollXcosPitch = sinRoll * cosPitch; 268 269 w = (cosRollXcosPitch * cosYaw - sinRollXsinPitch * sinYaw); 270 x = (cosRollXcosPitch * sinYaw + sinRollXsinPitch * cosYaw); 271 y = (sinRollXcosPitch * cosYaw + cosRollXsinPitch * sinYaw); 272 z = (cosRollXsinPitch * cosYaw - sinRollXcosPitch * sinYaw); 273 274 normalize(); 275 return this; 276 } 277 278 /** 279 * <code>toAngles</code> returns this quaternion converted to Euler 280 * rotation angles (yaw,roll,pitch).<br/> 281 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm</a> 282 * 283 * @param angles 284 * the float[] in which the angles should be stored, or null if 285 * you want a new float[] to be created 286 * @return the float[] in which the angles are stored. 287 */ toAngles(float[] angles)288 public float[] toAngles(float[] angles) { 289 if (angles == null) { 290 angles = new float[3]; 291 } else if (angles.length != 3) { 292 throw new IllegalArgumentException("Angles array must have three elements"); 293 } 294 295 float sqw = w * w; 296 float sqx = x * x; 297 float sqy = y * y; 298 float sqz = z * z; 299 float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise 300 // is correction factor 301 float test = x * y + z * w; 302 if (test > 0.499 * unit) { // singularity at north pole 303 angles[1] = 2 * FastMath.atan2(x, w); 304 angles[2] = FastMath.HALF_PI; 305 angles[0] = 0; 306 } else if (test < -0.499 * unit) { // singularity at south pole 307 angles[1] = -2 * FastMath.atan2(x, w); 308 angles[2] = -FastMath.HALF_PI; 309 angles[0] = 0; 310 } else { 311 angles[1] = FastMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // roll or heading 312 angles[2] = FastMath.asin(2 * test / unit); // pitch or attitude 313 angles[0] = FastMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // yaw or bank 314 } 315 return angles; 316 } 317 318 /** 319 * 320 * <code>fromRotationMatrix</code> generates a quaternion from a supplied 321 * matrix. This matrix is assumed to be a rotational matrix. 322 * 323 * @param matrix 324 * the matrix that defines the rotation. 325 */ fromRotationMatrix(Matrix3f matrix)326 public Quaternion fromRotationMatrix(Matrix3f matrix) { 327 return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, 328 matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22); 329 } 330 fromRotationMatrix(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)331 public Quaternion fromRotationMatrix(float m00, float m01, float m02, 332 float m10, float m11, float m12, 333 float m20, float m21, float m22) { 334 // Use the Graphics Gems code, from 335 // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z 336 // *NOT* the "Matrix and Quaternions FAQ", which has errors! 337 338 // the trace is the sum of the diagonal elements; see 339 // http://mathworld.wolfram.com/MatrixTrace.html 340 float t = m00 + m11 + m22; 341 342 // we protect the division by s by ensuring that s>=1 343 if (t >= 0) { // |w| >= .5 344 float s = FastMath.sqrt(t + 1); // |s|>=1 ... 345 w = 0.5f * s; 346 s = 0.5f / s; // so this division isn't bad 347 x = (m21 - m12) * s; 348 y = (m02 - m20) * s; 349 z = (m10 - m01) * s; 350 } else if ((m00 > m11) && (m00 > m22)) { 351 float s = FastMath.sqrt(1.0f + m00 - m11 - m22); // |s|>=1 352 x = s * 0.5f; // |x| >= .5 353 s = 0.5f / s; 354 y = (m10 + m01) * s; 355 z = (m02 + m20) * s; 356 w = (m21 - m12) * s; 357 } else if (m11 > m22) { 358 float s = FastMath.sqrt(1.0f + m11 - m00 - m22); // |s|>=1 359 y = s * 0.5f; // |y| >= .5 360 s = 0.5f / s; 361 x = (m10 + m01) * s; 362 z = (m21 + m12) * s; 363 w = (m02 - m20) * s; 364 } else { 365 float s = FastMath.sqrt(1.0f + m22 - m00 - m11); // |s|>=1 366 z = s * 0.5f; // |z| >= .5 367 s = 0.5f / s; 368 x = (m02 + m20) * s; 369 y = (m21 + m12) * s; 370 w = (m10 - m01) * s; 371 } 372 373 return this; 374 } 375 376 /** 377 * <code>toRotationMatrix</code> converts this quaternion to a rotational 378 * matrix. Note: the result is created from a normalized version of this quat. 379 * 380 * @return the rotation matrix representation of this quaternion. 381 */ toRotationMatrix()382 public Matrix3f toRotationMatrix() { 383 Matrix3f matrix = new Matrix3f(); 384 return toRotationMatrix(matrix); 385 } 386 387 /** 388 * <code>toRotationMatrix</code> converts this quaternion to a rotational 389 * matrix. The result is stored in result. 390 * 391 * @param result 392 * The Matrix3f to store the result in. 393 * @return the rotation matrix representation of this quaternion. 394 */ toRotationMatrix(Matrix3f result)395 public Matrix3f toRotationMatrix(Matrix3f result) { 396 397 float norm = norm(); 398 // we explicitly test norm against one here, saving a division 399 // at the cost of a test and branch. Is it worth it? 400 float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0; 401 402 // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs 403 // will be used 2-4 times each. 404 float xs = x * s; 405 float ys = y * s; 406 float zs = z * s; 407 float xx = x * xs; 408 float xy = x * ys; 409 float xz = x * zs; 410 float xw = w * xs; 411 float yy = y * ys; 412 float yz = y * zs; 413 float yw = w * ys; 414 float zz = z * zs; 415 float zw = w * zs; 416 417 // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here 418 result.m00 = 1 - (yy + zz); 419 result.m01 = (xy - zw); 420 result.m02 = (xz + yw); 421 result.m10 = (xy + zw); 422 result.m11 = 1 - (xx + zz); 423 result.m12 = (yz - xw); 424 result.m20 = (xz - yw); 425 result.m21 = (yz + xw); 426 result.m22 = 1 - (xx + yy); 427 428 return result; 429 } 430 431 /** 432 * <code>toRotationMatrix</code> converts this quaternion to a rotational 433 * matrix. The result is stored in result. 4th row and 4th column values are 434 * untouched. Note: the result is created from a normalized version of this quat. 435 * 436 * @param result 437 * The Matrix4f to store the result in. 438 * @return the rotation matrix representation of this quaternion. 439 */ toRotationMatrix(Matrix4f result)440 public Matrix4f toRotationMatrix(Matrix4f result) { 441 442 float norm = norm(); 443 // we explicitly test norm against one here, saving a division 444 // at the cost of a test and branch. Is it worth it? 445 float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0; 446 447 // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs 448 // will be used 2-4 times each. 449 float xs = x * s; 450 float ys = y * s; 451 float zs = z * s; 452 float xx = x * xs; 453 float xy = x * ys; 454 float xz = x * zs; 455 float xw = w * xs; 456 float yy = y * ys; 457 float yz = y * zs; 458 float yw = w * ys; 459 float zz = z * zs; 460 float zw = w * zs; 461 462 // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here 463 result.m00 = 1 - (yy + zz); 464 result.m01 = (xy - zw); 465 result.m02 = (xz + yw); 466 result.m10 = (xy + zw); 467 result.m11 = 1 - (xx + zz); 468 result.m12 = (yz - xw); 469 result.m20 = (xz - yw); 470 result.m21 = (yz + xw); 471 result.m22 = 1 - (xx + yy); 472 473 return result; 474 } 475 476 /** 477 * <code>getRotationColumn</code> returns one of three columns specified 478 * by the parameter. This column is returned as a <code>Vector3f</code> 479 * object. 480 * 481 * @param i 482 * the column to retrieve. Must be between 0 and 2. 483 * @return the column specified by the index. 484 */ getRotationColumn(int i)485 public Vector3f getRotationColumn(int i) { 486 return getRotationColumn(i, null); 487 } 488 489 /** 490 * <code>getRotationColumn</code> returns one of three columns specified 491 * by the parameter. This column is returned as a <code>Vector3f</code> 492 * object. The value is retrieved as if this quaternion was first normalized. 493 * 494 * @param i 495 * the column to retrieve. Must be between 0 and 2. 496 * @param store 497 * the vector object to store the result in. if null, a new one 498 * is created. 499 * @return the column specified by the index. 500 */ getRotationColumn(int i, Vector3f store)501 public Vector3f getRotationColumn(int i, Vector3f store) { 502 if (store == null) { 503 store = new Vector3f(); 504 } 505 506 float norm = norm(); 507 if (norm != 1.0f) { 508 norm = FastMath.invSqrt(norm); 509 } 510 511 float xx = x * x * norm; 512 float xy = x * y * norm; 513 float xz = x * z * norm; 514 float xw = x * w * norm; 515 float yy = y * y * norm; 516 float yz = y * z * norm; 517 float yw = y * w * norm; 518 float zz = z * z * norm; 519 float zw = z * w * norm; 520 521 switch (i) { 522 case 0: 523 store.x = 1 - 2 * (yy + zz); 524 store.y = 2 * (xy + zw); 525 store.z = 2 * (xz - yw); 526 break; 527 case 1: 528 store.x = 2 * (xy - zw); 529 store.y = 1 - 2 * (xx + zz); 530 store.z = 2 * (yz + xw); 531 break; 532 case 2: 533 store.x = 2 * (xz + yw); 534 store.y = 2 * (yz - xw); 535 store.z = 1 - 2 * (xx + yy); 536 break; 537 default: 538 logger.warning("Invalid column index."); 539 throw new IllegalArgumentException("Invalid column index. " + i); 540 } 541 542 return store; 543 } 544 545 /** 546 * <code>fromAngleAxis</code> sets this quaternion to the values specified 547 * by an angle and an axis of rotation. This method creates an object, so 548 * use fromAngleNormalAxis if your axis is already normalized. 549 * 550 * @param angle 551 * the angle to rotate (in radians). 552 * @param axis 553 * the axis of rotation. 554 * @return this quaternion 555 */ fromAngleAxis(float angle, Vector3f axis)556 public Quaternion fromAngleAxis(float angle, Vector3f axis) { 557 Vector3f normAxis = axis.normalize(); 558 fromAngleNormalAxis(angle, normAxis); 559 return this; 560 } 561 562 /** 563 * <code>fromAngleNormalAxis</code> sets this quaternion to the values 564 * specified by an angle and a normalized axis of rotation. 565 * 566 * @param angle 567 * the angle to rotate (in radians). 568 * @param axis 569 * the axis of rotation (already normalized). 570 */ fromAngleNormalAxis(float angle, Vector3f axis)571 public Quaternion fromAngleNormalAxis(float angle, Vector3f axis) { 572 if (axis.x == 0 && axis.y == 0 && axis.z == 0) { 573 loadIdentity(); 574 } else { 575 float halfAngle = 0.5f * angle; 576 float sin = FastMath.sin(halfAngle); 577 w = FastMath.cos(halfAngle); 578 x = sin * axis.x; 579 y = sin * axis.y; 580 z = sin * axis.z; 581 } 582 return this; 583 } 584 585 /** 586 * <code>toAngleAxis</code> sets a given angle and axis to that 587 * represented by the current quaternion. The values are stored as 588 * following: The axis is provided as a parameter and built by the method, 589 * the angle is returned as a float. 590 * 591 * @param axisStore 592 * the object we'll store the computed axis in. 593 * @return the angle of rotation in radians. 594 */ toAngleAxis(Vector3f axisStore)595 public float toAngleAxis(Vector3f axisStore) { 596 float sqrLength = x * x + y * y + z * z; 597 float angle; 598 if (sqrLength == 0.0f) { 599 angle = 0.0f; 600 if (axisStore != null) { 601 axisStore.x = 1.0f; 602 axisStore.y = 0.0f; 603 axisStore.z = 0.0f; 604 } 605 } else { 606 angle = (2.0f * FastMath.acos(w)); 607 if (axisStore != null) { 608 float invLength = (1.0f / FastMath.sqrt(sqrLength)); 609 axisStore.x = x * invLength; 610 axisStore.y = y * invLength; 611 axisStore.z = z * invLength; 612 } 613 } 614 615 return angle; 616 } 617 618 /** 619 * <code>slerp</code> sets this quaternion's value as an interpolation 620 * between two other quaternions. 621 * 622 * @param q1 623 * the first quaternion. 624 * @param q2 625 * the second quaternion. 626 * @param t 627 * the amount to interpolate between the two quaternions. 628 */ slerp(Quaternion q1, Quaternion q2, float t)629 public Quaternion slerp(Quaternion q1, Quaternion q2, float t) { 630 // Create a local quaternion to store the interpolated quaternion 631 if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) { 632 this.set(q1); 633 return this; 634 } 635 636 float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) 637 + (q1.w * q2.w); 638 639 if (result < 0.0f) { 640 // Negate the second quaternion and the result of the dot product 641 q2.x = -q2.x; 642 q2.y = -q2.y; 643 q2.z = -q2.z; 644 q2.w = -q2.w; 645 result = -result; 646 } 647 648 // Set the first and second scale for the interpolation 649 float scale0 = 1 - t; 650 float scale1 = t; 651 652 // Check if the angle between the 2 quaternions was big enough to 653 // warrant such calculations 654 if ((1 - result) > 0.1f) {// Get the angle between the 2 quaternions, 655 // and then store the sin() of that angle 656 float theta = FastMath.acos(result); 657 float invSinTheta = 1f / FastMath.sin(theta); 658 659 // Calculate the scale for q1 and q2, according to the angle and 660 // it's sine value 661 scale0 = FastMath.sin((1 - t) * theta) * invSinTheta; 662 scale1 = FastMath.sin((t * theta)) * invSinTheta; 663 } 664 665 // Calculate the x, y, z and w values for the quaternion by using a 666 // special 667 // form of linear interpolation for quaternions. 668 this.x = (scale0 * q1.x) + (scale1 * q2.x); 669 this.y = (scale0 * q1.y) + (scale1 * q2.y); 670 this.z = (scale0 * q1.z) + (scale1 * q2.z); 671 this.w = (scale0 * q1.w) + (scale1 * q2.w); 672 673 // Return the interpolated quaternion 674 return this; 675 } 676 677 /** 678 * Sets the values of this quaternion to the slerp from itself to q2 by 679 * changeAmnt 680 * 681 * @param q2 682 * Final interpolation value 683 * @param changeAmnt 684 * The amount diffrence 685 */ slerp(Quaternion q2, float changeAmnt)686 public void slerp(Quaternion q2, float changeAmnt) { 687 if (this.x == q2.x && this.y == q2.y && this.z == q2.z 688 && this.w == q2.w) { 689 return; 690 } 691 692 float result = (this.x * q2.x) + (this.y * q2.y) + (this.z * q2.z) 693 + (this.w * q2.w); 694 695 if (result < 0.0f) { 696 // Negate the second quaternion and the result of the dot product 697 q2.x = -q2.x; 698 q2.y = -q2.y; 699 q2.z = -q2.z; 700 q2.w = -q2.w; 701 result = -result; 702 } 703 704 // Set the first and second scale for the interpolation 705 float scale0 = 1 - changeAmnt; 706 float scale1 = changeAmnt; 707 708 // Check if the angle between the 2 quaternions was big enough to 709 // warrant such calculations 710 if ((1 - result) > 0.1f) { 711 // Get the angle between the 2 quaternions, and then store the sin() 712 // of that angle 713 float theta = FastMath.acos(result); 714 float invSinTheta = 1f / FastMath.sin(theta); 715 716 // Calculate the scale for q1 and q2, according to the angle and 717 // it's sine value 718 scale0 = FastMath.sin((1 - changeAmnt) * theta) * invSinTheta; 719 scale1 = FastMath.sin((changeAmnt * theta)) * invSinTheta; 720 } 721 722 // Calculate the x, y, z and w values for the quaternion by using a 723 // special 724 // form of linear interpolation for quaternions. 725 this.x = (scale0 * this.x) + (scale1 * q2.x); 726 this.y = (scale0 * this.y) + (scale1 * q2.y); 727 this.z = (scale0 * this.z) + (scale1 * q2.z); 728 this.w = (scale0 * this.w) + (scale1 * q2.w); 729 } 730 731 /** 732 * Sets the values of this quaternion to the nlerp from itself to q2 by blend. 733 * @param q2 734 * @param blend 735 */ nlerp(Quaternion q2, float blend)736 public void nlerp(Quaternion q2, float blend) { 737 float dot = dot(q2); 738 float blendI = 1.0f - blend; 739 if (dot < 0.0f) { 740 x = blendI * x - blend * q2.x; 741 y = blendI * y - blend * q2.y; 742 z = blendI * z - blend * q2.z; 743 w = blendI * w - blend * q2.w; 744 } else { 745 x = blendI * x + blend * q2.x; 746 y = blendI * y + blend * q2.y; 747 z = blendI * z + blend * q2.z; 748 w = blendI * w + blend * q2.w; 749 } 750 normalizeLocal(); 751 } 752 753 /** 754 * <code>add</code> adds the values of this quaternion to those of the 755 * parameter quaternion. The result is returned as a new quaternion. 756 * 757 * @param q 758 * the quaternion to add to this. 759 * @return the new quaternion. 760 */ add(Quaternion q)761 public Quaternion add(Quaternion q) { 762 return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w); 763 } 764 765 /** 766 * <code>add</code> adds the values of this quaternion to those of the 767 * parameter quaternion. The result is stored in this Quaternion. 768 * 769 * @param q 770 * the quaternion to add to this. 771 * @return This Quaternion after addition. 772 */ addLocal(Quaternion q)773 public Quaternion addLocal(Quaternion q) { 774 this.x += q.x; 775 this.y += q.y; 776 this.z += q.z; 777 this.w += q.w; 778 return this; 779 } 780 781 /** 782 * <code>subtract</code> subtracts the values of the parameter quaternion 783 * from those of this quaternion. The result is returned as a new 784 * quaternion. 785 * 786 * @param q 787 * the quaternion to subtract from this. 788 * @return the new quaternion. 789 */ subtract(Quaternion q)790 public Quaternion subtract(Quaternion q) { 791 return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w); 792 } 793 794 /** 795 * <code>subtract</code> subtracts the values of the parameter quaternion 796 * from those of this quaternion. The result is stored in this Quaternion. 797 * 798 * @param q 799 * the quaternion to subtract from this. 800 * @return This Quaternion after subtraction. 801 */ subtractLocal(Quaternion q)802 public Quaternion subtractLocal(Quaternion q) { 803 this.x -= q.x; 804 this.y -= q.y; 805 this.z -= q.z; 806 this.w -= q.w; 807 return this; 808 } 809 810 /** 811 * <code>mult</code> multiplies this quaternion by a parameter quaternion. 812 * The result is returned as a new quaternion. It should be noted that 813 * quaternion multiplication is not commutative so q * p != p * q. 814 * 815 * @param q 816 * the quaternion to multiply this quaternion by. 817 * @return the new quaternion. 818 */ mult(Quaternion q)819 public Quaternion mult(Quaternion q) { 820 return mult(q, null); 821 } 822 823 /** 824 * <code>mult</code> multiplies this quaternion by a parameter quaternion. 825 * The result is returned as a new quaternion. It should be noted that 826 * quaternion multiplication is not commutative so q * p != p * q. 827 * 828 * It IS safe for q and res to be the same object. 829 * It IS safe for this and res to be the same object. 830 * 831 * @param q 832 * the quaternion to multiply this quaternion by. 833 * @param res 834 * the quaternion to store the result in. 835 * @return the new quaternion. 836 */ mult(Quaternion q, Quaternion res)837 public Quaternion mult(Quaternion q, Quaternion res) { 838 if (res == null) { 839 res = new Quaternion(); 840 } 841 float qw = q.w, qx = q.x, qy = q.y, qz = q.z; 842 res.x = x * qw + y * qz - z * qy + w * qx; 843 res.y = -x * qz + y * qw + z * qx + w * qy; 844 res.z = x * qy - y * qx + z * qw + w * qz; 845 res.w = -x * qx - y * qy - z * qz + w * qw; 846 return res; 847 } 848 849 /** 850 * <code>apply</code> multiplies this quaternion by a parameter matrix 851 * internally. 852 * 853 * @param matrix 854 * the matrix to apply to this quaternion. 855 */ apply(Matrix3f matrix)856 public void apply(Matrix3f matrix) { 857 float oldX = x, oldY = y, oldZ = z, oldW = w; 858 fromRotationMatrix(matrix); 859 float tempX = x, tempY = y, tempZ = z, tempW = w; 860 861 x = oldX * tempW + oldY * tempZ - oldZ * tempY + oldW * tempX; 862 y = -oldX * tempZ + oldY * tempW + oldZ * tempX + oldW * tempY; 863 z = oldX * tempY - oldY * tempX + oldZ * tempW + oldW * tempZ; 864 w = -oldX * tempX - oldY * tempY - oldZ * tempZ + oldW * tempW; 865 } 866 867 /** 868 * 869 * <code>fromAxes</code> creates a <code>Quaternion</code> that 870 * represents the coordinate system defined by three axes. These axes are 871 * assumed to be orthogonal and no error checking is applied. Thus, the user 872 * must insure that the three axes being provided indeed represents a proper 873 * right handed coordinate system. 874 * 875 * @param axis 876 * the array containing the three vectors representing the 877 * coordinate system. 878 */ fromAxes(Vector3f[] axis)879 public Quaternion fromAxes(Vector3f[] axis) { 880 if (axis.length != 3) { 881 throw new IllegalArgumentException( 882 "Axis array must have three elements"); 883 } 884 return fromAxes(axis[0], axis[1], axis[2]); 885 } 886 887 /** 888 * 889 * <code>fromAxes</code> creates a <code>Quaternion</code> that 890 * represents the coordinate system defined by three axes. These axes are 891 * assumed to be orthogonal and no error checking is applied. Thus, the user 892 * must insure that the three axes being provided indeed represents a proper 893 * right handed coordinate system. 894 * 895 * @param xAxis vector representing the x-axis of the coordinate system. 896 * @param yAxis vector representing the y-axis of the coordinate system. 897 * @param zAxis vector representing the z-axis of the coordinate system. 898 */ fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis)899 public Quaternion fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis) { 900 return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y, 901 zAxis.y, xAxis.z, yAxis.z, zAxis.z); 902 } 903 904 /** 905 * 906 * <code>toAxes</code> takes in an array of three vectors. Each vector 907 * corresponds to an axis of the coordinate system defined by the quaternion 908 * rotation. 909 * 910 * @param axis 911 * the array of vectors to be filled. 912 */ toAxes(Vector3f axis[])913 public void toAxes(Vector3f axis[]) { 914 Matrix3f tempMat = toRotationMatrix(); 915 axis[0] = tempMat.getColumn(0, axis[0]); 916 axis[1] = tempMat.getColumn(1, axis[1]); 917 axis[2] = tempMat.getColumn(2, axis[2]); 918 } 919 920 /** 921 * <code>mult</code> multiplies this quaternion by a parameter vector. The 922 * result is returned as a new vector. 923 * 924 * @param v 925 * the vector to multiply this quaternion by. 926 * @return the new vector. 927 */ mult(Vector3f v)928 public Vector3f mult(Vector3f v) { 929 return mult(v, null); 930 } 931 932 /** 933 * <code>mult</code> multiplies this quaternion by a parameter vector. The 934 * result is stored in the supplied vector 935 * 936 * @param v 937 * the vector to multiply this quaternion by. 938 * @return v 939 */ multLocal(Vector3f v)940 public Vector3f multLocal(Vector3f v) { 941 float tempX, tempY; 942 tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x 943 + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x; 944 tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z 945 * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x 946 * v.y; 947 v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x 948 - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z; 949 v.x = tempX; 950 v.y = tempY; 951 return v; 952 } 953 954 /** 955 * Multiplies this Quaternion by the supplied quaternion. The result is 956 * stored in this Quaternion, which is also returned for chaining. Similar 957 * to this *= q. 958 * 959 * @param q 960 * The Quaternion to multiply this one by. 961 * @return This Quaternion, after multiplication. 962 */ multLocal(Quaternion q)963 public Quaternion multLocal(Quaternion q) { 964 float x1 = x * q.w + y * q.z - z * q.y + w * q.x; 965 float y1 = -x * q.z + y * q.w + z * q.x + w * q.y; 966 float z1 = x * q.y - y * q.x + z * q.w + w * q.z; 967 w = -x * q.x - y * q.y - z * q.z + w * q.w; 968 x = x1; 969 y = y1; 970 z = z1; 971 return this; 972 } 973 974 /** 975 * Multiplies this Quaternion by the supplied quaternion. The result is 976 * stored in this Quaternion, which is also returned for chaining. Similar 977 * to this *= q. 978 * 979 * @param qx - 980 * quat x value 981 * @param qy - 982 * quat y value 983 * @param qz - 984 * quat z value 985 * @param qw - 986 * quat w value 987 * 988 * @return This Quaternion, after multiplication. 989 */ multLocal(float qx, float qy, float qz, float qw)990 public Quaternion multLocal(float qx, float qy, float qz, float qw) { 991 float x1 = x * qw + y * qz - z * qy + w * qx; 992 float y1 = -x * qz + y * qw + z * qx + w * qy; 993 float z1 = x * qy - y * qx + z * qw + w * qz; 994 w = -x * qx - y * qy - z * qz + w * qw; 995 x = x1; 996 y = y1; 997 z = z1; 998 return this; 999 } 1000 1001 /** 1002 * <code>mult</code> multiplies this quaternion by a parameter vector. The 1003 * result is returned as a new vector. 1004 * 1005 * @param v 1006 * the vector to multiply this quaternion by. 1007 * @param store 1008 * the vector to store the result in. It IS safe for v and store 1009 * to be the same object. 1010 * @return the result vector. 1011 */ mult(Vector3f v, Vector3f store)1012 public Vector3f mult(Vector3f v, Vector3f store) { 1013 if (store == null) { 1014 store = new Vector3f(); 1015 } 1016 if (v.x == 0 && v.y == 0 && v.z == 0) { 1017 store.set(0, 0, 0); 1018 } else { 1019 float vx = v.x, vy = v.y, vz = v.z; 1020 store.x = w * w * vx + 2 * y * w * vz - 2 * z * w * vy + x * x 1021 * vx + 2 * y * x * vy + 2 * z * x * vz - z * z * vx - y 1022 * y * vx; 1023 store.y = 2 * x * y * vx + y * y * vy + 2 * z * y * vz + 2 * w 1024 * z * vx - z * z * vy + w * w * vy - 2 * x * w * vz - x 1025 * x * vy; 1026 store.z = 2 * x * z * vx + 2 * y * z * vy + z * z * vz - 2 * w 1027 * y * vx - y * y * vz + 2 * w * x * vy - x * x * vz + w 1028 * w * vz; 1029 } 1030 return store; 1031 } 1032 1033 /** 1034 * <code>mult</code> multiplies this quaternion by a parameter scalar. The 1035 * result is returned as a new quaternion. 1036 * 1037 * @param scalar 1038 * the quaternion to multiply this quaternion by. 1039 * @return the new quaternion. 1040 */ mult(float scalar)1041 public Quaternion mult(float scalar) { 1042 return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w); 1043 } 1044 1045 /** 1046 * <code>mult</code> multiplies this quaternion by a parameter scalar. The 1047 * result is stored locally. 1048 * 1049 * @param scalar 1050 * the quaternion to multiply this quaternion by. 1051 * @return this. 1052 */ multLocal(float scalar)1053 public Quaternion multLocal(float scalar) { 1054 w *= scalar; 1055 x *= scalar; 1056 y *= scalar; 1057 z *= scalar; 1058 return this; 1059 } 1060 1061 /** 1062 * <code>dot</code> calculates and returns the dot product of this 1063 * quaternion with that of the parameter quaternion. 1064 * 1065 * @param q 1066 * the quaternion to calculate the dot product of. 1067 * @return the dot product of this and the parameter quaternion. 1068 */ dot(Quaternion q)1069 public float dot(Quaternion q) { 1070 return w * q.w + x * q.x + y * q.y + z * q.z; 1071 } 1072 1073 /** 1074 * <code>norm</code> returns the norm of this quaternion. This is the dot 1075 * product of this quaternion with itself. 1076 * 1077 * @return the norm of the quaternion. 1078 */ norm()1079 public float norm() { 1080 return w * w + x * x + y * y + z * z; 1081 } 1082 1083 /** 1084 * <code>normalize</code> normalizes the current <code>Quaternion</code> 1085 * @deprecated The naming of this method doesn't follow convention. 1086 * Please use {@link Quaternion#normalizeLocal() } instead. 1087 */ 1088 @Deprecated normalize()1089 public void normalize() { 1090 float n = FastMath.invSqrt(norm()); 1091 x *= n; 1092 y *= n; 1093 z *= n; 1094 w *= n; 1095 } 1096 1097 /** 1098 * <code>normalize</code> normalizes the current <code>Quaternion</code> 1099 */ normalizeLocal()1100 public void normalizeLocal() { 1101 float n = FastMath.invSqrt(norm()); 1102 x *= n; 1103 y *= n; 1104 z *= n; 1105 w *= n; 1106 } 1107 1108 /** 1109 * <code>inverse</code> returns the inverse of this quaternion as a new 1110 * quaternion. If this quaternion does not have an inverse (if its normal is 1111 * 0 or less), then null is returned. 1112 * 1113 * @return the inverse of this quaternion or null if the inverse does not 1114 * exist. 1115 */ inverse()1116 public Quaternion inverse() { 1117 float norm = norm(); 1118 if (norm > 0.0) { 1119 float invNorm = 1.0f / norm; 1120 return new Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w 1121 * invNorm); 1122 } 1123 // return an invalid result to flag the error 1124 return null; 1125 } 1126 1127 /** 1128 * <code>inverse</code> calculates the inverse of this quaternion and 1129 * returns this quaternion after it is calculated. If this quaternion does 1130 * not have an inverse (if it's norma is 0 or less), nothing happens 1131 * 1132 * @return the inverse of this quaternion 1133 */ inverseLocal()1134 public Quaternion inverseLocal() { 1135 float norm = norm(); 1136 if (norm > 0.0) { 1137 float invNorm = 1.0f / norm; 1138 x *= -invNorm; 1139 y *= -invNorm; 1140 z *= -invNorm; 1141 w *= invNorm; 1142 } 1143 return this; 1144 } 1145 1146 /** 1147 * <code>negate</code> inverts the values of the quaternion. 1148 * 1149 */ negate()1150 public void negate() { 1151 x *= -1; 1152 y *= -1; 1153 z *= -1; 1154 w *= -1; 1155 } 1156 1157 /** 1158 * 1159 * <code>toString</code> creates the string representation of this 1160 * <code>Quaternion</code>. The values of the quaternion are displace (x, 1161 * y, z, w), in the following manner: <br> 1162 * (x, y, z, w) 1163 * 1164 * @return the string representation of this object. 1165 * @see java.lang.Object#toString() 1166 */ 1167 @Override toString()1168 public String toString() { 1169 return "(" + x + ", " + y + ", " + z + ", " + w + ")"; 1170 } 1171 1172 /** 1173 * <code>equals</code> determines if two quaternions are logically equal, 1174 * that is, if the values of (x, y, z, w) are the same for both quaternions. 1175 * 1176 * @param o 1177 * the object to compare for equality 1178 * @return true if they are equal, false otherwise. 1179 */ 1180 @Override equals(Object o)1181 public boolean equals(Object o) { 1182 if (!(o instanceof Quaternion)) { 1183 return false; 1184 } 1185 1186 if (this == o) { 1187 return true; 1188 } 1189 1190 Quaternion comp = (Quaternion) o; 1191 if (Float.compare(x, comp.x) != 0) { 1192 return false; 1193 } 1194 if (Float.compare(y, comp.y) != 0) { 1195 return false; 1196 } 1197 if (Float.compare(z, comp.z) != 0) { 1198 return false; 1199 } 1200 if (Float.compare(w, comp.w) != 0) { 1201 return false; 1202 } 1203 return true; 1204 } 1205 1206 /** 1207 * 1208 * <code>hashCode</code> returns the hash code value as an integer and is 1209 * supported for the benefit of hashing based collection classes such as 1210 * Hashtable, HashMap, HashSet etc. 1211 * 1212 * @return the hashcode for this instance of Quaternion. 1213 * @see java.lang.Object#hashCode() 1214 */ 1215 @Override hashCode()1216 public int hashCode() { 1217 int hash = 37; 1218 hash = 37 * hash + Float.floatToIntBits(x); 1219 hash = 37 * hash + Float.floatToIntBits(y); 1220 hash = 37 * hash + Float.floatToIntBits(z); 1221 hash = 37 * hash + Float.floatToIntBits(w); 1222 return hash; 1223 1224 } 1225 1226 /** 1227 * <code>readExternal</code> builds a quaternion from an 1228 * <code>ObjectInput</code> object. <br> 1229 * NOTE: Used with serialization. Not to be called manually. 1230 * 1231 * @param in 1232 * the ObjectInput value to read from. 1233 * @throws IOException 1234 * if the ObjectInput value has problems reading a float. 1235 * @see java.io.Externalizable 1236 */ readExternal(ObjectInput in)1237 public void readExternal(ObjectInput in) throws IOException { 1238 x = in.readFloat(); 1239 y = in.readFloat(); 1240 z = in.readFloat(); 1241 w = in.readFloat(); 1242 } 1243 1244 /** 1245 * <code>writeExternal</code> writes this quaternion out to a 1246 * <code>ObjectOutput</code> object. NOTE: Used with serialization. Not to 1247 * be called manually. 1248 * 1249 * @param out 1250 * the object to write to. 1251 * @throws IOException 1252 * if writing to the ObjectOutput fails. 1253 * @see java.io.Externalizable 1254 */ writeExternal(ObjectOutput out)1255 public void writeExternal(ObjectOutput out) throws IOException { 1256 out.writeFloat(x); 1257 out.writeFloat(y); 1258 out.writeFloat(z); 1259 out.writeFloat(w); 1260 } 1261 1262 /** 1263 * <code>lookAt</code> is a convienence method for auto-setting the 1264 * quaternion based on a direction and an up vector. It computes 1265 * the rotation to transform the z-axis to point into 'direction' 1266 * and the y-axis to 'up'. 1267 * 1268 * @param direction 1269 * where to look at in terms of local coordinates 1270 * @param up 1271 * a vector indicating the local up direction. 1272 * (typically {0, 1, 0} in jME.) 1273 */ lookAt(Vector3f direction, Vector3f up)1274 public void lookAt(Vector3f direction, Vector3f up) { 1275 TempVars vars = TempVars.get(); 1276 vars.vect3.set(direction).normalizeLocal(); 1277 vars.vect1.set(up).crossLocal(direction).normalizeLocal(); 1278 vars.vect2.set(direction).crossLocal(vars.vect1).normalizeLocal(); 1279 fromAxes(vars.vect1, vars.vect2, vars.vect3); 1280 vars.release(); 1281 } 1282 write(JmeExporter e)1283 public void write(JmeExporter e) throws IOException { 1284 OutputCapsule cap = e.getCapsule(this); 1285 cap.write(x, "x", 0); 1286 cap.write(y, "y", 0); 1287 cap.write(z, "z", 0); 1288 cap.write(w, "w", 1); 1289 } 1290 read(JmeImporter e)1291 public void read(JmeImporter e) throws IOException { 1292 InputCapsule cap = e.getCapsule(this); 1293 x = cap.readFloat("x", 0); 1294 y = cap.readFloat("y", 0); 1295 z = cap.readFloat("z", 0); 1296 w = cap.readFloat("w", 1); 1297 } 1298 1299 /** 1300 * @return A new quaternion that describes a rotation that would point you 1301 * in the exact opposite direction of this Quaternion. 1302 */ opposite()1303 public Quaternion opposite() { 1304 return opposite(null); 1305 } 1306 1307 /** 1308 * FIXME: This seems to have singularity type issues with angle == 0, possibly others such as PI. 1309 * @param store 1310 * A Quaternion to store our result in. If null, a new one is 1311 * created. 1312 * @return The store quaternion (or a new Quaterion, if store is null) that 1313 * describes a rotation that would point you in the exact opposite 1314 * direction of this Quaternion. 1315 */ opposite(Quaternion store)1316 public Quaternion opposite(Quaternion store) { 1317 if (store == null) { 1318 store = new Quaternion(); 1319 } 1320 1321 Vector3f axis = new Vector3f(); 1322 float angle = toAngleAxis(axis); 1323 1324 store.fromAngleAxis(FastMath.PI + angle, axis); 1325 return store; 1326 } 1327 1328 /** 1329 * @return This Quaternion, altered to describe a rotation that would point 1330 * you in the exact opposite direction of where it is pointing 1331 * currently. 1332 */ oppositeLocal()1333 public Quaternion oppositeLocal() { 1334 return opposite(this); 1335 } 1336 1337 @Override clone()1338 public Quaternion clone() { 1339 try { 1340 return (Quaternion) super.clone(); 1341 } catch (CloneNotSupportedException e) { 1342 throw new AssertionError(); // can not happen 1343 } 1344 } 1345 } 1346