javax.vecmath/src/javax/vecmath/Matrix4f.java

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