Commit 71504bef authored by garciay's avatar garciay
Browse files

Update rotation external function

parent 7505615e

javasrc/extfunc/org/etsi/its/extfunc/ItsExternalFunctionsProvider.java

+47 −46
Original line number Diff line number Diff line
@@ -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()));

        // Get rotation angle

        double rotation = Math.toRadians(p_rotation.getFloat());

        

    	FloatValue v_rotation = _tcicdWrapper.getFloat();

        v_rotation.setFloat(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));

        Long rlat = Math.round(lat);

        Long rlong = Math.round(long_);

        // 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_);

        _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<WGS84> polygonalArea = new ArrayList<WGS84>();
@@ -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<WGS84> parentArea = new ArrayList<WGS84>();
@@ -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<WGS84> > polygonalAreas = new ArrayList< ArrayList<WGS84> >();
@@ -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<WGS84> polygonalArea = new ArrayList<WGS84>();
@@ -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;

    }