make_local_oblique_3d.java

public class LocationDef {

    /** A "true" coordinate reference system for absolute positions on Earth. 
     * 
     * User-supplied relativeTo origin coordinates are converted into this 
     * system in the course of building a local coordinate system.  It is not
     * necessarily used again.
     */
    protected static final GeographicCRS referenceAbsolute2D = DefaultGeographicCRS.WGS84;
    protected static final GeographicCRS referenceAbsolute3D = DefaultGeographicCRS.WGS84_3D;
    
    /** This is the coordinate system used for all internal representations of 
     * object locations.
     * 
     * It will always be 3D Cartesian (X,Y,Z). At least for now, any positions 
     * supplied by experiment configurations or through movement APIs are 
     * implicitly treated as being in this CRS.
     * 
     * This should either be an abstract CRS (position relative to some 
     * arbitrary origin), which has previously been the case, or an 
     * appropriately-chosen projected CRS for the region of the Earth around
     * the user-specified absolute origin. Note that the Z/UP dimension does
     * <b>not</b> necessarily correspond to height above ground at any given 
     * point, because the ground is not necessarily flat, and flat is actually 
     * a very subtle concept on a squiggly Earth.
     */
    protected CoordinateReferenceSystem internalCRS;   //Gets set by the constructor
    
}

// These are all filled in from user-supplied values
String crs_name;
double origin_E;
double origin_N;
double origin_U;
log.debug("Relative origin at: " + crs_name + ": " + origin_E + "E, " + origin_N + "N, " + origin_U + "UP");
CoordinateReferenceSystem sourceCRS;


sourceCRS = CRS.decode(crs_name);
log.debug("Origin point (relativeTo) CRS is: " + sourceCRS.getName());
log.trace("Origin point (relativeTo) CRS is: " + sourceCRS.toString());
int source_d = sourceCRS.getCoordinateSystem().getDimension();
log.debug("That's a " + source_d + "D CRS.");
MathTransform map_to_abs = CRS.findMathTransform(sourceCRS, LocationDef.referenceAbsolute2D, false);
MathTransform abs_to_map = CRS.findMathTransform(LocationDef.referenceAbsolute2D, sourceCRS, false);
log.debug("Found transform from origin point CRS to absolute.");
log.trace("Found transform from origin point CRS to absolute: " + map_to_abs.toString());
GeneralDirectPosition origin = null;
if (source_d == 2) {
    origin = new GeneralDirectPosition(origin_E,origin_N);
} else {
    origin = new GeneralDirectPosition(origin_E, origin_N, origin_U);
}
origin.setCoordinateReferenceSystem(sourceCRS);
GeneralDirectPosition origin_prime = new GeneralDirectPosition(2);
origin_prime.setCoordinateReferenceSystem(LocationDef.referenceAbsolute2D);
map_to_abs.transform(origin, origin_prime);
log.trace("Converted " + origin +  " to " + origin_prime + " in WGS84");
log.trace("Inverse is: " + abs_to_map.transform(origin_prime, null));
final Ellipsoid ellipsoid = LocationDef.referenceAbsolute2D.getDatum().getEllipsoid();
final DefaultMathTransformFactory factory = new DefaultMathTransformFactory();
final ParameterValueGroup parameters;
parameters = factory.getDefaultParameters("Oblique_Mercator");
log.trace("Default parameters for Oblique Mercator projection are: " + parameters.toString());
parameters.parameter("semi_major").setValue(ellipsoid.getSemiMajorAxis());
parameters.parameter("semi_minor").setValue(ellipsoid.getSemiMinorAxis());
parameters.parameter("longitude_of_center").setValue(origin_prime.getCoordinate()[0]);
parameters.parameter("latitude_of_center").setValue(origin_prime.getCoordinate()[1]);
log.trace("Localized parameters for this origin (and WGS84) are: " + parameters.toString());
final MathTransform mt;
mt = factory.createParameterizedTransform(parameters);
ProjectedCRS local2D; // Projected (flat) coordinate system around origin point.
local2D = new DefaultProjectedCRS("Localized Oblique Mercator", LocationDef.referenceAbsolute2D, mt, DefaultCartesianCS.PROJE
log.trace("Local projected 2D coordinate system: " + local2D.toString());
CompoundCRS local2D_plus_Z = new DefaultCompoundCRS("LOM + 1D", new CoordinateReferenceSystem[] { local2D, DefaultVerticalCRS
log.trace("Local projected 2D with separate vertical: " + local2D_plus_Z.toString());
CoordinateReferenceSystem local3D;
ReferencingFactoryContainer rfc;
rfc = new ReferencingFactoryContainer(null);
local3D= rfc.toGeodetic3D(local2D_plus_Z);
log.debug("Geodetic 3D CRS: " + local3D);
//LinearTransform heightAdjustment = ProjectiveTransform.createTranslation(2, origin_U);
CoordinateReferenceSystem offsetLocal3D;
Matrix heightAdjustment = IdentityTransform.create(3).getMatrix();
heightAdjustment.setElement(2, 3, -origin_U); //Translate Z dimension (I think)
LinearTransform ha = ProjectiveTransform.create(heightAdjustment);
offsetLocal3D = new DefaultDerivedCRS("3D with offset", local3D, ha, local3D.getCoordinateSystem());
log.debug("Offset Geodetic 3D CRS: " + offsetLocal3D);
log.debug("Coordinate system: " + offsetLocal3D.getCoordinateSystem());
//MathTransform abs_to_local = local2D.getConversionFromBase().getMathTransform();
MathTransform abs_to_local = CRS.findMathTransform(LocationDef.referenceAbsolute3D, offsetLocal3D, false);
MathTransform local_to_abs = abs_to_local.inverse();
MathTransform local_to_source = CRS.findMathTransform(offsetLocal3D, sourceCRS, false);
    
//Extend origin_prime with the provided Z offset:
GeneralDirectPosition origin_prime_prime = new GeneralDirectPosition(3);
origin_prime_prime.setOrdinate(0, origin_prime.getOrdinate(0));
origin_prime_prime.setOrdinate(1, origin_prime.getOrdinate(1));
origin_prime_prime.setOrdinate(2, origin_U);
    
GeneralDirectPosition origin_local = new GeneralDirectPosition(3);
abs_to_local.transform(origin_prime_prime, origin_local);
log.debug("Origin point has local coordinates (should be epsilon-close to 0,0,0): " + origin_local);
GeneralDirectPosition origin_source = new GeneralDirectPosition(2);
local_to_source.transform(origin_local, origin_source);
log.debug("Origin point has source coordinates (should be very close to config file inputs): " + origin_source);
        
return new LocationDef(offsetLocal3D);