001/*
002*   AbstractRotation -- Partial implementation of a Rotation.
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
007*   modify it under the terms of the GNU Lesser General Public
008*   License as published by the Free Software Foundation; either
009*   version 2 of the License, or (at your option) any later version.
010*   
011*   This library is distributed in the hope that it will be useful,
012*   but WITHOUT ANY WARRANTY; without even the implied warranty of
013*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
014*   Lesser General Public License for more details.
015*
016*   You should have received a copy of the GNU Lesser General Public License
017*   along with this program; if not, write to the Free Software
018*   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
019*   Or visit:  http://www.gnu.org/licenses/lgpl.html
020**/
021package jahuwaldt.js.param;
022
023import jahuwaldt.tools.math.MathTools;
024import java.util.ResourceBundle;
025import javax.measure.Measurable;
026import javax.measure.unit.SI;
027import javax.measure.quantity.*;
028import static javolution.lang.MathLib.*;
029import org.jscience.mathematics.vector.Float64Vector;
030
031/**
032 * <p>
033 * This class represents a partial implementation of a Rotation object.</p>
034 *
035 * <p> Modified by: Joseph A. Huwaldt </p>
036 *
037 * @author Joseph A. Huwaldt Date: October 22, 2009
038 * @version February 23, 2025
039 */
040public abstract class AbstractRotation<T extends AbstractRotation<?>> implements Rotation<T> {
041
042    /**
043     * The resource bundle for this package.
044     */
045    protected static final ResourceBundle RESOURCES = Parameter.RESOURCES;
046
047    /**
048     * Returns the direction cosine T.M. of the aerospace psi-&gt;theta Euler sequence.
049     *
050     * @param psi   First rotation angle.
051     * @param theta Second rotation angle.
052     * @return the 3x3 transformation matrix
053     */
054    public static DCMatrix getPsiThetaTM(Measurable<Angle> psi, Measurable<Angle> theta) {
055        double psiR = psi.doubleValue(SI.RADIAN);
056        double thetaR = theta.doubleValue(SI.RADIAN);
057
058        double cpsi = cos(psiR);
059        double spsi = sin(psiR);
060        double ctheta = cos(thetaR);
061        double stheta = sin(thetaR);
062
063        Float64Vector row0 = Float64Vector.valueOf((ctheta * cpsi), -spsi, (stheta * cpsi));
064        Float64Vector row1 = Float64Vector.valueOf((ctheta * spsi), cpsi, (stheta * spsi));
065        Float64Vector row2 = Float64Vector.valueOf(-stheta, 0, ctheta);
066
067        return DCMatrix.valueOf(row0, row1, row2);
068    }
069
070    /**
071     * Returns the direction cosine T.M. of the aerospace psi-&gt;theta Euler sequence from a
072     * polar coordinate.
073     *
074     * @param coord A polar coordinate (magnitude, psi=azimuth, theta=elevation).
075     * @return the 3x3 transformation matrix
076     */
077    public static DCMatrix getPsiThetaTM(Polar3D<?> coord) {
078        return getPsiThetaTM(coord.getAzimuth(), coord.getElevation());
079    }
080
081    /**
082     * Returns the direction cosine T.M. of the aerospace Euler 3-2-1 (psi-&gt;theta-phi)
083     * rotation sequence. The Euler angle transformation matrix of flight mechanics.
084     *
085     * @param psi   First rotation angle.
086     * @param theta Second rotation angle.
087     * @param phi   Third rotation angle.
088     * @return the 3x3 transformation matrix
089     */
090    public static DCMatrix getEulerTM(Measurable<Angle> psi, Measurable<Angle> theta, Measurable<Angle> phi) {
091        double psiR = psi.doubleValue(SI.RADIAN);
092        double thetaR = theta.doubleValue(SI.RADIAN);
093        double phiR = phi.doubleValue(SI.RADIAN);
094
095        double cpsi = cos(psiR);
096        double spsi = sin(psiR);
097        double ctheta = cos(thetaR);
098        double stheta = sin(thetaR);
099        double cphi = cos(phiR);
100        double sphi = sin(phiR);
101
102        Float64Vector row0 = Float64Vector.valueOf((cpsi * ctheta), (cpsi * stheta * sphi - spsi * cphi), (cpsi * stheta * cphi + spsi * sphi));
103        Float64Vector row1 = Float64Vector.valueOf((spsi * ctheta), (spsi * stheta * sphi + cpsi * cphi), (spsi * stheta * cphi - cpsi * sphi));
104        Float64Vector row2 = Float64Vector.valueOf(-stheta, (ctheta * sphi), (ctheta * cphi));
105
106        return DCMatrix.valueOf(row0, row1, row2);
107    }
108
109    /**
110     * Returns the text representation of this rotation transform as a
111     * <code>java.lang.String</code>.
112     *
113     * @return <code>toText().toString()</code>
114     */
115    @Override
116    public final String toString() {
117        return toText().toString();
118    }
119
120    /**
121     * Compares this Rotation against the specified Rotation for approximate equality (a
122     * Rotation object with DCM values equal to this one to within the numerical roundoff
123     * tolerance).
124     *
125     * @param obj the Rotation object to compare with.
126     * @return <code>true</code> if this Rotation is approximately identical to that
127     * Rotation; <code>false</code> otherwise.
128     */
129    public boolean isApproxEqual(Rotation<?> obj) {
130        if (this == obj)
131            return true;
132        if (obj == null)
133            return false;
134
135        //      Check for exact equality.
136        if (this.equals(obj))
137            return true;
138
139        //      Check for approximate equality of DCM's.
140        DCMatrix thisDCM = this.toDCM();
141        DCMatrix thatDCM = obj.toDCM();
142        for (int i = 0; i < 3; ++i) {
143            for (int j = 0; j < 3; ++j) {
144                double thisVal = thisDCM.getValue(i, j);
145                double thatVal = thatDCM.getValue(i, j);
146                if (!MathTools.isApproxEqual(thisVal, thatVal, Parameter.EPS10))
147                    return false;
148            }
149        }
150
151        return true;
152    }
153
154}