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 { 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 */ 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 */ 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 103 public float getX() { 104 return x; 105 } 106 107 public float getY() { 108 return y; 109 } 110 111 public float getZ() { 112 return z; 113 } 114 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 */ 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 */ 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 */ 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 */ 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 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 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 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 */ 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 */ 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 */ 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 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 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 */ 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 */ 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 */ 1333 public Quaternion oppositeLocal() { 1334 return opposite(this); 1335 } 1336 1337 @Override 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
