From 365ab4fc4c9efdaa5ca83af09c21c90d8e30081b Mon Sep 17 00:00:00 2001 From: garciay Date: Wed, 2 Nov 2016 14:02:15 +0000 Subject: [PATCH] Finalise rotation external function --- .../IItsExternalFunctionsProvider.java | 4 +- .../extfunc/ItsExternalFunctionsProvider.java | 54 ++++++++++++------- .../etsi/its/tool/elvior/PluginAdapter.java | 4 +- .../ExternalFunctionsPluginProvider.java | 4 +- 4 files changed, 41 insertions(+), 25 deletions(-) diff --git a/javasrc/extfunc/org/etsi/its/extfunc/IItsExternalFunctionsProvider.java b/javasrc/extfunc/org/etsi/its/extfunc/IItsExternalFunctionsProvider.java index b033e3e68..de5439b97 100644 --- a/javasrc/extfunc/org/etsi/its/extfunc/IItsExternalFunctionsProvider.java +++ b/javasrc/extfunc/org/etsi/its/extfunc/IItsExternalFunctionsProvider.java @@ -80,9 +80,9 @@ public interface IItsExternalFunctionsProvider { * @param p_longitude Computed position's longitude * * TTCN-3 signature: - * external function fx_computePositionFromRotation(in UInt32 p_refLatitude, in Uint32 p_refLongitude, in Uint32 p_cenLatitude, in Uint32 p_cenLongitude, in float p_rotation, out UInt32 p_latitude, out UInt32 p_longitude); + * external function fx_computePositionFromRotation(in UInt32 p_refLatitude, in Uint32 p_refLongitude, in Uint32 p_cenLatitude, in Uint32 p_cenLongitude, in Int32 p_rotation, out UInt32 p_latitude, out UInt32 p_longitude); */ - public void fx_computePositionFromRotation(final IntegerValue p_refLatitude, final IntegerValue p_refLongitude, final IntegerValue p_cenLatitude, final IntegerValue p_cenLongitude, final FloatValue p_rotation, IntegerValue p_latitude, IntegerValue p_longitude); + public void fx_computePositionFromRotation(final IntegerValue p_refLatitude, final IntegerValue p_refLongitude, final IntegerValue p_cenLatitude, final IntegerValue p_cenLongitude, final IntegerValue p_rotation, IntegerValue p_latitude, IntegerValue p_longitude); /** * External function to compute radius of a given circular area diff --git a/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java b/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java index 5dc04f4bf..7d9454286 100644 --- a/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java +++ b/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java @@ -280,8 +280,9 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * * TTCN-3 signature: external function * fx_computePositionFromRotation(in LongPosVector - * p_iutLongPosVector, in float - * p_rotation, out UInt32 p_latitude, out UInt32 p_longitude); + * p_iutLongPosVector, in Int32 + * p_rotation, out Int32 p_latitude, out Int32 p_longitude); + * @remark See http://www.movable-type.co.uk/scripts/latlong.html */ @Override public synchronized void fx_computePositionFromRotation( @@ -289,32 +290,47 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid final IntegerValue p_refLongitude, final IntegerValue p_cenLatitude, final IntegerValue p_cenLongitude, - final FloatValue p_rotation, + final IntegerValue p_rotation, IntegerValue p_latitude, IntegerValue p_longitude) { - // //TERFactory.getInstance().logDebug( + //TERFactory.getInstance().logDebug( // "ItsExternalFunctionsProvider", // "fx_computePositionFromRotation", // String.format("%d", p_rotation.getFloat())); - // Get rotation angle - double rotation = Math.toRadians(p_rotation.getFloat()); - - // X = longitude, Y = latitude - // I assume this is a very small geographic area ==> 2D plan formula applied - double x = _tcicdWrapper.getInteger(p_refLongitude); - double y = _tcicdWrapper.getInteger(p_refLatitude); - double xcen = _tcicdWrapper.getInteger(p_cenLongitude); - double ycen = _tcicdWrapper.getInteger(p_cenLatitude); - double X = x - xcen; - double Y = y - ycen; - double long_ = xcen + Math.cos(rotation) * X - Math.sin(rotation) * Y; - double lat = ycen + Math.sin(rotation) * X + Math.cos(rotation) * Y; + // 1. Compute distance between the 2 points + double lat1 = Math.toRadians((double)p_cenLatitude.getInteger() / 10000000.0); + double long1 = Math.toRadians((double)p_cenLongitude.getInteger() / 10000000.0); + double lat2 = Math.toRadians((double)p_refLatitude.getInteger() / 10000000.0); + double long2 = Math.toRadians((double)p_refLongitude.getInteger() / 10000000.0); + double dlat = lat2 - lat1; + double dlong = long2 - long1; + double a = Math.sin(dlat / 2) * Math.sin(dlat / 2) + Math.cos(lat1) * Math.cos(lat2) * Math.sin(dlong / 2) * Math.sin(dlong / 2); + double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a)); + //double d = earthRadius * c; + //TERFactory.getInstance().logDebug("fx_computePositionFromRotation: Angular distance = " + c); + + // 2. Compute bearing + double y = Math.sin(long2 - long1) * Math.cos(lat2); + double x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(long2 - long1); + double brng = Math.atan2(y, x); + //TERFactory.getInstance().logDebug("fx_computePositionFromRotation: Bearing = " + Math.toDegrees(brng)); + + // 3. Compute distance between the 2 points + double rotation = Math.toRadians((double)p_rotation.getInteger() / 10.0) + brng; + //TERFactory.getInstance().logDebug("fx_computePositionFromRotation = rotation: " + Math.toDegrees(rotation)); + double lat = Math.asin(Math.sin(lat1) * Math.cos(c) + Math.cos(lat1) * Math.sin(c) * Math.cos(rotation)); + double long_ = long1 + Math.atan2(Math.sin(rotation) * Math.sin(c)*Math.cos(lat1), Math.cos(c) - Math.sin(lat1) * Math.sin(lat)); + + // 4. Normalize + // normalise to -180...+180 + long_ = (long_ + 3 * Math.PI) % (2 * Math.PI) - Math.PI; // The out parameter needs to be set on the object level - Long rlat = Math.round(lat); - Long rlong = Math.round(long_); + Long rlat = Math.round(Math.toDegrees(lat) * 10000000); + Long rlong = Math.round(Math.toDegrees(long_) * 10000000); _tcicdWrapper.setInteger(p_latitude, rlat.intValue()); _tcicdWrapper.setInteger(p_longitude, rlong.intValue()); + //TERFactory.getInstance().logDebug("Lat = " + p_latitude.getInteger() + " - long = " + p_longitude.getInteger()); } // End of method fx_computePositionFromRotation @Override diff --git a/javasrc/tool/org/etsi/its/tool/elvior/PluginAdapter.java b/javasrc/tool/org/etsi/its/tool/elvior/PluginAdapter.java index 88ff19277..3c4a2f6c0 100644 --- a/javasrc/tool/org/etsi/its/tool/elvior/PluginAdapter.java +++ b/javasrc/tool/org/etsi/its/tool/elvior/PluginAdapter.java @@ -648,8 +648,8 @@ public class PluginAdapter implements TriCommunicationSA, TriCommunicationTE, xT _logger.info(String.format("Setting parameter %s", parameterList.get(4).getParameterName())); value = parameterList.get(4).getEncodedParameter(); - FloatValue rotation = (FloatValue)TciProvider.getInstance().getTciCDRequired().getFloat().newInstance(); - rotation.setFloat(ByteHelper.byteArrayToInt(value)); // FIXME Should be byteArrayToFloat??? + IntegerValue rotation = (IntegerValue)TciProvider.getInstance().getTciCDRequired().getInteger().newInstance(); + rotation.setInteger(ByteHelper.byteArrayToInt(value)); IntegerValue latitude = (IntegerValue)TciProvider.getInstance().getTciCDRequired().getInteger().newInstance(); IntegerValue longitude = (IntegerValue)TciProvider.getInstance().getTciCDRequired().getInteger().newInstance(); diff --git a/javasrc/tool/org/etsi/its/tool/testingtech/ExternalFunctionsPluginProvider.java b/javasrc/tool/org/etsi/its/tool/testingtech/ExternalFunctionsPluginProvider.java index 45cf82521..770df95dc 100644 --- a/javasrc/tool/org/etsi/its/tool/testingtech/ExternalFunctionsPluginProvider.java +++ b/javasrc/tool/org/etsi/its/tool/testingtech/ExternalFunctionsPluginProvider.java @@ -106,13 +106,13 @@ public class ExternalFunctionsPluginProvider implements ExternalFunctionsProvide * @param pLongitude Computed position's longitude * * TTCN-3 signature: - * external function fx_computePositionFromRotation(in LongPosVector p_iutLongPosVector, in float p_rotation, out UInt32 p_latitude, out UInt32 p_longitude); + * external function fx_computePositionFromRotation(in LongPosVector p_iutLongPosVector, in Int32 p_rotation, out Int32 p_latitude, out Int32 p_longitude); */ @Override public void fx_computePositionFromRotation( IntegerValue p_refLatitude, IntegerValue p_refLongitude, IntegerValue p_cenLatitude, IntegerValue p_cenLongitude, - FloatValue pRotation, + IntegerValue pRotation, IntegerValue pLatitude, IntegerValue pLongitude) { _externalFunctionsPluginProvider.fx_computePositionFromRotation(p_refLatitude, p_refLongitude, p_cenLatitude, p_cenLongitude, pRotation, pLatitude, pLongitude); } -- GitLab