001/** 002 * GTransform -- A general transformation matrix. 003 * 004 * Copyright (C) 2009-2025, by Joseph A. Huwaldt. All rights reserved. 005 * 006 * This library is free software; you can redistribute it and/or modify it under the terms 007 * of the GNU Lesser General Public License as published by the Free Software Foundation; 008 * either version 2 of the License, or (at your option) any later version. 009 * 010 * This library is distributed in the hope that it will be useful, but WITHOUT ANY 011 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 012 * PARTICULAR PURPOSE. See the GNU Library General Public License for more details. 013 * 014 * You should have received a copy of the GNU Lesser General Public License along with 015 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - 016 * Suite 330, Boston, MA 02111-1307, USA. Or visit: http://www.gnu.org/licenses/lgpl.html 017 */ 018package geomss.geom; 019 020import jahuwaldt.js.param.*; 021import java.text.MessageFormat; 022import java.util.List; 023import java.util.Objects; 024import static java.util.Objects.requireNonNull; 025import java.util.ResourceBundle; 026import javax.measure.quantity.Angle; 027import javax.measure.quantity.Length; 028import javax.measure.unit.SI; 029import javax.measure.unit.Unit; 030import javolution.context.ArrayFactory; 031import javolution.context.ImmortalContext; 032import javolution.context.ObjectFactory; 033import javolution.context.StackContext; 034import static javolution.lang.MathLib.abs; 035import static javolution.lang.MathLib.cos; 036import static javolution.lang.MathLib.sin; 037import static javolution.lang.MathLib.sqrt; 038import javolution.lang.ValueType; 039import javolution.text.Text; 040import javolution.text.TextBuilder; 041import javolution.util.FastTable; 042import javolution.xml.XMLFormat; 043import javolution.xml.XMLSerializable; 044import javolution.xml.stream.XMLStreamException; 045import org.jscience.mathematics.number.Float64; 046import org.jscience.mathematics.vector.Float64Matrix; 047import org.jscience.mathematics.vector.Float64Vector; 048import org.jscience.mathematics.vector.Matrix; 049 050/** 051 * A general 4x4 transformation matrix that transforms {@link GeomPoint} objects. 052 * 053 * <p> Modified by: Joseph A. Huwaldt </p> 054 * 055 * @author Joseph A. Huwaldt, Date: April 29, 2009 056 * @version February 17, 2025 057 */ 058@SuppressWarnings("serial") 059public class GTransform implements ValueType, Cloneable, XMLSerializable { 060 061 /** 062 * The resource bundle for this package. 063 */ 064 private static final ResourceBundle RESOURCES = AbstractGeomElement.RESOURCES; 065 066 /** 067 * Constant used to identify the X coordinate in a vector and the 1st row/column in 068 * this matrix. 069 */ 070 private static final int X = 0; 071 072 /** 073 * Constant used to identify the Y coordinate in a vector and the 2nd row/column in 074 * this matrix. 075 */ 076 private static final int Y = 1; 077 078 /** 079 * Constant used to identify the Z coordinate in a vector and the 3rd row/column in 080 * this matrix. 081 */ 082 private static final int Z = 2; 083 084 /** 085 * Constant used to identify the 4th row/column in this matrix. 086 */ 087 private static final int W = 3; 088 089 /** 090 * The universal translation units used. The translation elements of a GTransform 091 * are always stated in meters. 092 */ 093 private static final Unit<Length> METER = SI.METER; 094 095 /////////////////////// 096 // Factory creation. // 097 /////////////////////// 098 private GTransform() { } 099 100 @SuppressWarnings("unchecked") 101 private static final ObjectFactory<GTransform> FACTORY = new ObjectFactory<GTransform>() { 102 @Override 103 protected GTransform create() { 104 return new GTransform(); 105 } 106 }; 107 108 @SuppressWarnings("unchecked") 109 private static GTransform copyOf(GTransform original) { 110 GTransform M = FACTORY.object(); 111 M._data = original._data.copy(); 112 return M; 113 } 114 115 @SuppressWarnings("unchecked") 116 private static GTransform newInstance(Float64Matrix data) { 117 GTransform M = FACTORY.object(); 118 M._data = data; 119 return M; 120 } 121 122 // The bottom row of many new matricies. 123 private static final Float64Vector BOTTOM_ROW; 124 125 /** 126 * A matrix containing all zero elements. 127 */ 128 public static final GTransform ZERO; 129 130 /** 131 * The identity matrix containing ones along the diagonal. 132 */ 133 public static final GTransform IDENTITY; 134 135 static { 136 // Forces constants to ImmortalMemory. 137 ImmortalContext.enter(); 138 try { 139 BOTTOM_ROW = Float64Vector.valueOf(0, 0, 0, 1); 140 ZERO = GTransform.valueOf(0, 0, 0, 0); 141 IDENTITY = GTransform.valueOf(1, 1, 1, 1); 142 } finally { 143 ImmortalContext.exit(); 144 } 145 } 146 147 /** 148 * The elements stored in this matrix. Serialization is handled by this class since 149 * Float64Matrix is not Serializable. 150 */ 151 private transient Float64Matrix _data; 152 153 /** 154 * Returns a transformation matrix holding the specified row vectors. Note: any 155 * transformations in the 4th column of the matrix must have units of meters! 156 * 157 * @param row0 the 1st row vector. May not be null. 158 * @param row1 the 2nd row vector. May not be null. 159 * @param row2 the 3rd row vector. May not be null. 160 * @param row3 the 4th row vector. May not be null. 161 * @return the transformation matrix having the specified rows. 162 * @throws DimensionException if the any of the rows do not have a dimension of 4. 163 */ 164 public static GTransform valueOf(Float64Vector row0, Float64Vector row1, Float64Vector row2, Float64Vector row3) { 165 if (row0.getDimension() != 4 || row1.getDimension() != 4 166 || row2.getDimension() != 4 || row3.getDimension() != 4) 167 throw new DimensionException(RESOURCES.getString("rowsMustHave4Elements")); 168 169 GTransform M = GTransform.newInstance(Float64Matrix.valueOf(row0, row1, row2, row3)); 170 171 return M; 172 } 173 174 /** 175 * Returns a transformation matrix holding the row vectors from the specified list. 176 * Note: any transformations in the 4th column of the matrix must have units of 177 * meters! 178 * 179 * @param rows The list of 4 row vectors. May not be null. 180 * @return the transformation matrix having the specified rows. 181 * @throws DimensionException if the rows do not have a dimension of 4. 182 */ 183 public static GTransform valueOf(List<Float64Vector> rows) { 184 if (rows.size() != 4) 185 throw new DimensionException(RESOURCES.getString("fourVectorsReq")); 186 187 return GTransform.newInstance(Float64Matrix.valueOf(rows)); 188 } 189 190 /** 191 * Returns a transformation matrix holding the specified diagonal vector with zeros 192 * placed in all the other elements. 193 * 194 * @param diagX the 1st element of the diagonal vector for the matrix. 195 * @param diagY the 2nd element of the diagonal vector for the matrix. 196 * @param diagZ the 3rd element of the diagonal vector for the matrix. 197 * @param diagW the 4th element of the diagonal vector for the matrix. 198 * @return the transformation matrix having the specified diagonal. 199 */ 200 public static GTransform valueOf(double diagX, double diagY, double diagZ, double diagW) { 201 202 Float64Vector row0 = Float64Vector.valueOf(diagX, 0, 0, 0); 203 Float64Vector row1 = Float64Vector.valueOf(0, diagY, 0, 0); 204 Float64Vector row2 = Float64Vector.valueOf(0, 0, diagZ, 0); 205 Float64Vector row3 = Float64Vector.valueOf(0, 0, 0, diagW); 206 207 GTransform M = GTransform.newInstance(Float64Matrix.valueOf(row0, row1, row2, row3)); 208 209 return M; 210 } 211 212 /** 213 * Returns a {@link GTransform} instance containing a copy of the specified matrix of 214 * Float64 values. The matrix must have dimensions of either 3x3 for a combined scale 215 * and direction cosine matrix or 4x4 for a general transformation matrix. If a 3x3 216 * matrix is input, then the upper-left 3x3 portion of the output matrix will be set 217 * to those values and the other elements will be set as if it were an identity matrix 218 * (i.e., affine matrix with no translational component). If a 4x4 matrix is input, 219 * then the 4th column (the translation) must be in units of meters! 220 * 221 * @param matrix the matrix of Float64 values to convert (must have dimension of 3x3 222 * or 4x4). May not be null. 223 * @return the transformation matrix having the specified elements. 224 */ 225 public static GTransform valueOf(Matrix<Float64> matrix) { 226 GTransform M = null; 227 228 if (matrix.getNumberOfColumns() == 3 && matrix.getNumberOfRows() == 3) { 229 FastTable<Float64Vector> rowList = FastTable.newInstance(); 230 231 for (int i = 0; i < 3; ++i) { 232 Float64Vector row = Float64Vector.valueOf(matrix.get(i, X).doubleValue(), 233 matrix.get(i, Y).doubleValue(), matrix.get(i, Z).doubleValue(), 0); 234 rowList.add(row); 235 } 236 rowList.add(BOTTOM_ROW); 237 238 M = GTransform.valueOf(rowList); 239 240 } else if (matrix.getNumberOfColumns() == 4 && matrix.getNumberOfRows() == 4) { 241 M = GTransform.newInstance(Float64Matrix.valueOf(matrix)); 242 243 } else 244 throw new DimensionException(RESOURCES.getString("3x3or4x4Req")); 245 246 return M; 247 } 248 249 /** 250 * Returns a {@link GTransform} instance containing the specified direction cosine 251 * rotation matrix, uniform scale factor, and translation vector. 252 * 253 * @param dcm The direction cosine matrix to convert (must have dimension of 3x3). 254 * May not be null. 255 * @param scale The uniform scale factor to apply to each axis. 256 * @param trans The amount to translate in each axis X, Y & Z. 257 * @return the transformation matrix having the specified elements. 258 */ 259 public static GTransform valueOf(Matrix<Float64> dcm, double scale, Vector<Length> trans) { 260 261 if (dcm.getNumberOfColumns() != 3 && dcm.getNumberOfRows() != 3) 262 throw new DimensionException(RESOURCES.getString("3x3DCMReq")); 263 264 // Convert translation vector to meters. 265 Float64Vector T = trans.to(METER).toFloat64Vector(); 266 267 // Create a list of rows with the input elements. 268 FastTable<Float64Vector> rowList = FastTable.newInstance(); 269 for (int i = 0; i < 3; ++i) { 270 double iX = dcm.get(i, X).doubleValue() * scale; 271 double iY = dcm.get(i, Y).doubleValue() * scale; 272 double iZ = dcm.get(i, Z).doubleValue() * scale; 273 Float64Vector row = Float64Vector.valueOf(iX, iY, iZ, T.getValue(i)); 274 rowList.add(row); 275 } 276 rowList.add(BOTTOM_ROW); 277 278 // Create the transformation matrix. 279 GTransform M = GTransform.valueOf(rowList); 280 281 return M; 282 } 283 284 /** 285 * Returns a {@link GTransform} instance containing a copy of the specified rotation 286 * transformation. The upper-left 3x3 portion of the output matrix will be set to DCM 287 * values from the rotation and the other elements will be set as if it were an 288 * identity matrix (i.e., affine matrix with no translational component). 289 * 290 * @param rotation the Rotation transformation to convert. May not be null. 291 * @return the general transformation matrix having the specified rotational elements. 292 */ 293 public static GTransform valueOf(Rotation rotation) { 294 return valueOf(rotation.toDCM().toFloat64Matrix()); 295 } 296 297 /** 298 * Returns a {@link GTransform} instance containing the specified rotation 299 * transformation, uniform scale factor, and translation vector. 300 * 301 * @param rotation The rotation transformation to convert. May not be null. 302 * @param scale The uniform scale factor to apply to each axis. 303 * @param trans The amount to translate in each axis X, Y & Z. May not be null. 304 * @return the transformation matrix having the specified elements. 305 */ 306 public static GTransform valueOf(Rotation rotation, double scale, Vector<Length> trans) { 307 return valueOf(rotation.toDCM().toFloat64Matrix(), scale, requireNonNull(trans)); 308 } 309 310 /** 311 * Returns a {@link GTransform} instance representing the transformation from one axis 312 * direction directly to another. Input vectors must be 3D. 313 * 314 * @param srcAxis A vector representing the 1st or starting axis. May not be null. 315 * @param destAxis A vector representing the 2nd or ending axis. May not be null. 316 * @return The transformation matrix representing the rotation from the 1st (source) 317 * axis directly to the 2nd (destination) axis. 318 */ 319 public static GTransform valueOf(GeomVector srcAxis, GeomVector destAxis) { 320 GeomVector axis = srcAxis.cross(requireNonNull(destAxis)); // Find a perpendicular axis to rotate about. 321 Parameter length = axis.mag(); 322 Parameter dot = srcAxis.dot(destAxis); 323 324 if (!length.isApproxZero()) { 325 Vector3D v3d = axis.toUnitVector().toParameterVector().toVector3D(); 326 Parameter<Angle> angle = Parameter.atan2(length, dot); 327 328 Rotation R = AxisAngle.valueOf(v3d, angle); 329 return GTransform.valueOf(R); 330 331 } else if (dot.isGreaterThan(Parameter.ZERO)) { 332 // Nearly positively aligned; skip rotation, or compute 333 // axis and angle using other means. 334 return GTransform.IDENTITY; 335 } 336 337 // Nearly negatively aligned; axis is any vector perpendicular 338 // to either vector, and angle is 180 degrees 339 axis = GeomUtil.calcYHat(srcAxis); 340 Vector3D v3d = axis.toParameterVector().toVector3D(); 341 Parameter<Angle> angle = Parameter.PI_ANGLE; 342 343 Rotation R = AxisAngle.valueOf(v3d, angle); 344 return GTransform.valueOf(R); 345 } 346 347 /** 348 * Returns a transformation matrix representing a scale matrix with a uniform scale 349 * factor applied to each dimension. 350 * 351 * @param scale The scale factor to create a scale matrix for. 352 * @return the transformation matrix representing the specified scale. 353 */ 354 public static GTransform newScale(double scale) { 355 return valueOf(scale, scale, scale, 1); 356 } 357 358 /** 359 * Returns a transformation matrix representing a translation matrix. 360 * 361 * @param transX The amount to translate, in meters, in the X axis direction. 362 * @param transY The amount to translate, in meters, in the Y axis direction. 363 * @param transZ The amount to translate, in meters, in the Z axis direction. 364 * @return the transformation matrix representing the specified translation. 365 * @throws DimensionException if the input vector does not have 3 elements. 366 */ 367 public static GTransform newTranslation(double transX, double transY, double transZ) { 368 369 Float64Vector row0 = Float64Vector.valueOf(1, 0, 0, transX); 370 Float64Vector row1 = Float64Vector.valueOf(0, 1, 0, transY); 371 Float64Vector row2 = Float64Vector.valueOf(0, 0, 1, transZ); 372 373 GTransform T = GTransform.valueOf(row0, row1, row2, BOTTOM_ROW); 374 375 return T; 376 } 377 378 /** 379 * Returns a transformation matrix representing a translation matrix. 380 * 381 * @param transX The amount to translate in the X axis direction. May not be null. 382 * @param transY The amount to translate in the Y axis direction. May not be null. 383 * @param transZ The amount to translate in the Z axis direction. May not be null. 384 * @return the transformation matrix representing the specified translation. 385 * @throws DimensionException if the input vector does not have 3 elements. 386 */ 387 public static GTransform newTranslation(Parameter<Length> transX, 388 Parameter<Length> transY, Parameter<Length> transZ) { 389 390 GTransform T = GTransform.newTranslation(transX.getValue(METER), 391 transY.getValue(METER), transZ.getValue(METER)); 392 393 return T; 394 } 395 396 /** 397 * Returns a transformation matrix representing a translation matrix. 398 * 399 * @param vector The amount to translate in each axis X, Y & Z. May not be null. 400 * @return the transformation matrix representing the specified translation. 401 */ 402 public static GTransform newTranslation(GeomVector<Length> vector) { 403 404 // Convert the translation to meters and create a new 405 // transformation matrix. 406 GTransform T = GTransform.newTranslation( 407 vector.getValue(X, METER), vector.getValue(Y, METER), vector.getValue(Z, METER)); 408 409 return T; 410 } 411 412 /** 413 * Returns a transformation matrix representing a translation matrix. 414 * 415 * @param trans The amount to translate, in meters, in each axis X, Y & Z. May not be null. 416 * @return the transformation matrix representing the specified translation. 417 * @throws DimensionException if the input vector does not have 3 elements. 418 */ 419 public static GTransform newTranslation(Float64Vector trans) { 420 421 if (trans.getDimension() != 3) 422 throw new DimensionException(RESOURCES.getString("3elementVectorReq")); 423 424 return newTranslation(trans.getValue(X), trans.getValue(Y), trans.getValue(Z)); 425 } 426 427 /** 428 * Return a transformation matrix representing a counter-clockwise or right-handed 429 * rotation about the X axis (when looking down the X-axis). 430 * 431 * @param angle The angle to rotate about the X axis. May not be null. 432 * @return A transformation matrix for a counter-clockwise rotation about the X axis. 433 */ 434 public static GTransform newRotationX(Parameter<Angle> angle) { 435 double angleR = angle.getValue(SI.RADIAN); 436 double sina = sin(angleR); 437 double cosa = cos(angleR); 438 439 Float64Vector row0 = Float64Vector.valueOf( 1, 0, 0, 0); 440 Float64Vector row1 = Float64Vector.valueOf( 0, cosa,-sina, 0); 441 Float64Vector row2 = Float64Vector.valueOf( 0, sina, cosa, 0); 442// Float64Vector row3 = Float64Vector.valueOf( 0, 0, 0, 1); 443 444 GTransform T = GTransform.valueOf(row0, row1, row2, BOTTOM_ROW); 445 446 return T; 447 } 448 449 /** 450 * Return a transformation matrix representing a counter-clockwise or right-handed 451 * rotation about the Y axis (when looking down the Y-axis). 452 * 453 * @param angle The angle to rotate about the Y axis. May not be null. 454 * @return A transformation matrix for a counter-clockwise rotation about the Y axis. 455 */ 456 public static GTransform newRotationY(Parameter<Angle> angle) { 457 double angleR = angle.getValue(SI.RADIAN); 458 double sina = sin(angleR); 459 double cosa = cos(angleR); 460 461 Float64Vector row0 = Float64Vector.valueOf( cosa, 0, sina, 0); 462 Float64Vector row1 = Float64Vector.valueOf( 0, 1, 0, 0); 463 Float64Vector row2 = Float64Vector.valueOf(-sina, 0, cosa, 0); 464// Float64Vector row3 = Float64Vector.valueOf( 0, 0, 0, 1); 465 466 GTransform T = GTransform.valueOf(row0, row1, row2, BOTTOM_ROW); 467 468 return T; 469 } 470 471 /** 472 * Return a transformation matrix representing a counter-clockwise or right-handed 473 * rotation about the Z axis (when looking down the Z-axis). 474 * 475 * @param angle The angle to rotate about the Z axis. May not be null. 476 * @return A transformation matrix for a counter-clockwise rotation about the Z axis. 477 */ 478 public static GTransform newRotationZ(Parameter<Angle> angle) { 479 double angleR = angle.getValue(SI.RADIAN); 480 double sina = sin(angleR); 481 double cosa = cos(angleR); 482 483 Float64Vector row0 = Float64Vector.valueOf(cosa, -sina, 0, 0); 484 Float64Vector row1 = Float64Vector.valueOf(sina, cosa, 0, 0); 485 Float64Vector row2 = Float64Vector.valueOf(0, 0, 1, 0); 486// Float64Vector row3 = Float64Vector.valueOf( 0, 0, 0, 1); 487 488 GTransform T = GTransform.valueOf(row0, row1, row2, BOTTOM_ROW); 489 490 return T; 491 } 492 493 /** 494 * Returns the number of rows for this matrix. This implementation always returns 4. 495 * 496 * @return The number of rows in this matrix (always 4). 497 */ 498 public int getNumberOfRows() { 499 return 4; 500 } 501 502 /** 503 * Returns the number of columns for this matrix. This implementation always returns 4. 504 * 505 * @return The number of columns in this matrix (always 4). 506 */ 507 public int getNumberOfColumns() { 508 return 4; 509 } 510 511 /** 512 * Returns a single element from this matrix. 513 * 514 * @param i the row index (range [0..3[). 515 * @param j the column index (range [0..3[). 516 * @return the matrix element at [i,j]. 517 */ 518 public Float64 get(int i, int j) { 519 return _data.get(i, j); 520 } 521 522 /** 523 * Returns a single element from this matrix as a <code>double</code>. 524 * <p> 525 * This is a convenience method for: <code>this.get(i,j).doubleValue()</code> 526 * </p> 527 * 528 * @param i the row index (range [0..3[). 529 * @param j the column index (range [0..3[). 530 * @return the matrix element at [i,j]. 531 */ 532 public double getValue(int i, int j) { 533 return _data.get(i, j).doubleValue(); 534 } 535 536 /** 537 * Return the transformation matrix that represents this GTransform object as a 538 * Float64Matrix. The translation column values have units of METER. 539 * 540 * @return The transformation matrix that represents this GTransform object. 541 */ 542 public Float64Matrix getFloat64Matrix() { 543 return _data; 544 } 545 546 /** 547 * Return the transformation matrix that represents this GTranfrom object as a 2D Java 548 * matrix. The translation column values have units of METER. 549 * 550 * @return The transformation matrix that represents this GTranfrom object. 551 */ 552 public double[][] getMatrix() { 553 StackContext.enter(); 554 try { 555 double[][] mat = new double[4][4]; 556 for (int i = 0; i < 4; ++i) { 557 for (int j = 0; j < 4; ++j) { 558 mat[i][j] = _data.get(i, j).doubleValue(); 559 } 560 } 561 return mat; 562 } finally { 563 StackContext.exit(); 564 } 565 } 566 567 /** 568 * Returns the row identified by the specified index in this matrix. 569 * 570 * @param i the row index (range [0..3[). 571 * @return The specified row in this matrix as a Float64Vector. 572 */ 573 public Float64Vector getRow(int i) { 574 return _data.getRow(i); 575 } 576 577 /** 578 * Returns the column identified by the specified index in this matrix. 579 * 580 * @param j the column index (range [0..3[). 581 * @return The specified column in this matrix as a Float64Vector. 582 */ 583 public Float64Vector getColumn(int j) { 584 return _data.getColumn(j); 585 } 586 587 /** 588 * Returns the diagonal vector. 589 * 590 * @return the vector holding the diagonal elements. 591 */ 592 public Float64Vector getDiagonal() { 593 return _data.getDiagonal(); 594 } 595 596 /** 597 * Transform the input point by multiplying it times this general transformation 598 * matrix. 599 * 600 * @param p The point to be transformed. May not be null. 601 * @return <code>this · p</code> 602 * @throws DimensionException if p.getPhyDimension() != 3 603 */ 604 public Point transform(GeomPoint p) { 605 if (p.getPhyDimension() != 3) 606 throw new DimensionException( 607 MessageFormat.format(RESOURCES.getString("dimensionNot3"), "point's", p.getPhyDimension())); 608 609 Unit<Length> pUnit = p.getUnit(); 610 Float64Vector V; 611 StackContext.enter(); 612 try { 613 // Convert the point to units of meters (to be consistant with any translation in 614 // this transformation matrix). 615 616 // Convert the point to a 4 element vector. 617 V = Float64Vector.valueOf(p.getValue(X, METER), p.getValue(Y, METER), p.getValue(Z, METER), 1); 618 619 // Multiply times the general transformation matrix. 620 V = _data.times(V); 621 622 V = StackContext.outerCopy(V); 623 624 } finally { 625 StackContext.exit(); 626 } 627 628 // Create output point. 629 Point Pout = Point.valueOf(V.getValue(X), V.getValue(Y), V.getValue(Z), METER); 630 return Pout.to(pUnit); 631 } 632 633 /** 634 * Returns the product of this matrix with the one specified. 635 * 636 * @param that the matrix multiplier. May not be null. 637 * @return <code>this · that</code> 638 */ 639 public GTransform times(GTransform that) { 640 641 GTransform M = GTransform.newInstance(this._data.times(that._data)); 642 643 return M; 644 } 645 646 /** 647 * Returns the inverse of this matrix. If the matrix is singular, the 648 * {@link #determinant()} will be zero (or nearly zero). 649 * 650 * @return <code>1 / this</code> 651 * @see #determinant 652 */ 653 public GTransform inverse() { 654 655 GTransform M = GTransform.newInstance(_data.inverse()); 656 657 return M; 658 } 659 660 /** 661 * Returns the determinant of this matrix. 662 * 663 * @return this matrix determinant. 664 */ 665 public Float64 determinant() { 666 return _data.determinant(); 667 } 668 669 /** 670 * Returns the transpose of this matrix. 671 * 672 * @return <code>A'</code> 673 */ 674 public GTransform transpose() { 675 676 GTransform M = GTransform.newInstance(_data.transpose()); 677 678 return M; 679 } 680 681 /** 682 * Returns the vectorization of this matrix. The vectorization of a matrix is the 683 * column vector obtained by stacking the columns of the matrix on top of one another. 684 * 685 * @return the vectorization of this matrix. 686 */ 687 public Float64Vector vectorization() { 688 return _data.vectorization(); 689 } 690 691 /** 692 * Returns a copy of this matrix allocated by the calling thread (possibly on the 693 * stack). 694 * 695 * @return an identical and independent copy of this matrix. 696 */ 697 @Override 698 public GTransform copy() { 699 return GTransform.copyOf(this); 700 } 701 702 /** 703 * Returns a copy of this GTransform instance 704 * {@link javolution.context.AllocatorContext allocated} by the calling thread 705 * (possibly on the stack). 706 * 707 * @return an identical and independent copy of this matrix. 708 * @throws java.lang.CloneNotSupportedException Never thrown. 709 */ 710 @Override 711 @SuppressWarnings("CloneDoesntCallSuperClone") 712 public Object clone() throws CloneNotSupportedException { 713 return copy(); 714 } 715 716 /** 717 * Returns the text representation of this matrix. 718 * 719 * @return the text representation of this matrix. 720 */ 721 public Text toText() { 722 final int m = this.getNumberOfRows(); 723 final int n = this.getNumberOfColumns(); 724 TextBuilder tmp = TextBuilder.newInstance(); 725 tmp.append('{'); 726 for (int i = 0; i < m; i++) { 727 tmp.append('{'); 728 for (int j = 0; j < n; j++) { 729 tmp.append(get(i, j)); 730 if (j != n - 1) { 731 tmp.append(", "); 732 } 733 } 734 tmp.append("}"); 735 if (i != m - 1) { 736 tmp.append(",\n"); 737 } 738 } 739 tmp.append("}"); 740 Text txt = tmp.toText(); 741 TextBuilder.recycle(tmp); 742 return txt; 743 } 744 745 /** 746 * Returns a string representation of this matrix. 747 */ 748 @Override 749 public String toString() { 750 return toText().toString(); 751 } 752 753 /** 754 * Compares this GTransform against the specified object for strict equality (same 755 * values). 756 * 757 * @param obj the object to compare with. 758 * @return <code>true</code> if this transform is identical to that transform; 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 GTransform that = (GTransform)obj; 769 770 return this._data.equals(that._data); 771 } 772 773 /** 774 * Returns the hash code for this GTransform object. 775 * 776 * @return the hash code value. 777 */ 778 @Override 779 public int hashCode() { 780 return Objects.hash(_data); 781 } 782 783 /** 784 * Returns the uniform scale factor for this matrix. If the matrix has non-uniform 785 * scale factors, than the maximum of the X, Y & Z scale factor is returned. 786 * 787 * @return the uniform or maximum scale factor. 788 */ 789 public double getScale() { 790 791 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 792 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 793 getScaleRotate(tmp_scale, tmp_rot); 794 795 double scale = max3(tmp_scale); 796 797 // Recycle scratch arrays. 798 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_rot); 799 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_scale); 800 801 return scale; 802 } 803 804 /** 805 * Returns a 3 element vector containing the scale factors in each dimension X, Y & Z. 806 * 807 * @return the scale factors of this matrix 808 */ 809 public Float64Vector getScaleVector() { 810 811 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 812 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 813 getScaleRotate(tmp_scale, tmp_rot); 814 815 // Create the output vector (Note: tmp_scale may be longer than 3 elements). 816 Float64Vector scale = Float64Vector.valueOf(tmp_scale[X], tmp_scale[Y], tmp_scale[Z]); 817 818 // Recycle scratch arrays. 819 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_rot); 820 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_scale); 821 822 return scale; 823 } 824 825 /** 826 * Returns a direction cosine matrix containing the rotational portion of this 827 * transformation matrix. 828 * 829 * @return the direction cosine matrix 830 */ 831 public DCMatrix getRotation() { 832 833 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 834 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 835 getScaleRotate(tmp_scale, tmp_rot); 836 837 // Create the output vector (Note: tmp_rot may be longer than 9 elements). 838 DCMatrix rot = DCMatrix.valueOf(tmp_rot[0], tmp_rot[1], tmp_rot[2], 839 tmp_rot[3], tmp_rot[4], tmp_rot[5], 840 tmp_rot[6], tmp_rot[7], tmp_rot[8]); 841 842 // Recycle scratch arrays. 843 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_rot); 844 ArrayFactory.DOUBLES_FACTORY.recycle(tmp_scale); 845 846 return rot; 847 } 848 849 /** 850 * Returns the upper 3x3 values of this matrix (which contains combined rotation and 851 * scale information) and places them into the output matrix. 852 * 853 * @return A 3x3 matrix representing the combined rotation and scaling of this matrix. 854 */ 855 public Matrix<Float64> getRotationScale() { 856 DCMatrix M = DCMatrix.valueOf(getValue(X, X), getValue(X, Y), getValue(X, Z), 857 getValue(Y, X), getValue(Y, Y), getValue(Y, Z), 858 getValue(Z, X), getValue(Z, Y), getValue(Z, Z)); 859 return M.toFloat64Matrix(); 860 } 861 862 /** 863 * Returns the translational components of this transformation matrix. 864 * 865 * @return The translational components of this transformation matrix. 866 */ 867 public Vector<Length> getTranslation() { 868 return Vector.valueOf(METER, getValue(X, W), getValue(Y, W), getValue(Z, W)); 869 } 870 871 /** 872 * Return a new transformation matrix that is identical to this one, but with the 873 * specified uniform scale factor applied. 874 * 875 * @param scale The scale factor to apply to this transformation. 876 * @return A new GTransform that is identical to this one, but with the specified 877 * uniform scale factor applied. 878 */ 879 public GTransform applyScale(double scale) { 880 881 /* Sets the scale component of the current matrix by factoring 882 out the current scale (by doing an SVD) from the rotational 883 component and multiplying by the new scale. 884 */ 885 886 StackContext.enter(); 887 try { 888 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 889 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 890 getScaleRotate(tmp_scale, tmp_rot); 891 892 Float64Vector row0 = Float64Vector.valueOf(tmp_rot[0] * scale, tmp_rot[1] * scale, tmp_rot[2] * scale, getValue(X, W)); 893 Float64Vector row1 = Float64Vector.valueOf(tmp_rot[3] * scale, tmp_rot[4] * scale, tmp_rot[5] * scale, getValue(Y, W)); 894 Float64Vector row2 = Float64Vector.valueOf(tmp_rot[6] * scale, tmp_rot[7] * scale, tmp_rot[8] * scale, getValue(Z, W)); 895 Float64Vector row3 = getRow(3); 896 897 GTransform T = GTransform.newInstance(Float64Matrix.valueOf(row0, row1, row2, row3)); 898 899 return StackContext.outerCopy(T); 900 901 } finally { 902 StackContext.exit(); 903 } 904 } 905 906 /** 907 * Return a new transformation matrix that is identical to this one, but with the 908 * specified set of three scale factors applied in each axis (X,Y,Z). 909 * 910 * @param scale The scale factor in each dimension (X, Y & Z) to apply to this 911 * transformation. May not be null. 912 * @return A new GTransform that is identical to this one, but with the specified set 913 * of three scale factors applied. 914 * @throws DimensionException if the input list of scale factors does not have 3 915 * elements. 916 */ 917 public GTransform applyScale(Float64Vector scale) { 918 if (scale.getDimension() != 3) 919 throw new DimensionException(RESOURCES.getString("3elementScaleFReq")); 920 921 /* Sets the scale component of the current matrix by factoring 922 out the current scale (by doing an SVD) from the rotational 923 component and multiplying by the new scale. 924 */ 925 926 StackContext.enter(); 927 try { 928 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 929 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 930 getScaleRotate(tmp_scale, tmp_rot); 931 932 FastTable<Float64Vector> rowList = FastTable.newInstance(); 933 for (int i = 0; i < 3; ++i) { 934 double iX = getValue(i, X) * scale.getValue(X); 935 double iY = getValue(i, Y) * scale.getValue(Y); 936 double iZ = getValue(i, Z) * scale.getValue(Z); 937 Float64Vector row = Float64Vector.valueOf(iX, iY, iZ, getValue(i, W)); 938 rowList.add(row); 939 } 940 rowList.add(getRow(3)); 941 942 GTransform T = GTransform.valueOf(rowList); 943 944 return StackContext.outerCopy(T); 945 } finally { 946 StackContext.exit(); 947 } 948 } 949 950 /** 951 * Return a new transformation matrix that is identical to this one, but with the 952 * rotational components replaced with those in the specified direction cosine matrix. 953 * The effect of scale is factored out, then the new rotation components are placed in 954 * the upper-left 3x3 portion of the matrix and then the scale is reapplied. 955 * 956 * @param m1 The direction cosine matrix to apply to this matrix. May not be null. 957 * @return A new GTransform that is identical to this one, but with the rotational 958 * components replaced with those in the specified direction cosine matrix. 959 * @throws DimensionException if matrix is not 3x3 960 */ 961 public GTransform applyRotation(Matrix<Float64> m1) { 962 963 if (m1.getNumberOfColumns() != 3 && m1.getNumberOfRows() != 3) 964 throw new DimensionException(RESOURCES.getString("3x3DCMReq")); 965 966 /* Sets the rotational component of the current matrix by factoring 967 out the current scale (by doing an SVD) from the rotational 968 component, replacing the upper-left 3x3 with the input matrix 969 and then reapplying the old scale to the new rotational components. 970 */ 971 972 StackContext.enter(); 973 try { 974 double[] tmp_rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; // scratch matrix 975 double[] tmp_scale = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; // scratch matrix 976 getScaleRotate(tmp_scale, tmp_rot); 977 978 FastTable<Float64Vector> rowList = FastTable.newInstance(); 979 for (int i = 0; i < 3; ++i) { 980 double iX = m1.get(i, X).doubleValue() * tmp_scale[X]; 981 double iY = m1.get(i, Y).doubleValue() * tmp_scale[Y]; 982 double iZ = m1.get(i, Z).doubleValue() * tmp_scale[Z]; 983 Float64Vector row = Float64Vector.valueOf(iX, iY, iZ, getValue(i, W)); 984 rowList.add(row); 985 } 986 rowList.add(getRow(3)); 987 988 GTransform T = GTransform.valueOf(rowList); 989 990 return StackContext.outerCopy(T); 991 992 } finally { 993 StackContext.exit(); 994 } 995 } 996 997 /** 998 * Return a new transformation matrix with the upper-left 3x3 portion of this matrix 999 * (rotation and scaling) replaced by the specified values. The remaining elements of 1000 * the matrix are left unchanged. 1001 * 1002 * @param m1 The 3x3 matrix that will be the new upper left portion. May not be null. 1003 * @return A new GTransform with the upper-left 3x3 portion of this matrix replaced by 1004 * the specified values. 1005 * @throws DimensionException if m1 is not 3x3 1006 */ 1007 public GTransform applyRotationScale(Matrix<Float64> m1) { 1008 1009 if (m1.getNumberOfColumns() != 3 && m1.getNumberOfRows() != 3) 1010 throw new DimensionException(RESOURCES.getString("3x3MatrixReq")); 1011 1012 StackContext.enter(); 1013 try { 1014 FastTable<Float64Vector> rowList = FastTable.newInstance(); 1015 for (int i = 0; i < 3; ++i) { 1016 double iX = m1.get(i, X).doubleValue(); 1017 double iY = m1.get(i, Y).doubleValue(); 1018 double iZ = m1.get(i, Z).doubleValue(); 1019 Float64Vector row = Float64Vector.valueOf(iX, iY, iZ, getValue(i, W)); 1020 rowList.add(row); 1021 } 1022 rowList.add(getRow(3)); 1023 1024 GTransform T = GTransform.valueOf(rowList); 1025 1026 return StackContext.outerCopy(T); 1027 1028 } finally { 1029 StackContext.exit(); 1030 } 1031 } 1032 1033 /** 1034 * Return a new transformation matrix that is identical to this one, but with the 1035 * translation components replaced by the specified values. The remaining elements of 1036 * the matrix are left unchanged. 1037 * 1038 * @param trans The 3 element translation offsets in (X, Y & Z). May not be null. 1039 * @return A new GTransform that is identical to this one, but with the translation 1040 * components replaced by the specified values. 1041 */ 1042 public GTransform applyTranslation(Vector<Length> trans) { 1043 1044 // Convert the translation to meters and then compute the new 1045 // transformation matrix. 1046 GTransform T = this.applyTranslation(trans.to(METER).toFloat64Vector()); 1047 1048 return T; 1049 } 1050 1051 /** 1052 * Return a new transformation matrix that is identical to this one, but with the 1053 * translation components replaced by the specified values. The remaining elements of 1054 * the matrix are left unchanged. 1055 * 1056 * @param trans The 3 element translation offsets in meters (X, Y & Z). May not be null. 1057 * @return A new GTransform that is identical to this one, but with the translation 1058 * components replaced by the specified values. 1059 * @throws DimensionException if input vector does not have exactly 3 elements. 1060 */ 1061 public GTransform applyTranslation(Float64Vector trans) { 1062 if (trans.getDimension() != 3) 1063 throw new DimensionException(RESOURCES.getString("3elementVectorReq")); 1064 1065 StackContext.enter(); 1066 try { 1067 FastTable<Float64Vector> rowList = FastTable.newInstance(); 1068 for (int i = 0; i < 3; ++i) { 1069 double iX = getValue(i, X); 1070 double iY = getValue(i, Y); 1071 double iZ = getValue(i, Z); 1072 Float64Vector row = Float64Vector.valueOf(iX, iY, iZ, trans.getValue(i)); 1073 rowList.add(row); 1074 } 1075 rowList.add(getRow(3)); 1076 1077 GTransform T = GTransform.valueOf(rowList); 1078 1079 return StackContext.outerCopy(T); 1080 1081 } finally { 1082 StackContext.exit(); 1083 } 1084 } 1085 1086 /** 1087 * During serialization, this will write out the Float64Matrix as a simple series of 1088 * <code>double</code> values. This method is ONLY called by the Java Serialization 1089 * mechanism and should not otherwise be used. 1090 */ 1091 private void writeObject(java.io.ObjectOutputStream out) throws java.io.IOException { 1092 1093 // Call the default write object method. 1094 out.defaultWriteObject(); 1095 1096 // Write out the matrix values. 1097 for (int i = 0; i < 4; ++i) 1098 for (int j = 0; j < 4; ++j) 1099 out.writeDouble(_data.get(i, j).doubleValue()); 1100 1101 } 1102 1103 /** 1104 * During de-serialization, this will handle the reconstruction of the Float64Matrix. 1105 * This method is ONLY called by the Java Serialization mechanism and should not 1106 * otherwise be used. 1107 */ 1108 private void readObject(java.io.ObjectInputStream in) throws java.io.IOException, ClassNotFoundException { 1109 1110 // Call the default read object method. 1111 in.defaultReadObject(); 1112 1113 // Read in the matrix values. 1114 FastTable<Float64Vector> rows = FastTable.newInstance(); 1115 for (int i = 0; i < 4; ++i) { 1116 double v1 = in.readDouble(); 1117 double v2 = in.readDouble(); 1118 double v3 = in.readDouble(); 1119 double v4 = in.readDouble(); 1120 Float64Vector row = Float64Vector.valueOf(v1, v2, v3, v4); 1121 rows.add(row); 1122 } 1123 1124 _data = Float64Matrix.valueOf(rows); 1125 1126 } 1127 1128 /** 1129 * ******* The Following taken from javax.vecmath.Matrix3d with minor mods ********** 1130 */ 1131 private void getScaleRotate(double scales[], double rots[]) { 1132 double[] tmp = ArrayFactory.DOUBLES_FACTORY.array(9); // scratch matrix 1133 1134 tmp[0] = getValue(X, X); 1135 tmp[1] = getValue(X, Y); 1136 tmp[2] = getValue(X, Z); 1137 1138 tmp[3] = getValue(Y, X); 1139 tmp[4] = getValue(Y, Y); 1140 tmp[5] = getValue(Y, Z); 1141 1142 tmp[6] = getValue(Z, X); 1143 tmp[7] = getValue(Z, Y); 1144 tmp[8] = getValue(Z, Z); 1145 1146 compute_svd(tmp, scales, rots); 1147 1148 // Recycle the scratch array. 1149 ArrayFactory.DOUBLES_FACTORY.recycle(tmp); 1150 } 1151 1152 private static final double EPS = 1.110223024E-16; 1153 1154 private static void compute_svd(double[] m, double[] outScale, double[] outRot) { 1155 double[] u1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1156 double[] v1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1157 double[] t1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1158 double[] t2 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1159 1160 double[] tmp = t1; 1161 double[] single_values = t2; 1162 1163 double[] rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1164 double[] e = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1165 double[] scales = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1166 1167 try { 1168 System.arraycopy(m, 0, rot, 0, 9); 1169 1170 // u1 1171 if (m[3] * m[3] < EPS) { 1172 u1[0] = 1.0; 1173 u1[1] = 0.0; 1174 u1[2] = 0.0; 1175 u1[3] = 0.0; 1176 u1[4] = 1.0; 1177 u1[5] = 0.0; 1178 u1[6] = 0.0; 1179 u1[7] = 0.0; 1180 u1[8] = 1.0; 1181 } else if (m[0] * m[0] < EPS) { 1182 tmp[0] = m[0]; 1183 tmp[1] = m[1]; 1184 tmp[2] = m[2]; 1185 m[0] = m[3]; 1186 m[1] = m[4]; 1187 m[2] = m[5]; 1188 1189 m[3] = -tmp[0]; // zero 1190 m[4] = -tmp[1]; 1191 m[5] = -tmp[2]; 1192 1193 u1[0] = 0.0; 1194 u1[1] = 1.0; 1195 u1[2] = 0.0; 1196 u1[3] = -1.0; 1197 u1[4] = 0.0; 1198 u1[5] = 0.0; 1199 u1[6] = 0.0; 1200 u1[7] = 0.0; 1201 u1[8] = 1.0; 1202 } else { 1203 double g = 1.0 / sqrt(m[0] * m[0] + m[3] * m[3]); 1204 double c1 = m[0] * g; 1205 double s1 = m[3] * g; 1206 tmp[0] = c1 * m[0] + s1 * m[3]; 1207 tmp[1] = c1 * m[1] + s1 * m[4]; 1208 tmp[2] = c1 * m[2] + s1 * m[5]; 1209 1210 m[3] = -s1 * m[0] + c1 * m[3]; // zero 1211 m[4] = -s1 * m[1] + c1 * m[4]; 1212 m[5] = -s1 * m[2] + c1 * m[5]; 1213 1214 m[0] = tmp[0]; 1215 m[1] = tmp[1]; 1216 m[2] = tmp[2]; 1217 u1[0] = c1; 1218 u1[1] = s1; 1219 u1[2] = 0.0; 1220 u1[3] = -s1; 1221 u1[4] = c1; 1222 u1[5] = 0.0; 1223 u1[6] = 0.0; 1224 u1[7] = 0.0; 1225 u1[8] = 1.0; 1226 } 1227 1228 // u2 1229 if (m[6] * m[6] < EPS) { 1230 } else if (m[0] * m[0] < EPS) { 1231 tmp[0] = m[0]; 1232 tmp[1] = m[1]; 1233 tmp[2] = m[2]; 1234 m[0] = m[6]; 1235 m[1] = m[7]; 1236 m[2] = m[8]; 1237 1238 m[6] = -tmp[0]; // zero 1239 m[7] = -tmp[1]; 1240 m[8] = -tmp[2]; 1241 1242 tmp[0] = u1[0]; 1243 tmp[1] = u1[1]; 1244 tmp[2] = u1[2]; 1245 u1[0] = u1[6]; 1246 u1[1] = u1[7]; 1247 u1[2] = u1[8]; 1248 1249 u1[6] = -tmp[0]; // zero 1250 u1[7] = -tmp[1]; 1251 u1[8] = -tmp[2]; 1252 } else { 1253 double g = 1.0 / sqrt(m[0] * m[0] + m[6] * m[6]); 1254 double c2 = m[0] * g; 1255 double s2 = m[6] * g; 1256 tmp[0] = c2 * m[0] + s2 * m[6]; 1257 tmp[1] = c2 * m[1] + s2 * m[7]; 1258 tmp[2] = c2 * m[2] + s2 * m[8]; 1259 1260 m[6] = -s2 * m[0] + c2 * m[6]; 1261 m[7] = -s2 * m[1] + c2 * m[7]; 1262 m[8] = -s2 * m[2] + c2 * m[8]; 1263 m[0] = tmp[0]; 1264 m[1] = tmp[1]; 1265 m[2] = tmp[2]; 1266 1267 tmp[0] = c2 * u1[0]; 1268 tmp[1] = c2 * u1[1]; 1269 u1[2] = s2; 1270 1271 tmp[6] = -u1[0] * s2; 1272 tmp[7] = -u1[1] * s2; 1273 u1[8] = c2; 1274 u1[0] = tmp[0]; 1275 u1[1] = tmp[1]; 1276 u1[6] = tmp[6]; 1277 u1[7] = tmp[7]; 1278 } 1279 1280 // v1 1281 if (m[2] * m[2] < EPS) { 1282 v1[0] = 1.0; 1283 v1[1] = 0.0; 1284 v1[2] = 0.0; 1285 v1[3] = 0.0; 1286 v1[4] = 1.0; 1287 v1[5] = 0.0; 1288 v1[6] = 0.0; 1289 v1[7] = 0.0; 1290 v1[8] = 1.0; 1291 } else if (m[1] * m[1] < EPS) { 1292 tmp[2] = m[2]; 1293 tmp[5] = m[5]; 1294 tmp[8] = m[8]; 1295 m[2] = -m[1]; 1296 m[5] = -m[4]; 1297 m[8] = -m[7]; 1298 1299 m[1] = tmp[2]; // zero 1300 m[4] = tmp[5]; 1301 m[7] = tmp[8]; 1302 1303 v1[0] = 1.0; 1304 v1[1] = 0.0; 1305 v1[2] = 0.0; 1306 v1[3] = 0.0; 1307 v1[4] = 0.0; 1308 v1[5] = -1.0; 1309 v1[6] = 0.0; 1310 v1[7] = 1.0; 1311 v1[8] = 0.0; 1312 } else { 1313 double g = 1.0 / sqrt(m[1] * m[1] + m[2] * m[2]); 1314 double c3 = m[1] * g; 1315 double s3 = m[2] * g; 1316 tmp[1] = c3 * m[1] + s3 * m[2]; // can assign to m[1]? 1317 m[2] = -s3 * m[1] + c3 * m[2]; // zero 1318 m[1] = tmp[1]; 1319 1320 tmp[4] = c3 * m[4] + s3 * m[5]; 1321 m[5] = -s3 * m[4] + c3 * m[5]; 1322 m[4] = tmp[4]; 1323 1324 tmp[7] = c3 * m[7] + s3 * m[8]; 1325 m[8] = -s3 * m[7] + c3 * m[8]; 1326 m[7] = tmp[7]; 1327 1328 v1[0] = 1.0; 1329 v1[1] = 0.0; 1330 v1[2] = 0.0; 1331 v1[3] = 0.0; 1332 v1[4] = c3; 1333 v1[5] = -s3; 1334 v1[6] = 0.0; 1335 v1[7] = s3; 1336 v1[8] = c3; 1337 } 1338 1339 // u3 1340 if (m[7] * m[7] < EPS) { 1341 } else if (m[4] * m[4] < EPS) { 1342 tmp[3] = m[3]; 1343 tmp[4] = m[4]; 1344 tmp[5] = m[5]; 1345 m[3] = m[6]; // zero 1346 m[4] = m[7]; 1347 m[5] = m[8]; 1348 1349 m[6] = -tmp[3]; // zero 1350 m[7] = -tmp[4]; // zero 1351 m[8] = -tmp[5]; 1352 1353 tmp[3] = u1[3]; 1354 tmp[4] = u1[4]; 1355 tmp[5] = u1[5]; 1356 u1[3] = u1[6]; 1357 u1[4] = u1[7]; 1358 u1[5] = u1[8]; 1359 1360 u1[6] = -tmp[3]; // zero 1361 u1[7] = -tmp[4]; 1362 u1[8] = -tmp[5]; 1363 1364 } else { 1365 double g = 1.0 / sqrt(m[4] * m[4] + m[7] * m[7]); 1366 double c4 = m[4] * g; 1367 double s4 = m[7] * g; 1368 tmp[3] = c4 * m[3] + s4 * m[6]; 1369 m[6] = -s4 * m[3] + c4 * m[6]; // zero 1370 m[3] = tmp[3]; 1371 1372 tmp[4] = c4 * m[4] + s4 * m[7]; 1373 m[7] = -s4 * m[4] + c4 * m[7]; 1374 m[4] = tmp[4]; 1375 1376 tmp[5] = c4 * m[5] + s4 * m[8]; 1377 m[8] = -s4 * m[5] + c4 * m[8]; 1378 m[5] = tmp[5]; 1379 1380 tmp[3] = c4 * u1[3] + s4 * u1[6]; 1381 u1[6] = -s4 * u1[3] + c4 * u1[6]; 1382 u1[3] = tmp[3]; 1383 1384 tmp[4] = c4 * u1[4] + s4 * u1[7]; 1385 u1[7] = -s4 * u1[4] + c4 * u1[7]; 1386 u1[4] = tmp[4]; 1387 1388 tmp[5] = c4 * u1[5] + s4 * u1[8]; 1389 u1[8] = -s4 * u1[5] + c4 * u1[8]; 1390 u1[5] = tmp[5]; 1391 } 1392 1393 single_values[0] = m[0]; 1394 single_values[1] = m[4]; 1395 single_values[2] = m[8]; 1396 e[0] = m[1]; 1397 e[1] = m[5]; 1398 1399 if (!(e[0] * e[0] < EPS && e[1] * e[1] < EPS)) { 1400 compute_qr(single_values, e, u1, v1); 1401 } 1402 1403 scales[0] = single_values[0]; 1404 scales[1] = single_values[1]; 1405 scales[2] = single_values[2]; 1406 1407 // Do some optimization here. If scale is unity, simply return the rotation matric. 1408 if (almostEqual(abs(scales[0]), 1.0) 1409 && almostEqual(abs(scales[1]), 1.0) 1410 && almostEqual(abs(scales[2]), 1.0)) { 1411 // System.out.println("Scale components almost to 1.0"); 1412 1413 int negCnt = 0; 1414 for (int i = 0; i < 3; i++) { 1415 if (scales[i] < 0.0) 1416 negCnt++; 1417 } 1418 1419 if ((negCnt == 0) || (negCnt == 2)) { 1420 //System.out.println("Optimize!!"); 1421 outScale[0] = outScale[1] = outScale[2] = 1.0; 1422 System.arraycopy(rot, 0, outRot, 0, 9); 1423 1424 return; 1425 } 1426 } 1427 1428 transpose_mat(u1, t1); 1429 transpose_mat(v1, t2); 1430 1431 /* 1432 System.out.println("t1 is \n" + t1); 1433 System.out.println("t1="+t1[0]+" "+t1[1]+" "+t1[2]); 1434 System.out.println("t1="+t1[3]+" "+t1[4]+" "+t1[5]); 1435 System.out.println("t1="+t1[6]+" "+t1[7]+" "+t1[8]); 1436 1437 System.out.println("t2 is \n" + t2); 1438 System.out.println("t2="+t2[0]+" "+t2[1]+" "+t2[2]); 1439 System.out.println("t2="+t2[3]+" "+t2[4]+" "+t2[5]); 1440 System.out.println("t2="+t2[6]+" "+t2[7]+" "+t2[8]); 1441 */ 1442 svdReorder(m, t1, t2, scales, outRot, outScale); 1443 1444 } finally { 1445 // Recycle scratch arrays. 1446 ArrayFactory.DOUBLES_FACTORY.recycle(u1); 1447 ArrayFactory.DOUBLES_FACTORY.recycle(v1); 1448 ArrayFactory.DOUBLES_FACTORY.recycle(t1); 1449 ArrayFactory.DOUBLES_FACTORY.recycle(t2); 1450 ArrayFactory.DOUBLES_FACTORY.recycle(rot); 1451 ArrayFactory.DOUBLES_FACTORY.recycle(e); 1452 ArrayFactory.DOUBLES_FACTORY.recycle(scales); 1453 } 1454 } 1455 1456 private static void svdReorder(double[] m, double[] t1, double[] t2, double[] scales, 1457 double[] outRot, double[] outScale) { 1458 1459 int[] out = ArrayFactory.INTS_FACTORY.array(3); // new int[3]; 1460 int[] in = ArrayFactory.INTS_FACTORY.array(3); // new int[3]; 1461 double[] mag = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1462 double[] rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1463 1464 // check for rotation information in the scales 1465 if (scales[0] < 0.0) { // move the rotation info to rotation matrix 1466 scales[0] = -scales[0]; 1467 t2[0] = -t2[0]; 1468 t2[1] = -t2[1]; 1469 t2[2] = -t2[2]; 1470 } 1471 if (scales[1] < 0.0) { // move the rotation info to rotation matrix 1472 scales[1] = -scales[1]; 1473 t2[3] = -t2[3]; 1474 t2[4] = -t2[4]; 1475 t2[5] = -t2[5]; 1476 } 1477 if (scales[2] < 0.0) { // move the rotation info to rotation matrix 1478 scales[2] = -scales[2]; 1479 t2[6] = -t2[6]; 1480 t2[7] = -t2[7]; 1481 t2[8] = -t2[8]; 1482 } 1483 1484 mat_mul(t1, t2, rot); 1485 1486 // check for equal scales case and do not reorder 1487 if (almostEqual(abs(scales[0]), abs(scales[1])) 1488 && almostEqual(abs(scales[1]), abs(scales[2]))) { 1489 System.arraycopy(rot, 0, outRot, 0, 9); 1490 System.arraycopy(scales, 0, outScale, 0, 3); 1491 1492 } else { 1493 1494 // sort the order of the results of SVD 1495 if (scales[0] > scales[1]) { 1496 if (scales[0] > scales[2]) { 1497 if (scales[2] > scales[1]) { 1498 out[0] = 0; 1499 out[1] = 2; 1500 out[2] = 1; // xzy 1501 } else { 1502 out[0] = 0; 1503 out[1] = 1; 1504 out[2] = 2; // xyz 1505 } 1506 } else { 1507 out[0] = 2; 1508 out[1] = 0; 1509 out[2] = 1; // zxy 1510 } 1511 } else { // y > x 1512 if (scales[1] > scales[2]) { 1513 if (scales[2] > scales[0]) { 1514 out[0] = 1; 1515 out[1] = 2; 1516 out[2] = 0; // yzx 1517 } else { 1518 out[0] = 1; 1519 out[1] = 0; 1520 out[2] = 2; // yxz 1521 } 1522 } else { 1523 out[0] = 2; 1524 out[1] = 1; 1525 out[2] = 0; // zyx 1526 } 1527 } 1528 1529 /* 1530 System.out.println("\nscales="+scales[0]+" "+scales[1]+" "+scales[2]); 1531 System.out.println("\nrot="+rot[0]+" "+rot[1]+" "+rot[2]); 1532 System.out.println("rot="+rot[3]+" "+rot[4]+" "+rot[5]); 1533 System.out.println("rot="+rot[6]+" "+rot[7]+" "+rot[8]); 1534 */ 1535 // sort the order of the input matrix 1536 mag[0] = (m[0] * m[0] + m[1] * m[1] + m[2] * m[2]); 1537 mag[1] = (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]); 1538 mag[2] = (m[6] * m[6] + m[7] * m[7] + m[8] * m[8]); 1539 1540 int in0, in1, in2; 1541 if (mag[0] > mag[1]) { 1542 if (mag[0] > mag[2]) { 1543 if (mag[2] > mag[1]) { 1544 // 0 - 2 - 1 1545 in0 = 0; 1546 in2 = 1; 1547 in1 = 2;// xzy 1548 } else { 1549 // 0 - 1 - 2 1550 in0 = 0; 1551 in1 = 1; 1552 in2 = 2; // xyz 1553 } 1554 } else { 1555 // 2 - 0 - 1 1556 in2 = 0; 1557 in0 = 1; 1558 in1 = 2; // zxy 1559 } 1560 } else { // y > x 1>0 1561 if (mag[1] > mag[2]) { 1562 if (mag[2] > mag[0]) { 1563 // 1 - 2 - 0 1564 in1 = 0; 1565 in2 = 1; 1566 in0 = 2; // yzx 1567 } else { 1568 // 1 - 0 - 2 1569 in1 = 0; 1570 in0 = 1; 1571 in2 = 2; // yxz 1572 } 1573 } else { 1574 // 2 - 1 - 0 1575 in2 = 0; 1576 in1 = 1; 1577 in0 = 2; // zyx 1578 } 1579 } 1580 1581 int index = out[in0]; 1582 outScale[0] = scales[index]; 1583 1584 index = out[in1]; 1585 outScale[1] = scales[index]; 1586 1587 index = out[in2]; 1588 outScale[2] = scales[index]; 1589 1590 index = out[in0]; 1591 outRot[0] = rot[index]; 1592 1593 index = out[in0] + 3; 1594 outRot[0 + 3] = rot[index]; 1595 1596 index = out[in0] + 6; 1597 outRot[0 + 6] = rot[index]; 1598 1599 index = out[in1]; 1600 outRot[1] = rot[index]; 1601 1602 index = out[in1] + 3; 1603 outRot[1 + 3] = rot[index]; 1604 1605 index = out[in1] + 6; 1606 outRot[1 + 6] = rot[index]; 1607 1608 index = out[in2]; 1609 outRot[2] = rot[index]; 1610 1611 index = out[in2] + 3; 1612 outRot[2 + 3] = rot[index]; 1613 1614 index = out[in2] + 6; 1615 outRot[2 + 6] = rot[index]; 1616 } 1617 1618 // Recycle scratch arrays. 1619 ArrayFactory.INTS_FACTORY.recycle(out); 1620 ArrayFactory.INTS_FACTORY.recycle(in); 1621 ArrayFactory.DOUBLES_FACTORY.recycle(mag); 1622 ArrayFactory.DOUBLES_FACTORY.recycle(rot); 1623 } 1624 1625 private static void compute_qr(double[] s, double[] e, double[] u, double[] v) { 1626 1627 double[] cosl = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1628 double[] cosr = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1629 double[] sinl = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1630 double[] sinr = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1631 double[] m = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1632 1633 final int MAX_INTERATIONS = 10; 1634 final double CONVERGE_TOL = 4.89E-15; 1635 final double c_b48 = 1.; 1636 1637 boolean converged = false; 1638 if (abs(e[1]) < CONVERGE_TOL || abs(e[0]) < CONVERGE_TOL) 1639 converged = true; 1640 1641 for (int k = 0; k < MAX_INTERATIONS && !converged; k++) { 1642 double shift = compute_shift(s[1], e[1], s[2]); 1643 double f = (abs(s[0]) - shift) * (d_sign(c_b48, s[0]) + shift / s[0]); 1644 double g = e[0]; 1645 compute_rot(f, g, sinr, cosr, 0); 1646 f = cosr[0] * s[0] + sinr[0] * e[0]; 1647 e[0] = cosr[0] * e[0] - sinr[0] * s[0]; 1648 g = sinr[0] * s[1]; 1649 s[1] = cosr[0] * s[1]; 1650 1651 double r = compute_rot(f, g, sinl, cosl, 0); 1652 s[0] = r; 1653 f = cosl[0] * e[0] + sinl[0] * s[1]; 1654 s[1] = cosl[0] * s[1] - sinl[0] * e[0]; 1655 g = sinl[0] * e[1]; 1656 e[1] = cosl[0] * e[1]; 1657 1658 r = compute_rot(f, g, sinr, cosr, 1); 1659 e[0] = r; 1660 f = cosr[1] * s[1] + sinr[1] * e[1]; 1661 e[1] = cosr[1] * e[1] - sinr[1] * s[1]; 1662 g = sinr[1] * s[2]; 1663 s[2] = cosr[1] * s[2]; 1664 1665 r = compute_rot(f, g, sinl, cosl, 1); 1666 s[1] = r; 1667 f = cosl[1] * e[1] + sinl[1] * s[2]; 1668 s[2] = cosl[1] * s[2] - sinl[1] * e[1]; 1669 e[1] = f; 1670 1671 // update u matrices 1672 double utemp = u[0]; 1673 u[0] = cosl[0] * utemp + sinl[0] * u[3]; 1674 u[3] = -sinl[0] * utemp + cosl[0] * u[3]; 1675 utemp = u[1]; 1676 u[1] = cosl[0] * utemp + sinl[0] * u[4]; 1677 u[4] = -sinl[0] * utemp + cosl[0] * u[4]; 1678 utemp = u[2]; 1679 u[2] = cosl[0] * utemp + sinl[0] * u[5]; 1680 u[5] = -sinl[0] * utemp + cosl[0] * u[5]; 1681 1682 utemp = u[3]; 1683 u[3] = cosl[1] * utemp + sinl[1] * u[6]; 1684 u[6] = -sinl[1] * utemp + cosl[1] * u[6]; 1685 utemp = u[4]; 1686 u[4] = cosl[1] * utemp + sinl[1] * u[7]; 1687 u[7] = -sinl[1] * utemp + cosl[1] * u[7]; 1688 utemp = u[5]; 1689 u[5] = cosl[1] * utemp + sinl[1] * u[8]; 1690 u[8] = -sinl[1] * utemp + cosl[1] * u[8]; 1691 1692 // update v matrices 1693 double vtemp = v[0]; 1694 v[0] = cosr[0] * vtemp + sinr[0] * v[1]; 1695 v[1] = -sinr[0] * vtemp + cosr[0] * v[1]; 1696 vtemp = v[3]; 1697 v[3] = cosr[0] * vtemp + sinr[0] * v[4]; 1698 v[4] = -sinr[0] * vtemp + cosr[0] * v[4]; 1699 vtemp = v[6]; 1700 v[6] = cosr[0] * vtemp + sinr[0] * v[7]; 1701 v[7] = -sinr[0] * vtemp + cosr[0] * v[7]; 1702 1703 vtemp = v[1]; 1704 v[1] = cosr[1] * vtemp + sinr[1] * v[2]; 1705 v[2] = -sinr[1] * vtemp + cosr[1] * v[2]; 1706 vtemp = v[4]; 1707 v[4] = cosr[1] * vtemp + sinr[1] * v[5]; 1708 v[5] = -sinr[1] * vtemp + cosr[1] * v[5]; 1709 vtemp = v[7]; 1710 v[7] = cosr[1] * vtemp + sinr[1] * v[8]; 1711 v[8] = -sinr[1] * vtemp + cosr[1] * v[8]; 1712 1713 m[0] = s[0]; 1714 m[1] = e[0]; 1715 m[2] = 0.0; 1716 m[3] = 0.0; 1717 m[4] = s[1]; 1718 m[5] = e[1]; 1719 m[6] = 0.0; 1720 m[7] = 0.0; 1721 m[8] = s[2]; 1722 1723 if (abs(e[1]) < CONVERGE_TOL || abs(e[0]) < CONVERGE_TOL) 1724 converged = true; 1725 } 1726 1727 if (abs(e[1]) < CONVERGE_TOL) { 1728 compute_2X2(s[0], e[0], s[1], s, sinl, cosl, sinr, cosr, 0); 1729 1730 double utemp = u[0]; 1731 u[0] = cosl[0] * utemp + sinl[0] * u[3]; 1732 u[3] = -sinl[0] * utemp + cosl[0] * u[3]; 1733 utemp = u[1]; 1734 u[1] = cosl[0] * utemp + sinl[0] * u[4]; 1735 u[4] = -sinl[0] * utemp + cosl[0] * u[4]; 1736 utemp = u[2]; 1737 u[2] = cosl[0] * utemp + sinl[0] * u[5]; 1738 u[5] = -sinl[0] * utemp + cosl[0] * u[5]; 1739 1740 // update v matrices 1741 double vtemp = v[0]; 1742 v[0] = cosr[0] * vtemp + sinr[0] * v[1]; 1743 v[1] = -sinr[0] * vtemp + cosr[0] * v[1]; 1744 vtemp = v[3]; 1745 v[3] = cosr[0] * vtemp + sinr[0] * v[4]; 1746 v[4] = -sinr[0] * vtemp + cosr[0] * v[4]; 1747 vtemp = v[6]; 1748 v[6] = cosr[0] * vtemp + sinr[0] * v[7]; 1749 v[7] = -sinr[0] * vtemp + cosr[0] * v[7]; 1750 } else { 1751 compute_2X2(s[1], e[1], s[2], s, sinl, cosl, sinr, cosr, 1); 1752 1753 double utemp = u[3]; 1754 u[3] = cosl[0] * utemp + sinl[0] * u[6]; 1755 u[6] = -sinl[0] * utemp + cosl[0] * u[6]; 1756 utemp = u[4]; 1757 u[4] = cosl[0] * utemp + sinl[0] * u[7]; 1758 u[7] = -sinl[0] * utemp + cosl[0] * u[7]; 1759 utemp = u[5]; 1760 u[5] = cosl[0] * utemp + sinl[0] * u[8]; 1761 u[8] = -sinl[0] * utemp + cosl[0] * u[8]; 1762 1763 // update v matrices 1764 double vtemp = v[1]; 1765 v[1] = cosr[0] * vtemp + sinr[0] * v[2]; 1766 v[2] = -sinr[0] * vtemp + cosr[0] * v[2]; 1767 vtemp = v[4]; 1768 v[4] = cosr[0] * vtemp + sinr[0] * v[5]; 1769 v[5] = -sinr[0] * vtemp + cosr[0] * v[5]; 1770 vtemp = v[7]; 1771 v[7] = cosr[0] * vtemp + sinr[0] * v[8]; 1772 v[8] = -sinr[0] * vtemp + cosr[0] * v[8]; 1773 } 1774 1775 // Recycle the temporary arrays. 1776 ArrayFactory.DOUBLES_FACTORY.recycle(cosl); 1777 ArrayFactory.DOUBLES_FACTORY.recycle(cosr); 1778 ArrayFactory.DOUBLES_FACTORY.recycle(sinl); 1779 ArrayFactory.DOUBLES_FACTORY.recycle(sinr); 1780 ArrayFactory.DOUBLES_FACTORY.recycle(m); 1781 1782 } 1783 1784 private static double max(double a, double b) { 1785 if (a > b) 1786 return (a); 1787 else 1788 return (b); 1789 } 1790 1791 private static double min(double a, double b) { 1792 if (a < b) 1793 return (a); 1794 else 1795 return (b); 1796 } 1797 1798 private static double d_sign(double a, double b) { 1799 double x = (a >= 0 ? a : -a); 1800 return (b >= 0 ? x : -x); 1801 } 1802 1803 private static double compute_shift(double f, double g, double h) { 1804 double ssmin; 1805 1806 double fa = abs(f); 1807 double ga = abs(g); 1808 double ha = abs(h); 1809 double fhmn = min(fa, ha); 1810 double fhmx = max(fa, ha); 1811 if (fhmn == 0.) { 1812 ssmin = 0.; 1813 } else { 1814 if (ga < fhmx) { 1815 double as = fhmn / fhmx + 1.; 1816 double at = (fhmx - fhmn) / fhmx; 1817 double d__1 = ga / fhmx; 1818 double au = d__1 * d__1; 1819 double c = 2. / (sqrt(as * as + au) + sqrt(at * at + au)); 1820 ssmin = fhmn * c; 1821 } else { 1822 double au = fhmx / ga; 1823 if (au == 0.) { 1824 ssmin = fhmn * fhmx / ga; 1825 } else { 1826 double as = fhmn / fhmx + 1.; 1827 double at = (fhmx - fhmn) / fhmx; 1828 double d__1 = as * au; 1829 double d__2 = at * au; 1830 double c = 1. / (sqrt(d__1 * d__1 + 1.) + sqrt(d__2 * d__2 + 1.)); 1831 ssmin = fhmn * c * au; 1832 ssmin += ssmin; 1833 } 1834 } 1835 } 1836 1837 return (ssmin); 1838 } 1839 1840 private static void compute_2X2(double f, double g, double h, double[] single_values, 1841 double[] snl, double[] csl, double[] snr, double[] csr, int index) { 1842 1843 final double c_b3 = 2.; 1844 final double c_b4 = 1.; 1845 1846 double ft = f; 1847 double fa = abs(ft); 1848 double ht = h; 1849 double ha = abs(h); 1850 1851 int pmax = 1; 1852 boolean swap = false; 1853 if (ha > fa) { 1854 swap = true; 1855 } 1856 1857 if (swap) { 1858 pmax = 3; 1859 double temp = ft; 1860 ft = ht; 1861 ht = temp; 1862 temp = fa; 1863 fa = ha; 1864 ha = temp; 1865 1866 } 1867 double gt = g; 1868 double ga = abs(gt); 1869 if (ga == 0.) { 1870 1871 single_values[1] = ha; 1872 single_values[0] = fa; 1873 //clt = 1.; 1874 //crt = 1.; 1875 //slt = 0.; 1876 //srt = 0.; 1877 } else { 1878 boolean gasmal = true; 1879 1880 double ssmax = single_values[0]; 1881 double ssmin = single_values[1]; 1882 1883 double clt = 0.0; 1884 double crt = 0.0; 1885 double slt = 0.0; 1886 double srt = 0.0; 1887 if (ga > fa) { 1888 pmax = 2; 1889 if (fa / ga < EPS) { 1890 1891 gasmal = false; 1892 ssmax = ga; 1893 if (ha > 1.) { 1894 ssmin = fa / (ga / ha); 1895 } else { 1896 ssmin = fa / ga * ha; 1897 } 1898 clt = 1.; 1899 slt = ht / gt; 1900 srt = 1.; 1901 crt = ft / gt; 1902 } 1903 } 1904 if (gasmal) { 1905 1906 if (ga > fa) { 1907 pmax = 2; 1908 if (fa / ga < EPS) { 1909 1910 gasmal = false; 1911 ssmax = ga; 1912 if (ha > 1.) { 1913 ssmin = fa / (ga / ha); 1914 } else { 1915 ssmin = fa / ga * ha; 1916 } 1917 clt = 1.; 1918 slt = ht / gt; 1919 srt = 1.; 1920 crt = ft / gt; 1921 } 1922 } 1923 if (gasmal) { 1924 1925 double d = fa - ha; 1926 double l; 1927 if (d == fa) { 1928 l = 1.; 1929 } else { 1930 l = d / fa; 1931 } 1932 1933 double m = gt / ft; 1934 1935 double t = 2. - l; 1936 1937 double mm = m * m; 1938 double tt = t * t; 1939 double s = sqrt(tt + mm); 1940 1941 double r; 1942 if (l == 0.) { 1943 r = abs(m); 1944 } else { 1945 r = sqrt(l * l + mm); 1946 } 1947 1948 double a = (s + r) * .5; 1949 1950 ssmin = ha / a; 1951 ssmax = fa * a; 1952 if (mm == 0.) { 1953 1954 if (l == 0.) { 1955 t = d_sign(c_b3, ft) * d_sign(c_b4, gt); 1956 } else { 1957 t = gt / d_sign(d, ft) + m / t; 1958 } 1959 } else { 1960 t = (m / (s + t) + m / (r + l)) * (a + 1.); 1961 } 1962 l = sqrt(t * t + 4.); 1963 crt = 2. / l; 1964 srt = t / l; 1965 clt = (crt + srt * m) / a; 1966 slt = ht / ft * srt / a; 1967 } 1968 } 1969 if (swap) { 1970 csl[0] = srt; 1971 snl[0] = crt; 1972 csr[0] = slt; 1973 snr[0] = clt; 1974 } else { 1975 csl[0] = clt; 1976 snl[0] = slt; 1977 csr[0] = crt; 1978 snr[0] = srt; 1979 } 1980 1981 double tsign = 0; 1982 if (pmax == 1) { 1983 tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, f); 1984 } 1985 if (pmax == 2) { 1986 tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, g); 1987 } 1988 if (pmax == 3) { 1989 tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0]) * d_sign(c_b4, h); 1990 } 1991 single_values[index] = d_sign(ssmax, tsign); 1992 double d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h); 1993 single_values[index + 1] = d_sign(ssmin, d__1); 1994 } 1995 1996 } 1997 1998 private static double compute_rot(double f, double g, double[] sin, double[] cos, int index) { 1999 double cs, sn; 2000 double scale; 2001 double r; 2002 final double safmn2 = 2.002083095183101E-146; 2003 final double safmx2 = 4.994797680505588E+145; 2004 2005 if (g == 0.) { 2006 cs = 1.; 2007 sn = 0.; 2008 r = f; 2009 } else if (f == 0.) { 2010 cs = 0.; 2011 sn = 1.; 2012 r = g; 2013 } else { 2014 double f1 = f; 2015 double g1 = g; 2016 scale = max(abs(f1), abs(g1)); 2017 if (scale >= safmx2) { 2018 int count = 0; 2019 while (scale >= safmx2) { 2020 ++count; 2021 f1 *= safmn2; 2022 g1 *= safmn2; 2023 scale = max(abs(f1), abs(g1)); 2024 } 2025 r = sqrt(f1 * f1 + g1 * g1); 2026 cs = f1 / r; 2027 sn = g1 / r; 2028 for (int i = 1; i <= count; ++i) { 2029 r *= safmx2; 2030 } 2031 } else if (scale <= safmn2) { 2032 int count = 0; 2033 while (scale <= safmn2) { 2034 ++count; 2035 f1 *= safmx2; 2036 g1 *= safmx2; 2037 scale = max(abs(f1), abs(g1)); 2038 } 2039 r = sqrt(f1 * f1 + g1 * g1); 2040 cs = f1 / r; 2041 sn = g1 / r; 2042 for (int i = 1; i <= count; ++i) { 2043 r *= safmn2; 2044 } 2045 } else { 2046 r = sqrt(f1 * f1 + g1 * g1); 2047 cs = f1 / r; 2048 sn = g1 / r; 2049 } 2050 if (abs(f) > abs(g) && cs < 0.) { 2051 cs = -cs; 2052 sn = -sn; 2053 r = -r; 2054 } 2055 } 2056 2057 sin[index] = sn; 2058 cos[index] = cs; 2059 2060 return r; 2061 } 2062 2063 private static void mat_mul(double[] m1, double[] m2, double[] m3) { 2064 double[] tmp = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 2065 2066 tmp[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6]; 2067 tmp[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7]; 2068 tmp[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8]; 2069 2070 tmp[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6]; 2071 tmp[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7]; 2072 tmp[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8]; 2073 2074 tmp[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6]; 2075 tmp[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7]; 2076 tmp[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8]; 2077 2078 System.arraycopy(tmp, 0, m3, 0, 9); 2079 2080 ArrayFactory.DOUBLES_FACTORY.recycle(tmp); 2081 } 2082 2083 private static void transpose_mat(double[] in, double[] out) { 2084 out[0] = in[0]; 2085 out[1] = in[3]; 2086 out[2] = in[6]; 2087 2088 out[3] = in[1]; 2089 out[4] = in[4]; 2090 out[5] = in[7]; 2091 2092 out[6] = in[2]; 2093 out[7] = in[5]; 2094 out[8] = in[8]; 2095 } 2096 2097 private static double max3(double[] values) { 2098 if (values[0] > values[1]) { 2099 if (values[0] > values[2]) 2100 return (values[0]); 2101 else 2102 return (values[2]); 2103 } else { 2104 if (values[1] > values[2]) 2105 return (values[1]); 2106 else 2107 return (values[2]); 2108 } 2109 } 2110 2111 private static boolean almostEqual(double a, double b) { 2112 if (a == b) 2113 return true; 2114 2115 final double EPSILON_ABSOLUTE = 1.0e-6; 2116 double diff = abs(a - b); 2117 2118 if (diff < EPSILON_ABSOLUTE) 2119 return true; 2120 2121 final double EPSILON_RELATIVE = 1.0e-4; 2122 double absA = abs(a); 2123 double absB = abs(b); 2124 double max = (absA >= absB) ? absA : absB; 2125 2126 return (diff / max) < EPSILON_RELATIVE; 2127 } 2128 2129 /** 2130 * Holds the default XML representation for this transformation matrix. For example: 2131 * <pre> 2132 * <GTransform> 2133 * <Float64 value="1" /> 2134 * <Float64 value="0" /> 2135 * <Float64 value="0" /> 2136 * <Float64 value="0" /> 2137 * <Float64 value="0" /> 2138 * <Float64 value="1" /> 2139 * <Float64 value="0" /> 2140 * <Float64 value="0" /> 2141 * <Float64 value="0" /> 2142 * <Float64 value="0" /> 2143 * <Float64 value="1" /> 2144 * <Float64 value="0" /> 2145 * <Float64 value="0" /> 2146 * <Float64 value="0" /> 2147 * <Float64 value="0" /> 2148 * <Float64 value="1" /> 2149 * </GTransform> 2150 * </pre> 2151 */ 2152 protected static final XMLFormat<GTransform> XML 2153 = new XMLFormat<GTransform>(GTransform.class) { 2154 2155 @Override 2156 public GTransform newInstance(Class<GTransform> cls, InputElement xml) throws XMLStreamException { 2157 return FACTORY.object(); 2158 } 2159 2160 @Override 2161 public void read(InputElement xml, GTransform M) throws XMLStreamException { 2162 2163 FastTable<Float64Vector> rowList = FastTable.newInstance(); 2164 2165 try { 2166 for (int i = 0; i < 4; ++i) { 2167 if (!xml.hasNext()) 2168 throw new XMLStreamException( 2169 MessageFormat.format(RESOURCES.getString("need16ElementsFoundLess"), i)); 2170 2171 double x = ((Float64)xml.getNext()).doubleValue(); 2172 double y = ((Float64)xml.getNext()).doubleValue(); 2173 double z = ((Float64)xml.getNext()).doubleValue(); 2174 double w = ((Float64)xml.getNext()).doubleValue(); 2175 Float64Vector row = Float64Vector.valueOf(x, y, z, w); 2176 rowList.add(row); 2177 } 2178 2179 if (xml.hasNext()) 2180 throw new XMLStreamException(RESOURCES.getString("gtToManyElements")); 2181 2182 M._data = Float64Matrix.valueOf(rowList); 2183 2184 } finally { 2185 FastTable.recycle(rowList); 2186 } 2187 2188 } 2189 2190 @Override 2191 public void write(GTransform M, OutputElement xml) throws XMLStreamException { 2192 2193 for (int i = 0; i < 4; i++) { 2194 for (int j = 0; j < 4; j++) { 2195 xml.add(M.get(i, j)); 2196 } 2197 } 2198 } 2199 }; 2200 2201 /** 2202 * Tests the methods in this class. 2203 * 2204 * @param args Command-line arguments (not used). 2205 */ 2206 public static void main(String args[]) { 2207 System.out.println("Testing GTransform:"); 2208 2209 Parameter<Angle> psi = Parameter.valueOf(60, javax.measure.unit.NonSI.DEGREE_ANGLE); 2210 Parameter<Angle> theta = Parameter.ZERO_ANGLE; 2211 Parameter<Angle> phi = Parameter.valueOf(20, javax.measure.unit.NonSI.DEGREE_ANGLE); 2212 System.out.println("\npsi = " + psi); 2213 2214 GTransform T1 = GTransform.newRotationZ(psi); 2215 System.out.println(" T1 = \n" + T1); 2216 2217 System.out.println("\nphi = " + phi); 2218 2219 GTransform T2 = GTransform.newRotationX(phi); 2220 System.out.println(" T2 = \n" + T2); 2221 System.out.println(" T1.times(T2) = \n" + T1.times(T2)); 2222 2223 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 2224 DCMatrix M3 = DCMatrix.getEulerTM(psi, theta, phi); 2225 System.out.println("M3 = \n" + M3); 2226 GTransform T3 = GTransform.valueOf(M3); 2227 System.out.println(" T3 = \n" + T3); 2228 2229 Point p1 = Point.valueOf(1, 1, 1, METER); 2230 System.out.println("\np1 = " + p1); 2231 System.out.println("M3.times(p1.toVector3D()) = " + M3.times(p1.toVector3D())); 2232 System.out.println("T3.transform(p1) = " + T3.transform(p1)); 2233 System.out.println("T3.transform(p1).norm() = " + T3.transform(p1).norm() + " [1.73205080756888 m]"); 2234 2235 GTransform T4 = T3.applyScale(2.5); 2236 System.out.println("\nT4 = T3.applyScale(2.5);"); 2237 System.out.println("T4.transform(p1) = " + T4.transform(p1)); 2238 System.out.println("T4.transform(p1).norm() = " + T4.transform(p1).norm() + " [4.33012701892219 m]"); 2239 System.out.println("T4.getScale() = " + T4.getScale()); 2240 System.out.println("T4.getRotation() = \n" + T4.getRotation()); 2241 2242 GTransform T5 = GTransform.newTranslation(Vector.valueOf(METER, 10, -5, 0.5)); 2243 System.out.println("\np1 = " + p1); 2244 System.out.println("T5.transform(p1) = " + T5.transform(p1) + " [11 m, -4 m, 1.5 m]"); 2245 2246 // Write out XML data. 2247 try { 2248 System.out.println(); 2249 2250 // Creates some useful aliases for class names. 2251 javolution.xml.XMLBinding binding = new GeomXMLBinding(); 2252 2253 javolution.xml.XMLObjectWriter writer = javolution.xml.XMLObjectWriter.newInstance(System.out); 2254 writer.setIndentation(" "); 2255 writer.setBinding(binding); 2256 writer.write(T4, "GTransform", GTransform.class); 2257 writer.flush(); 2258 2259 System.out.println(); 2260 } catch (Exception e) { 2261 e.printStackTrace(); 2262 } 2263 2264 } 2265 2266}