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