1 module vector_3d; 2 3 import Math = math; 4 5 import vector_2i; 6 import vector_2d; 7 import vector_3i; 8 import vector_3d; 9 10 import matrix_4d; 11 import matrix_3d; 12 import matrix_3x2d; 13 import matrix_4x3d; 14 15 import axis_angle_4d; 16 import quaternion_d; 17 /* 18 * The MIT License 19 * 20 * Copyright (c) 2015-2021 Richard Greenlees 21 @#$%# Translated by jordan4ibanez 22 * 23 * Permission is hereby granted, free of charge, to any person obtaining a copy 24 * of this software and associated documentation files (the "Software"), to deal 25 * in the Software without restriction, including without limitation the rights 26 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 27 * copies of the Software, and to permit persons to whom the Software is 28 * furnished to do so, subject to the following conditions: 29 * 30 * The above copyright notice and this permission notice shall be included in 31 * all copies or substantial portions of the Software. 32 * 33 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 34 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 35 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 36 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 37 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 38 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 39 * THE SOFTWARE. 40 */ 41 42 /** 43 * Contains the definition of a Vector comprising 3 doubles and associated 44 * transformations. 45 * 46 * @author Richard Greenlees 47 * @author Kai Burjack 48 * @author F. Neurath 49 */ 50 struct Vector3d { 51 52 /** 53 * The x component of the vector. 54 */ 55 public double x = 0.0; 56 /** 57 * The y component of the vector. 58 */ 59 public double y = 0.0; 60 /** 61 * The z component of the vector. 62 */ 63 public double z = 0.0; 64 65 /** 66 * Create a new {@link Vector3d} and initialize all three components with the given value. 67 * 68 * @param d 69 * the value of all three components 70 */ 71 this(double d) { 72 this.x = d; 73 this.y = d; 74 this.z = d; 75 } 76 77 /** 78 * Create a new {@link Vector3d} with the given component values. 79 * 80 * @param x 81 * the value of x 82 * @param y 83 * the value of y 84 * @param z 85 * the value of z 86 */ 87 this(double x, double y, double z) { 88 this.x = x; 89 this.y = y; 90 this.z = z; 91 } 92 93 94 /** 95 * Create a new {@link Vector3d} whose values will be copied from the given vector. 96 * 97 * @param v 98 * provides the initial values for the new vector 99 */ 100 this(Vector3i v) { 101 this.x = v.x; 102 this.y = v.y; 103 this.z = v.z; 104 } 105 106 /** 107 * Create a new {@link Vector3d} with the first two components from the 108 * given <code>v</code> and the given <code>z</code> 109 * 110 * @param v 111 * the {@link Vector2i} to copy the values from 112 * @param z 113 * the z component 114 */ 115 this(Vector2i v, double z) { 116 this.x = v.x; 117 this.y = v.y; 118 this.z = z; 119 } 120 121 /** 122 * Create a new {@link Vector3d} whose values will be copied from the given vector. 123 * 124 * @param v 125 * provides the initial values for the new vector 126 */ 127 this(Vector3d v) { 128 this.x = v.x; 129 this.y = v.y; 130 this.z = v.z; 131 } 132 133 /** 134 * Create a new {@link Vector3d} with the first two components from the 135 * given <code>v</code> and the given <code>z</code> 136 * 137 * @param v 138 * the {@link Vector2d} to copy the values from 139 * @param z 140 * the z component 141 */ 142 this(Vector2d v, double z) { 143 this.x = v.x; 144 this.y = v.y; 145 this.z = z; 146 } 147 148 /** 149 * Create a new {@link Vector3d} and initialize its three components from the first 150 * three elements of the given array. 151 * 152 * @param xyz 153 * the array containing at least three elements 154 */ 155 this(double[] xyz) { 156 this.x = xyz[0]; 157 this.y = xyz[1]; 158 this.z = xyz[2]; 159 } 160 161 /** 162 * Create a new {@link Vector3d} and initialize its three components from the first 163 * three elements of the given array. 164 * 165 * @param xyz 166 * the array containing at least three elements 167 */ 168 this(float[] xyz) { 169 this.x = xyz[0]; 170 this.y = xyz[1]; 171 this.z = xyz[2]; 172 } 173 174 /** 175 * Set the x, y and z components to match the supplied vector. 176 * 177 * @param v 178 * the vector to set this vector's components from 179 * @return this 180 */ 181 ref public Vector3d set(Vector3d v) return { 182 this.x = v.x; 183 this.y = v.y; 184 this.z = v.z; 185 return this; 186 } 187 188 /** 189 * Set the x, y and z components to match the supplied vector. 190 * 191 * @param v 192 * the vector to set this vector's components from 193 * @return this 194 */ 195 ref public Vector3d set(Vector3i v) return { 196 this.x = v.x; 197 this.y = v.y; 198 this.z = v.z; 199 return this; 200 } 201 202 /** 203 * Set the first two components from the given <code>v</code> 204 * and the z component from the given <code>z</code> 205 * 206 * @param v 207 * the {@link Vector2d} to copy the values from 208 * @param z 209 * the z component 210 * @return this 211 */ 212 ref public Vector3d set(Vector2d v, double z) return { 213 this.x = v.x; 214 this.y = v.y; 215 this.z = z; 216 return this; 217 } 218 219 /** 220 * Set the first two components from the given <code>v</code> 221 * and the z component from the given <code>z</code> 222 * 223 * @param v 224 * the {@link Vector2i} to copy the values from 225 * @param z 226 * the z component 227 * @return this 228 */ 229 ref public Vector3d set(Vector2i v, double z) return { 230 this.x = v.x; 231 this.y = v.y; 232 this.z = z; 233 return this; 234 } 235 236 /** 237 * Set the x, y, and z components to the supplied value. 238 * 239 * @param d 240 * the value of all three components 241 * @return this 242 */ 243 ref public Vector3d set(double d) return { 244 this.x = d; 245 this.y = d; 246 this.z = d; 247 return this; 248 } 249 250 /** 251 * Set the x, y and z components to the supplied values. 252 * 253 * @param x 254 * the x component 255 * @param y 256 * the y component 257 * @param z 258 * the z component 259 * @return this 260 */ 261 ref public Vector3d set(double x, double y, double z) return { 262 this.x = x; 263 this.y = y; 264 this.z = z; 265 return this; 266 } 267 268 /** 269 * Set the three components of this vector to the first three elements of the given array. 270 * 271 * @param xyz 272 * the array containing at least three elements 273 * @return this 274 */ 275 ref public Vector3d set(double[] xyz) return { 276 this.x = xyz[0]; 277 this.y = xyz[1]; 278 this.z = xyz[2]; 279 return this; 280 } 281 282 /** 283 * Set the three components of this vector to the first three elements of the given array. 284 * 285 * @param xyz 286 * the array containing at least three elements 287 * @return this 288 */ 289 ref public Vector3d set(float[] xyz) return { 290 this.x = xyz[0]; 291 this.y = xyz[1]; 292 this.z = xyz[2]; 293 return this; 294 } 295 296 /** 297 * Set the value of the specified component of this vector. 298 * 299 * @param component 300 * the component whose value to set, within <code>[0..2]</code> 301 * @param value 302 * the value to set 303 * @return this 304 * @throws IllegalArgumentException if <code>component</code> is not within <code>[0..2]</code> 305 */ 306 ref public Vector3d setComponent(int component, double value) return { 307 switch (component) { 308 case 0: 309 x = value; 310 break; 311 case 1: 312 y = value; 313 break; 314 case 2: 315 z = value; 316 break; 317 default: {} 318 } 319 return this; 320 } 321 322 /** 323 * Subtract the supplied vector from this one. 324 * 325 * @param v 326 * the vector to subtract from this 327 * @return this 328 */ 329 ref public Vector3d sub(Vector3d v) return { 330 this.x = x - v.x; 331 this.y = y - v.y; 332 this.z = z - v.z; 333 return this; 334 } 335 336 public Vector3d sub(Vector3d v, ref Vector3d dest) { 337 dest.x = x - v.x; 338 dest.y = y - v.y; 339 dest.z = z - v.z; 340 return dest; 341 } 342 343 344 /** 345 * Subtract <code>(x, y, z)</code> from this vector. 346 * 347 * @param x 348 * the x component to subtract 349 * @param y 350 * the y component to subtract 351 * @param z 352 * the z component to subtract 353 * @return this 354 */ 355 ref public Vector3d sub(double x, double y, double z) return { 356 this.x = this.x - x; 357 this.y = this.y - y; 358 this.z = this.z - z; 359 return this; 360 } 361 362 public Vector3d sub(double x, double y, double z, ref Vector3d dest) { 363 dest.x = this.x - x; 364 dest.y = this.y - y; 365 dest.z = this.z - z; 366 return dest; 367 } 368 369 /** 370 * Add the supplied vector to this one. 371 * 372 * @param v 373 * the vector to add 374 * @return this 375 */ 376 ref public Vector3d add(Vector3d v) return { 377 this.x = x + v.x; 378 this.y = y + v.y; 379 this.z = z + v.z; 380 return this; 381 } 382 383 public Vector3d add(Vector3d v, ref Vector3d dest) { 384 dest.x = x + v.x; 385 dest.y = y + v.y; 386 dest.z = z + v.z; 387 return dest; 388 } 389 390 391 /** 392 * Increment the components of this vector by the given values. 393 * 394 * @param x 395 * the x component to add 396 * @param y 397 * the y component to add 398 * @param z 399 * the z component to add 400 * @return this 401 */ 402 ref public Vector3d add(double x, double y, double z) return { 403 this.x = this.x + x; 404 this.y = this.y + y; 405 this.z = this.z + z; 406 return this; 407 } 408 409 public Vector3d add(double x, double y, double z, ref Vector3d dest) { 410 dest.x = this.x + x; 411 dest.y = this.y + y; 412 dest.z = this.z + z; 413 return dest; 414 } 415 416 /** 417 * Add the component-wise multiplication of <code>a * b</code> to this vector. 418 * 419 * @param a 420 * the first multiplicand 421 * @param b 422 * the second multiplicand 423 * @return this 424 */ 425 ref public Vector3d fma(Vector3d a, Vector3d b) return { 426 this.x = Math.fma(a.x, b.x, x); 427 this.y = Math.fma(a.y, b.y, y); 428 this.z = Math.fma(a.z, b.z, z); 429 return this; 430 } 431 432 /** 433 * Add the component-wise multiplication of <code>a * b</code> to this vector. 434 * 435 * @param a 436 * the first multiplicand 437 * @param b 438 * the second multiplicand 439 * @return this 440 */ 441 ref public Vector3d fma(double a, Vector3d b) return { 442 this.x = Math.fma(a, b.x, x); 443 this.y = Math.fma(a, b.y, y); 444 this.z = Math.fma(a, b.z, z); 445 return this; 446 } 447 448 public Vector3d fma(Vector3d a, Vector3d b, ref Vector3d dest) { 449 dest.x = Math.fma(a.x, b.x, x); 450 dest.y = Math.fma(a.y, b.y, y); 451 dest.z = Math.fma(a.z, b.z, z); 452 return dest; 453 } 454 455 public Vector3d fma(double a, Vector3d b, ref Vector3d dest) { 456 dest.x = Math.fma(a, b.x, x); 457 dest.y = Math.fma(a, b.y, y); 458 dest.z = Math.fma(a, b.z, z); 459 return dest; 460 } 461 462 /** 463 * Add the component-wise multiplication of <code>this * a</code> to <code>b</code> 464 * and store the result in <code>this</code>. 465 * 466 * @param a 467 * the multiplicand 468 * @param b 469 * the addend 470 * @return this 471 */ 472 ref public Vector3d mulAdd(Vector3d a, Vector3d b) return { 473 this.x = Math.fma(x, a.x, b.x); 474 this.y = Math.fma(y, a.y, b.y); 475 this.z = Math.fma(z, a.z, b.z); 476 return this; 477 } 478 479 /** 480 * Add the component-wise multiplication of <code>this * a</code> to <code>b</code> 481 * and store the result in <code>this</code>. 482 * 483 * @param a 484 * the multiplicand 485 * @param b 486 * the addend 487 * @return this 488 */ 489 ref public Vector3d mulAdd(double a, Vector3d b) return { 490 this.x = Math.fma(x, a, b.x); 491 this.y = Math.fma(y, a, b.y); 492 this.z = Math.fma(z, a, b.z); 493 return this; 494 } 495 496 public Vector3d mulAdd(Vector3d a, Vector3d b, ref Vector3d dest) { 497 dest.x = Math.fma(x, a.x, b.x); 498 dest.y = Math.fma(y, a.y, b.y); 499 dest.z = Math.fma(z, a.z, b.z); 500 return dest; 501 } 502 503 public Vector3d mulAdd(double a, Vector3d b, ref Vector3d dest) { 504 dest.x = Math.fma(x, a, b.x); 505 dest.y = Math.fma(y, a, b.y); 506 dest.z = Math.fma(z, a, b.z); 507 return dest; 508 } 509 510 /** 511 * Multiply this Vector3d component-wise by another Vector3d. 512 * 513 * @param v 514 * the vector to multiply by 515 * @return this 516 */ 517 ref public Vector3d mul(Vector3d v) return { 518 this.x = x * v.x; 519 this.y = y * v.y; 520 this.z = z * v.z; 521 return this; 522 } 523 524 public Vector3d mul(Vector3d v, ref Vector3d dest) { 525 dest.x = x * v.x; 526 dest.y = y * v.y; 527 dest.z = z * v.z; 528 return dest; 529 } 530 531 /** 532 * Divide this Vector3d component-wise by another Vector3d. 533 * 534 * @param v 535 * the vector to divide by 536 * @return this 537 */ 538 ref public Vector3d div(Vector3d v) return { 539 this.x = x / v.x; 540 this.y = y / v.y; 541 this.z = z / v.z; 542 return this; 543 } 544 545 546 public Vector3d div(Vector3d v, ref Vector3d dest) { 547 dest.x = x / v.x; 548 dest.y = y / v.y; 549 dest.z = z / v.z; 550 return dest; 551 } 552 553 public Vector3d mulProject(Matrix4d mat, double w, ref Vector3d dest) { 554 double invW = 1.0 / Math.fma(mat.m03, x, Math.fma(mat.m13, y, Math.fma(mat.m23, z, mat.m33 * w))); 555 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30 * w))) * invW; 556 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31 * w))) * invW; 557 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32 * w))) * invW; 558 dest.x = rx; 559 dest.y = ry; 560 dest.z = rz; 561 return dest; 562 } 563 564 public Vector3d mulProject(Matrix4d mat, ref Vector3d dest) { 565 double invW = 1.0 / Math.fma(mat.m03, x, Math.fma(mat.m13, y, Math.fma(mat.m23, z, mat.m33))); 566 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))) * invW; 567 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))) * invW; 568 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))) * invW; 569 dest.x = rx; 570 dest.y = ry; 571 dest.z = rz; 572 return dest; 573 } 574 575 /** 576 * Multiply the given matrix <code>mat</code> this Vector3d, perform perspective division. 577 * <p> 578 * This method uses <code>w=1.0</code> as the fourth vector component. 579 * 580 * @param mat 581 * the matrix to multiply this vector by 582 * @return this 583 */ 584 ref public Vector3d mulProject(Matrix4d mat) return { 585 double invW = 1.0 / Math.fma(mat.m03, x, Math.fma(mat.m13, y, Math.fma(mat.m23, z, mat.m33))); 586 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))) * invW; 587 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))) * invW; 588 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))) * invW; 589 this.x = rx; 590 this.y = ry; 591 this.z = rz; 592 return this; 593 } 594 595 596 /** 597 * Multiply the given matrix <code>mat</code> with this Vector3d. 598 * 599 * @param mat 600 * the matrix to multiply this vector by 601 * @return this 602 */ 603 ref public Vector3d mul(Matrix3d mat) return { 604 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 605 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 606 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 607 this.x = rx; 608 this.y = ry; 609 this.z = rz; 610 return this; 611 } 612 613 public Vector3d mul(Matrix3d mat, ref Vector3d dest) { 614 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 615 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 616 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 617 dest.x = rx; 618 dest.y = ry; 619 dest.z = rz; 620 return dest; 621 } 622 623 /** 624 * Multiply the given matrix with this Vector3d by assuming a third row in the matrix of <code>(0, 0, 1)</code> 625 * and store the result in <code>this</code>. 626 * 627 * @param mat 628 * the matrix 629 * @return this 630 */ 631 ref public Vector3d mul(Matrix3x2d mat) return { 632 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 633 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 634 this.x = rx; 635 this.y = ry; 636 return this; 637 } 638 639 public Vector3d mul(Matrix3x2d mat, ref Vector3d dest) { 640 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 641 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 642 dest.x = rx; 643 dest.y = ry; 644 dest.z = z; 645 return dest; 646 } 647 648 649 /** 650 * Multiply the transpose of the given matrix with this Vector3d and store the result in <code>this</code>. 651 * 652 * @param mat 653 * the matrix 654 * @return this 655 */ 656 ref public Vector3d mulTranspose(Matrix3d mat) return { 657 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, mat.m02 * z)); 658 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, mat.m12 * z)); 659 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, mat.m22 * z)); 660 this.x = rx; 661 this.y = ry; 662 this.z = rz; 663 return this; 664 } 665 666 public Vector3d mulTranspose(Matrix3d mat, ref Vector3d dest) { 667 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, mat.m02 * z)); 668 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, mat.m12 * z)); 669 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, mat.m22 * z)); 670 dest.x = rx; 671 dest.y = ry; 672 dest.z = rz; 673 return dest; 674 } 675 676 /** 677 * Multiply the given 4x4 matrix <code>mat</code> with <code>this</code>. 678 * <p> 679 * This method assumes the <code>w</code> component of <code>this</code> to be <code>1.0</code>. 680 * 681 * @param mat 682 * the matrix to multiply this vector by 683 * @return this 684 */ 685 ref public Vector3d mulPosition(Matrix4d mat) return { 686 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 687 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 688 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 689 this.x = rx; 690 this.y = ry; 691 this.z = rz; 692 return this; 693 } 694 695 /** 696 * Multiply the given 4x3 matrix <code>mat</code> with <code>this</code>. 697 * <p> 698 * This method assumes the <code>w</code> component of <code>this</code> to be <code>1.0</code>. 699 * 700 * @param mat 701 * the matrix to multiply this vector by 702 * @return this 703 */ 704 ref public Vector3d mulPosition(Matrix4x3d mat) return { 705 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 706 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 707 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 708 this.x = rx; 709 this.y = ry; 710 this.z = rz; 711 return this; 712 } 713 714 715 public Vector3d mulPosition(Matrix4d mat, ref Vector3d dest) { 716 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 717 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 718 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 719 dest.x = rx; 720 dest.y = ry; 721 dest.z = rz; 722 return dest; 723 } 724 725 public Vector3d mulPosition(Matrix4x3d mat, ref Vector3d dest) { 726 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 727 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 728 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 729 dest.x = rx; 730 dest.y = ry; 731 dest.z = rz; 732 return dest; 733 } 734 735 /** 736 * Multiply the transpose of the given 4x4 matrix <code>mat</code> with <code>this</code>. 737 * <p> 738 * This method assumes the <code>w</code> component of <code>this</code> to be <code>1.0</code>. 739 * 740 * @param mat 741 * the matrix whose transpose to multiply this vector by 742 * @return this 743 */ 744 ref public Vector3d mulTransposePosition(Matrix4d mat) return { 745 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, Math.fma(mat.m02, z, mat.m03))); 746 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, Math.fma(mat.m12, z, mat.m13))); 747 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, Math.fma(mat.m22, z, mat.m23))); 748 this.x = rx; 749 this.y = ry; 750 this.z = rz; 751 return this; 752 } 753 754 public Vector3d mulTransposePosition(Matrix4d mat, ref Vector3d dest) { 755 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, Math.fma(mat.m02, z, mat.m03))); 756 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, Math.fma(mat.m12, z, mat.m13))); 757 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, Math.fma(mat.m22, z, mat.m23))); 758 dest.x = rx; 759 dest.y = ry; 760 dest.z = rz; 761 return dest; 762 } 763 764 765 766 767 /** 768 * Multiply the given 4x4 matrix <code>mat</code> with <code>this</code> and return the <i>w</i> component 769 * of the resulting 4D vector. 770 * <p> 771 * This method assumes the <code>w</code> component of <code>this</code> to be <code>1.0</code>. 772 * 773 * @param mat 774 * the matrix to multiply this vector by 775 * @return the <i>w</i> component of the resulting 4D vector after multiplication 776 */ 777 public double mulPositionW(Matrix4d mat) { 778 double w = Math.fma(mat.m03, x, Math.fma(mat.m13, y, Math.fma(mat.m23, z, mat.m33))); 779 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 780 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 781 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 782 this.x = rx; 783 this.y = ry; 784 this.z = rz; 785 return w; 786 } 787 788 public double mulPositionW(Matrix4d mat, ref Vector3d dest) { 789 double w = Math.fma(mat.m03, x, Math.fma(mat.m13, y, Math.fma(mat.m23, z, mat.m33))); 790 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, Math.fma(mat.m20, z, mat.m30))); 791 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, Math.fma(mat.m21, z, mat.m31))); 792 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, Math.fma(mat.m22, z, mat.m32))); 793 dest.x = rx; 794 dest.y = ry; 795 dest.z = rz; 796 return w; 797 } 798 799 800 /** 801 * Multiply the given 4x4 matrix <code>mat</code> with <code>this</code>. 802 * <p> 803 * This method assumes the <code>w</code> component of <code>this</code> to be <code>0.0</code>. 804 * 805 * @param mat 806 * the matrix to multiply this vector by 807 * @return this 808 */ 809 ref public Vector3d mulDirection(Matrix4d mat) return { 810 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 811 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 812 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 813 this.x = rx; 814 this.y = ry; 815 this.z = rz; 816 return this; 817 } 818 819 /** 820 * Multiply the given 4x3 matrix <code>mat</code> with <code>this</code>. 821 * <p> 822 * This method assumes the <code>w</code> component of <code>this</code> to be <code>0.0</code>. 823 * 824 * @param mat 825 * the matrix to multiply this vector by 826 * @return this 827 */ 828 ref public Vector3d mulDirection(Matrix4x3d mat) return { 829 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 830 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 831 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 832 this.x = rx; 833 this.y = ry; 834 this.z = rz; 835 return this; 836 } 837 838 839 public Vector3d mulDirection(Matrix4d mat, Vector3d dest) { 840 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 841 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 842 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 843 dest.x = rx; 844 dest.y = ry; 845 dest.z = rz; 846 return dest; 847 } 848 849 public Vector3d mulDirection(Matrix4x3d mat, ref Vector3d dest) { 850 double rx = Math.fma(mat.m00, x, Math.fma(mat.m10, y, mat.m20 * z)); 851 double ry = Math.fma(mat.m01, x, Math.fma(mat.m11, y, mat.m21 * z)); 852 double rz = Math.fma(mat.m02, x, Math.fma(mat.m12, y, mat.m22 * z)); 853 dest.x = rx; 854 dest.y = ry; 855 dest.z = rz; 856 return dest; 857 } 858 859 860 /** 861 * Multiply the transpose of the given 4x4 matrix <code>mat</code> with <code>this</code>. 862 * <p> 863 * This method assumes the <code>w</code> component of <code>this</code> to be <code>0.0</code>. 864 * 865 * @param mat 866 * the matrix whose transpose to multiply this vector by 867 * @return this 868 */ 869 ref public Vector3d mulTransposeDirection(Matrix4d mat) return { 870 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, mat.m02 * z)); 871 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, mat.m12 * z)); 872 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, mat.m22 * z)); 873 this.x = rx; 874 this.y = ry; 875 this.z = rz; 876 return this; 877 } 878 879 public Vector3d mulTransposeDirection(Matrix4d mat, ref Vector3d dest) { 880 double rx = Math.fma(mat.m00, x, Math.fma(mat.m01, y, mat.m02 * z)); 881 double ry = Math.fma(mat.m10, x, Math.fma(mat.m11, y, mat.m12 * z)); 882 double rz = Math.fma(mat.m20, x, Math.fma(mat.m21, y, mat.m22 * z)); 883 dest.x = rx; 884 dest.y = ry; 885 dest.z = rz; 886 return dest; 887 } 888 889 /** 890 * Multiply this Vector3d by the given scalar value. 891 * 892 * @param scalar 893 * the scalar to multiply this vector by 894 * @return this 895 */ 896 ref public Vector3d mul(double scalar) return { 897 this.x = x * scalar; 898 this.y = y * scalar; 899 this.z = z * scalar; 900 return this; 901 } 902 903 public Vector3d mul(double scalar, ref Vector3d dest) { 904 dest.x = x * scalar; 905 dest.y = y * scalar; 906 dest.z = z * scalar; 907 return dest; 908 } 909 910 /** 911 * Multiply the components of this Vector3d by the given scalar values and store the result in <code>this</code>. 912 * 913 * @param x 914 * the x component to multiply this vector by 915 * @param y 916 * the y component to multiply this vector by 917 * @param z 918 * the z component to multiply this vector by 919 * @return this 920 */ 921 ref public Vector3d mul(double x, double y, double z) return { 922 this.x = this.x * x; 923 this.y = this.y * y; 924 this.z = this.z * z; 925 return this; 926 } 927 928 public Vector3d mul(double x, double y, double z, ref Vector3d dest) { 929 dest.x = this.x * x; 930 dest.y = this.y * y; 931 dest.z = this.z * z; 932 return dest; 933 } 934 935 /** 936 * Rotate this vector by the given quaternion <code>quat</code> and store the result in <code>this</code>. 937 * 938 * @see Quaterniond#transform(Vector3d) 939 * 940 * @param quat 941 * the quaternion to rotate this vector 942 * @return this 943 */ 944 ref public Vector3d rotate(Quaterniond quat) return { 945 quat.transform(this, this); 946 return this; 947 } 948 949 public Vector3d rotate(Quaterniond quat, ref Vector3d dest) { 950 return quat.transform(this, dest); 951 } 952 953 public Quaterniond rotationTo(Vector3d toDir, ref Quaterniond dest) { 954 return dest.rotationTo(this, toDir); 955 } 956 957 public Quaterniond rotationTo(double toDirX, double toDirY, double toDirZ, ref Quaterniond dest) { 958 return dest.rotationTo(x, y, z, toDirX, toDirY, toDirZ); 959 } 960 961 /** 962 * Rotate this vector the specified radians around the given rotation axis. 963 * 964 * @param angle 965 * the angle in radians 966 * @param x 967 * the x component of the rotation axis 968 * @param y 969 * the y component of the rotation axis 970 * @param z 971 * the z component of the rotation axis 972 * @return this 973 */ 974 ref public Vector3d rotateAxis(double angle, double x, double y, double z) return { 975 if (y == 0.0 && z == 0.0 && Math.absEqualsOne(x)) 976 rotateX(x * angle, this); 977 else if (x == 0.0 && z == 0.0 && Math.absEqualsOne(y)) 978 rotateY(y * angle, this); 979 else if (x == 0.0 && y == 0.0 && Math.absEqualsOne(z)) 980 rotateZ(z * angle, this); 981 else 982 rotateAxisInternal(angle, x, y, z, this); 983 984 return this; 985 } 986 987 public Vector3d rotateAxis(double angle, double aX, double aY, double aZ, ref Vector3d dest) { 988 if (aY == 0.0 && aZ == 0.0 && Math.absEqualsOne(aX)) 989 return rotateX(aX * angle, dest); 990 else if (aX == 0.0 && aZ == 0.0 && Math.absEqualsOne(aY)) 991 return rotateY(aY * angle, dest); 992 else if (aX == 0.0 && aY == 0.0 && Math.absEqualsOne(aZ)) 993 return rotateZ(aZ * angle, dest); 994 return rotateAxisInternal(angle, aX, aY, aZ, dest); 995 } 996 997 private Vector3d rotateAxisInternal(double angle, double aX, double aY, double aZ, ref Vector3d dest) { 998 double hangle = angle * 0.5; 999 double sinAngle = Math.sin(hangle); 1000 double qx = aX * sinAngle, qy = aY * sinAngle, qz = aZ * sinAngle; 1001 double qw = Math.cosFromSin(sinAngle, hangle); 1002 double w2 = qw * qw, x2 = qx * qx, y2 = qy * qy, z2 = qz * qz, zw = qz * qw; 1003 double xy = qx * qy, xz = qx * qz, yw = qy * qw, yz = qy * qz, xw = qx * qw; 1004 double nx = (w2 + x2 - z2 - y2) * x + (-zw + xy - zw + xy) * y + (yw + xz + xz + yw) * z; 1005 double ny = (xy + zw + zw + xy) * x + ( y2 - z2 + w2 - x2) * y + (yz + yz - xw - xw) * z; 1006 double nz = (xz - yw + xz - yw) * x + ( yz + yz + xw + xw) * y + (z2 - y2 - x2 + w2) * z; 1007 dest.x = nx; 1008 dest.y = ny; 1009 dest.z = nz; 1010 return dest; 1011 } 1012 1013 /** 1014 * Rotate this vector the specified radians around the X axis. 1015 * 1016 * @param angle 1017 * the angle in radians 1018 * @return this 1019 */ 1020 ref public Vector3d rotateX(double angle) return { 1021 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1022 double y = this.y * cos - this.z * sin; 1023 double z = this.y * sin + this.z * cos; 1024 this.y = y; 1025 this.z = z; 1026 return this; 1027 } 1028 1029 public Vector3d rotateX(double angle, ref Vector3d dest) { 1030 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1031 double y = this.y * cos - this.z * sin; 1032 double z = this.y * sin + this.z * cos; 1033 dest.x = this.x; 1034 dest.y = y; 1035 dest.z = z; 1036 return dest; 1037 } 1038 1039 /** 1040 * Rotate this vector the specified radians around the Y axis. 1041 * 1042 * @param angle 1043 * the angle in radians 1044 * @return this 1045 */ 1046 ref public Vector3d rotateY(double angle) return { 1047 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1048 double x = this.x * cos + this.z * sin; 1049 double z = -this.x * sin + this.z * cos; 1050 this.x = x; 1051 this.z = z; 1052 return this; 1053 } 1054 1055 public Vector3d rotateY(double angle, ref Vector3d dest) { 1056 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1057 double x = this.x * cos + this.z * sin; 1058 double z = -this.x * sin + this.z * cos; 1059 dest.x = x; 1060 dest.y = this.y; 1061 dest.z = z; 1062 return dest; 1063 } 1064 1065 /** 1066 * Rotate this vector the specified radians around the Z axis. 1067 * 1068 * @param angle 1069 * the angle in radians 1070 * @return this 1071 */ 1072 ref public Vector3d rotateZ(double angle) return { 1073 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1074 double x = this.x * cos - this.y * sin; 1075 double y = this.x * sin + this.y * cos; 1076 this.x = x; 1077 this.y = y; 1078 return this; 1079 } 1080 1081 public Vector3d rotateZ(double angle, ref Vector3d dest) { 1082 double sin = Math.sin(angle), cos = Math.cosFromSin(sin, angle); 1083 double x = this.x * cos - this.y * sin; 1084 double y = this.x * sin + this.y * cos; 1085 dest.x = x; 1086 dest.y = y; 1087 dest.z = this.z; 1088 return dest; 1089 } 1090 1091 /** 1092 * Divide this Vector3d by the given scalar value. 1093 * 1094 * @param scalar 1095 * the scalar to divide this vector by 1096 * @return this 1097 */ 1098 ref public Vector3d div(double scalar) return { 1099 double inv = 1.0 / scalar; 1100 this.x = x * inv; 1101 this.y = y * inv; 1102 this.z = z * inv; 1103 return this; 1104 } 1105 1106 public Vector3d div(double scalar, ref Vector3d dest) { 1107 double inv = 1.0 / scalar; 1108 dest.x = x * inv; 1109 dest.y = y * inv; 1110 dest.z = z * inv; 1111 return dest; 1112 } 1113 1114 /** 1115 * Divide the components of this Vector3d by the given scalar values and store the result in <code>this</code>. 1116 * 1117 * @param x 1118 * the x component to divide this vector by 1119 * @param y 1120 * the y component to divide this vector by 1121 * @param z 1122 * the z component to divide this vector by 1123 * @return this 1124 */ 1125 ref public Vector3d div(double x, double y, double z) return { 1126 this.x = this.x / x; 1127 this.y = this.y / y; 1128 this.z = this.z / z; 1129 return this; 1130 } 1131 1132 public Vector3d div(double x, double y, double z, ref Vector3d dest) { 1133 dest.x = this.x / x; 1134 dest.y = this.y / y; 1135 dest.z = this.z / z; 1136 return dest; 1137 } 1138 1139 public double lengthSquared() { 1140 return Math.fma(x, x, Math.fma(y, y, z * z)); 1141 } 1142 1143 /** 1144 * Get the length squared of a 3-dimensional double-precision vector. 1145 * 1146 * @param x The vector's x component 1147 * @param y The vector's y component 1148 * @param z The vector's z component 1149 * 1150 * @return the length squared of the given vector 1151 * 1152 * @author F. Neurath 1153 */ 1154 public static double lengthSquared(double x, double y, double z) { 1155 return Math.fma(x, x, Math.fma(y, y, z * z)); 1156 } 1157 1158 public double length() { 1159 return Math.sqrt(Math.fma(x, x, Math.fma(y, y, z * z))); 1160 } 1161 1162 /** 1163 * Get the length of a 3-dimensional double-precision vector. 1164 * 1165 * @param x The vector's x component 1166 * @param y The vector's y component 1167 * @param z The vector's z component 1168 * 1169 * @return the length of the given vector 1170 * 1171 * @author F. Neurath 1172 */ 1173 public static double length(double x, double y, double z) { 1174 return Math.sqrt(Math.fma(x, x, Math.fma(y, y, z * z))); 1175 } 1176 1177 /** 1178 * Normalize this vector. 1179 * 1180 * @return this 1181 */ 1182 ref public Vector3d normalize() return { 1183 double invLength = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, z * z))); 1184 this.x = x * invLength; 1185 this.y = y * invLength; 1186 this.z = z * invLength; 1187 return this; 1188 } 1189 1190 public Vector3d normalize(ref Vector3d dest) { 1191 double invLength = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, z * z))); 1192 dest.x = x * invLength; 1193 dest.y = y * invLength; 1194 dest.z = z * invLength; 1195 return dest; 1196 } 1197 1198 /** 1199 * Scale this vector to have the given length. 1200 * 1201 * @param length 1202 * the desired length 1203 * @return this 1204 */ 1205 ref public Vector3d normalize(double length) return { 1206 double invLength = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, z * z))) * length; 1207 this.x = x * invLength; 1208 this.y = y * invLength; 1209 this.z = z * invLength; 1210 return this; 1211 } 1212 1213 public Vector3d normalize(double length, ref Vector3d dest) { 1214 double invLength = Math.invsqrt(Math.fma(x, x, Math.fma(y, y, z * z))) * length; 1215 dest.x = x * invLength; 1216 dest.y = y * invLength; 1217 dest.z = z * invLength; 1218 return dest; 1219 } 1220 1221 /** 1222 * Set this vector to be the cross product of this and v2. 1223 * 1224 * @param v 1225 * the other vector 1226 * @return this 1227 */ 1228 ref public Vector3d cross(Vector3d v) return { 1229 double rx = Math.fma(y, v.z, -z * v.y); 1230 double ry = Math.fma(z, v.x, -x * v.z); 1231 double rz = Math.fma(x, v.y, -y * v.x); 1232 this.x = rx; 1233 this.y = ry; 1234 this.z = rz; 1235 return this; 1236 } 1237 1238 /** 1239 * Set this vector to be the cross product of itself and <code>(x, y, z)</code>. 1240 * 1241 * @param x 1242 * the x component of the other vector 1243 * @param y 1244 * the y component of the other vector 1245 * @param z 1246 * the z component of the other vector 1247 * @return this 1248 */ 1249 ref public Vector3d cross(double x, double y, double z) return { 1250 double rx = Math.fma(this.y, z, -this.z * y); 1251 double ry = Math.fma(this.z, x, -this.x * z); 1252 double rz = Math.fma(this.x, y, -this.y * x); 1253 this.x = rx; 1254 this.y = ry; 1255 this.z = rz; 1256 return this; 1257 } 1258 1259 public Vector3d cross(Vector3d v, ref Vector3d dest) { 1260 double rx = Math.fma(y, v.z, -z * v.y); 1261 double ry = Math.fma(z, v.x, -x * v.z); 1262 double rz = Math.fma(x, v.y, -y * v.x); 1263 dest.x = rx; 1264 dest.y = ry; 1265 dest.z = rz; 1266 return dest; 1267 } 1268 1269 public Vector3d cross(double x, double y, double z, ref Vector3d dest) { 1270 double rx = Math.fma(this.y, z, -this.z * y); 1271 double ry = Math.fma(this.z, x, -this.x * z); 1272 double rz = Math.fma(this.x, y, -this.y * x); 1273 dest.x = rx; 1274 dest.y = ry; 1275 dest.z = rz; 1276 return dest; 1277 } 1278 1279 public double distance(Vector3d v) { 1280 double dx = this.x - v.x; 1281 double dy = this.y - v.y; 1282 double dz = this.z - v.z; 1283 return Math.sqrt(Math.fma(dx, dx, Math.fma(dy, dy, dz * dz))); 1284 } 1285 1286 public double distance(double x, double y, double z) { 1287 double dx = this.x - x; 1288 double dy = this.y - y; 1289 double dz = this.z - z; 1290 return Math.sqrt(Math.fma(dx, dx, Math.fma(dy, dy, dz * dz))); 1291 } 1292 1293 public double distanceSquared(Vector3d v) { 1294 double dx = this.x - v.x; 1295 double dy = this.y - v.y; 1296 double dz = this.z - v.z; 1297 return Math.fma(dx, dx, Math.fma(dy, dy, dz * dz)); 1298 } 1299 1300 public double distanceSquared(double x, double y, double z) { 1301 double dx = this.x - x; 1302 double dy = this.y - y; 1303 double dz = this.z - z; 1304 return Math.fma(dx, dx, Math.fma(dy, dy, dz * dz)); 1305 } 1306 1307 /** 1308 * Return the distance between <code>(x1, y1, z1)</code> and <code>(x2, y2, z2)</code>. 1309 * 1310 * @param x1 1311 * the x component of the first vector 1312 * @param y1 1313 * the y component of the first vector 1314 * @param z1 1315 * the z component of the first vector 1316 * @param x2 1317 * the x component of the second vector 1318 * @param y2 1319 * the y component of the second vector 1320 * @param z2 1321 * the z component of the second vector 1322 * @return the euclidean distance 1323 */ 1324 public static double distance(double x1, double y1, double z1, double x2, double y2, double z2) { 1325 return Math.sqrt(distanceSquared(x1, y1, z1, x2, y2, z2)); 1326 } 1327 1328 /** 1329 * Return the squared distance between <code>(x1, y1, z1)</code> and <code>(x2, y2, z2)</code>. 1330 * 1331 * @param x1 1332 * the x component of the first vector 1333 * @param y1 1334 * the y component of the first vector 1335 * @param z1 1336 * the z component of the first vector 1337 * @param x2 1338 * the x component of the second vector 1339 * @param y2 1340 * the y component of the second vector 1341 * @param z2 1342 * the z component of the second vector 1343 * @return the euclidean distance squared 1344 */ 1345 public static double distanceSquared(double x1, double y1, double z1, double x2, double y2, double z2) { 1346 double dx = x1 - x2; 1347 double dy = y1 - y2; 1348 double dz = z1 - z2; 1349 return Math.fma(dx, dx, Math.fma(dy, dy, dz * dz)); 1350 } 1351 1352 public double dot(Vector3d v) { 1353 return Math.fma(this.x, v.x, Math.fma(this.y, v.y, this.z * v.z)); 1354 } 1355 1356 public double dot(double x, double y, double z) { 1357 return Math.fma(this.x, x, Math.fma(this.y, y, this.z * z)); 1358 } 1359 1360 public double angleCos(Vector3d v) { 1361 double length1Squared = Math.fma(x, x, Math.fma(y, y, z * z)); 1362 double length2Squared = Math.fma(v.x, v.x, Math.fma(v.y, v.y, v.z * v.z)); 1363 double dot = Math.fma(x, v.x, Math.fma(y, v.y, z * v.z)); 1364 return dot / Math.sqrt(length1Squared * length2Squared); 1365 } 1366 1367 public double angle(Vector3d v) { 1368 double cos = angleCos(v); 1369 // This is because sometimes cos goes above 1 or below -1 because of lost precision 1370 cos = cos < 1 ? cos : 1; 1371 cos = cos > -1 ? cos : -1; 1372 return Math.acos(cos); 1373 } 1374 1375 public double angleSigned(Vector3d v, Vector3d n) { 1376 double x = v.x; 1377 double y = v.y; 1378 double z = v.z; 1379 return Math.atan2( 1380 (this.y * z - this.z * y) * n.x + (this.z * x - this.x * z) * n.y + (this.x * y - this.y * x) * n.z, 1381 this.x * x + this.y * y + this.z * z); 1382 } 1383 1384 public double angleSigned(double x, double y, double z, double nx, double ny, double nz) { 1385 return Math.atan2( 1386 (this.y * z - this.z * y) * nx + (this.z * x - this.x * z) * ny + (this.x * y - this.y * x) * nz, 1387 this.x * x + this.y * y + this.z * z); 1388 } 1389 1390 /** 1391 * Set the components of this vector to be the component-wise minimum of this and the other vector. 1392 * 1393 * @param v 1394 * the other vector 1395 * @return this 1396 */ 1397 ref public Vector3d min(Vector3d v) return { 1398 this.x = x < v.x ? x : v.x; 1399 this.y = y < v.y ? y : v.y; 1400 this.z = z < v.z ? z : v.z; 1401 return this; 1402 } 1403 1404 public Vector3d min(Vector3d v, ref Vector3d dest) { 1405 dest.x = x < v.x ? x : v.x; 1406 dest.y = y < v.y ? y : v.y; 1407 dest.z = z < v.z ? z : v.z; 1408 return dest; 1409 } 1410 1411 /** 1412 * Set the components of this vector to be the component-wise maximum of this and the other vector. 1413 * 1414 * @param v 1415 * the other vector 1416 * @return this 1417 */ 1418 ref public Vector3d max(Vector3d v) return { 1419 this.x = x > v.x ? x : v.x; 1420 this.y = y > v.y ? y : v.y; 1421 this.z = z > v.z ? z : v.z; 1422 return this; 1423 } 1424 1425 public Vector3d max(Vector3d v, ref Vector3d dest) { 1426 dest.x = x > v.x ? x : v.x; 1427 dest.y = y > v.y ? y : v.y; 1428 dest.z = z > v.z ? z : v.z; 1429 return dest; 1430 } 1431 1432 /** 1433 * Set all components to zero. 1434 * 1435 * @return this 1436 */ 1437 ref public Vector3d zero() return { 1438 this.x = 0; 1439 this.y = 0; 1440 this.z = 0; 1441 return this; 1442 } 1443 1444 /** 1445 * Negate this vector. 1446 * 1447 * @return this 1448 */ 1449 ref public Vector3d negate() return { 1450 this.x = -x; 1451 this.y = -y; 1452 this.z = -z; 1453 return this; 1454 } 1455 1456 public Vector3d negate(ref Vector3d dest) { 1457 dest.x = -x; 1458 dest.y = -y; 1459 dest.z = -z; 1460 return dest; 1461 } 1462 1463 /** 1464 * Set <code>this</code> vector's components to their respective absolute values. 1465 * 1466 * @return this 1467 */ 1468 ref public Vector3d absolute() return { 1469 this.x = Math.abs(this.x); 1470 this.y = Math.abs(this.y); 1471 this.z = Math.abs(this.z); 1472 return this; 1473 } 1474 1475 public Vector3d absolute(ref Vector3d dest) { 1476 dest.x = Math.abs(this.x); 1477 dest.y = Math.abs(this.y); 1478 dest.z = Math.abs(this.z); 1479 return dest; 1480 } 1481 1482 public int hashCode() { 1483 immutable int prime = 31; 1484 int result = 1; 1485 long temp; 1486 temp = Math.doubleToLongBits(x); 1487 result = prime * result + cast(int) (temp ^ (temp >>> 32)); 1488 temp = Math.doubleToLongBits(y); 1489 result = prime * result + cast(int) (temp ^ (temp >>> 32)); 1490 temp = Math.doubleToLongBits(z); 1491 result = prime * result + cast(int) (temp ^ (temp >>> 32)); 1492 return result; 1493 } 1494 1495 1496 public bool equals(Vector3d v, double delta) { 1497 if (this == v) 1498 return true; 1499 if (!Math.equals(x, v.x, delta)) 1500 return false; 1501 if (!Math.equals(y, v.y, delta)) 1502 return false; 1503 if (!Math.equals(z, v.z, delta)) 1504 return false; 1505 return true; 1506 } 1507 1508 public bool equals(double x, double y, double z) { 1509 if (Math.doubleToLongBits(this.x) != Math.doubleToLongBits(x)) 1510 return false; 1511 if (Math.doubleToLongBits(this.y) != Math.doubleToLongBits(y)) 1512 return false; 1513 if (Math.doubleToLongBits(this.z) != Math.doubleToLongBits(z)) 1514 return false; 1515 return true; 1516 } 1517 1518 public bool equals(Vector3d other) { 1519 if (Math.doubleToLongBits(this.x) != Math.doubleToLongBits(other.x)) 1520 return false; 1521 if (Math.doubleToLongBits(this.y) != Math.doubleToLongBits(other.y)) 1522 return false; 1523 if (Math.doubleToLongBits(this.z) != Math.doubleToLongBits(other.z)) 1524 return false; 1525 return true; 1526 } 1527 1528 /** 1529 * Reflect this vector about the given normal vector. 1530 * 1531 * @param normal 1532 * the vector to reflect about 1533 * @return this 1534 */ 1535 ref public Vector3d reflect(Vector3d normal) return { 1536 double x = normal.x; 1537 double y = normal.y; 1538 double z = normal.z; 1539 double dot = Math.fma(this.x, x, Math.fma(this.y, y, this.z * z)); 1540 this.x = this.x - (dot + dot) * x; 1541 this.y = this.y - (dot + dot) * y; 1542 this.z = this.z - (dot + dot) * z; 1543 return this; 1544 } 1545 1546 /** 1547 * Reflect this vector about the given normal vector. 1548 * 1549 * @param x 1550 * the x component of the normal 1551 * @param y 1552 * the y component of the normal 1553 * @param z 1554 * the z component of the normal 1555 * @return this 1556 */ 1557 ref public Vector3d reflect(double x, double y, double z) return { 1558 double dot = Math.fma(this.x, x, Math.fma(this.y, y, this.z * z)); 1559 this.x = this.x - (dot + dot) * x; 1560 this.y = this.y - (dot + dot) * y; 1561 this.z = this.z - (dot + dot) * z; 1562 return this; 1563 } 1564 1565 public Vector3d reflect(Vector3d normal, ref Vector3d dest) { 1566 double x = normal.x; 1567 double y = normal.y; 1568 double z = normal.z; 1569 double dot = Math.fma(this.x, x, Math.fma(this.y, y, this.z * z)); 1570 dest.x = this.x - (dot + dot) * x; 1571 dest.y = this.y - (dot + dot) * y; 1572 dest.z = this.z - (dot + dot) * z; 1573 return dest; 1574 } 1575 1576 public Vector3d reflect(double x, double y, double z, ref Vector3d dest) { 1577 double dot = Math.fma(this.x, x, Math.fma(this.y, y, this.z * z)); 1578 dest.x = this.x - (dot + dot) * x; 1579 dest.y = this.y - (dot + dot) * y; 1580 dest.z = this.z - (dot + dot) * z; 1581 return dest; 1582 } 1583 1584 /** 1585 * Compute the half vector between this and the other vector. 1586 * 1587 * @param other 1588 * the other vector 1589 * @return this 1590 */ 1591 ref public Vector3d half(Vector3d other) return { 1592 return this.set(this).add(other.x, other.y, other.z).normalize(); 1593 } 1594 1595 /** 1596 * Compute the half vector between this and the vector <code>(x, y, z)</code>. 1597 * 1598 * @param x 1599 * the x component of the other vector 1600 * @param y 1601 * the y component of the other vector 1602 * @param z 1603 * the z component of the other vector 1604 * @return this 1605 */ 1606 ref public Vector3d half(double x, double y, double z) return { 1607 return this.set(this).add(x, y, z).normalize(); 1608 } 1609 1610 public Vector3d half(Vector3d other, ref Vector3d dest) { 1611 return dest.set(this).add(other.x, other.y, other.z).normalize(); 1612 } 1613 1614 public Vector3d half(double x, double y, double z, ref Vector3d dest) { 1615 return dest.set(this).add(x, y, z).normalize(); 1616 } 1617 1618 public Vector3d smoothStep(Vector3d v, double t, ref Vector3d dest) { 1619 double t2 = t * t; 1620 double t3 = t2 * t; 1621 dest.x = (x + x - v.x - v.x) * t3 + (3.0 * v.x - 3.0 * x) * t2 + x * t + x; 1622 dest.y = (y + y - v.y - v.y) * t3 + (3.0 * v.y - 3.0 * y) * t2 + y * t + y; 1623 dest.z = (z + z - v.z - v.z) * t3 + (3.0 * v.z - 3.0 * z) * t2 + z * t + z; 1624 return dest; 1625 } 1626 1627 public Vector3d hermite(Vector3d t0, Vector3d v1, Vector3d t1, double t, ref Vector3d dest) { 1628 double t2 = t * t; 1629 double t3 = t2 * t; 1630 dest.x = (x + x - v1.x - v1.x + t1.x + t0.x) * t3 + (3.0 * v1.x - 3.0 * x - t0.x - t0.x - t1.x) * t2 + x * t + x; 1631 dest.y = (y + y - v1.y - v1.y + t1.y + t0.y) * t3 + (3.0 * v1.y - 3.0 * y - t0.y - t0.y - t1.y) * t2 + y * t + y; 1632 dest.z = (z + z - v1.z - v1.z + t1.z + t0.z) * t3 + (3.0 * v1.z - 3.0 * z - t0.z - t0.z - t1.z) * t2 + z * t + z; 1633 return dest; 1634 } 1635 1636 /** 1637 * Linearly interpolate <code>this</code> and <code>other</code> using the given interpolation factor <code>t</code> 1638 * and store the result in <code>this</code>. 1639 * <p> 1640 * If <code>t</code> is <code>0.0</code> then the result is <code>this</code>. If the interpolation factor is <code>1.0</code> 1641 * then the result is <code>other</code>. 1642 * 1643 * @param other 1644 * the other vector 1645 * @param t 1646 * the interpolation factor between 0.0 and 1.0 1647 * @return this 1648 */ 1649 ref public Vector3d lerp(Vector3d other, double t) return { 1650 this.x = Math.fma(other.x - x, t, x); 1651 this.y = Math.fma(other.y - y, t, y); 1652 this.z = Math.fma(other.z - z, t, z); 1653 return this; 1654 } 1655 1656 public Vector3d lerp(Vector3d other, double t, ref Vector3d dest) { 1657 dest.x = Math.fma(other.x - x, t, x); 1658 dest.y = Math.fma(other.y - y, t, y); 1659 dest.z = Math.fma(other.z - z, t, z); 1660 return dest; 1661 } 1662 1663 public double get(int component) { 1664 switch (component) { 1665 case 0: 1666 return x; 1667 case 1: 1668 return y; 1669 case 2: 1670 return z; 1671 default: 1672 return 0; 1673 } 1674 } 1675 1676 public Vector3i get(int mode, ref Vector3i dest) { 1677 dest.x = Math.roundUsing(this.x, mode); 1678 dest.y = Math.roundUsing(this.y, mode); 1679 dest.z = Math.roundUsing(this.z, mode); 1680 return dest; 1681 } 1682 1683 public Vector3d get(ref Vector3d dest) { 1684 dest.x = this.x; 1685 dest.y = this.y; 1686 dest.z = this.z; 1687 return dest; 1688 } 1689 1690 public int maxComponent() { 1691 double absX = Math.abs(x); 1692 double absY = Math.abs(y); 1693 double absZ = Math.abs(z); 1694 if (absX >= absY && absX >= absZ) { 1695 return 0; 1696 } else if (absY >= absZ) { 1697 return 1; 1698 } 1699 return 2; 1700 } 1701 1702 public int minComponent() { 1703 double absX = Math.abs(x); 1704 double absY = Math.abs(y); 1705 double absZ = Math.abs(z); 1706 if (absX < absY && absX < absZ) { 1707 return 0; 1708 } else if (absY < absZ) { 1709 return 1; 1710 } 1711 return 2; 1712 } 1713 1714 public Vector3d orthogonalize(Vector3d v, ref Vector3d dest) { 1715 /* 1716 * http://lolengine.net/blog/2013/09/21/picking-orthogonal-vector-combing-coconuts 1717 */ 1718 double rx, ry, rz; 1719 if (Math.abs(v.x) > Math.abs(v.z)) { 1720 rx = -v.y; 1721 ry = v.x; 1722 rz = 0.0; 1723 } else { 1724 rx = 0.0; 1725 ry = -v.z; 1726 rz = v.y; 1727 } 1728 double invLen = Math.invsqrt(rx * rx + ry * ry + rz * rz); 1729 dest.x = rx * invLen; 1730 dest.y = ry * invLen; 1731 dest.z = rz * invLen; 1732 return dest; 1733 } 1734 1735 /** 1736 * Transform <code>this</code> vector so that it is orthogonal to the given vector <code>v</code> and normalize the result. 1737 * <p> 1738 * Reference: <a href="https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process">Gram–Schmidt process</a> 1739 * 1740 * @param v 1741 * the reference vector which the result should be orthogonal to 1742 * @return this 1743 */ 1744 ref public Vector3d orthogonalize(Vector3d v) return { 1745 orthogonalize(v, this); 1746 return this; 1747 } 1748 1749 public Vector3d orthogonalizeUnit(Vector3d v, ref Vector3d dest) { 1750 return orthogonalize(v, dest); 1751 } 1752 1753 /** 1754 * Transform <code>this</code> vector so that it is orthogonal to the given unit vector <code>v</code> and normalize the result. 1755 * <p> 1756 * The vector <code>v</code> is assumed to be a {@link #normalize() unit} vector. 1757 * <p> 1758 * Reference: <a href="https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process">Gram–Schmidt process</a> 1759 * 1760 * @param v 1761 * the reference unit vector which the result should be orthogonal to 1762 * @return this 1763 */ 1764 ref public Vector3d orthogonalizeUnit(Vector3d v) return { 1765 orthogonalizeUnit(v, this); 1766 return this; 1767 } 1768 1769 /** 1770 * Set each component of this vector to the largest (closest to positive 1771 * infinity) {@code double} value that is less than or equal to that 1772 * component and is equal to a mathematical integer. 1773 * 1774 * @return this 1775 */ 1776 ref public Vector3d floor() return { 1777 this.x = Math.floor(x); 1778 this.y = Math.floor(y); 1779 this.z = Math.floor(z); 1780 return this; 1781 } 1782 1783 public Vector3d floor(ref Vector3d dest) { 1784 dest.x = Math.floor(x); 1785 dest.y = Math.floor(y); 1786 dest.z = Math.floor(z); 1787 return dest; 1788 } 1789 1790 /** 1791 * Set each component of this vector to the smallest (closest to negative 1792 * infinity) {@code double} value that is greater than or equal to that 1793 * component and is equal to a mathematical integer. 1794 * 1795 * @return this 1796 */ 1797 ref public Vector3d ceil() return { 1798 this.x = Math.ceil(x); 1799 this.y = Math.ceil(y); 1800 this.z = Math.ceil(z); 1801 return this; 1802 } 1803 1804 public Vector3d ceil(ref Vector3d dest) { 1805 dest.x = Math.ceil(x); 1806 dest.y = Math.ceil(y); 1807 dest.z = Math.ceil(z); 1808 return dest; 1809 } 1810 1811 /** 1812 * Set each component of this vector to the closest double that is equal to 1813 * a mathematical integer, with ties rounding to positive infinity. 1814 * 1815 * @return this 1816 */ 1817 ref public Vector3d round() return { 1818 this.x = Math.round(x); 1819 this.y = Math.round(y); 1820 this.z = Math.round(z); 1821 return this; 1822 } 1823 1824 public Vector3d round(ref Vector3d dest) { 1825 dest.x = Math.round(x); 1826 dest.y = Math.round(y); 1827 dest.z = Math.round(z); 1828 return dest; 1829 } 1830 1831 public bool isFinite() { 1832 return Math.isFinite(x) && Math.isFinite(y) && Math.isFinite(z); 1833 } 1834 }