diff --git a/lib/srm-4.4.0.jar b/lib/srm-4.4.0.jar
new file mode 100644
index 0000000000000000000000000000000000000000..0bcfa932d1be8e892ff931bc3dd14bdab58e206c
Binary files /dev/null and b/lib/srm-4.4.0.jar differ
diff --git a/nbproject/build-impl.xml b/nbproject/build-impl.xml
index 707769d3d430406e4ca3f69a82052eb9545dbf83..009869874b47e81daa2285d000491d28332fd562 100644
--- a/nbproject/build-impl.xml
+++ b/nbproject/build-impl.xml
@@ -90,7 +90,7 @@ is divided into following sections:
</not>
</condition>
</fail>
- <j2seproject3:modulename property="module.name" sourcepath="${src.dir}:${src.src-generated.dir}"/>
+ <j2seproject3:modulename property="module.name" sourcepath="${src.dir}"/>
<condition property="named.module.internal">
<and>
<isset property="module.name"/>
@@ -186,7 +186,6 @@ is divided into following sections:
<condition property="have.sources">
<or>
<available file="${src.dir}"/>
- <available file="${src.src-generated.dir}"/>
</or>
</condition>
<condition property="netbeans.home+have.tests">
@@ -290,7 +289,6 @@ is divided into following sections:
</target>
<target depends="-pre-init,-init-private,-init-user,-init-project,-do-init" name="-init-check">
<fail unless="src.dir">Must set src.dir</fail>
- <fail unless="src.src-generated.dir">Must set src.src-generated.dir</fail>
<fail unless="test.src.dir">Must set test.src.dir</fail>
<fail unless="build.dir">Must set build.dir</fail>
<fail unless="dist.dir">Must set dist.dir</fail>
@@ -312,7 +310,7 @@ is divided into following sections:
</target>
<target depends="-init-ap-cmdline-properties,-init-source-module-properties" if="modules.supported.internal" name="-init-macrodef-javac-with-module">
<macrodef name="javac" uri="http://www.netbeans.org/ns/j2se-project/3">
- <attribute default="${src.dir}:${src.src-generated.dir}" name="srcdir"/>
+ <attribute default="${src.dir}" name="srcdir"/>
<attribute default="${build.classes.dir}" name="destdir"/>
<attribute default="${javac.classpath}" name="classpath"/>
<attribute default="${javac.modulepath}" name="modulepath"/>
@@ -324,7 +322,7 @@ is divided into following sections:
<attribute default="${excludes}" name="excludes"/>
<attribute default="${javac.debug}" name="debug"/>
<attribute default="${empty.dir}" name="sourcepath" unless:set="named.module.internal"/>
- <attribute default="${src.dir}:${src.src-generated.dir}" if:set="named.module.internal" name="sourcepath"/>
+ <attribute default="${src.dir}" if:set="named.module.internal" name="sourcepath"/>
<attribute default="${empty.dir}" name="gensrcdir"/>
<element name="customize" optional="true"/>
<sequential>
@@ -379,7 +377,7 @@ is divided into following sections:
</target>
<target depends="-init-ap-cmdline-properties,-init-source-module-properties" if="ap.supported.internal" name="-init-macrodef-javac-with-processors" unless="modules.supported.internal">
<macrodef name="javac" uri="http://www.netbeans.org/ns/j2se-project/3">
- <attribute default="${src.dir}:${src.src-generated.dir}" name="srcdir"/>
+ <attribute default="${src.dir}" name="srcdir"/>
<attribute default="${build.classes.dir}" name="destdir"/>
<attribute default="${javac.classpath}" name="classpath"/>
<attribute default="${javac.modulepath}" name="modulepath"/>
@@ -422,7 +420,7 @@ is divided into following sections:
</target>
<target depends="-init-ap-cmdline-properties,-init-source-module-properties" name="-init-macrodef-javac-without-processors" unless="ap.supported.internal">
<macrodef name="javac" uri="http://www.netbeans.org/ns/j2se-project/3">
- <attribute default="${src.dir}:${src.src-generated.dir}" name="srcdir"/>
+ <attribute default="${src.dir}" name="srcdir"/>
<attribute default="${build.classes.dir}" name="destdir"/>
<attribute default="${javac.classpath}" name="classpath"/>
<attribute default="${javac.modulepath}" name="modulepath"/>
@@ -457,7 +455,7 @@ is divided into following sections:
</target>
<target depends="-init-macrodef-javac-with-module,-init-macrodef-javac-with-processors,-init-macrodef-javac-without-processors" name="-init-macrodef-javac">
<macrodef name="depend" uri="http://www.netbeans.org/ns/j2se-project/3">
- <attribute default="${src.dir}:${src.src-generated.dir}" name="srcdir"/>
+ <attribute default="${src.dir}" name="srcdir"/>
<attribute default="${build.classes.dir}" name="destdir"/>
<attribute default="${javac.classpath}" name="classpath"/>
<sequential>
@@ -1092,13 +1090,12 @@ is divided into following sections:
<include name="*"/>
</dirset>
</pathconvert>
- <j2seproject3:depend srcdir="${src.dir}:${src.src-generated.dir}:${build.generated.subdirs}"/>
+ <j2seproject3:depend srcdir="${src.dir}:${build.generated.subdirs}"/>
</target>
<target depends="init,deps-jar,-pre-pre-compile,-pre-compile, -copy-persistence-xml,-compile-depend" if="have.sources" name="-do-compile">
<j2seproject3:javac gensrcdir="${build.generated.sources.dir}"/>
<copy todir="${build.classes.dir}">
<fileset dir="${src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/>
- <fileset dir="${src.src-generated.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/>
</copy>
</target>
<target if="has.persistence.xml" name="-copy-persistence-xml">
@@ -1119,7 +1116,7 @@ is divided into following sections:
<target depends="init,deps-jar,-pre-pre-compile" name="-do-compile-single">
<fail unless="javac.includes">Must select some files in the IDE or set javac.includes</fail>
<j2seproject3:force-recompile/>
- <j2seproject3:javac excludes="" gensrcdir="${build.generated.sources.dir}" includes="${javac.includes}, module-info.java" sourcepath="${src.dir}:${src.src-generated.dir}"/>
+ <j2seproject3:javac excludes="" gensrcdir="${build.generated.sources.dir}" includes="${javac.includes}, module-info.java" sourcepath="${src.dir}"/>
</target>
<target name="-post-compile-single">
<!-- Empty placeholder for easier customization. -->
@@ -1514,9 +1511,6 @@ is divided into following sections:
<fileset dir="${src.dir}" excludes="${bug5101868workaround},${excludes}" includes="${includes}">
<filename name="**/*.java"/>
</fileset>
- <fileset dir="${src.src-generated.dir}" excludes="${bug5101868workaround},${excludes}" includes="${includes}">
- <filename name="**/*.java"/>
- </fileset>
<fileset dir="${build.generated.sources.dir}" erroronmissingdir="false">
<include name="**/*.java"/>
<exclude name="*.java"/>
@@ -1528,9 +1522,6 @@ is divided into following sections:
<fileset dir="${src.dir}" excludes="${excludes}" includes="${includes}">
<filename name="**/doc-files/**"/>
</fileset>
- <fileset dir="${src.src-generated.dir}" excludes="${excludes}" includes="${includes}">
- <filename name="**/doc-files/**"/>
- </fileset>
<fileset dir="${build.generated.sources.dir}" erroronmissingdir="false">
<include name="**/doc-files/**"/>
</fileset>
diff --git a/nbproject/genfiles.properties b/nbproject/genfiles.properties
new file mode 100644
index 0000000000000000000000000000000000000000..b0c65b6d925f69f38be5b14b10a4d7fac8815d7a
--- /dev/null
+++ b/nbproject/genfiles.properties
@@ -0,0 +1,5 @@
+# This file is used by a NetBeans-based IDE to track changes in generated files such as build-impl.xml.
+# Do not edit this file. You may delete it but then the IDE will never regenerate such files for you.
+nbproject/build-impl.xml.data.CRC32=be778aa9
+nbproject/build-impl.xml.script.CRC32=c34c2285
+nbproject/build-impl.xml.stylesheet.CRC32=f89f7d21@1.95.0.48
diff --git a/nbproject/project.properties b/nbproject/project.properties
index 6048477c9704d4c138c56e84bd535c4f1d0201a7..991f88ea8fd612d0426ded42ae0929313850540b 100644
--- a/nbproject/project.properties
+++ b/nbproject/project.properties
@@ -47,18 +47,18 @@ file.reference.junit-platform-console-1.6.2.jar=testlibs/junit-platform-console-
file.reference.junit-platform-engine-1.6.2.jar=testlibs/junit-platform-engine-1.6.2.jar
file.reference.junit-platform-launcher-1.6.2.jar=testlibs/junit-platform-launcher-1.6.2.jar
file.reference.junit-platform-reporting-1.6.2.jar=testlibs/junit-platform-reporting-1.6.2.jar
-file.reference.open-dis7-java-src=src
-file.reference.open-dis7-java-src-generated=src-generated
+file.reference.open-dis7-enumerations-classes.jar=dist/open-dis7-enumerations-classes.jar
file.reference.opentest4j-1.2.0.jar=testlibs/opentest4j-1.2.0.jar
+file.reference.srm-4.4.0.jar=lib/srm-4.4.0.jar
includes=**
jar.archive.disabled=${jnlp.enabled}
jar.compress=false
jar.index=${jnlp.enabled}
javac.classpath=\
- ${file.reference.open-dis7-java-src}:\
- ${file.reference.open-dis7-java-src-generated}:\
${file.reference.commons-io-2.6.jar}:\
- ${file.reference.guava-28.0-jre.jar}
+ ${file.reference.guava-28.0-jre.jar}:\
+ ${file.reference.open-dis7-enumerations-classes.jar}:\
+ ${file.reference.srm-4.4.0.jar}
# Space-separated list of extra javac options
javac.compilerargs=-Xlint:unchecked
javac.deprecation=true
@@ -70,8 +70,6 @@ javac.processorpath=\
javac.source=1.8
javac.target=1.8
javac.test.classpath=\
- ${file.reference.open-dis7-java-src}:\
- ${file.reference.open-dis7-java-src-generated}:\
${libs.junit_5.classpath}:\
${file.reference.apiguardian-api-1.1.0.jar}:\
${libs.hamcrest.classpath}:\
@@ -131,8 +129,6 @@ run.jvmargs=
run.modulepath=\
${javac.modulepath}
run.test.classpath=\
- ${file.reference.open-dis7-java-src}:\
- ${file.reference.open-dis7-java-src-generated}:\
${file.reference.junit-platform-commons-1.6.2.jar}:\
${file.reference.junit-platform-console-1.6.2.jar}:\
${file.reference.junit-platform-engine-1.6.2.jar}:\
@@ -145,5 +141,4 @@ run.test.modulepath=\
${javac.test.modulepath}
source.encoding=UTF-8
src.dir=src
-src.src-generated.dir=src-generated
test.src.dir=test
diff --git a/nbproject/project.xml b/nbproject/project.xml
index 0394c47dc5938e5494b0483393696c3c6b33d367..4ad60298c11cec54374d6008f53519da444108a9 100644
--- a/nbproject/project.xml
+++ b/nbproject/project.xml
@@ -6,7 +6,6 @@
<name>open-dis7-java</name>
<source-roots>
<root id="src.dir"/>
- <root id="src.src-generated.dir"/>
</source-roots>
<test-roots>
<root id="test.src.dir"/>
diff --git a/src/edu/nps/moves/spatial/EntityBodyCoordinates.java b/src/edu/nps/moves/spatial/EntityBodyCoordinates.java
new file mode 100644
index 0000000000000000000000000000000000000000..c2255026994af65ad150567809f4132bf271ee66
--- /dev/null
+++ b/src/edu/nps/moves/spatial/EntityBodyCoordinates.java
@@ -0,0 +1,72 @@
+package edu.nps.moves.spatial;
+
+import SRM.*;
+
+/**
+ * Local coordinate system for an entity, e.g., relative to one UAV. This
+ * is typically embedded in another coordinate system, such as a range
+ * coordinate system. The origin of the local coordinate system should be
+ * the center of mass of the entity. The u axis points out the front, v axis
+ * out the right side of the entity, and the w axis down. The SRF is specified
+ * via a point (in the reference frame of the parent) and two unit vectors
+ * in the parent SRF, which are parallel to the entity u,v plane. Directions,
+ * orientations, and vector
+ * quantities are independent of the position of the lococenter WRT the
+ * parent SRF; if you're concerned only with directions, orientations, and
+ * vector quantities it doesn't matter where the origin is, so you can
+ * pick someplace handy, like the origin of the parent SRF.
+ *
+ * @author DMcG
+ */
+public class EntityBodyCoordinates
+{
+ /** body-centric coordinate system for the entity */
+ SRF_LococentricEuclidean3D bodySRF;
+
+ /** The coordinate system in which this body-centric coordinate system is embedded */
+ BaseSRF_3D parentSRF;
+
+ /** origin of body-centric coordinate system, in the parent SRF */
+ Coord3D lococenter;
+
+ /** Create a new lococentric Euclidean reference frame embedded in a parent SRF.
+ * The origin of the lococentric coordinate system is specified, along with
+ * two unit vectors, parallel to the u and v axes.
+ * @param parentSRF
+ * @param x
+ * @param y
+ * @param z
+ * @param primaryDirectionX
+ * @param primaryDirectionY
+ * @param primaryDirectionZ
+ * @param secondaryDirectionX
+ * @param secondaryDirectionY
+ * @param secondaryDirectionZ
+ */
+ public EntityBodyCoordinates(BaseSRF_3D parentSRF,
+ float x, float y, float z, // lococenter, in parent SRF
+ float primaryDirectionX, float primaryDirectionY, float primaryDirectionZ, // unit vector parallel to u axis
+ float secondaryDirectionX, float secondaryDirectionY, float secondaryDirectionZ) // unit vector parallel to v axis
+ {
+ try
+ {
+ this.parentSRF = parentSRF;
+ this.lococenter = parentSRF.createCoordinate3D(x, y, z);
+
+ // Unit vector along entity u axis in parent SRF
+ double[] d1 = {primaryDirectionX, primaryDirectionY, primaryDirectionZ};
+ Direction primaryAxisDirection = parentSRF.createDirection(lococenter, d1);
+
+ // Unit vector along v axis in parent SRF
+ double[] d2 = {secondaryDirectionX, secondaryDirectionY, secondaryDirectionZ};
+ Direction secondaryAxisDirection = parentSRF.createDirection(lococenter, d2);
+
+ this.bodySRF = parentSRF.createLococentricEuclidean3DSRF(lococenter, primaryAxisDirection, primaryAxisDirection);
+ }
+ catch(SrmException e)
+ {
+ System.err.print(e);
+ }
+ }
+
+}
diff --git a/src/edu/nps/moves/spatial/RangeCoordinates.java b/src/edu/nps/moves/spatial/RangeCoordinates.java
new file mode 100644
index 0000000000000000000000000000000000000000..3cc0a0e74a988375fd5f436faa4917bd47e5dbab
--- /dev/null
+++ b/src/edu/nps/moves/spatial/RangeCoordinates.java
@@ -0,0 +1,483 @@
+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;
+ }
+}