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