Finalise rotation external function

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

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

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();

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);
     }