javax.vecmath/src/javax/vecmath/Matrix3d.java

   1 /*
   2  * $RCSfile: Matrix3d.java,v $
   3  *
   4  * Copyright 1996-2008 Sun Microsystems, Inc.  All Rights Reserved.
   5  * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
   6  *
   7  * This code is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License version 2 only, as
   9  * published by the Free Software Foundation.  Sun designates this
  10  * particular file as subject to the "Classpath" exception as provided
  11  * by Sun in the LICENSE file that accompanied this code.
  12  *
  13  * This code is distributed in the hope that it will be useful, but WITHOUT
  14  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  15  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  16  * version 2 for more details (a copy is included in the LICENSE file that
  17  * accompanied this code).
  18  *
  19  * You should have received a copy of the GNU General Public License version
  20  * 2 along with this work; if not, write to the Free Software Foundation,
  21  * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
  22  *
  23  * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
  24  * CA 95054 USA or visit www.sun.com if you need additional information or
  25  * have any questions.
  26  *
  27  * $Revision: 1.8 $
  28  * $Date: 2008/02/28 20:18:50 $
  29  * $State: Exp $
  30  */
  31
  32 package javax.vecmath;
  33
  34 import java.lang.Math;
  35
  36 /**
  37  * A double precision floating point 3 by 3 matrix.
  38  * Primarily to support 3D rotations.
  39  *
  40  */
  41 public class Matrix3d implements java.io.Serializable, Cloneable {
  42
  43     // Compatible with 1.1
  44     static final long serialVersionUID = 6837536777072402710L;
  45
  46     /**
  47      * The first matrix element in the first row.
  48      */
  49     public      double  m00;
  50
  51     /**
  52      * The second matrix element in the first row.
  53      */
  54     public      double  m01;
  55
  56     /**
  57      * The third matrix element in the first row.
  58      */
  59     public      double  m02;
  60
  61     /**
  62      * The first matrix element in the second row.
  63      */
  64     public      double  m10;
  65
  66     /**
  67      * The second matrix element in the second row.
  68      */
  69     public      double  m11;
  70
  71     /**
  72      * The third matrix element in the second row.
  73      */
  74     public      double  m12;
  75
  76     /**
  77      * The first matrix element in the third row.
  78      */
  79     public      double  m20;
  80
  81     /**
  82      * The second matrix element in the third row.
  83      */
  84     public      double  m21;
  85
  86     /**
  87      * The third matrix element in the third row.
  88      */
  89     public      double  m22;
  90
  91     //double[]    tmp = new double[9];  // scratch matrix
  92     //double[]    tmp_rot = new double[9];  // scratch matrix
  93     //double[]    tmp_scale = new double[3];  // scratch matrix
  94     private static final double EPS = 1.110223024E-16;
  95     private static final double ERR_EPS = 1.0E-8;
  96     private static double xin,yin,zin,xout,yout,zout;
  97
  98     /**
  99      * Constructs and initializes a Matrix3d from the specified nine values.
 100      * @param m00 the [0][0] element
 101      * @param m01 the [0][1] element
 102      * @param m02 the [0][2] element
 103      * @param m10 the [1][0] element
 104      * @param m11 the [1][1] element
 105      * @param m12 the [1][2] element
 106      * @param m20 the [2][0] element
 107      * @param m21 the [2][1] element
 108      * @param m22 the [2][2] element
 109      */
 110     public Matrix3d(double m00, double m01, double m02,
 111                     double m10, double m11, double m12,
 112                     double m20, double m21, double m22)
 113     {
 114         this.m00 = m00;
 115         this.m01 = m01;
 116         this.m02 = m02;
 117
 118         this.m10 = m10;
 119         this.m11 = m11;
 120         this.m12 = m12;
 121
 122         this.m20 = m20;
 123         this.m21 = m21;
 124         this.m22 = m22;
 125
 126     }
 127
 128     /**
 129      * Constructs and initializes a Matrix3d from the specified nine-
 130      * element array.
 131      * @param v the array of length 9 containing in order
 132      */
 133     public Matrix3d(double[] v)
 134     {
 135         this.m00 = v[0];
 136         this.m01 = v[1];
 137         this.m02 = v[2];
 138
 139         this.m10 = v[3];
 140         this.m11 = v[4];
 141         this.m12 = v[5];
 142
 143         this.m20 = v[6];
 144         this.m21 = v[7];
 145         this.m22 = v[8];
 146
 147     }
 148
 149    /**
 150      *  Constructs a new matrix with the same values as the
 151      *  Matrix3d parameter.
 152      *  @param m1  the source matrix
 153      */
 154    public Matrix3d(Matrix3d m1)
 155    {
 156         this.m00 = m1.m00;
 157         this.m01 = m1.m01;
 158         this.m02 = m1.m02;
 159
 160         this.m10 = m1.m10;
 161         this.m11 = m1.m11;
 162         this.m12 = m1.m12;
 163
 164         this.m20 = m1.m20;
 165         this.m21 = m1.m21;
 166         this.m22 = m1.m22;
 167
 168    }
 169
 170    /**
 171      *  Constructs a new matrix with the same values as the
 172      *  Matrix3f parameter.
 173      *  @param m1  the source matrix
 174      */
 175    public Matrix3d(Matrix3f m1)
 176    {
 177         this.m00 = m1.m00;
 178         this.m01 = m1.m01;
 179         this.m02 = m1.m02;
 180
 181         this.m10 = m1.m10;
 182         this.m11 = m1.m11;
 183         this.m12 = m1.m12;
 184
 185         this.m20 = m1.m20;
 186         this.m21 = m1.m21;
 187         this.m22 = m1.m22;
 188
 189    }
 190
 191     /**
 192      * Constructs and initializes a Matrix3d to all zeros.
 193      */
 194     public Matrix3d()
 195     {
 196         this.m00 = 0.0;
 197         this.m01 = 0.0;
 198         this.m02 = 0.0;
 199
 200         this.m10 = 0.0;
 201         this.m11 = 0.0;
 202         this.m12 = 0.0;
 203
 204         this.m20 = 0.0;
 205         this.m21 = 0.0;
 206         this.m22 = 0.0;
 207
 208     }
 209
 210    /**
 211      * Returns a string that contains the values of this Matrix3d.
 212      * @return the String representation
 213      */
 214     public String toString() {
 215       return
 216         this.m00 + ", " + this.m01 + ", " + this.m02 + "\n" +
 217         this.m10 + ", " + this.m11 + ", " + this.m12 + "\n" +
 218         this.m20 + ", " + this.m21 + ", " + this.m22 + "\n";
 219     }
 220
 221     /**
 222      * Sets this Matrix3d to identity.
 223      */
 224     public final void setIdentity()
 225     {
 226         this.m00 = 1.0;
 227         this.m01 = 0.0;
 228         this.m02 = 0.0;
 229
 230         this.m10 = 0.0;
 231         this.m11 = 1.0;
 232         this.m12 = 0.0;
 233
 234         this.m20 = 0.0;
 235         this.m21 = 0.0;
 236         this.m22 = 1.0;
 237     }
 238
 239    /**
 240      * Sets the scale component of the current matrix by factoring
 241      * out the current scale (by doing an SVD) and multiplying by
 242      * the new scale.
 243      * @param scale  the new scale amount
 244      */
 245     public final void setScale(double scale)
 246     {
 247
 248         double[]    tmp_rot = new double[9];  // scratch matrix
 249         double[]    tmp_scale = new double[3];  // scratch matrix
 250
 251         getScaleRotate(tmp_scale, tmp_rot);
 252
 253         this.m00 = tmp_rot[0] * scale;
 254         this.m01 = tmp_rot[1] * scale;
 255         this.m02 = tmp_rot[2] * scale;
 256
 257         this.m10 = tmp_rot[3] * scale;
 258         this.m11 = tmp_rot[4] * scale;
 259         this.m12 = tmp_rot[5] * scale;
 260
 261         this.m20 = tmp_rot[6] * scale;
 262         this.m21 = tmp_rot[7] * scale;
 263         this.m22 = tmp_rot[8] * scale;
 264     }
 265
 266     /**
 267      * Sets the specified element of this matrix3f to the value provided.
 268      * @param row the row number to be modified (zero indexed)
 269      * @param column the column number to be modified (zero indexed)
 270      * @param value the new value
 271      */
 272     public final void setElement(int row, int column, double value)
 273     {
 274         switch (row)
 275           {
 276           case 0:
 277             switch(column)
 278               {
 279               case 0:
 280                 this.m00 = value;
 281                 break;
 282               case 1:
 283                 this.m01 = value;
 284                 break;
 285               case 2:
 286                 this.m02 = value;
 287                 break;
 288               default:
 289                 throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d0"));
 290               }
 291             break;
 292
 293           case 1:
 294             switch(column)
 295               {
 296               case 0:
 297                 this.m10 = value;
 298                 break;
 299               case 1:
 300                 this.m11 = value;
 301                 break;
 302               case 2:
 303                 this.m12 = value;
 304                 break;
 305               default:
 306                 throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d0"));
 307               }
 308             break;
 309
 310
 311           case 2:
 312             switch(column)
 313               {
 314               case 0:
 315                 this.m20 = value;
 316                 break;
 317               case 1:
 318                 this.m21 = value;
 319                 break;
 320               case 2:
 321                 this.m22 = value;
 322                 break;
 323               default:
 324                 throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d0"));
 325               }
 326             break;
 327
 328           default:
 329                 throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d0"));
 330           }
 331     }
 332
 333     /**
 334      * Retrieves the value at the specified row and column of the specified
 335      * matrix.
 336      * @param row the row number to be retrieved (zero indexed)
 337      * @param column the column number to be retrieved (zero indexed)
 338      * @return the value at the indexed element.
 339      */
 340     public final double getElement(int row, int column)
 341     {
 342         switch (row)
 343           {
 344           case 0:
 345             switch(column)
 346               {
 347               case 0:
 348                 return(this.m00);
 349               case 1:
 350                 return(this.m01);
 351               case 2:
 352                 return(this.m02);
 353               default:
 354                 break;
 355               }
 356             break;
 357           case 1:
 358             switch(column)
 359               {
 360               case 0:
 361                 return(this.m10);
 362               case 1:
 363                 return(this.m11);
 364               case 2:
 365                 return(this.m12);
 366               default:
 367                 break;
 368               }
 369             break;
 370
 371           case 2:
 372             switch(column)
 373               {
 374               case 0:
 375                 return(this.m20);
 376               case 1:
 377                 return(this.m21);
 378               case 2:
 379                 return(this.m22);
 380               default:
 381                 break;
 382               }
 383             break;
 384
 385           default:
 386             break;
 387           }
 388
 389         throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d1"));
 390     }
 391
 392     /**
 393      * Copies the matrix values in the specified row into the vector parameter.
 394      * @param row  the matrix row
 395      * @param v    the vector into which the matrix row values will be copied
 396      */
 397     public final void getRow(int row, Vector3d v) {
 398         if( row == 0 ) {
 399            v.x = m00;
 400            v.y = m01;
 401            v.z = m02;
 402         } else if(row == 1) {
 403            v.x = m10;
 404            v.y = m11;
 405            v.z = m12;
 406         } else if(row == 2) {
 407            v.x = m20;
 408            v.y = m21;
 409            v.z = m22;
 410         } else {
 411           throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d2"));
 412         }
 413
 414     }
 415
 416     /**
 417      * Copies the matrix values in the specified row into the array parameter.
 418      * @param row  the matrix row
 419      * @param v    the array into which the matrix row values will be copied
 420      */
 421     public final void getRow(int row, double v[]) {
 422         if( row == 0 ) {
 423            v[0] = m00;
 424            v[1] = m01;
 425            v[2] = m02;
 426         } else if(row == 1) {
 427            v[0] = m10;
 428            v[1] = m11;
 429            v[2] = m12;
 430         } else if(row == 2) {
 431            v[0] = m20;
 432            v[1] = m21;
 433            v[2] = m22;
 434         } else {
 435           throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d2"));
 436         }
 437
 438     }
 439
 440     /**
 441      * Copies the matrix values in the specified column into the vector
 442      * parameter.
 443      * @param column  the matrix column
 444      * @param v    the vector into which the matrix row values will be copied
 445      */
 446     public final void getColumn(int column, Vector3d v) {
 447         if( column == 0 ) {
 448            v.x = m00;
 449            v.y = m10;
 450            v.z = m20;
 451         } else if(column == 1) {
 452            v.x = m01;
 453            v.y = m11;
 454            v.z = m21;
 455         }else if(column == 2){
 456            v.x = m02;
 457            v.y = m12;
 458            v.z = m22;
 459         } else {
 460            throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d4"));
 461         }
 462
 463     }
 464
 465     /**
 466      * Copies the matrix values in the specified column into the array
 467      * parameter.
 468      * @param column  the matrix column
 469      * @param v    the array into which the matrix row values will be copied
 470      */
 471     public final void getColumn(int column, double v[]) {
 472         if( column == 0 ) {
 473            v[0] = m00;
 474            v[1] = m10;
 475            v[2] = m20;
 476         } else if(column == 1) {
 477            v[0] = m01;
 478            v[1] = m11;
 479            v[2] = m21;
 480         }else if(column == 2) {
 481            v[0] = m02;
 482            v[1] = m12;
 483            v[2] = m22;
 484         }else {
 485           throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d4"));
 486         }
 487
 488     }
 489
 490
 491     /**
 492      * Sets the specified row of this matrix3d to the 4 values provided.
 493      * @param row the row number to be modified (zero indexed)
 494      * @param x the first column element
 495      * @param y the second column element
 496      * @param z the third column element
 497      */
 498     public final void setRow(int row, double x, double y, double z)
 499     {
 500         switch (row) {
 501         case 0:
 502             this.m00 = x;
 503             this.m01 = y;
 504             this.m02 = z;
 505             break;
 506
 507         case 1:
 508             this.m10 = x;
 509             this.m11 = y;
 510             this.m12 = z;
 511             break;
 512
 513         case 2:
 514             this.m20 = x;
 515             this.m21 = y;
 516             this.m22 = z;
 517             break;
 518
 519         default:
 520           throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d6"));
 521         }
 522     }
 523
 524     /**
 525      * Sets the specified row of this matrix3d to the Vector provided.
 526      * @param row the row number to be modified (zero indexed)
 527      * @param v the replacement row
 528      */
 529     public final void setRow(int row, Vector3d v)
 530     {
 531         switch (row) {
 532         case 0:
 533             this.m00 = v.x;
 534             this.m01 = v.y;
 535             this.m02 = v.z;
 536             break;
 537
 538         case 1:
 539             this.m10 = v.x;
 540             this.m11 = v.y;
 541             this.m12 = v.z;
 542             break;
 543
 544         case 2:
 545             this.m20 = v.x;
 546             this.m21 = v.y;
 547             this.m22 = v.z;
 548             break;
 549
 550         default:
 551             throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d6"));
 552         }
 553     }
 554
 555     /**
 556      * Sets the specified row of this matrix3d to the three values provided.
 557      * @param row the row number to be modified (zero indexed)
 558      * @param v the replacement row
 559      */
 560     public final void setRow(int row, double v[])
 561     {
 562         switch (row) {
 563         case 0:
 564             this.m00 = v[0];
 565             this.m01 = v[1];
 566             this.m02 = v[2];
 567             break;
 568
 569         case 1:
 570             this.m10 = v[0];
 571             this.m11 = v[1];
 572             this.m12 = v[2];
 573             break;
 574
 575         case 2:
 576             this.m20 = v[0];
 577             this.m21 = v[1];
 578             this.m22 = v[2];
 579             break;
 580
 581         default:
 582             throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d6"));
 583         }
 584     }
 585
 586     /**
 587      * Sets the specified column of this matrix3d to the three values provided.
 588      * @param column the column number to be modified (zero indexed)
 589      * @param x the first row element
 590      * @param y the second row element
 591      * @param z the third row element
 592      */
 593     public final void setColumn(int column, double x, double y, double z)
 594     {
 595         switch (column) {
 596         case 0:
 597             this.m00 = x;
 598             this.m10 = y;
 599             this.m20 = z;
 600             break;
 601
 602         case 1:
 603             this.m01 = x;
 604             this.m11 = y;
 605             this.m21 = z;
 606             break;
 607
 608         case 2:
 609             this.m02 = x;
 610             this.m12 = y;
 611             this.m22 = z;
 612             break;
 613
 614         default:
 615             throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d9"));
 616         }
 617     }
 618
 619     /**
 620      * Sets the specified column of this matrix3d to the vector provided.
 621      * @param column the column number to be modified (zero indexed)
 622      * @param v the replacement column
 623      */
 624     public final void setColumn(int column, Vector3d v)
 625     {
 626         switch (column) {
 627         case 0:
 628             this.m00 = v.x;
 629             this.m10 = v.y;
 630             this.m20 = v.z;
 631             break;
 632
 633         case 1:
 634             this.m01 = v.x;
 635             this.m11 = v.y;
 636             this.m21 = v.z;
 637             break;
 638
 639         case 2:
 640             this.m02 = v.x;
 641             this.m12 = v.y;
 642             this.m22 = v.z;
 643             break;
 644
 645         default:
 646             throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d9"));
 647         }
 648     }
 649
 650     /**
 651      * Sets the specified column of this matrix3d to the three values provided.
 652      * @param column the column number to be modified (zero indexed)
 653      * @param v the replacement column
 654      */
 655     public final void setColumn(int column, double v[])
 656     {
 657         switch (column) {
 658         case 0:
 659             this.m00 = v[0];
 660             this.m10 = v[1];
 661             this.m20 = v[2];
 662             break;
 663
 664         case 1:
 665             this.m01 = v[0];
 666             this.m11 = v[1];
 667             this.m21 = v[2];
 668             break;
 669
 670         case 2:
 671             this.m02 = v[0];
 672             this.m12 = v[1];
 673             this.m22 = v[2];
 674             break;
 675
 676         default:
 677             throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3d9"));
 678         }
 679     }
 680
 681    /**
 682      * Performs an SVD normalization of this matrix to calculate
 683      * and return the uniform scale factor. If the matrix has non-uniform
 684      * scale factors, the largest of the x, y, and z scale factors will
 685      * be returned. This matrix is not modified.
 686      * @return  the scale factor of this matrix
 687      */
 688     public final double getScale()
 689     {
 690
 691         double[]    tmp_scale = new double[3];  // scratch matrix
 692         double[]    tmp_rot = new double[9];  // scratch matrix
 693         getScaleRotate(tmp_scale, tmp_rot);
 694
 695         return( max3(tmp_scale) );
 696
 697     }
 698
 699    /**
 700      *  Adds a scalar to each component of this matrix.
 701      *  @param scalar  the scalar adder
 702      */
 703     public final void add(double scalar)
 704     {
 705         m00 += scalar;
 706         m01 += scalar;
 707         m02 += scalar;
 708
 709         m10 += scalar;
 710         m11 += scalar;
 711         m12 += scalar;
 712
 713         m20 += scalar;
 714         m21 += scalar;
 715         m22 += scalar;
 716
 717     }
 718
 719    /**
 720      *  Adds a scalar to each component of the matrix m1 and places
 721      *  the result into this.  Matrix m1 is not modified.
 722      *  @param scalar  the scalar adder
 723      *  @param m1  the original matrix values
 724      */
 725     public final void add(double scalar, Matrix3d m1)
 726     {
 727         this.m00 = m1.m00 + scalar;
 728         this.m01 = m1.m01 + scalar;
 729         this.m02 = m1.m02 + scalar;
 730
 731         this.m10 = m1.m10 + scalar;
 732         this.m11 = m1.m11 + scalar;
 733         this.m12 = m1.m12 + scalar;
 734
 735         this.m20 = m1.m20 + scalar;
 736         this.m21 = m1.m21 + scalar;
 737         this.m22 = m1.m22 + scalar;
 738     }
 739
 740     /**
 741      * Sets the value of this matrix to the matrix sum of matrices m1 and m2.
 742      * @param m1 the first matrix
 743      * @param m2 the second matrix
 744      */
 745     public final void add(Matrix3d m1, Matrix3d m2)
 746     {
 747         this.m00 = m1.m00 + m2.m00;
 748         this.m01 = m1.m01 + m2.m01;
 749         this.m02 = m1.m02 + m2.m02;
 750
 751         this.m10 = m1.m10 + m2.m10;
 752         this.m11 = m1.m11 + m2.m11;
 753         this.m12 = m1.m12 + m2.m12;
 754
 755         this.m20 = m1.m20 + m2.m20;
 756         this.m21 = m1.m21 + m2.m21;
 757         this.m22 = m1.m22 + m2.m22;
 758     }
 759
 760     /**
 761      * Sets the value of this matrix to the sum of itself and matrix m1.
 762      * @param m1 the other matrix
 763      */
 764     public final void add(Matrix3d m1)
 765     {
 766         this.m00 += m1.m00;
 767         this.m01 += m1.m01;
 768         this.m02 += m1.m02;
 769
 770         this.m10 += m1.m10;
 771         this.m11 += m1.m11;
 772         this.m12 += m1.m12;
 773
 774         this.m20 += m1.m20;
 775         this.m21 += m1.m21;
 776         this.m22 += m1.m22;
 777     }
 778
 779     /**
 780      * Sets the value of this matrix to the matrix difference
 781      * of matrices m1 and m2.
 782      * @param m1 the first matrix
 783      * @param m2 the second matrix
 784      */
 785     public final void sub(Matrix3d m1, Matrix3d m2)
 786     {
 787         this.m00 = m1.m00 - m2.m00;
 788         this.m01 = m1.m01 - m2.m01;
 789         this.m02 = m1.m02 - m2.m02;
 790
 791         this.m10 = m1.m10 - m2.m10;
 792         this.m11 = m1.m11 - m2.m11;
 793         this.m12 = m1.m12 - m2.m12;
 794
 795         this.m20 = m1.m20 - m2.m20;
 796         this.m21 = m1.m21 - m2.m21;
 797         this.m22 = m1.m22 - m2.m22;
 798     }
 799
 800     /**
 801      * Sets the value of this matrix to the matrix difference of itself and
 802      * matrix m1 (this = this - m1).
 803      * @param m1 the other matrix
 804      */
 805     public final void sub(Matrix3d m1)
 806     {
 807         this.m00 -= m1.m00;
 808         this.m01 -= m1.m01;
 809         this.m02 -= m1.m02;
 810
 811         this.m10 -= m1.m10;
 812         this.m11 -= m1.m11;
 813         this.m12 -= m1.m12;
 814
 815         this.m20 -= m1.m20;
 816         this.m21 -= m1.m21;
 817         this.m22 -= m1.m22;
 818     }
 819
 820     /**
 821      * Sets the value of this matrix to its transpose.
 822      */
 823     public final void transpose()
 824     {
 825         double temp;
 826
 827         temp = this.m10;
 828         this.m10 = this.m01;
 829         this.m01 = temp;
 830
 831         temp = this.m20;
 832         this.m20 = this.m02;
 833         this.m02 = temp;
 834
 835         temp = this.m21;
 836         this.m21 = this.m12;
 837         this.m12 = temp;
 838     }
 839
 840     /**
 841      * Sets the value of this matrix to the transpose of the argument matrix.
 842      * @param m1 the matrix to be transposed
 843      */
 844     public final void transpose(Matrix3d m1)
 845     {
 846         if (this != m1) {
 847             this.m00 = m1.m00;
 848             this.m01 = m1.m10;
 849             this.m02 = m1.m20;
 850
 851             this.m10 = m1.m01;
 852             this.m11 = m1.m11;
 853             this.m12 = m1.m21;
 854
 855             this.m20 = m1.m02;
 856             this.m21 = m1.m12;
 857             this.m22 = m1.m22;
 858         } else
 859             this.transpose();
 860     }
 861
 862     /**
 863      * Sets the value of this matrix to the matrix conversion of the
 864      * double precision quaternion argument.
 865      * @param q1 the quaternion to be converted
 866      */
 867     public final void set(Quat4d q1)
 868     {
 869         this.m00 = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z);
 870         this.m10 = (2.0*(q1.x*q1.y + q1.w*q1.z));
 871         this.m20 = (2.0*(q1.x*q1.z - q1.w*q1.y));
 872
 873         this.m01 = (2.0*(q1.x*q1.y - q1.w*q1.z));
 874         this.m11 = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z);
 875         this.m21 = (2.0*(q1.y*q1.z + q1.w*q1.x));
 876
 877         this.m02 = (2.0*(q1.x*q1.z + q1.w*q1.y));
 878         this.m12 = (2.0*(q1.y*q1.z - q1.w*q1.x));
 879         this.m22 = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y);
 880     }
 881
 882     /**
 883      * Sets the value of this matrix to the matrix conversion of the
 884      * double precision axis and angle argument.
 885      * @param a1 the axis and angle to be converted
 886      */
 887     public final void set(AxisAngle4d a1)
 888     {
 889       double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
 890
 891       if( mag < EPS ) {
 892         m00 = 1.0;
 893         m01 = 0.0;
 894         m02 = 0.0;
 895
 896         m10 = 0.0;
 897         m11 = 1.0;
 898         m12 = 0.0;
 899
 900         m20 = 0.0;
 901         m21 = 0.0;
 902         m22 = 1.0;
 903       } else {
 904         mag = 1.0/mag;
 905         double ax = a1.x*mag;
 906         double ay = a1.y*mag;
 907         double az = a1.z*mag;
 908
 909         double sinTheta = Math.sin(a1.angle);
 910         double cosTheta = Math.cos(a1.angle);
 911         double t = 1.0 - cosTheta;
 912
 913         double xz = ax * az;
 914         double xy = ax * ay;
 915         double yz = ay * az;
 916
 917         m00 = t * ax * ax + cosTheta;
 918         m01 = t * xy - sinTheta * az;
 919         m02 = t * xz + sinTheta * ay;
 920
 921         m10 = t * xy + sinTheta * az;
 922         m11 = t * ay * ay + cosTheta;
 923         m12 = t * yz - sinTheta * ax;
 924
 925         m20 = t * xz - sinTheta * ay;
 926         m21 = t * yz + sinTheta * ax;
 927         m22 = t * az * az + cosTheta;
 928       }
 929     }
 930
 931     /**
 932      * Sets the value of this matrix to the matrix conversion of the
 933      * single precision quaternion argument.
 934      * @param q1 the quaternion to be converted
 935      */
 936     public final void set(Quat4f q1)
 937     {
 938         this.m00 = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z);
 939         this.m10 = (2.0*(q1.x*q1.y + q1.w*q1.z));
 940         this.m20 = (2.0*(q1.x*q1.z - q1.w*q1.y));
 941
 942         this.m01 = (2.0*(q1.x*q1.y - q1.w*q1.z));
 943         this.m11 = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z);
 944         this.m21 = (2.0*(q1.y*q1.z + q1.w*q1.x));
 945
 946         this.m02 = (2.0*(q1.x*q1.z + q1.w*q1.y));
 947         this.m12 = (2.0*(q1.y*q1.z - q1.w*q1.x));
 948         this.m22 = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y);
 949     }
 950
 951     /**
 952      * Sets the value of this matrix to the matrix conversion of the
 953      * single precision axis and angle argument.
 954      * @param a1 the axis and angle to be converted
 955      */
 956     public final void set(AxisAngle4f a1)
 957     {
 958       double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
 959       if( mag < EPS ) {
 960         m00 = 1.0;
 961         m01 = 0.0;
 962         m02 = 0.0;
 963
 964         m10 = 0.0;
 965         m11 = 1.0;
 966         m12 = 0.0;
 967
 968         m20 = 0.0;
 969         m21 = 0.0;
 970         m22 = 1.0;
 971       } else {
 972         mag = 1.0/mag;
 973         double ax = a1.x*mag;
 974         double ay = a1.y*mag;
 975         double az = a1.z*mag;
 976         double sinTheta = Math.sin(a1.angle);
 977         double cosTheta = Math.cos(a1.angle);
 978         double t = 1.0 - cosTheta;
 979
 980         double xz = ax * az;
 981         double xy = ax * ay;
 982         double yz = ay * az;
 983
 984         m00 = t * ax * ax + cosTheta;
 985         m01 = t * xy - sinTheta * az;
 986         m02 = t * xz + sinTheta * ay;
 987
 988         m10 = t * xy + sinTheta * az;
 989         m11 = t * ay * ay + cosTheta;
 990         m12 = t * yz - sinTheta * ax;
 991
 992         m20 = t * xz - sinTheta * ay;
 993         m21 = t * yz + sinTheta * ax;
 994         m22 = t * az * az + cosTheta;
 995       }
 996     }
 997
 998     /**
 999      * Sets the value of this matrix to the double value of the Matrix3f
1000      * argument.
1001      * @param m1 the matrix3d to be converted to double
1002      */
1003     public final void set(Matrix3f m1)
1004     {
1005         this.m00 = m1.m00;
1006         this.m01 = m1.m01;
1007         this.m02 = m1.m02;
1008
1009         this.m10 = m1.m10;
1010         this.m11 = m1.m11;
1011         this.m12 = m1.m12;
1012
1013         this.m20 = m1.m20;
1014         this.m21 = m1.m21;
1015         this.m22 = m1.m22;
1016     }
1017
1018     /**
1019      * Sets the value of this matrix to the value of the Matrix3d
1020      * argument.
1021      * @param m1 the source matrix3d
1022      */
1023     public final void set(Matrix3d m1)
1024     {
1025         this.m00 = m1.m00;
1026         this.m01 = m1.m01;
1027         this.m02 = m1.m02;
1028
1029         this.m10 = m1.m10;
1030         this.m11 = m1.m11;
1031         this.m12 = m1.m12;
1032
1033         this.m20 = m1.m20;
1034         this.m21 = m1.m21;
1035         this.m22 = m1.m22;
1036     }
1037
1038     /**
1039      *  Sets the values in this Matrix3d equal to the row-major
1040      *  array parameter (ie, the first three elements of the
1041      *  array will be copied into the first row of this matrix, etc.).
1042      *  @param m  the double precision array of length 9
1043      */
1044     public final void set(double[] m)
1045     {
1046        m00 = m[0];
1047        m01 = m[1];
1048        m02 = m[2];
1049
1050        m10 = m[3];
1051        m11 = m[4];
1052        m12 = m[5];
1053
1054        m20 = m[6];
1055        m21 = m[7];
1056        m22 = m[8];
1057
1058     }
1059
1060     /**
1061      * Sets the value of this matrix to the matrix inverse
1062      * of the passed matrix m1.
1063      * @param m1 the matrix to be inverted
1064      */
1065     public final void invert(Matrix3d m1)
1066     {
1067          invertGeneral( m1 );
1068     }
1069
1070     /**
1071      * Inverts this matrix in place.
1072      */
1073     public final void invert()
1074     {
1075          invertGeneral( this );
1076     }
1077
1078     /**
1079      * General invert routine.  Inverts m1 and places the result in "this".
1080      * Note that this routine handles both the "this" version and the
1081      * non-"this" version.
1082      *
1083      * Also note that since this routine is slow anyway, we won't worry
1084      * about allocating a little bit of garbage.
1085      */
1086     private final void invertGeneral(Matrix3d  m1) {
1087         double result[] = new double[9];
1088         int row_perm[] = new int[3];
1089         int i, r, c;
1090         double[]    tmp = new double[9];  // scratch matrix
1091
1092         // Use LU decomposition and backsubstitution code specifically
1093         // for floating-point 3x3 matrices.
1094
1095         // Copy source matrix to t1tmp
1096         tmp[0] = m1.m00;
1097         tmp[1] = m1.m01;
1098         tmp[2] = m1.m02;
1099
1100         tmp[3] = m1.m10;
1101         tmp[4] = m1.m11;
1102         tmp[5] = m1.m12;
1103
1104         tmp[6] = m1.m20;
1105         tmp[7] = m1.m21;
1106         tmp[8] = m1.m22;
1107
1108
1109         // Calculate LU decomposition: Is the matrix singular?
1110         if (!luDecomposition(tmp, row_perm)) {
1111             // Matrix has no inverse
1112             throw new SingularMatrixException(VecMathI18N.getString("Matrix3d12"));
1113         }
1114
1115         // Perform back substitution on the identity matrix
1116         for(i=0;i<9;i++) result[i] = 0.0;
1117         result[0] = 1.0; result[4] = 1.0; result[8] = 1.0;
1118         luBacksubstitution(tmp, row_perm, result);
1119
1120         this.m00 = result[0];
1121         this.m01 = result[1];
1122         this.m02 = result[2];
1123
1124         this.m10 = result[3];
1125         this.m11 = result[4];
1126         this.m12 = result[5];
1127
1128         this.m20 = result[6];
1129         this.m21 = result[7];
1130         this.m22 = result[8];
1131
1132     }
1133
1134     /**
1135      * Given a 3x3 array "matrix0", this function replaces it with the
1136      * LU decomposition of a row-wise permutation of itself.  The input
1137      * parameters are "matrix0" and "dimen".  The array "matrix0" is also
1138      * an output parameter.  The vector "row_perm[3]" is an output
1139      * parameter that contains the row permutations resulting from partial
1140      * pivoting.  The output parameter "even_row_xchg" is 1 when the
1141      * number of row exchanges is even, or -1 otherwise.  Assumes data
1142      * type is always double.
1143      *
1144      * This function is similar to luDecomposition, except that it
1145      * is tuned specifically for 3x3 matrices.
1146      *
1147      * @return true if the matrix is nonsingular, or false otherwise.
1148      */
1149     //
1150     // Reference: Press, Flannery, Teukolsky, Vetterling,
1151     //        _Numerical_Recipes_in_C_, Cambridge University Press,
1152     //        1988, pp 40-45.
1153     //
1154     static boolean luDecomposition(double[] matrix0,
1155                                    int[] row_perm) {
1156
1157         double row_scale[] = new double[3];
1158
1159         // Determine implicit scaling information by looping over rows
1160         {
1161             int i, j;
1162             int ptr, rs;
1163             double big, temp;
1164
1165             ptr = 0;
1166             rs = 0;
1167
1168             // For each row ...
1169             i = 3;
1170             while (i-- != 0) {
1171                 big = 0.0;
1172
1173                 // For each column, find the largest element in the row
1174                 j = 3;
1175                 while (j-- != 0) {
1176                     temp = matrix0[ptr++];
1177                     temp = Math.abs(temp);
1178                     if (temp > big) {
1179                         big = temp;
1180                     }
1181                 }
1182
1183                 // Is the matrix singular?
1184                 if (big == 0.0) {
1185                     return false;
1186                 }
1187                 row_scale[rs++] = 1.0 / big;
1188             }
1189         }
1190
1191         {
1192             int j;
1193             int mtx;
1194
1195             mtx = 0;
1196
1197             // For all columns, execute Crout's method
1198             for (j = 0; j < 3; j++) {
1199                 int i, imax, k;
1200                 int target, p1, p2;
1201                 double sum, big, temp;
1202
1203                 // Determine elements of upper diagonal matrix U
1204                 for (i = 0; i < j; i++) {
1205                     target = mtx + (3*i) + j;
1206                     sum = matrix0[target];
1207                     k = i;
1208                     p1 = mtx + (3*i);
1209                     p2 = mtx + j;
1210                     while (k-- != 0) {
1211                         sum -= matrix0[p1] * matrix0[p2];
1212                         p1++;
1213                         p2 += 3;
1214                     }
1215                     matrix0[target] = sum;
1216                 }
1217
1218                 // Search for largest pivot element and calculate
1219                 // intermediate elements of lower diagonal matrix L.
1220                 big = 0.0;
1221                 imax = -1;
1222                 for (i = j; i < 3; i++) {
1223                     target = mtx + (3*i) + j;
1224                     sum = matrix0[target];
1225                     k = j;
1226                     p1 = mtx + (3*i);
1227                     p2 = mtx + j;
1228                     while (k-- != 0) {
1229                         sum -= matrix0[p1] * matrix0[p2];
1230                         p1++;
1231                         p2 += 3;
1232                     }
1233                     matrix0[target] = sum;
1234
1235                     // Is this the best pivot so far?
1236                     if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
1237                         big = temp;
1238                         imax = i;
1239                     }
1240                 }
1241
1242                 if (imax < 0) {
1243                     throw new RuntimeException(VecMathI18N.getString("Matrix3d13"));
1244                 }
1245
1246                 // Is a row exchange necessary?
1247                 if (j != imax) {
1248                     // Yes: exchange rows
1249                     k = 3;
1250                     p1 = mtx + (3*imax);
1251                     p2 = mtx + (3*j);
1252                     while (k-- != 0) {
1253                         temp = matrix0[p1];
1254                         matrix0[p1++] = matrix0[p2];
1255                         matrix0[p2++] = temp;
1256                     }
1257
1258                     // Record change in scale factor
1259                     row_scale[imax] = row_scale[j];
1260                 }
1261
1262                 // Record row permutation
1263                 row_perm[j] = imax;
1264
1265                 // Is the matrix singular
1266                 if (matrix0[(mtx + (3*j) + j)] == 0.0) {
1267                     return false;
1268                 }
1269
1270                 // Divide elements of lower diagonal matrix L by pivot
1271                 if (j != (3-1)) {
1272                     temp = 1.0 / (matrix0[(mtx + (3*j) + j)]);
1273                     target = mtx + (3*(j+1)) + j;
1274                     i = 2 - j;
1275                     while (i-- != 0) {
1276                         matrix0[target] *= temp;
1277                         target += 3;
1278                     }
1279                 }
1280             }
1281         }
1282
1283         return true;
1284     }
1285
1286     /**
1287      * Solves a set of linear equations.  The input parameters "matrix1",
1288      * and "row_perm" come from luDecompostionD3x3 and do not change
1289      * here.  The parameter "matrix2" is a set of column vectors assembled
1290      * into a 3x3 matrix of floating-point values.  The procedure takes each
1291      * column of "matrix2" in turn and treats it as the right-hand side of the
1292      * matrix equation Ax = LUx = b.  The solution vector replaces the
1293      * original column of the matrix.
1294      *
1295      * If "matrix2" is the identity matrix, the procedure replaces its contents
1296      * with the inverse of the matrix from which "matrix1" was originally
1297      * derived.
1298      */
1299     //
1300     // Reference: Press, Flannery, Teukolsky, Vetterling,
1301     //        _Numerical_Recipes_in_C_, Cambridge University Press,
1302     //        1988, pp 44-45.
1303     //
1304     static void luBacksubstitution(double[] matrix1,
1305                                    int[] row_perm,
1306                                    double[] matrix2) {
1307
1308         int i, ii, ip, j, k;
1309         int rp;
1310         int cv, rv;
1311
1312         //      rp = row_perm;
1313         rp = 0;
1314
1315         // For each column vector of matrix2 ...
1316         for (k = 0; k < 3; k++) {
1317             //      cv = &(matrix2[0][k]);
1318             cv = k;
1319             ii = -1;
1320
1321             // Forward substitution
1322             for (i = 0; i < 3; i++) {
1323                 double sum;
1324
1325                 ip = row_perm[rp+i];
1326                 sum = matrix2[cv+3*ip];
1327                 matrix2[cv+3*ip] = matrix2[cv+3*i];
1328                 if (ii >= 0) {
1329                     //              rv = &(matrix1[i][0]);
1330                     rv = i*3;
1331                     for (j = ii; j <= i-1; j++) {
1332                         sum -= matrix1[rv+j] * matrix2[cv+3*j];
1333                     }
1334                 }
1335                 else if (sum != 0.0) {
1336                     ii = i;
1337                 }
1338                 matrix2[cv+3*i] = sum;
1339             }
1340
1341             // Backsubstitution
1342             //      rv = &(matrix1[3][0]);
1343             rv = 2*3;
1344             matrix2[cv+3*2] /= matrix1[rv+2];
1345
1346             rv -= 3;
1347             matrix2[cv+3*1] = (matrix2[cv+3*1] -
1348                             matrix1[rv+2] * matrix2[cv+3*2]) / matrix1[rv+1];
1349
1350             rv -= 3;
1351             matrix2[cv+4*0] = (matrix2[cv+3*0] -
1352                             matrix1[rv+1] * matrix2[cv+3*1] -
1353                             matrix1[rv+2] * matrix2[cv+3*2]) / matrix1[rv+0];
1354
1355         }
1356     }
1357
1358     /**
1359      * Computes the determinant of this matrix.
1360      * @return the determinant of the matrix
1361      */
1362     public final double determinant()
1363     {
1364        double total;
1365
1366        total =  this.m00*(this.m11*this.m22 - this.m12*this.m21)
1367               + this.m01*(this.m12*this.m20 - this.m10*this.m22)
1368               + this.m02*(this.m10*this.m21 - this.m11*this.m20);
1369        return total;
1370     }
1371
1372     /**
1373      * Sets the value of this matrix to a scale matrix with
1374      * the passed scale amount.
1375      * @param scale the scale factor for the matrix
1376      */
1377     public final void set(double scale)
1378     {
1379         this.m00 = scale;
1380         this.m01 = 0.0;
1381         this.m02 = 0.0;
1382
1383         this.m10 = 0.0;
1384         this.m11 = scale;
1385         this.m12 = 0.0;
1386
1387         this.m20 = 0.0;
1388         this.m21 = 0.0;
1389         this.m22 = scale;
1390     }
1391
1392     /**
1393      * Sets the value of this matrix to a counter clockwise rotation
1394      * about the x axis.
1395      * @param angle the angle to rotate about the X axis in radians
1396      */
1397     public final void rotX(double angle)
1398     {
1399         double  sinAngle, cosAngle;
1400
1401         sinAngle = Math.sin(angle);
1402         cosAngle = Math.cos(angle);
1403
1404         this.m00 = 1.0;
1405         this.m01 = 0.0;
1406         this.m02 = 0.0;
1407
1408         this.m10 = 0.0;
1409         this.m11 = cosAngle;
1410         this.m12 = -sinAngle;
1411
1412         this.m20 = 0.0;
1413         this.m21 = sinAngle;
1414         this.m22 = cosAngle;
1415     }
1416
1417     /**
1418      * Sets the value of this matrix to a counter clockwise rotation
1419      * about the y axis.
1420      * @param angle the angle to rotate about the Y axis in radians
1421      */
1422     public final void rotY(double angle)
1423     {
1424         double  sinAngle, cosAngle;
1425
1426         sinAngle = Math.sin(angle);
1427         cosAngle = Math.cos(angle);
1428
1429         this.m00 = cosAngle;
1430         this.m01 = 0.0;
1431         this.m02 = sinAngle;
1432
1433         this.m10 = 0.0;
1434         this.m11 = 1.0;
1435         this.m12 = 0.0;
1436
1437         this.m20 = -sinAngle;
1438         this.m21 = 0.0;
1439         this.m22 = cosAngle;
1440     }
1441
1442     /**
1443      * Sets the value of this matrix to a counter clockwise rotation
1444      * about the z axis.
1445      * @param angle the angle to rotate about the Z axis in radians
1446      */
1447     public final void rotZ(double angle)
1448     {
1449         double  sinAngle, cosAngle;
1450
1451         sinAngle = Math.sin(angle);
1452         cosAngle = Math.cos(angle);
1453
1454         this.m00 = cosAngle;
1455         this.m01 = -sinAngle;
1456         this.m02 = 0.0;
1457
1458         this.m10 = sinAngle;
1459         this.m11 = cosAngle;
1460         this.m12 = 0.0;
1461
1462         this.m20 = 0.0;
1463         this.m21 = 0.0;
1464         this.m22 = 1.0;
1465     }
1466
1467    /**
1468      * Multiplies each element of this matrix by a scalar.
1469      * @param scalar  The scalar multiplier.
1470      */
1471     public final void mul(double scalar)
1472     {
1473        m00 *= scalar;
1474        m01 *= scalar;
1475        m02 *= scalar;
1476
1477        m10 *= scalar;
1478        m11 *= scalar;
1479        m12 *= scalar;
1480
1481        m20 *= scalar;
1482        m21 *= scalar;
1483        m22 *= scalar;
1484
1485     }
1486
1487    /**
1488      * Multiplies each element of matrix m1 by a scalar and places
1489      * the result into this.  Matrix m1 is not modified.
1490      * @param scalar  the scalar multiplier
1491      * @param m1  the original matrix
1492      */
1493     public final void mul(double scalar, Matrix3d m1)
1494     {
1495         this.m00 = scalar * m1.m00;
1496         this.m01 = scalar * m1.m01;
1497         this.m02 = scalar * m1.m02;
1498
1499         this.m10 = scalar * m1.m10;
1500         this.m11 = scalar * m1.m11;
1501         this.m12 = scalar * m1.m12;
1502
1503         this.m20 = scalar * m1.m20;
1504         this.m21 = scalar * m1.m21;
1505         this.m22 = scalar * m1.m22;
1506
1507     }
1508
1509    /**
1510      * Sets the value of this matrix to the result of multiplying itself
1511      * with matrix m1.
1512      * @param m1 the other matrix
1513      */
1514     public final void mul(Matrix3d m1)
1515     {
1516             double      m00, m01, m02,
1517                         m10, m11, m12,
1518                         m20, m21, m22;
1519
1520             m00 = this.m00*m1.m00 + this.m01*m1.m10 + this.m02*m1.m20;
1521             m01 = this.m00*m1.m01 + this.m01*m1.m11 + this.m02*m1.m21;
1522             m02 = this.m00*m1.m02 + this.m01*m1.m12 + this.m02*m1.m22;
1523
1524             m10 = this.m10*m1.m00 + this.m11*m1.m10 + this.m12*m1.m20;
1525             m11 = this.m10*m1.m01 + this.m11*m1.m11 + this.m12*m1.m21;
1526             m12 = this.m10*m1.m02 + this.m11*m1.m12 + this.m12*m1.m22;
1527
1528             m20 = this.m20*m1.m00 + this.m21*m1.m10 + this.m22*m1.m20;
1529             m21 = this.m20*m1.m01 + this.m21*m1.m11 + this.m22*m1.m21;
1530             m22 = this.m20*m1.m02 + this.m21*m1.m12 + this.m22*m1.m22;
1531
1532             this.m00 = m00; this.m01 = m01; this.m02 = m02;
1533             this.m10 = m10; this.m11 = m11; this.m12 = m12;
1534             this.m20 = m20; this.m21 = m21; this.m22 = m22;
1535     }
1536
1537     /**
1538      * Sets the value of this matrix to the result of multiplying
1539      * the two argument matrices together.
1540      * @param m1 the first matrix
1541      * @param m2 the second matrix
1542      */
1543     public final void mul(Matrix3d m1, Matrix3d m2)
1544     {
1545         if (this != m1 && this != m2) {
1546             this.m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20;
1547             this.m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21;
1548             this.m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22;
1549
1550             this.m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20;
1551             this.m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21;
1552             this.m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22;
1553
1554             this.m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20;
1555             this.m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21;
1556             this.m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22;
1557         } else {
1558             double      m00, m01, m02,
1559                         m10, m11, m12,
1560                 m20, m21, m22;  // vars for temp result matrix
1561
1562             m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20;
1563             m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21;
1564             m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22;
1565
1566             m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20;
1567             m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21;
1568             m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22;
1569
1570             m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20;
1571             m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21;
1572             m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22;
1573
1574             this.m00 = m00; this.m01 = m01; this.m02 = m02;
1575             this.m10 = m10; this.m11 = m11; this.m12 = m12;
1576             this.m20 = m20; this.m21 = m21; this.m22 = m22;
1577         }
1578     }
1579
1580    /**
1581      *  Multiplies this matrix by matrix m1, does an SVD normalization
1582      *  of the result, and places the result back into this matrix
1583      *  this = SVDnorm(this*m1).
1584      *  @param  m1   the matrix on the right hand side of the multiplication
1585      */
1586     public final void mulNormalize(Matrix3d m1){
1587
1588         double[]    tmp = new double[9];  // scratch matrix
1589         double[]    tmp_rot = new double[9];  // scratch matrix
1590         double[]    tmp_scale = new double[3];  // scratch matrix
1591
1592         tmp[0] = this.m00*m1.m00 + this.m01*m1.m10 + this.m02*m1.m20;
1593         tmp[1] = this.m00*m1.m01 + this.m01*m1.m11 + this.m02*m1.m21;
1594         tmp[2] = this.m00*m1.m02 + this.m01*m1.m12 + this.m02*m1.m22;
1595
1596         tmp[3] = this.m10*m1.m00 + this.m11*m1.m10 + this.m12*m1.m20;
1597         tmp[4] = this.m10*m1.m01 + this.m11*m1.m11 + this.m12*m1.m21;
1598         tmp[5] = this.m10*m1.m02 + this.m11*m1.m12 + this.m12*m1.m22;
1599
1600         tmp[6] = this.m20*m1.m00 + this.m21*m1.m10 + this.m22*m1.m20;
1601         tmp[7] = this.m20*m1.m01 + this.m21*m1.m11 + this.m22*m1.m21;
1602         tmp[8] = this.m20*m1.m02 + this.m21*m1.m12 + this.m22*m1.m22;
1603
1604         compute_svd( tmp, tmp_scale, tmp_rot);
1605
1606         this.m00 = tmp_rot[0];
1607         this.m01 = tmp_rot[1];
1608         this.m02 = tmp_rot[2];
1609
1610         this.m10 = tmp_rot[3];
1611         this.m11 = tmp_rot[4];
1612         this.m12 = tmp_rot[5];
1613
1614         this.m20 = tmp_rot[6];
1615         this.m21 = tmp_rot[7];
1616         this.m22 = tmp_rot[8];
1617
1618     }
1619
1620
1621    /**
1622      *  Multiplies matrix m1 by matrix m2, does an SVD normalization
1623      *  of the result, and places the result into this matrix
1624      *  this = SVDnorm(m1*m2).
1625      *  @param  m1  the matrix on the left hand side of the multiplication
1626      *  @param  m2  the matrix on the right hand side of the multiplication
1627      */
1628     public final void mulNormalize(Matrix3d m1, Matrix3d m2){
1629
1630         double[]    tmp = new double[9];  // scratch matrix
1631         double[]    tmp_rot = new double[9];  // scratch matrix
1632         double[]    tmp_scale = new double[3];  // scratch matrix
1633
1634         tmp[0] = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20;
1635         tmp[1] = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21;
1636         tmp[2] = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22;
1637
1638         tmp[3] = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20;
1639         tmp[4] = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21;
1640         tmp[5] = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22;
1641
1642         tmp[6] = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20;
1643         tmp[7] = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21;
1644         tmp[8] = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22;
1645
1646         compute_svd( tmp, tmp_scale, tmp_rot);
1647
1648         this.m00 = tmp_rot[0];
1649         this.m01 = tmp_rot[1];
1650         this.m02 = tmp_rot[2];
1651
1652         this.m10 = tmp_rot[3];
1653         this.m11 = tmp_rot[4];
1654         this.m12 = tmp_rot[5];
1655
1656         this.m20 = tmp_rot[6];
1657         this.m21 = tmp_rot[7];
1658         this.m22 = tmp_rot[8];
1659
1660     }
1661
1662    /**
1663      *  Multiplies the transpose of matrix m1 times the transpose of matrix
1664      *  m2, and places the result into this.
1665      *  @param m1  the matrix on the left hand side of the multiplication
1666      *  @param m2  the matrix on the right hand side of the multiplication
1667      */
1668     public final void mulTransposeBoth(Matrix3d m1, Matrix3d m2)
1669     {
1670         if (this != m1 && this != m2) {
1671             this.m00 = m1.m00*m2.m00 + m1.m10*m2.m01 + m1.m20*m2.m02;
1672             this.m01 = m1.m00*m2.m10 + m1.m10*m2.m11 + m1.m20*m2.m12;
1673             this.m02 = m1.m00*m2.m20 + m1.m10*m2.m21 + m1.m20*m2.m22;
1674
1675             this.m10 = m1.m01*m2.m00 + m1.m11*m2.m01 + m1.m21*m2.m02;
1676             this.m11 = m1.m01*m2.m10 + m1.m11*m2.m11 + m1.m21*m2.m12;
1677             this.m12 = m1.m01*m2.m20 + m1.m11*m2.m21 + m1.m21*m2.m22;
1678
1679             this.m20 = m1.m02*m2.m00 + m1.m12*m2.m01 + m1.m22*m2.m02;
1680             this.m21 = m1.m02*m2.m10 + m1.m12*m2.m11 + m1.m22*m2.m12;
1681             this.m22 = m1.m02*m2.m20 + m1.m12*m2.m21 + m1.m22*m2.m22;
1682         } else {
1683             double      m00, m01, m02,
1684                         m10, m11, m12,
1685                 m20, m21, m22;  // vars for temp result matrix
1686
1687             m00 = m1.m00*m2.m00 + m1.m10*m2.m01 + m1.m20*m2.m02;
1688             m01 = m1.m00*m2.m10 + m1.m10*m2.m11 + m1.m20*m2.m12;
1689             m02 = m1.m00*m2.m20 + m1.m10*m2.m21 + m1.m20*m2.m22;
1690
1691             m10 = m1.m01*m2.m00 + m1.m11*m2.m01 + m1.m21*m2.m02;
1692             m11 = m1.m01*m2.m10 + m1.m11*m2.m11 + m1.m21*m2.m12;
1693             m12 = m1.m01*m2.m20 + m1.m11*m2.m21 + m1.m21*m2.m22;
1694
1695             m20 = m1.m02*m2.m00 + m1.m12*m2.m01 + m1.m22*m2.m02;
1696             m21 = m1.m02*m2.m10 + m1.m12*m2.m11 + m1.m22*m2.m12;
1697             m22 = m1.m02*m2.m20 + m1.m12*m2.m21 + m1.m22*m2.m22;
1698
1699             this.m00 = m00; this.m01 = m01; this.m02 = m02;
1700             this.m10 = m10; this.m11 = m11; this.m12 = m12;
1701             this.m20 = m20; this.m21 = m21; this.m22 = m22;
1702         }
1703
1704     }
1705
1706    /**
1707      *  Multiplies matrix m1 times the transpose of matrix m2, and
1708      *  places the result into this.
1709      *  @param m1  the matrix on the left hand side of the multiplication
1710      *  @param m2  the matrix on the right hand side of the multiplication
1711      */
1712     public final void mulTransposeRight(Matrix3d m1, Matrix3d m2)
1713   {
1714     if (this != m1 && this != m2) {
1715       this.m00 = m1.m00*m2.m00 + m1.m01*m2.m01 + m1.m02*m2.m02;
1716       this.m01 = m1.m00*m2.m10 + m1.m01*m2.m11 + m1.m02*m2.m12;
1717       this.m02 = m1.m00*m2.m20 + m1.m01*m2.m21 + m1.m02*m2.m22;
1718
1719       this.m10 = m1.m10*m2.m00 + m1.m11*m2.m01 + m1.m12*m2.m02;
1720       this.m11 = m1.m10*m2.m10 + m1.m11*m2.m11 + m1.m12*m2.m12;
1721       this.m12 = m1.m10*m2.m20 + m1.m11*m2.m21 + m1.m12*m2.m22;
1722
1723       this.m20 = m1.m20*m2.m00 + m1.m21*m2.m01 + m1.m22*m2.m02;
1724       this.m21 = m1.m20*m2.m10 + m1.m21*m2.m11 + m1.m22*m2.m12;
1725       this.m22 = m1.m20*m2.m20 + m1.m21*m2.m21 + m1.m22*m2.m22;
1726     } else {
1727       double    m00, m01, m02,
1728         m10, m11, m12,
1729           m20, m21, m22;  // vars for temp result matrix
1730
1731       m00 = m1.m00*m2.m00 + m1.m01*m2.m01 + m1.m02*m2.m02;
1732       m01 = m1.m00*m2.m10 + m1.m01*m2.m11 + m1.m02*m2.m12;
1733       m02 = m1.m00*m2.m20 + m1.m01*m2.m21 + m1.m02*m2.m22;
1734
1735       m10 = m1.m10*m2.m00 + m1.m11*m2.m01 + m1.m12*m2.m02;
1736       m11 = m1.m10*m2.m10 + m1.m11*m2.m11 + m1.m12*m2.m12;
1737       m12 = m1.m10*m2.m20 + m1.m11*m2.m21 + m1.m12*m2.m22;
1738
1739       m20 = m1.m20*m2.m00 + m1.m21*m2.m01 + m1.m22*m2.m02;
1740       m21 = m1.m20*m2.m10 + m1.m21*m2.m11 + m1.m22*m2.m12;
1741       m22 = m1.m20*m2.m20 + m1.m21*m2.m21 + m1.m22*m2.m22;
1742
1743       this.m00 = m00; this.m01 = m01; this.m02 = m02;
1744       this.m10 = m10; this.m11 = m11; this.m12 = m12;
1745       this.m20 = m20; this.m21 = m21; this.m22 = m22;
1746     }
1747   }
1748
1749
1750    /**
1751      *  Multiplies the transpose of matrix m1 times matrix m2, and
1752      *  places the result into this.
1753      *  @param m1  the matrix on the left hand side of the multiplication
1754      *  @param m2  the matrix on the right hand side of the multiplication
1755      */
1756     public final void mulTransposeLeft(Matrix3d m1, Matrix3d m2) {
1757         if (this != m1 && this != m2) {
1758             this.m00 = m1.m00*m2.m00 + m1.m10*m2.m10 + m1.m20*m2.m20;
1759             this.m01 = m1.m00*m2.m01 + m1.m10*m2.m11 + m1.m20*m2.m21;
1760             this.m02 = m1.m00*m2.m02 + m1.m10*m2.m12 + m1.m20*m2.m22;
1761
1762             this.m10 = m1.m01*m2.m00 + m1.m11*m2.m10 + m1.m21*m2.m20;
1763             this.m11 = m1.m01*m2.m01 + m1.m11*m2.m11 + m1.m21*m2.m21;
1764             this.m12 = m1.m01*m2.m02 + m1.m11*m2.m12 + m1.m21*m2.m22;
1765
1766             this.m20 = m1.m02*m2.m00 + m1.m12*m2.m10 + m1.m22*m2.m20;
1767             this.m21 = m1.m02*m2.m01 + m1.m12*m2.m11 + m1.m22*m2.m21;
1768             this.m22 = m1.m02*m2.m02 + m1.m12*m2.m12 + m1.m22*m2.m22;
1769         } else {
1770             double      m00, m01, m02,
1771                         m10, m11, m12,
1772                 m20, m21, m22;  // vars for temp result matrix
1773
1774             m00 = m1.m00*m2.m00 + m1.m10*m2.m10 + m1.m20*m2.m20;
1775             m01 = m1.m00*m2.m01 + m1.m10*m2.m11 + m1.m20*m2.m21;
1776             m02 = m1.m00*m2.m02 + m1.m10*m2.m12 + m1.m20*m2.m22;
1777
1778             m10 = m1.m01*m2.m00 + m1.m11*m2.m10 + m1.m21*m2.m20;
1779             m11 = m1.m01*m2.m01 + m1.m11*m2.m11 + m1.m21*m2.m21;
1780             m12 = m1.m01*m2.m02 + m1.m11*m2.m12 + m1.m21*m2.m22;
1781
1782             m20 = m1.m02*m2.m00 + m1.m12*m2.m10 + m1.m22*m2.m20;
1783             m21 = m1.m02*m2.m01 + m1.m12*m2.m11 + m1.m22*m2.m21;
1784             m22 = m1.m02*m2.m02 + m1.m12*m2.m12 + m1.m22*m2.m22;
1785
1786             this.m00 = m00; this.m01 = m01; this.m02 = m02;
1787             this.m10 = m10; this.m11 = m11; this.m12 = m12;
1788             this.m20 = m20; this.m21 = m21; this.m22 = m22;
1789         }
1790     }
1791
1792
1793
1794    /**
1795      * Performs singular value decomposition normalization of this matrix.
1796      */
1797     public final void normalize(){
1798         double[]    tmp_rot = new double[9];  // scratch matrix
1799         double[]    tmp_scale = new double[3];  // scratch matrix
1800
1801         getScaleRotate( tmp_scale, tmp_rot );
1802
1803         this.m00 = tmp_rot[0];
1804         this.m01 = tmp_rot[1];
1805         this.m02 = tmp_rot[2];
1806
1807         this.m10 = tmp_rot[3];
1808         this.m11 = tmp_rot[4];
1809         this.m12 = tmp_rot[5];
1810
1811         this.m20 = tmp_rot[6];
1812         this.m21 = tmp_rot[7];
1813         this.m22 = tmp_rot[8];
1814
1815     }
1816
1817
1818    /**
1819      * Perform singular value decomposition normalization of matrix m1 and
1820      * place the normalized values into this.
1821      * @param m1  Provides the matrix values to be normalized
1822      */
1823     public final void normalize(Matrix3d m1){
1824
1825         double[]    tmp = new double[9];  // scratch matrix
1826         double[]    tmp_rot = new double[9];  // scratch matrix
1827         double[]    tmp_scale = new double[3];  // scratch matrix
1828
1829         tmp[0] = m1.m00;
1830         tmp[1] = m1.m01;
1831         tmp[2] = m1.m02;
1832
1833         tmp[3] = m1.m10;
1834         tmp[4] = m1.m11;
1835         tmp[5] = m1.m12;
1836
1837         tmp[6] = m1.m20;
1838         tmp[7] = m1.m21;
1839         tmp[8] = m1.m22;
1840
1841         compute_svd( tmp, tmp_scale, tmp_rot);
1842
1843         this.m00 = tmp_rot[0];
1844         this.m01 = tmp_rot[1];
1845         this.m02 = tmp_rot[2];
1846
1847         this.m10 = tmp_rot[3];
1848         this.m11 = tmp_rot[4];
1849         this.m12 = tmp_rot[5];
1850
1851         this.m20 = tmp_rot[6];
1852         this.m21 = tmp_rot[7];
1853         this.m22 = tmp_rot[8];
1854     }
1855
1856
1857    /**
1858      * Perform cross product normalization of this matrix.
1859      */
1860
1861     public final void normalizeCP()
1862     {
1863        double mag = 1.0/Math.sqrt(m00*m00 + m10*m10 + m20*m20);
1864        m00 = m00*mag;
1865        m10 = m10*mag;
1866        m20 = m20*mag;
1867
1868        mag = 1.0/Math.sqrt(m01*m01 + m11*m11 + m21*m21);
1869        m01 = m01*mag;
1870        m11 = m11*mag;
1871        m21 = m21*mag;
1872
1873        m02 = m10*m21 - m11*m20;
1874        m12 = m01*m20 - m00*m21;
1875        m22 = m00*m11 - m01*m10;
1876     }
1877
1878
1879    /**
1880      * Perform cross product normalization of matrix m1 and place the
1881      * normalized values into this.
1882      * @param m1  Provides the matrix values to be normalized
1883      */
1884     public final void normalizeCP(Matrix3d m1)
1885     {
1886        double mag = 1.0/Math.sqrt(m1.m00*m1.m00 + m1.m10*m1.m10 + m1.m20*m1.m20);
1887        m00 = m1.m00*mag;
1888        m10 = m1.m10*mag;
1889        m20 = m1.m20*mag;
1890
1891        mag = 1.0/Math.sqrt(m1.m01*m1.m01 + m1.m11*m1.m11 + m1.m21*m1.m21);
1892        m01 = m1.m01*mag;
1893        m11 = m1.m11*mag;
1894        m21 = m1.m21*mag;
1895
1896        m02 = m10*m21 - m11*m20;
1897        m12 = m01*m20 - m00*m21;
1898        m22 = m00*m11 - m01*m10;
1899     }
1900
1901    /**
1902      * Returns true if all of the data members of Matrix3d m1 are
1903      * equal to the corresponding data members in this Matrix3d.
1904      * @param m1  the matrix with which the comparison is made
1905      * @return  true or false
1906      */
1907     public boolean equals(Matrix3d m1)
1908     {
1909       try {
1910          return(this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02
1911             && this.m10 == m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12
1912             && this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1.m22);
1913       }
1914       catch (NullPointerException e2) { return false; }
1915
1916     }
1917
1918    /**
1919      * Returns true if the Object t1 is of type Matrix3d and all of the
1920      * data members of t1 are equal to the corresponding data members in
1921      * this Matrix3d.
1922      * @param t1  the matrix with which the comparison is made
1923      * @return  true or false
1924      */
1925     public boolean equals(Object t1)
1926     {
1927         try {
1928            Matrix3d m2 = (Matrix3d) t1;
1929            return(this.m00 == m2.m00 && this.m01 == m2.m01 && this.m02 == m2.m02
1930              && this.m10 == m2.m10 && this.m11 == m2.m11 && this.m12 == m2.m12
1931              && this.m20 == m2.m20 && this.m21 == m2.m21 && this.m22 == m2.m22);
1932         }
1933         catch (ClassCastException   e1) { return false; }
1934         catch (NullPointerException e2) { return false; }
1935
1936     }
1937
1938    /**
1939      * Returns true if the L-infinite distance between this matrix
1940      * and matrix m1 is less than or equal to the epsilon parameter,
1941      * otherwise returns false.  The L-infinite
1942      * distance is equal to
1943      * MAX[i=0,1,2 ; j=0,1,2 ; abs(this.m(i,j) - m1.m(i,j)]
1944      * @param m1  the matrix to be compared to this matrix
1945      * @param epsilon  the threshold value
1946      */
1947     public boolean epsilonEquals(Matrix3d m1, double epsilon)
1948     {
1949        double diff;
1950
1951        diff = m00 - m1.m00;
1952        if((diff<0?-diff:diff) > epsilon) return false;
1953
1954        diff = m01 - m1.m01;
1955        if((diff<0?-diff:diff) > epsilon) return false;
1956
1957        diff = m02 - m1.m02;
1958        if((diff<0?-diff:diff) > epsilon) return false;
1959
1960        diff = m10 - m1.m10;
1961        if((diff<0?-diff:diff) > epsilon) return false;
1962
1963        diff = m11 - m1.m11;
1964        if((diff<0?-diff:diff) > epsilon) return false;
1965
1966        diff = m12 - m1.m12;
1967        if((diff<0?-diff:diff) > epsilon) return false;
1968
1969        diff = m20 - m1.m20;
1970        if((diff<0?-diff:diff) > epsilon) return false;
1971
1972        diff = m21 - m1.m21;
1973        if((diff<0?-diff:diff) > epsilon) return false;
1974
1975        diff = m22 - m1.m22;
1976        if((diff<0?-diff:diff) > epsilon) return false;
1977
1978        return true;
1979     }
1980
1981
1982     /**
1983      * Returns a hash code value based on the data values in this
1984      * object.  Two different Matrix3d objects with identical data values
1985      * (i.e., Matrix3d.equals returns true) will return the same hash
1986      * code value.  Two objects with different data members may return the
1987      * same hash value, although this is not likely.
1988      * @return the integer hash code value
1989      */
1990     public int hashCode() {
1991         long bits = 1L;
1992         bits = 31L * bits + VecMathUtil.doubleToLongBits(m00);
1993         bits = 31L * bits + VecMathUtil.doubleToLongBits(m01);
1994         bits = 31L * bits + VecMathUtil.doubleToLongBits(m02);
1995         bits = 31L * bits + VecMathUtil.doubleToLongBits(m10);
1996         bits = 31L * bits + VecMathUtil.doubleToLongBits(m11);
1997         bits = 31L * bits + VecMathUtil.doubleToLongBits(m12);
1998         bits = 31L * bits + VecMathUtil.doubleToLongBits(m20);
1999         bits = 31L * bits + VecMathUtil.doubleToLongBits(m21);
2000         bits = 31L * bits + VecMathUtil.doubleToLongBits(m22);
2001         return (int) (bits ^ (bits >> 32));
2002     }
2003
2004
2005   /**
2006     *  Sets this matrix to all zeros.
2007     */
2008    public final void setZero()
2009    {
2010         m00 = 0.0;
2011         m01 = 0.0;
2012         m02 = 0.0;
2013
2014         m10 = 0.0;
2015         m11 = 0.0;
2016         m12 = 0.0;
2017
2018         m20 = 0.0;
2019         m21 = 0.0;
2020         m22 = 0.0;
2021
2022    }
2023
2024    /**
2025      * Negates the value of this matrix: this = -this.
2026      */
2027     public final void negate()
2028     {
2029         this.m00 = -this.m00;
2030         this.m01 = -this.m01;
2031         this.m02 = -this.m02;
2032
2033         this.m10 = -this.m10;
2034         this.m11 = -this.m11;
2035         this.m12 = -this.m12;
2036
2037         this.m20 = -this.m20;
2038         this.m21 = -this.m21;
2039         this.m22 = -this.m22;
2040
2041     }
2042
2043    /**
2044      *  Sets the value of this matrix equal to the negation of
2045      *  of the Matrix3d parameter.
2046      *  @param m1  the source matrix
2047      */
2048     public final void negate(Matrix3d m1)
2049     {
2050         this.m00 = -m1.m00;
2051         this.m01 = -m1.m01;
2052         this.m02 = -m1.m02;
2053
2054         this.m10 = -m1.m10;
2055         this.m11 = -m1.m11;
2056         this.m12 = -m1.m12;
2057
2058         this.m20 = -m1.m20;
2059         this.m21 = -m1.m21;
2060         this.m22 = -m1.m22;
2061
2062     }
2063
2064    /**
2065      * Multiply this matrix by the tuple t and place the result
2066      * back into the tuple (t = this*t).
2067      * @param t  the tuple to be multiplied by this matrix and then replaced
2068      */
2069     public final void transform(Tuple3d t) {
2070      double x,y,z;
2071      x = m00* t.x + m01*t.y + m02*t.z;
2072      y = m10* t.x + m11*t.y + m12*t.z;
2073      z = m20* t.x + m21*t.y + m22*t.z;
2074      t.set(x,y,z);
2075     }
2076
2077    /**
2078      * Multiply this matrix by the tuple t and and place the result
2079      * into the tuple "result" (result = this*t).
2080      * @param t  the tuple to be multiplied by this matrix
2081      * @param result  the tuple into which the product is placed
2082      */
2083     public final void transform(Tuple3d t, Tuple3d result) {
2084      double x,y,z;
2085         x = m00* t.x + m01*t.y + m02*t.z;
2086         y = m10* t.x + m11*t.y + m12*t.z;
2087         result.z = m20* t.x + m21*t.y + m22*t.z;
2088         result.x = x;
2089         result.y = y;
2090     }
2091
2092     /**
2093      * perform SVD (if necessary to get rotational component
2094      */
2095     final void getScaleRotate(double scales[], double rots[]) {
2096
2097         double[]    tmp = new double[9];  // scratch matrix
2098
2099         tmp[0] = m00;
2100         tmp[1] = m01;
2101         tmp[2] = m02;
2102
2103         tmp[3] = m10;
2104         tmp[4] = m11;
2105         tmp[5] = m12;
2106
2107         tmp[6] = m20;
2108         tmp[7] = m21;
2109         tmp[8] = m22;
2110         compute_svd( tmp, scales, rots);
2111
2112         return;
2113     }
2114
2115     static void compute_svd( double[] m, double[] outScale, double[] outRot) {
2116         int i,j;
2117         double g,scale;
2118         double[] u1 = new double[9];
2119         double[] v1 = new double[9];
2120         double[] t1 = new double[9];
2121         double[] t2 = new double[9];
2122
2123         double[] tmp = t1;
2124         double[] single_values = t2;
2125
2126         double[] rot = new double[9];
2127         double[] e = new double[3];
2128         double[] scales = new double[3];
2129
2130         int converged, negCnt=0;
2131         double cs,sn;
2132         double c1,c2,c3,c4;
2133         double s1,s2,s3,s4;
2134         double cl1,cl2,cl3;
2135
2136
2137         for(i=0; i<9; i++)
2138             rot[i] = m[i];
2139
2140         // u1
2141
2142         if( m[3]*m[3] < EPS ) {
2143             u1[0] = 1.0; u1[1] = 0.0; u1[2] = 0.0;
2144             u1[3] = 0.0; u1[4] = 1.0; u1[5] = 0.0;
2145             u1[6] = 0.0; u1[7] = 0.0; u1[8] = 1.0;
2146         } else if( m[0]*m[0] < EPS ) {
2147             tmp[0] = m[0];
2148             tmp[1] = m[1];
2149             tmp[2] = m[2];
2150             m[0] = m[3];
2151             m[1] = m[4];
2152             m[2] = m[5];
2153
2154             m[3] = -tmp[0]; // zero
2155             m[4] = -tmp[1];
2156             m[5] = -tmp[2];
2157
2158             u1[0] =  0.0; u1[1] = 1.0;  u1[2] = 0.0;
2159             u1[3] = -1.0; u1[4] = 0.0;  u1[5] = 0.0;
2160             u1[6] =  0.0; u1[7] = 0.0;  u1[8] = 1.0;
2161         } else {
2162             g = 1.0/Math.sqrt(m[0]*m[0] + m[3]*m[3]);
2163             c1 = m[0]*g;
2164             s1 = m[3]*g;
2165             tmp[0] = c1*m[0] + s1*m[3];
2166             tmp[1] = c1*m[1] + s1*m[4];
2167             tmp[2] = c1*m[2] + s1*m[5];
2168
2169             m[3] = -s1*m[0] + c1*m[3]; // zero
2170             m[4] = -s1*m[1] + c1*m[4];
2171             m[5] = -s1*m[2] + c1*m[5];
2172
2173             m[0] = tmp[0];
2174             m[1] = tmp[1];
2175             m[2] = tmp[2];
2176             u1[0] = c1;  u1[1] = s1;  u1[2] = 0.0;
2177             u1[3] = -s1; u1[4] = c1;  u1[5] = 0.0;
2178             u1[6] = 0.0; u1[7] = 0.0; u1[8] = 1.0;
2179         }
2180
2181         // u2
2182
2183         if( m[6]*m[6] < EPS  ) {
2184         } else if( m[0]*m[0] < EPS ){
2185             tmp[0] = m[0];
2186             tmp[1] = m[1];
2187             tmp[2] = m[2];
2188             m[0] = m[6];
2189             m[1] = m[7];
2190             m[2] = m[8];
2191
2192             m[6] = -tmp[0]; // zero
2193             m[7] = -tmp[1];
2194             m[8] = -tmp[2];
2195
2196             tmp[0] = u1[0];
2197             tmp[1] = u1[1];
2198             tmp[2] = u1[2];
2199             u1[0] = u1[6];
2200             u1[1] = u1[7];
2201             u1[2] = u1[8];
2202
2203             u1[6] = -tmp[0]; // zero
2204             u1[7] = -tmp[1];
2205             u1[8] = -tmp[2];
2206         } else {
2207             g = 1.0/Math.sqrt(m[0]*m[0] + m[6]*m[6]);
2208             c2 = m[0]*g;
2209             s2 = m[6]*g;
2210             tmp[0] = c2*m[0] + s2*m[6];
2211             tmp[1] = c2*m[1] + s2*m[7];
2212             tmp[2] = c2*m[2] + s2*m[8];
2213
2214             m[6] = -s2*m[0] + c2*m[6];
2215             m[7] = -s2*m[1] + c2*m[7];
2216             m[8] = -s2*m[2] + c2*m[8];
2217             m[0] = tmp[0];
2218             m[1] = tmp[1];
2219             m[2] = tmp[2];
2220
2221             tmp[0] = c2*u1[0];
2222             tmp[1] = c2*u1[1];
2223             u1[2]  = s2;
2224
2225             tmp[6] = -u1[0]*s2;
2226             tmp[7] = -u1[1]*s2;
2227             u1[8] = c2;
2228             u1[0] = tmp[0];
2229             u1[1] = tmp[1];
2230             u1[6] = tmp[6];
2231             u1[7] = tmp[7];
2232         }
2233
2234         // v1
2235
2236         if( m[2]*m[2] < EPS ) {
2237             v1[0] = 1.0; v1[1] = 0.0; v1[2] = 0.0;
2238             v1[3] = 0.0; v1[4] = 1.0; v1[5] = 0.0;
2239             v1[6] = 0.0; v1[7] = 0.0; v1[8] = 1.0;
2240         } else if( m[1]*m[1] < EPS ) {
2241             tmp[2] = m[2];
2242             tmp[5] = m[5];
2243             tmp[8] = m[8];
2244             m[2] = -m[1];
2245             m[5] = -m[4];
2246             m[8] = -m[7];
2247
2248             m[1] = tmp[2]; // zero
2249             m[4] = tmp[5];
2250             m[7] = tmp[8];
2251
2252             v1[0] =  1.0; v1[1] = 0.0;  v1[2] = 0.0;
2253             v1[3] =  0.0; v1[4] = 0.0;  v1[5] =-1.0;
2254             v1[6] =  0.0; v1[7] = 1.0;  v1[8] = 0.0;
2255         } else {
2256             g = 1.0/Math.sqrt(m[1]*m[1] + m[2]*m[2]);
2257             c3 = m[1]*g;
2258             s3 = m[2]*g;
2259             tmp[1] = c3*m[1] + s3*m[2];  // can assign to m[1]?
2260             m[2] =-s3*m[1] + c3*m[2];  // zero
2261             m[1] = tmp[1];
2262
2263             tmp[4] = c3*m[4] + s3*m[5];
2264             m[5] =-s3*m[4] + c3*m[5];
2265             m[4] = tmp[4];
2266
2267             tmp[7] = c3*m[7] + s3*m[8];
2268             m[8] =-s3*m[7] + c3*m[8];
2269             m[7] = tmp[7];
2270
2271             v1[0] = 1.0; v1[1] = 0.0; v1[2] = 0.0;
2272             v1[3] = 0.0; v1[4] =  c3; v1[5] = -s3;
2273             v1[6] = 0.0; v1[7] =  s3; v1[8] =  c3;
2274         }
2275
2276         // u3
2277
2278         if( m[7]*m[7] < EPS ) {
2279         } else if( m[4]*m[4] < EPS ) {
2280             tmp[3] = m[3];
2281             tmp[4] = m[4];
2282             tmp[5] = m[5];
2283             m[3] = m[6];   // zero
2284             m[4] = m[7];
2285             m[5] = m[8];
2286
2287             m[6] = -tmp[3]; // zero
2288             m[7] = -tmp[4]; // zero
2289             m[8] = -tmp[5];
2290
2291             tmp[3] = u1[3];
2292             tmp[4] = u1[4];
2293             tmp[5] = u1[5];
2294             u1[3] = u1[6];
2295             u1[4] = u1[7];
2296             u1[5] = u1[8];
2297
2298             u1[6] = -tmp[3]; // zero
2299             u1[7] = -tmp[4];
2300             u1[8] = -tmp[5];
2301
2302         } else {
2303             g = 1.0/Math.sqrt(m[4]*m[4] + m[7]*m[7]);
2304             c4 = m[4]*g;
2305             s4 = m[7]*g;
2306             tmp[3] = c4*m[3] + s4*m[6];
2307             m[6] =-s4*m[3] + c4*m[6];  // zero
2308             m[3] = tmp[3];
2309
2310             tmp[4] = c4*m[4] + s4*m[7];
2311             m[7] =-s4*m[4] + c4*m[7];
2312             m[4] = tmp[4];
2313
2314             tmp[5] = c4*m[5] + s4*m[8];
2315             m[8] =-s4*m[5] + c4*m[8];
2316             m[5] = tmp[5];
2317
2318             tmp[3] = c4*u1[3] + s4*u1[6];
2319             u1[6] =-s4*u1[3] + c4*u1[6];
2320             u1[3] = tmp[3];
2321
2322             tmp[4] = c4*u1[4] + s4*u1[7];
2323             u1[7] =-s4*u1[4] + c4*u1[7];
2324             u1[4] = tmp[4];
2325
2326             tmp[5] = c4*u1[5] + s4*u1[8];
2327             u1[8] =-s4*u1[5] + c4*u1[8];
2328             u1[5] = tmp[5];
2329         }
2330
2331         single_values[0] = m[0];
2332         single_values[1] = m[4];
2333         single_values[2] = m[8];
2334         e[0] = m[1];
2335         e[1] = m[5];
2336
2337         if( e[0]*e[0]<EPS  && e[1]*e[1]<EPS ) {
2338
2339         } else {
2340             compute_qr( single_values, e, u1, v1);
2341         }
2342
2343         scales[0] = single_values[0];
2344         scales[1] = single_values[1];
2345         scales[2] = single_values[2];
2346
2347
2348         // Do some optimization here. If scale is unity, simply return the rotation matric.
2349         if(almostEqual(Math.abs(scales[0]), 1.0) &&
2350            almostEqual(Math.abs(scales[1]), 1.0) &&
2351            almostEqual(Math.abs(scales[2]), 1.0)) {
2352             //  System.out.println("Scale components almost to 1.0");
2353
2354             for(i=0;i<3;i++)
2355                 if(scales[i]<0.0)
2356                     negCnt++;
2357
2358             if((negCnt==0)||(negCnt==2)) {
2359                 //System.out.println("Optimize!!");
2360                 outScale[0] = outScale[1] = outScale[2] = 1.0;
2361                 for(i=0;i<9;i++)
2362                     outRot[i] = rot[i];
2363
2364                 return;
2365             }
2366         }
2367
2368
2369         transpose_mat(u1, t1);
2370         transpose_mat(v1, t2);
2371
2372         /*
2373           System.out.println("t1 is \n" + t1);
2374           System.out.println("t1="+t1[0]+" "+t1[1]+" "+t1[2]);
2375           System.out.println("t1="+t1[3]+" "+t1[4]+" "+t1[5]);
2376           System.out.println("t1="+t1[6]+" "+t1[7]+" "+t1[8]);
2377
2378           System.out.println("t2 is \n" + t2);
2379           System.out.println("t2="+t2[0]+" "+t2[1]+" "+t2[2]);
2380           System.out.println("t2="+t2[3]+" "+t2[4]+" "+t2[5]);
2381           System.out.println("t2="+t2[6]+" "+t2[7]+" "+t2[8]);
2382           */
2383
2384         svdReorder( m, t1, t2, scales, outRot, outScale);
2385
2386     }
2387
2388     static void svdReorder( double[] m, double[] t1, double[] t2, double[] scales,
2389                             double[] outRot, double[] outScale) {
2390
2391         int[] out = new int[3];
2392         int[] in = new int[3];
2393         int in0, in1, in2, index, i;
2394         double[] mag = new double[3];
2395         double[] rot = new double[9];
2396
2397
2398         // check for rotation information in the scales
2399         if(scales[0] < 0.0 ) {   // move the rotation info to rotation matrix
2400             scales[0] = -scales[0];
2401             t2[0] = -t2[0];
2402             t2[1] = -t2[1];
2403             t2[2] = -t2[2];
2404         }
2405         if(scales[1] < 0.0 ) {   // move the rotation info to rotation matrix
2406             scales[1] = -scales[1];
2407             t2[3] = -t2[3];
2408             t2[4] = -t2[4];
2409             t2[5] = -t2[5];
2410         }
2411         if(scales[2] < 0.0 ) {   // move the rotation info to rotation matrix
2412             scales[2] = -scales[2];
2413             t2[6] = -t2[6];
2414             t2[7] = -t2[7];
2415             t2[8] = -t2[8];
2416         }
2417
2418         mat_mul(t1,t2,rot);
2419
2420         // check for equal scales case  and do not reorder
2421         if(almostEqual(Math.abs(scales[0]), Math.abs(scales[1])) &&
2422            almostEqual(Math.abs(scales[1]), Math.abs(scales[2]))   ){
2423             for(i=0;i<9;i++){
2424                 outRot[i] = rot[i];
2425             }
2426             for(i=0;i<3;i++){
2427                 outScale[i] = scales[i];
2428             }
2429
2430         }else {
2431
2432             // sort the order of the results of SVD
2433             if( scales[0] > scales[1]) {
2434                 if( scales[0] > scales[2] ) {
2435                     if( scales[2] > scales[1] ) {
2436                         out[0] = 0; out[1] = 2; out[2] = 1; // xzy
2437                     } else {
2438                         out[0] = 0; out[1] = 1; out[2] = 2; // xyz
2439                     }
2440                 } else {
2441                     out[0] = 2; out[1] = 0; out[2] = 1; // zxy
2442                 }
2443             } else {  // y > x
2444                 if( scales[1] > scales[2] ) {
2445                     if( scales[2] > scales[0] ) {
2446                         out[0] = 1; out[1] = 2; out[2] = 0; // yzx
2447                     } else {
2448                         out[0] = 1; out[1] = 0; out[2] = 2; // yxz
2449                     }
2450                 } else  {
2451                     out[0] = 2; out[1] = 1; out[2] = 0; // zyx
2452                 }
2453             }
2454
2455             /*
2456                 System.out.println("\nscales="+scales[0]+" "+scales[1]+" "+scales[2]);
2457                 System.out.println("\nrot="+rot[0]+" "+rot[1]+" "+rot[2]);
2458                 System.out.println("rot="+rot[3]+" "+rot[4]+" "+rot[5]);
2459                 System.out.println("rot="+rot[6]+" "+rot[7]+" "+rot[8]);
2460                 */
2461
2462             // sort the order of the input matrix
2463             mag[0] = (m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
2464             mag[1] = (m[3]*m[3] + m[4]*m[4] + m[5]*m[5]);
2465             mag[2] = (m[6]*m[6] + m[7]*m[7] + m[8]*m[8]);
2466
2467             if( mag[0] > mag[1]) {
2468                 if( mag[0] > mag[2] ) {
2469                     if( mag[2] > mag[1] )  {
2470                         // 0 - 2 - 1
2471                         in0 = 0; in2 = 1; in1 = 2;// xzy
2472                     } else {
2473                         // 0 - 1 - 2
2474                         in0 = 0; in1 = 1; in2 = 2; // xyz
2475                     }
2476                 } else {
2477                     // 2 - 0 - 1
2478                     in2 = 0; in0 = 1; in1 = 2;  // zxy
2479                 }
2480             } else {  // y > x   1>0
2481                 if( mag[1] > mag[2] ) {
2482                     if( mag[2] > mag[0] )  {
2483                         // 1 - 2 - 0
2484                         in1 = 0; in2 = 1; in0 = 2; // yzx
2485                     } else {
2486                         // 1 - 0 - 2
2487                         in1 = 0; in0 = 1; in2 = 2; // yxz
2488                     }
2489                 } else  {
2490                     // 2 - 1 - 0
2491                     in2 = 0; in1 = 1; in0 = 2; // zyx
2492                 }
2493             }
2494
2495
2496             index = out[in0];
2497             outScale[0] = scales[index];
2498
2499             index = out[in1];
2500             outScale[1] = scales[index];
2501
2502             index = out[in2];
2503             outScale[2] = scales[index];
2504
2505
2506             index = out[in0];
2507             outRot[0] = rot[index];
2508
2509             index = out[in0]+3;
2510             outRot[0+3] = rot[index];
2511
2512             index = out[in0]+6;
2513             outRot[0+6] = rot[index];
2514
2515             index = out[in1];
2516             outRot[1] = rot[index];
2517
2518             index = out[in1]+3;
2519             outRot[1+3] = rot[index];
2520
2521             index = out[in1]+6;
2522             outRot[1+6] = rot[index];
2523
2524             index = out[in2];
2525             outRot[2] = rot[index];
2526
2527             index = out[in2]+3;
2528             outRot[2+3] = rot[index];
2529
2530             index = out[in2]+6;
2531             outRot[2+6] = rot[index];
2532         }
2533     }
2534
2535     static  int compute_qr( double[] s, double[] e, double[] u, double[] v) {
2536
2537         int i,j,k;
2538         boolean converged;
2539         double shift,ssmin,ssmax,r;
2540         double[]   cosl  = new double[2];
2541         double[]   cosr  = new double[2];
2542         double[]   sinl  = new double[2];
2543         double[]   sinr  = new double[2];
2544         double[]   m = new double[9];
2545
2546         double utemp,vtemp;
2547         double f,g;
2548
2549         final int MAX_INTERATIONS = 10;
2550         final double CONVERGE_TOL = 4.89E-15;
2551
2552         double c_b48 = 1.;
2553         double c_b71 = -1.;
2554         int first;
2555         converged = false;
2556
2557
2558         first = 1;
2559
2560         if( Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) converged = true;
2561
2562         for(k=0;k<MAX_INTERATIONS && !converged;k++) {
2563             shift = compute_shift( s[1], e[1], s[2]);
2564             f = (Math.abs(s[0]) - shift) * (d_sign(c_b48, s[0]) + shift/s[0]);
2565             g = e[0];
2566             r = compute_rot(f, g, sinr, cosr,  0, first);
2567             f = cosr[0] * s[0] + sinr[0] * e[0];
2568             e[0] = cosr[0] * e[0] - sinr[0] * s[0];
2569             g = sinr[0] * s[1];
2570             s[1] = cosr[0] * s[1];
2571
2572             r = compute_rot(f, g, sinl, cosl, 0, first);
2573             first = 0;
2574             s[0] = r;
2575             f = cosl[0] * e[0] + sinl[0] * s[1];
2576             s[1] = cosl[0] * s[1] - sinl[0] * e[0];
2577             g = sinl[0] * e[1];
2578             e[1] =  cosl[0] * e[1];
2579
2580             r = compute_rot(f, g, sinr, cosr, 1, first);
2581             e[0] = r;
2582             f = cosr[1] * s[1] + sinr[1] * e[1];
2583             e[1] = cosr[1] * e[1] - sinr[1] * s[1];
2584             g = sinr[1] * s[2];
2585             s[2] = cosr[1] * s[2];
2586
2587             r = compute_rot(f, g, sinl, cosl, 1, first);
2588             s[1] = r;
2589             f = cosl[1] * e[1] + sinl[1] * s[2];
2590             s[2] = cosl[1] * s[2] - sinl[1] * e[1];
2591             e[1] = f;
2592
2593             // update u  matrices
2594             utemp = u[0];
2595             u[0] = cosl[0]*utemp + sinl[0]*u[3];
2596             u[3] = -sinl[0]*utemp + cosl[0]*u[3];
2597             utemp = u[1];
2598             u[1] = cosl[0]*utemp + sinl[0]*u[4];
2599             u[4] = -sinl[0]*utemp + cosl[0]*u[4];
2600             utemp = u[2];
2601             u[2] = cosl[0]*utemp + sinl[0]*u[5];
2602             u[5] = -sinl[0]*utemp + cosl[0]*u[5];
2603
2604             utemp = u[3];
2605             u[3] = cosl[1]*utemp + sinl[1]*u[6];
2606             u[6] = -sinl[1]*utemp + cosl[1]*u[6];
2607             utemp = u[4];
2608             u[4] = cosl[1]*utemp + sinl[1]*u[7];
2609             u[7] = -sinl[1]*utemp + cosl[1]*u[7];
2610             utemp = u[5];
2611             u[5] = cosl[1]*utemp + sinl[1]*u[8];
2612             u[8] = -sinl[1]*utemp + cosl[1]*u[8];
2613
2614             // update v  matrices
2615
2616             vtemp = v[0];
2617             v[0] = cosr[0]*vtemp + sinr[0]*v[1];
2618             v[1] = -sinr[0]*vtemp + cosr[0]*v[1];
2619             vtemp = v[3];
2620             v[3] = cosr[0]*vtemp + sinr[0]*v[4];
2621             v[4] = -sinr[0]*vtemp + cosr[0]*v[4];
2622             vtemp = v[6];
2623             v[6] = cosr[0]*vtemp + sinr[0]*v[7];
2624             v[7] = -sinr[0]*vtemp + cosr[0]*v[7];
2625
2626             vtemp = v[1];
2627             v[1] = cosr[1]*vtemp + sinr[1]*v[2];
2628             v[2] = -sinr[1]*vtemp + cosr[1]*v[2];
2629             vtemp = v[4];
2630             v[4] = cosr[1]*vtemp + sinr[1]*v[5];
2631             v[5] = -sinr[1]*vtemp + cosr[1]*v[5];
2632             vtemp = v[7];
2633             v[7] = cosr[1]*vtemp + sinr[1]*v[8];
2634             v[8] = -sinr[1]*vtemp + cosr[1]*v[8];
2635
2636
2637    m[0] = s[0];  m[1] = e[0]; m[2] = 0.0;
2638    m[3] =  0.0;  m[4] = s[1]; m[5] =e[1];
2639    m[6] =  0.0;  m[7] =  0.0; m[8] =s[2];
2640
2641       if( Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) converged = true;
2642    }
2643
2644    if( Math.abs(e[1]) < CONVERGE_TOL ) {
2645        compute_2X2( s[0],e[0],s[1],s,sinl,cosl,sinr,cosr, 0);
2646
2647        utemp = u[0];
2648        u[0] = cosl[0]*utemp + sinl[0]*u[3];
2649        u[3] = -sinl[0]*utemp + cosl[0]*u[3];
2650        utemp = u[1];
2651        u[1] = cosl[0]*utemp + sinl[0]*u[4];
2652        u[4] = -sinl[0]*utemp + cosl[0]*u[4];
2653        utemp = u[2];
2654        u[2] = cosl[0]*utemp + sinl[0]*u[5];
2655        u[5] = -sinl[0]*utemp + cosl[0]*u[5];
2656
2657        // update v  matrices
2658
2659        vtemp = v[0];
2660        v[0] = cosr[0]*vtemp + sinr[0]*v[1];
2661        v[1] = -sinr[0]*vtemp + cosr[0]*v[1];
2662        vtemp = v[3];
2663        v[3] = cosr[0]*vtemp + sinr[0]*v[4];
2664        v[4] = -sinr[0]*vtemp + cosr[0]*v[4];
2665        vtemp = v[6];
2666        v[6] = cosr[0]*vtemp + sinr[0]*v[7];
2667        v[7] = -sinr[0]*vtemp + cosr[0]*v[7];
2668    } else {
2669        compute_2X2( s[1],e[1],s[2],s,sinl,cosl,sinr,cosr,1);
2670
2671        utemp = u[3];
2672        u[3] = cosl[0]*utemp + sinl[0]*u[6];
2673        u[6] = -sinl[0]*utemp + cosl[0]*u[6];
2674        utemp = u[4];
2675        u[4] = cosl[0]*utemp + sinl[0]*u[7];
2676        u[7] = -sinl[0]*utemp + cosl[0]*u[7];
2677        utemp = u[5];
2678        u[5] = cosl[0]*utemp + sinl[0]*u[8];
2679        u[8] = -sinl[0]*utemp + cosl[0]*u[8];
2680
2681        // update v  matrices
2682
2683        vtemp = v[1];
2684        v[1] = cosr[0]*vtemp + sinr[0]*v[2];
2685        v[2] = -sinr[0]*vtemp + cosr[0]*v[2];
2686        vtemp = v[4];
2687        v[4] = cosr[0]*vtemp + sinr[0]*v[5];
2688        v[5] = -sinr[0]*vtemp + cosr[0]*v[5];
2689        vtemp = v[7];
2690        v[7] = cosr[0]*vtemp + sinr[0]*v[8];
2691        v[8] = -sinr[0]*vtemp + cosr[0]*v[8];
2692    }
2693
2694        return(0);
2695 }
2696 static double max( double a, double b) {
2697     if( a > b)
2698       return( a);
2699     else
2700       return( b);
2701 }
2702 static double min( double a, double b) {
2703     if( a < b)
2704       return( a);
2705     else
2706       return( b);
2707 }
2708 static double d_sign(double a, double b) {
2709 double x;
2710 x = (a >= 0 ? a : - a);
2711 return( b >= 0 ? x : -x);
2712 }
2713
2714 static double compute_shift( double f, double g, double h) {
2715     double d__1, d__2;
2716     double fhmn, fhmx, c, fa, ga, ha, as, at, au;
2717     double ssmin;
2718
2719     fa = Math.abs(f);
2720     ga = Math.abs(g);
2721     ha = Math.abs(h);
2722     fhmn = min(fa,ha);
2723     fhmx = max(fa,ha);
2724     if (fhmn == 0.) {
2725         ssmin = 0.;
2726         if (fhmx == 0.) {
2727         } else {
2728             d__1 = min(fhmx,ga) / max(fhmx,ga);
2729         }
2730     } else {
2731         if (ga < fhmx) {
2732             as = fhmn / fhmx + 1.;
2733             at = (fhmx - fhmn) / fhmx;
2734             d__1 = ga / fhmx;
2735             au = d__1 * d__1;
2736             c = 2. / (Math.sqrt(as * as + au) + Math.sqrt(at * at + au));
2737             ssmin = fhmn * c;
2738         } else {
2739             au = fhmx / ga;
2740             if (au == 0.) {
2741                 ssmin = fhmn * fhmx / ga;
2742             } else {
2743                 as = fhmn / fhmx + 1.;
2744                 at = (fhmx - fhmn) / fhmx;
2745                 d__1 = as * au;
2746                 d__2 = at * au;
2747                 c = 1. / (Math.sqrt(d__1 * d__1 + 1.) + Math.sqrt(d__2 * d__2 + 1.));
2748                 ssmin = fhmn * c * au;
2749                 ssmin += ssmin;
2750             }
2751         }
2752     }
2753
2754     return(ssmin);
2755 }
2756 static int compute_2X2( double f, double g, double h, double[] single_values,
2757                 double[] snl, double[] csl, double[] snr, double[] csr, int index)  {
2758
2759     double c_b3 = 2.;
2760     double c_b4 = 1.;
2761
2762     double d__1;
2763     int pmax;
2764     double temp;
2765     boolean swap;
2766     double a, d, l, m, r, s, t, tsign, fa, ga, ha;
2767     double ft, gt, ht, mm;
2768     boolean gasmal;
2769     double tt, clt, crt, slt, srt;
2770     double ssmin,ssmax;
2771
2772     ssmax = single_values[0];
2773     ssmin = single_values[1];
2774     clt = 0.0;
2775     crt = 0.0;
2776     slt = 0.0;
2777     srt = 0.0;
2778     tsign = 0.0;
2779
2780     ft = f;
2781     fa = Math.abs(ft);
2782     ht = h;
2783     ha = Math.abs(h);
2784
2785     pmax = 1;
2786     if( ha > fa)
2787        swap = true;
2788     else
2789        swap = false;
2790
2791     if (swap) {
2792         pmax = 3;
2793         temp = ft;
2794         ft = ht;
2795         ht = temp;
2796         temp = fa;
2797         fa = ha;
2798         ha = temp;
2799
2800     }
2801     gt = g;
2802     ga = Math.abs(gt);
2803     if (ga == 0.) {
2804
2805         single_values[1] = ha;
2806         single_values[0] = fa;
2807         clt = 1.;
2808         crt = 1.;
2809         slt = 0.;
2810         srt = 0.;
2811     } else {
2812         gasmal = true;
2813
2814        if (ga > fa) {
2815             pmax = 2;
2816             if (fa / ga < EPS) {
2817
2818                 gasmal = false;
2819                 ssmax = ga;
2820                 if (ha > 1.) {
2821                     ssmin = fa / (ga / ha);
2822                 } else {
2823                     ssmin = fa / ga * ha;
2824                 }
2825                 clt = 1.;
2826                 slt = ht / gt;
2827                 srt = 1.;
2828                 crt = ft / gt;
2829             }
2830         }
2831         if (gasmal) {
2832
2833             d = fa - ha;
2834             if (d == fa) {
2835
2836                 l = 1.;
2837             } else {
2838                 l = d / fa;
2839             }
2840
2841             m = gt / ft;
2842
2843             t = 2. - l;
2844
2845             mm = m * m;
2846             tt = t * t;
2847             s = Math.sqrt(tt + mm);
2848
2849             if (l == 0.) {
2850                 r = Math.abs(m);
2851             } else {
2852                 r = Math.sqrt(l * l + mm);
2853             }
2854
2855             a = (s + r) * .5;
2856
2857        if (ga > fa) {
2858             pmax = 2;
2859             if (fa / ga < EPS) {
2860
2861                 gasmal = false;
2862                 ssmax = ga;
2863                 if (ha > 1.) {
2864                     ssmin = fa / (ga / ha);
2865                 } else {
2866                     ssmin = fa / ga * ha;
2867                 }
2868                 clt = 1.;
2869                 slt = ht / gt;
2870                 srt = 1.;
2871                 crt = ft / gt;
2872             }
2873         }
2874         if (gasmal) {
2875
2876             d = fa - ha;
2877             if (d == fa) {
2878
2879                 l = 1.;
2880             } else {
2881                 l = d / fa;
2882             }
2883
2884             m = gt / ft;
2885
2886             t = 2. - l;
2887
2888             mm = m * m;
2889             tt = t * t;
2890             s = Math.sqrt(tt + mm);
2891
2892             if (l == 0.) {
2893                 r = Math.abs(m);
2894             } else {
2895                 r = Math.sqrt(l * l + mm);
2896             }
2897
2898             a = (s + r) * .5;
2899
2900
2901             ssmin = ha / a;
2902             ssmax = fa * a;
2903             if (mm == 0.) {
2904
2905                 if (l == 0.) {
2906                     t = d_sign(c_b3, ft) * d_sign(c_b4, gt);
2907                 } else {
2908                     t = gt / d_sign(d, ft) + m / t;
2909                 }
2910             } else {
2911                 t = (m / (s + t) + m / (r + l)) * (a + 1.);
2912             }
2913             l = Math.sqrt(t * t + 4.);
2914             crt = 2. / l;
2915             srt = t / l;
2916             clt = (crt + srt * m) / a;
2917             slt = ht / ft * srt / a;
2918         }
2919     }
2920     if (swap) {
2921         csl[0] = srt;
2922         snl[0] = crt;
2923         csr[0] = slt;
2924         snr[0] = clt;
2925     } else {
2926         csl[0] = clt;
2927         snl[0] = slt;
2928         csr[0] = crt;
2929         snr[0] = srt;
2930     }
2931
2932     if (pmax == 1) {
2933         tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, f);
2934     }
2935     if (pmax == 2) {
2936         tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, g);
2937     }
2938     if (pmax == 3) {
2939         tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0]) * d_sign(c_b4, h);
2940     }
2941     single_values[index] = d_sign(ssmax, tsign);
2942     d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h);
2943     single_values[index+1] = d_sign(ssmin, d__1);
2944
2945
2946    }
2947     return 0;
2948  }
2949   static double compute_rot( double f, double g, double[] sin, double[] cos, int index, int first) {
2950     int i__1;
2951     double d__1, d__2;
2952     double cs,sn;
2953     int i;
2954     double scale;
2955     int count;
2956     double f1, g1;
2957     double r;
2958     final double safmn2 = 2.002083095183101E-146;
2959     final double safmx2 = 4.994797680505588E+145;
2960
2961     if (g == 0.) {
2962         cs = 1.;
2963         sn = 0.;
2964         r = f;
2965     } else if (f == 0.) {
2966         cs = 0.;
2967         sn = 1.;
2968         r = g;
2969     } else {
2970         f1 = f;
2971         g1 = g;
2972         scale = max(Math.abs(f1),Math.abs(g1));
2973         if (scale >= safmx2) {
2974             count = 0;
2975             while(scale >= safmx2) {
2976                ++count;
2977                f1 *= safmn2;
2978                g1 *= safmn2;
2979                scale = max(Math.abs(f1),Math.abs(g1));
2980             }
2981             r = Math.sqrt(f1*f1 + g1*g1);
2982             cs = f1 / r;
2983             sn = g1 / r;
2984             i__1 = count;
2985             for (i = 1; i <= count; ++i) {
2986                 r *= safmx2;
2987             }
2988         } else if (scale <= safmn2) {
2989             count = 0;
2990             while(scale <= safmn2) {
2991                ++count;
2992                f1 *= safmx2;
2993                g1 *= safmx2;
2994                scale = max(Math.abs(f1),Math.abs(g1));
2995             }
2996             r = Math.sqrt(f1*f1 + g1*g1);
2997             cs = f1 / r;
2998             sn = g1 / r;
2999             i__1 = count;
3000             for (i = 1; i <= count; ++i) {
3001                 r *= safmn2;
3002             }
3003         } else {
3004             r = Math.sqrt(f1*f1 + g1*g1);
3005             cs = f1 / r;
3006             sn = g1 / r;
3007         }
3008         if (Math.abs(f) > Math.abs(g) && cs < 0.) {
3009             cs = -cs;
3010             sn = -sn;
3011             r = -r;
3012         }
3013     }
3014     sin[index] = sn;
3015     cos[index] = cs;
3016     return r;
3017
3018   }
3019 static void print_mat( double[]  mat) {
3020 int i;
3021   for(i=0;i<3;i++){
3022     System.out.println(mat[i*3+0]+" "+mat[i*3+1]+" "+mat[i*3+2]+"\n");
3023   }
3024
3025 }
3026 static void print_det( double[] mat) {
3027 double det;
3028
3029   det = mat[0]*mat[4]*mat[8] +
3030         mat[1]*mat[5]*mat[6] +
3031         mat[2]*mat[3]*mat[7] -
3032         mat[2]*mat[4]*mat[6] -
3033         mat[0]*mat[5]*mat[7] -
3034         mat[1]*mat[3]*mat[8];
3035   System.out.println("det= "+det);
3036 }
3037 static void  mat_mul(double[] m1, double[] m2, double[] m3) {
3038   int i;
3039   double[] tmp = new double[9];
3040
3041     tmp[0] =  m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6];
3042     tmp[1] =  m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7];
3043     tmp[2] =  m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8];
3044
3045     tmp[3] =  m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6];
3046     tmp[4] =  m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7];
3047     tmp[5] =  m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8];
3048
3049     tmp[6] =  m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6];
3050     tmp[7] =  m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7];
3051     tmp[8] =  m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8];
3052
3053     for(i=0;i<9;i++) {
3054        m3[i] = tmp[i];
3055     }
3056 }
3057 static void  transpose_mat(double[] in, double[] out) {
3058        out[0] = in[0];
3059        out[1] = in[3];
3060        out[2] = in[6];
3061
3062        out[3] = in[1];
3063        out[4] = in[4];
3064        out[5] = in[7];
3065
3066        out[6] = in[2];
3067        out[7] = in[5];
3068        out[8] = in[8];
3069 }
3070 static  double max3( double[] values) {
3071      if( values[0] > values[1] ) {
3072         if( values[0] > values[2] )
3073            return(values[0]);
3074         else
3075            return(values[2]);
3076      } else {
3077         if( values[1] > values[2] )
3078            return(values[1]);
3079         else
3080            return(values[2]);
3081      }
3082   }
3083
3084     private static final boolean almostEqual(double a, double b) {
3085         if (a == b)
3086             return true;
3087
3088         final double EPSILON_ABSOLUTE = 1.0e-6;
3089         final double EPSILON_RELATIVE = 1.0e-4;
3090         double diff = Math.abs(a-b);
3091         double absA = Math.abs(a);
3092         double absB = Math.abs(b);
3093         double max = (absA >= absB) ? absA : absB;
3094
3095         if (diff < EPSILON_ABSOLUTE)
3096             return true;
3097
3098         if ((diff / max) < EPSILON_RELATIVE)
3099             return true;
3100
3101         return false;
3102     }
3103
3104     /**
3105      * Creates a new object of the same class as this object.
3106      *
3107      * @return a clone of this instance.
3108      * @exception OutOfMemoryError if there is not enough memory.
3109      * @see java.lang.Cloneable
3110      * @since vecmath 1.3
3111      */
3112     public Object clone() {
3113         Matrix3d m1 = null;
3114         try {
3115             m1 = (Matrix3d)super.clone();
3116         } catch (CloneNotSupportedException e) {
3117             // this shouldn't happen, since we are Cloneable
3118             throw new InternalError();
3119         }
3120
3121         // Also need to create new tmp arrays (no need to actually clone them)
3122         return m1;
3123     }
3124
3125         /**
3126          * Get the first matrix element in the first row.
3127          * @return Returns the m00.
3128          * @since vecmath 1.5
3129          */
3130         public final  double getM00() {
3131                 return m00;
3132         }
3133
3134         /**
3135          * Set the first matrix element in the first row.
3136          *
3137          * @param m00 The m00 to set.
3138          *
3139          * @since vecmath 1.5
3140          */
3141         public final  void setM00(double m00) {
3142                 this.m00 = m00;
3143         }
3144
3145         /**
3146          * Get the second matrix element in the first row.
3147          *
3148          * @return Returns the m01.
3149          *
3150          * @since vecmath 1.5
3151          */
3152         public final  double getM01() {
3153                 return m01;
3154         }
3155
3156         /**
3157          * Set the second matrix element in the first row.
3158          *
3159          * @param m01 The m01 to set.
3160          *
3161          * @since vecmath 1.5
3162          */
3163         public  final void setM01(double m01) {
3164                 this.m01 = m01;
3165         }
3166
3167         /**
3168          * Get the third matrix element in the first row.
3169          *
3170          * @return Returns the m02.
3171          *
3172          * @since vecmath 1.5
3173          */
3174         public final double getM02() {
3175                 return m02;
3176         }
3177
3178         /**
3179          * Set the third matrix element in the first row.
3180          *
3181          * @param m02 The m02 to set.
3182          *
3183          * @since vecmath 1.5
3184          */
3185         public final  void setM02(double m02) {
3186                 this.m02 = m02;
3187         }
3188
3189         /**
3190          * Get first matrix element in the second row.
3191          *
3192          * @return Returns the m10.
3193          *
3194          * @since vecmath 1.5
3195          */
3196         public final  double getM10() {
3197                 return m10;
3198         }
3199
3200         /**
3201          * Set first matrix element in the second row.
3202          *
3203          * @param m10 The m10 to set.
3204          *
3205          * @since vecmath 1.5
3206          */
3207         public final  void setM10(double m10) {
3208                 this.m10 = m10;
3209         }
3210
3211         /**
3212          * Get second matrix element in the second row.
3213          *
3214          * @return Returns the m11.
3215          *
3216          * @since vecmath 1.5
3217          */
3218         public final  double getM11() {
3219                 return m11;
3220         }
3221
3222         /**
3223          * Set the second matrix element in the second row.
3224          *
3225          * @param m11 The m11 to set.
3226          *
3227          * @since vecmath 1.5
3228          */
3229         public final  void setM11(double m11) {
3230                 this.m11 = m11;
3231         }
3232
3233         /**
3234          * Get the third matrix element in the second row.
3235          *
3236          * @return Returns the m12.
3237          *
3238          * @since vecmath 1.5
3239          */
3240         public final  double getM12() {
3241                 return m12;
3242         }
3243
3244         /**
3245          * Set the third matrix element in the second row.
3246          *
3247          * @param m12 The m12 to set.
3248          *
3249          * @since vecmath 1.5
3250          */
3251         public final  void setM12(double m12) {
3252                 this.m12 = m12;
3253         }
3254
3255         /**
3256          * Get the first matrix element in the third row.
3257          *
3258          * @return Returns the m20.
3259          *
3260          * @since vecmath 1.5
3261          */
3262         public final  double getM20() {
3263                 return m20;
3264         }
3265
3266         /**
3267          * Set the first matrix element in the third row.
3268          *
3269          * @param m20 The m20 to set.
3270          *
3271          * @since vecmath 1.5
3272          */
3273         public final void setM20(double m20) {
3274                 this.m20 = m20;
3275         }
3276
3277         /**
3278          * Get the second matrix element in the third row.
3279          *
3280          * @return Returns the m21.
3281          *
3282          * @since vecmath 1.5
3283          */
3284         public final double getM21() {
3285                 return m21;
3286         }
3287
3288         /**
3289          * Set the second matrix element in the third row.
3290          *
3291          * @param m21 The m21 to set.
3292          *
3293          * @since vecmath 1.5
3294          */
3295         public final void setM21(double m21) {
3296                 this.m21 = m21;
3297         }
3298
3299         /**
3300          * Get the third matrix element in the third row .
3301          *
3302          * @return Returns the m22.
3303          *
3304          * @since vecmath 1.5
3305          */
3306         public final double getM22() {
3307                 return m22;
3308         }
3309
3310         /**
3311          * Set the third matrix element in the third row.
3312          *
3313          * @param m22 The m22 to set.
3314          *
3315          * @since vecmath 1.5
3316          */
3317         public final void setM22(double m22) {
3318                 this.m22 = m22;
3319         }
3320
3321 }