Commit 365ab4fc authored by garciay's avatar garciay

Finalise rotation external function

parent 19b869d8
......@@ -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
......
......@@ -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
......
......@@ -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();
......
......@@ -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);
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment