From 71504befcddc19f42e717ff45529431e7feb31a4 Mon Sep 17 00:00:00 2001 From: garciay Date: Fri, 28 Oct 2016 15:38:16 +0000 Subject: [PATCH] Update rotation external function --- .../extfunc/ItsExternalFunctionsProvider.java | 93 ++++++++++--------- 1 file changed, 47 insertions(+), 46 deletions(-) diff --git a/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java b/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java index ffcafae53..5dc04f4bf 100644 --- a/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java +++ b/javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java @@ -54,11 +54,6 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid */ public static final String Version = "1.0.0.0"; - /** - * Logger instance - */ - //private final static Logger _logger = Logger.getLogger("org.etsi.its"); - /** * Unique instance of TciCDWrapper class */ @@ -85,7 +80,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * Default ctor */ public ItsExternalFunctionsProvider() { - //_logger.entering("ItsExternalFunctionsProvider", "Constructor", String.format("version:%s", Version)); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "Constructor", String.format("version:%s", Version)); _tcicdWrapper = TciCDWrapperFactory.getTciCDInstance(); @@ -111,7 +106,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid @Override public synchronized IntegerValue fx_getCurrentTime() { IntegerValue now = null; - //_logger.entering("ItsExternalFunctionsProvider", "fx_getCurrentTime"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getCurrentTime"); if (gnssScenarioSupport) { now = _tcicdWrapper.setInteger(GNSS.getGpsTime()); @@ -131,7 +126,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid now = _tcicdWrapper.setInteger(0); } - //_logger.exiting("ItsExternalFunctionsProvider", "fx_getCurrentTime", String.format("%10d", _tcicdWrapper.getBigInteger(now))); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getCurrentTime", String.format("%10d", _tcicdWrapper.getBigInteger(now))); } return now; } // End of method fx_getCurrentTime @@ -139,24 +134,24 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid @Override public IntegerValue fx_getCurrentTimeMark() { IntegerValue now = null; - //_logger.entering("ItsExternalFunctionsProvider", "fx_getCurrentTimeMark"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getCurrentTimeMark"); ZonedDateTime t = ZonedDateTime.now(ZoneOffset.UTC); now = _tcicdWrapper.setInteger(new Integer((t.getMinute() * 60 + t.getSecond()) * 10)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_getCurrentTimeMark", String.format("%4d", _tcicdWrapper.getBigInteger(now))); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getCurrentTimeMark", String.format("%4d", _tcicdWrapper.getBigInteger(now))); return now; } @Override public IntegerValue fx_getMinuteOfTheYear() { IntegerValue now = null; - //_logger.entering("ItsExternalFunctionsProvider", "fx_getMinuteOfTheYear"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getMinuteOfTheYear"); ZonedDateTime t = ZonedDateTime.now(ZoneOffset.UTC); now = _tcicdWrapper.setInteger(new Integer((t.getDayOfYear() - 1) * 24 * 60 + t.getHour() * 60 + t.getMinute())); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_getMinuteOfTheYear", String.format("%4d", _tcicdWrapper.getBigInteger(now))); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_getMinuteOfTheYear", String.format("%4d", _tcicdWrapper.getBigInteger(now))); return now; } @@ -181,7 +176,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid public synchronized FloatValue fx_computeDistance( final IntegerValue p_latitudeA, final IntegerValue p_longitudeA, final IntegerValue p_latitudeB, final IntegerValue p_longitudeB) { -// //_logger.entering("ItsExternalFunctionsProvider", "fx_computeDistance", +// //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_computeDistance", // String.format("%d, %d, %d, %d", // p_latitudeA.getInteger(), // p_longitudeA.getInteger(), @@ -233,7 +228,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid final IntegerValue p_refLongitude, final FloatValue p_distance, final IntegerValue p_orientation, IntegerValue p_latitude, IntegerValue p_longitude) { -// //_logger.entering( +// //TERFactory.getInstance().logDebug( // "ItsExternalFunctionsProvider", // "fx_computePositionUsingDistance", // String.format("%d, %d", p_distance.getInteger(), @@ -297,23 +292,29 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid final FloatValue p_rotation, IntegerValue p_latitude, IntegerValue p_longitude) { - // //_logger.entering( + // //TERFactory.getInstance().logDebug( // "ItsExternalFunctionsProvider", // "fx_computePositionFromRotation", // String.format("%d", p_rotation.getFloat())); - - FloatValue v_rotation = _tcicdWrapper.getFloat(); - v_rotation.setFloat(p_rotation.getFloat()); + // Get rotation angle + double rotation = Math.toRadians(p_rotation.getFloat()); + // X = longitude, Y = latitude - double long_ = _tcicdWrapper.getInteger(p_cenLongitude) + Math.cos(v_rotation.getFloat()) * (_tcicdWrapper.getInteger(p_refLongitude) - _tcicdWrapper.getInteger(p_cenLongitude)) - Math.sin(v_rotation.getFloat()) * (_tcicdWrapper.getInteger(p_refLatitude) - _tcicdWrapper.getInteger(p_cenLatitude)); - double lat = _tcicdWrapper.getInteger(p_cenLatitude) + Math.sin(v_rotation.getFloat()) * (_tcicdWrapper.getInteger(p_refLongitude) - _tcicdWrapper.getInteger(p_cenLongitude)) + Math.cos(v_rotation.getFloat()) * (_tcicdWrapper.getInteger(p_refLatitude) - _tcicdWrapper.getInteger(p_cenLatitude)); + // 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; + + // The out parameter needs to be set on the object level Long rlat = Math.round(lat); Long rlong = Math.round(long_); - - // The out parameter needs to be set on the object level _tcicdWrapper.setInteger(p_latitude, rlat.intValue()); _tcicdWrapper.setInteger(p_longitude, rlong.intValue()); - } // End of method fx_computePositionFromRotation @Override @@ -402,7 +403,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid final OctetstringValue p_destinationAddress, final IntegerValue p_payloadLength, final OctetstringValue p_payload, final IntegerValue p_nextHdr) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum"); // Build the pseudo header according RFC 2460 - Clause 8.1 ByteArrayOutputStream pseudoheader = new ByteArrayOutputStream(); @@ -446,14 +447,14 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid --length; } sum = (~((sum & 0xFFFF) + (sum >> 16))) & 0xFFFF; - //_logger.info(String.format("ItsExternalFunctionsProvider.fx_computeIPv6CheckSum: finalSum=%d",sum)); + //TERFactory.getInstance().logDebug(String.format("ItsExternalFunctionsProvider.fx_computeIPv6CheckSum: finalSum=%d",sum)); // Set the return value OctetstringValue checksum = _tcicdWrapper.getOctetstring(); checksum.setLength(2); checksum.setOctet(0, (byte) ((byte) (sum >> 8) & 0xff)); checksum.setOctet(1, (byte) (sum & 0x00ff)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum", checksum); // FIXME Check which method to call for logging + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum", checksum); // FIXME Check which method to call for logging return checksum; } @@ -485,7 +486,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * @return The hash value */ public OctetstringValue fx_hashWithSha256(final OctetstringValue p_toBeHashedData) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_hashWithSha256"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_hashWithSha256"); byte[] toBeHashedData = new byte[p_toBeHashedData.getLength()]; for (int i = 0; i < toBeHashedData.length; i++) { @@ -508,7 +509,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * @return The signature value */ public OctetstringValue fx_signWithEcdsaNistp256WithSha256(final OctetstringValue p_toBeSignedData, final OctetstringValue/*IntegerValue*/ p_privateKey) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_signWithEcdsaNistp256WithSha256"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_signWithEcdsaNistp256WithSha256"); byte[] toBeSignedData = new byte[p_toBeSignedData.getLength()]; for (int i = 0; i < toBeSignedData.length; i++) { @@ -545,7 +546,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * @return true on success, false otherwise */ public BooleanValue fx_verifyWithEcdsaNistp256WithSha256(final OctetstringValue p_toBeVerifiedData, final OctetstringValue p_signature, final OctetstringValue p_ecdsaNistp256PublicKeyX, final OctetstringValue p_ecdsaNistp256PublicKeyY) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_verifyWithEcdsaNistp256WithSha256"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_verifyWithEcdsaNistp256WithSha256"); BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(false); @@ -577,7 +578,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid e.printStackTrace(); } - //_logger.exiting("ItsExternalFunctionsProvider", "fx_verifyWithEcdsaNistp256WithSha256"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_verifyWithEcdsaNistp256WithSha256"); return result; } @@ -589,7 +590,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * @return true on success, false otherwise */ public BooleanValue fx_generateKeyPair(OctetstringValue/*IntegerValue*/ p_privateKey, OctetstringValue p_publicKeyX, OctetstringValue p_publicKeyY) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_generateKeyPair"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_generateKeyPair"); BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(true); @@ -611,7 +612,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid p_publicKeyY.setOctet(i, ref[i]); } // End 'for' statement - //_logger.exiting("ItsExternalFunctionsProvider", "fx_generateKeyPair"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_generateKeyPair"); return result; } @@ -621,7 +622,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid * @return true on success, false otherwise */ public BooleanValue fx_isValidPolygonalRegion(final RecordOfValue p_region) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isValidPolygonalRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isValidPolygonalRegion"); // Setup arguments ArrayList polygonalArea = new ArrayList(); @@ -638,13 +639,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isValidPolygonArea(polygonalArea)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isValidPolygonalRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isValidPolygonalRegion"); return result; } @Override public BooleanValue fx_isPolygonalRegionInside(final RecordOfValue p_parent, final RecordOfValue p_region) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isPolygonalRegionInside"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isPolygonalRegionInside"); // Setup arguments ArrayList parentArea = new ArrayList(); @@ -670,13 +671,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isPolygonalRegionInside(parentArea, regionArea)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isPolygonalRegionInside"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isPolygonalRegionInside"); return result; } @Override public BooleanValue fx_isLocationInsideCircularRegion(final RecordValue p_region, final RecordValue p_location) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isLocationInsideCircularRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideCircularRegion"); // Setup arguments RecordValue rv = (RecordValue)p_region.getField("center"); // Center @@ -694,13 +695,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isLocationInsideCircularArea(location, center, radius)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isLocationInsideCircularRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideCircularRegion"); return result; } @Override public BooleanValue fx_isLocationInsideRectangularRegion(final RecordOfValue p_region, final RecordValue p_location) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isLocationInsideRectangularRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideRectangularRegion"); // Setup arguments ArrayList< ArrayList > polygonalAreas = new ArrayList< ArrayList >(); @@ -748,13 +749,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isLocationInsidePolygonalAreas(location, polygonalAreas)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isLocationInsideRectangularRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideRectangularRegion"); return result; } @Override public BooleanValue fx_isLocationInsidePolygonalRegion(final RecordOfValue p_region, final RecordValue p_location) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isLocationInsidePolygonalRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsidePolygonalRegion"); // Setup arguments ArrayList polygonalArea = new ArrayList(); @@ -775,13 +776,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isLocationInsidePolygonalArea(location, polygonalArea)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isLocationInsidePolygonalRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsidePolygonalRegion"); return result; } @Override public BooleanValue fx_isLocationInsideIdentifiedRegion(final RecordValue p_region, final RecordValue p_location) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_isLocationInsideIdentifiedRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideIdentifiedRegion"); // Setup arguments int regionDictionary = ((EnumeratedValue) p_region.getField("region_dictionary")).getInt(); @@ -796,13 +797,13 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid BooleanValue result = _tcicdWrapper.getBoolean(); result.setBoolean(Positioning.getInstance().isLocationInsideIdentifiedRegion(regionDictionary, regionId, localRegion, location)); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_isLocationInsideIdentifiedRegion"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_isLocationInsideIdentifiedRegion"); return result; } @Override public FloatValue fx_dms2dd(final IntegerValue p_degrees, final IntegerValue p_minutes, final FloatValue p_seconds, final OctetstringValue p_latlon) { - //_logger.entering("ItsExternalFunctionsProvider", "fx_dms2dd"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_dms2dd"); // Call Geodesic function FloatValue result = _tcicdWrapper.getFloat(); @@ -814,7 +815,7 @@ public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvid ); result.setFloat((float) location.toDD()); - //_logger.exiting("ItsExternalFunctionsProvider", "fx_dms2dd"); + //TERFactory.getInstance().logDebug("ItsExternalFunctionsProvider", "fx_dms2dd"); return result; } -- GitLab