001/* 002 * DCMatrix -- A 3x3 direction cosine matrix used to represent the orientation between 003 * two reference frames. 004 * 005 * Copyright (C) 2008-2016, Joseph A. Huwaldt. 006 * All rights reserved. 007 * 008 * This library is free software; you can redistribute it and/or 009 * modify it under the terms of the GNU Lesser General Public 010 * License as published by the Free Software Foundation; either 011 * version 2 of the License, or (at your option) any later version. 012 * 013 * This library is distributed in the hope that it will be useful, 014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 016 * Lesser General Public License for more details. 017 * 018 * You should have received a copy of the GNU Lesser General Public License 019 * along with this program; if not, write to the Free Software 020 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 021 * Or visit: http://www.gnu.org/licenses/lgpl.html 022 */ 023package jahuwaldt.js.param; 024 025import jahuwaldt.tools.math.MathTools; 026import java.text.MessageFormat; 027import java.util.List; 028import javax.measure.quantity.*; 029import javax.measure.unit.SI; 030import javolution.context.ImmortalContext; 031import javolution.context.ObjectFactory; 032import javolution.context.StackContext; 033import static javolution.lang.MathLib.*; 034import javolution.text.Text; 035import javolution.text.TextBuilder; 036import javolution.util.FastTable; 037import javolution.xml.XMLFormat; 038import javolution.xml.XMLSerializable; 039import javolution.xml.stream.XMLStreamException; 040import org.jscience.mathematics.number.Float64; 041import org.jscience.mathematics.vector.*; 042 043/** 044 * <p> 045 * This class represents a 3x3 transformation or direction cosine {@link Matrix matrix} 046 * that represents a relative orientation (attitude or rotation transformation) between 047 * two different reference frames; B wrt A or BA. It can be used to transform coordinates 048 * in reference frame A to reference frame B (A2B).</p> 049 * <p> 050 * Assumes matrix is used to multiply column vector on the left: 051 * <code>vnew = mat * vold</code>. Works correctly for right-handed coordinate system and 052 * right-handed rotations.</p> 053 * 054 * <p> Modified by: Joseph A. Huwaldt </p> 055 * 056 * @author Joseph A. Huwaldt, Date: November 26, 2008 057 * @version October 25, 2016 058 */ 059public final class DCMatrix extends AbstractRotation<DCMatrix> implements XMLSerializable { 060 061 private static final long serialVersionUID = -1311083554884668584L; 062 063 /** 064 * Constant used to identify the X coordinate in a vector. 065 */ 066 private static final int X = 0; 067 068 /** 069 * Constant used to identify the Y coordinate in a vector. 070 */ 071 private static final int Y = 1; 072 073 /** 074 * Constant used to identify the Z coordinate in a vector. 075 */ 076 private static final int Z = 2; 077 078 /** 079 * Constant used to identify the W coordinate in a quaternion. 080 */ 081 private static final int W = 3; 082 083 /** 084 * The elements stored in this matrix. Serialization is handled by this class since 085 * Float64Matrix is not Serializable. 086 */ 087 private transient Float64Matrix _data; 088 089 /** 090 * <p> 091 * Returns a 3D direction cosine matrix from a sequence of 9 <code>double</code> 092 * values.</p> 093 * <p> 094 * Values are ordered as follows: 095 * <pre> 096 * | a b c | 097 * | d e f | 098 * | g h i | 099 * </pre></p> 100 * 101 * @param a matrix element 0,0 102 * @param b matrix element 0,1 103 * @param c matrix element 0,2 104 * @param d matrix element 1,0 105 * @param e matrix element 1,1 106 * @param f matrix element 1,2 107 * @param g matrix element 2,0 108 * @param h matrix element 2,1 109 * @param i matrix element 2,2 110 * @return the 3D matrix having the specified elements. 111 */ 112 public static DCMatrix valueOf(double a, double b, double c, double d, double e, double f, 113 double g, double h, double i) { 114 115 StackContext.enter(); 116 try { 117 Float64Vector row0 = Float64Vector.valueOf(a, b, c); 118 Float64Vector row1 = Float64Vector.valueOf(d, e, f); 119 Float64Vector row2 = Float64Vector.valueOf(g, h, i); 120 121 DCMatrix M = DCMatrix.newInstance(Float64Matrix.valueOf(row0, row1, row2)); 122 123 return StackContext.outerCopy(M); 124 } finally { 125 StackContext.exit(); 126 } 127 } 128 129 /** 130 * Returns a 3D direction cosine matrix holding the specified row vectors (column 131 * vectors if {@link #transpose transposed}). 132 * 133 * @param row0 the 1st row vector. 134 * @param row1 the 2nd row vector. 135 * @param row2 the 3rd row vector. 136 * @return the 3D matrix having the specified rows. 137 */ 138 public static DCMatrix valueOf(double[] row0, double[] row1, double[] row2) { 139 140 StackContext.enter(); 141 try { 142 Float64Vector row0V = Float64Vector.valueOf(row0); 143 Float64Vector row1V = Float64Vector.valueOf(row1); 144 Float64Vector row2V = Float64Vector.valueOf(row2); 145 146 DCMatrix M = DCMatrix.newInstance(Float64Matrix.valueOf(row0V, row1V, row2V)); 147 148 return StackContext.outerCopy(M); 149 } finally { 150 StackContext.exit(); 151 } 152 } 153 154 /** 155 * Returns a 3D direction cosine matrix holding the specified row vectors (column 156 * vectors if {@link #transpose transposed}). 157 * 158 * @param row0 the 1st row vector. 159 * @param row1 the 2nd row vector. 160 * @param row2 the 3rd row vector. 161 * @return the 3D matrix having the specified rows. 162 */ 163 public static DCMatrix valueOf(Float64Vector row0, Float64Vector row1, Float64Vector row2) { 164 165 DCMatrix M = DCMatrix.newInstance(Float64Matrix.valueOf(row0, row1, row2)); 166 167 return M; 168 } 169 170 /** 171 * Returns a 3D direction cosine matrix holding the specified diagonal vector with 172 * zeros placed in all the other elements. 173 * 174 * @param diagX the 1st element of the diagonal vector for the matrix. 175 * @param diagY the 2nd element of the diagonal vector for the matrix. 176 * @param diagZ the 3rd element of the diagonal vector for the matrix. 177 * @return the 3D matrix having the specified diagonal. 178 */ 179 public static DCMatrix valueOf(double diagX, double diagY, double diagZ) { 180 181 StackContext.enter(); 182 try { 183 Float64Vector row0 = Float64Vector.valueOf(diagX, 0, 0); 184 Float64Vector row1 = Float64Vector.valueOf(0, diagY, 0); 185 Float64Vector row2 = Float64Vector.valueOf(0, 0, diagZ); 186 187 DCMatrix M = DCMatrix.newInstance(Float64Matrix.valueOf(row0, row1, row2)); 188 189 return StackContext.outerCopy(M); 190 } finally { 191 StackContext.exit(); 192 } 193 } 194 195 /** 196 * Returns a 3D direction cosine matrix holding the specified diagonal vector with 197 * zeros placed in all the other elements. 198 * 199 * @param diag the diagonal vector for the matrix. 200 * @return the 3D matrix having the specified diagonal. 201 */ 202 public static DCMatrix valueOf(Float64Vector diag) { 203 return DCMatrix.valueOf(diag.getValue(X), diag.getValue(Y), diag.getValue(Z)); 204 } 205 206 /** 207 * Returns a 3D direction cosine matrix holding the row vectors from the specified 208 * collection (column vectors if {@link #transpose transposed}). 209 * 210 * @param rows The list of row vectors. If there are more than 3 elements, an 211 * exception is thrown. 212 * @return the 3D matrix having the specified rows. 213 * @throws DimensionException if the rows do not have a dimension of 3. 214 */ 215 public static DCMatrix valueOf(List<Float64Vector> rows) { 216 if (rows.size() != 3) 217 throw new DimensionException( 218 MessageFormat.format(RESOURCES.getString("3vectorsExpectedErr"), rows.size())); 219 220 return DCMatrix.valueOf(rows.get(0), rows.get(1), rows.get(2)); 221 } 222 223 /** 224 * Returns a {@link DCMatrix} instance containing a copy of the specified matrix of 225 * Float64 values. The matrix must have dimensions of 3x3. 226 * 227 * @param matrix the matrix of Float64 values to convert (must have dimension of 3x3). 228 * @return the matrix having the specified elements. 229 */ 230 public static DCMatrix valueOf(Matrix<Float64> matrix) { 231 if (matrix.getNumberOfColumns() != 3 || matrix.getNumberOfRows() != 3) 232 throw new DimensionException(MessageFormat.format(RESOURCES.getString("3x3MatrixExpectedErr"), 233 matrix.getNumberOfRows(), matrix.getNumberOfColumns())); 234 235 DCMatrix M = DCMatrix.newInstance(Float64Matrix.valueOf(matrix)); 236 237 return M; 238 } 239 240 /** 241 * Returns a {@link DCMatrix} instance equivalent to the specified rotation 242 * transform. 243 * 244 * @param transform The Rotation to convert to a direction cosine matrix. 245 * @return the direction cosine matrix representing the specified rotation transform. 246 */ 247 public static DCMatrix valueOf(Rotation<?> transform) { 248 if (transform instanceof DCMatrix) 249 return (DCMatrix)transform; 250 251 return transform.toDCM(); 252 } 253 254 /** 255 * Return a new {@link DCMatrix} instance constructed from the specified axis pair. 256 * 257 * @param vector1 The vector defining the 1st axis. 258 * @param vector2 The vector defining the 2nd axis. 259 * @param axis1 Constant from this class designating which axis the 1st axis is 260 * (e.g.: X=0, or Z=2). 261 * @param axis2 Constant from this class designating which axis the 2nd axis is 262 * (e.g.: Y=1 or X=0). 263 * @return A direction cosine matrix representing a rotation from the base axis system 264 * to the specified axis orientations. 265 */ 266 public static DCMatrix valueOf(Coordinate3D vector1, Coordinate3D vector2, int axis1, int axis2) { 267 if (axis1 == axis2) 268 throw new IllegalArgumentException(RESOURCES.getString("equalAxisDesignationErr")); 269 if (axis1 < X || axis1 > Z) 270 throw new IllegalArgumentException(MessageFormat.format(RESOURCES.getString("axisDesignationRangeErr"),1)); 271 if (axis2 < X || axis2 > Z) 272 throw new IllegalArgumentException(MessageFormat.format(RESOURCES.getString("axisDesignationRangeErr"),2)); 273 274 StackContext.enter(); 275 try { 276 // Convert input vectors to unit vectors. 277 Vector3D<Dimensionless> uv1 = vector1.toVector3D().toUnitVector(); 278 Vector3D<Dimensionless> uv2 = vector2.toVector3D().toUnitVector(); 279 280 // Create a 3rd axis that is orthogonal to the plane defined by the 1st two. 281 Vector3D<Dimensionless> uv3 = ((Coordinate3D)vector1.cross(vector2)).toVector3D(); 282 283 // Correct vector2 to be exactly orthogonal to vector1 and vector3. 284 double delta = 1.0 - uv3.magValue(); 285 if (delta >= 1.0E-10) { 286 if (MathTools.isApproxEqual(delta, 1.0, 1e-9)) { 287 throw new IllegalStateException(RESOURCES.getString("axesParallelErr")); 288 } 289 uv3 = uv3.toUnitVector(); 290 uv2 = uv2.cross(uv1); 291 } 292 293 int axis3 = 3 - (axis1 + axis2); 294 int sign = axis2 - axis1; 295 if ((sign == 2) || (sign == -2)) 296 sign = -sign / 2; 297 298 // Create a list of rows of the DCM. 299 FastTable<FastTable<Float64>> matrix = FastTable.newInstance(); 300 for (int i = 0; i < 3; ++i) { 301 FastTable<Float64> row = FastTable.newInstance(); 302 row.setSize(3); 303 matrix.add(row); 304 } 305 306 // Fill in the DCM values. 307 for (int i = 0; i < 3; ++i) { 308 FastTable<Float64> row = matrix.get(i); 309 row.set(axis1, Float64.valueOf(uv1.getValue(i))); 310 row.set(axis2, Float64.valueOf(uv2.getValue(i))); 311 row.set(axis3, Float64.valueOf(uv3.getValue(i) * sign)); 312 } 313 314 // Create and output the DCM. 315 FastTable<Float64Vector> mat2 = FastTable.newInstance(); 316 for (int i = 0; i < 3; ++i) 317 mat2.add(Float64Vector.valueOf(matrix.get(i))); 318 DCMatrix dcm = DCMatrix.valueOf(mat2); 319 320 return StackContext.outerCopy(dcm); 321 322 } finally { 323 StackContext.exit(); 324 } 325 } 326 327 /** 328 * Returns the number of rows <code>m</code> for this matrix. This implementation 329 * always returns 3. 330 * 331 * @return The number of rows for this matrix 332 */ 333 public int getNumberOfRows() { 334 return 3; 335 } 336 337 /** 338 * Returns the number of columns <code>n</code> for this matrix. This implementation 339 * always returns 3. 340 * 341 * @return The number of columns for this matrix 342 */ 343 public int getNumberOfColumns() { 344 return 3; 345 } 346 347 /** 348 * Returns a single element from this matrix. 349 * 350 * @param i the row index (range [0..3[). 351 * @param j the column index (range [0..3[). 352 * @return the matrix element at [i,j]. 353 */ 354 public Float64 get(int i, int j) { 355 return _data.get(i, j); 356 } 357 358 /** 359 * <p> 360 * Returns a single element from this matrix as a <code>double</code>.</p> 361 * 362 * <p> 363 * This is a convenience method for: <code> this.get(i,j).doubleValue();</code></p> 364 * 365 * @param i the row index (range [0..3[). 366 * @param j the column index (range [0..3[). 367 * @return the matrix element at [i,j]. 368 */ 369 public double getValue(int i, int j) { 370 return _data.get(i, j).doubleValue(); 371 } 372 373 /** 374 * Returns the row identified by the specified index in this matrix. 375 * 376 * @param i the row index (range [0..3[). 377 * @return The specified row of this matrix. 378 */ 379 public Float64Vector getRow(int i) { 380 return _data.getRow(i); 381 } 382 383 /** 384 * Returns the column identified by the specified index in this matrix. 385 * 386 * @param j the column index (range [0..3[). 387 * @return The specified column of this matrix. 388 */ 389 public Float64Vector getColumn(int j) { 390 return _data.getColumn(j); 391 } 392 393 /** 394 * Returns the diagonal vector. 395 * 396 * @return the vector holding the diagonal elements. 397 */ 398 public Float64Vector getDiagonal() { 399 return _data.getDiagonal(); 400 } 401 402 /** 403 * Returns the negation of this matrix. 404 * 405 * @return <code>-this</code> 406 */ 407 public DCMatrix opposite() { 408 DCMatrix M = DCMatrix.newInstance(_data.opposite()); 409 return M; 410 } 411 412 /** 413 * Returns the sum of this matrix with the one specified. 414 * 415 * @param that the matrix to be added. 416 * @return <code>this + that</code> 417 */ 418 public DCMatrix plus(DCMatrix that) { 419 420 DCMatrix M = DCMatrix.newInstance(this._data.plus(that._data)); 421 422 return M; 423 } 424 425 /** 426 * Returns the difference between this matrix and the one specified. 427 * 428 * @param that the matrix to be subtracted from this one. 429 * @return <code>this - that</code> 430 */ 431 public DCMatrix minus(DCMatrix that) { 432 433 DCMatrix M = DCMatrix.newInstance(this._data.minus(that._data)); 434 435 return M; 436 } 437 438 /** 439 * Returns the product of this matrix by the specified factor. 440 * 441 * @param k the coefficient multiplier 442 * @return <code>this · k</code> 443 */ 444 public DCMatrix times(Float64 k) { 445 DCMatrix M = DCMatrix.newInstance(this._data.times(k)); 446 return M; 447 } 448 449 /** 450 * Returns the product of this matrix by the specified vector. 451 * 452 * @param v the vector. 453 * @return <code>this · v</code> 454 * @throws DimensionException - if v.getDimension() != this.getNumberOfColumns() 455 */ 456 public Float64Vector times(Vector<Float64> v) { 457 458 // Convert the input vector to a Float64Vector. 459 Float64Vector V; 460 if (v instanceof Float64Vector) 461 V = (Float64Vector)v; 462 else 463 V = Float64Vector.valueOf(v); 464 465 return _data.times(V); 466 } 467 468 /** 469 * Returns the product of this matrix by the specified vector. 470 * 471 * @param <Q> The Quantity (unit type) of the vector. 472 * @param v the vector. 473 * @return <code>this · v</code> 474 */ 475 public <Q extends Quantity> Vector3D<Q> times(Coordinate3D<Q> v) { 476 return this.transform(v); 477 } 478 479 /** 480 * Transforms a 3D vector from frame A to B using this rotation transformation. 481 * 482 * @param <Q> The Quantity (unit type) of the vector. 483 * @param v the vector expressed in frame A. 484 * @return the vector expressed in frame B. 485 */ 486 @Override 487 public <Q extends Quantity> Vector3D<Q> transform(Coordinate3D<Q> v) { 488 489 // Convert the input vector to a Float64Vector. 490 Float64Vector V = v.toVector3D().toFloat64Vector(); 491 492 // Create an output vector. 493 Vector3D<Q> result = Vector3D.valueOf(_data.times(V), v.getUnit()); 494 495 return result; 496 } 497 498 /** 499 * Returns the product of this direction cosine matrix and another rotation transform. 500 * If this rotation transform is BA and that is AC then the returned value is: 501 * <code>BC = BA · AC</code> (or <code>C2B = A2B · C2A</code>). 502 * 503 * @param that the rotation transform multiplier. 504 * @return <code>this · that</code> 505 */ 506 @Override 507 public DCMatrix times(Rotation<?> that) { 508 DCMatrix thatDCM = that.toDCM(); 509 return DCMatrix.newInstance(this._data.times(thatDCM._data)); 510 } 511 512 /** 513 * Returns the inverse of this matrix. If the matrix is singular, the 514 * {@link #determinant()} will be zero (or nearly zero). 515 * 516 * @return <code>1 / this</code> 517 */ 518 public DCMatrix inverse() { 519 520 DCMatrix M = DCMatrix.newInstance(_data.inverse()); 521 522 return M; 523 } 524 525 /** 526 * Returns the determinant of this matrix. 527 * 528 * @return this matrix determinant. 529 */ 530 public Float64 determinant() { 531 return _data.determinant(); 532 } 533 534 /** 535 * Returns the transpose of this matrix. This results in the spatial inverse of this 536 * transformation: AB rather than BA. 537 * 538 * @return <code>this'</code> 539 */ 540 @Override 541 public DCMatrix transpose() { 542 543 DCMatrix M = DCMatrix.newInstance(_data.transpose()); 544 545 return M; 546 } 547 548 /** 549 * Returns the cofactor of an element in this matrix. It is the value obtained by 550 * evaluating the determinant formed by the elements not in that particular row or 551 * column. 552 * 553 * @param i the row index. 554 * @param j the column index. 555 * @return the cofactor of THIS[i,j]. 556 */ 557 public Float64 cofactor(int i, int j) { 558 return _data.cofactor(i, j); 559 } 560 561 /** 562 * Returns the adjoint of this matrix. It is obtained by replacing each element in 563 * this matrix with its cofactor and applying a + or - sign according to 564 * <code>(-1)**(i+j)</code>, and then finding the transpose of the resulting matrix. 565 * 566 * @return the adjoint of this matrix. 567 */ 568 public DCMatrix adjoint() { 569 570 DCMatrix M = DCMatrix.newInstance(_data.adjoint()); 571 572 return M; 573 } 574 575 /** 576 * Returns the linear algebraic matrix tensor product of this matrix and another 577 * matrix (Kronecker product). 578 * 579 * @param that the second matrix 580 * @return <code>this times that</code> 581 * @see <a href="http://en.wikipedia.org/wiki/Kronecker_product"> 582 * Wikipedia: Kronecker Product</a> 583 */ 584 public Float64Matrix tensor(DCMatrix that) { 585 return _data.tensor(that._data); 586 } 587 588 /** 589 * Returns the vectorization of this matrix. The vectorization of a matrix is the 590 * column vector obtained by stacking the columns of the matrix on top of one another. 591 * 592 * @return the vectorization of this matrix. 593 */ 594 public Float64Vector vectorization() { 595 return _data.vectorization(); 596 } 597 598 /** 599 * Returns a copy of this matrix allocated by the calling thread (possibly on the 600 * stack). 601 * 602 * @return an identical and independent copy of this matrix. 603 */ 604 @Override 605 public DCMatrix copy() { 606 return DCMatrix.copyOf(this); 607 } 608 609 /** 610 * Returns the equivalent of this direction cosine matrix as a Float64Matix. 611 * 612 * @return The equivalent of this direction cosine matrix as a Float64Matix. 613 */ 614 public Float64Matrix toFloat64Matrix() { 615 return _data; 616 } 617 618 /** 619 * Returns the equivalent of this direction cosine matrix as a Java 2D matrix. 620 * 621 * @return The equivalent of this direction cosine matrix as a Java 2D matrix. 622 */ 623 public double[][] toMatrix() { 624 StackContext.enter(); 625 try { 626 double[][] mat = new double[3][3]; 627 for (int i = 0; i < 3; ++i) { 628 for (int j = 0; j < 3; ++j) { 629 mat[i][j] = _data.get(i, j).doubleValue(); 630 } 631 } 632 return mat; 633 } finally { 634 StackContext.exit(); 635 } 636 } 637 638 /** 639 * Returns a direction cosine transformation matrix from this rotation transformation. 640 * 641 * @return a direction cosine matrix that converts from frame A to B. 642 * @see <a href="http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)"> 643 * Wikipedia: Rotation representation (mathematics)</a> 644 */ 645 @Override 646 public DCMatrix toDCM() { 647 return this; 648 } 649 650 /** 651 * Returns a {@link Quaternion} constructed from the this transformation matrix. 652 * 653 * @return The quaternion representing this direction cosine rotation matrix. 654 */ 655 @Override 656 public Quaternion toQuaternion() { 657 Quaternion Q = null; 658 659 // From: "Quaternions", by Ken Shoemake 660 // http://www.cs.caltech.edu/courses/cs171/quatut.pdf 661 /* This algorithm avoids near-zero divides by looking for a large component 662 * — first w, then x, y, or z. When the trace is greater than zero, 663 * |w| is greater than 1/2, which is as small as a largest component can be. 664 * Otherwise, the largest diagonal entry corresponds to the largest of |x|, 665 * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. 666 */ 667 double tr = this.getValue(X, X) + this.getValue(Y, Y) + this.getValue(Z, Z); 668 if (tr >= 0.0) { 669 double s = sqrt(1.0 + tr); 670 double w = 0.5 * s; 671 s = 0.5 / s; 672 673 double x = (this.getValue(Z, Y) - this.getValue(Y, Z)) * s; 674 double y = (this.getValue(X, Z) - this.getValue(Z, X)) * s; 675 double z = (this.getValue(Y, X) - this.getValue(X, Y)) * s; 676 677 Q = Quaternion.valueOf(x, y, z, w); 678 679 } else { 680 int h = X; 681 if (this.getValue(Y, Y) > this.getValue(X, X)) 682 h = Y; 683 if (this.getValue(Z, Z) > this.getValue(h, h)) 684 h = Z; 685 switch (h) { 686 case X: 687 Q = switchCase(X, Y, Z, this); 688 break; 689 case Y: 690 Q = switchCase(Y, Z, X, this); 691 break; 692 case Z: 693 Q = switchCase(Z, X, Y, this); 694 break; 695 } 696 } 697 698 return Q; 699 } 700 701 private static Quaternion switchCase(int i, int j, int k, DCMatrix dc) { 702 double[] qu = QU_FACTORY.object(); 703 704 double s = sqrt(1.0 + (dc.getValue(i, i) - (dc.getValue(j, j) + dc.getValue(k, k)))); 705 qu[i] = 0.5 * s; 706 s = 0.5 / s; 707 708 qu[j] = (dc.getValue(i, j) + dc.getValue(j, i)) * s; 709 qu[k] = (dc.getValue(k, i) + dc.getValue(i, k)) * s; 710 qu[W] = (dc.getValue(k, j) - dc.getValue(j, k)) * s; 711 712 Quaternion q = Quaternion.valueOf(qu); 713 714 QU_FACTORY.recycle(qu); // Recycle the array. 715 716 return q; 717 } 718 719 /** 720 * Returns the text representation of this direction cosine matrix. 721 * 722 * @return the text representation of this matrix. 723 */ 724 @Override 725 public Text toText() { 726 final int m = this.getNumberOfRows(); 727 final int n = this.getNumberOfColumns(); 728 TextBuilder tmp = TextBuilder.newInstance(); 729 if (this.isApproxEqual(IDENTITY)) 730 tmp.append("{IDENTITY}"); 731 else { 732 tmp.append('{'); 733 for (int i = 0; i < m; i++) { 734 tmp.append('{'); 735 for (int j = 0; j < n; j++) { 736 tmp.append(get(i, j)); 737 if (j != n - 1) { 738 tmp.append(", "); 739 } 740 } 741 tmp.append("}"); 742 if (i != m - 1) { 743 tmp.append(",\n"); 744 } 745 } 746 tmp.append("}"); 747 } 748 Text txt = tmp.toText(); 749 TextBuilder.recycle(tmp); 750 return txt; 751 } 752 753 /** 754 * Compares this DCMatrix against the specified object for strict equality (same 755 * rotation type and same values). 756 * 757 * @param obj the object to compare with. 758 * @return <code>true</code> if this rotation is identical to that rotation; 759 * <code>false</code> otherwise. 760 */ 761 @Override 762 public boolean equals(Object obj) { 763 if (this == obj) 764 return true; 765 if ((obj == null) || (obj.getClass() != this.getClass())) 766 return false; 767 768 DCMatrix that = (DCMatrix)obj; 769 return this._data.equals(that._data); 770 } 771 772 /** 773 * Returns the hash code for this rotation. 774 * 775 * @return the hash code value. 776 */ 777 @Override 778 public int hashCode() { 779 int hash = 7; 780 781 int var_code = _data.hashCode(); 782 hash = hash * 31 + var_code; 783 784 return hash; 785 } 786 787 private static ObjectFactory<double[]> QU_FACTORY = new ObjectFactory<double[]>() { 788 @Override 789 protected double[] create() { 790 return new double[4]; 791 } 792 }; 793 794 /** 795 * During serialization, this will write out the Float64Matrix as a simple series of 796 * <code>double</code> values. This method is ONLY called by the Java Serialization 797 * mechanism and should not otherwise be used. 798 */ 799 private void writeObject(java.io.ObjectOutputStream out) throws java.io.IOException { 800 801 // Call the default write object method. 802 out.defaultWriteObject(); 803 804 // Write out the three coordinate values. 805 for (int i = 0; i < 3; ++i) 806 for (int j = 0; j < 3; ++j) 807 out.writeDouble(_data.get(i, j).doubleValue()); 808 809 } 810 811 /** 812 * During de-serialization, this will handle the reconstruction of the Float64Matrix. 813 * This method is ONLY called by the Java Serialization mechanism and should not 814 * otherwise be used. 815 */ 816 private void readObject(java.io.ObjectInputStream in) throws java.io.IOException, ClassNotFoundException { 817 818 // Call the default read object method. 819 in.defaultReadObject(); 820 821 // Read in the three coordinate values. 822 FastTable<Float64Vector> rows = FastTable.newInstance(); 823 for (int i = 0; i < 3; ++i) { 824 double v1 = in.readDouble(); 825 double v2 = in.readDouble(); 826 double v3 = in.readDouble(); 827 Float64Vector row = Float64Vector.valueOf(v1, v2, v3); 828 rows.add(row); 829 } 830 831 _data = Float64Matrix.valueOf(rows); 832 FastTable.recycle(rows); 833 834 } 835 836 /** 837 * Holds the default XML representation for the DCMatrix object. This representation 838 * consists of a series of 9 double values that represent a 3x3 matrix. 839 * <pre> 840 * <DCMatrix> 841 * <Float64Matrix rows="3" columns="3"> 842 * <Float64 value="0.7848855672213958"/> 843 * <Float64 value="-0.56696369630552457"/> 844 * <Float64 value="0.2500136265068858"/> 845 * <Float64 value="0.4531538935183249"/> 846 * <Float64 value="0.25001362650688608"/> 847 * <Float64 value="-0.8556545654351748"/> 848 * <Float64 value="0.42261826174069944"/> 849 * <Float64 value="0.7848855672213958"/> 850 * <Float64 value="0.45315389351832508"/> 851 * </Float64Matrix> 852 * </pre> 853 */ 854 protected static final XMLFormat<DCMatrix> XML = new XMLFormat<DCMatrix>(DCMatrix.class) { 855 856 @Override 857 public DCMatrix newInstance(Class<DCMatrix> cls, XMLFormat.InputElement xml) throws XMLStreamException { 858 return FACTORY.object(); 859 } 860 861 @Override 862 public void read(javolution.xml.XMLFormat.InputElement xml, DCMatrix dcm) throws XMLStreamException { 863 864 FastTable<Float64Vector> rowList = FastTable.newInstance(); 865 try { 866 for (int i = 0; i < 3; ++i) { 867 if (!xml.hasNext()) 868 throw new XMLStreamException( 869 MessageFormat.format(RESOURCES.getString("need9ElementsFoundLess"), i)); 870 871 double x = ((Float64)xml.getNext()).doubleValue(); 872 double y = ((Float64)xml.getNext()).doubleValue(); 873 double z = ((Float64)xml.getNext()).doubleValue(); 874 Float64Vector row = Float64Vector.valueOf(x, y, z); 875 rowList.add(row); 876 } 877 878 if (xml.hasNext()) 879 throw new XMLStreamException(RESOURCES.getString("toManyXMLElementsErr")); 880 881 Float64Matrix m = Float64Matrix.valueOf(rowList); 882 dcm._data = m; 883 884 } finally { 885 FastTable.recycle(rowList); 886 } 887 888 } 889 890 @Override 891 public void write(DCMatrix m, XMLFormat.OutputElement xml) throws XMLStreamException { 892 for (int i = 0; i < 3; i++) { 893 for (int j = 0; j < 3; j++) { 894 xml.add(m.get(i, j)); 895 } 896 } 897 } 898 }; 899 900 /////////////////////// 901 // Factory creation. // 902 /////////////////////// 903 private DCMatrix() { } 904 905 private static final ObjectFactory<DCMatrix> FACTORY = new ObjectFactory<DCMatrix>() { 906 @Override 907 protected DCMatrix create() { 908 return new DCMatrix(); 909 } 910 }; 911 912 private static DCMatrix copyOf(DCMatrix original) { 913 DCMatrix M = FACTORY.object(); 914 M._data = original._data.copy(); 915 return M; 916 } 917 918 private static DCMatrix newInstance(Float64Matrix data) { 919 DCMatrix M = FACTORY.object(); 920 M._data = data; 921 return M; 922 } 923 924 /** 925 * A matrix containing all zero elements. 926 */ 927 public static final DCMatrix ZERO; 928 929 /** 930 * A DCM object that represents no relative change in orientation. 931 */ 932 public static final DCMatrix IDENT; 933 934 static { 935 ImmortalContext.enter(); 936 try { 937 ZERO = DCMatrix.valueOf(0, 0, 0, 0, 0, 0, 0, 0, 0); 938 IDENT = DCMatrix.valueOf(1, 1, 1); 939 940 } finally { 941 ImmortalContext.exit(); 942 } 943 } 944 945 /** 946 * Tests the methods in this class. 947 * 948 * @param args Command line arguments (ignored). 949 */ 950 public static void main(String args[]) { 951 System.out.println("Testing DCMatrix:"); 952 953 DCMatrix m1 = DCMatrix.valueOf(1, 2, 3, 4, 5, 6, 7, 8, 9); 954 System.out.println("m1 = \n" + m1); 955 System.out.println(" m1.getRow(1) = " + m1.getRow(1)); 956 System.out.println(" m1.getColumn(2) = " + m1.getColumn(2)); 957 System.out.println(" m1.getDiagonal() = " + m1.getDiagonal()); 958 System.out.println(" m1.opposite() = \n" + m1.opposite()); 959 System.out.println(" m1.transpose() = \n" + m1.transpose()); 960 System.out.println(" m1.vectorization() = \n" + m1.vectorization()); 961 System.out.println(" m1.determinant() = " + m1.determinant() + "; matrix is singular and can not be inverted"); 962 963 Float64 p1 = Float64.valueOf(2); 964 System.out.println("\np1 = " + p1); 965 System.out.println(" m1.times(p1) = \n" + m1.times(p1)); 966 967 DCMatrix m2 = DCMatrix.valueOf(1, 0, 1, 0, 1, 0, -1, 0, 1); 968 System.out.println("\nm2 = \n" + m2); 969 System.out.println(" m2.determinant() = " + m2.determinant()); 970 System.out.println(" m2.inverse() = \n" + m2.inverse()); 971 System.out.println(" m2.cofactor(0,2) = " + m2.cofactor(0, 2)); 972 System.out.println(" m2.adjoint() = \n" + m2.adjoint()); 973 System.out.println(" m1.times(m2) = \n" + m1.times(m2)); 974 System.out.println(" m1.tensor(m2) = \n" + m1.tensor(m2)); 975 976 Vector3D<Length> v1 = Vector3D.valueOf(1, 2, 3, SI.METER); 977 System.out.println("\nv1 = " + v1); 978 System.out.println(" m1.times(v1) = " + m1.times(v1)); 979 980 Parameter<Angle> psi = Parameter.valueOf(60, javax.measure.unit.NonSI.DEGREE_ANGLE); 981 Parameter<Angle> theta = Parameter.ZERO_ANGLE; 982 Parameter<Angle> phi = Parameter.ZERO_ANGLE; 983 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 984 985 DCMatrix m3 = DCMatrix.getEulerTM(psi, theta, phi); 986 System.out.println("m3 = \n" + m3); 987 System.out.println("m3.toQuaternion() = " + m3.toQuaternion()); 988 989 Vector3D<Length> v2 = Vector3D.valueOf(1, 1, 1, SI.METER); 990 System.out.println("\nv2 = " + v2); 991 System.out.println(" m3.times(v2) = " + m3.times(v2)); 992 993 DCMatrix m4 = DCMatrix.getPsiThetaTM(psi, theta); 994 System.out.println(" m4.times(v2) = " + m4.times(v2)); 995 996 psi = Parameter.valueOf(30, javax.measure.unit.NonSI.DEGREE_ANGLE); 997 theta = Parameter.valueOf(-25, javax.measure.unit.NonSI.DEGREE_ANGLE); 998 phi = Parameter.valueOf(60, javax.measure.unit.NonSI.DEGREE_ANGLE); 999 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 1000 1001 DCMatrix m5 = DCMatrix.getEulerTM(psi, theta, phi); 1002 System.out.println("m5 = \n" + m5); 1003 Vector3D<Length> v3 = Vector3D.valueOf(1, 1, 0, SI.METER); 1004 System.out.println("\nv3 = " + v3); 1005 System.out.println(" m5.transform(v3) = " + m5.transform(v3)); 1006 System.out.println(" m5.toQuaternion().transform(v3) = " + m5.toQuaternion().transform(v3)); 1007 System.out.println(" m5.toQuaternion().toDCM().transform(v3) = " + m5.toQuaternion().toDCM().transform(v3)); 1008 1009 // Write out XML data. 1010 try { 1011 System.out.println(); 1012 1013 // Creates some useful aliases for class names. 1014 javolution.xml.XMLBinding binding = new javolution.xml.XMLBinding(); 1015 binding.setAlias(org.jscience.mathematics.number.Float64.class, "Float64"); 1016 1017 javolution.xml.XMLObjectWriter writer = javolution.xml.XMLObjectWriter.newInstance(System.out); 1018 writer.setIndentation(" "); 1019 writer.setBinding(binding); 1020 writer.write(m5, "DCMatrix", DCMatrix.class); 1021 writer.flush(); 1022 1023 System.out.println(); 1024 } catch (Exception e) { 1025 e.printStackTrace(); 1026 } 1027 1028 } 1029 1030}