Skip to content
Snippets Groups Projects
Commit 20d273a7 authored by Brutzman, Don's avatar Brutzman, Don
Browse files

accept Terry's changes

parent ab797725
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/edu/nps/moves/spatial/RangeCoordinates.java 0 → 100644
+ 483
− 0
View file @ 20d273a7
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;
/**
* 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>
*
* 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>
*
* There are several major reference frames that may be useful in various contexts:<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 deg east lon, and Z up through
* the north pole. This is the coordinate system used by DIS world coordinates.<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>
*
* 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>
*
* Body Centric/Lococentric/Platform-centric: The origin is at the volumentric 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>
*
* We can also convert between these coordinate systems using standard libraries
* in the SRM. <p>
*
* See User’s Manual for SRM Orientation, Velocity, & 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 deg 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;
/**
* 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.<p>
*
* @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
*/
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
*/
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
* @param y
* @param z
* @return
*/
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;
}
/**
* Converts a roll, pitch, 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>
*
* phi = roll, theta = pitch, psi = yaw/heading<p>, by one popular convention.
* All units are in radians.<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
* @param localPosition
* @return
*/
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;
}
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);
}
}
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);
}
}
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 deg 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);
}
public SRF_LococentricEuclidean3D getPlatformReferenceFrame(Vector3Double rangePositionCoordinates)
{
try
{
// The x,y,z location of the platform in range coordinates (ie, the LTSE).
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;
}
}
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