gov/nasa/worldwind/geom/Matrix4.java

   1 /*
   2 Copyright (C) 2001, 2006 United States Government
   3 as represented by the Administrator of the
   4 National Aeronautics and Space Administration.
   5 All Rights Reserved.
   6 */
   7 package gov.nasa.worldwind.geom;
   8
   9 import gov.nasa.worldwind.*;
  10
  11 /**
  12  * @author Tom Gaskins
  13  * @version $Id: Matrix4.java 1749 2007-05-06 19:48:14Z tgaskins $
  14  */
  15 public class Matrix4 implements Matrix
  16 {
  17     // TODO: scalar operations
  18     private double m11 = 1, m12, m13, m14, m21, m22 = 1, m23, m24, m31, m32, m33 = 1, m34, m41, m42, m43, m44 = 1;
  19     private boolean isOrthonormal = true;
  20
  21     /**
  22      * Creates a new <code>Matrix4</code> as the identity matrix.
  23      */
  24     public Matrix4()
  25     {
  26     }
  27
  28     /**
  29      * Creates a new <code>Matrix4</code> from an array of double precision floating point values. The caller must
  30      * provide at least sixteen values, and any values beyond the sixteenth are ignored.  Values are assigned in the
  31      * following order: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3), (2, 3), (3, 3), (4, 3),
  32      * (1, 4), (2, 4), (3, 4), (4, 4).
  33      *
  34      * @param entries the values, must contain at least 16 values and may not be null
  35      * @throws IllegalArgumentException if <code>entries</code> is too short or null
  36      */
  37     public Matrix4(double[] entries)
  38     {
  39         if (entries == null)
  40         {
  41             String msg = WorldWind.retrieveErrMsg("nullValue.EntriesIsNull");
  42             WorldWind.logger().log(java.util.logging.Level.FINE, msg);
  43             throw new IllegalArgumentException(msg);
  44         }
  45
  46         if (entries.length < 16)
  47         {
  48             String msg = WorldWind.retrieveErrMsg("geom.Matrix4.ArrayTooShort");
  49             WorldWind.logger().log(java.util.logging.Level.FINE, msg);
  50             throw new IllegalArgumentException(msg);
  51         }
  52
  53         this.m11 = entries[0];
  54         this.m21 = entries[1];
  55         this.m31 = entries[2];
  56         this.m41 = entries[3];
  57
  58         this.m12 = entries[4];
  59         this.m22 = entries[5];
  60         this.m32 = entries[6];
  61         this.m42 = entries[7];
  62
  63         this.m13 = entries[8];
  64         this.m23 = entries[9];
  65         this.m33 = entries[10];
  66         this.m43 = entries[11];
  67
  68         this.m14 = entries[12];
  69         this.m24 = entries[13];
  70         this.m34 = entries[14];
  71         this.m44 = entries[15];
  72
  73         // TODO: Determine this by checking key entries
  74         this.isOrthonormal = false;
  75     }
  76
  77     /**
  78      * Creates a new <code>Matrix4</code> from an array of single precision floating point values. The caller must
  79      * provide at least sixteen values, and any values beyond the sixteenth are ignored. Values are assigned in the
  80      * following order: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3), (2, 3), (3, 3), (4, 3),
  81      * (1, 4), (2, 4), (3, 4), (4, 4).
  82      *
  83      * @param entries the values, must contain at least 16 values and may not be null
  84      * @throws IllegalArgumentException if <code>entries</code> is too short or null
  85      */
  86     public Matrix4(float[] entries)
  87     {
  88         if (entries == null)
  89         {
  90             String message = WorldWind.retrieveErrMsg("nullValue.EntriesIsNull");
  91             WorldWind.logger().log(java.util.logging.Level.FINE, message);
  92             throw new IllegalArgumentException(message);
  93         }
  94
  95         if (entries.length < 16)
  96         {
  97             String msg = WorldWind.retrieveErrMsg("geom.Matrix4.ArrayTooShort");
  98             WorldWind.logger().log(java.util.logging.Level.FINE, msg);
  99             throw new IllegalArgumentException(msg);
 100         }
 101
 102         this.m11 = entries[0];
 103         this.m21 = entries[1];
 104         this.m31 = entries[2];
 105         this.m41 = entries[3];
 106
 107         this.m12 = entries[4];
 108         this.m22 = entries[5];
 109         this.m32 = entries[6];
 110         this.m42 = entries[7];
 111
 112         this.m13 = entries[8];
 113         this.m23 = entries[9];
 114         this.m33 = entries[10];
 115         this.m43 = entries[11];
 116
 117         this.m14 = entries[12];
 118         this.m24 = entries[13];
 119         this.m34 = entries[14];
 120         this.m44 = entries[15];
 121
 122         // TODO: Optimize this by checking key entries
 123         this.isOrthonormal = false;
 124     }
 125
 126     /**
 127      * Retrieves the entries comprising this <code>Matrix</code>. The returned array is always 16 entries long. Values
 128      * are place in as in the aray as follows: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3),
 129      * (2, 3), (3, 3), (4, 3), (1, 4), (2, 4), (3, 4), (4, 4).
 130      *
 131      * @return an array, of length 16, containing this Matrices' entries.
 132      */
 133     public final double[] getEntries()
 134     {
 135         double[] entries = new double[16];
 136
 137         entries[0] = this.m11;
 138         entries[1] = this.m21;
 139         entries[2] = this.m31;
 140         entries[3] = this.m41;
 141
 142         entries[4] = this.m12;
 143         entries[5] = this.m22;
 144         entries[6] = this.m32;
 145         entries[7] = this.m42;
 146
 147         entries[8] = this.m13;
 148         entries[9] = this.m23;
 149         entries[10] = this.m33;
 150         entries[11] = this.m43;
 151
 152         entries[12] = this.m14;
 153         entries[13] = this.m24;
 154         entries[14] = this.m34;
 155         entries[15] = this.m44;
 156
 157         return entries;
 158     }
 159
 160     /**
 161      * Sets this <code>Matrix</code> to the identity matrix. This method causes internal changes to the
 162      * <code>Matrix</code> it operates on.
 163      *
 164      * @return <code>this</code>, set to the identity
 165      */
 166     public final Matrix setToIdentity()
 167     {
 168         this.m11 = 1;
 169         this.m12 = 0;
 170         this.m13 = 0;
 171         this.m14 = 0;
 172         this.m21 = 0;
 173         this.m22 = 1;
 174         this.m23 = 0;
 175         this.m24 = 0;
 176         this.m31 = 0;
 177         this.m32 = 0;
 178         this.m33 = 1;
 179         this.m34 = 0;
 180         this.m41 = 0;
 181         this.m42 = 0;
 182         this.m43 = 0;
 183         this.m44 = 1;
 184
 185         this.isOrthonormal = true;
 186
 187         return this;
 188     }
 189
 190     /**
 191      * Obtains whether or not this <code>Matrix</code> is orthonormal. Orthonormal matrices possess unique properties
 192      * that can make algorithms more efficient.
 193      *
 194      * @return true if this is orthonormal, false otherwise
 195      */
 196     public final boolean isOrthonormal()
 197     {
 198         return this.isOrthonormal;
 199     }
 200
 201     /**
 202      * Rotate this matrix by some angle around an arbitrary axis. A positive <code>Angle</code> indicates an
 203      * anti-clockwise direction. This method affects the internal state of this matrix.
 204      *
 205      * @param rotation the distance to rotate this matrix
 206      * @param axisX    the x component of the axis of rotation
 207      * @param axisY    the y component of the axis of rotation
 208      * @param axisZ    the z component of the axis of rotation
 209      * @return this <code>Matrix</code>, with the rotation applied
 210      * @throws IllegalArgumentException if <code>rotation</code> is null
 211      */
 212     public final Matrix rotate(Angle rotation, double axisX, double axisY, double axisZ)
 213     {
 214         if (rotation == null)
 215         {
 216             String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
 217             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 218             throw new IllegalArgumentException(message);
 219         }
 220
 221         double ll = axisX * axisX + axisY * axisY + axisZ * axisZ;
 222         if (rotation.getDegrees() == 0 || ll == 0)
 223             return this;
 224
 225         if (ll != 1) // if axis not unit length, normalize it.
 226         {
 227             double l = Math.sqrt(ll);
 228             axisX /= l;
 229             axisY /= l;
 230             axisZ /= l;
 231         }
 232
 233         double c = rotation.cos();
 234         double s = rotation.sin();
 235         double c1 = 1 - c;
 236         Matrix4 o = new Matrix4();
 237
 238         o.m11 = c + axisX * axisX * c1;
 239         o.m12 = axisX * axisY * c1 - axisZ * s;
 240         o.m13 = axisX * axisZ * c1 + axisY * s;
 241         o.m14 = 0;
 242         o.m21 = axisX * axisY * c1 + axisZ * s;
 243         o.m22 = c + axisY * axisY * c1;
 244         o.m23 = axisY * axisZ * c1 - axisX * s;
 245         o.m24 = 0;
 246         o.m31 = axisX * axisZ * c1 - axisY * s;
 247         o.m32 = axisY * axisZ * c1 + axisX * s;
 248         o.m33 = c + axisZ * axisZ * c1;
 249         o.m34 = 0;
 250         o.m41 = 0;
 251         o.m42 = 0;
 252         o.m43 = 0;
 253         o.m44 = 1;
 254
 255         return this.multiply(o);
 256     }
 257
 258     /**
 259      * Rotate this <code>Matrix</code> around the x-axis. A positive <code>Angle</code> indicates an anti-clockwise
 260      * direction. Changes the internal state of this <code>Matrix</code>.
 261      *
 262      * @param rotation the distance to rotate
 263      * @return this <code>Matrix</code>, rotated around the x-axis by <code>rotation</code> distance
 264      * @throws IllegalArgumentException if <code>rotation</code> is null
 265      */
 266     public final Matrix rotateX(Angle rotation)
 267     {
 268         if (rotation == null)
 269         {
 270             String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
 271             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 272             throw new IllegalArgumentException(message);
 273         }
 274
 275         double c = rotation.cos();
 276         double s = rotation.sin();
 277
 278         double n12 = this.m12 * c + this.m13 * s;
 279         double n13 = this.m12 * -s + this.m13 * c;
 280
 281         double n22 = this.m22 * c + this.m23 * s;
 282         double n23 = this.m22 * -s + this.m23 * c;
 283
 284         double n32 = this.m32 * c + this.m33 * s;
 285         double n33 = this.m32 * -s + this.m33 * c;
 286
 287         double n42 = this.m42 * c + this.m43 * s;
 288         double n43 = this.m42 * -s + this.m43 * c;
 289
 290         this.m12 = n12;
 291         this.m13 = n13;
 292         this.m22 = n22;
 293         this.m23 = n23;
 294         this.m32 = n32;
 295         this.m33 = n33;
 296         this.m42 = n42;
 297         this.m43 = n43;
 298
 299         return this;
 300     }
 301
 302     /**
 303      * Rotate this <code>Matrix</code> around the y-axis. A positive <code>Angle</code> indicates an anti-clockwise
 304      * direction. Changes the internal state of this <code>Matrix</code>.
 305      *
 306      * @param rotation the distance to rotate
 307      * @return this <code>Matrix</code>, rotated around the y-axis by <code>rotation</code> distance
 308      * @throws IllegalArgumentException if <code>rotation</code> is null
 309      */
 310     public final Matrix rotateY(Angle rotation)
 311     {
 312         if (rotation == null)
 313         {
 314             String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
 315             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 316             throw new IllegalArgumentException(message);
 317         }
 318
 319         double c = rotation.cos();
 320         double s = rotation.sin();
 321
 322         double n11 = this.m11 * c + this.m13 * -s;
 323         double n13 = this.m11 * s + this.m13 * c;
 324
 325         double n21 = this.m21 * c + this.m23 * -s;
 326         double n23 = this.m21 * s + this.m23 * c;
 327
 328         double n31 = this.m31 * c + this.m33 * -s;
 329         double n33 = this.m31 * s + this.m33 * c;
 330
 331         double n41 = this.m41 * c + this.m43 * -s;
 332         double n43 = this.m41 * s + this.m43 * c;
 333
 334         this.m11 = n11;
 335         this.m13 = n13;
 336         this.m21 = n21;
 337         this.m23 = n23;
 338         this.m31 = n31;
 339         this.m33 = n33;
 340         this.m41 = n41;
 341         this.m43 = n43;
 342
 343         return this;
 344     }
 345
 346     /**
 347      * Rotate this <code>Matrix</code> around the z-axis. A positive <code>Angle</code> indicates an anti-clockwise
 348      * direction. Changes the internal state of this <code>Matrix</code>.
 349      *
 350      * @param rotation the distance to rotate
 351      * @return this <code>Matrix</code>, rotated around the z-axis by <code>rotation</code> distance
 352      * @throws IllegalArgumentException if <code>rotation</code> is null
 353      */
 354     public final Matrix rotateZ(Angle rotation)
 355     {
 356         if (rotation == null)
 357         {
 358             String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
 359             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 360             throw new IllegalArgumentException(message);
 361         }
 362
 363         double c = rotation.cos();
 364         double s = rotation.sin();
 365
 366         double n11 = this.m11 * c + this.m12 * s;
 367         double n12 = this.m11 * -s + this.m12 * c;
 368
 369         double n21 = this.m21 * c + this.m22 * s;
 370         double n22 = this.m21 * -s + this.m22 * c;
 371
 372         double n31 = this.m31 * c + this.m32 * s;
 373         double n32 = this.m31 * -s + this.m32 * c;
 374
 375         double n41 = this.m41 * c + this.m42 * s;
 376         double n42 = this.m41 * -s + this.m42 * c;
 377
 378         this.m11 = n11;
 379         this.m12 = n12;
 380         this.m21 = n21;
 381         this.m22 = n22;
 382         this.m31 = n31;
 383         this.m32 = n32;
 384         this.m41 = n41;
 385         this.m42 = n42;
 386
 387         return this;
 388     }
 389
 390     /**
 391      * Translates this <code>Matrix</code> in three dimensional space. Changes the internal state of this
 392      * <code>Matrix</code>.
 393      *
 394      * @param x the distance to translate along the x-axis
 395      * @param y the distance to translate along the y-axis
 396      * @param z the distance to translate along the z-axis
 397      * @return this matrix, translated by (x, y, z)
 398      */
 399     public Matrix translate(double x, double y, double z)
 400     {
 401         this.m14 = this.m11 * x + this.m12 * y + this.m13 * z + this.m14;
 402         this.m24 = this.m21 * x + this.m22 * y + this.m23 * z + this.m24;
 403         this.m34 = this.m31 * x + this.m32 * y + this.m33 * z + this.m34;
 404         this.m44 = this.m41 * x + this.m42 * y + this.m43 * z + this.m44;
 405
 406         return this;
 407     }
 408
 409     /**
 410      * Translates this <code>Matrix</code> in three dimansional space. Changes the internal state of this
 411      * <code>Matrix</code>. The x, y and z co-ordinates are used to translate along the x, y and z axes respectively.
 412      *
 413      * @param p the x, y and z distances to translate as a <code>Point</code>
 414      * @return this <code>Matrix</code>, translated by the distances defined in <code>p</code>
 415      * @throws IllegalArgumentException if <code>p</code> is null
 416      */
 417     public final Matrix translate(Point p)
 418     {
 419         if (p == null)
 420         {
 421             String message = WorldWind.retrieveErrMsg("nullValue.PointIsNull");
 422             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 423             throw new IllegalArgumentException(message);
 424         }
 425
 426         return this.translate(p.x(), p.y(), p.z());
 427     }
 428
 429     /**
 430      * Adds this another matrix to this one.
 431      *
 432      * @param m the <code>Matrix</code> to add to this one
 433      * @return this Matrix, with the <code>m</code> added to it
 434      * @throws IllegalArgumentException if <code>m</code> is null
 435      */
 436     public final Matrix add(Matrix m)
 437     {
 438         if (m == null)
 439         {
 440             String message = WorldWind.retrieveErrMsg("nullValue.MatrixIsNull");
 441             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 442             throw new IllegalArgumentException(message);
 443         }
 444
 445         Matrix4 o = (Matrix4) m;
 446
 447         this.m11 += o.m11;
 448         this.m12 += o.m12;
 449         this.m13 += o.m13;
 450         this.m14 += o.m14;
 451         this.m21 += o.m21;
 452         this.m22 += o.m22;
 453         this.m23 += o.m23;
 454         this.m24 += o.m24;
 455         this.m31 += o.m31;
 456         this.m32 += o.m32;
 457         this.m33 += o.m33;
 458         this.m34 += o.m34;
 459         this.m41 += o.m41;
 460         this.m42 += o.m42;
 461         this.m43 += o.m43;
 462         this.m44 += o.m44;
 463
 464         this.isOrthonormal = this.isOrthonormal || o.isOrthonormal;
 465
 466         return this;
 467     }
 468
 469     /**
 470      * Performs a cross multiplication with another <code>Matrix</code>. Alters the state of this <code>Matrix</code>.
 471      *
 472      * @param m another <code>Matrix</code>
 473      * @return this, postmultiplied by <code>m</code>
 474      * @throws IllegalArgumentException if <code>m</code> is null
 475      */
 476     public final Matrix multiply(Matrix m)
 477     {
 478         if (m == null)
 479         {
 480             String message = WorldWind.retrieveErrMsg("nullValue.MatrixIsNull");
 481             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 482             throw new IllegalArgumentException(message);
 483         }
 484
 485         Matrix4 o = (Matrix4) m;
 486
 487         double n11 = this.m11 * o.m11 + this.m12 * o.m21 + this.m13 * o.m31 + this.m14 * o.m41;
 488         double n12 = this.m11 * o.m12 + this.m12 * o.m22 + this.m13 * o.m32 + this.m14 * o.m42;
 489         double n13 = this.m11 * o.m13 + this.m12 * o.m23 + this.m13 * o.m33 + this.m14 * o.m43;
 490         double n14 = this.m11 * o.m14 + this.m12 * o.m24 + this.m13 * o.m34 + this.m14 * o.m44;
 491
 492         double n21 = this.m21 * o.m11 + this.m22 * o.m21 + this.m23 * o.m31 + this.m24 * o.m41;
 493         double n22 = this.m21 * o.m12 + this.m22 * o.m22 + this.m23 * o.m32 + this.m24 * o.m42;
 494         double n23 = this.m21 * o.m13 + this.m22 * o.m23 + this.m23 * o.m33 + this.m24 * o.m43;
 495         double n24 = this.m21 * o.m14 + this.m22 * o.m24 + this.m23 * o.m34 + this.m24 * o.m44;
 496
 497         double n31 = this.m31 * o.m11 + this.m32 * o.m21 + this.m33 * o.m31 + this.m34 * o.m41;
 498         double n32 = this.m31 * o.m12 + this.m32 * o.m22 + this.m33 * o.m32 + this.m34 * o.m42;
 499         double n33 = this.m31 * o.m13 + this.m32 * o.m23 + this.m33 * o.m33 + this.m34 * o.m43;
 500         double n34 = this.m31 * o.m14 + this.m32 * o.m24 + this.m33 * o.m34 + this.m34 * o.m44;
 501
 502         double n41 = this.m41 * o.m11 + this.m42 * o.m21 + this.m43 * o.m31 + this.m44 * o.m41;
 503         double n42 = this.m41 * o.m12 + this.m42 * o.m22 + this.m43 * o.m32 + this.m44 * o.m42;
 504         double n43 = this.m41 * o.m13 + this.m42 * o.m23 + this.m43 * o.m33 + this.m44 * o.m43;
 505         double n44 = this.m41 * o.m14 + this.m42 * o.m24 + this.m43 * o.m34 + this.m44 * o.m44;
 506
 507         this.m11 = n11;
 508         this.m12 = n12;
 509         this.m13 = n13;
 510         this.m14 = n14;
 511         this.m21 = n21;
 512         this.m22 = n22;
 513         this.m23 = n23;
 514         this.m24 = n24;
 515         this.m31 = n31;
 516         this.m32 = n32;
 517         this.m33 = n33;
 518         this.m34 = n34;
 519         this.m41 = n41;
 520         this.m42 = n42;
 521         this.m43 = n43;
 522         this.m44 = n44;
 523
 524         this.isOrthonormal = this.isOrthonormal || o.isOrthonormal;
 525
 526         return this;
 527     }
 528
 529     /**
 530      * Obtains the transpose of this <code>Matrix</code>. Does not alter the state of this <code>Matrix</code>.
 531      *
 532      * @return the transpoase of this <code>Matrix</code>
 533      */
 534     public final Matrix getTranspose()
 535     {
 536         Matrix4 transpose = new Matrix4();
 537
 538         transpose.m11 = this.m11;
 539         transpose.m12 = this.m21;
 540         transpose.m13 = this.m31;
 541         transpose.m14 = this.m41;
 542         transpose.m21 = this.m12;
 543         transpose.m22 = this.m22;
 544         transpose.m23 = this.m32;
 545         transpose.m24 = this.m42;
 546         transpose.m31 = this.m13;
 547         transpose.m32 = this.m23;
 548         transpose.m33 = this.m33;
 549         transpose.m34 = this.m43;
 550         transpose.m41 = this.m14;
 551         transpose.m42 = this.m24;
 552         transpose.m43 = this.m34;
 553         transpose.m44 = this.m44;
 554
 555         transpose.isOrthonormal = this.isOrthonormal;
 556
 557         return transpose;
 558     }
 559
 560     /**
 561      * Obtain the inverse of this <code>Matrix</code>.
 562      *
 563      * @return the inverse of this <code>Matrix</code>.
 564      */
 565     public final Matrix getInverse()
 566     {
 567         Matrix4 inverse;
 568
 569         if (this.isOrthonormal)
 570         {
 571             inverse = this.orthonormalInverse();
 572         }
 573         else
 574         {
 575             inverse = this.generalInverse();
 576         }
 577
 578         inverse.isOrthonormal = this.isOrthonormal;
 579
 580         return inverse;
 581     }
 582
 583     private Matrix4 orthonormalInverse()
 584     {
 585         Matrix4 inverse = new Matrix4();
 586
 587         // Transpose of upper 3x3.
 588         inverse.m11 = this.m11;
 589         inverse.m12 = this.m21;
 590         inverse.m13 = this.m31;
 591
 592         inverse.m21 = this.m12;
 593         inverse.m22 = this.m22;
 594         inverse.m23 = this.m32;
 595
 596         inverse.m31 = this.m13;
 597         inverse.m32 = this.m23;
 598         inverse.m33 = this.m33;
 599
 600         // Upper 3x3 inverse times current translation (4th column).
 601         inverse.m14 = -(inverse.m11 * this.m14 + inverse.m12 * this.m24 + inverse.m13 * this.m34);
 602         inverse.m24 = -(inverse.m21 * this.m14 + inverse.m22 * this.m24 + inverse.m23 * this.m34);
 603         inverse.m34 = -(inverse.m31 * this.m14 + inverse.m32 * this.m24 + inverse.m33 * this.m34);
 604
 605         return inverse;
 606     }
 607
 608     // TODO: Fix generalInverse. It's not producing correct inverses.
 609     private Matrix4 generalInverse()
 610     {
 611         double d = this.determinant();
 612         if (d == 0)
 613         {
 614             return null;
 615         }
 616
 617         double id = 1 / d;
 618         Matrix4 inverse = new Matrix4();
 619
 620         // Form the adjoint matrix.
 621         double a1 = this.m33 * this.m44 - this.m34 * this.m43;
 622         double a2 = this.m34 * this.m42 - this.m32 * this.m44;
 623         double a3 = this.m32 * this.m43 - this.m33 * this.m42;
 624         double a4 = this.m23 * this.m44 - this.m24 * this.m43;
 625         double a5 = this.m24 * this.m42 - this.m22 * this.m44;
 626         double a6 = this.m23 * this.m34 - this.m24 * this.m33;
 627         double a7 = this.m22 * this.m33 - this.m23 * this.m32;
 628         double a8 = this.m34 * this.m41 - this.m31 * this.m44;
 629         double a9 = this.m31 * this.m43 - this.m33 * this.m41;
 630         double a21 = this.m24 * this.m41 - this.m21 * this.m44;
 631         double a22 = this.m24 * this.m31 - this.m21 * this.m34;
 632         double a23 = this.m32 * this.m44 - this.m34 * this.m42;
 633         double a24 = this.m31 * this.m42 - this.m32 * this.m41;
 634         double a14 = this.m21 * this.m42 - this.m22 * this.m41;
 635         double a15 = this.m21 * this.m32 - this.m22 * this.m31;
 636         double a16 = this.m33 * this.m41 - this.m31 * this.m43;
 637
 638         inverse.m11 = id
 639             * this.m22 * a1
 640             + this.m23 * a2
 641             + this.m24 * a3;
 642         inverse.m12 = -id
 643             * this.m12 * a1
 644             + this.m13 * a2
 645             + this.m14 * a3;
 646         inverse.m13 = id
 647             * this.m12 * a4
 648             + this.m13 * (this.m24 * this.m42 - this.m22 * this.m44)
 649             + this.m14 * a5;
 650         inverse.m14 = -id
 651             * this.m12 * a6
 652             + this.m13 * (this.m24 * this.m32 - this.m22 * this.m34)
 653             + this.m14 * a7;
 654         inverse.m21 = -id
 655             * this.m21 * a1
 656             + this.m23 * a8
 657             + this.m24 * a9;
 658         inverse.m22 = id
 659             * this.m11 * a1
 660             + this.m13 * a8
 661             + this.m14 * a9;
 662         inverse.m23 = -id
 663             * this.m11 * a4
 664             + this.m13 * a21
 665             + this.m14 * (this.m21 * this.m43 - this.m23 * this.m41);
 666         inverse.m24 = -id
 667             * this.m11 * a6
 668             + this.m13 * a22
 669             + this.m14 * (this.m21 * this.m33 - this.m23 * this.m31);
 670         inverse.m31 = id
 671             * this.m21 * a23
 672             + this.m22 * a8
 673             + this.m24 * a24;
 674         inverse.m32 = -id
 675             * this.m11 * a23
 676             + this.m12 * a8
 677             + this.m14 * a24;
 678         inverse.m33 = -id
 679             * this.m11 * (this.m22 * this.m44 - this.m24 * this.m42)
 680             + this.m12 * a21
 681             + this.m14 * a14;
 682         inverse.m34 = id
 683             * this.m11 * (this.m22 * this.m34 - this.m24 * this.m32)
 684             + this.m12 * a22
 685             + this.m14 * a15;
 686         inverse.m41 = -id
 687             * this.m21 * a3
 688             + this.m22 * a16
 689             + this.m23 * a24;
 690         inverse.m42 = -id
 691             * this.m11 * a3
 692             + this.m12 * a16
 693             + this.m13 * a24;
 694         inverse.m43 = id
 695             * this.m11 * a5
 696             + this.m12 * (this.m23 * this.m41 - this.m21 * this.m43)
 697             + this.m13 * a14;
 698         inverse.m44 = -id
 699             * this.m11 * a7
 700             + this.m12 * (this.m23 * this.m31 - this.m21 * this.m33)
 701             + this.m13 * a15;
 702
 703         return inverse;
 704     }
 705
 706     /**
 707      * Obtains the determinant of this <code>Matrix</code>.
 708      *
 709      * @return the determinant
 710      */
 711     public final double determinant()
 712     {
 713         return
 714             this.m11 * (
 715                 (this.m22 * this.m33 * this.m44 + this.m23 * this.m34 * this.m42 + this.m24 * this.m32 * this.m43)
 716                     - this.m24 * this.m33 * this.m42
 717                     - this.m22 * this.m34 * this.m43
 718                     - this.m23 * this.m32 * this.m44)
 719                 - this.m12 * (
 720                 (this.m21 * this.m33 * this.m44 + this.m23 * this.m34 * this.m41 + this.m24 * this.m31 * this.m43)
 721                     - this.m24 * this.m33 * this.m41
 722                     - this.m21 * this.m34 * this.m43
 723                     - this.m23 * this.m31 * this.m44)
 724                 + this.m13 * (
 725                 (this.m21 * this.m32 * this.m44 + this.m22 * this.m34 * this.m41 + this.m24 * this.m31 * this.m42)
 726                     - this.m24 * this.m32 * this.m41
 727                     - this.m21 * this.m34 * this.m42
 728                     - this.m22 * this.m31 * this.m44)
 729                 - this.m14 * (
 730                 (this.m21 * this.m32 * this.m43 + this.m22 * this.m33 * this.m41 + this.m23 * this.m31 * this.m42)
 731                     - this.m23 * this.m32 * this.m41
 732                     - this.m21 * this.m33 * this.m42
 733                     - this.m22 * this.m31 * this.m43);
 734     }
 735
 736     /**
 737      * Applies this <code>Matrix</code> to a <code>Point</code>.
 738      *
 739      * @param p the <code>Point</code> to transform
 740      * @return p, trasformed by the current state of this <code>Matrix</code>
 741      * @throws IllegalArgumentException if <code>p</code> is null
 742      */
 743     public final Point transform(Point p)
 744     {
 745         if (p == null)
 746         {
 747             String message = WorldWind.retrieveErrMsg("nullValue.PointIsNull");
 748             WorldWind.logger().log(java.util.logging.Level.FINE, message);
 749             throw new IllegalArgumentException(message);
 750         }
 751
 752         double x = this.m11 * p.x() + this.m12 * p.y() + this.m13 * p.z() + this.m14 * p.w();
 753         double y = this.m21 * p.x() + this.m22 * p.y() + this.m23 * p.z() + this.m24 * p.w();
 754         double z = this.m31 * p.x() + this.m32 * p.y() + this.m33 * p.z() + this.m34 * p.w();
 755         double w = this.m41 * p.x() + this.m42 * p.y() + this.m43 * p.z() + this.m44 * p.w();
 756
 757         return new Point(x, y, z, w);
 758     }
 759
 760     @Override
 761     public boolean equals(Object o)
 762     {
 763         if (this == o)
 764             return true;
 765         if (o == null || getClass() != o.getClass())
 766             return false;
 767
 768         final gov.nasa.worldwind.geom.Matrix4 matrix4 = (gov.nasa.worldwind.geom.Matrix4) o;
 769
 770         if (isOrthonormal != matrix4.isOrthonormal)
 771             return false;
 772         if (Double.compare(matrix4.m11, m11) != 0)
 773             return false;
 774         if (Double.compare(matrix4.m12, m12) != 0)
 775             return false;
 776         if (Double.compare(matrix4.m13, m13) != 0)
 777             return false;
 778         if (Double.compare(matrix4.m14, m14) != 0)
 779             return false;
 780         if (Double.compare(matrix4.m21, m21) != 0)
 781             return false;
 782         if (Double.compare(matrix4.m22, m22) != 0)
 783             return false;
 784         if (Double.compare(matrix4.m23, m23) != 0)
 785             return false;
 786         if (Double.compare(matrix4.m24, m24) != 0)
 787             return false;
 788         if (Double.compare(matrix4.m31, m31) != 0)
 789             return false;
 790         if (Double.compare(matrix4.m32, m32) != 0)
 791             return false;
 792         if (Double.compare(matrix4.m33, m33) != 0)
 793             return false;
 794         if (Double.compare(matrix4.m34, m34) != 0)
 795             return false;
 796         if (Double.compare(matrix4.m41, m41) != 0)
 797             return false;
 798         if (Double.compare(matrix4.m42, m42) != 0)
 799             return false;
 800         if (Double.compare(matrix4.m43, m43) != 0)
 801             return false;
 802         if (Double.compare(matrix4.m44, m44) != 0)
 803             return false;
 804
 805         return true;
 806     }
 807
 808     @Override
 809     public int hashCode()
 810     {
 811         int result;
 812         long temp;
 813         temp = m11 != +0.0d ? Double.doubleToLongBits(m11) : 0L;
 814         result = (int) (temp ^ (temp >>> 32));
 815         temp = m12 != +0.0d ? Double.doubleToLongBits(m12) : 0L;
 816         result = 29 * result + (int) (temp ^ (temp >>> 32));
 817         temp = m13 != +0.0d ? Double.doubleToLongBits(m13) : 0L;
 818         result = 29 * result + (int) (temp ^ (temp >>> 32));
 819         temp = m14 != +0.0d ? Double.doubleToLongBits(m14) : 0L;
 820         result = 29 * result + (int) (temp ^ (temp >>> 32));
 821         temp = m21 != +0.0d ? Double.doubleToLongBits(m21) : 0L;
 822         result = 29 * result + (int) (temp ^ (temp >>> 32));
 823         temp = m22 != +0.0d ? Double.doubleToLongBits(m22) : 0L;
 824         result = 29 * result + (int) (temp ^ (temp >>> 32));
 825         temp = m23 != +0.0d ? Double.doubleToLongBits(m23) : 0L;
 826         result = 29 * result + (int) (temp ^ (temp >>> 32));
 827         temp = m24 != +0.0d ? Double.doubleToLongBits(m24) : 0L;
 828         result = 29 * result + (int) (temp ^ (temp >>> 32));
 829         temp = m31 != +0.0d ? Double.doubleToLongBits(m31) : 0L;
 830         result = 29 * result + (int) (temp ^ (temp >>> 32));
 831         temp = m32 != +0.0d ? Double.doubleToLongBits(m32) : 0L;
 832         result = 29 * result + (int) (temp ^ (temp >>> 32));
 833         temp = m33 != +0.0d ? Double.doubleToLongBits(m33) : 0L;
 834         result = 29 * result + (int) (temp ^ (temp >>> 32));
 835         temp = m34 != +0.0d ? Double.doubleToLongBits(m34) : 0L;
 836         result = 29 * result + (int) (temp ^ (temp >>> 32));
 837         temp = m41 != +0.0d ? Double.doubleToLongBits(m41) : 0L;
 838         result = 29 * result + (int) (temp ^ (temp >>> 32));
 839         temp = m42 != +0.0d ? Double.doubleToLongBits(m42) : 0L;
 840         result = 29 * result + (int) (temp ^ (temp >>> 32));
 841         temp = m43 != +0.0d ? Double.doubleToLongBits(m43) : 0L;
 842         result = 29 * result + (int) (temp ^ (temp >>> 32));
 843         temp = m44 != +0.0d ? Double.doubleToLongBits(m44) : 0L;
 844         result = 29 * result + (int) (temp ^ (temp >>> 32));
 845         result = 29 * result + (isOrthonormal ? 1 : 0);
 846         return result;
 847     }
 848
 849     @Override
 850     public String toString()
 851     {
 852         return "Matrix4 :\n[ " + this.m11 + ", " + this.m12 + ", " + this.m13 + ", " + this.m14 + ",\n "
 853             + this.m21 + ", " + this.m22 + ", " + this.m23 + ", " + this.m24 + ",\n "
 854             + this.m31 + ", " + this.m32 + ", " + this.m33 + ", " + this.m34 + ",\n "
 855             + this.m41 + ", " + this.m42 + ", " + this.m43 + ", " + this.m44 + " ]";
 856     }
 857 }