Skip to content
Snippets Groups Projects
Commit f9f5dbd0 authored by terry-norbraten's avatar terry-norbraten
Browse files

[Terry N.] update conversions. Add Euler conv

parent 7c839b25
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/edu/nps/moves/dis7/utilities/CoordinateConversions.java
+ 14
− 14
View file @ f9f5dbd0
......@@ -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
src/edu/nps/moves/dis7/utilities/EulerConversions.java 0 → 100644
+ 202
− 0
View file @ f9f5dbd0
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);
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment