Cxc 91 update pose transform lib

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/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,195 @@
+/*
+ * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
+ *
+ * See the NOTICE file(s) distributed with this work for additional
+ * information regarding copyright ownership.
+ *
+ * This program and the accompanying materials are made available under the
+ * terms of the Eclipse Public License 2.0 which is available at
+ * http://www.eclipse.org/legal/epl-2.0
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *
+ * Contact: mosaic@fokus.fraunhofer.de
+ */
+
+ 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;
+    CoordinateReferenceSystem wgs84CRS;
+    CoordinateReferenceSystem cartesianCRS;
+
+
+    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);
+
+    }
+
+    @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();
+        CoordinateTransformFactory ctFactory = new CoordinateTransformFactory();
+        CoordinateTransform transform = 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();
+        CoordinateTransformFactory ctFactory = new CoordinateTransformFactory();
+        CoordinateTransform transform = 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();
+        CoordinateTransformFactory ctFactory = new CoordinateTransformFactory();
+
+        //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 = ctFactory.createTransform(wgs84CRS, utmCRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        result.set(targetCoord.x, targetCoord.y, 0.0, getUTMZone(geoPoint));
+
+        return result;
+    }
+
+    @Override
+    public MutableGeoPoint utmToGeographic(UtmPoint utmPoint, MutableGeoPoint result) {
+
+        ProjCoordinate sourceCoord = new ProjCoordinate(utmPoint.getEasting(), utmPoint.getNorthing());
+        ProjCoordinate targetCoord = new ProjCoordinate();
+        CoordinateTransformFactory ctFactory = new CoordinateTransformFactory();
+
+        //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 = ctFactory.createTransform(utmCRS, wgs84CRS);
+
+        transform.transform(sourceCoord, targetCoord);
+        //Transform returned as lon,lat = (targetCoord.x, targetCoord.y)
+        result.set(targetCoord.x, targetCoord.y, 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,83 @@
+/*
+ * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
+ *
+ * See the NOTICE file(s) distributed with this work for additional
+ * information regarding copyright ownership.
+ *
+ * This program and the accompanying materials are made available under the
+ * terms of the Eclipse Public License 2.0 which is available at
+ * http://www.eclipse.org/legal/epl-2.0
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *
+ * Contact: mosaic@fokus.fraunhofer.de
+ */
+
+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);
+    }
+
+}

co-simulation/pom.xml

@@ -94,7 +94,7 @@
         <sonar.java.coveragePlugin>jacoco</sonar.java.coveragePlugin>
         <sonar.dynamicAnalysis>reuseReports</sonar.dynamicAnalysis>
         <sonar.coverage.jacoco.xmlReportPaths>${project.basedir}/target/site/jacoco/jacoco.xml</sonar.coverage.jacoco.xmlReportPaths>
-        <sonar.language>java</sonar.language>	  
+        <sonar.language>java</sonar.language>
         <!-- JaCoCo Properties -->
         <jacoco.version>0.8.8</jacoco.version>
         <mosaic.version>22.1-SNAPSHOT</mosaic.version>
@@ -152,6 +152,16 @@
 
     <dependencyManagement>
         <dependencies>
+            <dependency>
+                <groupId>org.locationtech.proj4j</groupId>
+                <artifactId>proj4j</artifactId>
+                <version>1.3.0</version>
+            </dependency>
+            <dependency>
+                <groupId>org.locationtech.proj4j</groupId>
+                <artifactId>proj4j-epsg</artifactId>
+                <version>1.3.0</version>
+            </dependency>
             <dependency>
                 <!-- PURPOSE: Logging -->
                 <groupId>ch.qos.logback</groupId>

co-simulation/rti/mosaic-rti-api/src/main/java/org/eclipse/mosaic/rti/config/CProjection.java

@@ -18,6 +18,8 @@
 import org.eclipse.mosaic.lib.geo.CartesianPoint;
 import org.eclipse.mosaic.lib.geo.GeoPoint;
 
+import edu.umd.cs.findbugs.annotations.Nullable;
+
 /**
  * Configuration for the Projection of geographic coordinates to cartesian coordinates.
  */
@@ -32,5 +34,8 @@ public class CProjection {
      * The cartesian offset which is considered when transformation from or to geographic coordinates.
      */
     public CartesianPoint cartesianOffset;
+
+    @Nullable
+    public String georeference;
 }
 

co-simulation/rti/mosaic-starter/src/main/java/org/eclipse/mosaic/starter/MosaicSimulation.java

@@ -15,10 +15,12 @@
 
 package org.eclipse.mosaic.starter;
 
+import org.eclipse.mosaic.lib.geo.MutableGeoPoint;
 import org.eclipse.mosaic.lib.geo.UtmPoint;
 import org.eclipse.mosaic.lib.geo.UtmZone;
 import org.eclipse.mosaic.lib.objects.addressing.IpResolver;
 import org.eclipse.mosaic.lib.transform.GeoProjection;
+import org.eclipse.mosaic.lib.transform.Proj4Projection;
 import org.eclipse.mosaic.lib.transform.UtmGeoCalculator;
 import org.eclipse.mosaic.lib.transform.Wgs84Projection;
 import org.eclipse.mosaic.lib.util.NameGenerator;
@@ -255,6 +257,13 @@ private GeoProjection createTransformation(CScenario scenarioConfiguration) {
             Validate.notNull(projectionConfig.cartesianOffset,
                     "Invalid Wgs84UtmTransform configuration: no cartesian offset given");
 
+            // Check if georeference is specified
+            if (scenarioConfiguration.simulation.georeference != null)
+            {
+                MutableGeoPoint origin = new MutableGeoPoint(projectionConfig.centerCoordinates.getLongitude(), projectionConfig.centerCoordinates.getLongitude(), projectionConfig.centerCoordinates.getAltitude());
+                return new Proj4Projection(origin, projectionConfig.cartesianOffset.getX(), projectionConfig.cartesianOffset.getY(), scenarioConfiguration.simulation.georeference);
+            }
+
             UtmPoint origin = UtmPoint.eastNorth(
                     UtmZone.from(projectionConfig.centerCoordinates),
                     -projectionConfig.cartesianOffset.getX(),