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 &amp; 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 &amp; 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 &amp; 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 &amp; 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 &amp; 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 &amp; 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 &amp; 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 &amp; 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 &amp; 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     *    &lt;GTransform&gt;
2140     *        &lt;Float64 value="1" /&gt;
2141     *        &lt;Float64 value="0" /&gt;
2142     *        &lt;Float64 value="0" /&gt;
2143     *        &lt;Float64 value="0" /&gt;
2144     *        &lt;Float64 value="0" /&gt;
2145     *        &lt;Float64 value="1" /&gt;
2146     *        &lt;Float64 value="0" /&gt;
2147     *        &lt;Float64 value="0" /&gt;
2148     *        &lt;Float64 value="0" /&gt;
2149     *        &lt;Float64 value="0" /&gt;
2150     *        &lt;Float64 value="1" /&gt;
2151     *        &lt;Float64 value="0" /&gt;
2152     *        &lt;Float64 value="0" /&gt;
2153     *        &lt;Float64 value="0" /&gt;
2154     *        &lt;Float64 value="0" /&gt;
2155     *        &lt;Float64 value="1" /&gt;
2156     *    &lt;/GTransform&gt;
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}