Something went wrong on our end
-
Brutzman, Don authoredBrutzman, Don authored
RangeCoordinates.java 24.42 KiB
package edu.nps.moves.spatial;
import SRM.*; // Sedris spatial reference model version 4.4
import edu.nps.moves.dis7.pdus.LiveEntityOrientation;
import edu.nps.moves.dis7.pdus.Vector3Double;
/**
* <p>Represents a local, flat range area with Euclidean coordinates, which is
* convenient for somewhat small simulated areas. This class assumes a local,
* flat, coordinate system with an origin at (lat, lon, altitude) and positive X
* pointing local east, positive Y pointing local north, and positive Z pointing
* up. Specified in WGS_84 geodesic coordinate system. Altitude is distance
* above the ellipsoid.</p>
*
* <p>The coordinate system has its origin at the given (lat, lon) and creates a
* plane tangent and normal to the ellipsoid at that point. </p>
*
* <p>There are several major reference frames that may be useful in various contexts:</p>
*
* <p>Geocentric: Origin at the center of the earth. Positive X out at the intersection
* of the equator and prime meridian, Y out at 90 degrees east lon, and Z up through
* the north pole. This is the coordinate system used by DIS world coordinates.</p>
*
* <p>Geodetic: The coordinate system uses lat/lon/altitude. This is handy for positioning
* an object on the earth (or close to it) but not so handy for describing things
* like velocity.</p>
*
* <p>Local Tangent Surface Euclidean (LTSE): Pick a lat/lon/altitude, and then at
* that point you can define a single plane normal and tangent to the globe. Positive X points
* local east, positive Y points local north, and positive Z points local up. This
* is handy for describing the position of an object in, for example, a range of
* somewhat small dimensions, perhaps 20KM X 20KM, where we don't want to get sucked
* into the whole curved earth scene and just want to be simple.</p>
*
* <p>Body Centric/Lococentric/Platform-centric: The origin is at the volumetric center
* of an entity (in DIS); Positive
* x points out the long axis, positive Y points to the right, and positive Z points
* down. This is widely used to describe (roll, pitch, yaw) in aircraft. Note that you
* need a transform from (for example) the LTSE to body coordinates to define the
* position of the body axis origin and orientation WRT the LTSE origin. Note that
* the direction of the Z axis is the opposite of that used by LTSE. The axes are
* often named (u,v,w) in this frame of reference. </p>
*
* <p>We can also convert between these coordinate systems using standard libraries
* in the SRM. </p>
*
* See User’s Manual for SRM Orientation, Velocity, and Acceleration
* Transformations Version 2.0, 18 Nov 2009, available with the
* sedris Java SDK download.
*
* @author DMcG
*/
public class RangeCoordinates
{
/** A reference frame for the earth's surface, ie an ellipsoid with coordinates
* of the form (lat, lon, altitude). The technical term for this would be
* geodetic.
*/
SRF_Celestiodetic earthSurfaceReferenceFrame;
Coord3D earthSurfaceReferenceFrameOrigin;
/** A DIS reference frame, with a Euclidean coordinate system with origin
* at the center of of the earth. Coordinates, in (x, y, z), in meters. This
* is the reference frame used by many DIS fields on the wire. The technical
* term for this would be geocentric. Z is through the north pole, x out
* the prime meridian at the equator, and y out the equator at 90 degrees east.
*/
SRF_Celestiocentric disCoordinateReferenceFrame;
/** A local, flat, Euclidean reference frame. This is tangent to a (lat, lon, altitudeOrigin) * on an earth that is supplied by the user in the constructor.
* This allows users to set up a local, relatively small area
* for moving things around without in the nuisance of worrying about curved
* earth. The technical term for this would be Local Tangent Euclidean, ie
* a plane tangent to the earth at the given (lat, lon, alt). Coordinate system
* is x east, y north, z up.
*/
SRF_LocalTangentSpaceEuclidean localTangentSurfaceReferenceFrame;
/** The origin of the local Euclidean reference frame, in Sedris data structure */
Coord3D localEuclidianOrigin;
/** The latitude and longitude of the local, flat, Euclidean coordinate system origin */
double latitudeOrigin, longitudeOrigin;
/** The altitude of the local coordinate system origin, i.e. the distance
* above the ellipsoid, not distance above terrain
*/
double altitudeOrigin;
/**
* <p>Constructor for a local flat coordinate system. Takes the latitude and
* longitude (in degrees) for WGS_84 and the height above the ellipsoid
* and creates a local, flat coordinate system at that point.</p>
*
* @param originLat Origin of the flat local coordinate system, in degrees, latitude
* @param originLon Origin of the flat local coordinate system, in degrees, longitude
* @param heightOffset altitudeOrigin above ellipsoid surface, in meters
*/
public RangeCoordinates(double originLat, double originLon, double heightOffset)
{
latitudeOrigin = originLat;
longitudeOrigin = originLon;
altitudeOrigin = heightOffset;
try
{
// Create a Celestiodetic SRF with WGS 1984, ie a curved coordinate
// system (lat/lon/alt)
earthSurfaceReferenceFrame = new SRF_Celestiodetic(SRM_ORM_Code.ORMCOD_WGS_1984,
SRM_RT_Code.RTCOD_WGS_1984_IDENTITY);
// Create a Celesticentric SRF with WGS 1984, ie a rectilinear,
// earth-centered coordinate system as used in DIS
disCoordinateReferenceFrame = new SRF_Celestiocentric(SRM_ORM_Code.ORMCOD_WGS_1984,
SRM_RT_Code.RTCOD_WGS_1984_IDENTITY);
double latInRadians = Math.toRadians(originLat);
double lonInRadians = Math.toRadians(originLon);
// Reference system for a local tangent euclidian space plane, tangent to the lat/lon
// at a give altitude.
localTangentSurfaceReferenceFrame =
new SRF_LocalTangentSpaceEuclidean(SRM_ORM_Code.ORMCOD_WGS_1984,
SRM_RT_Code.RTCOD_WGS_1984_IDENTITY,
lonInRadians, latInRadians, // Origin (note: lon, lat)
0.0, // Azimuth; can rotate axis, but don't.
0.0, 0.0, // False x,y origin (can offset origin to avoid negative coordinates)
heightOffset); // Height offset
// It's handy to have this pre-created in some calculations
localEuclidianOrigin = localTangentSurfaceReferenceFrame.createCoordinate3D(0.0, 0.0, 0.0);
earthSurfaceReferenceFrameOrigin = earthSurfaceReferenceFrame.createCoordinate3D(latInRadians, lonInRadians, heightOffset);
}
catch(SrmException e)
{
System.err.println("problem creating coordinate systems" + e);
}
}
/** Changes a Vector3Double from the local coordinate system (flat, Euclidean,
* origin given at (lat, lon, alt)) to a global, DIS, earth-centric coordinate
* system. Overwrites the values currently in Vector3Double passed in.
*
* @param localCoordinates Position in local Euclidean coordinate system. Values are overwritten to the DIS earth-centric coordinate system on return
*/
public void changeVectorToDisCoordFromLocalFlat(Vector3Double localCoordinates)
{
Vector3Double vec = this.DISCoordFromLocalFlat(localCoordinates.getX(),
localCoordinates.getY(),
localCoordinates.getZ());
localCoordinates.setX(vec.getX());
localCoordinates.setY(vec.getY());
localCoordinates.setZ(vec.getZ());
}
/**
* Transform from local, flat coordinate system to the DIS coordinate system.
* All units in meters, positive x east, y north, z altitude.
*
* @param x x coordinate in local, flat coordinate system
* @param y y coordinate in meters in local, flat coordinate system
* @param z z coordinate, altitude, in meters in local flat coordinate system
* @return DIS coordinates
*/
public Vector3Double DISCoordFromLocalFlat(double x, double y, double z)
{
Vector3Double disCoordinates = new Vector3Double();
try
{
// Holds coordinates in the local tangent plane
Coord3D localCoordinates = localTangentSurfaceReferenceFrame.createCoordinate3D(x, y, z);
// holds coordinates in the DIS (geocentric) coordinate frame
Coord3D disCoord = disCoordinateReferenceFrame.createCoordinate3D(0.0, 0.0, 0.0);
SRM_Coordinate_Valid_Region_Code region = disCoordinateReferenceFrame.changeCoordinateSRF(localCoordinates, disCoord);
//System.out.println(region);
// convert from the local tanget plane coordinates to the DIS coordinate frame
double values[] = disCoordinateReferenceFrame.getCoordinate3DValues(disCoord);
//System.out.println("DIS x:" + values[0] + " y:" + values[1] + " z:" + values[2] );
// Set the values in the return object
disCoordinates.setX(values[0]);
disCoordinates.setY(values[1]);
disCoordinates.setZ(values[2]);
}
catch(SrmException e)
{
//Should throw exception here
System.err.println("can't change to DIS coord " + e);
return null;
}
return disCoordinates;
}
/**
* Changes the world-coordinates vector3double to the local euclidian flat
* coordinate system. Overwrites the values in worldCoordinates.
*
* @param worldCoordinates local euclidian flat coordinates
*/
public void changeVectorToLocalCoordFromDIS(Vector3Double worldCoordinates)
{
Vector3Double vec = this.localCoordFromDis(worldCoordinates.getX(),
worldCoordinates.getY(),
worldCoordinates.getZ());
worldCoordinates.setX(vec.getX()); worldCoordinates.setY(vec.getY());
worldCoordinates.setZ(vec.getZ());
}
/**
* Given DIS coordinates, convert to the local Euclidean plane coordinates.
*
* @param x DIS coordinates x
* @param y DIS coordinates y
* @param z DIS coordinates z
* @return local Euclidean plane coordinates
*/
public Vector3Double localCoordFromDis(double x, double y, double z)
{
Vector3Double local = new Vector3Double();
try
{
// Holds position in the local tangent plane
Coord3D localCoordinates = localTangentSurfaceReferenceFrame.createCoordinate3D(0.0, 0.0, 0.0);
// Holds position in the DIS reference frame
Coord3D disCoord = disCoordinateReferenceFrame.createCoordinate3D(x, y, z);
SRM_Coordinate_Valid_Region_Code region = localTangentSurfaceReferenceFrame.changeCoordinateSRF(disCoord, localCoordinates);
//System.out.println("Region:" + region);
// Get the position in the local tangent place (ie, range coordinates) from DIS coordinates (ie, geocentric)
double values[] = localTangentSurfaceReferenceFrame.getCoordinate3DValues(localCoordinates);
//System.out.println("-->Local x:" + values[0] + " y:" + values[1] + " z:" + values[2] );
local.setX(values[0]);
local.setY(values[1]);
local.setZ(values[2]);
}
catch(SrmException e)
{
System.err.println("can't change from DIS coord to Local" + e);
return null;
}
return local;
}
/**
* <p>Converts a pitch, roll, and heading/yaw in the local flat coordinate system to DIS Euler
* angles. Input orientation is in units of radians.DIS uses Euler angles to describe
* the orientation of an object, using an earth-centered coordinate system, with
* successive rotations about the original x, y, and z axes. You need to be careful
* here because there are all sorts of conventions for "Euler angles" including
* the order in which the axes are rotated about. </p>
*
* <p>phi = roll, theta = pitch, psi = yaw/heading<p>, by one popular convention.
* All units are in radians.</p>
*
* <p>Note that we also need the position of the object in the local coordinate system.
* The DIS Euler angles will vary depending on not just the roll/pitch/heading,
* but also where in the local coordinate frame the object is. Also, the pitch/roll/heading
* are in the local coordinate system, NOT the coordinate system of the object.</p>
*
* @param pitchRollHeading pitch, roll, and heading/yaw in local flat coordinate system
* @param localPosition position in local flat coordinate system
* @return live entity orientation
*/
public LiveEntityOrientation localRollPitchHeadingToDisEuler(LiveEntityOrientation pitchRollHeading,
Vector3Double localPosition)
{
// Tait-Bryan angles is jargon/correct terminology for the roll, pitch, and yaw. It refers
// to rotation about the original x, y, and z axes of the reference frame in use; it's also
// called cardano angles or tait-bryan. DIS uses rotation about the earth-centric origin. In
// a body-centric reference frame we have roll, pitch, and yaw. In the local frame of reference // it is rotation about x (phi), y (theta) and z (psi).
// Recall that the local frame of reference has X pointing east, y pointing north, and
// z pointing up. We are NOT using a body-centric reference frame here; we are using the
// range reference frame.
try
{
// DIS euler angles in open-dis object. This is returned by the method.
LiveEntityOrientation openDisOrientation = new LiveEntityOrientation();
// Local coordinate system orientation, in tait-bryan
OrientationTaitBryanAngles localOrientation =
new OrientationTaitBryanAngles(pitchRollHeading.getPhi(),
pitchRollHeading.getTheta(),
pitchRollHeading.getPsi());
// Local frame of reference position
Coord3D localSRMPosition = localTangentSurfaceReferenceFrame.createCoordinate3D(localPosition.getX(), localPosition.getY(), localPosition.getZ());
// DIS frame of reference position
Vector3Double disLocation = this.DISCoordFromLocalFlat(localPosition.getX(), localPosition.getY(), localPosition.getZ());
//Coord3D disCoord = disCoordinateFrame.createCoordinate3D(disLocation.getX(), disLocation.getY(), disLocation.getZ());
Coord3D disCoord = disCoordinateReferenceFrame.createCoordinate3D(6378137.0, 0.0, 0.0);
//double[] pos = disCoord.getValues();
// System.out.println("DIS position for orienation change:" + pos[0] + "," + pos[1] + "," + pos[2]);
// An empty object passed into the method call below. When returned it's filled out with
// the DIS euler angles.
SRM.Orientation disOrientation = new OrientationTaitBryanAngles();
disCoordinateReferenceFrame.transformOrientation(localSRMPosition, // INPUT: Local coordinate frame location
localOrientation, // INPUT: Local coordinate frame orientation (roll, pitch, heading)
disCoord, // INPUT: DIS coordinate frame location
disOrientation); // Output: DIS orientation in euler angles (xyz/Tait-Bryan)
SRM_Tait_Bryan_Angles_Params tait = disOrientation.getTaitBryanAngles();
openDisOrientation.setPhi((int) tait.pitch);
openDisOrientation.setTheta((int)(float)tait.roll);
openDisOrientation.setPsi((int)(float)tait.yaw);
System.out.println("disOrientation:" + disOrientation.getTaitBryanAngles());
System.out.println("DIS coordinates:" + disCoord);
return openDisOrientation;
}
catch(SrmException e)
{
System.err.println(e);
}
return null;
}
/**
*
* @param lat latitude, decimal degrees
* @param lon longitude, decimal degrees
* @param alt altitude meters
* @param bank bank angle, decimal degrees from level, positive is right bank
* @param pitch pitch angle, decimal degrees
* @param head heading angle, decimal degrees
*/
public void c(double lat, double lon, double alt,
double bank, double pitch, double head)
{
try
{ // The geocentric aircraft location
Coord3D gd_coord = earthSurfaceReferenceFrame.createCoordinate3D(Math.toRadians(lon), Math.toRadians(lat), alt);
//The geocentric location to be computed.
Coord3D gc_coord = disCoordinateReferenceFrame.createCoordinate3D(0.0, 0.0, 0.0);
disCoordinateReferenceFrame.changeCoordinate3DSRF(gd_coord, gc_coord);
System.out.println("Geocentric coordinates for location " + lat + "," + lon + " :" + gc_coord);
// Create Space-fixed Entity-to-NED Tait-Bryan Orientation ...
OrientationTaitBryanAngles tbE2NED_ori = new OrientationTaitBryanAngles(Math.toRadians(bank),
Math.toRadians(pitch),
Math.toRadians(head)); //AirCraft
System.out.println("Orientation for bank, pitch, heading:" + tbE2NED_ori);
// Transform GC Identity to GD to get the World-to-LTSE matrix
OrientationMatrix mW2LTSE_ori = new OrientationMatrix(1,0,0, 0,1,0, 0,0,1); //result target
earthSurfaceReferenceFrame.transformOrientation(gc_coord, mW2LTSE_ori, gd_coord, mW2LTSE_ori);
OrientationMatrix mNED2LTSE_ori = new OrientationMatrix(0,1,0, 1,0,0, 0,0,-1);
// The DIS orientation is the composition of 3 orientations: tbE2NED_ori and mNED2LTSE_ori and mNED2LTP_ori
// Let tbDIS_ori =( tbE2NED_ori o mNED2LTSE_ori o mNED2LTP_ori )
System.out.println("tbE2NED_ori:" + tbE2NED_ori);
System.out.println("tbE2NED_ori:" + tbE2NED_ori);
System.out.println("mW2LTSE_ori:" + mW2LTSE_ori);
//OrientationTaitBryanAngles
// tbDIS_ori = new OrientationTaitBryanAngles(tbE2NED_ori.composeWith(tbE2NED_ori.composeWith(mW2LTSE_ori)).getTaitBryanAngles());
}
catch(SrmException e)
{
System.err.println(e);
}
}
/**
*
* @param localX coordinate
* @param localY coordinate
* @param localZ coordinate
* @param bank bank angle, decimal degrees from level, positive is right bank
* @param noseUp in LTSE: 0 + positive up angle from horizon
* @param bearing heading angle, clockwise from true north is positive
*/
public void change(double localX, double localY, double localZ,
double bank, double noseUp, double bearing)
{
// bearing, in degrees, clockwise from true north is positive
// noseUp, in degrees, angle at which the body is WRT the local flat plane, zero is level, positive is up angle
// bank, degrees from level, positive is right bank
// bearing to LTSE: 360 - bearing - 270 = yaw in LTSE
// noseUp in LTSE: 0 + positive up angle from horizon
// roll in LTSE: -180 + bank angle
try
{
Coord3D localLTSEPosition = localTangentSurfaceReferenceFrame.createCoordinate3D(localZ, localY, localZ);
Coord3D localLTSEOrigin = localTangentSurfaceReferenceFrame.createCoordinate3D(0.0, 0.0, 0.0);
Coord3D disPosition = disCoordinateReferenceFrame.createCoordinate3D();
Coord3D disOrigin = disCoordinateReferenceFrame.createCoordinate3D();
//Coord3D geodeticPosition = earthSurfaceReferenceFrame.createCoordinate3D(latitudeOrigin, longitudeOrigin, altitudeOrigin);
// changes the contents of disPosition to reflect the geocentric coordinates of the LTSE position
SRM_Coordinate_Valid_Region_Code region = disCoordinateReferenceFrame.changeCoordinateSRF(localLTSEPosition, disPosition);
disCoordinateReferenceFrame.changeCoordinateSRF(localLTSEOrigin, disOrigin);
//double values[] = disCoordinateReferenceFrame.getCoordinate3DValues(disPosition);
System.out.println("DIS position, should be equal to DIS coords for LTSE orig:" + disPosition); System.out.println("ORigin of LTSE in DIS coordinates:" + disOrigin);
//disCoordinateReferenceFrame.changeCoordinateSRF(geodeticPosition, disPosition);
//disCoordinateReferenceFrame.getCoordinate3DValues(disPosition);
//System.out.println("Geodetic position:" + geodeticPosition);
OrientationTaitBryanAngles taitBryanOrientation =
new OrientationTaitBryanAngles (Math.toRadians(-180.0 + bank),
Math.toRadians(noseUp),
Math.toRadians(360.0 - bearing -270.0));
System.out.println("tb orientation:" + taitBryanOrientation);
// Will hold output, the orientation in DIS reference frame
OrientationTaitBryanAngles taitBryanDis = new OrientationTaitBryanAngles();
localTangentSurfaceReferenceFrame.transformOrientation(disOrigin, // position of object in DIS
taitBryanOrientation, // Orientation of body, wrt TB->LTSE
localLTSEPosition, // Position, in LTSE coordinates
taitBryanDis); // output: the orientation in DIS ref frame
System.out.println("orientation in DIS:" + taitBryanDis.toString());
}
catch(SrmException e)
{
System.err.println(e);
}
}
/** Command-line invocation (CLI)
* @param args command-line arguments
*/
public static void main(String args[])
{
// x-axis intercept: prime meridian, equator, and zero altitude.
RangeCoordinates primeMeridian = new RangeCoordinates(0.0, 0.0, 0.0);
primeMeridian.DISCoordFromLocalFlat(0.0, 0.0, 0.0);
primeMeridian.c(0.0, 0.0, 0.0, // lat lon alt
0.0, // bank angle
0.0, // noseup angle
180.0); //bearing
// North pole: z-axis intercept with earth surface
RangeCoordinates northPole = new RangeCoordinates(0.0, 180.0, 0.0); // north pole
northPole.DISCoordFromLocalFlat(0.0, 0.0, 0.0);
// y-axis: equator, 90 degrees east. x and z should be near-zero
RangeCoordinates yAxis = new RangeCoordinates(90.0, 0.0, 0.0); // y axis
yAxis.DISCoordFromLocalFlat(0.0, 0.0, 0.0);
// Move west a bit from the equator/prime meridian
RangeCoordinates westALittle = new RangeCoordinates(0.0, -1.0, 0.0);
westALittle.DISCoordFromLocalFlat(0.0, 0.0, 0.0);
}
/**
* Get Spatial Reference Frame (SRF) for this platform at the given coordinate
* @param rangePositionCoordinates local coordinates for location of interest
* @return Spatial Reference Frame (SRF) for this platform
*/
public SRF_LococentricEuclidean3D getPlatformReferenceFrame(Vector3Double rangePositionCoordinates)
{
try
{
// The x,y,z location of the platform in range coordinates (ie, the LTSE).
// TODO unused??
Coord3D lococenter = localTangentSurfaceReferenceFrame.createCoordinate3D(rangePositionCoordinates.getX(),
rangePositionCoordinates.getY(),
rangePositionCoordinates.getZ());
// We also need two unit vectors to describe the orientation of the platform in the LTSE
// primary axis direction--x, along the long axis of the platform
double[] orientationX = new double[3];
Direction xAxis = localTangentSurfaceReferenceFrame.createDirection(localEuclidianOrigin, orientationX);
}
catch(SrmException e)
{
System.err.println(e);
}
return null; // TODO compute and return correct value
}
}