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 * @param out The output stream to serialized this object to. 1092 * @throws java.io.IOException if the output stream could not be written to. 1093 */ 1094 private void writeObject(java.io.ObjectOutputStream out) throws java.io.IOException { 1095 1096 // Call the default write object method. 1097 out.defaultWriteObject(); 1098 1099 // Write out the matrix values. 1100 for (int i = 0; i < 4; ++i) 1101 for (int j = 0; j < 4; ++j) 1102 out.writeDouble(_data.get(i, j).doubleValue()); 1103 1104 } 1105 1106 /** 1107 * During de-serialization, this will handle the reconstruction of the Float64Matrix. 1108 * This method is ONLY called by the Java Serialization mechanism and should not 1109 * otherwise be used. 1110 * 1111 * @param in The input stream to be de-serialized 1112 * @throws java.io.IOException if there is a problem reading from the input stream. 1113 * @throws ClassNotFoundException if the class could not be constructed. 1114 */ 1115 private void readObject(java.io.ObjectInputStream in) throws java.io.IOException, ClassNotFoundException { 1116 1117 // Call the default read object method. 1118 in.defaultReadObject(); 1119 1120 // Read in the matrix values. 1121 FastTable<Float64Vector> rows = FastTable.newInstance(); 1122 for (int i = 0; i < 4; ++i) { 1123 double v1 = in.readDouble(); 1124 double v2 = in.readDouble(); 1125 double v3 = in.readDouble(); 1126 double v4 = in.readDouble(); 1127 Float64Vector row = Float64Vector.valueOf(v1, v2, v3, v4); 1128 rows.add(row); 1129 } 1130 1131 _data = Float64Matrix.valueOf(rows); 1132 1133 } 1134 1135 /** 1136 * ******* The Following taken from javax.vecmath.Matrix3d with minor mods ********** 1137 */ 1138 private void getScaleRotate(double scales[], double rots[]) { 1139 double[] tmp = ArrayFactory.DOUBLES_FACTORY.array(9); // scratch matrix 1140 1141 tmp[0] = getValue(X, X); 1142 tmp[1] = getValue(X, Y); 1143 tmp[2] = getValue(X, Z); 1144 1145 tmp[3] = getValue(Y, X); 1146 tmp[4] = getValue(Y, Y); 1147 tmp[5] = getValue(Y, Z); 1148 1149 tmp[6] = getValue(Z, X); 1150 tmp[7] = getValue(Z, Y); 1151 tmp[8] = getValue(Z, Z); 1152 1153 compute_svd(tmp, scales, rots); 1154 1155 // Recycle the scratch array. 1156 ArrayFactory.DOUBLES_FACTORY.recycle(tmp); 1157 } 1158 1159 private static final double EPS = 1.110223024E-16; 1160 1161 private static void compute_svd(double[] m, double[] outScale, double[] outRot) { 1162 double[] u1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1163 double[] v1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1164 double[] t1 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1165 double[] t2 = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1166 1167 double[] tmp = t1; 1168 double[] single_values = t2; 1169 1170 double[] rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1171 double[] e = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1172 double[] scales = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1173 1174 try { 1175 System.arraycopy(m, 0, rot, 0, 9); 1176 1177 // u1 1178 if (m[3] * m[3] < EPS) { 1179 u1[0] = 1.0; 1180 u1[1] = 0.0; 1181 u1[2] = 0.0; 1182 u1[3] = 0.0; 1183 u1[4] = 1.0; 1184 u1[5] = 0.0; 1185 u1[6] = 0.0; 1186 u1[7] = 0.0; 1187 u1[8] = 1.0; 1188 } else if (m[0] * m[0] < EPS) { 1189 tmp[0] = m[0]; 1190 tmp[1] = m[1]; 1191 tmp[2] = m[2]; 1192 m[0] = m[3]; 1193 m[1] = m[4]; 1194 m[2] = m[5]; 1195 1196 m[3] = -tmp[0]; // zero 1197 m[4] = -tmp[1]; 1198 m[5] = -tmp[2]; 1199 1200 u1[0] = 0.0; 1201 u1[1] = 1.0; 1202 u1[2] = 0.0; 1203 u1[3] = -1.0; 1204 u1[4] = 0.0; 1205 u1[5] = 0.0; 1206 u1[6] = 0.0; 1207 u1[7] = 0.0; 1208 u1[8] = 1.0; 1209 } else { 1210 double g = 1.0 / sqrt(m[0] * m[0] + m[3] * m[3]); 1211 double c1 = m[0] * g; 1212 double s1 = m[3] * g; 1213 tmp[0] = c1 * m[0] + s1 * m[3]; 1214 tmp[1] = c1 * m[1] + s1 * m[4]; 1215 tmp[2] = c1 * m[2] + s1 * m[5]; 1216 1217 m[3] = -s1 * m[0] + c1 * m[3]; // zero 1218 m[4] = -s1 * m[1] + c1 * m[4]; 1219 m[5] = -s1 * m[2] + c1 * m[5]; 1220 1221 m[0] = tmp[0]; 1222 m[1] = tmp[1]; 1223 m[2] = tmp[2]; 1224 u1[0] = c1; 1225 u1[1] = s1; 1226 u1[2] = 0.0; 1227 u1[3] = -s1; 1228 u1[4] = c1; 1229 u1[5] = 0.0; 1230 u1[6] = 0.0; 1231 u1[7] = 0.0; 1232 u1[8] = 1.0; 1233 } 1234 1235 // u2 1236 if (m[6] * m[6] < EPS) { 1237 } else if (m[0] * m[0] < EPS) { 1238 tmp[0] = m[0]; 1239 tmp[1] = m[1]; 1240 tmp[2] = m[2]; 1241 m[0] = m[6]; 1242 m[1] = m[7]; 1243 m[2] = m[8]; 1244 1245 m[6] = -tmp[0]; // zero 1246 m[7] = -tmp[1]; 1247 m[8] = -tmp[2]; 1248 1249 tmp[0] = u1[0]; 1250 tmp[1] = u1[1]; 1251 tmp[2] = u1[2]; 1252 u1[0] = u1[6]; 1253 u1[1] = u1[7]; 1254 u1[2] = u1[8]; 1255 1256 u1[6] = -tmp[0]; // zero 1257 u1[7] = -tmp[1]; 1258 u1[8] = -tmp[2]; 1259 } else { 1260 double g = 1.0 / sqrt(m[0] * m[0] + m[6] * m[6]); 1261 double c2 = m[0] * g; 1262 double s2 = m[6] * g; 1263 tmp[0] = c2 * m[0] + s2 * m[6]; 1264 tmp[1] = c2 * m[1] + s2 * m[7]; 1265 tmp[2] = c2 * m[2] + s2 * m[8]; 1266 1267 m[6] = -s2 * m[0] + c2 * m[6]; 1268 m[7] = -s2 * m[1] + c2 * m[7]; 1269 m[8] = -s2 * m[2] + c2 * m[8]; 1270 m[0] = tmp[0]; 1271 m[1] = tmp[1]; 1272 m[2] = tmp[2]; 1273 1274 tmp[0] = c2 * u1[0]; 1275 tmp[1] = c2 * u1[1]; 1276 u1[2] = s2; 1277 1278 tmp[6] = -u1[0] * s2; 1279 tmp[7] = -u1[1] * s2; 1280 u1[8] = c2; 1281 u1[0] = tmp[0]; 1282 u1[1] = tmp[1]; 1283 u1[6] = tmp[6]; 1284 u1[7] = tmp[7]; 1285 } 1286 1287 // v1 1288 if (m[2] * m[2] < EPS) { 1289 v1[0] = 1.0; 1290 v1[1] = 0.0; 1291 v1[2] = 0.0; 1292 v1[3] = 0.0; 1293 v1[4] = 1.0; 1294 v1[5] = 0.0; 1295 v1[6] = 0.0; 1296 v1[7] = 0.0; 1297 v1[8] = 1.0; 1298 } else if (m[1] * m[1] < EPS) { 1299 tmp[2] = m[2]; 1300 tmp[5] = m[5]; 1301 tmp[8] = m[8]; 1302 m[2] = -m[1]; 1303 m[5] = -m[4]; 1304 m[8] = -m[7]; 1305 1306 m[1] = tmp[2]; // zero 1307 m[4] = tmp[5]; 1308 m[7] = tmp[8]; 1309 1310 v1[0] = 1.0; 1311 v1[1] = 0.0; 1312 v1[2] = 0.0; 1313 v1[3] = 0.0; 1314 v1[4] = 0.0; 1315 v1[5] = -1.0; 1316 v1[6] = 0.0; 1317 v1[7] = 1.0; 1318 v1[8] = 0.0; 1319 } else { 1320 double g = 1.0 / sqrt(m[1] * m[1] + m[2] * m[2]); 1321 double c3 = m[1] * g; 1322 double s3 = m[2] * g; 1323 tmp[1] = c3 * m[1] + s3 * m[2]; // can assign to m[1]? 1324 m[2] = -s3 * m[1] + c3 * m[2]; // zero 1325 m[1] = tmp[1]; 1326 1327 tmp[4] = c3 * m[4] + s3 * m[5]; 1328 m[5] = -s3 * m[4] + c3 * m[5]; 1329 m[4] = tmp[4]; 1330 1331 tmp[7] = c3 * m[7] + s3 * m[8]; 1332 m[8] = -s3 * m[7] + c3 * m[8]; 1333 m[7] = tmp[7]; 1334 1335 v1[0] = 1.0; 1336 v1[1] = 0.0; 1337 v1[2] = 0.0; 1338 v1[3] = 0.0; 1339 v1[4] = c3; 1340 v1[5] = -s3; 1341 v1[6] = 0.0; 1342 v1[7] = s3; 1343 v1[8] = c3; 1344 } 1345 1346 // u3 1347 if (m[7] * m[7] < EPS) { 1348 } else if (m[4] * m[4] < EPS) { 1349 tmp[3] = m[3]; 1350 tmp[4] = m[4]; 1351 tmp[5] = m[5]; 1352 m[3] = m[6]; // zero 1353 m[4] = m[7]; 1354 m[5] = m[8]; 1355 1356 m[6] = -tmp[3]; // zero 1357 m[7] = -tmp[4]; // zero 1358 m[8] = -tmp[5]; 1359 1360 tmp[3] = u1[3]; 1361 tmp[4] = u1[4]; 1362 tmp[5] = u1[5]; 1363 u1[3] = u1[6]; 1364 u1[4] = u1[7]; 1365 u1[5] = u1[8]; 1366 1367 u1[6] = -tmp[3]; // zero 1368 u1[7] = -tmp[4]; 1369 u1[8] = -tmp[5]; 1370 1371 } else { 1372 double g = 1.0 / sqrt(m[4] * m[4] + m[7] * m[7]); 1373 double c4 = m[4] * g; 1374 double s4 = m[7] * g; 1375 tmp[3] = c4 * m[3] + s4 * m[6]; 1376 m[6] = -s4 * m[3] + c4 * m[6]; // zero 1377 m[3] = tmp[3]; 1378 1379 tmp[4] = c4 * m[4] + s4 * m[7]; 1380 m[7] = -s4 * m[4] + c4 * m[7]; 1381 m[4] = tmp[4]; 1382 1383 tmp[5] = c4 * m[5] + s4 * m[8]; 1384 m[8] = -s4 * m[5] + c4 * m[8]; 1385 m[5] = tmp[5]; 1386 1387 tmp[3] = c4 * u1[3] + s4 * u1[6]; 1388 u1[6] = -s4 * u1[3] + c4 * u1[6]; 1389 u1[3] = tmp[3]; 1390 1391 tmp[4] = c4 * u1[4] + s4 * u1[7]; 1392 u1[7] = -s4 * u1[4] + c4 * u1[7]; 1393 u1[4] = tmp[4]; 1394 1395 tmp[5] = c4 * u1[5] + s4 * u1[8]; 1396 u1[8] = -s4 * u1[5] + c4 * u1[8]; 1397 u1[5] = tmp[5]; 1398 } 1399 1400 single_values[0] = m[0]; 1401 single_values[1] = m[4]; 1402 single_values[2] = m[8]; 1403 e[0] = m[1]; 1404 e[1] = m[5]; 1405 1406 if (!(e[0] * e[0] < EPS && e[1] * e[1] < EPS)) { 1407 compute_qr(single_values, e, u1, v1); 1408 } 1409 1410 scales[0] = single_values[0]; 1411 scales[1] = single_values[1]; 1412 scales[2] = single_values[2]; 1413 1414 // Do some optimization here. If scale is unity, simply return the rotation matric. 1415 if (almostEqual(abs(scales[0]), 1.0) 1416 && almostEqual(abs(scales[1]), 1.0) 1417 && almostEqual(abs(scales[2]), 1.0)) { 1418 // System.out.println("Scale components almost to 1.0"); 1419 1420 int negCnt = 0; 1421 for (int i = 0; i < 3; i++) { 1422 if (scales[i] < 0.0) 1423 negCnt++; 1424 } 1425 1426 if ((negCnt == 0) || (negCnt == 2)) { 1427 //System.out.println("Optimize!!"); 1428 outScale[0] = outScale[1] = outScale[2] = 1.0; 1429 System.arraycopy(rot, 0, outRot, 0, 9); 1430 1431 return; 1432 } 1433 } 1434 1435 transpose_mat(u1, t1); 1436 transpose_mat(v1, t2); 1437 1438 /* 1439 System.out.println("t1 is \n" + t1); 1440 System.out.println("t1="+t1[0]+" "+t1[1]+" "+t1[2]); 1441 System.out.println("t1="+t1[3]+" "+t1[4]+" "+t1[5]); 1442 System.out.println("t1="+t1[6]+" "+t1[7]+" "+t1[8]); 1443 1444 System.out.println("t2 is \n" + t2); 1445 System.out.println("t2="+t2[0]+" "+t2[1]+" "+t2[2]); 1446 System.out.println("t2="+t2[3]+" "+t2[4]+" "+t2[5]); 1447 System.out.println("t2="+t2[6]+" "+t2[7]+" "+t2[8]); 1448 */ 1449 svdReorder(m, t1, t2, scales, outRot, outScale); 1450 1451 } finally { 1452 // Recycle scratch arrays. 1453 ArrayFactory.DOUBLES_FACTORY.recycle(u1); 1454 ArrayFactory.DOUBLES_FACTORY.recycle(v1); 1455 ArrayFactory.DOUBLES_FACTORY.recycle(t1); 1456 ArrayFactory.DOUBLES_FACTORY.recycle(t2); 1457 ArrayFactory.DOUBLES_FACTORY.recycle(rot); 1458 ArrayFactory.DOUBLES_FACTORY.recycle(e); 1459 ArrayFactory.DOUBLES_FACTORY.recycle(scales); 1460 } 1461 } 1462 1463 private static void svdReorder(double[] m, double[] t1, double[] t2, double[] scales, 1464 double[] outRot, double[] outScale) { 1465 1466 int[] out = ArrayFactory.INTS_FACTORY.array(3); // new int[3]; 1467 int[] in = ArrayFactory.INTS_FACTORY.array(3); // new int[3]; 1468 double[] mag = ArrayFactory.DOUBLES_FACTORY.array(3); // new double[3]; 1469 double[] rot = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1470 1471 // check for rotation information in the scales 1472 if (scales[0] < 0.0) { // move the rotation info to rotation matrix 1473 scales[0] = -scales[0]; 1474 t2[0] = -t2[0]; 1475 t2[1] = -t2[1]; 1476 t2[2] = -t2[2]; 1477 } 1478 if (scales[1] < 0.0) { // move the rotation info to rotation matrix 1479 scales[1] = -scales[1]; 1480 t2[3] = -t2[3]; 1481 t2[4] = -t2[4]; 1482 t2[5] = -t2[5]; 1483 } 1484 if (scales[2] < 0.0) { // move the rotation info to rotation matrix 1485 scales[2] = -scales[2]; 1486 t2[6] = -t2[6]; 1487 t2[7] = -t2[7]; 1488 t2[8] = -t2[8]; 1489 } 1490 1491 mat_mul(t1, t2, rot); 1492 1493 // check for equal scales case and do not reorder 1494 if (almostEqual(abs(scales[0]), abs(scales[1])) 1495 && almostEqual(abs(scales[1]), abs(scales[2]))) { 1496 System.arraycopy(rot, 0, outRot, 0, 9); 1497 System.arraycopy(scales, 0, outScale, 0, 3); 1498 1499 } else { 1500 1501 // sort the order of the results of SVD 1502 if (scales[0] > scales[1]) { 1503 if (scales[0] > scales[2]) { 1504 if (scales[2] > scales[1]) { 1505 out[0] = 0; 1506 out[1] = 2; 1507 out[2] = 1; // xzy 1508 } else { 1509 out[0] = 0; 1510 out[1] = 1; 1511 out[2] = 2; // xyz 1512 } 1513 } else { 1514 out[0] = 2; 1515 out[1] = 0; 1516 out[2] = 1; // zxy 1517 } 1518 } else { // y > x 1519 if (scales[1] > scales[2]) { 1520 if (scales[2] > scales[0]) { 1521 out[0] = 1; 1522 out[1] = 2; 1523 out[2] = 0; // yzx 1524 } else { 1525 out[0] = 1; 1526 out[1] = 0; 1527 out[2] = 2; // yxz 1528 } 1529 } else { 1530 out[0] = 2; 1531 out[1] = 1; 1532 out[2] = 0; // zyx 1533 } 1534 } 1535 1536 /* 1537 System.out.println("\nscales="+scales[0]+" "+scales[1]+" "+scales[2]); 1538 System.out.println("\nrot="+rot[0]+" "+rot[1]+" "+rot[2]); 1539 System.out.println("rot="+rot[3]+" "+rot[4]+" "+rot[5]); 1540 System.out.println("rot="+rot[6]+" "+rot[7]+" "+rot[8]); 1541 */ 1542 // sort the order of the input matrix 1543 mag[0] = (m[0] * m[0] + m[1] * m[1] + m[2] * m[2]); 1544 mag[1] = (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]); 1545 mag[2] = (m[6] * m[6] + m[7] * m[7] + m[8] * m[8]); 1546 1547 int in0, in1, in2; 1548 if (mag[0] > mag[1]) { 1549 if (mag[0] > mag[2]) { 1550 if (mag[2] > mag[1]) { 1551 // 0 - 2 - 1 1552 in0 = 0; 1553 in2 = 1; 1554 in1 = 2;// xzy 1555 } else { 1556 // 0 - 1 - 2 1557 in0 = 0; 1558 in1 = 1; 1559 in2 = 2; // xyz 1560 } 1561 } else { 1562 // 2 - 0 - 1 1563 in2 = 0; 1564 in0 = 1; 1565 in1 = 2; // zxy 1566 } 1567 } else { // y > x 1>0 1568 if (mag[1] > mag[2]) { 1569 if (mag[2] > mag[0]) { 1570 // 1 - 2 - 0 1571 in1 = 0; 1572 in2 = 1; 1573 in0 = 2; // yzx 1574 } else { 1575 // 1 - 0 - 2 1576 in1 = 0; 1577 in0 = 1; 1578 in2 = 2; // yxz 1579 } 1580 } else { 1581 // 2 - 1 - 0 1582 in2 = 0; 1583 in1 = 1; 1584 in0 = 2; // zyx 1585 } 1586 } 1587 1588 int index = out[in0]; 1589 outScale[0] = scales[index]; 1590 1591 index = out[in1]; 1592 outScale[1] = scales[index]; 1593 1594 index = out[in2]; 1595 outScale[2] = scales[index]; 1596 1597 index = out[in0]; 1598 outRot[0] = rot[index]; 1599 1600 index = out[in0] + 3; 1601 outRot[0 + 3] = rot[index]; 1602 1603 index = out[in0] + 6; 1604 outRot[0 + 6] = rot[index]; 1605 1606 index = out[in1]; 1607 outRot[1] = rot[index]; 1608 1609 index = out[in1] + 3; 1610 outRot[1 + 3] = rot[index]; 1611 1612 index = out[in1] + 6; 1613 outRot[1 + 6] = rot[index]; 1614 1615 index = out[in2]; 1616 outRot[2] = rot[index]; 1617 1618 index = out[in2] + 3; 1619 outRot[2 + 3] = rot[index]; 1620 1621 index = out[in2] + 6; 1622 outRot[2 + 6] = rot[index]; 1623 } 1624 1625 // Recycle scratch arrays. 1626 ArrayFactory.INTS_FACTORY.recycle(out); 1627 ArrayFactory.INTS_FACTORY.recycle(in); 1628 ArrayFactory.DOUBLES_FACTORY.recycle(mag); 1629 ArrayFactory.DOUBLES_FACTORY.recycle(rot); 1630 } 1631 1632 private static void compute_qr(double[] s, double[] e, double[] u, double[] v) { 1633 1634 double[] cosl = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1635 double[] cosr = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1636 double[] sinl = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1637 double[] sinr = ArrayFactory.DOUBLES_FACTORY.array(2); // new double[2]; 1638 double[] m = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 1639 1640 final int MAX_INTERATIONS = 10; 1641 final double CONVERGE_TOL = 4.89E-15; 1642 final double c_b48 = 1.; 1643 1644 boolean converged = false; 1645 if (abs(e[1]) < CONVERGE_TOL || abs(e[0]) < CONVERGE_TOL) 1646 converged = true; 1647 1648 for (int k = 0; k < MAX_INTERATIONS && !converged; k++) { 1649 double shift = compute_shift(s[1], e[1], s[2]); 1650 double f = (abs(s[0]) - shift) * (d_sign(c_b48, s[0]) + shift / s[0]); 1651 double g = e[0]; 1652 compute_rot(f, g, sinr, cosr, 0); 1653 f = cosr[0] * s[0] + sinr[0] * e[0]; 1654 e[0] = cosr[0] * e[0] - sinr[0] * s[0]; 1655 g = sinr[0] * s[1]; 1656 s[1] = cosr[0] * s[1]; 1657 1658 double r = compute_rot(f, g, sinl, cosl, 0); 1659 s[0] = r; 1660 f = cosl[0] * e[0] + sinl[0] * s[1]; 1661 s[1] = cosl[0] * s[1] - sinl[0] * e[0]; 1662 g = sinl[0] * e[1]; 1663 e[1] = cosl[0] * e[1]; 1664 1665 r = compute_rot(f, g, sinr, cosr, 1); 1666 e[0] = r; 1667 f = cosr[1] * s[1] + sinr[1] * e[1]; 1668 e[1] = cosr[1] * e[1] - sinr[1] * s[1]; 1669 g = sinr[1] * s[2]; 1670 s[2] = cosr[1] * s[2]; 1671 1672 r = compute_rot(f, g, sinl, cosl, 1); 1673 s[1] = r; 1674 f = cosl[1] * e[1] + sinl[1] * s[2]; 1675 s[2] = cosl[1] * s[2] - sinl[1] * e[1]; 1676 e[1] = f; 1677 1678 // update u matrices 1679 double utemp = u[0]; 1680 u[0] = cosl[0] * utemp + sinl[0] * u[3]; 1681 u[3] = -sinl[0] * utemp + cosl[0] * u[3]; 1682 utemp = u[1]; 1683 u[1] = cosl[0] * utemp + sinl[0] * u[4]; 1684 u[4] = -sinl[0] * utemp + cosl[0] * u[4]; 1685 utemp = u[2]; 1686 u[2] = cosl[0] * utemp + sinl[0] * u[5]; 1687 u[5] = -sinl[0] * utemp + cosl[0] * u[5]; 1688 1689 utemp = u[3]; 1690 u[3] = cosl[1] * utemp + sinl[1] * u[6]; 1691 u[6] = -sinl[1] * utemp + cosl[1] * u[6]; 1692 utemp = u[4]; 1693 u[4] = cosl[1] * utemp + sinl[1] * u[7]; 1694 u[7] = -sinl[1] * utemp + cosl[1] * u[7]; 1695 utemp = u[5]; 1696 u[5] = cosl[1] * utemp + sinl[1] * u[8]; 1697 u[8] = -sinl[1] * utemp + cosl[1] * u[8]; 1698 1699 // update v matrices 1700 double vtemp = v[0]; 1701 v[0] = cosr[0] * vtemp + sinr[0] * v[1]; 1702 v[1] = -sinr[0] * vtemp + cosr[0] * v[1]; 1703 vtemp = v[3]; 1704 v[3] = cosr[0] * vtemp + sinr[0] * v[4]; 1705 v[4] = -sinr[0] * vtemp + cosr[0] * v[4]; 1706 vtemp = v[6]; 1707 v[6] = cosr[0] * vtemp + sinr[0] * v[7]; 1708 v[7] = -sinr[0] * vtemp + cosr[0] * v[7]; 1709 1710 vtemp = v[1]; 1711 v[1] = cosr[1] * vtemp + sinr[1] * v[2]; 1712 v[2] = -sinr[1] * vtemp + cosr[1] * v[2]; 1713 vtemp = v[4]; 1714 v[4] = cosr[1] * vtemp + sinr[1] * v[5]; 1715 v[5] = -sinr[1] * vtemp + cosr[1] * v[5]; 1716 vtemp = v[7]; 1717 v[7] = cosr[1] * vtemp + sinr[1] * v[8]; 1718 v[8] = -sinr[1] * vtemp + cosr[1] * v[8]; 1719 1720 m[0] = s[0]; 1721 m[1] = e[0]; 1722 m[2] = 0.0; 1723 m[3] = 0.0; 1724 m[4] = s[1]; 1725 m[5] = e[1]; 1726 m[6] = 0.0; 1727 m[7] = 0.0; 1728 m[8] = s[2]; 1729 1730 if (abs(e[1]) < CONVERGE_TOL || abs(e[0]) < CONVERGE_TOL) 1731 converged = true; 1732 } 1733 1734 if (abs(e[1]) < CONVERGE_TOL) { 1735 compute_2X2(s[0], e[0], s[1], s, sinl, cosl, sinr, cosr, 0); 1736 1737 double utemp = u[0]; 1738 u[0] = cosl[0] * utemp + sinl[0] * u[3]; 1739 u[3] = -sinl[0] * utemp + cosl[0] * u[3]; 1740 utemp = u[1]; 1741 u[1] = cosl[0] * utemp + sinl[0] * u[4]; 1742 u[4] = -sinl[0] * utemp + cosl[0] * u[4]; 1743 utemp = u[2]; 1744 u[2] = cosl[0] * utemp + sinl[0] * u[5]; 1745 u[5] = -sinl[0] * utemp + cosl[0] * u[5]; 1746 1747 // update v matrices 1748 double vtemp = v[0]; 1749 v[0] = cosr[0] * vtemp + sinr[0] * v[1]; 1750 v[1] = -sinr[0] * vtemp + cosr[0] * v[1]; 1751 vtemp = v[3]; 1752 v[3] = cosr[0] * vtemp + sinr[0] * v[4]; 1753 v[4] = -sinr[0] * vtemp + cosr[0] * v[4]; 1754 vtemp = v[6]; 1755 v[6] = cosr[0] * vtemp + sinr[0] * v[7]; 1756 v[7] = -sinr[0] * vtemp + cosr[0] * v[7]; 1757 } else { 1758 compute_2X2(s[1], e[1], s[2], s, sinl, cosl, sinr, cosr, 1); 1759 1760 double utemp = u[3]; 1761 u[3] = cosl[0] * utemp + sinl[0] * u[6]; 1762 u[6] = -sinl[0] * utemp + cosl[0] * u[6]; 1763 utemp = u[4]; 1764 u[4] = cosl[0] * utemp + sinl[0] * u[7]; 1765 u[7] = -sinl[0] * utemp + cosl[0] * u[7]; 1766 utemp = u[5]; 1767 u[5] = cosl[0] * utemp + sinl[0] * u[8]; 1768 u[8] = -sinl[0] * utemp + cosl[0] * u[8]; 1769 1770 // update v matrices 1771 double vtemp = v[1]; 1772 v[1] = cosr[0] * vtemp + sinr[0] * v[2]; 1773 v[2] = -sinr[0] * vtemp + cosr[0] * v[2]; 1774 vtemp = v[4]; 1775 v[4] = cosr[0] * vtemp + sinr[0] * v[5]; 1776 v[5] = -sinr[0] * vtemp + cosr[0] * v[5]; 1777 vtemp = v[7]; 1778 v[7] = cosr[0] * vtemp + sinr[0] * v[8]; 1779 v[8] = -sinr[0] * vtemp + cosr[0] * v[8]; 1780 } 1781 1782 // Recycle the temporary arrays. 1783 ArrayFactory.DOUBLES_FACTORY.recycle(cosl); 1784 ArrayFactory.DOUBLES_FACTORY.recycle(cosr); 1785 ArrayFactory.DOUBLES_FACTORY.recycle(sinl); 1786 ArrayFactory.DOUBLES_FACTORY.recycle(sinr); 1787 ArrayFactory.DOUBLES_FACTORY.recycle(m); 1788 1789 } 1790 1791 private static double max(double a, double b) { 1792 if (a > b) 1793 return (a); 1794 else 1795 return (b); 1796 } 1797 1798 private static double min(double a, double b) { 1799 if (a < b) 1800 return (a); 1801 else 1802 return (b); 1803 } 1804 1805 private static double d_sign(double a, double b) { 1806 double x = (a >= 0 ? a : -a); 1807 return (b >= 0 ? x : -x); 1808 } 1809 1810 private static double compute_shift(double f, double g, double h) { 1811 double ssmin; 1812 1813 double fa = abs(f); 1814 double ga = abs(g); 1815 double ha = abs(h); 1816 double fhmn = min(fa, ha); 1817 double fhmx = max(fa, ha); 1818 if (fhmn == 0.) { 1819 ssmin = 0.; 1820 } else { 1821 if (ga < fhmx) { 1822 double as = fhmn / fhmx + 1.; 1823 double at = (fhmx - fhmn) / fhmx; 1824 double d__1 = ga / fhmx; 1825 double au = d__1 * d__1; 1826 double c = 2. / (sqrt(as * as + au) + sqrt(at * at + au)); 1827 ssmin = fhmn * c; 1828 } else { 1829 double au = fhmx / ga; 1830 if (au == 0.) { 1831 ssmin = fhmn * fhmx / ga; 1832 } else { 1833 double as = fhmn / fhmx + 1.; 1834 double at = (fhmx - fhmn) / fhmx; 1835 double d__1 = as * au; 1836 double d__2 = at * au; 1837 double c = 1. / (sqrt(d__1 * d__1 + 1.) + sqrt(d__2 * d__2 + 1.)); 1838 ssmin = fhmn * c * au; 1839 ssmin += ssmin; 1840 } 1841 } 1842 } 1843 1844 return (ssmin); 1845 } 1846 1847 private static void compute_2X2(double f, double g, double h, double[] single_values, 1848 double[] snl, double[] csl, double[] snr, double[] csr, int index) { 1849 1850 final double c_b3 = 2.; 1851 final double c_b4 = 1.; 1852 1853 double ft = f; 1854 double fa = abs(ft); 1855 double ht = h; 1856 double ha = abs(h); 1857 1858 int pmax = 1; 1859 boolean swap = false; 1860 if (ha > fa) { 1861 swap = true; 1862 } 1863 1864 if (swap) { 1865 pmax = 3; 1866 double temp = ft; 1867 ft = ht; 1868 ht = temp; 1869 temp = fa; 1870 fa = ha; 1871 ha = temp; 1872 1873 } 1874 double gt = g; 1875 double ga = abs(gt); 1876 if (ga == 0.) { 1877 1878 single_values[1] = ha; 1879 single_values[0] = fa; 1880 //clt = 1.; 1881 //crt = 1.; 1882 //slt = 0.; 1883 //srt = 0.; 1884 } else { 1885 boolean gasmal = true; 1886 1887 double ssmax = single_values[0]; 1888 double ssmin = single_values[1]; 1889 1890 double clt = 0.0; 1891 double crt = 0.0; 1892 double slt = 0.0; 1893 double srt = 0.0; 1894 if (ga > fa) { 1895 pmax = 2; 1896 if (fa / ga < EPS) { 1897 1898 gasmal = false; 1899 ssmax = ga; 1900 if (ha > 1.) { 1901 ssmin = fa / (ga / ha); 1902 } else { 1903 ssmin = fa / ga * ha; 1904 } 1905 clt = 1.; 1906 slt = ht / gt; 1907 srt = 1.; 1908 crt = ft / gt; 1909 } 1910 } 1911 if (gasmal) { 1912 1913 if (ga > fa) { 1914 pmax = 2; 1915 if (fa / ga < EPS) { 1916 1917 gasmal = false; 1918 ssmax = ga; 1919 if (ha > 1.) { 1920 ssmin = fa / (ga / ha); 1921 } else { 1922 ssmin = fa / ga * ha; 1923 } 1924 clt = 1.; 1925 slt = ht / gt; 1926 srt = 1.; 1927 crt = ft / gt; 1928 } 1929 } 1930 if (gasmal) { 1931 1932 double d = fa - ha; 1933 double l; 1934 if (d == fa) { 1935 l = 1.; 1936 } else { 1937 l = d / fa; 1938 } 1939 1940 double m = gt / ft; 1941 1942 double t = 2. - l; 1943 1944 double mm = m * m; 1945 double tt = t * t; 1946 double s = sqrt(tt + mm); 1947 1948 double r; 1949 if (l == 0.) { 1950 r = abs(m); 1951 } else { 1952 r = sqrt(l * l + mm); 1953 } 1954 1955 double a = (s + r) * .5; 1956 1957 ssmin = ha / a; 1958 ssmax = fa * a; 1959 if (mm == 0.) { 1960 1961 if (l == 0.) { 1962 t = d_sign(c_b3, ft) * d_sign(c_b4, gt); 1963 } else { 1964 t = gt / d_sign(d, ft) + m / t; 1965 } 1966 } else { 1967 t = (m / (s + t) + m / (r + l)) * (a + 1.); 1968 } 1969 l = sqrt(t * t + 4.); 1970 crt = 2. / l; 1971 srt = t / l; 1972 clt = (crt + srt * m) / a; 1973 slt = ht / ft * srt / a; 1974 } 1975 } 1976 if (swap) { 1977 csl[0] = srt; 1978 snl[0] = crt; 1979 csr[0] = slt; 1980 snr[0] = clt; 1981 } else { 1982 csl[0] = clt; 1983 snl[0] = slt; 1984 csr[0] = crt; 1985 snr[0] = srt; 1986 } 1987 1988 double tsign = 0; 1989 if (pmax == 1) { 1990 tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, f); 1991 } 1992 if (pmax == 2) { 1993 tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, g); 1994 } 1995 if (pmax == 3) { 1996 tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0]) * d_sign(c_b4, h); 1997 } 1998 single_values[index] = d_sign(ssmax, tsign); 1999 double d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h); 2000 single_values[index + 1] = d_sign(ssmin, d__1); 2001 } 2002 2003 } 2004 2005 private static double compute_rot(double f, double g, double[] sin, double[] cos, int index) { 2006 double cs, sn; 2007 double scale; 2008 double r; 2009 final double safmn2 = 2.002083095183101E-146; 2010 final double safmx2 = 4.994797680505588E+145; 2011 2012 if (g == 0.) { 2013 cs = 1.; 2014 sn = 0.; 2015 r = f; 2016 } else if (f == 0.) { 2017 cs = 0.; 2018 sn = 1.; 2019 r = g; 2020 } else { 2021 double f1 = f; 2022 double g1 = g; 2023 scale = max(abs(f1), abs(g1)); 2024 if (scale >= safmx2) { 2025 int count = 0; 2026 while (scale >= safmx2) { 2027 ++count; 2028 f1 *= safmn2; 2029 g1 *= safmn2; 2030 scale = max(abs(f1), abs(g1)); 2031 } 2032 r = sqrt(f1 * f1 + g1 * g1); 2033 cs = f1 / r; 2034 sn = g1 / r; 2035 for (int i = 1; i <= count; ++i) { 2036 r *= safmx2; 2037 } 2038 } else if (scale <= safmn2) { 2039 int count = 0; 2040 while (scale <= safmn2) { 2041 ++count; 2042 f1 *= safmx2; 2043 g1 *= safmx2; 2044 scale = max(abs(f1), abs(g1)); 2045 } 2046 r = sqrt(f1 * f1 + g1 * g1); 2047 cs = f1 / r; 2048 sn = g1 / r; 2049 for (int i = 1; i <= count; ++i) { 2050 r *= safmn2; 2051 } 2052 } else { 2053 r = sqrt(f1 * f1 + g1 * g1); 2054 cs = f1 / r; 2055 sn = g1 / r; 2056 } 2057 if (abs(f) > abs(g) && cs < 0.) { 2058 cs = -cs; 2059 sn = -sn; 2060 r = -r; 2061 } 2062 } 2063 2064 sin[index] = sn; 2065 cos[index] = cs; 2066 2067 return r; 2068 } 2069 2070 private static void mat_mul(double[] m1, double[] m2, double[] m3) { 2071 double[] tmp = ArrayFactory.DOUBLES_FACTORY.array(9); // new double[9]; 2072 2073 tmp[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6]; 2074 tmp[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7]; 2075 tmp[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8]; 2076 2077 tmp[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6]; 2078 tmp[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7]; 2079 tmp[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8]; 2080 2081 tmp[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6]; 2082 tmp[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7]; 2083 tmp[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8]; 2084 2085 System.arraycopy(tmp, 0, m3, 0, 9); 2086 2087 ArrayFactory.DOUBLES_FACTORY.recycle(tmp); 2088 } 2089 2090 private static void transpose_mat(double[] in, double[] out) { 2091 out[0] = in[0]; 2092 out[1] = in[3]; 2093 out[2] = in[6]; 2094 2095 out[3] = in[1]; 2096 out[4] = in[4]; 2097 out[5] = in[7]; 2098 2099 out[6] = in[2]; 2100 out[7] = in[5]; 2101 out[8] = in[8]; 2102 } 2103 2104 private static double max3(double[] values) { 2105 if (values[0] > values[1]) { 2106 if (values[0] > values[2]) 2107 return (values[0]); 2108 else 2109 return (values[2]); 2110 } else { 2111 if (values[1] > values[2]) 2112 return (values[1]); 2113 else 2114 return (values[2]); 2115 } 2116 } 2117 2118 private static boolean almostEqual(double a, double b) { 2119 if (a == b) 2120 return true; 2121 2122 final double EPSILON_ABSOLUTE = 1.0e-6; 2123 double diff = abs(a - b); 2124 2125 if (diff < EPSILON_ABSOLUTE) 2126 return true; 2127 2128 final double EPSILON_RELATIVE = 1.0e-4; 2129 double absA = abs(a); 2130 double absB = abs(b); 2131 double max = (absA >= absB) ? absA : absB; 2132 2133 return (diff / max) < EPSILON_RELATIVE; 2134 } 2135 2136 /** 2137 * Holds the default XML representation for this transformation matrix. For example: 2138 * <pre> 2139 * <GTransform> 2140 * <Float64 value="1" /> 2141 * <Float64 value="0" /> 2142 * <Float64 value="0" /> 2143 * <Float64 value="0" /> 2144 * <Float64 value="0" /> 2145 * <Float64 value="1" /> 2146 * <Float64 value="0" /> 2147 * <Float64 value="0" /> 2148 * <Float64 value="0" /> 2149 * <Float64 value="0" /> 2150 * <Float64 value="1" /> 2151 * <Float64 value="0" /> 2152 * <Float64 value="0" /> 2153 * <Float64 value="0" /> 2154 * <Float64 value="0" /> 2155 * <Float64 value="1" /> 2156 * </GTransform> 2157 * </pre> 2158 */ 2159 protected static final XMLFormat<GTransform> XML 2160 = new XMLFormat<GTransform>(GTransform.class) { 2161 2162 @Override 2163 public GTransform newInstance(Class<GTransform> cls, InputElement xml) throws XMLStreamException { 2164 return FACTORY.object(); 2165 } 2166 2167 @Override 2168 public void read(InputElement xml, GTransform M) throws XMLStreamException { 2169 2170 FastTable<Float64Vector> rowList = FastTable.newInstance(); 2171 2172 try { 2173 for (int i = 0; i < 4; ++i) { 2174 if (!xml.hasNext()) 2175 throw new XMLStreamException( 2176 MessageFormat.format(RESOURCES.getString("need16ElementsFoundLess"), i)); 2177 2178 double x = ((Float64)xml.getNext()).doubleValue(); 2179 double y = ((Float64)xml.getNext()).doubleValue(); 2180 double z = ((Float64)xml.getNext()).doubleValue(); 2181 double w = ((Float64)xml.getNext()).doubleValue(); 2182 Float64Vector row = Float64Vector.valueOf(x, y, z, w); 2183 rowList.add(row); 2184 } 2185 2186 if (xml.hasNext()) 2187 throw new XMLStreamException(RESOURCES.getString("gtToManyElements")); 2188 2189 M._data = Float64Matrix.valueOf(rowList); 2190 2191 } finally { 2192 FastTable.recycle(rowList); 2193 } 2194 2195 } 2196 2197 @Override 2198 public void write(GTransform M, OutputElement xml) throws XMLStreamException { 2199 2200 for (int i = 0; i < 4; i++) { 2201 for (int j = 0; j < 4; j++) { 2202 xml.add(M.get(i, j)); 2203 } 2204 } 2205 } 2206 }; 2207 2208 /** 2209 * Tests the methods in this class. 2210 * 2211 * @param args Command-line arguments (not used). 2212 */ 2213 public static void main(String args[]) { 2214 System.out.println("Testing GTransform:"); 2215 2216 Parameter<Angle> psi = Parameter.valueOf(60, javax.measure.unit.NonSI.DEGREE_ANGLE); 2217 Parameter<Angle> theta = Parameter.ZERO_ANGLE; 2218 Parameter<Angle> phi = Parameter.valueOf(20, javax.measure.unit.NonSI.DEGREE_ANGLE); 2219 System.out.println("\npsi = " + psi); 2220 2221 GTransform T1 = GTransform.newRotationZ(psi); 2222 System.out.println(" T1 = \n" + T1); 2223 2224 System.out.println("\nphi = " + phi); 2225 2226 GTransform T2 = GTransform.newRotationX(phi); 2227 System.out.println(" T2 = \n" + T2); 2228 System.out.println(" T1.times(T2) = \n" + T1.times(T2)); 2229 2230 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 2231 DCMatrix M3 = DCMatrix.getEulerTM(psi, theta, phi); 2232 System.out.println("M3 = \n" + M3); 2233 GTransform T3 = GTransform.valueOf(M3); 2234 System.out.println(" T3 = \n" + T3); 2235 2236 Point p1 = Point.valueOf(1, 1, 1, METER); 2237 System.out.println("\np1 = " + p1); 2238 System.out.println("M3.times(p1.toVector3D()) = " + M3.times(p1.toVector3D())); 2239 System.out.println("T3.transform(p1) = " + T3.transform(p1)); 2240 System.out.println("T3.transform(p1).norm() = " + T3.transform(p1).norm() + " [1.73205080756888 m]"); 2241 2242 GTransform T4 = T3.applyScale(2.5); 2243 System.out.println("\nT4 = T3.applyScale(2.5);"); 2244 System.out.println("T4.transform(p1) = " + T4.transform(p1)); 2245 System.out.println("T4.transform(p1).norm() = " + T4.transform(p1).norm() + " [4.33012701892219 m]"); 2246 System.out.println("T4.getScale() = " + T4.getScale()); 2247 System.out.println("T4.getRotation() = \n" + T4.getRotation()); 2248 2249 GTransform T5 = GTransform.newTranslation(Vector.valueOf(METER, 10, -5, 0.5)); 2250 System.out.println("\np1 = " + p1); 2251 System.out.println("T5.transform(p1) = " + T5.transform(p1) + " [11 m, -4 m, 1.5 m]"); 2252 2253 // Write out XML data. 2254 try { 2255 System.out.println(); 2256 2257 // Creates some useful aliases for class names. 2258 javolution.xml.XMLBinding binding = new GeomXMLBinding(); 2259 2260 javolution.xml.XMLObjectWriter writer = javolution.xml.XMLObjectWriter.newInstance(System.out); 2261 writer.setIndentation(" "); 2262 writer.setBinding(binding); 2263 writer.write(T4, "GTransform", GTransform.class); 2264 writer.flush(); 2265 2266 System.out.println(); 2267 } catch (Exception e) { 2268 e.printStackTrace(); 2269 } 2270 2271 } 2272 2273}