Cxc 91 update pose transform lib (#227)

<!-- Thanks for the contribution, this is awesome. -->

# PR Details
This PR adds pose transformation in mosaic using Proj. During testing it
was found that the map transform calculated by mosaic on the data
subscribed to from Traci, does not have geodetic coordinates that match
the values calculated by carma-platform.
In order to fix this, the transform library in mosaic was extended to
accept another parameter for the georeference string which can be used
to calculate the geodetic coordinates like it is being calculated in
carma-platform and sumo natively (Using proj).

The added georeference field is an optional argument to the mosaic
scenario_configuration.
## Description

<!--- Describe your changes in detail -->

## Related GitHub Issue

<!--- This project only accepts pull requests related to open GitHub
issues or Jira Keys -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please DO NOT name partially fixed issues, instead open an issue
specific to this fix -->
<!--- Please link to the issue here: -->

## Related Jira Key

<!-- e.g. CAR-123 -->
https://usdot-carma.atlassian.net/browse/CXC-91

## Motivation and Context

<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?

<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->
Integration tested on xil_carma_cloud scenario on develop and vru
scenario on carma-system-4.5.0.

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [ ] Defect fix (non-breaking change that fixes an issue)
- [x] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [ ] My change requires a change to the documentation.
- [ ] I have updated the documentation accordingly.
- [x] I have read the
[**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md)
document.
- [x] I have added tests to cover my changes.
- [x] All new and existing tests passed.

co-simulation/bundle/src/assembly/resources/scenarios/Town04/scenario_config.json

@@ -21,7 +21,8 @@
             "csNet": "10.4.0.0",
             "serverNet": "10.5.0.0",
             "tmcNet": "10.6.0.0"
-        }
+        },
+        "georeference":"+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +no_defs"
     },
     "federates": {
         "application": true,

co-simulation/legal/templates/leidos-license-header-2024.txt

@@ -0,0 +1,13 @@
+Copyright (C) 2024 LEIDOS.
+
+Licensed under the Apache License, Version 2.0 (the "License"); you may not
+use this file except in compliance with the License. You may obtain a copy of
+the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+License for the specific language governing permissions and limitations under
+the License.

co-simulation/lib/mosaic-geomath/pom.xml

@@ -18,6 +18,14 @@
             <groupId>com.google.code.gson</groupId>
             <artifactId>gson</artifactId>
         </dependency>
+        <dependency>
+            <groupId>org.locationtech.proj4j</groupId>
+            <artifactId>proj4j</artifactId>
+        </dependency>
+        <dependency>
+            <groupId>org.locationtech.proj4j</groupId>
+            <artifactId>proj4j-epsg</artifactId>
+        </dependency>
     </dependencies>
 
     <build>
@@ -33,6 +41,19 @@
                     </execution>
                 </executions>
             </plugin>
+         <plugin>
+            <groupId>org.apache.maven.plugins</groupId>
+            <artifactId>maven-shade-plugin</artifactId>
+            <version>3.4.1</version>
+            <executions>
+                <execution>
+                    <phase>package</phase>
+                    <goals>
+                        <goal>shade</goal>
+                    </goals>
+                </execution>
+            </executions>
+            </plugin>
         </plugins>
     </build>
 

co-simulation/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/transform/Proj4Projection.java

@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) 2024 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+ package org.eclipse.mosaic.lib.transform;
+
+import static java.lang.Math.toDegrees;
+import static java.lang.Math.toRadians;
+
+
+import org.eclipse.mosaic.lib.geo.CartesianPoint;
+import org.eclipse.mosaic.lib.geo.GeoPoint;
+import org.eclipse.mosaic.lib.geo.MutableCartesianPoint;
+import org.eclipse.mosaic.lib.geo.MutableGeoPoint;
+import org.eclipse.mosaic.lib.geo.MutableUtmPoint;
+import org.eclipse.mosaic.lib.geo.UtmPoint;
+import org.eclipse.mosaic.lib.geo.UtmZone;
+import org.eclipse.mosaic.lib.math.MathUtils;
+import org.eclipse.mosaic.lib.math.Vector3d;
+
+
+import org.locationtech.proj4j.CRSFactory;
+import org.locationtech.proj4j.CoordinateReferenceSystem;
+import org.locationtech.proj4j.CoordinateTransform;
+import org.locationtech.proj4j.CoordinateTransformFactory;
+import org.locationtech.proj4j.ProjCoordinate;
+
+
+
+public class Proj4Projection extends GeoProjection {
+
+    // private final String georeference;
+    private GeoPoint geoOrigin;
+    UtmZone zone;
+
+    private double x_offset;
+    private double y_offset;
+
+    private String wgs84ReferenceString;
+    private String cartesianReferenceString;
+    private CoordinateReferenceSystem wgs84CRS;
+    private CoordinateReferenceSystem cartesianCRS;
+    private CoordinateTransformFactory ctFactory;
+
+
+    public Proj4Projection(GeoPoint origin, double x_offset, double y_offset, String georef){
+        this.geoOrigin = origin;
+        this.x_offset = x_offset;
+        this.y_offset = y_offset;
+
+        // The proj library being used currently doesn't support use of vertical keywords
+        //such as geodgrids and vunits, resulting in no vertical accuracy in the transforms calculated
+        //Isssue tracked under: https://github.com/locationtech/proj4j/issues/20
+        //Remove unaccepted parameters from georeference
+        String regex = "(\\+geoidgrids=[^\\s]+\\s?)|(\\+vunits=[^\\s]+\\s?)";
+
+        this.cartesianReferenceString = georef.replaceAll(regex, "").trim();;
+        this.wgs84ReferenceString = "EPSG:4326";
+
+        CRSFactory crsFactory = new CRSFactory();
+        this.cartesianCRS = crsFactory.createFromParameters("custom_proj", this.cartesianReferenceString);
+        this.wgs84CRS = crsFactory.createFromName(wgs84ReferenceString);
+
+        this.ctFactory = new CoordinateTransformFactory();
+
+        setGeoCalculator(new UtmGeoCalculator(this));
+    }
+
+    @Override
+    public Vector3d geographicToVector(GeoPoint geographic, Vector3d result){
+        getGeoCalculator().distanceBetween(geoOrigin, geographic, result);
+        return result;
+    }
+
+    @Override
+    public MutableGeoPoint vectorToGeographic(Vector3d vector3d, MutableGeoPoint result) {
+        getGeoCalculator().pointFromDirection(geoOrigin, vector3d, result);
+        return result;
+    }
+
+    @Override
+    public MutableCartesianPoint geographicToCartesian(GeoPoint geographic, MutableCartesianPoint result) {
+
+        ProjCoordinate sourceCoord = new ProjCoordinate(geographic.getLongitude(),geographic.getLatitude());
+        ProjCoordinate targetCoord = new ProjCoordinate();
+        CoordinateTransform transform = this.ctFactory.createTransform(wgs84CRS, cartesianCRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        result.set(targetCoord.x, targetCoord.y, 0.0);
+
+        return result;
+    }
+
+    @Override
+    public MutableGeoPoint cartesianToGeographic(CartesianPoint cartesian, MutableGeoPoint result) {
+
+        ProjCoordinate sourceCoord = new ProjCoordinate(cartesian.getX() - this.x_offset, cartesian.getY() - this.y_offset);
+        ProjCoordinate targetCoord = new ProjCoordinate();
+
+        CoordinateTransform transform = this.ctFactory.createTransform(cartesianCRS, wgs84CRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        //Transform returned as lat,lon = (targetCoord.y, targetCoord.x)
+        result.set(targetCoord.y, targetCoord.x, 0.0);
+
+        return result;
+    }
+
+    @Override
+    public Vector3d utmToVector(UtmPoint utm, Vector3d result) {
+        return geographicToVector(utmToGeographic(utm));
+    }
+
+    @Override
+    public MutableUtmPoint vectorToUtm(Vector3d vector, MutableUtmPoint result) {
+        return geographicToUtm(vectorToGeographic(vector), result);
+    }
+
+    @Override
+    public MutableUtmPoint geographicToUtm(GeoPoint geoPoint, MutableUtmPoint result) {
+
+        ProjCoordinate sourceCoord = new ProjCoordinate(geoPoint.getLongitude(),geoPoint.getLatitude());
+        ProjCoordinate targetCoord = new ProjCoordinate();
+
+        //UTMCRS created dynamically since it depends on the zone
+        zone = getUTMZone(geoPoint);
+        String utm_proj_str = "+proj=utm +zone=" + zone.number;
+        CRSFactory crsFactory = new CRSFactory();
+        CoordinateReferenceSystem utmCRS = crsFactory.createFromParameters("custom_proj", utm_proj_str);
+
+        CoordinateTransform transform = this.ctFactory.createTransform(wgs84CRS, utmCRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        result.set(targetCoord.x, targetCoord.y, 0.0, zone);
+
+        return result;
+    }
+
+    @Override
+    public MutableGeoPoint utmToGeographic(UtmPoint utmPoint, MutableGeoPoint result) {
+
+        ProjCoordinate sourceCoord = new ProjCoordinate(utmPoint.getEasting(), utmPoint.getNorthing());
+        ProjCoordinate targetCoord = new ProjCoordinate();
+
+        //UTMCRS created dynamically since it depends on the zone
+        String utm_proj_str = "+proj=utm +zone=" + utmPoint.getZone().number;
+        CRSFactory crsFactory = new CRSFactory();
+        CoordinateReferenceSystem utmCRS = crsFactory.createFromParameters("custom_proj", utm_proj_str);
+
+        CoordinateTransform transform = this.ctFactory.createTransform(utmCRS, wgs84CRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        //Transform returned as lon,lat = (targetCoord.x, targetCoord.y)
+        result.set(targetCoord.y, targetCoord.x, 0.0);
+
+        return result;
+    }
+
+    public UtmZone getUTMZone(GeoPoint geoPoint){
+        int zoneNumber;
+
+        double longTemp = (geoPoint.getLongitude() + 180) - (int) ((geoPoint.getLongitude() + 180) / 360) * 360 - 180;
+
+        zoneNumber = (int) ((longTemp + 180) / 6) + 1;
+
+        if (geoPoint.getLatitude() >= 56.0 && geoPoint.getLatitude() < 64.0 && longTemp >= 3.0 && longTemp < 12.0) {
+            zoneNumber = 32;
+        }
+
+        // Special zones for Svalbard
+        if (geoPoint.getLatitude() >= 72.0 && geoPoint.getLatitude() < 84.0) {
+            if (longTemp >= 0.0 && longTemp < 9.0) {
+                zoneNumber = 31;
+            } else if (longTemp >= 9.0 && longTemp < 21.0) {
+                zoneNumber = 33;
+            } else if (longTemp >= 21.0 && longTemp < 33.0) {
+                zoneNumber = 35;
+            } else if (longTemp >= 33.0 && longTemp < 42.0) {
+                zoneNumber = 37;
+            }
+        }
+
+        return UtmZone.from(zoneNumber, UtmZone.getLetter(zoneNumber, geoPoint.getLatitude()));
+    }
+}

co-simulation/lib/mosaic-geomath/src/test/java/org/eclipse/mosaic/lib/transform/Proj4ProjectionTest.java

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2024 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+package org.eclipse.mosaic.lib.transform;
+
+import static org.junit.Assert.assertEquals;
+
+import org.eclipse.mosaic.lib.geo.CartesianPoint;
+import org.eclipse.mosaic.lib.geo.GeoPoint;
+import org.eclipse.mosaic.lib.geo.MutableGeoPoint;
+import org.eclipse.mosaic.lib.geo.MutableCartesianPoint;
+import org.eclipse.mosaic.lib.geo.MutableUtmPoint;
+import org.eclipse.mosaic.lib.geo.UtmPoint;
+import org.eclipse.mosaic.lib.geo.UtmZone;
+import org.eclipse.mosaic.lib.math.Vector3d;
+
+import org.junit.Test;
+
+
+public class Proj4ProjectionTest {
+    @Test
+    public void convertCartesianToGeographic() {
+
+        String georeference = " +proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs";
+
+        MutableCartesianPoint cartesianOffset = new MutableCartesianPoint(200.00, 300.00, 0);
+        GeoProjection transform = new Proj4Projection(GeoPoint.latLon(0.0, 0.0), cartesianOffset.getX(), cartesianOffset.getY(), georeference);
+
+        MutableCartesianPoint testCartesianPoint = new MutableCartesianPoint(400.00, 600.00, 0);
+        GeoPoint actualGeoPoint = transform.cartesianToGeographic(testCartesianPoint);
+
+        assertEquals(actualGeoPoint.getLatitude(), 0.0027131084297879367, 0.0001d);
+        assertEquals(actualGeoPoint.getLongitude(), 0.0017966305699434167, 0.0001d);
+        assertEquals(actualGeoPoint.getAltitude(), 0.0, 0.0001d);
+
+    }
+
+    @Test
+    public void convertGeographicToCartesian(){
+        String georeference = " +proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs";
+        GeoProjection transform = new Proj4Projection(GeoPoint.latLon(0.0, 0.0), 0.0,0.0, georeference);
+
+        GeoPoint testGeoPoint = GeoPoint.latLon(0.000,0.000);
+
+        CartesianPoint actualCartesian = transform.geographicToCartesian(testGeoPoint);
+        assertEquals(actualCartesian.getX(), 0.0, 1d);
+        assertEquals(actualCartesian.getY(), 0.0, 1d);
+        assertEquals(actualCartesian.getZ(), 0.0, 1d);
+
+    }
+
+
+    @Test
+    public void convertGeographictoUTM(){
+        String georeference = " +proj=tmerc +lat_0=42.30059341574939 +lon_0=-83.69928318881136 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs";
+
+        GeoProjection transform = new Proj4Projection(GeoPoint.latLon(42.30059341574939, -83.69928318881136), 0.0, 0.0, georeference);
+
+        GeoPoint testGeoPoint = GeoPoint.latLon(38.9548994, -77.1481211);
+        UtmZone zone = UtmZone.from(18,'n');
+        UtmPoint actualUtmPoint = new MutableUtmPoint(313863.1656767028, 4313966.065972516, 0.0, zone);
+
+
+        UtmPoint calculatedUtmPoint = transform.geographicToUtm(testGeoPoint);
+        assertEquals(actualUtmPoint.getEasting(), calculatedUtmPoint.getEasting(), 1d);
+        assertEquals(actualUtmPoint.getNorthing(), calculatedUtmPoint.getNorthing(), 1d);
+        assertEquals(actualUtmPoint.getAltitude(), calculatedUtmPoint.getAltitude(), 1d);
+        assertEquals(18, calculatedUtmPoint.getZone().number);
+
+        //Test Special Zone Norway
+        GeoPoint testGeoPoint2 = GeoPoint.latLon(77.8750, 20.9752);
+        UtmPoint calculatedUtmPoint2 = transform.geographicToUtm(testGeoPoint2);
+        assertEquals(33, calculatedUtmPoint2.getZone().number);
+
+        //Test convert back to geographic
+        GeoPoint calculatedGeoPoint = transform.utmToGeographic(calculatedUtmPoint);
+        assertEquals(calculatedGeoPoint.getLatitude(), testGeoPoint.getLatitude(), 0.0001d);
+        assertEquals(calculatedGeoPoint.getLongitude(),testGeoPoint.getLongitude(), 0.0001d);
+    }
+
+    @Test
+    public void geo_utm_vector_conversion() {
+        String georeference = " +proj=tmerc +lat_0=0.0 +lon_0=0.0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs";
+        GeoProjection transform = new Proj4Projection(GeoPoint.latLon(0.0, 0.0), 0.0, 0.0, georeference);
+        GeoPoint origin = GeoPoint.latLon(0.0, 0.0);
+
+
+        for (int i = 0; i < 3; i++) {
+            double x = Math.cos(i * Math.PI / 5) * 0.1;
+            double y = Math.sin(i * Math.PI / 5) * 0.1;
+
+            GeoPoint tstGeoPt = GeoPoint.latLon(origin.getLatitude() + y, origin.getLongitude() + x);
+
+            UtmPoint geoToUtm = transform.geographicToUtm(tstGeoPt);
+            Vector3d geoToVec = transform.geographicToVector(tstGeoPt);
+
+            Vector3d utmToVec = transform.utmToVector(geoToUtm);
+            GeoPoint utmToGeo = transform.utmToGeographic(geoToUtm);
+
+            GeoPoint vecToGeo = transform.vectorToGeographic(geoToVec);
+            UtmPoint vecToUtm = transform.vectorToUtm(geoToVec);
+
+            assertEquals(geoToVec.x, utmToVec.x, 0.01);
+            assertEquals(geoToVec.y, utmToVec.y, 0.01);
+            assertEquals(geoToVec.z, utmToVec.z, 0.01);
+
+            assertEquals(geoToUtm.getNorthing(), vecToUtm.getNorthing(), 0.01);
+            assertEquals(geoToUtm.getEasting(), vecToUtm.getEasting(), 0.01);
+            assertEquals(geoToUtm.getAltitude(), vecToUtm.getAltitude(), 0.01);
+
+            assertEquals(utmToGeo.getLatitude(), vecToGeo.getLatitude(), 0.01);
+            assertEquals(utmToGeo.getLongitude(), vecToGeo.getLongitude(), 0.01);
+            assertEquals(utmToGeo.getAltitude(), vecToGeo.getAltitude(), 0.01);
+        }
+
+    }
+
+}