diff --git a/src/edu/nps/moves/dis7/utilities/CoordinateConversions.java b/src/edu/nps/moves/dis7/utilities/CoordinateConversions.java
index 823ea30e351917891dc03ba8f3938b676afa0652..92614862463c67429fd8b95e8bff3735ed35de60 100644
--- a/src/edu/nps/moves/dis7/utilities/CoordinateConversions.java
+++ b/src/edu/nps/moves/dis7/utilities/CoordinateConversions.java
@@ -2,7 +2,6 @@
* Copyright (c) 2008-2021, MOVES Institute, Naval Postgraduate School (NPS). All rights reserved.
* This work is provided under a BSD open-source license, see project license.html and license.txt
*/
-
package edu.nps.moves.dis7.utilities;
/**
@@ -14,16 +13,17 @@ public class CoordinateConversions
{
/** conversion factor */
public static final double RADIANS_TO_DEGREES = 180.0/Math.PI;
+
/** conversion factor */
public static final double DEGREES_TO_RADIANS = Math.PI/180.0;
- private CoordinateConversions()
- {
- }
+ private CoordinateConversions() {}
+
/**
* Converts DIS xyz world coordinates to latitude and longitude (IN RADIANS). This algorithm may not be 100% accurate
- * near the poles. Uses WGS84 , though you can change the ellipsoid constants a and b if you want to use something
+ * near the poles. Uses WGS84, though you can change the ellipsoid constants a and b if you want to use something
* else. These formulas were obtained from Military Handbook 600008
+ *
* @param xyz A double array with the x, y, and z coordinates, in that order.
* @return An array with the lat, long, and elevation corresponding to those coordinates.
* Elevation is in meters, lat and long are in radians
@@ -45,7 +45,7 @@ public class CoordinateConversions
eSquared = (a*a - b*b) / (a*a);
ePrimeSquared = (a*a - b*b) / (b*b);
- /**
+ /*
* Get the longitude.
*/
if(x >= 0 )
@@ -61,7 +61,7 @@ public class CoordinateConversions
answer[1] = Math.atan(y/x) - Math.PI;
}
- /**
+ /*
* Longitude calculation done. Now calculate latitude.
* NOTE: The handbook mentions using the calculated phi (latitude) value to recalculate B
* using tan B = (1-f) tan phi and then performing the entire calculation again to get more accurate values.
@@ -74,7 +74,8 @@ public class CoordinateConversions
double tanPhi = (z + (ePrimeSquared * b * (Math.pow(Math.sin(BZero), 3))) ) /(W - (a * eSquared * (Math.pow(Math.cos(BZero), 3))));
double phi = Math.atan(tanPhi);
answer[0] = phi;
- /**
+
+ /*
* Latitude done, now get the elevation. Note: The handbook states that near the poles, it is preferable to use
* h = (Z / sin phi ) - rSubN + (eSquared * rSubN). Our applications are never near the poles, so this formula
* was left unimplemented.
@@ -88,8 +89,9 @@ public class CoordinateConversions
/**
* Converts DIS xyz world coordinates to latitude and longitude (IN DEGREES). This algorithm may not be 100% accurate
- * near the poles. Uses WGS84 , though you can change the ellipsoid constants a and b if you want to use something
+ * near the poles. Uses WGS84, though you can change the ellipsoid constants a and b if you want to use something
* else. These formulas were obtained from Military Handbook 600008
+ *
* @param xyz A double array with the x, y, and z coordinates, in that order.
* @return An array with the lat, lon, and elevation corresponding to those coordinates.
* Elevation is in meters, lat and long are in degrees
@@ -102,11 +104,10 @@ public class CoordinateConversions
degrees[1] = degrees[1] * CoordinateConversions.RADIANS_TO_DEGREES;
return degrees;
-
}
/**
- * Converts lat long and geodetic height (elevation) into DIS XYZ
+ * Converts lat long and geodetic height (elevation) into DIS XYZ.
* This algorithm also uses the WGS84 ellipsoid, though you can change the values
* of a and b for a different ellipsoid. Adapted from Military Handbook 600008
* @param latitude The latitude, IN RADIANS
@@ -121,7 +122,6 @@ public class CoordinateConversions
double cosLat = Math.cos(latitude);
double sinLat = Math.sin(latitude);
-
double rSubN = (a*a) / Math.sqrt(((a*a) * (cosLat*cosLat) + ((b*b) * (sinLat*sinLat))));
double X = (rSubN + height) * cosLat * Math.cos(longitude);
@@ -132,9 +132,10 @@ public class CoordinateConversions
}
/**
- * Converts lat long IN DEGREES and geodetic height (elevation) into DIS XYZ
+ * Converts lat long IN DEGREES and geodetic height (elevation) into DIS XYZ.
* This algorithm also uses the WGS84 ellipsoid, though you can change the values
* of a and b for a different ellipsoid. Adapted from Military Handbook 600008
+ *
* @param latitude The latitude, IN DEGREES
* @param longitude The longitude, in DEGREES
* @param height The elevation, in meters
@@ -145,7 +146,6 @@ public class CoordinateConversions
double degrees[] = CoordinateConversions.getXYZfromLatLonRadians(latitude * CoordinateConversions.DEGREES_TO_RADIANS,
longitude * CoordinateConversions.DEGREES_TO_RADIANS,
height);
-
return degrees;
}
}
\ No newline at end of file
diff --git a/src/edu/nps/moves/dis7/utilities/EulerConversions.java b/src/edu/nps/moves/dis7/utilities/EulerConversions.java
new file mode 100644
index 0000000000000000000000000000000000000000..2fca76ed2f73fa4062f43d2050451e69e85e8980
--- /dev/null
+++ b/src/edu/nps/moves/dis7/utilities/EulerConversions.java
@@ -0,0 +1,202 @@
+package edu.nps.moves.dis7.utilities;
+
+/**
+ * Class contains methods that convert to Tait_Bryan_angles (i.e., roll, pitch
+ * and yaw/heading) given the position (i.e., latitude, longitude) and the
+ * euler angles (i.e., psi, theta, and phi).
+ *
+ * Class also has methods for the corollary: converting to psi, theta, and phi
+ * given the lat/lon position and the entity's roll, pitch and yaw angles
+ *
+ * In this class roll, pitch and yaw are always expressed in degrees
+ * whereas psi, theta, and phi are always in radians.
+ *
+ * Note: latitude and longitude are also expressed in radians.
+ *
+ *
+ * @author loyaj & bhughes
+ *
+ */
+
+public class EulerConversions
+{
+
+ static double _toDegrees = 57.2957795131;
+ static double _toRadians = 0.01745329252;
+
+ /**
+ * Gets a degree heading for an entity based on euler angles. All angular values passed in must be in radians.
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param psi Psi angle, IN RADIANS
+ * @param theta Theta angle, IN RADIANS
+ * @return the heading, in degrees, with 0 being north, positive angles going clockwise,
+ * and negative angles going counterclockwise (i.e., 90 deg is east, -90 is west)
+ */
+ public static double getOrientationFromEuler(double lat, double lon, double psi, double theta)
+ {
+ double sinlat = Math.sin(lat);
+ double sinlon = Math.sin(lon);
+ double coslon = Math.cos(lon);
+ double coslat = Math.cos(lat);
+ double sinsin = sinlat * sinlon;
+
+ double cosTheta = Math.cos(theta);
+ double cosPsi = Math.cos(psi);
+ double sinPsi = Math.sin(psi);
+ double sinTheta = Math.sin(theta);
+
+
+ double cosThetaCosPsi = cosTheta * cosPsi;
+ double cosThetaSinPsi = cosTheta * sinPsi;
+ double sincos = sinlat * coslon;
+
+ double b11 = -sinlon * cosThetaCosPsi + coslon * cosThetaSinPsi;
+ double b12 = -sincos * cosThetaCosPsi - sinsin * cosThetaSinPsi - coslat * sinTheta;
+
+ return Math.toDegrees(Math.atan2(b11, b12));//range is -pi to pi
+ }
+
+ /**
+ * Gets a degree pitch for an entity based on euler angles. All angular values passed in must be in radians.
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param psi Psi angle, IN RADIANS
+ * @param theta Theta angle, IN RADIANS
+ * @return the pitch, in degrees, with 0 being level. A negative values is when the entity's
+ * nose is pointing downward, positive value is when the entity's nose is pointing upward.
+ */
+ public static double getPitchFromEuler(double lat, double lon, double psi, double theta)
+ {
+ double sinlat = Math.sin(lat);
+ double sinlon = Math.sin(lon);
+ double coslon = Math.cos(lon);
+ double coslat = Math.cos(lat);
+ double cosLatCosLon = coslat * coslon;
+ double cosLatSinLon = coslat * sinlon;
+
+ double cosTheta = Math.cos(theta);
+ double cosPsi = Math.cos(psi);
+ double sinPsi = Math.sin(psi);
+ double sinTheta = Math.sin(theta);
+
+ return Math.toDegrees(Math.asin(cosLatCosLon*cosTheta*cosPsi + cosLatSinLon*cosTheta*sinPsi - sinlat*sinTheta));
+ }
+
+ /**
+ * Gets the degree roll for an entity based on euler angles. All angular values passed in must be in radians.
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param psi Psi angle, IN RADIANS
+ * @param theta Theta angle, IN RADIANS
+ * @param phi Phi angle, IN RADIANS
+ * @return the roll, in degrees, with 0 being level flight, + roll is clockwise when looking out the front of the entity.
+ */
+ public static double getRollFromEuler(double lat, double lon, double psi, double theta, double phi)
+ {
+ double sinlat = Math.sin(lat);
+ double sinlon = Math.sin(lon);
+ double coslon = Math.cos(lon);
+ double coslat = Math.cos(lat);
+ double cosLatCosLon = coslat * coslon;
+ double cosLatSinLon = coslat * sinlon;
+
+ double cosTheta = Math.cos(theta);
+ double sinTheta = Math.sin(theta);
+ double cosPsi = Math.cos(psi);
+ double sinPsi = Math.sin(psi);
+ double sinPhi = Math.sin(phi);
+ double cosPhi = Math.cos(phi);
+
+ double sinPhiSinTheta = sinPhi * sinTheta;
+ double cosPhiSinTheta = cosPhi * sinTheta;
+
+ double b23 = cosLatCosLon*(-cosPhi*sinPsi + sinPhiSinTheta*cosPsi) +
+ cosLatSinLon*( cosPhi*cosPsi + sinPhiSinTheta*sinPsi) +
+ sinlat * (sinPhi * cosTheta);
+
+ double b33 = cosLatCosLon*( sinPhi*sinPsi + cosPhiSinTheta*cosPsi) +
+ cosLatSinLon*(-sinPhi*cosPsi + cosPhiSinTheta*sinPsi) +
+ sinlat * (cosPhi * cosTheta);
+
+ return Math.toDegrees(Math.atan2(-b23, -b33));
+ }
+
+ /**
+ * Gets the Euler Theta value (in radians) from position and Tait-Brayn yaw and roll angles
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param yaw entity's yaw angle (also know as the entity's bearing or heading angle), in degrees
+ * @param pitch entity's pitch angle, in degrees
+ * @return the Theta value in radians
+ */
+ public static double getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch)
+ {
+ double sinLat = Math.sin(lat);
+ double cosLat = Math.cos(lat);
+
+ double cosPitch = Math.cos(pitch*_toRadians);
+ double sinPitch = Math.sin(pitch*_toRadians);
+ double cosYaw = Math.cos(yaw*_toRadians);
+
+ return Math.asin( -cosLat * cosYaw * cosPitch - sinLat * sinPitch );
+ }
+
+ /**
+ * Gets the Euler Psi value (in radians) from position and Tait-Brayn yaw and roll angles
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param yaw ettity's yaw angle (also know as the entity's bearing or heading angle), in degrees
+ * @param pitch entity's pitch angle, in degrees
+ * @return the Psi value in radians
+ */
+ public static double getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch){
+
+ double sinLat = Math.sin(lat);
+ double sinLon = Math.sin(lon);
+ double cosLon = Math.cos(lon);
+ double cosLat = Math.cos(lat);
+ double cosLatCosLon = cosLat * cosLon;
+ double cosLatSinLon = cosLat * sinLon;
+ double sinLatCosLon = sinLat * cosLon;
+ double sinLatSinLon = sinLat * sinLon;
+
+ double cosPitch = Math.cos(pitch*_toRadians);
+ double sinPitch = Math.sin(pitch*_toRadians);
+ double sinYaw = Math.sin(yaw*_toRadians);
+ double cosYaw = Math.cos(yaw*_toRadians);
+
+ double a_11 = -sinLon * sinYaw * cosPitch - sinLatCosLon * cosYaw * cosPitch + cosLatCosLon * sinPitch;
+ double a_12 = cosLon * sinYaw * cosPitch - sinLatSinLon * cosYaw * cosPitch + cosLatSinLon * sinPitch;
+
+ return Math.atan2(a_12, a_11);
+ }
+
+ /**
+ * Gets the Euler Phi value (in radians) from position and Tait-Brayn yaw, pitch and roll angles
+ * @param lat Entity's latitude, IN RADIANS
+ * @param lon Entity's longitude, IN RADIANS
+ * @param yaw yaw angle (also know as the entity's bearing or heading angle), in degrees
+ * @param pitch entity's pitch angle, in degrees
+ * @param roll entity's roll angle (0 is level flight, + roll is clockwise looking out the nose), in degrees
+ * @return the Phi value in radians
+ */
+ public static double getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll){
+
+ double sinLat = Math.sin(lat);
+ double cosLat = Math.cos(lat);
+
+ double cosRoll = Math.cos(roll*_toRadians);
+ double sinRoll = Math.sin(roll*_toRadians);
+ double cosPitch = Math.cos(pitch*_toRadians);
+ double sinPitch = Math.sin(pitch*_toRadians);
+ double sinYaw = Math.sin(yaw*_toRadians);
+ double cosYaw = Math.cos(yaw*_toRadians);
+
+ double a_23 = cosLat * (-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll) - sinLat * cosPitch * sinRoll;
+ double a_33 = cosLat * ( sinYaw * sinRoll + cosYaw * sinPitch * cosRoll) - sinLat * cosPitch * cosRoll;
+
+ return Math.atan2(a_23, a_33);
+ }
+
+}