update

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_adapter/jeely_xingyueL_adapter.cpp

@@ -88,10 +88,7 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
         controlSpeed=min(controlSpeed,-0.9f);
       //  controlSpeed=max(controlSpeed,-0.4f);
     }
-//    if(controlSpeed<-0.2)//debug ,lianxu,zhangaiwu buwen
-//    {
-//        controlSpeed=-0.1;
-//    }
+
 
 
     if(DecideGps00::minDecelerate<0){

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.cpp

@@ -1557,7 +1557,7 @@ void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusio
                 fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
                 fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
                 fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().y();
                 fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.velocity_linear_x();
                 fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
                 fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.cpp

@@ -2026,6 +2026,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         if(avoidXNewPreFilter!=avoidXNewLast){
             avoidXNew=avoidXNewPre;
+           //avoidXNew = avoidXNewPreFilter;
             if(avoidXNew<0)
                 turnLampFlag=-1;
             else if(avoidXNew>0)
@@ -2046,7 +2047,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
            else if(avoidXNew==0)
            {
                count_avoidx_0++;
-               if(count_avoidx_0>60)
+               if(count_avoidx_0>30)// you 60 gaicheng 30
                {
                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
 
@@ -2444,6 +2445,11 @@ if (vehState == preAvoid)
         }
     }
 
+    if(obsDistance==0)
+    {
+        obsDistance=500;  //chongxin guihua zhangaiwu juli 0  ,jisuan  acc=-0.5,20220823
+    }
+
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
     Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
@@ -2651,7 +2657,7 @@ if (vehState == preAvoid)
     //                           <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
     //                           <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
     //                           <<"Trace_Point"<< ","<<PathPoint<< ","
-    //                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                               <<"changingDangWei"<< ","<<changingDangWei<< ","
                                <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
     //                           <<"TTC"<< ","<<ttc<< ","
 //                               <<"Decide_torque"  << ","  <<setprecision(2)<<gps_decition->torque<< ","

src/driver/driver_map_xodrload/boost.h

@@ -1,51 +1,51 @@
-#pragma once
-
-#ifndef _IV_BOOST_H_
-#define _IV_BOOST_H_
-
-#if defined __GNUC__
-#  pragma GCC system_header 
-#endif
-#ifndef __CUDACC__
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
-#include <boost/version.hpp>
-#include <boost/numeric/conversion/cast.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/condition.hpp>
-#include <boost/thread.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/filesystem.hpp>
-#include <boost/bind.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/function.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/mpl/fold.hpp>
-#include <boost/mpl/inherit.hpp>
-#include <boost/mpl/inherit_linearly.hpp>
-#include <boost/mpl/joint_view.hpp>
-#include <boost/mpl/transform.hpp>
-#include <boost/mpl/vector.hpp>
-#include <boost/algorithm/string.hpp>
-#ifndef Q_MOC_RUN
-#include <boost/date_time/posix_time/posix_time.hpp>
-#endif
-#if BOOST_VERSION >= 104700
-#include <boost/chrono.hpp>
-#endif
-#include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
-#include <boost/shared_array.hpp>
-#include <boost/interprocess/sync/file_lock.hpp>
-#if BOOST_VERSION >= 104900
-#include <boost/interprocess/permissions.hpp>
-#endif
-#include <boost/iostreams/device/mapped_file.hpp>
-#define BOOST_PARAMETER_MAX_ARITY 7
-#include <boost/signals2.hpp>
-#include <boost/signals2/slot.hpp>
-#endif
-#endif
-#endif    // _IV_BOOST_H_
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/driver/driver_map_xodrload/const.cpp

@@ -1,255 +1,255 @@
-/*							const.c
- *
- *	Globally declared constants
- *
- *
- *
- * SYNOPSIS:
- *
- * extern double nameofconstant;
- *
- *
- *
- *
- * DESCRIPTION:
- *
- * This file contains a number of mathematical constants and
- * also some needed size parameters of the computer arithmetic.
- * The values are supplied as arrays of hexadecimal integers
- * for IEEE arithmetic; arrays of octal constants for DEC
- * arithmetic; and in a normal decimal scientific notation for
- * other machines.  The particular notation used is determined
- * by a symbol (DEC, IBMPC, or UNK) defined in the include file
- * mconf.h.
- *
- * The default size parameters are as follows.
- *
- * For DEC and UNK modes:
- * MACHEP =  1.38777878078144567553E-17       2**-56
- * MAXLOG =  8.8029691931113054295988E1       log(2**127)
- * MINLOG = -8.872283911167299960540E1        log(2**-128)
- * MAXNUM =  1.701411834604692317316873e38    2**127
- *
- * For IEEE arithmetic (IBMPC):
- * MACHEP =  1.11022302462515654042E-16       2**-53
- * MAXLOG =  7.09782712893383996843E2         log(2**1024)
- * MINLOG = -7.08396418532264106224E2         log(2**-1022)
- * MAXNUM =  1.7976931348623158E308           2**1024
- *
- * The global symbols for mathematical constants are
- * PI     =  3.14159265358979323846           pi
- * PIO2   =  1.57079632679489661923           pi/2
- * PIO4   =  7.85398163397448309616E-1        pi/4
- * SQRT2  =  1.41421356237309504880           sqrt(2)
- * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
- * LOG2E  =  1.4426950408889634073599         1/log(2)
- * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
- * LOGE2  =  6.93147180559945309417E-1        log(2)
- * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
- * THPIO4 =  2.35619449019234492885           3*pi/4
- * TWOOPI =  6.36619772367581343075535E-1     2/pi
- *
- * These lists are subject to change.
- */
-
-/*							const.c */
-
-/*
-Cephes Math Library Release 2.3:  March, 1995
-Copyright 1984, 1995 by Stephen L. Moshier
-*/
-
-#include "mconf.h"
-#include <stdlib.h>
-#include <stdio.h>
-
-#ifdef UNK
-#if 1
-double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
-#else
-double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
-#endif
-double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
-#ifdef DENORMAL
-double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
-/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
-double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
-#else
-double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
-double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
-#endif
-double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
-double PI     =  3.14159265358979323846;       /* pi */
-double PIO2   =  1.57079632679489661923;       /* pi/2 */
-double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
-double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
-double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
-double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
-double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
-double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
-double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
-double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
-double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
-#ifdef INFINITIES
-//double INFINITY = 1.0/0.0;  /* 99e999; */
-double INFINITY = atof("infinity");  /* 99e999; */
-#else
-double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
-#endif
-#ifdef NANS
-double NAN =atof("infinity") - atof("infinity");
-#else
-double NAN = 0.0;
-#endif
-#ifdef MINUSZERO
-double NEGZERO = -0.0;
-#else
-double NEGZERO = 0.0;
-#endif
-#endif
-
-#ifdef IBMPC
-			/* 2**-53 =  1.11022302462515654042E-16 */
-unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
-unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
-#ifdef DENORMAL
-			/* log(MAXNUM) =  7.09782712893383996732224E2 */
-unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
-			/* log(2**-1074) = - -7.44440071921381262314E2 */
-/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
-unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
-#else
-			/* log(2**1022) =   7.08396418532264106224E2 */
-unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
-			/* log(2**-1022) = - 7.08396418532264106224E2 */
-unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
-#endif
-			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
-unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
-unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
-unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
-unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
-unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
-unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
-unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
-unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
-unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
-unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
-unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
-unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
-#ifdef INFINITIES
-unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
-#else
-unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
-#endif
-#ifdef NANS
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
-#else
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
-#else
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#endif
-
-#ifdef MIEEE
-			/* 2**-53 =  1.11022302462515654042E-16 */
-unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
-unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
-#ifdef DENORMAL
-			/* log(2**1024) =   7.09782712893383996843E2 */
-unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
-			/* log(2**-1074) = - -7.44440071921381262314E2 */
-/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
-unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
-#else
-			/* log(2**1022) =  7.08396418532264106224E2 */
-unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
-			/* log(2**-1022) = - 7.08396418532264106224E2 */
-unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
-#endif
-			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
-unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
-unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
-unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
-unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
-unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
-unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
-unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
-unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
-unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
-unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
-unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
-unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
-#ifdef INFINITIES
-unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
-#else
-unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
-#endif
-#ifdef NANS
-unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
-#else
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
-#else
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#endif
-
-#ifdef DEC
-			/* 2**-56 =  1.38777878078144567553E-17 */
-unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
-unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
-			/* log 2**127 = 88.029691931113054295988 */
-unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
-			/* log 2**-128 = -88.72283911167299960540 */
-unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
-			/* 2**127 = 1.701411834604692317316873e38 */
-unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
-unsigned short PI[4]     = {040511,007732,0121041,064302,};
-unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
-unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
-unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
-unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
-unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
-unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
-unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
-unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
-unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
-unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
-/* Approximate infinity by MAXNUM.  */
-unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
-unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
-#else
-unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
-#endif
-#endif
-
-#ifndef UNK
-extern unsigned short MACHEP[];
-extern unsigned short UFLOWTHRESH[];
-extern unsigned short MAXLOG[];
-extern unsigned short UNDLOG[];
-extern unsigned short MINLOG[];
-extern unsigned short MAXNUM[];
-extern unsigned short PI[];
-extern unsigned short PIO2[];
-extern unsigned short PIO4[];
-extern unsigned short SQRT2[];
-extern unsigned short SQRTH[];
-extern unsigned short LOG2E[];
-extern unsigned short SQ2OPI[];
-extern unsigned short LOGE2[];
-extern unsigned short LOGSQ2[];
-extern unsigned short THPIO4[];
-extern unsigned short TWOOPI[];
-extern unsigned short INFINITY[];
-extern unsigned short NAN[];
-extern unsigned short NEGZERO[];
-#endif
+/*							const.c
+ *
+ *	Globally declared constants
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * extern double nameofconstant;
+ *
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains a number of mathematical constants and
+ * also some needed size parameters of the computer arithmetic.
+ * The values are supplied as arrays of hexadecimal integers
+ * for IEEE arithmetic; arrays of octal constants for DEC
+ * arithmetic; and in a normal decimal scientific notation for
+ * other machines.  The particular notation used is determined
+ * by a symbol (DEC, IBMPC, or UNK) defined in the include file
+ * mconf.h.
+ *
+ * The default size parameters are as follows.
+ *
+ * For DEC and UNK modes:
+ * MACHEP =  1.38777878078144567553E-17       2**-56
+ * MAXLOG =  8.8029691931113054295988E1       log(2**127)
+ * MINLOG = -8.872283911167299960540E1        log(2**-128)
+ * MAXNUM =  1.701411834604692317316873e38    2**127
+ *
+ * For IEEE arithmetic (IBMPC):
+ * MACHEP =  1.11022302462515654042E-16       2**-53
+ * MAXLOG =  7.09782712893383996843E2         log(2**1024)
+ * MINLOG = -7.08396418532264106224E2         log(2**-1022)
+ * MAXNUM =  1.7976931348623158E308           2**1024
+ *
+ * The global symbols for mathematical constants are
+ * PI     =  3.14159265358979323846           pi
+ * PIO2   =  1.57079632679489661923           pi/2
+ * PIO4   =  7.85398163397448309616E-1        pi/4
+ * SQRT2  =  1.41421356237309504880           sqrt(2)
+ * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
+ * LOG2E  =  1.4426950408889634073599         1/log(2)
+ * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
+ * LOGE2  =  6.93147180559945309417E-1        log(2)
+ * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
+ * THPIO4 =  2.35619449019234492885           3*pi/4
+ * TWOOPI =  6.36619772367581343075535E-1     2/pi
+ *
+ * These lists are subject to change.
+ */
+
+/*							const.c */
+
+/*
+Cephes Math Library Release 2.3:  March, 1995
+Copyright 1984, 1995 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef UNK
+#if 1
+double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
+#else
+double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
+#endif
+double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
+#ifdef DENORMAL
+double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
+/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
+double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
+#else
+double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
+double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
+#endif
+double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+double PI     =  3.14159265358979323846;       /* pi */
+double PIO2   =  1.57079632679489661923;       /* pi/2 */
+double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
+double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
+double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
+double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
+double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
+double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
+double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
+double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
+double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
+#ifdef INFINITIES
+//double INFINITY = 1.0/0.0;  /* 99e999; */
+double INFINITY = atof("infinity");  /* 99e999; */
+#else
+double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+#endif
+#ifdef NANS
+double NAN =atof("infinity") - atof("infinity");
+#else
+double NAN = 0.0;
+#endif
+#ifdef MINUSZERO
+double NEGZERO = -0.0;
+#else
+double NEGZERO = 0.0;
+#endif
+#endif
+
+#ifdef IBMPC
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
+unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
+#ifdef DENORMAL
+			/* log(MAXNUM) =  7.09782712893383996732224E2 */
+unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
+unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
+#else
+			/* log(2**1022) =   7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
+unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
+unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
+unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
+unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
+unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
+unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
+unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
+unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
+unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
+unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
+unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
+#else
+unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef MIEEE
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
+unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
+#ifdef DENORMAL
+			/* log(2**1024) =   7.09782712893383996843E2 */
+unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
+unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
+#else
+			/* log(2**1022) =  7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
+unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
+unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
+unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
+unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
+unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
+unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
+unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
+unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
+unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
+unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
+unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
+#else
+unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef DEC
+			/* 2**-56 =  1.38777878078144567553E-17 */
+unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
+unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
+			/* log 2**127 = 88.029691931113054295988 */
+unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
+			/* log 2**-128 = -88.72283911167299960540 */
+unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
+			/* 2**127 = 1.701411834604692317316873e38 */
+unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
+unsigned short PI[4]     = {040511,007732,0121041,064302,};
+unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
+unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
+unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
+unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
+unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
+unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
+unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
+unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
+unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
+unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
+/* Approximate infinity by MAXNUM.  */
+unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
+unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
+#else
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
+#endif
+#endif
+
+#ifndef UNK
+extern unsigned short MACHEP[];
+extern unsigned short UFLOWTHRESH[];
+extern unsigned short MAXLOG[];
+extern unsigned short UNDLOG[];
+extern unsigned short MINLOG[];
+extern unsigned short MAXNUM[];
+extern unsigned short PI[];
+extern unsigned short PIO2[];
+extern unsigned short PIO4[];
+extern unsigned short SQRT2[];
+extern unsigned short SQRTH[];
+extern unsigned short LOG2E[];
+extern unsigned short SQ2OPI[];
+extern unsigned short LOGE2[];
+extern unsigned short LOGSQ2[];
+extern unsigned short THPIO4[];
+extern unsigned short TWOOPI[];
+extern unsigned short INFINITY[];
+extern unsigned short NAN[];
+extern unsigned short NEGZERO[];
+#endif

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -1,91 +1,80 @@
-QT -= gui
-
-QT += xml dbus
-
-CONFIG += c++11 console
-CONFIG -= app_bundle
-
-
-# The following define makes your compiler emit warnings if you use
-# any feature of Qt which as been marked deprecated (the exact warnings
-# depend on your compiler). Please consult the documentation of the
-# deprecated API in order to know how to port your code away from it.
-DEFINES += QT_DEPRECATED_WARNINGS
-DEFINES +=INPILOT
-QMAKE_CXXFLAGS +=  -g
-
-# You can also make your code fail to compile if you use deprecated APIs.
-# In order to do so, uncomment the following line.
-# You can also select to disable deprecated APIs only up to a certain version of Qt.
-#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-
-SOURCES += main.cpp     \
-    ../../include/msgtype/roireqest.pb.cc \
-    ../../include/msgtype/roiresult.pb.cc \
-    ../../include/msgtype/v2x.pb.cc \
-    fresnl.cpp \
-    const.cpp \
-    polevl.c \
-    globalplan.cpp \
-    dubins.c \
-    ../../include/msgtype/gps.pb.cc \
-    ../../include/msgtype/gpsimu.pb.cc \
-    ../../include/msgtype/imu.pb.cc \
-    gnss_coordinate_convert.cpp \
-    service_roi_xodr.cpp \
-    xodrplan.cpp \
-    ../../include/msgtype/mapdata.pb.cc
-
-HEADERS += \
-    ../../../include/ivexit.h \
-    ../../include/msgtype/roireqest.pb.h \
-    ../../include/msgtype/roiresult.pb.h \
-    ../../include/msgtype/v2x.pb.h \
-    mconf.h \
-    globalplan.h \
-    gps_type.h \
-    dubins.h \
-    ../../include/msgtype/gps.pb.h \
-    ../../include/msgtype/gpsimu.pb.h \
-    ../../include/msgtype/imu.pb.h \
-    gnss_coordinate_convert.h \
-    planpoint.h \
-    service_roi_xodr.h \
-    xodrplan.h \
-    ../../include/msgtype/mapdata.pb.h
-
-
-!include(../../../include/common.pri ) {
-    error( "Couldn't find the common.pri file!" )
-}
-
-!include(../../../include/ivprotobuf.pri ) {
-    error( "Couldn't find the ivprotobuf.pri file!" )
-}
-
-!include(../../../include/ivboost.pri ) {
-    error( "Couldn't find the ivboost.pri file!" )
-}
-
-!include(../../../include/iveigen.pri ) {
-    error( "Couldn't find the iveigen.pri file!" )
-}
-
-
-!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
-    error( "Couldn't find the OpenDrive.pri file!" )
-}
-
-!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
-    error( "Couldn't find the TinyXML.pri file!" )
-}
-
-!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
-    error( "Couldn't find the xodrfunc.pri file!" )
-}
-
-INCLUDEPATH += $$PWD/../../common/common/xodr
-INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
-
-
-
+QT -= gui
+
+QT += xml dbus
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+QMAKE_CXXFLAGS +=  -g
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp     \
+    fresnl.cpp \
+    const.cpp \
+    polevl.c \
+    globalplan.cpp \
+    dubins.c \
+    gnss_coordinate_convert.cpp \
+    service_roi_xodr.cpp \
+    xodrplan.cpp
+
+HEADERS += \
+    ../../../include/ivexit.h \
+    mconf.h \
+    globalplan.h \
+    gps_type.h \
+    dubins.h \
+    gnss_coordinate_convert.h \
+    planpoint.h \
+    service_roi_xodr.h \
+    xodrplan.h
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
+    error( "Couldn't find the TinyXML.pri file!" )
+}
+
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
+DEFINES += INPILOT
+
+LIBS += -livprotoif
+
+

src/driver/driver_map_xodrload/driver_map_xodrload.xml

@@ -1,6 +1,6 @@
-<xml>	
-	<node name="driver_map_xodrload">
-		<param name="extendmap" value="true" />
-	</node>
-</xml>
-
+<xml>	
+	<node name="driver_map_xodrload">
+		<param name="extendmap" value="true" />
+	</node>
+</xml>
+

src/driver/driver_map_xodrload/dubins.c

@@ -1,439 +1,439 @@
-/*
- * Copyright (c) 2008-2018, Andrew Walker
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifdef WIN32
-#define _USE_MATH_DEFINES
-#endif
-#include <math.h>
-#include "dubins.h"
-
-#define EPSILON (10e-10)
-
-typedef enum 
-{
-    L_SEG = 0,
-    S_SEG = 1,
-    R_SEG = 2
-} SegmentType;
-
-/* The segment types for each of the Path types */
-const SegmentType DIRDATA[][3] = {
-    { L_SEG, S_SEG, L_SEG },
-    { L_SEG, S_SEG, R_SEG },
-    { R_SEG, S_SEG, L_SEG },
-    { R_SEG, S_SEG, R_SEG },
-    { R_SEG, L_SEG, R_SEG },
-    { L_SEG, R_SEG, L_SEG }
-};
-
-typedef struct 
-{
-    double alpha;
-    double beta;
-    double d;
-    double sa;
-    double sb;
-    double ca;
-    double cb;
-    double c_ab;
-    double d_sq;
-} DubinsIntermediateResults;
-
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
-
-/**
- * Floating point modulus suitable for rings
- *
- * fmod doesn't behave correctly for angular quantities, this function does
- */
-double fmodr( double x, double y)
-{
-    return x - y*floor(x/y);
-}
-
-double mod2pi( double theta )
-{
-    return fmodr( theta, 2 * M_PI );
-}
-
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
-{
-    int i, errcode;
-    DubinsIntermediateResults in;
-    double params[3];
-    double cost;
-    double best_cost = INFINITY;
-    int best_word = -1;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode != EDUBOK) {
-        return errcode;
-    }
-
-
-    path->qi[0] = q0[0];
-    path->qi[1] = q0[1];
-    path->qi[2] = q0[2];
-    path->rho = rho;
- 
-    for( i = 0; i < 6; i++ ) {
-        DubinsPathType pathType = (DubinsPathType)i;
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            cost = params[0] + params[1] + params[2];
-            if(cost < best_cost) {
-                best_word = i;
-                best_cost = cost;
-                path->param[0] = params[0];
-                path->param[1] = params[1];
-                path->param[2] = params[2];
-                path->type = pathType;
-            }
-        }
-    }
-    if(best_word == -1) {
-        return EDUBNOPATH;
-    }
-    return EDUBOK;
-}
-
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
-{
-    int errcode;
-    DubinsIntermediateResults in;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode == EDUBOK) {
-        double params[3];
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            path->param[0] = params[0];
-            path->param[1] = params[1];
-            path->param[2] = params[2];
-            path->qi[0] = q0[0];
-            path->qi[1] = q0[1];
-            path->qi[2] = q0[2];
-            path->rho = rho;
-            path->type = pathType;
-        }
-    }
-    return errcode;
-}
-
-double dubins_path_length( DubinsPath* path )
-{
-    double length = 0.;
-    length += path->param[0];
-    length += path->param[1];
-    length += path->param[2];
-    length = length * path->rho;
-    return length;
-}
-
-double dubins_segment_length( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i] * path->rho;
-}
-
-double dubins_segment_length_normalized( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i];
-} 
-
-DubinsPathType dubins_path_type( DubinsPath* path ) 
-{
-    return path->type;
-}
-
-void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
-{
-    double st = sin(qi[2]);
-    double ct = cos(qi[2]);
-    if( type == L_SEG ) {
-        qt[0] = +sin(qi[2]+t) - st;
-        qt[1] = -cos(qi[2]+t) + ct;
-        qt[2] = t;
-    }
-    else if( type == R_SEG ) {
-        qt[0] = -sin(qi[2]-t) + st;
-        qt[1] = +cos(qi[2]-t) - ct;
-        qt[2] = -t;
-    }
-    else if( type == S_SEG ) {
-        qt[0] = ct * t;
-        qt[1] = st * t;
-        qt[2] = 0.0;
-    }
-    qt[0] += qi[0];
-    qt[1] += qi[1];
-    qt[2] += qi[2];
-}
-
-int dubins_path_sample( DubinsPath* path, double t, double q[3] )
-{
-    /* tprime is the normalised variant of the parameter t */
-    double tprime = t / path->rho;
-    double qi[3]; /* The translated initial configuration */
-    double q1[3]; /* end-of segment 1 */
-    double q2[3]; /* end-of segment 2 */
-    const SegmentType* types = DIRDATA[path->type];
-    double p1, p2;
-
-    if( t < 0 || t > dubins_path_length(path) ) {
-        return EDUBPARAM;
-    }
-
-    /* initial configuration */
-    qi[0] = 0.0;
-    qi[1] = 0.0;
-    qi[2] = path->qi[2];
-
-    /* generate the target configuration */
-    p1 = path->param[0];
-    p2 = path->param[1];
-    dubins_segment( p1,      qi,    q1, types[0] );
-    dubins_segment( p2,      q1,    q2, types[1] );
-    if( tprime < p1 ) {
-        dubins_segment( tprime, qi, q, types[0] );
-    }
-    else if( tprime < (p1+p2) ) {
-        dubins_segment( tprime-p1, q1, q,  types[1] );
-    }
-    else {
-        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
-    }
-
-    /* scale the target configuration, translate back to the original starting point */
-    q[0] = q[0] * path->rho + path->qi[0];
-    q[1] = q[1] * path->rho + path->qi[1];
-    q[2] = mod2pi(q[2]);
-
-    return EDUBOK;
-}
-
-int dubins_path_sample_many(DubinsPath* path, double stepSize, 
-                            DubinsPathSamplingCallback cb, void* user_data)
-{
-    int retcode;
-    double q[3];
-    double x = 0.0;
-    double length = dubins_path_length(path);
-    while( x <  length ) {
-        dubins_path_sample( path, x, q );
-        retcode = cb(q, x, user_data);
-        if( retcode != 0 ) {
-            return retcode;
-        }
-        x += stepSize;
-    }
-    return 0;
-}
-
-int dubins_path_endpoint( DubinsPath* path, double q[3] )
-{
-    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
-}
-
-int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
-{
-    /* calculate the true parameter */
-    double tprime = t / path->rho;
-
-    if((t < 0) || (t > dubins_path_length(path)))
-    {
-        return EDUBPARAM; 
-    }
-
-    /* copy most of the data */
-    newpath->qi[0] = path->qi[0];
-    newpath->qi[1] = path->qi[1];
-    newpath->qi[2] = path->qi[2];
-    newpath->rho   = path->rho;
-    newpath->type  = path->type;
-
-    /* fix the parameters */
-    newpath->param[0] = fmin( path->param[0], tprime );
-    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
-    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
-    return 0;
-}
-
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
-{
-    double dx, dy, D, d, theta, alpha, beta;
-    if( rho <= 0.0 ) {
-        return EDUBBADRHO;
-    }
-
-    dx = q1[0] - q0[0];
-    dy = q1[1] - q0[1];
-    D = sqrt( dx * dx + dy * dy );
-    d = D / rho;
-    theta = 0;
-
-    /* test required to prevent domain errors if dx=0 and dy=0 */
-    if(d > 0) {
-        theta = mod2pi(atan2( dy, dx ));
-    }
-    alpha = mod2pi(q0[2] - theta);
-    beta  = mod2pi(q1[2] - theta);
-
-    in->alpha = alpha;
-    in->beta  = beta;
-    in->d     = d;
-    in->sa    = sin(alpha);
-    in->sb    = sin(beta);
-    in->ca    = cos(alpha);
-    in->cb    = cos(beta);
-    in->c_ab  = cos(alpha - beta);
-    in->d_sq  = d * d;
-
-    return EDUBOK;
-}
-
-int dubins_LSL(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0, tmp1, p_sq;
-    
-    tmp0 = in->d + in->sa - in->sb;
-    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
-
-    if(p_sq >= 0) {
-        tmp1 = atan2( (in->cb - in->ca), tmp0 );
-        out[0] = mod2pi(tmp1 - in->alpha);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(in->beta - tmp1);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-
-int dubins_RSR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0 = in->d - in->sa + in->sb;
-    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
-    if( p_sq >= 0 ) {
-        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
-        out[0] = mod2pi(in->alpha - tmp1);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(tmp1 -in->beta);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LSR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
-        out[0] = mod2pi(tmp0 - in->alpha);
-        out[1] = p;
-        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RSL(DubinsIntermediateResults* in, double out[3]) 
-{
-    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
-        out[0] = mod2pi(in->alpha - tmp0);
-        out[1] = p;
-        out[2] = mod2pi(in->beta - tmp0);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RLR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
-    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi((2*M_PI) - acos(tmp0) );
-        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LRL(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
-    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi( 2*M_PI - acos( tmp0) );
-        double t = mod2pi(-in->alpha - phi + p/2.);
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]) 
-{
-    int result;
-    switch(pathType)
-    {
-    case LSL:
-        result = dubins_LSL(in, out);
-        break;
-    case RSL:
-        result = dubins_RSL(in, out);
-        break;
-    case LSR:
-        result = dubins_LSR(in, out);
-        break;
-    case RSR:
-        result = dubins_RSR(in, out);
-        break;
-    case LRL:
-        result = dubins_LRL(in, out);
-        break;
-    case RLR:
-        result = dubins_RLR(in, out);
-        break;
-    default:
-        result = EDUBNOPATH;
-    }
-    return result;
-}
-
-
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include "dubins.h"
+
+#define EPSILON (10e-10)
+
+typedef enum 
+{
+    L_SEG = 0,
+    S_SEG = 1,
+    R_SEG = 2
+} SegmentType;
+
+/* The segment types for each of the Path types */
+const SegmentType DIRDATA[][3] = {
+    { L_SEG, S_SEG, L_SEG },
+    { L_SEG, S_SEG, R_SEG },
+    { R_SEG, S_SEG, L_SEG },
+    { R_SEG, S_SEG, R_SEG },
+    { R_SEG, L_SEG, R_SEG },
+    { L_SEG, R_SEG, L_SEG }
+};
+
+typedef struct 
+{
+    double alpha;
+    double beta;
+    double d;
+    double sa;
+    double sb;
+    double ca;
+    double cb;
+    double c_ab;
+    double d_sq;
+} DubinsIntermediateResults;
+
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+double fmodr( double x, double y)
+{
+    return x - y*floor(x/y);
+}
+
+double mod2pi( double theta )
+{
+    return fmodr( theta, 2 * M_PI );
+}
+
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
+{
+    int i, errcode;
+    DubinsIntermediateResults in;
+    double params[3];
+    double cost;
+    double best_cost = INFINITY;
+    int best_word = -1;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode != EDUBOK) {
+        return errcode;
+    }
+
+
+    path->qi[0] = q0[0];
+    path->qi[1] = q0[1];
+    path->qi[2] = q0[2];
+    path->rho = rho;
+ 
+    for( i = 0; i < 6; i++ ) {
+        DubinsPathType pathType = (DubinsPathType)i;
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            cost = params[0] + params[1] + params[2];
+            if(cost < best_cost) {
+                best_word = i;
+                best_cost = cost;
+                path->param[0] = params[0];
+                path->param[1] = params[1];
+                path->param[2] = params[2];
+                path->type = pathType;
+            }
+        }
+    }
+    if(best_word == -1) {
+        return EDUBNOPATH;
+    }
+    return EDUBOK;
+}
+
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
+{
+    int errcode;
+    DubinsIntermediateResults in;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode == EDUBOK) {
+        double params[3];
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            path->param[0] = params[0];
+            path->param[1] = params[1];
+            path->param[2] = params[2];
+            path->qi[0] = q0[0];
+            path->qi[1] = q0[1];
+            path->qi[2] = q0[2];
+            path->rho = rho;
+            path->type = pathType;
+        }
+    }
+    return errcode;
+}
+
+double dubins_path_length( DubinsPath* path )
+{
+    double length = 0.;
+    length += path->param[0];
+    length += path->param[1];
+    length += path->param[2];
+    length = length * path->rho;
+    return length;
+}
+
+double dubins_segment_length( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i] * path->rho;
+}
+
+double dubins_segment_length_normalized( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i];
+} 
+
+DubinsPathType dubins_path_type( DubinsPath* path ) 
+{
+    return path->type;
+}
+
+void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
+{
+    double st = sin(qi[2]);
+    double ct = cos(qi[2]);
+    if( type == L_SEG ) {
+        qt[0] = +sin(qi[2]+t) - st;
+        qt[1] = -cos(qi[2]+t) + ct;
+        qt[2] = t;
+    }
+    else if( type == R_SEG ) {
+        qt[0] = -sin(qi[2]-t) + st;
+        qt[1] = +cos(qi[2]-t) - ct;
+        qt[2] = -t;
+    }
+    else if( type == S_SEG ) {
+        qt[0] = ct * t;
+        qt[1] = st * t;
+        qt[2] = 0.0;
+    }
+    qt[0] += qi[0];
+    qt[1] += qi[1];
+    qt[2] += qi[2];
+}
+
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+    /* tprime is the normalised variant of the parameter t */
+    double tprime = t / path->rho;
+    double qi[3]; /* The translated initial configuration */
+    double q1[3]; /* end-of segment 1 */
+    double q2[3]; /* end-of segment 2 */
+    const SegmentType* types = DIRDATA[path->type];
+    double p1, p2;
+
+    if( t < 0 || t > dubins_path_length(path) ) {
+        return EDUBPARAM;
+    }
+
+    /* initial configuration */
+    qi[0] = 0.0;
+    qi[1] = 0.0;
+    qi[2] = path->qi[2];
+
+    /* generate the target configuration */
+    p1 = path->param[0];
+    p2 = path->param[1];
+    dubins_segment( p1,      qi,    q1, types[0] );
+    dubins_segment( p2,      q1,    q2, types[1] );
+    if( tprime < p1 ) {
+        dubins_segment( tprime, qi, q, types[0] );
+    }
+    else if( tprime < (p1+p2) ) {
+        dubins_segment( tprime-p1, q1, q,  types[1] );
+    }
+    else {
+        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
+    }
+
+    /* scale the target configuration, translate back to the original starting point */
+    q[0] = q[0] * path->rho + path->qi[0];
+    q[1] = q[1] * path->rho + path->qi[1];
+    q[2] = mod2pi(q[2]);
+
+    return EDUBOK;
+}
+
+int dubins_path_sample_many(DubinsPath* path, double stepSize, 
+                            DubinsPathSamplingCallback cb, void* user_data)
+{
+    int retcode;
+    double q[3];
+    double x = 0.0;
+    double length = dubins_path_length(path);
+    while( x <  length ) {
+        dubins_path_sample( path, x, q );
+        retcode = cb(q, x, user_data);
+        if( retcode != 0 ) {
+            return retcode;
+        }
+        x += stepSize;
+    }
+    return 0;
+}
+
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+    /* calculate the true parameter */
+    double tprime = t / path->rho;
+
+    if((t < 0) || (t > dubins_path_length(path)))
+    {
+        return EDUBPARAM; 
+    }
+
+    /* copy most of the data */
+    newpath->qi[0] = path->qi[0];
+    newpath->qi[1] = path->qi[1];
+    newpath->qi[2] = path->qi[2];
+    newpath->rho   = path->rho;
+    newpath->type  = path->type;
+
+    /* fix the parameters */
+    newpath->param[0] = fmin( path->param[0], tprime );
+    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+    return 0;
+}
+
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
+{
+    double dx, dy, D, d, theta, alpha, beta;
+    if( rho <= 0.0 ) {
+        return EDUBBADRHO;
+    }
+
+    dx = q1[0] - q0[0];
+    dy = q1[1] - q0[1];
+    D = sqrt( dx * dx + dy * dy );
+    d = D / rho;
+    theta = 0;
+
+    /* test required to prevent domain errors if dx=0 and dy=0 */
+    if(d > 0) {
+        theta = mod2pi(atan2( dy, dx ));
+    }
+    alpha = mod2pi(q0[2] - theta);
+    beta  = mod2pi(q1[2] - theta);
+
+    in->alpha = alpha;
+    in->beta  = beta;
+    in->d     = d;
+    in->sa    = sin(alpha);
+    in->sb    = sin(beta);
+    in->ca    = cos(alpha);
+    in->cb    = cos(beta);
+    in->c_ab  = cos(alpha - beta);
+    in->d_sq  = d * d;
+
+    return EDUBOK;
+}
+
+int dubins_LSL(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0, tmp1, p_sq;
+    
+    tmp0 = in->d + in->sa - in->sb;
+    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
+
+    if(p_sq >= 0) {
+        tmp1 = atan2( (in->cb - in->ca), tmp0 );
+        out[0] = mod2pi(tmp1 - in->alpha);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(in->beta - tmp1);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+
+int dubins_RSR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0 = in->d - in->sa + in->sb;
+    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
+    if( p_sq >= 0 ) {
+        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
+        out[0] = mod2pi(in->alpha - tmp1);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(tmp1 -in->beta);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LSR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
+        out[0] = mod2pi(tmp0 - in->alpha);
+        out[1] = p;
+        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RSL(DubinsIntermediateResults* in, double out[3]) 
+{
+    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
+        out[0] = mod2pi(in->alpha - tmp0);
+        out[1] = p;
+        out[2] = mod2pi(in->beta - tmp0);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RLR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
+    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi((2*M_PI) - acos(tmp0) );
+        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LRL(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
+    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi( 2*M_PI - acos( tmp0) );
+        double t = mod2pi(-in->alpha - phi + p/2.);
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]) 
+{
+    int result;
+    switch(pathType)
+    {
+    case LSL:
+        result = dubins_LSL(in, out);
+        break;
+    case RSL:
+        result = dubins_RSL(in, out);
+        break;
+    case LSR:
+        result = dubins_LSR(in, out);
+        break;
+    case RSR:
+        result = dubins_RSR(in, out);
+        break;
+    case LRL:
+        result = dubins_LRL(in, out);
+        break;
+    case RLR:
+        result = dubins_RLR(in, out);
+        break;
+    default:
+        result = EDUBNOPATH;
+    }
+    return result;
+}
+
+

src/driver/driver_map_xodrload/dubins.h

@@ -1,170 +1,170 @@
-/*
- * Copyright (c) 2008-2018, Andrew Walker
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifndef DUBINS_H
-#define DUBINS_H
-
-typedef enum 
-{
-    LSL = 0,
-    LSR = 1,
-    RSL = 2,
-    RSR = 3,
-    RLR = 4,
-    LRL = 5
-} DubinsPathType;
-
-typedef struct 
-{
-    /* the initial configuration */
-    double qi[3];        
-    /* the lengths of the three segments */
-    double param[3];     
-    /* model forward velocity / model angular velocity */
-    double rho;          
-    /* the path type described */
-    DubinsPathType type; 
-} DubinsPath;
-
-#define EDUBOK        (0)   /* No error */
-#define EDUBCOCONFIGS (1)   /* Colocated configurations */
-#define EDUBPARAM     (2)   /* Path parameterisitation error */
-#define EDUBBADRHO    (3)   /* the rho value is invalid */
-#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
-
-/**
- * Callback function for path sampling
- *
- * @note the q parameter is a configuration
- * @note the t parameter is the distance along the path
- * @note the user_data parameter is forwarded from the caller
- * @note return non-zero to denote sampling should be stopped
- */
-typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
-
-/**
- * Generate a path from an initial configuration to
- * a target configuration, with a specified maximum turning
- * radii
- *
- * A configuration is (x, y, theta), where theta is in radians, with zero
- * along the line x = 0, and counter-clockwise is positive
- *
- * @param path  - the resultant path
- * @param q0    - a configuration specified as an array of x, y, theta
- * @param q1    - a configuration specified as an array of x, y, theta
- * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @return      - non-zero on error
- */
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
-
-/**
- * Generate a path with a specified word from an initial configuration to
- * a target configuration, with a specified turning radius 
- *
- * @param path     - the resultant path
- * @param q0       - a configuration specified as an array of x, y, theta
- * @param q1       - a configuration specified as an array of x, y, theta
- * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @param pathType - the specific path type to use
- * @return         - non-zero on error
- */
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
-
-/**
- * Calculate the length of an initialised path
- *
- * @param path - the path to find the length of
- */
-double dubins_path_length(DubinsPath* path);
-
-/**
- * Return the length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length(DubinsPath* path, int i);
-
-/**
- * Return the normalized length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length_normalized( DubinsPath* path, int i );
-
-/**
- * Extract an integer that represents which path type was used
- *
- * @param path    - an initialised path
- * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL 
- */
-DubinsPathType dubins_path_type(DubinsPath* path);
-
-/**
- * Calculate the configuration along the path, using the parameter t
- *
- * @param path - an initialised path
- * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
- * @param q    - the configuration result
- * @returns    - non-zero if 't' is not in the correct range
- */
-int dubins_path_sample(DubinsPath* path, double t, double q[3]);
-
-/**
- * Walk along the path at a fixed sampling interval, calling the
- * callback function at each interval
- *
- * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
- *
- * @param path      - the path to sample
- * @param stepSize  - the distance along the path for subsequent samples
- * @param cb        - the callback function to call for each sample
- * @param user_data - optional information to pass on to the callback
- *
- * @returns - zero on successful completion, or the result of the callback
- */
-int dubins_path_sample_many(DubinsPath* path, 
-                            double stepSize, 
-                            DubinsPathSamplingCallback cb, 
-                            void* user_data);
-
-/**
- * Convenience function to identify the endpoint of a path
- *
- * @param path - an initialised path
- * @param q    - the configuration result
- */
-int dubins_path_endpoint(DubinsPath* path, double q[3]);
-
-/**
- * Convenience function to extract a subset of a path
- *
- * @param path    - an initialised path
- * @param t       - a length measure, where 0 < t < dubins_path_length(path)
- * @param newpath - the resultant path
- */
-int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
-
-
-#endif /* DUBINS_H */
-
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef DUBINS_H
+#define DUBINS_H
+
+typedef enum 
+{
+    LSL = 0,
+    LSR = 1,
+    RSL = 2,
+    RSR = 3,
+    RLR = 4,
+    LRL = 5
+} DubinsPathType;
+
+typedef struct 
+{
+    /* the initial configuration */
+    double qi[3];        
+    /* the lengths of the three segments */
+    double param[3];     
+    /* model forward velocity / model angular velocity */
+    double rho;          
+    /* the path type described */
+    DubinsPathType type; 
+} DubinsPath;
+
+#define EDUBOK        (0)   /* No error */
+#define EDUBCOCONFIGS (1)   /* Colocated configurations */
+#define EDUBPARAM     (2)   /* Path parameterisitation error */
+#define EDUBBADRHO    (3)   /* the rho value is invalid */
+#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path  - the resultant path
+ * @param q0    - a configuration specified as an array of x, y, theta
+ * @param q1    - a configuration specified as an array of x, y, theta
+ * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return      - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius 
+ *
+ * @param path     - the resultant path
+ * @param q0       - a configuration specified as an array of x, y, theta
+ * @param q1       - a configuration specified as an array of x, y, theta
+ * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return         - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path    - an initialised path
+ * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL 
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q    - the configuration result
+ * @returns    - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path      - the path to sample
+ * @param stepSize  - the distance along the path for subsequent samples
+ * @param cb        - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path, 
+                            double stepSize, 
+                            DubinsPathSamplingCallback cb, 
+                            void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q    - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path    - an initialised path
+ * @param t       - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+
+#endif /* DUBINS_H */
+

src/driver/driver_map_xodrload/fresnl.cpp

@@ -1,518 +1,518 @@
-/*							fresnl.c
- *
- *	Fresnel integral
- *
- *
- *
- * SYNOPSIS:
- *
- * double x, S, C;
- * void fresnl();
- *
- * fresnl( x, _&S, _&C );
- *
- *
- * DESCRIPTION:
- *
- * Evaluates the Fresnel integrals
- *
- *           x
- *           -
- *          | |
- * C(x) =   |   cos(pi/2 t**2) dt,
- *        | |
- *         -
- *          0
- *
- *           x
- *           -
- *          | |
- * S(x) =   |   sin(pi/2 t**2) dt.
- *        | |
- *         -
- *          0
- *
- *
- * The integrals are evaluated by a power series for x < 1.
- * For x >= 1 auxiliary functions f(x) and g(x) are employed
- * such that
- *
- * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
- * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
- *
- *
- *
- * ACCURACY:
- *
- *  Relative error.
- *
- * Arithmetic  function   domain     # trials      peak         rms
- *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
- *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
- *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
- *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
- */
-
-/*
-Cephes Math Library Release 2.8:  June, 2000
-Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
-*/
-
-#include "mconf.h"
-
-
-/* S(x) for small x */
-#ifdef UNK
-static double sn[6] = {
--2.99181919401019853726E3,
- 7.08840045257738576863E5,
--6.29741486205862506537E7,
- 2.54890880573376359104E9,
--4.42979518059697779103E10,
- 3.18016297876567817986E11,
-};
-static double sd[6] = {
-/* 1.00000000000000000000E0,*/
- 2.81376268889994315696E2,
- 4.55847810806532581675E4,
- 5.17343888770096400730E6,
- 4.19320245898111231129E8,
- 2.24411795645340920940E10,
- 6.07366389490084639049E11,
-};
-#endif
-#ifdef DEC
-static unsigned short sn[24] = {
-0143072,0176433,0065455,0127034,
-0045055,0007200,0134540,0026661,
-0146560,0035061,0023667,0127545,
-0050027,0166503,0002673,0153756,
-0151045,0002721,0121737,0102066,
-0051624,0013177,0033451,0021271,
-};
-static unsigned short sd[24] = {
-/*0040200,0000000,0000000,0000000,*/
-0042214,0130051,0112070,0101617,
-0044062,0010307,0172346,0152510,
-0045635,0160575,0143200,0136642,
-0047307,0171215,0127457,0052361,
-0050647,0031447,0032621,0013510,
-0052015,0064733,0117362,0012653,
-};
-#endif
-#ifdef IBMPC
-static unsigned short sn[24] = {
-0xb5c3,0x6d65,0x5fa3,0xc0a7,
-0x05b6,0x172c,0xa1d0,0x4125,
-0xf5ed,0x24f6,0x0746,0xc18e,
-0x7afe,0x60b7,0xfda8,0x41e2,
-0xf087,0x347b,0xa0ba,0xc224,
-0x2457,0xe6e5,0x82cf,0x4252,
-};
-static unsigned short sd[24] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x1072,0x3287,0x9605,0x4071,
-0xdaa9,0xfe9c,0x4218,0x40e6,
-0x17b4,0xb8d0,0xbc2f,0x4153,
-0xea9e,0xb5e5,0xfe51,0x41b8,
-0x22e9,0xe6b2,0xe664,0x4214,
-0x42b5,0x73de,0xad3b,0x4261,
-};
-#endif
-#ifdef MIEEE
-static unsigned short sn[24] = {
-0xc0a7,0x5fa3,0x6d65,0xb5c3,
-0x4125,0xa1d0,0x172c,0x05b6,
-0xc18e,0x0746,0x24f6,0xf5ed,
-0x41e2,0xfda8,0x60b7,0x7afe,
-0xc224,0xa0ba,0x347b,0xf087,
-0x4252,0x82cf,0xe6e5,0x2457,
-};
-static unsigned short sd[24] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x4071,0x9605,0x3287,0x1072,
-0x40e6,0x4218,0xfe9c,0xdaa9,
-0x4153,0xbc2f,0xb8d0,0x17b4,
-0x41b8,0xfe51,0xb5e5,0xea9e,
-0x4214,0xe664,0xe6b2,0x22e9,
-0x4261,0xad3b,0x73de,0x42b5,
-};
-#endif
-
-/* C(x) for small x */
-#ifdef UNK
-static double cn[6] = {
--4.98843114573573548651E-8,
- 9.50428062829859605134E-6,
--6.45191435683965050962E-4,
- 1.88843319396703850064E-2,
--2.05525900955013891793E-1,
- 9.99999999999999998822E-1,
-};
-static double cd[7] = {
- 3.99982968972495980367E-12,
- 9.15439215774657478799E-10,
- 1.25001862479598821474E-7,
- 1.22262789024179030997E-5,
- 8.68029542941784300606E-4,
- 4.12142090722199792936E-2,
- 1.00000000000000000118E0,
-};
-#endif
-#ifdef DEC
-static unsigned short cn[24] = {
-0132126,0040141,0063733,0013231,
-0034037,0072223,0010200,0075637,
-0135451,0021020,0073264,0036057,
-0036632,0131520,0101316,0060233,
-0137522,0072541,0136124,0132202,
-0040200,0000000,0000000,0000000,
-};
-static unsigned short cd[28] = {
-0026614,0135503,0051776,0032631,
-0030573,0121116,0154033,0126712,
-0032406,0034100,0012442,0106212,
-0034115,0017567,0150520,0164623,
-0035543,0106171,0177336,0146351,
-0037050,0150073,0000607,0171635,
-0040200,0000000,0000000,0000000,
-};
-#endif
-#ifdef IBMPC
-static unsigned short cn[24] = {
-0x62d3,0x2cfb,0xc80c,0xbe6a,
-0x0f74,0x6210,0xee92,0x3ee3,
-0x8786,0x0ed6,0x2442,0xbf45,
-0xcc13,0x1059,0x566a,0x3f93,
-0x9690,0x378a,0x4eac,0xbfca,
-0x0000,0x0000,0x0000,0x3ff0,
-};
-static unsigned short cd[28] = {
-0xc6b3,0x6a7f,0x9768,0x3d91,
-0x75b9,0xdb03,0x7449,0x3e0f,
-0x5191,0x02a4,0xc708,0x3e80,
-0x1d32,0xfa2a,0xa3ee,0x3ee9,
-0xd99d,0x3fdb,0x718f,0x3f4c,
-0xfe74,0x6030,0x1a07,0x3fa5,
-0x0000,0x0000,0x0000,0x3ff0,
-};
-#endif
-#ifdef MIEEE
-static unsigned short cn[24] = {
-0xbe6a,0xc80c,0x2cfb,0x62d3,
-0x3ee3,0xee92,0x6210,0x0f74,
-0xbf45,0x2442,0x0ed6,0x8786,
-0x3f93,0x566a,0x1059,0xcc13,
-0xbfca,0x4eac,0x378a,0x9690,
-0x3ff0,0x0000,0x0000,0x0000,
-};
-static unsigned short cd[28] = {
-0x3d91,0x9768,0x6a7f,0xc6b3,
-0x3e0f,0x7449,0xdb03,0x75b9,
-0x3e80,0xc708,0x02a4,0x5191,
-0x3ee9,0xa3ee,0xfa2a,0x1d32,
-0x3f4c,0x718f,0x3fdb,0xd99d,
-0x3fa5,0x1a07,0x6030,0xfe74,
-0x3ff0,0x0000,0x0000,0x0000,
-};
-#endif
-
-/* Auxiliary function f(x) */
-#ifdef UNK
-static double fn[10] = {
-  4.21543555043677546506E-1,
-  1.43407919780758885261E-1,
-  1.15220955073585758835E-2,
-  3.45017939782574027900E-4,
-  4.63613749287867322088E-6,
-  3.05568983790257605827E-8,
-  1.02304514164907233465E-10,
-  1.72010743268161828879E-13,
-  1.34283276233062758925E-16,
-  3.76329711269987889006E-20,
-};
-static double fd[10] = {
-/*  1.00000000000000000000E0,*/
-  7.51586398353378947175E-1,
-  1.16888925859191382142E-1,
-  6.44051526508858611005E-3,
-  1.55934409164153020873E-4,
-  1.84627567348930545870E-6,
-  1.12699224763999035261E-8,
-  3.60140029589371370404E-11,
-  5.88754533621578410010E-14,
-  4.52001434074129701496E-17,
-  1.25443237090011264384E-20,
-};
-#endif
-#ifdef DEC
-static unsigned short fn[40] = {
-0037727,0152216,0106601,0016214,
-0037422,0154606,0112710,0071355,
-0036474,0143453,0154253,0166545,
-0035264,0161606,0022250,0073743,
-0033633,0110036,0024653,0136246,
-0032003,0036652,0041164,0036413,
-0027740,0174122,0046305,0036726,
-0025501,0125270,0121317,0167667,
-0023032,0150555,0076175,0047443,
-0020061,0133570,0070130,0027657,
-};
-static unsigned short fd[40] = {
-/*0040200,0000000,0000000,0000000,*/
-0040100,0063767,0054413,0151452,
-0037357,0061566,0007243,0065754,
-0036323,0005365,0033552,0133625,
-0035043,0101123,0000275,0165402,
-0033367,0146614,0110623,0023647,
-0031501,0116644,0125222,0144263,
-0027436,0062051,0117235,0001411,
-0025204,0111543,0056370,0036201,
-0022520,0071351,0015227,0122144,
-0017554,0172240,0112713,0005006,
-};
-#endif
-#ifdef IBMPC
-static unsigned short fn[40] = {
-0x2391,0xd1b0,0xfa91,0x3fda,
-0x0e5e,0xd2b9,0x5b30,0x3fc2,
-0x7dad,0x7b15,0x98e5,0x3f87,
-0x0efc,0xc495,0x9c70,0x3f36,
-0x7795,0xc535,0x7203,0x3ed3,
-0x87a1,0x484e,0x67b5,0x3e60,
-0xa7bb,0x4998,0x1f0a,0x3ddc,
-0xfdf7,0x1459,0x3557,0x3d48,
-0xa9e4,0xaf8f,0x5a2d,0x3ca3,
-0x05f6,0x0e0b,0x36ef,0x3be6,
-};
-static unsigned short fd[40] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x7a65,0xeb21,0x0cfe,0x3fe8,
-0x6d7d,0xc1d4,0xec6e,0x3fbd,
-0x56f3,0xa6ed,0x615e,0x3f7a,
-0xbd60,0x6017,0x704a,0x3f24,
-0x64f5,0x9232,0xf9b1,0x3ebe,
-0x5916,0x9552,0x33b4,0x3e48,
-0xa061,0x33d3,0xcc85,0x3dc3,
-0x0790,0x6b9f,0x926c,0x3d30,
-0xf48d,0x2352,0x0e5d,0x3c8a,
-0x6141,0x12b9,0x9e94,0x3bcd,
-};
-#endif
-#ifdef MIEEE
-static unsigned short fn[40] = {
-0x3fda,0xfa91,0xd1b0,0x2391,
-0x3fc2,0x5b30,0xd2b9,0x0e5e,
-0x3f87,0x98e5,0x7b15,0x7dad,
-0x3f36,0x9c70,0xc495,0x0efc,
-0x3ed3,0x7203,0xc535,0x7795,
-0x3e60,0x67b5,0x484e,0x87a1,
-0x3ddc,0x1f0a,0x4998,0xa7bb,
-0x3d48,0x3557,0x1459,0xfdf7,
-0x3ca3,0x5a2d,0xaf8f,0xa9e4,
-0x3be6,0x36ef,0x0e0b,0x05f6,
-};
-static unsigned short fd[40] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x3fe8,0x0cfe,0xeb21,0x7a65,
-0x3fbd,0xec6e,0xc1d4,0x6d7d,
-0x3f7a,0x615e,0xa6ed,0x56f3,
-0x3f24,0x704a,0x6017,0xbd60,
-0x3ebe,0xf9b1,0x9232,0x64f5,
-0x3e48,0x33b4,0x9552,0x5916,
-0x3dc3,0xcc85,0x33d3,0xa061,
-0x3d30,0x926c,0x6b9f,0x0790,
-0x3c8a,0x0e5d,0x2352,0xf48d,
-0x3bcd,0x9e94,0x12b9,0x6141,
-};
-#endif
-
-
-/* Auxiliary function g(x) */
-#ifdef UNK
-static double gn[11] = {
-  5.04442073643383265887E-1,
-  1.97102833525523411709E-1,
-  1.87648584092575249293E-2,
-  6.84079380915393090172E-4,
-  1.15138826111884280931E-5,
-  9.82852443688422223854E-8,
-  4.45344415861750144738E-10,
-  1.08268041139020870318E-12,
-  1.37555460633261799868E-15,
-  8.36354435630677421531E-19,
-  1.86958710162783235106E-22,
-};
-static double gd[11] = {
-/*  1.00000000000000000000E0,*/
-  1.47495759925128324529E0,
-  3.37748989120019970451E-1,
-  2.53603741420338795122E-2,
-  8.14679107184306179049E-4,
-  1.27545075667729118702E-5,
-  1.04314589657571990585E-7,
-  4.60680728146520428211E-10,
-  1.10273215066240270757E-12,
-  1.38796531259578871258E-15,
-  8.39158816283118707363E-19,
-  1.86958710162783236342E-22,
-};
-#endif
-#ifdef DEC
-static unsigned short gn[44] = {
-0040001,0021435,0120406,0053123,
-0037511,0152523,0037703,0122011,
-0036631,0134302,0122721,0110235,
-0035463,0051712,0043215,0114732,
-0034101,0025677,0147725,0057630,
-0032323,0010342,0067523,0002206,
-0030364,0152247,0110007,0054107,
-0026230,0057654,0035464,0047124,
-0023706,0036401,0167705,0045440,
-0021166,0154447,0105632,0142461,
-0016142,0002353,0011175,0170530,
-};
-static unsigned short gd[44] = {
-/*0040200,0000000,0000000,0000000,*/
-0040274,0145551,0016742,0127005,
-0037654,0166557,0076416,0015165,
-0036717,0140217,0030675,0050111,
-0035525,0110060,0076405,0070502,
-0034125,0176061,0060120,0031730,
-0032340,0001615,0054343,0120501,
-0030375,0041414,0070747,0107060,
-0026233,0031034,0160757,0074526,
-0023710,0003341,0137100,0144664,
-0021167,0126414,0023774,0015435,
-0016142,0002353,0011175,0170530,
-};
-#endif
-#ifdef IBMPC
-static unsigned short gn[44] = {
-0xcaca,0xb420,0x2463,0x3fe0,
-0x7481,0x67f8,0x3aaa,0x3fc9,
-0x3214,0x54ba,0x3718,0x3f93,
-0xb33b,0x48d1,0x6a79,0x3f46,
-0xabf3,0xf9fa,0x2577,0x3ee8,
-0x6091,0x4dea,0x621c,0x3e7a,
-0xeb09,0xf200,0x9a94,0x3dfe,
-0x89cb,0x8766,0x0bf5,0x3d73,
-0xa964,0x3df8,0xc7a0,0x3cd8,
-0x58a6,0xf173,0xdb24,0x3c2e,
-0xbe2b,0x624f,0x409d,0x3b6c,
-};
-static unsigned short gd[44] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x55c1,0x23bc,0x996d,0x3ff7,
-0xc34f,0xefa1,0x9dad,0x3fd5,
-0xaa09,0xe637,0xf811,0x3f99,
-0xae28,0x0fa0,0xb206,0x3f4a,
-0x067b,0x2c0a,0xbf86,0x3eea,
-0x7428,0xab1c,0x0071,0x3e7c,
-0xf1c6,0x8e3c,0xa861,0x3dff,
-0xef2b,0x9c3d,0x6643,0x3d73,
-0x1936,0x37c8,0x00dc,0x3cd9,
-0x8364,0x84ff,0xf5a1,0x3c2e,
-0xbe2b,0x624f,0x409d,0x3b6c,
-};
-#endif
-#ifdef MIEEE
-static unsigned short gn[44] = {
-0x3fe0,0x2463,0xb420,0xcaca,
-0x3fc9,0x3aaa,0x67f8,0x7481,
-0x3f93,0x3718,0x54ba,0x3214,
-0x3f46,0x6a79,0x48d1,0xb33b,
-0x3ee8,0x2577,0xf9fa,0xabf3,
-0x3e7a,0x621c,0x4dea,0x6091,
-0x3dfe,0x9a94,0xf200,0xeb09,
-0x3d73,0x0bf5,0x8766,0x89cb,
-0x3cd8,0xc7a0,0x3df8,0xa964,
-0x3c2e,0xdb24,0xf173,0x58a6,
-0x3b6c,0x409d,0x624f,0xbe2b,
-};
-static unsigned short gd[44] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x3ff7,0x996d,0x23bc,0x55c1,
-0x3fd5,0x9dad,0xefa1,0xc34f,
-0x3f99,0xf811,0xe637,0xaa09,
-0x3f4a,0xb206,0x0fa0,0xae28,
-0x3eea,0xbf86,0x2c0a,0x067b,
-0x3e7c,0x0071,0xab1c,0x7428,
-0x3dff,0xa861,0x8e3c,0xf1c6,
-0x3d73,0x6643,0x9c3d,0xef2b,
-0x3cd9,0x00dc,0x37c8,0x1936,
-0x3c2e,0xf5a1,0x84ff,0x8364,
-0x3b6c,0x409d,0x624f,0xbe2b,
-};
-#endif
-
-#ifdef ANSIPROT
-extern "C" {double fabs ( double );}
-extern "C" {double cos ( double );}
-extern "C" {double sin ( double );}
-extern "C" {double polevl ( double, double *, int );}
-extern "C" {double p1evl ( double, double *, int );}
-#else
-double fabs(), cos(), sin(), polevl(), p1evl();
-#endif
-extern "C" {extern double PI, PIO2, MACHEP;}
-
-//int fresnl( xxa, ssa, cca )
-//double xxa, *ssa, *cca;
-
-int fresnl( double xxa, double *ssa, double *cca )
-{
-double f, g, cc, ss, c, s, t, u;
-double x, x2;
-
-x = fabs(xxa);
-x2 = x * x;
-if( x2 < 2.5625 )
-	{
-	t = x2 * x2;
-	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
-	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
-	goto done;
-	}
-
-
-
-
-
-
-if( x > 36974.0 )
-	{
-	cc = 0.5;
-	ss = 0.5;
-	goto done;
-	}
-
-
-/*		Asymptotic power series auxiliary functions
- *		for large argument
- */
-	x2 = x * x;
-	t = PI * x2;
-	u = 1.0/(t * t);
-	t = 1.0/t;
-	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
-	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
-
-	t = PIO2 * x2;
-	c = cos(t);
-	s = sin(t);
-	t = PI * x;
-	cc = 0.5  +  (f * s  -  g * c)/t;
-	ss = 0.5  -  (f * c  +  g * s)/t;
-
-done:
-if( xxa < 0.0 )
-	{
-	cc = -cc;
-	ss = -ss;
-	}
-
-*cca = cc;
-*ssa = ss;
-return(0);
-}
+/*							fresnl.c
+ *
+ *	Fresnel integral
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * double x, S, C;
+ * void fresnl();
+ *
+ * fresnl( x, _&S, _&C );
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates the Fresnel integrals
+ *
+ *           x
+ *           -
+ *          | |
+ * C(x) =   |   cos(pi/2 t**2) dt,
+ *        | |
+ *         -
+ *          0
+ *
+ *           x
+ *           -
+ *          | |
+ * S(x) =   |   sin(pi/2 t**2) dt.
+ *        | |
+ *         -
+ *          0
+ *
+ *
+ * The integrals are evaluated by a power series for x < 1.
+ * For x >= 1 auxiliary functions f(x) and g(x) are employed
+ * such that
+ *
+ * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
+ * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
+ *
+ *
+ *
+ * ACCURACY:
+ *
+ *  Relative error.
+ *
+ * Arithmetic  function   domain     # trials      peak         rms
+ *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
+ *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
+ *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
+ *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
+ */
+
+/*
+Cephes Math Library Release 2.8:  June, 2000
+Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+
+
+/* S(x) for small x */
+#ifdef UNK
+static double sn[6] = {
+-2.99181919401019853726E3,
+ 7.08840045257738576863E5,
+-6.29741486205862506537E7,
+ 2.54890880573376359104E9,
+-4.42979518059697779103E10,
+ 3.18016297876567817986E11,
+};
+static double sd[6] = {
+/* 1.00000000000000000000E0,*/
+ 2.81376268889994315696E2,
+ 4.55847810806532581675E4,
+ 5.17343888770096400730E6,
+ 4.19320245898111231129E8,
+ 2.24411795645340920940E10,
+ 6.07366389490084639049E11,
+};
+#endif
+#ifdef DEC
+static unsigned short sn[24] = {
+0143072,0176433,0065455,0127034,
+0045055,0007200,0134540,0026661,
+0146560,0035061,0023667,0127545,
+0050027,0166503,0002673,0153756,
+0151045,0002721,0121737,0102066,
+0051624,0013177,0033451,0021271,
+};
+static unsigned short sd[24] = {
+/*0040200,0000000,0000000,0000000,*/
+0042214,0130051,0112070,0101617,
+0044062,0010307,0172346,0152510,
+0045635,0160575,0143200,0136642,
+0047307,0171215,0127457,0052361,
+0050647,0031447,0032621,0013510,
+0052015,0064733,0117362,0012653,
+};
+#endif
+#ifdef IBMPC
+static unsigned short sn[24] = {
+0xb5c3,0x6d65,0x5fa3,0xc0a7,
+0x05b6,0x172c,0xa1d0,0x4125,
+0xf5ed,0x24f6,0x0746,0xc18e,
+0x7afe,0x60b7,0xfda8,0x41e2,
+0xf087,0x347b,0xa0ba,0xc224,
+0x2457,0xe6e5,0x82cf,0x4252,
+};
+static unsigned short sd[24] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x1072,0x3287,0x9605,0x4071,
+0xdaa9,0xfe9c,0x4218,0x40e6,
+0x17b4,0xb8d0,0xbc2f,0x4153,
+0xea9e,0xb5e5,0xfe51,0x41b8,
+0x22e9,0xe6b2,0xe664,0x4214,
+0x42b5,0x73de,0xad3b,0x4261,
+};
+#endif
+#ifdef MIEEE
+static unsigned short sn[24] = {
+0xc0a7,0x5fa3,0x6d65,0xb5c3,
+0x4125,0xa1d0,0x172c,0x05b6,
+0xc18e,0x0746,0x24f6,0xf5ed,
+0x41e2,0xfda8,0x60b7,0x7afe,
+0xc224,0xa0ba,0x347b,0xf087,
+0x4252,0x82cf,0xe6e5,0x2457,
+};
+static unsigned short sd[24] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x4071,0x9605,0x3287,0x1072,
+0x40e6,0x4218,0xfe9c,0xdaa9,
+0x4153,0xbc2f,0xb8d0,0x17b4,
+0x41b8,0xfe51,0xb5e5,0xea9e,
+0x4214,0xe664,0xe6b2,0x22e9,
+0x4261,0xad3b,0x73de,0x42b5,
+};
+#endif
+
+/* C(x) for small x */
+#ifdef UNK
+static double cn[6] = {
+-4.98843114573573548651E-8,
+ 9.50428062829859605134E-6,
+-6.45191435683965050962E-4,
+ 1.88843319396703850064E-2,
+-2.05525900955013891793E-1,
+ 9.99999999999999998822E-1,
+};
+static double cd[7] = {
+ 3.99982968972495980367E-12,
+ 9.15439215774657478799E-10,
+ 1.25001862479598821474E-7,
+ 1.22262789024179030997E-5,
+ 8.68029542941784300606E-4,
+ 4.12142090722199792936E-2,
+ 1.00000000000000000118E0,
+};
+#endif
+#ifdef DEC
+static unsigned short cn[24] = {
+0132126,0040141,0063733,0013231,
+0034037,0072223,0010200,0075637,
+0135451,0021020,0073264,0036057,
+0036632,0131520,0101316,0060233,
+0137522,0072541,0136124,0132202,
+0040200,0000000,0000000,0000000,
+};
+static unsigned short cd[28] = {
+0026614,0135503,0051776,0032631,
+0030573,0121116,0154033,0126712,
+0032406,0034100,0012442,0106212,
+0034115,0017567,0150520,0164623,
+0035543,0106171,0177336,0146351,
+0037050,0150073,0000607,0171635,
+0040200,0000000,0000000,0000000,
+};
+#endif
+#ifdef IBMPC
+static unsigned short cn[24] = {
+0x62d3,0x2cfb,0xc80c,0xbe6a,
+0x0f74,0x6210,0xee92,0x3ee3,
+0x8786,0x0ed6,0x2442,0xbf45,
+0xcc13,0x1059,0x566a,0x3f93,
+0x9690,0x378a,0x4eac,0xbfca,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+static unsigned short cd[28] = {
+0xc6b3,0x6a7f,0x9768,0x3d91,
+0x75b9,0xdb03,0x7449,0x3e0f,
+0x5191,0x02a4,0xc708,0x3e80,
+0x1d32,0xfa2a,0xa3ee,0x3ee9,
+0xd99d,0x3fdb,0x718f,0x3f4c,
+0xfe74,0x6030,0x1a07,0x3fa5,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+#endif
+#ifdef MIEEE
+static unsigned short cn[24] = {
+0xbe6a,0xc80c,0x2cfb,0x62d3,
+0x3ee3,0xee92,0x6210,0x0f74,
+0xbf45,0x2442,0x0ed6,0x8786,
+0x3f93,0x566a,0x1059,0xcc13,
+0xbfca,0x4eac,0x378a,0x9690,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+static unsigned short cd[28] = {
+0x3d91,0x9768,0x6a7f,0xc6b3,
+0x3e0f,0x7449,0xdb03,0x75b9,
+0x3e80,0xc708,0x02a4,0x5191,
+0x3ee9,0xa3ee,0xfa2a,0x1d32,
+0x3f4c,0x718f,0x3fdb,0xd99d,
+0x3fa5,0x1a07,0x6030,0xfe74,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+#endif
+
+/* Auxiliary function f(x) */
+#ifdef UNK
+static double fn[10] = {
+  4.21543555043677546506E-1,
+  1.43407919780758885261E-1,
+  1.15220955073585758835E-2,
+  3.45017939782574027900E-4,
+  4.63613749287867322088E-6,
+  3.05568983790257605827E-8,
+  1.02304514164907233465E-10,
+  1.72010743268161828879E-13,
+  1.34283276233062758925E-16,
+  3.76329711269987889006E-20,
+};
+static double fd[10] = {
+/*  1.00000000000000000000E0,*/
+  7.51586398353378947175E-1,
+  1.16888925859191382142E-1,
+  6.44051526508858611005E-3,
+  1.55934409164153020873E-4,
+  1.84627567348930545870E-6,
+  1.12699224763999035261E-8,
+  3.60140029589371370404E-11,
+  5.88754533621578410010E-14,
+  4.52001434074129701496E-17,
+  1.25443237090011264384E-20,
+};
+#endif
+#ifdef DEC
+static unsigned short fn[40] = {
+0037727,0152216,0106601,0016214,
+0037422,0154606,0112710,0071355,
+0036474,0143453,0154253,0166545,
+0035264,0161606,0022250,0073743,
+0033633,0110036,0024653,0136246,
+0032003,0036652,0041164,0036413,
+0027740,0174122,0046305,0036726,
+0025501,0125270,0121317,0167667,
+0023032,0150555,0076175,0047443,
+0020061,0133570,0070130,0027657,
+};
+static unsigned short fd[40] = {
+/*0040200,0000000,0000000,0000000,*/
+0040100,0063767,0054413,0151452,
+0037357,0061566,0007243,0065754,
+0036323,0005365,0033552,0133625,
+0035043,0101123,0000275,0165402,
+0033367,0146614,0110623,0023647,
+0031501,0116644,0125222,0144263,
+0027436,0062051,0117235,0001411,
+0025204,0111543,0056370,0036201,
+0022520,0071351,0015227,0122144,
+0017554,0172240,0112713,0005006,
+};
+#endif
+#ifdef IBMPC
+static unsigned short fn[40] = {
+0x2391,0xd1b0,0xfa91,0x3fda,
+0x0e5e,0xd2b9,0x5b30,0x3fc2,
+0x7dad,0x7b15,0x98e5,0x3f87,
+0x0efc,0xc495,0x9c70,0x3f36,
+0x7795,0xc535,0x7203,0x3ed3,
+0x87a1,0x484e,0x67b5,0x3e60,
+0xa7bb,0x4998,0x1f0a,0x3ddc,
+0xfdf7,0x1459,0x3557,0x3d48,
+0xa9e4,0xaf8f,0x5a2d,0x3ca3,
+0x05f6,0x0e0b,0x36ef,0x3be6,
+};
+static unsigned short fd[40] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x7a65,0xeb21,0x0cfe,0x3fe8,
+0x6d7d,0xc1d4,0xec6e,0x3fbd,
+0x56f3,0xa6ed,0x615e,0x3f7a,
+0xbd60,0x6017,0x704a,0x3f24,
+0x64f5,0x9232,0xf9b1,0x3ebe,
+0x5916,0x9552,0x33b4,0x3e48,
+0xa061,0x33d3,0xcc85,0x3dc3,
+0x0790,0x6b9f,0x926c,0x3d30,
+0xf48d,0x2352,0x0e5d,0x3c8a,
+0x6141,0x12b9,0x9e94,0x3bcd,
+};
+#endif
+#ifdef MIEEE
+static unsigned short fn[40] = {
+0x3fda,0xfa91,0xd1b0,0x2391,
+0x3fc2,0x5b30,0xd2b9,0x0e5e,
+0x3f87,0x98e5,0x7b15,0x7dad,
+0x3f36,0x9c70,0xc495,0x0efc,
+0x3ed3,0x7203,0xc535,0x7795,
+0x3e60,0x67b5,0x484e,0x87a1,
+0x3ddc,0x1f0a,0x4998,0xa7bb,
+0x3d48,0x3557,0x1459,0xfdf7,
+0x3ca3,0x5a2d,0xaf8f,0xa9e4,
+0x3be6,0x36ef,0x0e0b,0x05f6,
+};
+static unsigned short fd[40] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3fe8,0x0cfe,0xeb21,0x7a65,
+0x3fbd,0xec6e,0xc1d4,0x6d7d,
+0x3f7a,0x615e,0xa6ed,0x56f3,
+0x3f24,0x704a,0x6017,0xbd60,
+0x3ebe,0xf9b1,0x9232,0x64f5,
+0x3e48,0x33b4,0x9552,0x5916,
+0x3dc3,0xcc85,0x33d3,0xa061,
+0x3d30,0x926c,0x6b9f,0x0790,
+0x3c8a,0x0e5d,0x2352,0xf48d,
+0x3bcd,0x9e94,0x12b9,0x6141,
+};
+#endif
+
+
+/* Auxiliary function g(x) */
+#ifdef UNK
+static double gn[11] = {
+  5.04442073643383265887E-1,
+  1.97102833525523411709E-1,
+  1.87648584092575249293E-2,
+  6.84079380915393090172E-4,
+  1.15138826111884280931E-5,
+  9.82852443688422223854E-8,
+  4.45344415861750144738E-10,
+  1.08268041139020870318E-12,
+  1.37555460633261799868E-15,
+  8.36354435630677421531E-19,
+  1.86958710162783235106E-22,
+};
+static double gd[11] = {
+/*  1.00000000000000000000E0,*/
+  1.47495759925128324529E0,
+  3.37748989120019970451E-1,
+  2.53603741420338795122E-2,
+  8.14679107184306179049E-4,
+  1.27545075667729118702E-5,
+  1.04314589657571990585E-7,
+  4.60680728146520428211E-10,
+  1.10273215066240270757E-12,
+  1.38796531259578871258E-15,
+  8.39158816283118707363E-19,
+  1.86958710162783236342E-22,
+};
+#endif
+#ifdef DEC
+static unsigned short gn[44] = {
+0040001,0021435,0120406,0053123,
+0037511,0152523,0037703,0122011,
+0036631,0134302,0122721,0110235,
+0035463,0051712,0043215,0114732,
+0034101,0025677,0147725,0057630,
+0032323,0010342,0067523,0002206,
+0030364,0152247,0110007,0054107,
+0026230,0057654,0035464,0047124,
+0023706,0036401,0167705,0045440,
+0021166,0154447,0105632,0142461,
+0016142,0002353,0011175,0170530,
+};
+static unsigned short gd[44] = {
+/*0040200,0000000,0000000,0000000,*/
+0040274,0145551,0016742,0127005,
+0037654,0166557,0076416,0015165,
+0036717,0140217,0030675,0050111,
+0035525,0110060,0076405,0070502,
+0034125,0176061,0060120,0031730,
+0032340,0001615,0054343,0120501,
+0030375,0041414,0070747,0107060,
+0026233,0031034,0160757,0074526,
+0023710,0003341,0137100,0144664,
+0021167,0126414,0023774,0015435,
+0016142,0002353,0011175,0170530,
+};
+#endif
+#ifdef IBMPC
+static unsigned short gn[44] = {
+0xcaca,0xb420,0x2463,0x3fe0,
+0x7481,0x67f8,0x3aaa,0x3fc9,
+0x3214,0x54ba,0x3718,0x3f93,
+0xb33b,0x48d1,0x6a79,0x3f46,
+0xabf3,0xf9fa,0x2577,0x3ee8,
+0x6091,0x4dea,0x621c,0x3e7a,
+0xeb09,0xf200,0x9a94,0x3dfe,
+0x89cb,0x8766,0x0bf5,0x3d73,
+0xa964,0x3df8,0xc7a0,0x3cd8,
+0x58a6,0xf173,0xdb24,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+static unsigned short gd[44] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x55c1,0x23bc,0x996d,0x3ff7,
+0xc34f,0xefa1,0x9dad,0x3fd5,
+0xaa09,0xe637,0xf811,0x3f99,
+0xae28,0x0fa0,0xb206,0x3f4a,
+0x067b,0x2c0a,0xbf86,0x3eea,
+0x7428,0xab1c,0x0071,0x3e7c,
+0xf1c6,0x8e3c,0xa861,0x3dff,
+0xef2b,0x9c3d,0x6643,0x3d73,
+0x1936,0x37c8,0x00dc,0x3cd9,
+0x8364,0x84ff,0xf5a1,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+#endif
+#ifdef MIEEE
+static unsigned short gn[44] = {
+0x3fe0,0x2463,0xb420,0xcaca,
+0x3fc9,0x3aaa,0x67f8,0x7481,
+0x3f93,0x3718,0x54ba,0x3214,
+0x3f46,0x6a79,0x48d1,0xb33b,
+0x3ee8,0x2577,0xf9fa,0xabf3,
+0x3e7a,0x621c,0x4dea,0x6091,
+0x3dfe,0x9a94,0xf200,0xeb09,
+0x3d73,0x0bf5,0x8766,0x89cb,
+0x3cd8,0xc7a0,0x3df8,0xa964,
+0x3c2e,0xdb24,0xf173,0x58a6,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+static unsigned short gd[44] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3ff7,0x996d,0x23bc,0x55c1,
+0x3fd5,0x9dad,0xefa1,0xc34f,
+0x3f99,0xf811,0xe637,0xaa09,
+0x3f4a,0xb206,0x0fa0,0xae28,
+0x3eea,0xbf86,0x2c0a,0x067b,
+0x3e7c,0x0071,0xab1c,0x7428,
+0x3dff,0xa861,0x8e3c,0xf1c6,
+0x3d73,0x6643,0x9c3d,0xef2b,
+0x3cd9,0x00dc,0x37c8,0x1936,
+0x3c2e,0xf5a1,0x84ff,0x8364,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+#endif
+
+#ifdef ANSIPROT
+extern "C" {double fabs ( double );}
+extern "C" {double cos ( double );}
+extern "C" {double sin ( double );}
+extern "C" {double polevl ( double, double *, int );}
+extern "C" {double p1evl ( double, double *, int );}
+#else
+double fabs(), cos(), sin(), polevl(), p1evl();
+#endif
+extern "C" {extern double PI, PIO2, MACHEP;}
+
+//int fresnl( xxa, ssa, cca )
+//double xxa, *ssa, *cca;
+
+int fresnl( double xxa, double *ssa, double *cca )
+{
+double f, g, cc, ss, c, s, t, u;
+double x, x2;
+
+x = fabs(xxa);
+x2 = x * x;
+if( x2 < 2.5625 )
+	{
+	t = x2 * x2;
+	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
+	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
+	goto done;
+	}
+
+
+
+
+
+
+if( x > 36974.0 )
+	{
+	cc = 0.5;
+	ss = 0.5;
+	goto done;
+	}
+
+
+/*		Asymptotic power series auxiliary functions
+ *		for large argument
+ */
+	x2 = x * x;
+	t = PI * x2;
+	u = 1.0/(t * t);
+	t = 1.0/t;
+	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
+	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
+
+	t = PIO2 * x2;
+	c = cos(t);
+	s = sin(t);
+	t = PI * x;
+	cc = 0.5  +  (f * s  -  g * c)/t;
+	ss = 0.5  -  (f * c  +  g * s)/t;
+
+done:
+if( xxa < 0.0 )
+	{
+	cc = -cc;
+	ss = -ss;
+	}
+
+*cca = cc;
+*ssa = ss;
+return(0);
+}

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1,3179 +1,3028 @@
-#include "globalplan.h"
-#include "limits"
-#include <iostream>
-
-#include <Eigen/Dense>
-#include <Eigen/Cholesky>
-#include <Eigen/LU>
-#include <Eigen/QR>
-#include <Eigen/SVD>
-
-#include <QDebug>
-#include <QPointF>
-
-
-using namespace  Eigen;
-
-extern "C"
-{
-#include "dubins.h"
-}
-
-
-static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
-/**
- * @brief GetLineDis 获得点到直线Geometry的距离。
- * @param pline
- * @param x
- * @param y
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
-                         double & neary,double & nearhead)
-{
-    double fRtn = 1000.0;
-
-    double a1,a2,a3,a4,b1,b2;
-    double ratio = pline->GetHdg();
-    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
-    while(ratio<0)ratio = ratio+2.0*M_PI;
-
-    double dis1,dis2,dis3;
-    double x1,x2,x3,y1,y2,y3;
-    x1 = pline->GetX();y1=pline->GetY();
-    if((ratio == 0)||(ratio == M_PI))
-    {
-        a1 = 0;a4=0;
-        a2 = 1;b1= pline->GetY();
-        a3 = 1;b2= x;
-    }
-    else
-    {
-        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
-        {
-            a2=0;a3=0;
-            a1=1,b1=pline->GetX();
-            a4 = 1;b2 = y;
-        }
-        else
-        {
-            a1 = tan(ratio) *(-1.0);
-            a2 = 1;
-            a3 = tan(ratio+M_PI/2.0)*(-1.0);
-            a4 = 1;
-            b1 = a1*pline->GetX() + a2 * pline->GetY();
-            b2 = a3*x+a4*y;
-        }
-    }
-
-    y2 = y1 + pline->GetLength() * sin(ratio);
-    x2 = x1 + pline->GetLength() * cos(ratio);
-
-    Eigen::Matrix2d A;
-    A<<a1,a2,
-            a3,a4;
-    Eigen::Vector2d B(b1,b2);
-
-    Eigen::Vector2d opoint  = A.lu().solve(B);
-
- //   std::cout<<opoint<<std::endl;
-
-    x3 = opoint(0);
-    y3 = opoint(1);
-
-    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
-    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
-    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
-
-//    std::cout<<" dis 1 is "<<dis1<<std::endl;
-//    std::cout<<" dis 2 is "<<dis2<<std::endl;
-//    std::cout<<" dis 3 is "<<dis3<<std::endl;
-
-    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
-    {
- //       std::cout<<" out line"<<std::endl;
-        if(dis1<dis2)
-        {
-            fRtn = dis1;
-            nearx = x1;neary=y1;nearhead = pline->GetHdg();
-        }
-        else
-        {
-            fRtn = dis2;
-            nearx = x2;neary=y2;nearhead = pline->GetHdg();
-        }
-    }
-    else
-    {
-        fRtn = dis3;
-        nearx = x3;neary=y3;nearhead = pline->GetHdg();
-    }
-
-
-//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
-    return fRtn;
-
-
-
-}
-
-
-static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
-{
-     double m=0;
-     double k;
-     double b;
-
-      //计算分子
-      m=startPos.x()-endPos.x();
-
-      //求直线的方程
-      if(0==m)
-      {
-          k=100000;
-          b=startPos.y()-k*startPos.x();
-      }
-      else
-      {
-          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
-          b=startPos.y()-k*startPos.x();
-      }
-//      qDebug()<<k<<b;
-
-     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
-      //求直线与圆的交点 前提是圆需要与直线有交点
-     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
-      {
-          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
-          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
-          point1.setY(k*point1.x()+b);
-          point2.setY(k*point2.x()+b);
-          return 0;
-      }
-
-     return -1;
-}
-
-
-/**
-  * @brief CalcHdg
-  * 计算点0到点1的航向
-  * @param p0        Point 0
-  * @param p1        Point 1
-**/
-static double CalcHdg(QPointF p0, QPointF p1)
-{
-    double x0,y0,x1,y1;
-    x0 = p0.x();
-    y0 = p0.y();
-    x1 = p1.x();
-    y1 = p1.y();
-    if(x0 == x1)
-    {
-        if(y0 < y1)
-        {
-            return M_PI/2.0;
-        }
-        else
-            return M_PI*3.0/2.0;
-    }
-
-    double ratio = (y1-y0)/(x1-x0);
-
-    double hdg = atan(ratio);
-
-    if(ratio > 0)
-    {
-        if(y1 > y0)
-        {
-
-        }
-        else
-        {
-            hdg = hdg + M_PI;
-        }
-    }
-    else
-    {
-        if(y1 > y0)
-        {
-            hdg = hdg + M_PI;
-        }
-        else
-        {
-            hdg = hdg + 2.0*M_PI;
-        }
-    }
-
-    return hdg;
-}
-
-
-
-static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
-{
-    double hdg = CalcHdg(poingarc,point1);
-    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
-    else hdg = hdg - M_PI/2.0;
-    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
-    if(hdg < 0)hdg = hdg + 2.0*M_PI;
-
-    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
-
-    double hdgdiff = hdg - parc->GetHdg();
-
-    if(hdgrange >= 0 )
-    {
-        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
-    }
-    else
-    {
-        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
-    }
-
-    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
-    return false;
-
-}
-
-static inline double calcpointdis(QPointF p1,QPointF p2)
-{
-    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
-}
-
-/**
-  * @brief GetArcDis
-  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
-  * @param parc        pointer to a arc geomery
-  * @param x           current x
-  * @param y           current y
-  * @param nearx       near x
-  * @param neary       near y
-  * @param nearhead    nearhead
-**/
-
-static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
-                        double & neary,double & nearhead)
-{
-
- //   double fRtn = 1000.0;
-    if(parc->GetCurvature() == 0.0)return 1000.0;
-
-    double R = fabs(1.0/parc->GetCurvature());
-
-//    if(parc->GetX() == 4.8213842690322309e+01)
-//    {
-//        int a = 1;
-//        a = 3;
-//    }
-//    if(parc->GetCurvature() == -0.0000110203)
-//    {
-//        int a = 1;
-//        a = 3;
-//    }
-
-    //calculate arc center
-    double x_center,y_center;
-    if(parc->GetCurvature() > 0)
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
-    }
-    else
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
-    }
-
-    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
-
-
-    QPointF arcpoint;
-    arcpoint.setX(x_center);arcpoint.setY(y_center);
-
-    QPointF pointnow;
-    pointnow.setX(x);pointnow.setY(y);
-    QPointF point1,point2;
-    point1.setX(x_center + (R * cos(hdgltoa)));
-    point1.setY(y_center + (R * sin(hdgltoa)));
-    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
-    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
-
-    //calculat dis
-    bool bp1inarc,bp2inarc;
-    bp1inarc =pointinarc(parc,arcpoint,point1);
-    bp2inarc =pointinarc(parc,arcpoint,point2);
-    double fdis[4];
-    fdis[0] = calcpointdis(pointnow,point1);
-    fdis[1] = calcpointdis(pointnow,point2);
-    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
-    QPointF pointend;
-    double hdgrange = parc->GetLength()*parc->GetCurvature();
-    double hdgend = parc->GetHdg() + hdgrange;
-    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
-    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
-
-    if(parc->GetCurvature() >0)
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
-    }
-    else
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
-    }
-
-    fdis[3] = calcpointdis(pointnow,pointend);
-    int indexmin = -1;
-    double fdismin = 1000000.0;
-    if(bp1inarc)
-    {
-        indexmin = 0;fdismin = fdis[0];
-    }
-    if(bp2inarc)
-    {
-        if(indexmin == -1)
-        {
-            indexmin = 1;fdismin = fdis[1];
-        }
-        else
-        {
-            if(fdis[1]<fdismin)
-            {
-                indexmin = 1;fdismin = fdis[1];
-            }
-        }
-    }
-    if(indexmin == -1)
-    {
-        indexmin = 2;fdismin = fdis[2];
-    }
-    else
-    {
-        if(fdis[2]<fdismin)
-        {
-            indexmin = 2;fdismin = fdis[2];
-        }
-    }
-    if(fdis[3]<fdismin)
-    {
-        indexmin = 3;fdismin = fdis[3];
-    }
-
-    switch (indexmin) {
-    case 0:
-        nearx = point1.x();
-        neary = point1.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
-        }
-        break;
-    case 1:
-        nearx = point2.x();
-        neary = point2.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
-        }
-        break;
-    case 2:
-        nearx = parc->GetX();
-        neary = parc->GetY();
-        nearhead = parc->GetHdg();
-        break;
-    case 3:
-        nearx = pointend.x();
-        neary = pointend.y();
-        nearhead = hdgend;
-        break;
-    default:
-        std::cout<<"error in arcdis "<<std::endl;
-        break;
-    }
-
-    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
-    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-    return fdismin;
-}
-
-
-static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-
-    double x,y,hdg;
-    double s = 0.0;
-    double fdismin = 100000.0;
-    double s0 = pspiral->GetS();
-
-    while(s<pspiral->GetLength())
-    {
-        pspiral->GetCoords(s0+s,x,y,hdg);
-        s = s+0.1;
-
-        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-    }
-
-    return fdismin;
-}
-
-/**
- * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
- * @param parc
- * @param xnow
- * @param ynow
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-
-    double s = 0.1;
-    double fdismin = 100000.0;
-
-    double xold,yold;
-    xold = parc->GetX();
-    yold = parc->GetY();
-
-    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
-    if(fdis<fdismin)
-    {
-        fdismin = fdis;
-        nearhead = parc->GetHdg();
-        nearx = parc->GetX();
-        neary = parc->GetY();
-    }
-
-    double s_start = parc->GetS();
-
-    while(s < parc->GetLength())
-    {
-
-        double x, y,hdg;//xtem,ytem;
-        parc->GetCoords(s_start+s,x,y,hdg);
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-
-//        xold = x;
-//        yold = y;
-        s = s+ 0.1;
-    }
-
-    return fdismin;
-
-}
-
-
-static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-    double x,y,hdg;
-//    double s = 0.0;
-    double fdismin = 100000.0;
- //   double s0 = ppoly->GetS();
-
-    x = ppoly->GetX();
-    y = ppoly->GetY();
-    double A,B,C,D;
-    A = ppoly->GetA();
-    B = ppoly->GetB();
-    C = ppoly->GetC();
-    D = ppoly->GetD();
-    const double steplim = 0.3;
-    double du = steplim;
-    double u = 0;
-    double v = 0;
-    double oldx,oldy;
-    oldx = x;
-    oldy = y;
-    double xstart,ystart;
-    xstart = x;
-    ystart = y;
-
-
-    double hdgstart = ppoly->GetHdg();
-    double flen = 0;
-    u = u+du;
-    while(flen < ppoly->GetLength())
-    {
-        double fdis = 0;
-        v = A + B*u + C*u*u + D*u*u*u;
-        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
-        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
-        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
-
-        if(fdis>(steplim*2.0))du = du/2.0;
-        flen = flen + fdis;
-        u = u + du;
-        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
-
-        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdisnow<fdismin)
-        {
-            fdismin = fdisnow;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-
-        oldx = x;
-        oldy = y;
-    }
-
-    return fdismin;
-}
-
-
-/**
-  * @brief GetNearPoint
-  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
-  * @param x           current x
-  * @param y           current y
-  * @param pxodr       OpenDrive
-  * @param pObjRoad    Road
-  * @param pgeo        Geometry of road
-  * @param nearx       near x
-  * @param neary       near y
-  * @param nearhead    nearhead
-  * @param nearthresh  near threshhold
-  * @retval            if == 0 successfull  <0 fail.
-**/
-int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh,double &froads)
-{
-
-    double dismin = std::numeric_limits<double>::infinity();
-    s = dismin;
-    int i;
-    double frels;
-    for(i=0;i<pxodr->GetRoadCount();i++)
-    {
-        int j;
-        Road * proad = pxodr->GetRoad(i);
-        double nx,ny,nh;
-
-        for(j=0;j<proad->GetGeometryBlockCount();j++)
-        {
-            GeometryBlock * pgb = proad->GetGeometryBlock(j);
-            double dis;
-            RoadGeometry * pg;
-            pg = pgb->GetGeometryAt(0);
-
-            switch (pg->GetGeomType()) {
-            case 0:   //line
-                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
- //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
-                break;
-            case 1:
-                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 2:  //arc
-                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
-                break;
-
-            case 3:
-                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 4:
-                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            default:
-                dis = 100000.0;
-                break;
-            }
-
-            if(dis < dismin)
-            {
-                dismin = dis;
-                nearx = nx;
-                neary = ny;
-                nearhead = nh;
-                s = dis;
-                froads = frels;
-                *pObjRoad = proad;
-                *pgeo = pgb;
-            }
-
-
-//            if(pgb->CheckIfLine())
-//            {
-//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
-
-//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//            }
-//            else
-//            {
-//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
-//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
-//                dis = GetArcDis(parc,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
-//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//            }
-
-        }
-    }
-    if(s > nearthresh)return -1;
-    return 0;
-}
-
-
-int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh,double &froads)
-{
-
-    double dismin = std::numeric_limits<double>::infinity();
-    s = dismin;
-    int i;
-    double frels;
-    std::vector<Road * > xvectorroad;
-    std::vector<double > xvectordismin;
-    std::vector<double > xvecotrnearx;
-    std::vector<double > xvectorneary;
-    std::vector<double > xvectornearhead;
-    std::vector<double > xvectors;
-    std::vector<GeometryBlock *> xvectorgeob;
-    std::vector<double > xvectorfrels;
-    double VECTORTHRESH = 5;
-    for(i=0;i<pxodr->GetRoadCount();i++)
-    {
-        int j;
-        Road * proad = pxodr->GetRoad(i);
-        double nx,ny,nh;
-
-        bool bchange = false;
-        double localdismin = std::numeric_limits<double>::infinity();
-
-        double localnearx = 0;
-        double localneary = 0;
-        double localnearhead = 0;
-        double locals = 0;
-        double localfrels = 0;
-        GeometryBlock * pgeolocal;
-
-        for(j=0;j<proad->GetGeometryBlockCount();j++)
-        {
-            GeometryBlock * pgb = proad->GetGeometryBlock(j);
-            double dis;
-            RoadGeometry * pg;
-            pg = pgb->GetGeometryAt(0);
-
-            switch (pg->GetGeomType()) {
-            case 0:   //line
-                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
-                break;
-            case 1:
-                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 2:  //arc
-                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
-                break;
-
-            case 3:
-                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 4:
-                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            default:
-                dis = 100000.0;
-                break;
-            }
-
-            if(dis < dismin)
-            {
-                dismin = dis;
-                nearx = nx;
-                neary = ny;
-                nearhead = nh;
-                s = dis;
-                froads = frels;
-   //             qDebug("rels s is %f ",frels);
-                *pObjRoad = proad;
-                *pgeo = pgb;
-
-            }
-
-            if((dis<VECTORTHRESH) &&(dis<localdismin))
-            {
-                localdismin = dis;
-                localnearx = nx;
-                localneary = ny;
-                localnearhead = nh;
-                locals = dis;
-                localfrels = frels;
-                pgeolocal = pgb;
-                bchange = true;
-            }
-
-
-
-
-        }
-
-        if(bchange)
-        {
-            xvectorroad.push_back(proad);
-            xvecotrnearx.push_back(localnearx);
-            xvectorneary.push_back(localneary);
-            xvectordismin.push_back(localdismin);
-            xvectors.push_back(locals);
-            xvectorgeob.push_back(pgeolocal);
-            xvectornearhead.push_back(localnearhead);
-            xvectorfrels.push_back(localfrels);
-        }
-    }
-
-
-    if(s > nearthresh)return -1;
-
-    if((*pObjRoad)->GetLaneSectionCount()>0)
-    {
-        LaneSection * pLS = (*pObjRoad)->GetLaneSection(0);
-        if((*pObjRoad)->GetLaneSection(0)->GetLeftLaneCount()&&(*pObjRoad)->GetLaneSection(0)->GetRightLaneCount()>0)
-        {
-            return 0;
-        }
-        else
-        {
-            double headdiff = hdg - nearhead;
-            while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-            while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-            if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
-            {
-               return 0;
-            }
-            else
-            {
-                if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0)&&(pLS->GetLeftLaneCount()>0))
-                {
-                    return 0;
-                }
-                else
-                {
-                    bool bHaveSame = false;
-                    for(i=0;i<xvectorroad.size();i++)
-                    {
-                        if(xvectorroad[i]->GetLaneSectionCount()<1)continue;
-                        bool bNeedFind = false;
-                        if(bHaveSame == false)bNeedFind = true;
-                        else
-                        {
-                            if(xvectordismin[i]<dismin)bNeedFind = true;
-                        }
-                        if(bNeedFind)
-                        {
-                            double fdiff = hdg - xvectornearhead[i];
-                            while(fdiff < 2.0*M_PI) fdiff = fdiff + 2.0*M_PI;
-                            while(fdiff >= 2.0*M_PI)fdiff = fdiff - 2.0*M_PI;
-
-                            bool bUse = false;
-                            LaneSection * pLS = xvectorroad[i]->GetLaneSection(0);
-                            if(((fdiff <M_PI/2.0)||(fdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
-                            {
-                                bUse = true;
-                            }
-                            if(((fdiff >=M_PI/2.0)&&(fdiff <= M_PI*3.0/2.0))&&(pLS->GetLeftLaneCount()>0))
-                            {
-                                bUse = true;
-                            }
-
-                            if(bUse)
-                            {
-                                dismin = xvectordismin[i];
-                                nearx = xvecotrnearx[i];
-                                neary = xvectorneary[i];
-                                nearhead = xvectornearhead[i];
-                                s = xvectors[i];
-                                froads = xvectorfrels[i];
-                                *pObjRoad = xvectorroad[i];
-                                *pgeo = xvectorgeob[i];
-                                bHaveSame = true;
-                                std::cout<<"change road. "<<std::endl;
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    return 0;
-}
-
-
-
-/**
-  * @brief SelectRoadLeftRight
-  * 选择左侧道路或右侧道路
-  * 1.选择角度差小于90度的道路一侧，即同侧道路
-  * 2.单行路，选择存在的那一侧道路。
-  * @param pRoad       道路
-  * @param head1       车的航向或目标点的航向
-  * @param head_road   目标点的航向
-  * @retval            1 left   2 right
-**/
-static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
-{
-    int nrtn = 2;
-
-
-    double headdiff = head1 - head_road;
-    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-
-    bool bSel = false;
-    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
-    {
-        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
-        {
-
-            nrtn = 1;
-            bSel = true;
-        }
-    }
-    else
-    {
-        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
-        {
-            nrtn = 2;
-            bSel = true;
-        }
-    }
-
-    if(bSel == false)
-    {
-        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
-        {
-            nrtn = 1;
-        }
-        else
-        {
-            nrtn = 2;
-        }
-    }
-
-    return nrtn;
-
-}
-
-
-
-//static double getlinereldis(GeometryLine * pline,double x,double y)
-//{
-//    double x0,y0;
-//    x0 = pline->GetX();
-//    y0 = pline->GetY();
-//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
-//    if(dis > pline->GetS())
-//    {
-//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
-//        return dis;
-//    }
-//    return dis;
-//}
-
-//static double getarcreldis(GeometryArc * parc,double x,double y)
-//{
-//    double x0,y0;
-//    x0 = parc->GetX();
-//    y0 = parc->GetY();
-//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
-//    double R = fabs(1.0/parc->GetCurvature());
-//    double ang = 2.0* asin(dis/(2.0*R));
-//    double frtn = ang * R;
-//    return frtn;
-//}
-
-//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
-//{
-//    double s = 0.1;
-
-//    double xold,yold;
-//    xold = parc->GetX();
-//    yold = parc->GetY();
-
-
-//    while(s < parc->GetLength())
-//    {
-
-//        double x, y,xtem,ytem;
-//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
-//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
-//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
-//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
-
-//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
-//        {
-//            break;
-//        }
-
-//        xold = x;
-//        yold = y;
-//        s = s+ 0.1;
-//    }
-
-//    return s;
-//}
-
-//static double getreldis(RoadGeometry * prg,double x,double y)
-//{
-//    int ngeotype = prg->GetGeomType();
-//    double frtn = 0;
-//    switch (ngeotype) {
-//    case 0:
-//        frtn = getlinereldis((GeometryLine *)prg,x,y);
-//        break;
-//    case 1:
-//        break;
-//    case 2:
-//        frtn = getarcreldis((GeometryArc *)prg,x,y);
-//        break;
-//    case 3:
-//        break;
-//    case 4:
-//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
-//        break;
-//    default:
-//        break;
-//    }
-
-//    return frtn;
-//}
-
-
-
-
-//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
-//{
-//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
-//    double s1 = prg->GetS();
-//    double srtn = s1;
-
-//    return s1 + getreldis(prg,x,y);
-//}
-
-
-static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
-{
-    std::vector<PlanPoint> xvectorPP;
-    int i;
-    double s = pline->GetLength();
-    int nsize = s/fspace;
-    if(nsize ==0)nsize = 1;
-
-    for(i=0;i<nsize;i++)
-    {
-        double x,y;
-        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
-        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
-        PlanPoint pp;
-        pp.x = x;
-        pp.y = y;
-        pp.dis = i*fspace;
-        pp.hdg = pline->GetHdg();
-        pp.mS = pline->GetS() + i*fspace;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
-{
-    std::vector<PlanPoint> xvectorPP;
-
-    //   double fRtn = 1000.0;
-    if(parc->GetCurvature() == 0.0)return xvectorPP;
-
-    double R = fabs(1.0/parc->GetCurvature());
-
-    //calculate arc center
-    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
-    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
-
-    double arcdiff = fspace/R;
-    int nsize = parc->GetLength()/fspace;
-    if(nsize == 0)nsize = 1;
-    int i;
-    for(i=0;i<nsize;i++)
-    {
-        double x,y;
-        PlanPoint pp;
-        if(parc->GetCurvature() > 0)
-        {
-            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
-            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
-            pp.hdg = parc->GetHdg() + i*arcdiff;
-        }
-        else
-        {
-            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
-            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
-            pp.hdg = parc->GetHdg() - i*arcdiff;
-        }
-
-        pp.x = x;
-        pp.y = y;
-        pp.dis = i*fspace;
-        pp.mS = parc->GetS() + i*fspace;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-
-static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
-{
-
-    double x,y,hdg;
-    double s = 0.0;
-    double s0 = pspiral->GetS();
-
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-
-    while(s<pspiral->GetLength())
-    {
-        pspiral->GetCoords(s0+s,x,y,hdg);
-
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-        pp.dis = s;
-        pp.mS = pspiral->GetS() + s;
-        xvectorPP.push_back(pp);
-
-        s = s+fspace;
-
-    }
-    return xvectorPP;
-}
-
-static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
-{
-    double x,y;
-    x = ppoly->GetX();
-    y = ppoly->GetY();
-    double A,B,C,D;
-    A = ppoly->GetA();
-    B = ppoly->GetB();
-    C = ppoly->GetC();
-    D = ppoly->GetD();
-    const double steplim = fspace;
-    double du = steplim;
-    double u = 0;
-    double v = 0;
-    double oldx,oldy;
-    oldx = x;
-    oldy = y;
-    double xstart,ystart;
-    xstart = x;
-    ystart = y;
-    double hdgstart = ppoly->GetHdg();
-    double flen = 0;
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-    pp.x = xstart;
-    pp.y = ystart;
-    pp.hdg = hdgstart;
-    pp.dis = 0;
-    pp.mS = ppoly->GetS();
-    xvectorPP.push_back(pp);
-    u = du;
-    while(flen < ppoly->GetLength())
-    {
-        double fdis = 0;
-        v = A + B*u + C*u*u + D*u*u*u;
-        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
-        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
-        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
-        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
-        oldx = x;
-        oldy = y;
-        if(fdis>(steplim*2.0))du = du/2.0;
-        flen = flen + fdis;
-        u = u + du;
-
-
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-
- //       s = s+ dt;
-        pp.dis = flen;;
-        pp.mS = ppoly->GetS() + flen;
-        xvectorPP.push_back(pp);
-    }
-
-    return xvectorPP;
-
-}
-
-static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
-{
-    double s = 0.1;
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-
-    double xold,yold;
-    xold = parc->GetX();
-    yold = parc->GetY();
-    double hdg0 = parc->GetHdg();
-    pp.x = xold;
-    pp.y = yold;
-    pp.hdg = hdg0;
-    pp.dis = 0;
-//    xvectorPP.push_back(pp);
-
-    double dt = 1.0;
-    double tcount = parc->GetLength()/0.1;
-    if(tcount > 0)
-    {
-        dt = 1.0/tcount;
-    }
-    double t;
-    s = dt;
-    s = 0;
-
-    double ua,ub,uc,ud,va,vb,vc,vd;
-    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
-    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
-
-    double s_start = parc->GetS();
-    while(s < parc->GetLength())
-    {
-        double x, y,hdg;
-        parc->GetCoords(s_start+s,x,y,hdg);
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-        s = s+ fspace;
-        pp.dis = pp.dis + fspace;;
-        pp.mS = s_start + s;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-
-std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
-{
-    Road * pRoad = xpath.mpRoad;
-    //s_start  s_end for select now section data.
-    double s_start,s_end;
-    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
-    s_start = pLS->GetS();
-    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
-    else
-    {
-        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
-    }
-
-//    if(xpath.mroadid == 10190)
-//    {
-//        int a = 1;
-//        a++;
-//    }
-
-    std::vector<PlanPoint> xvectorPPS;
-    double s = 0;
-
-    int i;
-    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
-    {
-        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
-        RoadGeometry * prg = pgb->GetGeometryAt(0);
-        std::vector<PlanPoint> xvectorPP;
-        if(i<(pRoad->GetGeometryBlockCount() -0))
-        {
-            if(prg->GetS()>s_end)
-            {
-                continue;
-            }
-            if((prg->GetS() + prg->GetLength())<s_start)
-            {
-                continue;
-            }
-        }
-
-
-        double s1 = prg->GetLength();
-
-        switch (prg->GetGeomType()) {
-        case 0:
-            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
-
-            break;
-        case 1:
-            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
-            break;
-        case 2:
-            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
-            break;
-        case 3:
-
-            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
-            break;
-        case 4:
-            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
-            break;
-        default:
-            break;
-        }
-        int j;
-        if(prg->GetS()<s_start)
-        {
-            s1 = prg->GetLength() -(s_start - prg->GetS());
-        }
-        if((prg->GetS() + prg->GetLength())>s_end)
-        {
-            s1 = s_end - prg->GetS();
-        }
-        for(j=0;j<xvectorPP.size();j++)
-        {
-            PlanPoint pp = xvectorPP.at(j);
-
-            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
-
-            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
-            {
-                if(s_start > prg->GetS())
-                {
-                    pp.dis = s + pp.dis -(s_start - prg->GetS());
-                }
-                else
-                {
-                    pp.dis = s+ pp.dis;
-                }
-                pp.nRoadID = atoi(pRoad->GetRoadId().data());
-                xvectorPPS.push_back(pp);
-            }
-//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
-//            {
-//                pp.dis = s + pp.dis;
-//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
-//                xvectorPPS.push_back(pp);
-//            }
-//            else
-//            {
-//                if(prg->GetS()<s_start)
-//                {
-
-//                }
-//                else
-//                {
-//                    if(pp.dis<(s_end -prg->GetS()))
-//                    {
-//                        pp.dis = s + pp.dis;
-//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
-//                        xvectorPPS.push_back(pp);
-//                    }
-//                }
-//            }
-        }
-        s = s+ s1;
-
-    }
-    return xvectorPPS;
-}
-
-std::vector<PlanPoint> GetPoint(Road * pRoad)
-{
-    std::vector<PlanPoint> xvectorPPS;
-    double s = 0;
-
-    int i;
-    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
-    {
-        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
-        RoadGeometry * prg = pgb->GetGeometryAt(0);
-        std::vector<PlanPoint> xvectorPP;
-        double s1 = prg->GetLength();
-        switch (prg->GetGeomType()) {
-        case 0:
-            xvectorPP = getlinepoint((GeometryLine *)prg);
-            break;
-        case 1:
-            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
-            break;
-        case 2:
-            xvectorPP = getarcpoint((GeometryArc *)prg);
-
-            break;
-        case 3:
-            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
-            break;
-        case 4:
-            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
-            break;
-        default:
-            break;
-        }
-        int j;
-        for(j=0;j<xvectorPP.size();j++)
-        {
-            PlanPoint pp = xvectorPP.at(j);
-            pp.dis = s + pp.dis;
-            pp.nRoadID = atoi(pRoad->GetRoadId().data());
-            xvectorPPS.push_back(pp);
-        }
-        s = s+ s1;
-
-    }
-    return xvectorPPS;
-
-}
-
-int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
-{
-    int nrtn = 0;
-    int i;
-    int dismin = 1000;
-    for(i=0;i<xvectorPP.size();i++)
-    {
-        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
-        if(dis <dismin)
-        {
-            dismin = dis;
-            nrtn = i;
-        }
-    }
-
-    if(dismin > 10.0)
-    {
-        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
-    }
-    return nrtn;
-}
-
-
-double getwidth(Road * pRoad, int nlane)
-{
-    double frtn = 0;
-
-    int i;
-    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
-    for(i=0;i<nlanecount;i++)
-    {
-        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
-        {
-            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
-            if(pLW != 0)
-            {
-            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
-            break;
-            }
-        }
-    }
-    return frtn;
-}
-
-double getoff(Road * pRoad,int nlane)
-{
-    double off = getwidth(pRoad,nlane)/2.0;
-    if(nlane < 0)off = off * (-1.0);
- //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
-    int i;
-    if(nlane > 0)
-        off = off + getwidth(pRoad,0)/2.0;
-    else
-        off = off - getwidth(pRoad,0)/2.0;
-    if(nlane > 0)
-    {
-        for(i=1;i<nlane;i++)
-        {
-            off = off + getwidth(pRoad,i);
-        }
-    }
-    else
-    {
-        for(i=-1;i>nlane;i--)
-        {
-            off = off - getwidth(pRoad,i);
-        }
-    }
-//    return 0;
-    return off;
-
-}
-
-
-namespace  iv {
-
-struct lanewidthabcd
-{
-    int nlane;
-    double A;
-    double B;
-    double C;
-    double D;
-};
-}
-
-double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
-{
-    double frtn = 0;
-
-
-    int i;
-    int nlanecount = xvectorlwa.size();
-    for(i=0;i<nlanecount;i++)
-    {
-        if(nlane == xvectorlwa.at(i).nlane)
-        {
-            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
-            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
-            break;
-        }
-    }
-    return frtn;
-}
-
-
-
-std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
-{
-    std::vector<iv::lanewidthabcd> xvectorlwa;
-    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
-    int i;
-
-
-    LaneSection * pLS = pRoad->GetLaneSection(0);
-
-//    if(pRoad->GetLaneSectionCount() > 1)
-//    {
-//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
-//        {
-//            if(s>pRoad->GetLaneSection(i+1)->GetS())
-//            {
-//                pLS = pRoad->GetLaneSection(i+1);
-//            }
-//            else
-//            {
-//                break;
-//            }
-//        }
-//    }
-
-    int nlanecount = pLS->GetLaneCount();
-
-    for(i=0;i<nlanecount;i++)
-    {
-        iv::lanewidthabcd xlwa;
-
-
-        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
-        xlwa.nlane = pLS->GetLane(i)->GetId();
-        if(pLW != 0)
-        {
-
-            xlwa.A = pLW->GetA();
-            xlwa.B = pLW->GetB();
-            xlwa.C = pLW->GetC();
-            xlwa.D = pLW->GetD();
-
-        }
-        else
-        {
-            xlwa.A = 0;
-            xlwa.B = 0;
-            xlwa.C = 0;
-            xlwa.D = 0;
-        }
-
- //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
-
-    }
-    return xvectorlwa;
-
-}
-
-
-inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
-{
-    int i;
-    double off = 0;
-    double a,b,c,d;
-    if(xvectorIndex.size() == 0)return off;
-
-    for(i=0;i<(xvectorIndex.size()-1);i++)
-    {
-
-        a = xvectorLWA[xvectorIndex[i]].A;
-        b = xvectorLWA[xvectorIndex[i]].B;
-        c = xvectorLWA[xvectorIndex[i]].C;
-        d = xvectorLWA[xvectorIndex[i]].D;
-        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
-        {
-            off = off + a + b*s + c*s*s + d*s*s*s;
-        }
-        else
-        {
-            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
-        }
-    }
-    i = xvectorIndex.size()-1;
-    a = xvectorLWA[xvectorIndex[i]].A;
-    b = xvectorLWA[xvectorIndex[i]].B;
-    c = xvectorLWA[xvectorIndex[i]].C;
-    d = xvectorLWA[xvectorIndex[i]].D;
-    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
-    if(nlane < 0) off = off*(-1.0);
-//    std::cout<<"s is "<<s<<std::endl;
-//    std::cout<<" off is "<<off<<std::endl;
-    return off;
-
-}
-
-double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
-{
-    double fwidth = 0;
-    int i;
-    double a,b,c,d;
-
-    int nsize = xvectorLWA.size();
-    for(i=0;i<nsize;i++)
-    {
-        if(nlane*(xvectorLWA[i].nlane)>0)
-        {
-            a = xvectorLWA[i].A;
-            b = xvectorLWA[i].B;
-            c = xvectorLWA[i].C;
-            d = xvectorLWA[i].D;
-            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
-        }
-    }
-    return fwidth;
-
-}
-
-int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
-{
-    if(pRoad->GetLaneSectionCount() < 1)return -1;
-
-    LaneSection * pLS = pRoad->GetLaneSection(0);
-
-    int i;
-
-    if(pRoad->GetLaneSectionCount() > 1)
-    {
-        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
-        {
-            if(s>pRoad->GetLaneSection(i+1)->GetS())
-            {
-                pLS = pRoad->GetLaneSection(i+1);
-            }
-            else
-            {
-                break;
-            }
-        }
-    }
-
-    nori = 0;
-    ntotal = 0;
-    fSpeed = -1; //if -1 no speedlimit for map
-
-    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.
-
-    pRoad->GetRoadNoavoid(s,bNoavoid);
-
-    int nlanecount = pLS->GetLaneCount();
-    for(i=0;i<nlanecount;i++)
-    {
-        int nlaneid = pLS->GetLane(i)->GetId();
-
-        if(nlaneid == nlane)
-        {
-            Lane * pLane = pLS->GetLane(i);
-            if(pLane->GetLaneSpeedCount() > 0)
-            {
-                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
-                int j;
-                int nspeedcount = pLane->GetLaneSpeedCount();
-                for(j=0;j<(nspeedcount -1);j++)
-                {
-                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
-                    {
-                        pLSpeed = pLane->GetLaneSpeed(j+1);
-                    }
-                    else
-                    {
-                        break;
-                    }
-                }
-                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
-
-            }
-        }
-        if(nlane<0)
-        {
-
-            if(nlaneid < 0)
-            {
-                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-                {
-                    ntotal++;
-                    if(nlaneid<nlane)nori++;
-                }
-            }
-
-
-        }
-        else
-        {
-            if(nlaneid > 0)
-            {
-                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-                {
-                    ntotal++;
-                    if(nlaneid > nlane)nori++;
-                }
-            }
-        }
-    }
-
-    return 0;
-}
-
-std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
-{
-    std::vector<int> xvectorindex;
-    int i;
-    if(nlane >= 0)
-    {
-    for(i=0;i<=nlane;i++)
-    {
-       int j;
-       int nsize = xvectorLWA.size();
-       for(j=0;j<nsize;j++)
-       {
-           if(i == xvectorLWA.at(j).nlane )
-           {
-               xvectorindex.push_back(j);
-               break;
-           }
-       }
-    }
-    }
-    else
-    {
-        for(i=0;i>=nlane;i--)
-        {
-           int j;
-           int nsize = xvectorLWA.size();
-           for(j=0;j<nsize;j++)
-           {
-               if(i == xvectorLWA.at(j).nlane )
-               {
-                   xvectorindex.push_back(j);
-                   break;
-               }
-           }
-        }
-    }
-    return xvectorindex;
-}
-
-void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
-{
-    if(xps.mpRoad->GetRoadBorrowCount() == 0)
-    {
-        return;
-    }
-
-    Road * pRoad = xps.mpRoad;
-    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
-    unsigned int i;
-    unsigned int nPPCount = xvectorPP.size();
-    for(i=0;i<nborrowsize;i++)
-    {
-        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
-        if(pborrow == NULL)
-        {
-            std::cout<<"warning:can't get borrow"<<std::endl;
-            continue;
-        }
-        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
-        {
-            unsigned int j;
-            double soffset = pborrow->GetS();
-            double borrowlen = pborrow->GetLength();
-            for(j=0;j<xvectorPP.size();j++)
-            {
-                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
-                {
-                    xvectorPP[j].mbBoringRoad = true;
-                }
-            }
-        }
-    }
-}
-
-void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
-                     const int nchang1,const int nchang2,const int nchangpoint)
-{
-    if(xvectorPP.size()<2)return;
-    bool bInlaneavoid = false;
-    int i;
-    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
-    {
-        if(xps.mpRoad->GetRoadLength()<30)
-        {
-            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
-            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
-            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
-                bInlaneavoid = false;
-            else
-                bInlaneavoid = true;
-        }
-        else
-        {
-            bInlaneavoid = true;
-        }
-    }
-    else
-    {
-        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
-    }
-
-    if(bInlaneavoid == false)
-    {
-        int nvpsize = xvectorPP.size();
-        for(i=0;i<nvpsize;i++)
-        {
-            xvectorPP.at(i).bInlaneAvoid = false;
-            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
-            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
-            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
-            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
-        }
-    }
-    else
-    {
-      int nvpsize = xvectorPP.size();
-      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
-      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
-      {
-          if(xps.mbStartToMainChange == true)
-          {
-              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
-              {
-                  if((i>=0)&&(i<nvpsize))
-                    xvectorPP.at(i).bInlaneAvoid = false;
-              }
-
-          }
-          if(xps.mbMainToEndChange)
-          {
-              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
-              {
-                  if((i>=0)&&(i<nvpsize))
-                    xvectorPP.at(i).bInlaneAvoid = false;
-              }
-          }
-      }
-
-      for(i=0;i<nvpsize;i++)
-      {
-          if(xvectorPP.at(i).bInlaneAvoid)
-          {
-              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
-              if(foff < 0.3)
-              {
-                  xvectorPP.at(i).bInlaneAvoid = false;
-                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
-                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
-                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
-                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
-              }
-              else
-              {
-                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
-                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
-                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
-                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
-              }
-          }
-      }
-    }
-
-}
-
-
-double getoff(Road * pRoad,int nlane,const double s)
-{
-    int i;
-    int nLSCount = pRoad->GetLaneSectionCount();
-    double s_section = 0;
-
-    double foff = 0;
-
-    for(i=0;i<nLSCount;i++)
-    {
-
-        LaneSection * pLS = pRoad->GetLaneSection(i);
-        if(i<(nLSCount -1))
-        {
-            if(pRoad->GetLaneSection(i+1)->GetS()<s)
-            {
-                continue;
-            }
-        }
-        s_section = pLS->GetS();
-        int nlanecount = pLS->GetLaneCount();
-        int j;
-        for(j=0;j<nlanecount;j++)
-        {
-            Lane * pLane = pLS->GetLane(j);
-
-            int k;
-            double s_lane = s-s_section;
-
-
-            if(pLane->GetId() != 0)
-            {
-
-                for(k=0;k<pLane->GetLaneWidthCount();k++)
-                {
-                    if(k<(pLane->GetLaneWidthCount()-1))
-                    {
-                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
-                        {
-                            continue;
-                        }
-                    }
-                    s_lane = pLane->GetLaneWidth(k)->GetS();
-                    break;
-                }
-                LaneWidth * pLW  = pLane->GetLaneWidth(k);
-                if(pLW == 0)
-                {
-                    std::cout<<"not find LaneWidth"<<std::endl;
-                    break;
-                }
-
-                double fds = s - s_lane - s_section;
-                double fwidth= pLW->GetA() + pLW->GetB() * fds
-                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
-
-//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
-//                {
-//                    qDebug("fs is %f width is %f",fds,fwidth);
-//                }
-                if(nlane == pLane->GetId())
-                {
-                    foff = foff + fwidth/2.0;
-                }
-                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
-                {
-                    foff = foff + fwidth;
-                }
-
-            }
-
-        }
-
-
-        break;
-
-    }
-
-    if(pRoad->GetLaneOffsetCount()>0)
-    {
-
-        int nLOCount = pRoad->GetLaneOffsetCount();
-        int isel = -1;
-        for(i=0;i<(nLOCount-1);i++)
-        {
-            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
-            {
-                isel = i;
-                break;
-            }
-        }
-        if(isel < 0)isel = nLOCount-1;
-        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
-        double s_off = s - pLO->GetS();
-        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
-                +pLO->Getd() * s_off * s_off * s_off;
-        foff = foff - off1;
-    }
-
-    if(nlane<0)foff = foff*(-1);
-    return foff;
-}
-
-
-std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
-{
-    std::vector<PlanPoint> xvectorPP;
-
-    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
-    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
-    int nchange1,nchange2;
-    int nlane1,nlane2,nlane3;
-    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
-    {
-        xps.mainsel = xps.mnStartLaneSel;
-        xps.mbStartToMainChange = false;
-        xps.mbMainToEndChange = false;
-    }
-    else
-    {
-        if(xps.mpRoad->GetRoadLength() < 100)
-        {
-            xps.mainsel = xps.mnEndLaneSel;
-            xps.mbStartToMainChange = true;
-            xps.mbMainToEndChange = false;
-        }
-    }
-
-    double off1,off2,off3;
-    if(xps.mnStartLaneSel < 0)
-    {
-    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
-    off2 = getoff(xps.mpRoad,xps.mainsel);
-    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
-    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
-    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
-    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
-    nlane1 = xps.mnStartLaneSel;
-    nlane2 = xps.mainsel;
-    nlane3 = xps.mnEndLaneSel;
-    }
-    else
-    {
-        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
-        off2 = getoff(xps.mpRoad,xps.mainsel);
-        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
-        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
-        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
-        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
-        nlane3 = xps.mnStartLaneSel;
-        nlane2 = xps.mainsel;
-        nlane1 = xps.mnEndLaneSel;
-    }
-
-    int nchangepoint = 300;
-    if((nchangepoint * 3) > xvPP.size())
-    {
-        nchangepoint = xvPP.size()/3;
-    }
-    int nsize = xvPP.size();
-
-    nchange1 = nsize/3;
-    if(nchange1>500)nchange1 = 500;
-    nchange2 = nsize*2/3;
-    if( (nsize-nchange2)>500)nchange2 = nsize-500;
-//    if(nsize < 1000)
-//    {
-//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
-//        return xvectorPP;
-//    }
-    double x,y;
-    int i;
-    if(xps.mainsel < 0)
-    {
-
-        for(i=0;i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-
-            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
-            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-
-            pp.nlrchange = 1;
-
-            if(xps.mainsel != xps.secondsel)
-            {
-                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
-                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
-                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
-
-                if(xps.mainsel >xps.secondsel)
-                {
-                    pp.nlrchange = 1;
-                }
-                else
-                {
-                    pp.nlrchange = 2;
-                }
-            }
-            else
-            {
-                pp.mfSecx = pp.x ;
-                pp.mfSecy = pp.y ;
-            }
-
-            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off1*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-            xvectorPP.push_back(pp);
-        }
-
-
-    }
-    else
-    {
-
-        for(i=0;i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-
-
-            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
-            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-
-            pp.nlrchange = 1;
-
-            if(xps.mainsel != xps.secondsel)
-            {
-                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
-                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
-                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
-
-                if(xps.mainsel >xps.secondsel)
-                {
-                    pp.nlrchange = 2;
-                }
-                else
-                {
-                    pp.nlrchange = 1;
-                }
-            }
-            else
-            {
-                pp.mfSecx = pp.x ;
-                pp.mfSecy = pp.y ;
-            }
-
-            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off1;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-            pp.hdg = pp.hdg + M_PI;
-            xvectorPP.push_back(pp);
-        }
-
-//        for(i=0;i<(nchange1- nchangepoint/2);i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off1;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
-//        {
-
-//            PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
-//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-
-//            pp.mfDisToRoadLeft = offx;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-//            if(nlane1 == nlane2)
-//            {
-//                pp.mWidth = flanewidth1;
-//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//                pp.lanmp = 0;
-//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//            }
-//            else
-//            {
-//                if(nlane1 < nlane2)
-//                {
-//                    pp.lanmp = -1;
-//                }
-//                else
-//                {
-//                    pp.lanmp = 1;
-//                }
-
-//                if(i<nchange1)
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
-//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-//                else
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
-//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-
-//            }
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off2;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
-//        {
-
-//            PlanPoint pp = xvPP.at(i);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
-//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-
-//            pp.mfDisToRoadLeft = offx;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-//            if(nlane2 == nlane3)
-//            {
-//                pp.mWidth = flanewidth1;
-//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//                pp.lanmp = 0;
-//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//            }
-//            else
-//            {
-//                if(nlane2 < nlane3)
-//                {
-//                    pp.lanmp = -1;
-//                }
-//                else
-//                {
-//                    pp.lanmp = 1;
-//                }
-
-//                if(i<nchange2)
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
-//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-//                else
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
-//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-
-//            }
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off3;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-
-    }
-
-
-
-    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
-
-    if(xps.mpRoad->GetRoadBorrowCount()>0)
-    {
-        CalcBorringRoad(xps,xvectorPP);
-    }
-
-
-    if(xps.mnStartLaneSel > 0)
-    {
-        std::vector<PlanPoint> xvvPP;
-        int nvsize = xvectorPP.size();
-        for(i=0;i<nvsize;i++)
-        {
-            xvvPP.push_back(xvectorPP.at(nvsize-1-i));
-        }
-        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
-        return xvvPP;
-    }
-
-//    pRoad->GetLaneSection(xps.msectionid)
-
-    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
-    return xvectorPP;
-}
-
-
-static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
-{
-    if(pRoad->GetSignalCount() == 0)return;
-    int nsigcount = pRoad->GetSignalCount();
-    int i;
-    for(i=0;i<nsigcount;i++)
-    {
-        Signal * pSig = pRoad->GetSignal(i);
-        int nfromlane = -100;
-        int ntolane = 100;
-        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
-        if(pSig_laneValidity != 0)
-        {
-            nfromlane = pSig_laneValidity->GetfromLane();
-            ntolane = pSig_laneValidity->GettoLane();
-        }
-        if((nlane < 0)&&(nfromlane >= 0))
-        {
-            continue;
-        }
-        if((nlane > 0)&&(ntolane<=0))
-        {
-            continue;
-        }
-
-        double s = pSig->Gets();
-        double fpos = s/pRoad->GetRoadLength();
-        if(nlane > 0)fpos = 1.0 -fpos;
-        int npos = fpos *xvectorPP.size();
-        if(npos <0)npos = 0;
-        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
-        while(xvectorPP.at(npos).nSignal != -1)
-        {
-            if(npos > 0)npos--;
-            else break;
-        }
-        while(xvectorPP.at(npos).nSignal != -1)
-        {
-            if(npos < (xvectorPP.size()-1))npos++;
-            else break;
-        }
-        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
-
-
-    }
-}
-
-static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
-                                          Road * pRoad_obj,GeometryBlock * pgeob_obj,
-                                           const double x_now,const double y_now,const double head,
-                                           double nearx,double neary, double nearhead,
-                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
-{
-
-  std::vector<PlanPoint> xvectorPPs;
-
-  double fspace = 0.1;
-
-  if(flen<2000)fspace = 0.1;
-  else
-  {
-      if(flen<5000)fspace = 0.3;
-      else
-      {
-          if(flen<10000)fspace = 0.5;
-          else
-              fspace = 1.0;
-      }
-  }
-
-  int i;
-
-
-//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
-  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
-
-
-  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
-
-  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
-
-  if(xpathsection[0].mainsel < 0)
-  {
-  for(i=indexstart;i<xvPP.size();i++)
-  {
-      xvectorPPs.push_back(xvPP.at(i));
-  }
-  }
-  else
-  {
-
-      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
-      {
-          PlanPoint pp;
-          pp = xvPP.at(i);
-//          pp.hdg = pp.hdg +M_PI;
-          xvectorPPs.push_back(pp);
-      }
-  }
-
-  int npathlast = xpathsection.size() - 1;
-//  while(npathlast >= 0)
-//  {
-//    if(npathlast == 0)break;
-//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
-//  }
-  for(i=1;i<(npathlast);i++)
-  {
-//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
-//      {
-//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
-//          {
-//            continue;
-//          }
-//      }
-//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
-//      {
-//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
-//          {
-//            break;
-//          }
-//      }
- //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
-      xvectorPP = GetPoint( xpathsection[i],fspace);
-      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
-//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
-//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
-//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
-      int j;
-      for(j=0;j<xvPP.size();j++)
-      {
-
-          PlanPoint pp;
-          pp = xvPP.at(j);
-//          pp.hdg = pp.hdg +M_PI;
-          xvectorPPs.push_back(pp);
-      }
-  }
-
-  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
-
-
-//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
-  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
-  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
-  int nlastsize;
-  if(xpathsection[npathlast].mainsel<0)
-  {
-      nlastsize = indexend;
-  }
-  else
-  {
-      if(indexend>0) nlastsize = xvPP.size() - indexend;
-      else nlastsize = xvPP.size();
-  }
-  for(i=0;i<nlastsize;i++)
-  {
-      xvectorPPs.push_back(xvPP.at(i));
-
-  }
-
-  //Find StartPoint
-//  if
-
-//  int a = 1;
-
-
-
-
-  return xvectorPPs;
-}
-
-std::vector<PlanPoint> gTempPP;
-int getPoint(double q[3], double x, void* user_data) {
-//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
-    PlanPoint pp;
-    pp.x = q[0];
-    pp.y = q[1];
-    pp.hdg = q[2];
-    pp.dis = x;
-    pp.speed = *((double *)user_data);
-    gTempPP.push_back(pp);
-//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
-    return 0;
-}
-
-/**
- * @brief getlanesel
- * @param nSel
- * @param pLaneSection
- * @param nlr
- * @return
- */
-int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
-{
-    int nlane = nSel;
-    int mainselindex = 0;
-    if(nlr == 2)nlane = nlane*(-1);
-    int nlanecount = pLaneSection->GetLaneCount();
-
-    int i;
-    int nTrueSel = nSel;
-    if(nTrueSel <= 1)nTrueSel= 1;
-    int nCurSel = 1;
-    if(nlr ==  2)nCurSel = nCurSel *(-1);
-    bool bOK = false;
-    int nxsel = 1;
-    int nlasttid = 0;
-    bool bfindlast = false;
-    while(bOK == false)
-    {
-        bool bHaveDriving = false;
-        int ncc = 0;
-        for(i=0;i<nlanecount;i++)
-        {
-            Lane * pLane = pLaneSection->GetLane(i);
-            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
-            {
-                if((nlr == 1)&&(pLane->GetId()>0))
-                {
-                    ncc++;
-                }
-                if((nlr == 2)&&(pLane->GetId()<0))
-                {
-                    ncc++;
-                }
-                if(ncc == nxsel)
-                {
-                    bHaveDriving = true;
-                    bfindlast = true;
-                    nlasttid = pLane->GetId();
-                    break;
-                }
-            }
-        }
-        if(bHaveDriving == true)
-        {
-            if(nxsel == nTrueSel)
-            {
-                return nlasttid;
-            }
-            else
-            {
-                nxsel++;
-            }
-        }
-        else
-        {
-            if(bfindlast)
-            {
-                return nlasttid;
-            }
-            else
-            {
-                std::cout<<"can't find lane."<<std::endl;
-                return 0;
-            }
-            //Use Last
-        }
-
-    }
-
-    return 0;
-
-    int k;
-    bool bFind = false;
-    while(bFind == false)
-    {
-        for(k=0;k<nlanecount;k++)
-        {
-            Lane * pLane = pLaneSection->GetLane(k);
-            if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
-            {
-                bFind = true;
-                mainselindex = k;
-                break;
-            }
-        }
-        if(bFind)continue;
-        if(nlr == 1)nlane--;
-        else nlane++;
-        if(nlane == 0)
-        {
-            std::cout<<" Fail. can't find lane"<<std::endl;
-            break;
-        }
-    }
-    return nlane;
-}
-
-
-
-void checktrace(std::vector<PlanPoint> & xPlan)
-{
-    int i;
-    int nsize = xPlan.size();
-    for(i=1;i<nsize;i++)
-    {
-        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
-        if(dis > 0.3)
-        {
-            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
-
-            int ncount = dis/0.1;
-            int j;
-            for(j=1;j<ncount;j++)
-            {
-                double x, y;
-
-                PlanPoint pp;
-                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
-                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
-                pp.hdg = hdg;
-                xPlan.insert(xPlan.begin()+i+j-1,pp);
-
-            }
-            qDebug("dis is %f",dis);
-        }
-    }
-}
-
-
-
-void ChangeLane(std::vector<PlanPoint> & xvectorPP)
-{
-    int i = 0;
-    int nsize = xvectorPP.size();
-    for(i=0;i<nsize;i++)
-    {
-        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
-        {
-
-        }
-        else
-        {
-            int k;
-            for(k=i;k<(nsize-1);k++)
-            {
-                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
-                {
-                    break;
-                }
-            }
-            int nnum = k-i;
-            int nchangepoint = 300;
-            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
-                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
-            const double fMAXCHANGE = 100;
-            if(froadlen<fMAXCHANGE)
-            {
-                nchangepoint = nnum;
-            }
-            else
-            {
-                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
-            }
-
-            qDebug(" road %d len is %f changepoint is %d nnum is %d",
-                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
-
-            int nstart = i + nnum/2 -nchangepoint/2;
-            int nend = i+nnum/2 + nchangepoint/2;
-            if(nnum<300)
-            {
-                nstart = i;
-                nend = k;
-            }
-            int j;
-            for(j=i;j<nstart;j++)
-            {
-                xvectorPP[j].x = xvectorPP[j].mfSecx;
-                xvectorPP[j].y = xvectorPP[j].mfSecy;
-            }
-            nnum = nend - nstart;
-            for(j=nstart;j<nend;j++)
-            {
-                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
-                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
-                double foff = fdis *(j-nstart)/nnum;
-                if(xvectorPP[j].nlrchange == 1)
-                {
-//                    std::cout<<"foff is "<<foff<<std::endl;
-                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
-                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
-                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
-                }
-                else
-                {
-                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
-                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
-                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
-                }
-            }
-            i =k;
-
-        }
-    }
-}
-
-#include <QFile>
-
-/**
- * @brief MakePlan         全局路径规划
- * @param pxd              有向图
- * @param pxodr            OpenDrive地图
- * @param x_now            当前x坐标
- * @param y_now            当前y坐标
- * @param head             当前航向
- * @param x_obj            目标点x坐标
- * @param y_obj            目标点y坐标
- * @param obj_dis          目标点到路径的距离
- * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
- * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
- * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
- * @param xPlan            返回的轨迹点
- * @return                 0 成功  <0 失败
- */
-int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-             const double x_obj,const double y_obj,const double & obj_dis,
-             const double srcnearthresh,const double dstnearthresh,
-             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
-{
-    double  s;double nearx;
-    double  neary;double  nearhead;
-    Road * pRoad;GeometryBlock * pgeob;
-    double fs1,fs2;
-//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
-
-    int nfind = GetNearPointWithHead(x_now,y_now,head,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh,fs1);
-
-    if(nfind < 0)return -1;
-    fs1 =fs1 + pgeob->GetGeometryAt(0)->GetS();
-
-    double  s_obj;double nearx_obj;
-    double  neary_obj;double  nearhead_obj;
-    Road * pRoad_obj;GeometryBlock * pgeob_obj;
-    int nfind_obj = GetNearPoint(x_obj,y_obj,pxodr,&pRoad_obj,&pgeob_obj,s_obj,nearx_obj,neary_obj,
-                                 nearhead_obj,dstnearthresh,fs2);
-
-
-    if(nfind_obj < 0)return -2;
-    fs2 = fs2 + pgeob_obj->GetGeometryAt(0)->GetS();
-
-
-    //计算终点在道路的左侧还是右侧
-    int lr_end = 2;
-    double hdg_end;
-    hdg_end = CalcHdg(QPointF(nearx_obj,neary_obj),QPointF(x_obj,y_obj));
-    double hdgdiff = nearhead_obj - hdg_end;
-    while(hdgdiff<0)hdgdiff= hdgdiff + 2.0*M_PI;
-    while(hdgdiff >= 2.0*M_PI)hdgdiff = hdgdiff- 2.0*M_PI;
-    if(hdgdiff<= M_PI)
-    {
-        lr_end = 2;
-    }
-    else
-    {
-        lr_end = 1;
-    }
-
-    if((pRoad_obj->GetLaneSection(0)->GetLeftLaneCount()<1)&&(lr_end == 1))
-    {
-        lr_end = 2;
-    }
-
-    if((pRoad_obj->GetLaneSection(0)->GetRightLaneCount()<1)&&(lr_end == 2))
-    {
-        lr_end = 2;
-    }
-
-    int lr_start = SelectRoadLeftRight(pRoad,head,nearhead);
-
-    bool bNeedDikstra = true;
-    if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
-    {
-        std::vector<PlanPoint> xvPP = GetPoint(pRoad);
-        int nindexstart = indexinroadpoint(xvPP,nearx,neary);
-        int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
-        int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
-        AddSignalMark(pRoad,nlane,xvPP);
-        if((nindexend >nindexstart)&&(lr_start == 2))
-        {
-            bNeedDikstra = false;
-        }
-        if((nindexend <nindexstart)&&(lr_start == 1))
-        {
-            bNeedDikstra = false;
-        }
-        if(bNeedDikstra == false)
-        {
-
-            std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
-            std::vector<int> xvectorindex1;
-
-            xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
-
-            double foff = getoff(pRoad,nlane);
-
-            foff = foff + 0.0;
-            std::vector<PlanPoint> xvectorPP;
-            int i = nindexstart;
-            while(i!=nindexend)
-            {
-
-                if(lr_start == 2)
-                {
-                    PlanPoint pp = xvPP.at(i);
-                    foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
-                    pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
-                    pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
-                    pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
-                    pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                    pp.lanmp = 0;
-                    pp.mfDisToRoadLeft = foff *(-1.0);
-                    pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
-                    GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-                    xvectorPP.push_back(pp);
-                    i++;
-                }
-                else
-                {
-                    PlanPoint pp = xvPP.at(i);
-                    foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
-                    pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
-                    pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
-                    pp.hdg = pp.hdg + M_PI;
-
-                    pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
-                    pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                    pp.lanmp = 0;
-                    pp.mfDisToRoadLeft = foff;
-                    pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
-                    GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-
-                    xvectorPP.push_back(pp);
-                    i--;
-                }
-            }
-
-            for(i=0;i<(int)xvectorPP.size();i++)
-            {
-                xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
-            }
-
-            pathsection xps;
-            xps.mbStartToMainChange = false;
-            xps.mbMainToEndChange = false;
- //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
-            xPlan = xvectorPP;
-
-        }
-    }
-
-    if(bNeedDikstra)
-    {
-        bool bSuc = false;
-    std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
-    if(xpath.size()<1)return -1;
-    if(bSuc == false)
-    {
-        return -1;
-    }
-    double flen = pxd->getpathlength(xpath);
-    std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
-
-    std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
-                 head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
-
-    ChangeLane(xvectorPP);
-    xPlan = xvectorPP;
-
-    }
-
-    int i;
-
-//    QFile xFile;
-//    xFile.setFileName("/home/yuchuli/tempgps.txt");
-//    xFile.open(QIODevice::ReadWrite);
-
-//    for(i<0;i<xPlan.size();i++)
-//    {
-//        char strout[1000];
-//        snprintf(strout,1000,"%d %f %f %d %d  %d %f \n",i,xPlan[i].mfDisToLaneLeft,
-//                 xPlan[i].mfDisToRoadLeft,xPlan[i].mnLaneori,xPlan[i].mnLaneTotal,
-//                 xPlan[i].lanmp,xPlan[i].mWidth);
-//        xFile.write(strout,strnlen(strout,1000));
-//    }
-//    xFile.close();
-
- //   checktrace(xPlan);
-//    std::cout<<"pp size is "<<xvectorPP.size()<<std::endl;
-    return 0;
-
-    if(nfind_obj < 0)return -2;
-
-    if(pRoad != pRoad_obj)return -3; //temp,simple plan
-
-    if(pgeob != pgeob_obj)return -4; //temp,simple geo
-
-    if(!pgeob->CheckIfLine())return -5; //temp,only line plan;
-
-
-    int nlane = nlanesel;
-    if(nlane <= 0 )nlane = 1;
-
-    if(nlane >= pRoad->GetLaneSection(0)->GetLaneCount())
-    {
-        nlane = pRoad->GetLaneSection(0)->GetLaneCount()-1;
-    }
-
-    double bianxiandis = 0;
-    double bianxiandis_obj = 0;
-
-    double park_x,park_y;//Park point
-
-    bianxiandis = pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(1)/2.0;
-
-//    int i = 1;
-    for(i=1;i<nlane;i++)
-    {
-        bianxiandis = bianxiandis + pRoad->GetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0
-                +pRoad->GetLaneSection(0)->GetLane(i+1)->GetWidthValue(1)/2.0;
-    }
-
-    double bianxianx1 = bianxiandis;
-    bianxiandis = fabs(s - bianxiandis);
-
-    bianxiandis_obj = 0;
-    int ag = pRoad->GetLaneSection(0)->GetLaneCount();
-
-
-
-    for(i=(nlane +1);i<pRoad->GetLaneSection(0)->GetLaneCount();i++)
-    {
-        bianxiandis_obj = bianxiandis_obj
-                +pRoad->GetLaneSection(0)->GetLane(i-1)->GetWidthValue(1)/2.0
-                +pRoad->GetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0;
-    }
-
- //   double x_objreal = nearx_obj +  bianxiandis_obj * cos(nearhead_obj - M_PI/2.0);
-
-//    park_x = nearx_obj + (bianxiandis_obj +bianxianx1 + pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(0)/2.0)*cos(nearhead_obj -M_PI/2.0);
-//    park_y = neary_obj + (bianxiandis_obj +bianxianx1+ pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(0)/2.0)*sin(nearhead_obj -M_PI/2.0);
-
-    park_x = nearx_obj + (bianxiandis_obj +bianxianx1 )*cos(nearhead_obj -M_PI/2.0);
-    park_y = neary_obj + (bianxiandis_obj +bianxianx1 )*sin(nearhead_obj -M_PI/2.0);
-
-    const double bianxian_ang = 0.2;
-    double src_len,dst_len;
-    src_len = bianxiandis/atan(bianxian_ang);
-
-    dst_len = bianxiandis_obj/atan(bianxian_ang);
-
-    double xl1,yl1,xl2,yl2;
-
-    xl1 = nearx + src_len * cos(nearhead);
-    yl1 = neary + src_len * sin(nearhead);
-
-    xl2 = nearx_obj - dst_len * cos(nearhead_obj);
-    yl2 = neary_obj - dst_len * sin(nearhead_obj);
-
-    xl1 = xl1 + bianxianx1 * cos(nearhead - M_PI/2.0);
-    yl1 = yl1 + bianxianx1 * sin(nearhead - M_PI/2.0);
-
-    xl2 = xl2 + bianxianx1 * cos(nearhead_obj - M_PI/2.0);
-    yl2 = yl2 + bianxianx1 * sin(nearhead_obj - M_PI/2.0);
-
-
-    double q0[3],q1[3];
-    gTempPP.clear();
-    q0[0] = x_now;q0[1] = y_now; q0[2] = head;
-    q1[0] = xl1;q1[1] =yl1;q1[2] = nearhead;
-
-    int  indexp = 0;
-    double turning_radius = 15.0;
-    double speedvalue = 3;
-    DubinsPath path;
-    dubins_shortest_path( &path, q0, q1, turning_radius);
-    dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue);
-
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 1;
-    indexp = gTempPP.size();
-
-
-    if(pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeedCount()>1)
-        speedvalue = pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeed(1)->GetMax();
-    else speedvalue = 10.0;
-
-    memcpy(q0,q1,3*sizeof(double));
-    q1[0]=xl2; q1[1]=yl2; q1[2]=nearhead;
-    dubins_shortest_path( &path, q0, q1, turning_radius);
-    dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue);
-
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 0;
-    indexp = gTempPP.size();
-
-    double vx = 3.0;
-    for(i= (gTempPP.size() -1);i>0;i--)
-    {
-        gTempPP.at(i).speed = vx;
-        vx = vx + 0.03;
-        gTempPP.at(i).lanmp = -1;
-        if(vx > speedvalue)break;
-    }
-
-    speedvalue = 3.0;
-    memcpy(q0,q1,3*sizeof(double));
-    q1[0]=park_x; q1[1]=park_y; q1[2]=nearhead_obj;
-    dubins_shortest_path( &path, q0, q1, turning_radius);
-    dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue);
-
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = -1;
-    indexp = gTempPP.size();
-
-    double parkx_ext = park_x + 30 * cos(nearhead);
-    double parky_ext = park_y + 30 * sin(nearhead);
-    speedvalue = 0.0;
-    memcpy(q0,q1,3*sizeof(double));
-    q1[0]=parkx_ext; q1[1]=parky_ext; q1[2]=nearhead;
-    dubins_shortest_path( &path, q0, q1, turning_radius);
-    dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue);
-
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 0;
-    indexp = gTempPP.size();
-
-    for(i=0;i<gTempPP.size();i++)
-    {
-        xPlan.push_back(gTempPP.at(i));
-    }
-    return 0;
-
-
-    PlanPoint pp;
-    double hdgsrc,hdgdst;
-    double ldis1 = sqrt(pow(xl1 -x_now,2) + pow(yl1 -y_now,2) );
-    if(ldis1 > 0)
-    {
-        hdgsrc = asin((yl1-y_now)/ldis1);
-        if(xl1  < x_now)hdgsrc = M_PI - hdgsrc;
-        if(hdgsrc < 0)hdgsrc = hdgsrc + 2.0 * M_PI;
-    }
-
-    double ldis2 = sqrt(pow(park_x-xl2,2)+pow(park_y-yl2,2));
-    if(ldis2 > 0)
-    {
-        hdgdst = asin((park_y - yl2)/ldis2);
-        if(park_x < xl2)hdgdst = M_PI - hdgdst;
-        if(hdgdst < 0)hdgdst = hdgdst + 2.0*M_PI;
-    }
-
-    double xv,yv,speed;
-    i = 0;
-    double disx = 0.0;
-    const double step = 0.1;
-    xv = x_now;
-    yv = y_now;
-    pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;pp.lanmp = 1;xPlan.push_back(pp);
-//    qDebug("%d %f %f",i,xv,yv);
-    if(ldis1 > 0)
-    {
-        while((disx+step) < ldis1)
-        {
-            disx = disx + step;
-            i++;
-            xv = x_now + disx * cos(hdgsrc);
-            yv = y_now + disx * sin(hdgsrc);
-            pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;xPlan.push_back(pp);
- //           qDebug("%d %f %f",i,xv,yv);
-        }
-    }
-
-    i++;
-    xv = xl1;yv = yl1;
-  //  qDebug("1:%d %f %f",i,xv,yv);
-    pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;pp.lanmp = 0;xPlan.push_back(pp);
-
-    double dis2p = sqrt(pow(xl2-xl1,2) + pow(yl2 -yl1,2));
-    disx = 0;
-    if(dis2p > 0)
-    {
-        while((disx+step) < dis2p)
-        {
-            disx = disx + step;
-            i++;
-            xv = xl1 + disx * cos(nearhead);
-            yv = yl1 + disx * sin(nearhead);
-            if(pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeedCount()>1)
-                speed = pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeed(1)->GetMax();
-            else speed = 10.0;
-            if((dis2p -disx)<((speed - 3.0)*5)){
-                speed = 3.0;
-                pp.lanmp = -1;
-            }
-            else
-            {
-                pp.lanmp = 0;
-            }
-            pp.x = xv;pp.y = yv;pp.hdg = nearhead;pp.speed= speed;xPlan.push_back(pp);
- //           qDebug("%d %f %f speed is %f",i,xv,yv,speed);
-
-        }
-    }
-
-    i++;
-    xv = xl2;yv = yl2;
-    pp.x = xv;pp.y = yv;pp.hdg = hdgdst;pp.speed= 3.0;pp.lanmp = -1;xPlan.push_back(pp);
- //   qDebug("2: %d %f %f",i,xv,yv);
-
-    disx = 0;
-    if(ldis2 > 0)
-    {
-        while((disx+step) < ldis2)
-        {
-            disx = disx + step;
-            i++;
-            xv = xl2 + disx * cos(hdgdst);
-            yv = yl2 + disx * sin(hdgdst);
-            speed = 3.0;
-            pp.x = xv;pp.y = yv;pp.hdg = hdgdst;pp.speed= 3.0;pp.lanmp = -1;xPlan.push_back(pp);
-  //          qDebug("%d %f %f",i,xv,yv);
-        }
-    }
-
-    i++;
-    xv = park_x;yv = park_y;
-    pp.x = xv;pp.y = yv;pp.hdg = nearhead_obj;pp.speed= 0.0;pp.lanmp = 0;xPlan.push_back(pp);
- //   qDebug("obj: %d %f %f",i,xv,yv);
-
-    disx = 0;
-    while((disx+step) < 30)
-    {
-        disx = disx + step;
-        i++;
-        xv = park_x + disx * cos(nearhead);
-        yv = park_y + disx * sin(nearhead);
-        speed = 0.0;
-        pp.x = xv;pp.y = yv;pp.hdg = nearhead_obj;pp.speed= speed;pp.lanmp = 0;xPlan.push_back(pp);
- //       qDebug("%d %f %f",i,xv,yv);
-    }
-}
+#include "globalplan.h"
+#include "limits"
+#include <iostream>
+
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <Eigen/SVD>
+
+#include <QDebug>
+#include <QPointF>
+
+
+using namespace  Eigen;
+
+extern "C"
+{
+#include "dubins.h"
+}
+
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
+/**
+ * @brief GetLineDis 获得点到直线Geometry的距离。
+ * @param pline
+ * @param x
+ * @param y
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
+                         double & neary,double & nearhead)
+{
+    double fRtn = 1000.0;
+
+    double a1,a2,a3,a4,b1,b2;
+    double ratio = pline->GetHdg();
+    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
+    while(ratio<0)ratio = ratio+2.0*M_PI;
+
+    double dis1,dis2,dis3;
+    double x1,x2,x3,y1,y2,y3;
+    x1 = pline->GetX();y1=pline->GetY();
+    if((ratio == 0)||(ratio == M_PI))
+    {
+        a1 = 0;a4=0;
+        a2 = 1;b1= pline->GetY();
+        a3 = 1;b2= x;
+    }
+    else
+    {
+        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
+        {
+            a2=0;a3=0;
+            a1=1,b1=pline->GetX();
+            a4 = 1;b2 = y;
+        }
+        else
+        {
+            a1 = tan(ratio) *(-1.0);
+            a2 = 1;
+            a3 = tan(ratio+M_PI/2.0)*(-1.0);
+            a4 = 1;
+            b1 = a1*pline->GetX() + a2 * pline->GetY();
+            b2 = a3*x+a4*y;
+        }
+    }
+
+    y2 = y1 + pline->GetLength() * sin(ratio);
+    x2 = x1 + pline->GetLength() * cos(ratio);
+
+    Eigen::Matrix2d A;
+    A<<a1,a2,
+            a3,a4;
+    Eigen::Vector2d B(b1,b2);
+
+    Eigen::Vector2d opoint  = A.lu().solve(B);
+
+ //   std::cout<<opoint<<std::endl;
+
+    x3 = opoint(0);
+    y3 = opoint(1);
+
+    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
+    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
+    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
+
+//    std::cout<<" dis 1 is "<<dis1<<std::endl;
+//    std::cout<<" dis 2 is "<<dis2<<std::endl;
+//    std::cout<<" dis 3 is "<<dis3<<std::endl;
+
+    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
+    {
+ //       std::cout<<" out line"<<std::endl;
+        if(dis1<dis2)
+        {
+            fRtn = dis1;
+            nearx = x1;neary=y1;nearhead = pline->GetHdg();
+        }
+        else
+        {
+            fRtn = dis2;
+            nearx = x2;neary=y2;nearhead = pline->GetHdg();
+        }
+    }
+    else
+    {
+        fRtn = dis3;
+        nearx = x3;neary=y3;nearhead = pline->GetHdg();
+    }
+
+
+//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
+    return fRtn;
+
+
+
+}
+
+
+static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
+{
+     double m=0;
+     double k;
+     double b;
+
+      //计算分子
+      m=startPos.x()-endPos.x();
+
+      //求直线的方程
+      if(0==m)
+      {
+          k=100000;
+          b=startPos.y()-k*startPos.x();
+      }
+      else
+      {
+          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
+          b=startPos.y()-k*startPos.x();
+      }
+//      qDebug()<<k<<b;
+
+     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
+      //求直线与圆的交点 前提是圆需要与直线有交点
+     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
+      {
+          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point1.setY(k*point1.x()+b);
+          point2.setY(k*point2.x()+b);
+          return 0;
+      }
+
+     return -1;
+}
+
+
+/**
+  * @brief CalcHdg
+  * 计算点0到点1的航向
+  * @param p0        Point 0
+  * @param p1        Point 1
+**/
+static double CalcHdg(QPointF p0, QPointF p1)
+{
+    double x0,y0,x1,y1;
+    x0 = p0.x();
+    y0 = p0.y();
+    x1 = p1.x();
+    y1 = p1.y();
+    if(x0 == x1)
+    {
+        if(y0 < y1)
+        {
+            return M_PI/2.0;
+        }
+        else
+            return M_PI*3.0/2.0;
+    }
+
+    double ratio = (y1-y0)/(x1-x0);
+
+    double hdg = atan(ratio);
+
+    if(ratio > 0)
+    {
+        if(y1 > y0)
+        {
+
+        }
+        else
+        {
+            hdg = hdg + M_PI;
+        }
+    }
+    else
+    {
+        if(y1 > y0)
+        {
+            hdg = hdg + M_PI;
+        }
+        else
+        {
+            hdg = hdg + 2.0*M_PI;
+        }
+    }
+
+    return hdg;
+}
+
+
+
+static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
+{
+    double hdg = CalcHdg(poingarc,point1);
+    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
+    else hdg = hdg - M_PI/2.0;
+    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
+    if(hdg < 0)hdg = hdg + 2.0*M_PI;
+
+    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
+
+    double hdgdiff = hdg - parc->GetHdg();
+
+    if(hdgrange >= 0 )
+    {
+        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
+    }
+    else
+    {
+        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
+    }
+
+    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
+    return false;
+
+}
+
+static inline double calcpointdis(QPointF p1,QPointF p2)
+{
+    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
+}
+
+/**
+  * @brief GetArcDis
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param parc        pointer to a arc geomery
+  * @param x           current x
+  * @param y           current y
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+**/
+
+static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+ //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return 1000.0;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+//    if(parc->GetX() == 4.8213842690322309e+01)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+//    if(parc->GetCurvature() == -0.0000110203)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+
+    //calculate arc center
+    double x_center,y_center;
+    if(parc->GetCurvature() > 0)
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
+    }
+    else
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
+    }
+
+    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
+
+
+    QPointF arcpoint;
+    arcpoint.setX(x_center);arcpoint.setY(y_center);
+
+    QPointF pointnow;
+    pointnow.setX(x);pointnow.setY(y);
+    QPointF point1,point2;
+    point1.setX(x_center + (R * cos(hdgltoa)));
+    point1.setY(y_center + (R * sin(hdgltoa)));
+    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
+    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
+
+    //calculat dis
+    bool bp1inarc,bp2inarc;
+    bp1inarc =pointinarc(parc,arcpoint,point1);
+    bp2inarc =pointinarc(parc,arcpoint,point2);
+    double fdis[4];
+    fdis[0] = calcpointdis(pointnow,point1);
+    fdis[1] = calcpointdis(pointnow,point2);
+    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
+    QPointF pointend;
+    double hdgrange = parc->GetLength()*parc->GetCurvature();
+    double hdgend = parc->GetHdg() + hdgrange;
+    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
+    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
+
+    if(parc->GetCurvature() >0)
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
+    }
+    else
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
+    }
+
+    fdis[3] = calcpointdis(pointnow,pointend);
+    int indexmin = -1;
+    double fdismin = 1000000.0;
+    if(bp1inarc)
+    {
+        indexmin = 0;fdismin = fdis[0];
+    }
+    if(bp2inarc)
+    {
+        if(indexmin == -1)
+        {
+            indexmin = 1;fdismin = fdis[1];
+        }
+        else
+        {
+            if(fdis[1]<fdismin)
+            {
+                indexmin = 1;fdismin = fdis[1];
+            }
+        }
+    }
+    if(indexmin == -1)
+    {
+        indexmin = 2;fdismin = fdis[2];
+    }
+    else
+    {
+        if(fdis[2]<fdismin)
+        {
+            indexmin = 2;fdismin = fdis[2];
+        }
+    }
+    if(fdis[3]<fdismin)
+    {
+        indexmin = 3;fdismin = fdis[3];
+    }
+
+    switch (indexmin) {
+    case 0:
+        nearx = point1.x();
+        neary = point1.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
+        }
+        break;
+    case 1:
+        nearx = point2.x();
+        neary = point2.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
+        }
+        break;
+    case 2:
+        nearx = parc->GetX();
+        neary = parc->GetY();
+        nearhead = parc->GetHdg();
+        break;
+    case 3:
+        nearx = pointend.x();
+        neary = pointend.y();
+        nearhead = hdgend;
+        break;
+    default:
+        std::cout<<"error in arcdis "<<std::endl;
+        break;
+    }
+
+    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+    return fdismin;
+}
+
+
+static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double fdismin = 100000.0;
+    double s0 = pspiral->GetS();
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+        s = s+0.1;
+
+        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+    }
+
+    return fdismin;
+}
+
+/**
+ * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
+ * @param parc
+ * @param xnow
+ * @param ynow
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double s = 0.1;
+    double fdismin = 100000.0;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+
+    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
+    if(fdis<fdismin)
+    {
+        fdismin = fdis;
+        nearhead = parc->GetHdg();
+        nearx = parc->GetX();
+        neary = parc->GetY();
+    }
+
+    double s_start = parc->GetS();
+
+    while(s < parc->GetLength())
+    {
+
+        double x, y,hdg;//xtem,ytem;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+//        xold = x;
+//        yold = y;
+        s = s+ 0.1;
+    }
+
+    return fdismin;
+
+}
+
+
+static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+    double x,y,hdg;
+//    double s = 0.0;
+    double fdismin = 100000.0;
+ //   double s0 = ppoly->GetS();
+
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = 0.3;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+
+
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    u = u+du;
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+
+        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdisnow<fdismin)
+        {
+            fdismin = fdisnow;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+        oldx = x;
+        oldy = y;
+    }
+
+    return fdismin;
+}
+
+
+/**
+  * @brief GetNearPoint
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param x           current x
+  * @param y           current y
+  * @param pxodr       OpenDrive
+  * @param pObjRoad    Road
+  * @param pgeo        Geometry of road
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+  * @param nearthresh  near threshhold
+  * @retval            if == 0 successfull  <0 fail.
+**/
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh,double &froads)
+{
+
+    double dismin = std::numeric_limits<double>::infinity();
+    s = dismin;
+    int i;
+    double frels;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+ //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+            if(dis < dismin)
+            {
+                dismin = dis;
+                nearx = nx;
+                neary = ny;
+                nearhead = nh;
+                s = dis;
+                froads = frels;
+                *pObjRoad = proad;
+                *pgeo = pgb;
+            }
+
+
+//            if(pgb->CheckIfLine())
+//            {
+//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
+
+//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+//            else
+//            {
+//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
+//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
+//                dis = GetArcDis(parc,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
+//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+
+        }
+    }
+    if(s > nearthresh)return -1;
+    return 0;
+}
+
+
+namespace iv {
+struct nearroad
+{
+Road * pObjRoad;
+GeometryBlock * pgeob;
+double nearx;
+double neary;
+double nearhead;
+double frels;
+double fdis;
+int lr = 2; //1 left 2 right;
+int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction dis<5 3 oposite  dis <5   4 same direction > 5 5 oposite direction;
+};
+}
+
+int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
+                         const double nearthresh,bool bhdgvalid = true)
+{
+    int i;
+    double frels;
+    int nminmode = 5;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        bool bchange = false;
+        double localdismin = std::numeric_limits<double>::infinity();
+
+        double localnearx = 0;
+        double localneary = 0;
+        double localnearhead = 0;
+        double locals = 0;
+        double localfrels = 0;
+        GeometryBlock * pgeolocal;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            localdismin = std::numeric_limits<double>::infinity();
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+
+            if(dis<localdismin)
+            {
+                localdismin = dis;
+                localnearx = nx;
+                localneary = ny;
+                localnearhead = nh;
+                locals = dis;
+                localfrels = frels;
+                pgeolocal = pgb;
+                bchange = true;
+            }
+
+
+        }
+
+        if(localdismin < nearthresh)
+        {
+            iv::nearroad xnear;
+            xnear.pObjRoad = proad;
+            xnear.pgeob = pgeolocal;
+            xnear.nearx = localnearx;
+            xnear.neary = localneary;
+            xnear.fdis = localdismin;
+            xnear.nearhead = localnearhead;
+            xnear.frels = localfrels;
+            if(xnear.fdis>0)
+            {
+                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
+                double fhdgdiff = fcalhdg - xnear.nearhead;
+                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+                if(fhdgdiff<M_PI)
+                {
+                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 1;
+                }
+                else
+                {
+                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 2;
+                }
+
+
+            }
+            else
+            {
+                if(bhdgvalid == false)
+                {
+                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
+                    {
+                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                        if(pLS->GetRightLaneCount()>0)
+                        {
+                            xnear.lr = 2;
+                        }
+                        else
+                        {
+                            xnear.lr = 1;
+                        }
+                    }
+                    else
+                    {
+                        xnear.lr = 2;
+                    }
+                }
+            }
+            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+            if(pLS != NULL)
+            {
+            if(xnear.fdis < 0.00001)
+            {
+                if(bhdgvalid == false)
+                {
+                    xnear.nmode = 0;
+                }
+                else
+                {
+                    double headdiff = hdg - xnear.nearhead;
+                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                    {
+                       xnear.nmode = 0;
+                    }
+                    else
+                    {
+                        xnear.nmode = 1;
+                    }
+                }
+
+            }
+            else
+            {
+                if(xnear.fdis<5)
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 2;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 2;
+                        }
+                        else
+                        {
+                            xnear.nmode = 3;
+                        }
+                    }
+
+                }
+                else
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 4;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 4;
+                        }
+                        else
+                        {
+                            xnear.nmode = 5;
+                        }
+                    }
+                }
+            }
+            if(xnear.nmode < nminmode)nminmode = xnear.nmode;
+            xvectornear.push_back(xnear);
+            }
+        }
+
+    }
+
+    if(xvectornear.size() == 0)return -1;
+
+
+    if(xvectornear.size() > 1)
+    {
+        int i;
+        for(i=0;i<(int)xvectornear.size();i++)
+        {
+            if(xvectornear[i].nmode > nminmode)
+            {
+                xvectornear.erase(xvectornear.begin() + i);
+                i = i-1;
+            }
+        }
+    }
+    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
+    {
+        int i;
+        while(xvectornear.size()>1)
+        {
+            if(xvectornear[1].fdis < xvectornear[0].fdis)
+            {
+                xvectornear.erase(xvectornear.begin());
+            }
+            else
+            {
+                xvectornear.erase(xvectornear.begin()+1);
+            }
+        }
+    }
+    return 0;
+
+}
+
+
+
+/**
+  * @brief SelectRoadLeftRight
+  * 选择左侧道路或右侧道路
+  * 1.选择角度差小于90度的道路一侧，即同侧道路
+  * 2.单行路，选择存在的那一侧道路。
+  * @param pRoad       道路
+  * @param head1       车的航向或目标点的航向
+  * @param head_road   目标点的航向
+  * @retval            1 left   2 right
+**/
+static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
+{
+    int nrtn = 2;
+
+
+    double headdiff = head1 - head_road;
+    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+
+    bool bSel = false;
+    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
+        {
+
+            nrtn = 1;
+            bSel = true;
+        }
+    }
+    else
+    {
+        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
+        {
+            nrtn = 2;
+            bSel = true;
+        }
+    }
+
+    if(bSel == false)
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
+        {
+            nrtn = 1;
+        }
+        else
+        {
+            nrtn = 2;
+        }
+    }
+
+    return nrtn;
+
+}
+
+
+
+//static double getlinereldis(GeometryLine * pline,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = pline->GetX();
+//    y0 = pline->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    if(dis > pline->GetS())
+//    {
+//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
+//        return dis;
+//    }
+//    return dis;
+//}
+
+//static double getarcreldis(GeometryArc * parc,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = parc->GetX();
+//    y0 = parc->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    double R = fabs(1.0/parc->GetCurvature());
+//    double ang = 2.0* asin(dis/(2.0*R));
+//    double frtn = ang * R;
+//    return frtn;
+//}
+
+//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
+//{
+//    double s = 0.1;
+
+//    double xold,yold;
+//    xold = parc->GetX();
+//    yold = parc->GetY();
+
+
+//    while(s < parc->GetLength())
+//    {
+
+//        double x, y,xtem,ytem;
+//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
+//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
+//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
+//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
+
+//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
+//        {
+//            break;
+//        }
+
+//        xold = x;
+//        yold = y;
+//        s = s+ 0.1;
+//    }
+
+//    return s;
+//}
+
+//static double getreldis(RoadGeometry * prg,double x,double y)
+//{
+//    int ngeotype = prg->GetGeomType();
+//    double frtn = 0;
+//    switch (ngeotype) {
+//    case 0:
+//        frtn = getlinereldis((GeometryLine *)prg,x,y);
+//        break;
+//    case 1:
+//        break;
+//    case 2:
+//        frtn = getarcreldis((GeometryArc *)prg,x,y);
+//        break;
+//    case 3:
+//        break;
+//    case 4:
+//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
+//        break;
+//    default:
+//        break;
+//    }
+
+//    return frtn;
+//}
+
+
+
+
+//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
+//{
+//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
+//    double s1 = prg->GetS();
+//    double srtn = s1;
+
+//    return s1 + getreldis(prg,x,y);
+//}
+
+
+static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+    int i;
+    double s = pline->GetLength();
+    int nsize = s/fspace;
+    if(nsize ==0)nsize = 1;
+
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
+        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
+        PlanPoint pp;
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.hdg = pline->GetHdg();
+        pp.mS = pline->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return xvectorPP;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+    //calculate arc center
+    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
+    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
+
+    double arcdiff = fspace/R;
+    int nsize = parc->GetLength()/fspace;
+    if(nsize == 0)nsize = 1;
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        PlanPoint pp;
+        if(parc->GetCurvature() > 0)
+        {
+            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            pp.hdg = parc->GetHdg() + i*arcdiff;
+        }
+        else
+        {
+            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            pp.hdg = parc->GetHdg() - i*arcdiff;
+        }
+
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.mS = parc->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double s0 = pspiral->GetS();
+
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        pp.dis = s;
+        pp.mS = pspiral->GetS() + s;
+        xvectorPP.push_back(pp);
+
+        s = s+fspace;
+
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
+{
+    double x,y;
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = fspace;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+    pp.x = xstart;
+    pp.y = ystart;
+    pp.hdg = hdgstart;
+    pp.dis = 0;
+    pp.mS = ppoly->GetS();
+    xvectorPP.push_back(pp);
+    u = du;
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+        oldx = x;
+        oldy = y;
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+
+ //       s = s+ dt;
+        pp.dis = flen;;
+        pp.mS = ppoly->GetS() + flen;
+        xvectorPP.push_back(pp);
+    }
+
+    return xvectorPP;
+
+}
+
+static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
+{
+    double s = 0.1;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+    double hdg0 = parc->GetHdg();
+    pp.x = xold;
+    pp.y = yold;
+    pp.hdg = hdg0;
+    pp.dis = 0;
+//    xvectorPP.push_back(pp);
+
+    double dt = 1.0;
+    double tcount = parc->GetLength()/0.1;
+    if(tcount > 0)
+    {
+        dt = 1.0/tcount;
+    }
+    double t;
+    s = dt;
+    s = 0;
+
+    double ua,ub,uc,ud,va,vb,vc,vd;
+    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
+    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
+
+    double s_start = parc->GetS();
+    while(s < parc->GetLength())
+    {
+        double x, y,hdg;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        s = s+ fspace;
+        pp.dis = pp.dis + fspace;;
+        pp.mS = s_start + s;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
+{
+    Road * pRoad = xpath.mpRoad;
+    //s_start  s_end for select now section data.
+    double s_start,s_end;
+    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
+    s_start = pLS->GetS();
+    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
+    else
+    {
+        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
+    }
+
+//    if(xpath.mroadid == 10190)
+//    {
+//        int a = 1;
+//        a++;
+//    }
+
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        if(i<(pRoad->GetGeometryBlockCount() -0))
+        {
+            if(prg->GetS()>s_end)
+            {
+                continue;
+            }
+            if((prg->GetS() + prg->GetLength())<s_start)
+            {
+                continue;
+            }
+        }
+
+
+        double s1 = prg->GetLength();
+
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
+
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
+            break;
+        case 3:
+
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
+            break;
+        default:
+            break;
+        }
+        int j;
+        if(prg->GetS()<s_start)
+        {
+            s1 = prg->GetLength() -(s_start - prg->GetS());
+        }
+        if((prg->GetS() + prg->GetLength())>s_end)
+        {
+            s1 = s_end - prg->GetS();
+        }
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+
+            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
+
+            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
+            {
+                if(s_start > prg->GetS())
+                {
+                    pp.dis = s + pp.dis -(s_start - prg->GetS());
+                }
+                else
+                {
+                    pp.dis = s+ pp.dis;
+                }
+                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+                xvectorPPS.push_back(pp);
+            }
+//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
+//            {
+//                pp.dis = s + pp.dis;
+//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                xvectorPPS.push_back(pp);
+//            }
+//            else
+//            {
+//                if(prg->GetS()<s_start)
+//                {
+
+//                }
+//                else
+//                {
+//                    if(pp.dis<(s_end -prg->GetS()))
+//                    {
+//                        pp.dis = s + pp.dis;
+//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                        xvectorPPS.push_back(pp);
+//                    }
+//                }
+//            }
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+}
+
+std::vector<PlanPoint> GetPoint(Road * pRoad)
+{
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        double s1 = prg->GetLength();
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg);
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg);
+
+            break;
+        case 3:
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
+            break;
+        default:
+            break;
+        }
+        int j;
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+            pp.dis = s + pp.dis;
+            pp.nRoadID = atoi(pRoad->GetRoadId().data());
+            xvectorPPS.push_back(pp);
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+
+}
+
+int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
+{
+    int nrtn = 0;
+    int i;
+    int dismin = 1000;
+    for(i=0;i<xvectorPP.size();i++)
+    {
+        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
+        if(dis <dismin)
+        {
+            dismin = dis;
+            nrtn = i;
+        }
+    }
+
+    if(dismin > 10.0)
+    {
+        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
+    }
+    return nrtn;
+}
+
+
+double getwidth(Road * pRoad, int nlane)
+{
+    double frtn = 0;
+
+    int i;
+    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
+        {
+            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
+            if(pLW != 0)
+            {
+            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
+            break;
+            }
+        }
+    }
+    return frtn;
+}
+
+double getoff(Road * pRoad,int nlane)
+{
+    double off = getwidth(pRoad,nlane)/2.0;
+    if(nlane < 0)off = off * (-1.0);
+ //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    int i;
+    if(nlane > 0)
+        off = off + getwidth(pRoad,0)/2.0;
+    else
+        off = off - getwidth(pRoad,0)/2.0;
+    if(nlane > 0)
+    {
+        for(i=1;i<nlane;i++)
+        {
+            off = off + getwidth(pRoad,i);
+        }
+    }
+    else
+    {
+        for(i=-1;i>nlane;i--)
+        {
+            off = off - getwidth(pRoad,i);
+        }
+    }
+//    return 0;
+    return off;
+
+}
+
+
+namespace  iv {
+
+struct lanewidthabcd
+{
+    int nlane;
+    double A;
+    double B;
+    double C;
+    double D;
+};
+}
+
+double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
+{
+    double frtn = 0;
+
+
+    int i;
+    int nlanecount = xvectorlwa.size();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == xvectorlwa.at(i).nlane)
+        {
+            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
+            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
+            break;
+        }
+    }
+    return frtn;
+}
+
+
+
+std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
+{
+    std::vector<iv::lanewidthabcd> xvectorlwa;
+    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
+    int i;
+
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+//    if(pRoad->GetLaneSectionCount() > 1)
+//    {
+//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+//        {
+//            if(s>pRoad->GetLaneSection(i+1)->GetS())
+//            {
+//                pLS = pRoad->GetLaneSection(i+1);
+//            }
+//            else
+//            {
+//                break;
+//            }
+//        }
+//    }
+
+    int nlanecount = pLS->GetLaneCount();
+
+    for(i=0;i<nlanecount;i++)
+    {
+        iv::lanewidthabcd xlwa;
+
+
+        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
+        xlwa.nlane = pLS->GetLane(i)->GetId();
+        if(pLW != 0)
+        {
+
+            xlwa.A = pLW->GetA();
+            xlwa.B = pLW->GetB();
+            xlwa.C = pLW->GetC();
+            xlwa.D = pLW->GetD();
+
+        }
+        else
+        {
+            xlwa.A = 0;
+            xlwa.B = 0;
+            xlwa.C = 0;
+            xlwa.D = 0;
+        }
+
+ //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
+
+    }
+    return xvectorlwa;
+
+}
+
+
+inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
+{
+    int i;
+    double off = 0;
+    double a,b,c,d;
+    if(xvectorIndex.size() == 0)return off;
+
+    for(i=0;i<(xvectorIndex.size()-1);i++)
+    {
+
+        a = xvectorLWA[xvectorIndex[i]].A;
+        b = xvectorLWA[xvectorIndex[i]].B;
+        c = xvectorLWA[xvectorIndex[i]].C;
+        d = xvectorLWA[xvectorIndex[i]].D;
+        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
+        {
+            off = off + a + b*s + c*s*s + d*s*s*s;
+        }
+        else
+        {
+            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+        }
+    }
+    i = xvectorIndex.size()-1;
+    a = xvectorLWA[xvectorIndex[i]].A;
+    b = xvectorLWA[xvectorIndex[i]].B;
+    c = xvectorLWA[xvectorIndex[i]].C;
+    d = xvectorLWA[xvectorIndex[i]].D;
+    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+    if(nlane < 0) off = off*(-1.0);
+//    std::cout<<"s is "<<s<<std::endl;
+//    std::cout<<" off is "<<off<<std::endl;
+    return off;
+
+}
+
+double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+{
+    double fwidth = 0;
+    int i;
+    double a,b,c,d;
+
+    int nsize = xvectorLWA.size();
+    for(i=0;i<nsize;i++)
+    {
+        if(nlane*(xvectorLWA[i].nlane)>0)
+        {
+            a = xvectorLWA[i].A;
+            b = xvectorLWA[i].B;
+            c = xvectorLWA[i].C;
+            d = xvectorLWA[i].D;
+            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+        }
+    }
+    return fwidth;
+
+}
+
+int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
+{
+    if(pRoad->GetLaneSectionCount() < 1)return -1;
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+    int i;
+
+    if(pRoad->GetLaneSectionCount() > 1)
+    {
+        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+        {
+            if(s>pRoad->GetLaneSection(i+1)->GetS())
+            {
+                pLS = pRoad->GetLaneSection(i+1);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    nori = 0;
+    ntotal = 0;
+    fSpeed = -1; //if -1 no speedlimit for map
+
+    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.
+
+    pRoad->GetRoadNoavoid(s,bNoavoid);
+
+    int nlanecount = pLS->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        int nlaneid = pLS->GetLane(i)->GetId();
+
+        if(nlaneid == nlane)
+        {
+            Lane * pLane = pLS->GetLane(i);
+            if(pLane->GetLaneSpeedCount() > 0)
+            {
+                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
+                int j;
+                int nspeedcount = pLane->GetLaneSpeedCount();
+                for(j=0;j<(nspeedcount -1);j++)
+                {
+                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
+                    {
+                        pLSpeed = pLane->GetLaneSpeed(j+1);
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
+
+            }
+        }
+        if(nlane<0)
+        {
+
+            if(nlaneid < 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid<nlane)nori++;
+                }
+            }
+
+
+        }
+        else
+        {
+            if(nlaneid > 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid > nlane)nori++;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
+{
+    std::vector<int> xvectorindex;
+    int i;
+    if(nlane >= 0)
+    {
+    for(i=0;i<=nlane;i++)
+    {
+       int j;
+       int nsize = xvectorLWA.size();
+       for(j=0;j<nsize;j++)
+       {
+           if(i == xvectorLWA.at(j).nlane )
+           {
+               xvectorindex.push_back(j);
+               break;
+           }
+       }
+    }
+    }
+    else
+    {
+        for(i=0;i>=nlane;i--)
+        {
+           int j;
+           int nsize = xvectorLWA.size();
+           for(j=0;j<nsize;j++)
+           {
+               if(i == xvectorLWA.at(j).nlane )
+               {
+                   xvectorindex.push_back(j);
+                   break;
+               }
+           }
+        }
+    }
+    return xvectorindex;
+}
+
+void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
+{
+    if(xps.mpRoad->GetRoadBorrowCount() == 0)
+    {
+        return;
+    }
+
+    Road * pRoad = xps.mpRoad;
+    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
+    unsigned int i;
+    unsigned int nPPCount = xvectorPP.size();
+    for(i=0;i<nborrowsize;i++)
+    {
+        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
+        if(pborrow == NULL)
+        {
+            std::cout<<"warning:can't get borrow"<<std::endl;
+            continue;
+        }
+        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
+        {
+            unsigned int j;
+            double soffset = pborrow->GetS();
+            double borrowlen = pborrow->GetLength();
+            for(j=0;j<xvectorPP.size();j++)
+            {
+                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
+                {
+                    xvectorPP[j].mbBoringRoad = true;
+                }
+            }
+        }
+    }
+}
+
+void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
+                     const int nchang1,const int nchang2,const int nchangpoint)
+{
+    if(xvectorPP.size()<2)return;
+    bool bInlaneavoid = false;
+    int i;
+    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
+    {
+        if(xps.mpRoad->GetRoadLength()<30)
+        {
+            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
+            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
+                bInlaneavoid = false;
+            else
+                bInlaneavoid = true;
+        }
+        else
+        {
+            bInlaneavoid = true;
+        }
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
+    }
+
+    if(bInlaneavoid == false)
+    {
+        int nvpsize = xvectorPP.size();
+        for(i=0;i<nvpsize;i++)
+        {
+            xvectorPP.at(i).bInlaneAvoid = false;
+            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+        }
+    }
+    else
+    {
+      int nvpsize = xvectorPP.size();
+      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
+      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
+      {
+          if(xps.mbStartToMainChange == true)
+          {
+              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+
+          }
+          if(xps.mbMainToEndChange)
+          {
+              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+          }
+      }
+
+      for(i=0;i<nvpsize;i++)
+      {
+          if(xvectorPP.at(i).bInlaneAvoid)
+          {
+              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
+              if(foff < 0.3)
+              {
+                  xvectorPP.at(i).bInlaneAvoid = false;
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+              }
+              else
+              {
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
+              }
+          }
+      }
+    }
+
+}
+
+
+double getoff(Road * pRoad,int nlane,const double s)
+{
+    int i;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    double s_section = 0;
+
+    double foff = 0;
+
+    for(i=0;i<nLSCount;i++)
+    {
+
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(i<(nLSCount -1))
+        {
+            if(pRoad->GetLaneSection(i+1)->GetS()<s)
+            {
+                continue;
+            }
+        }
+        s_section = pLS->GetS();
+        int nlanecount = pLS->GetLaneCount();
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            Lane * pLane = pLS->GetLane(j);
+
+            int k;
+            double s_lane = s-s_section;
+
+
+            if(pLane->GetId() != 0)
+            {
+
+                for(k=0;k<pLane->GetLaneWidthCount();k++)
+                {
+                    if(k<(pLane->GetLaneWidthCount()-1))
+                    {
+                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
+                        {
+                            continue;
+                        }
+                    }
+                    s_lane = pLane->GetLaneWidth(k)->GetS();
+                    break;
+                }
+                LaneWidth * pLW  = pLane->GetLaneWidth(k);
+                if(pLW == 0)
+                {
+                    std::cout<<"not find LaneWidth"<<std::endl;
+                    break;
+                }
+
+                double fds = s - s_lane - s_section;
+                double fwidth= pLW->GetA() + pLW->GetB() * fds
+                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
+
+//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
+//                {
+//                    qDebug("fs is %f width is %f",fds,fwidth);
+//                }
+                if(nlane == pLane->GetId())
+                {
+                    foff = foff + fwidth/2.0;
+                }
+                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
+                {
+                    foff = foff + fwidth;
+                }
+
+            }
+
+        }
+
+
+        break;
+
+    }
+
+    if(pRoad->GetLaneOffsetCount()>0)
+    {
+
+        int nLOCount = pRoad->GetLaneOffsetCount();
+        int isel = -1;
+        for(i=0;i<(nLOCount-1);i++)
+        {
+            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
+            {
+                isel = i;
+                break;
+            }
+        }
+        if(isel < 0)isel = nLOCount-1;
+        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
+        double s_off = s - pLO->GetS();
+        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
+                +pLO->Getd() * s_off * s_off * s_off;
+        foff = foff - off1;
+    }
+
+    if(nlane<0)foff = foff*(-1);
+    return foff;
+}
+
+
+std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
+    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
+    int nchange1,nchange2;
+    int nlane1,nlane2,nlane3;
+    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
+    {
+        xps.mainsel = xps.mnStartLaneSel;
+        xps.mbStartToMainChange = false;
+        xps.mbMainToEndChange = false;
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength() < 100)
+        {
+            xps.mainsel = xps.mnEndLaneSel;
+            xps.mbStartToMainChange = true;
+            xps.mbMainToEndChange = false;
+        }
+    }
+
+    double off1,off2,off3;
+    if(xps.mnStartLaneSel < 0)
+    {
+    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+    off2 = getoff(xps.mpRoad,xps.mainsel);
+    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+    nlane1 = xps.mnStartLaneSel;
+    nlane2 = xps.mainsel;
+    nlane3 = xps.mnEndLaneSel;
+    }
+    else
+    {
+        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+        off2 = getoff(xps.mpRoad,xps.mainsel);
+        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+        nlane3 = xps.mnStartLaneSel;
+        nlane2 = xps.mainsel;
+        nlane1 = xps.mnEndLaneSel;
+    }
+
+    int nchangepoint = 300;
+    if((nchangepoint * 3) > xvPP.size())
+    {
+        nchangepoint = xvPP.size()/3;
+    }
+    int nsize = xvPP.size();
+
+    nchange1 = nsize/3;
+    if(nchange1>500)nchange1 = 500;
+    nchange2 = nsize*2/3;
+    if( (nsize-nchange2)>500)nchange2 = nsize-500;
+//    if(nsize < 1000)
+//    {
+//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
+//        return xvectorPP;
+//    }
+    double x,y;
+    int i;
+    if(xps.mainsel < 0)
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 1;
+                }
+                else
+                {
+                    pp.nlrchange = 2;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            xvectorPP.push_back(pp);
+        }
+
+
+    }
+    else
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 2;
+                }
+                else
+                {
+                    pp.nlrchange = 1;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1;
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            pp.hdg = pp.hdg + M_PI;
+            xvectorPP.push_back(pp);
+        }
+
+//        for(i=0;i<(nchange1- nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off1;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            if(nlane1 == nlane2)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane1 < nlane2)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange1)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off2;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            if(nlane2 == nlane3)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane2 < nlane3)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange2)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off3;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+
+    }
+
+
+
+    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
+
+    if(xps.mpRoad->GetRoadBorrowCount()>0)
+    {
+        CalcBorringRoad(xps,xvectorPP);
+    }
+
+
+    if(xps.mnStartLaneSel > 0)
+    {
+        std::vector<PlanPoint> xvvPP;
+        int nvsize = xvectorPP.size();
+        for(i=0;i<nvsize;i++)
+        {
+            xvvPP.push_back(xvectorPP.at(nvsize-1-i));
+        }
+        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
+        return xvvPP;
+    }
+
+//    pRoad->GetLaneSection(xps.msectionid)
+
+    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
+    return xvectorPP;
+}
+
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
+{
+    if(pRoad->GetSignalCount() == 0)return;
+    int nsigcount = pRoad->GetSignalCount();
+    int i;
+    for(i=0;i<nsigcount;i++)
+    {
+        Signal * pSig = pRoad->GetSignal(i);
+        int nfromlane = -100;
+        int ntolane = 100;
+        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
+        if(pSig_laneValidity != 0)
+        {
+            nfromlane = pSig_laneValidity->GetfromLane();
+            ntolane = pSig_laneValidity->GettoLane();
+        }
+        if((nlane < 0)&&(nfromlane >= 0))
+        {
+            continue;
+        }
+        if((nlane > 0)&&(ntolane<=0))
+        {
+            continue;
+        }
+
+        double s = pSig->Gets();
+        double fpos = s/pRoad->GetRoadLength();
+        if(nlane > 0)fpos = 1.0 -fpos;
+        int npos = fpos *xvectorPP.size();
+        if(npos <0)npos = 0;
+        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos > 0)npos--;
+            else break;
+        }
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos < (xvectorPP.size()-1))npos++;
+            else break;
+        }
+        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
+
+
+    }
+}
+
+static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
+                                          Road * pRoad_obj,GeometryBlock * pgeob_obj,
+                                           const double x_now,const double y_now,const double head,
+                                           double nearx,double neary, double nearhead,
+                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
+{
+
+  std::vector<PlanPoint> xvectorPPs;
+
+  double fspace = 0.1;
+
+  if(flen<2000)fspace = 0.1;
+  else
+  {
+      if(flen<5000)fspace = 0.3;
+      else
+      {
+          if(flen<10000)fspace = 0.5;
+          else
+              fspace = 1.0;
+      }
+  }
+
+  int i;
+
+
+//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
+  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
+
+
+  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
+
+  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
+
+  if(xpathsection[0].mainsel < 0)
+  {
+  for(i=indexstart;i<xvPP.size();i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+  }
+  }
+  else
+  {
+
+      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
+      {
+          PlanPoint pp;
+          pp = xvPP.at(i);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  int npathlast = xpathsection.size() - 1;
+//  while(npathlast >= 0)
+//  {
+//    if(npathlast == 0)break;
+//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
+//  }
+  for(i=1;i<(npathlast);i++)
+  {
+//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
+//          {
+//            continue;
+//          }
+//      }
+//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
+//          {
+//            break;
+//          }
+//      }
+ //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
+      xvectorPP = GetPoint( xpathsection[i],fspace);
+      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
+//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
+//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
+//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
+      int j;
+      for(j=0;j<xvPP.size();j++)
+      {
+
+          PlanPoint pp;
+          pp = xvPP.at(j);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
+
+
+//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
+  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
+  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
+  int nlastsize;
+  if(xpathsection[npathlast].mainsel<0)
+  {
+      nlastsize = indexend;
+  }
+  else
+  {
+      if(indexend>0) nlastsize = xvPP.size() - indexend;
+      else nlastsize = xvPP.size();
+  }
+  for(i=0;i<nlastsize;i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+
+  }
+
+  //Find StartPoint
+//  if
+
+//  int a = 1;
+
+
+
+
+  return xvectorPPs;
+}
+
+std::vector<PlanPoint> gTempPP;
+int getPoint(double q[3], double x, void* user_data) {
+//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
+    PlanPoint pp;
+    pp.x = q[0];
+    pp.y = q[1];
+    pp.hdg = q[2];
+    pp.dis = x;
+    pp.speed = *((double *)user_data);
+    gTempPP.push_back(pp);
+//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
+    return 0;
+}
+
+/**
+ * @brief getlanesel
+ * @param nSel
+ * @param pLaneSection
+ * @param nlr
+ * @return
+ */
+int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
+{
+    int nlane = nSel;
+    int mainselindex = 0;
+    if(nlr == 2)nlane = nlane*(-1);
+    int nlanecount = pLaneSection->GetLaneCount();
+
+    int i;
+    int nTrueSel = nSel;
+    if(nTrueSel <= 1)nTrueSel= 1;
+    int nCurSel = 1;
+    if(nlr ==  2)nCurSel = nCurSel *(-1);
+    bool bOK = false;
+    int nxsel = 1;
+    int nlasttid = 0;
+    bool bfindlast = false;
+    while(bOK == false)
+    {
+        bool bHaveDriving = false;
+        int ncc = 0;
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLaneSection->GetLane(i);
+            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
+            {
+                if((nlr == 1)&&(pLane->GetId()>0))
+                {
+                    ncc++;
+                }
+                if((nlr == 2)&&(pLane->GetId()<0))
+                {
+                    ncc++;
+                }
+                if(ncc == nxsel)
+                {
+                    bHaveDriving = true;
+                    bfindlast = true;
+                    nlasttid = pLane->GetId();
+                    break;
+                }
+            }
+        }
+        if(bHaveDriving == true)
+        {
+            if(nxsel == nTrueSel)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                nxsel++;
+            }
+        }
+        else
+        {
+            if(bfindlast)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                std::cout<<"can't find lane."<<std::endl;
+                return 0;
+            }
+            //Use Last
+        }
+
+    }
+
+    return 0;
+
+    int k;
+    bool bFind = false;
+    while(bFind == false)
+    {
+        for(k=0;k<nlanecount;k++)
+        {
+            Lane * pLane = pLaneSection->GetLane(k);
+            if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
+            {
+                bFind = true;
+                mainselindex = k;
+                break;
+            }
+        }
+        if(bFind)continue;
+        if(nlr == 1)nlane--;
+        else nlane++;
+        if(nlane == 0)
+        {
+            std::cout<<" Fail. can't find lane"<<std::endl;
+            break;
+        }
+    }
+    return nlane;
+}
+
+
+
+void checktrace(std::vector<PlanPoint> & xPlan)
+{
+    int i;
+    int nsize = xPlan.size();
+    for(i=1;i<nsize;i++)
+    {
+        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
+        if(dis > 0.3)
+        {
+            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
+
+            int ncount = dis/0.1;
+            int j;
+            for(j=1;j<ncount;j++)
+            {
+                double x, y;
+
+                PlanPoint pp;
+                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
+                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
+                pp.hdg = hdg;
+                xPlan.insert(xPlan.begin()+i+j-1,pp);
+
+            }
+            qDebug("dis is %f",dis);
+        }
+    }
+}
+
+
+
+void ChangeLane(std::vector<PlanPoint> & xvectorPP)
+{
+    int i = 0;
+    int nsize = xvectorPP.size();
+    for(i=0;i<nsize;i++)
+    {
+        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
+        {
+
+        }
+        else
+        {
+            int k;
+            for(k=i;k<(nsize-1);k++)
+            {
+                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
+                {
+                    break;
+                }
+            }
+            int nnum = k-i;
+            int nchangepoint = 300;
+            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
+                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
+            const double fMAXCHANGE = 100;
+            if(froadlen<fMAXCHANGE)
+            {
+                nchangepoint = nnum;
+            }
+            else
+            {
+                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
+            }
+
+            qDebug(" road %d len is %f changepoint is %d nnum is %d",
+                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
+
+            int nstart = i + nnum/2 -nchangepoint/2;
+            int nend = i+nnum/2 + nchangepoint/2;
+            if(nnum<300)
+            {
+                nstart = i;
+                nend = k;
+            }
+            int j;
+            for(j=i;j<nstart;j++)
+            {
+                xvectorPP[j].x = xvectorPP[j].mfSecx;
+                xvectorPP[j].y = xvectorPP[j].mfSecy;
+            }
+            nnum = nend - nstart;
+            for(j=nstart;j<nend;j++)
+            {
+                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
+                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
+                double foff = fdis *(j-nstart)/nnum;
+                if(xvectorPP[j].nlrchange == 1)
+                {
+//                    std::cout<<"foff is "<<foff<<std::endl;
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
+                }
+                else
+                {
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
+                }
+            }
+            i =k;
+
+        }
+    }
+}
+
+#include <QFile>
+
+/**
+ * @brief MakePlan         全局路径规划
+ * @param pxd              有向图
+ * @param pxodr            OpenDrive地图
+ * @param x_now            当前x坐标
+ * @param y_now            当前y坐标
+ * @param head             当前航向
+ * @param x_obj            目标点x坐标
+ * @param y_obj            目标点y坐标
+ * @param obj_dis          目标点到路径的距离
+ * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
+ * @param xPlan            返回的轨迹点
+ * @return                 0 成功  <0 失败
+ */
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
+{
+    double  s;double nearx;
+    double  neary;double  nearhead;
+    Road * pRoad;GeometryBlock * pgeob;
+    Road * pRoad_obj;GeometryBlock * pgeob_obj;
+    double  s_obj;double nearx_obj;
+    double  neary_obj;double  nearhead_obj;
+    int lr_end = 2;
+    int lr_start = 2;
+    double fs1,fs2;
+//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+
+    std::vector<iv::nearroad> xvectornearstart;
+    std::vector<iv::nearroad> xvectornearend;
+
+    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
+    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
+
+    if(xvectornearstart.size()<1)
+    {
+        std::cout<<" plan fail. can't find start point."<<std::endl;
+        return -1;
+    }
+    if(xvectornearend.size() < 1)
+    {
+        std::cout<<" plan fail. can't find end point."<<std::endl;
+        return -2;
+    }
+
+    std::vector<std::vector<PlanPoint>> xvectorplans;
+    std::vector<double> xvectorroutedis;
+
+    int i;
+    int j;
+    for(i=0;i<(int)xvectornearstart.size();i++)
+    {
+        for(j=0;j<(int)xvectornearend.size();j++)
+        {
+            iv::nearroad * pnearstart = &xvectornearstart[i];
+            iv::nearroad * pnearend = &xvectornearend[j];
+            pRoad = pnearstart->pObjRoad;
+            pgeob = pnearstart->pgeob;
+            pRoad_obj = pnearend->pObjRoad;
+            pgeob_obj = pnearend->pgeob;
+            lr_start = pnearstart->lr;
+            lr_end = pnearend->lr;
+
+            nearx = pnearstart->nearx;
+            neary = pnearstart->neary;
+
+            nearx_obj = pnearend->nearx;
+            neary_obj = pnearend->neary;
+
+            bool bNeedDikstra = true;
+            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
+            {
+                std::vector<PlanPoint> xvPP = GetPoint(pRoad);
+                int nindexstart = indexinroadpoint(xvPP,nearx,neary);
+                int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
+                int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
+                AddSignalMark(pRoad,nlane,xvPP);
+                if((nindexend >nindexstart)&&(lr_start == 2))
+                {
+                    bNeedDikstra = false;
+                }
+                if((nindexend <nindexstart)&&(lr_start == 1))
+                {
+                    bNeedDikstra = false;
+                }
+                if(bNeedDikstra == false)
+                {
+
+                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
+                    std::vector<int> xvectorindex1;
+
+                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
+
+                    double foff = getoff(pRoad,nlane);
+
+                    foff = foff + 0.0;
+                    std::vector<PlanPoint> xvectorPP;
+                    int i = nindexstart;
+                    while(i!=nindexend)
+                    {
+
+                        if(lr_start == 2)
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff *(-1.0);
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+                            xvectorPP.push_back(pp);
+                            i++;
+                        }
+                        else
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.hdg = pp.hdg + M_PI;
+
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff;
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+
+                            xvectorPP.push_back(pp);
+                            i--;
+                        }
+                    }
+
+                    for(i=0;i<(int)xvectorPP.size();i++)
+                    {
+                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
+                    }
+
+                    pathsection xps;
+                    xps.mbStartToMainChange = false;
+                    xps.mbMainToEndChange = false;
+         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
+                    xPlan = xvectorPP;
+
+                    xvectorplans.push_back(xvectorPP);
+                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);
+
+                }
+            }
+
+            if(bNeedDikstra)
+            {
+                bool bSuc = false;
+            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
+            if(xpath.size()<1)
+            {
+                continue;
+            }
+            if(bSuc == false)
+            {
+                continue;
+            }
+            double flen = pxd->getpathlength(xpath);
+            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
+
+            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
+                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
+
+            ChangeLane(xvectorPP);
+            xvectorplans.push_back(xvectorPP);
+            xvectorroutedis.push_back(flen);
+
+            }
+
+        }
+    }
+
+    if(xvectorplans.size() > 0)
+    {
+        if(xvectorplans.size() == 1)
+        {
+            xPlan = xvectorplans[0];
+        }
+        else
+        {
+            int nsel = 0;
+            double fdismin = xvectorroutedis[0];
+            for(i=1;i<(int)xvectorroutedis.size();i++)
+            {
+                if(xvectorroutedis[i]<fdismin)
+                {
+                    nsel = i;
+                }
+            }
+            xPlan = xvectorplans[nsel];
+        }
+        return 0;
+    }
+    std::cout<<" plan fail. can't find path."<<std::endl;
+    return -1000;
+
+}

src/driver/driver_map_xodrload/globalplan.h

@@ -1,26 +1,26 @@
-#ifndef GLOBALPLAN_H
-#define GLOBALPLAN_H
-#include "OpenDrive/OpenDrive.h"
-
-#include <vector>
-
-#include "xodrfunc.h"
-
-#include "xodrdijkstra.h"
-
-#include "planpoint.h"
-
-class LaneChangePoint
-{
-    int nRoadID = -1;
-    double mS = 0;
-};
-
-int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh);
-
-int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-             const double x_obj,const double y_obj,const double & obj_dis,
-             const double srcnearthresh,const double dstnearthresh,
-             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
-#endif // GLOBALPLAN_H
+#ifndef GLOBALPLAN_H
+#define GLOBALPLAN_H
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class LaneChangePoint
+{
+    int nRoadID = -1;
+    double mS = 0;
+};
+
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh);
+
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+#endif // GLOBALPLAN_H

src/driver/driver_map_xodrload/gnss_coordinate_convert.cpp

@@ -1,153 +1,153 @@
-#include <gnss_coordinate_convert.h>
-
-void gps2xy(double J4, double K4, double *x, double *y)
-{
-    int L4 = (int)((K4 - 1.5) / 3) + 1;
-    double M4 = K4 - (L4 * 3);
-    double N4 = sin(J4 * 3.1415926536 / 180);
-    double O4 = cos(J4 * 3.1415926536 / 180);
-    double P4 = tan(J4 * 3.1415926536 / 180);
-    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
-    double R4 = sqrt(0.006738525414683) * O4;
-    double S4 = sqrt(1 + R4 * R4);
-    double T4 = 6399698.901783 / S4;
-    double U4 = (T4 / S4) / S4;
-    double V4 = O4 * M4 * 3.1415926536 / 180;
-    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
-    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
-    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
-    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
-
-    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
-    *x = 500000 + T4 * V4 * (Y4 + Z4);
-}
-
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-{
-    int ProjNo = 0; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double a, f, e2, ee, NN, T, C, A, M, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    ZoneWide = 6; ////6度带宽
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ProjNo = (int)(longitude / ZoneWide);
-    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI;
-    latitude0 = 0;
-    longitude1 = longitude * iPI; //经度转换为弧度
-    latitude1 = latitude * iPI; //纬度转换为弧度
-    e2 = 2 * f - f * f;
-    ee = e2 * (1.0 - e2);
-    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-    T = tan(latitude1)*tan(latitude1);
-    C = ee * cos(latitude1)*cos(latitude1);
-    A = (longitude1 - longitude0)*cos(latitude1);
-    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-        *e2 / 1024)*sin(2 * latitude1)
-        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-    X0 = 1000000L * (ProjNo + 1) + 500000L;
-    Y0 = 0;
-    xval = xval + X0; yval = yval + Y0;
-    *X = xval;
-    *Y = yval;
-}
-
-
-
-//=======================zhaobo0904
-#define PI  3.14159265358979
-void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
-{
-    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
-    double a = 6378140.0;
-    double e2 = 0.006694384999588;
-    double ep2 = e2/(1-e2);
-
-    // 原点所在经度
-    double lon_origin = 6.0*int(lon/6) + 3.0;
-    lon_origin *= PI / 180.0;
-
-    double k0 = 0.9996;
-
-    // 角度转弧度
-    double lat1 = lat * PI / 180.0;
-    double lon1 = lon * PI / 180.0;
-
-
-    // 经线在该点处的曲率半径,
-    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
-
-
-    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
-    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
-    // 首先计算前四项的系数 a1~a4.
-    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
-    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
-    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
-    double a4 = (35*e2*e2*e2)/3072;
-    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
-
-    // 辅助量
-    double T = tan(lat1)*tan(lat1);
-    double C = ep2*cos(lat1)*cos(lat1);
-    double A = (lon1 - lon_origin)*cos(lat1);
-
-    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
-    *Y = M + N * tan(lat1) * ((A*A)/2 +
-                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
-                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
-
-
-    *Y *= k0;
-    return;
-}
-//==========================================================
-
-
-
-
-
-//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
-{
-    int ProjNo; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ZoneWide = 6; ////6度带宽
-    ProjNo = (int)(X / 1000000L); //查找带号
-    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI; //中央经线
-    X0 = ProjNo * 1000000L + 500000L;
-    Y0 = 0;
-    xval = X - X0; yval = Y - Y0; //带内大地坐标
-    e2 = 2 * f - f * f;
-    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
-    ee = e2 / (1 - e2);
-    M = yval;
-    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
-    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
-                4 * u)
-            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
-    C = ee * cos(fai)*cos(fai);
-    T = tan(fai)*tan(fai);
-    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
-    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
-                                                                                       (fai)*sin(fai)));
-    D = xval / NN;
-    //计算经度(Longitude) 纬度(Latitude)
-    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
-                               *D*D*D*D / 120) / cos(fai);
-    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
-                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
-    //转换为度 DD
-    *longitude = longitude1 / iPI;
-    *latitude = latitude1 / iPI;
-}
+#include <gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/driver/driver_map_xodrload/gnss_coordinate_convert.h

@@ -1,27 +1,27 @@
-#pragma once
-#ifndef _IV_PERCEPTION_GNSS_CONVERT_
-#define _IV_PERCEPTION_GNSS_CONVERT_
-
-#include <math.h>
-//double nmeaConvert2Lat(string lat)
-//{
-//	Console.WriteLine(lat);
-//	double nmea_d = double.Parse(lat.Substring(0, 2));
-//	double nmea_m = double.Parse(lat.Substring(2, 6));
-//	return nmea_d + nmea_m / 60;
-//}
-//
-//double nmeaConvert2Lon(string lon)
-//{
-//	Console.WriteLine(lon);
-//	double nmea_d = double.Parse(lon.Substring(0, 3));
-//	double nmea_m = double.Parse(lon.Substring(3, 7));
-//	return nmea_d + nmea_m / 60;
-//}
-
-void gps2xy(double , double , double *, double *);
-void GaussProjCal(double longitude, double latitude, double *X, double *Y);
-void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
-
-#endif // !_IV_PERCEPTION_GNSS_CONVERT_
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/driver/driver_map_xodrload/main.cpp

@@ -1,1201 +1,1252 @@
-#include <QCoreApplication>
-#include <math.h>
-#include <string>
-#include <thread>
-
-#include <QFile>
-
-#include "OpenDrive/OpenDrive.h"
-#include "OpenDrive/OpenDriveXmlParser.h"
-
-#include "globalplan.h"
-
-#include "gpsimu.pb.h"
-
-#include "mapdata.pb.h"
-
-#include "v2x.pb.h"
-
-#include "modulecomm.h"
-
-#include "xmlparam.h"
-
-#include "gps_type.h"
-
-#include "xodrdijkstra.h"
-
-#include "gnss_coordinate_convert.h"
-
-#include "ivexit.h"
-
-#include "ivversion.h"
-
-#include "ivbacktrace.h"
-
-#include <signal.h>
-
-#include "service_roi_xodr.h"
-#include "ivservice.h"
-
-OpenDrive mxodr;
-xodrdijkstra * gpxd;
-bool gmapload = false;
-
-double glat0,glon0,ghead0;
-
-double gvehiclewidth = 2.0;
-
-bool gbExtendMap = true;
-
-static bool gbSideEnable = false;
-
-void * gpa;
-void * gpasrc;
-void * gpmap;
-void * gpagps;
-void * gpasimple;
-void * gpav2x;
-static void * gpanewtrace;
-
-iv::Ivfault *gfault = nullptr;
-iv::Ivlog *givlog = nullptr;
-
-static QCoreApplication * gApp;
-
-service_roi_xodr gsrx;
-
-
-namespace iv {
-struct simpletrace
-{
-    double gps_lat = 0;//纬度
-    double gps_lng = 0;//经度
-
-    double gps_x = 0;
-    double gps_y = 0;
-    double gps_z = 0;
-
-
-    double ins_heading_angle = 0;	//航向角
-};
-
-}
-/**
- *
- *
- *
- *
- * */
-bool LoadXODR(std::string strpath)
-{
-    OpenDriveXmlParser xp(&mxodr);
-    xp.ReadFile(strpath);
-    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
-    if(mxodr.GetRoadCount() < 1)
-    {
-        gmapload = true;
-        return false;
-    }
-
-
-    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
-    gpxd = pxd;
-//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);
-
-//    pxd->getgpspoint(10001,2,30012,2,xpath);
-
-    int i;
-    double  nlenth = 0;
-    int nroadsize = mxodr.GetRoadCount();
-    for(i=0;i<nroadsize;i++)
-    {
-        Road * px = mxodr.GetRoad(i);
-        nlenth = nlenth + px->GetRoadLength();
-        int bloksize = px->GetGeometryBlockCount();
-        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
-        {
-            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
-            double a = p->GetuA();
-            a = p->GetuB();
-            a = p->GetuC();
-            a = p->GetuD();
-            a = p->GetvA();
-        }
-
-
-    }
-
-//    void Header::GetAllParams(unsigned short int &revMajor, unsigned short int &revMinor, string &name, float &version, string &date,
-//                              double &north, double &south, double &east,double &west,double &lat0,double &lon0, double & hdg0)
-
-
-    unsigned short int revMajor,revMinor;
-    std::string name,date;
-    float version;
-    double north,south,east,west,lat0,lon0,hdg0;
-    if(mxodr.GetHeader() != 0)
-    {
-        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
-        glat0 = lat0;
-        glon0 = lon0;
-    }
-
-
-
-    Road * proad1 = mxodr.GetRoad(0);
-    givlog->info("road name is %s",proad1->GetRoadName().data());
-    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
-
-    gsrx.SetXODR(&mxodr);
-}
-
-class roadx
-{
-  public:
-    roadx * para;
-    std::vector<roadx> child;
-    int nlen;
-};
-
-
-#define EARTH_RADIUS 6370856.0
-
-//从1到2的距离和方向
-int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
-{
-    double a,b;
-    double LonDis,LatDis;
-    double LonRadius;
-    double Dis;
-    double angle;
-    if((lat1 == lat2)&&(lon1 == lon2))return -1;
-
-    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
-    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
-    b = lat2 - lat1; b = b*100000.0;
-    LatDis = a*b;
-    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
-    b = lon2 - lon1; b = b*100000.0;
-    LonDis = a*b;
-    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
-    angle = acos(fabs(LonDis)/Dis);
-    angle = angle * 180.0/M_PI;
-
-    if(LonDis > 0)
-    {
-        if(LatDis > 0)angle = 90.0 - angle;
-        else angle= 90.0+angle;
-    }
-    else
-    {
-        if(LatDis > 0)angle = 270.0+angle;
-        else angle = 270.0-angle;
-    }
-
-    if(pLatDis != 0)*pLatDis = LatDis;
-    if(pLonDis != 0)*pLonDis = LonDis;
-    if(pDis != 0)*pDis = Dis;
-    if(pangle != 0)*pangle = angle;
-}
-
-
-
-//==========================================================
-
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-//void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-//{
-//    int ProjNo = 0; int ZoneWide; ////带宽
-//    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-//    double a, f, e2, ee, NN, T, C, A, M, iPI;
-//    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-//    ZoneWide = 6; ////6度带宽
-//    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-//                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-//    ProjNo = (int)(longitude / ZoneWide);
-//    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-//    longitude0 = longitude0 * iPI;
-//    latitude0 = 0;
-//    longitude1 = longitude * iPI; //经度转换为弧度
-//    latitude1 = latitude * iPI; //纬度转换为弧度
-//    e2 = 2 * f - f * f;
-//    ee = e2 * (1.0 - e2);
-//    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-//    T = tan(latitude1)*tan(latitude1);
-//    C = ee * cos(latitude1)*cos(latitude1);
-//    A = (longitude1 - longitude0)*cos(latitude1);
-//    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-//        *e2 / 1024)*sin(2 * latitude1)
-//        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-//    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-//    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-//        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-//    X0 = 1000000L * (ProjNo + 1) + 500000L;
-//    Y0 = 0;
-//    xval = xval + X0; yval = yval + Y0;
-//    *X = xval;
-//    *Y = yval;
-//}
-
-#include <math.h>
-static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
-{
-    double fxdiff,fydiff;
-
-    double xoff = y*(-1);
-    double yoff = x;
-
-    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
-    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South
-
-    double fEarthRadius = 6378245.0;
-
-    double ns1d = fEarthRadius*2*M_PI/360.0;
-    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
-    double ew1d = fewRadius * 2*M_PI/360.0;
-
-    fLat = fLat0 + fydiff/ns1d;
-    fLng = fLng0 + fxdiff/ew1d;
-
-
-}
-
-void CalcXY(const double lat0,const double lon0,const double hdg0,
-              const double lat,const double lon,
-              double & x,double & y)
-{
-
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    GaussProjCal(lon,lat,&x,&y);
-    x = x - x0;
-    y = y-  y0;
-
-//    double ang,dis;
-//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
-//    double xang = hdg0 - ang;
-//    while(xang<0)xang = xang + 360.0;
-//    x = dis * cos(xang * M_PI/180.0);
-//    y = dis * sin(xang * M_PI/180.0);
-}
-
-//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
-//                const double x,const double y,const double xyhdg,
-//                double &lat,double & lon, double & hdg)
-//{
-//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
-//    hdg = hdg0 - xyhdg * 180.0/M_PI;
-//    while(hdg < 0)hdg = hdg + 360;
-//    while(hdg >= 360)hdg = hdg - 360;
-//}
-
-
-void CalcLatLon(const double lat0,const double lon0,
-                const double x,const double y,
-                double &lat,double & lon)
-{
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    double x_gps,y_gps;
-    x_gps = x0+x;
-    y_gps = y0+y;
-    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
-}
-
-
-void CalcLatLon(const double lat0,const double lon0,const double hdg0,
-                const double x,const double y,const double xyhdg,
-                double &lat,double & lon, double & hdg)
-{
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    double x_gps,y_gps;
-    x_gps = x0+x;
-    y_gps = y0+y;
-    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
- //   hdg = hdg0 -xyhdg * 270/M_PI;
-    hdg = 90 -  xyhdg* 180.0/M_PI;
-
-//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
-//    hdg = hdg0 - xyhdg * 180.0/M_PI;
-    while(hdg < 0)hdg = hdg + 360;
-    while(hdg >= 360)hdg = hdg - 360;
-}
-
-class xodrobj
-{
-public:
-    double flatsrc;
-    double flonsrc;
-    double fhgdsrc;
-    double flat;
-    double flon;
-    int lane;
-};
-
-xodrobj gsrc;
-
-void ShareMap(std::vector<iv::GPSData> navigation_data)
-{
-    if(navigation_data.size()<1)return;
-    iv::GPS_INS x;
-    x = *(navigation_data.at(0));
-    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
-    int gpssize = sizeof(iv::GPS_INS);
-    int i;
-    for(i=0;i<navigation_data.size();i++)
-    {
-        x = *(navigation_data.at(i));
-        memcpy(data+i*gpssize,&x,gpssize);
-    }
-
-    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);
-
-
-    int nsize = 100;
-    int nstep = 1;
-    if(navigation_data.size() < 100)
-    {
-        nsize = navigation_data.size();
-
-    }
-    else
-    {
-        nstep = navigation_data.size()/100;
-    }
-
-    iv::simpletrace psim[100];
-    for(i=0;i<nsize;i++)
-    {
-        x = *(navigation_data.at(i*nstep));
-        psim[i].gps_lat = x.gps_lat;
-        psim[i].gps_lng = x.gps_lng;
-        psim[i].gps_z = x.gps_z;
-        psim[i].gps_x = x.gps_x;
-        psim[i].gps_y = x.gps_y;
-        psim[i].ins_heading_angle = x.ins_heading_angle;
-    }
-    if(navigation_data.size()>100)
-    {
-        int nlast = 99;
-        x = *(navigation_data.at(navigation_data.size()-1));
-        psim[nlast].gps_lat = x.gps_lat;
-        psim[nlast].gps_lng = x.gps_lng;
-        psim[nlast].gps_z = x.gps_z;
-        psim[nlast].gps_x = x.gps_x;
-        psim[nlast].gps_y = x.gps_y;
-        psim[nlast].ins_heading_angle = x.ins_heading_angle;
-    }
-
-    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));
-
-    delete data;
-}
-
-static void ToGPSTrace(std::vector<PlanPoint> xPlan)
-{
-//    double x_src,y_src,x_dst,y_dst;
-
-
-
-//    x_src = -26;y_src = 10;
-//    x_dst = -50;y_dst = -220;
-
-    int i;
-    int nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-
-
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::GPSData data(new iv::GPS_INS);
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-                   data->gps_lng,data->ins_heading_angle);
-
-        data->index = i;
-        data->speed = xPlan[i].speed;
-        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-
-
-        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->roadMode = 0;
-        data->roadOri = xPlan[i].mnLaneori;
-
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-        data->mnLaneChangeMark = xPlan[i].lanmp;
-
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
-
-        mapdata.push_back(data);
-
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                 i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,0,0);
-        if(bFileOpen)  xfile.write(strline);
-    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;
-
-    }
-    if(bFileOpen)xfile.close();
-
-    ShareMap(mapdata);
-}
-
-int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};
-
-inline bool isboringroad(int nroadid)
-{
-    int i;
-    bool brtn = false;
-    for(i=0;i<11;i++)
-    {
-        if(avoidroadid[i] == nroadid)
-        {
-            brtn = true;
-            break;
-        }
-    }
-    return brtn;
-}
-
-
-void CalcSide(std::vector<PlanPoint> & xPlan)
-{
-    const double fsidedis = 0.3;
-    const int nChangePoint = 150;
-    const int nStopPoint = 50;
-    if(xPlan.size()<nChangePoint)return;
-    bool bChange = true;
-    int i;
-    int nsize = xPlan.size();
-    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
-    {
-        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
-        {
-            std::cout<<" Because Lane is narrow, not change."<<std::endl;
-            bChange = false;
-            break;
-        }
-    }
-
-    if(bChange == false)return;
-
-    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
-    {
-        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
-    }
-
-    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
-    {
-        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
-        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
-    }
-    return;
-}
-
-void SetPlan(xodrobj xo)
-{
-
-    double x_src,y_src,x_dst,y_dst;
-
-    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
-    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);
-
-
-    std::vector<PlanPoint> xPlan;
-
-    double s;
-
-//    x_src = -5;y_src = 6;
-//    x_dst = -60;y_src = -150;
-    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);
-
-    if(nRtn < 0)
-    {
-        qDebug("plan fail.");
-        return;
-    }
-
-    int i;
-    int nSize = xPlan.size();
-
-    if(gbSideEnable)
-    {
-        CalcSide(xPlan);
-    }
-
-    if(nSize<1)
-    {
-        qDebug("plan fail.");
-        return;
-    }
-    PlanPoint xLastPoint = xPlan[nSize -1];
-    for(i=0;i<600;i++)
-    {
-        PlanPoint pp = xLastPoint;
-        double fdis = 0.1*(i+1);
-        pp.mS = pp.mS + i*0.1;
-        pp.x = pp.x + fdis * cos(pp.hdg);
-        pp.y = pp.y + fdis * sin(pp.hdg);
-        pp.nSignal = 23;
-        if(gbExtendMap)
-        {
-            xPlan.push_back(pp);
-        }
-
-    }
-    
-
-   iv::map::tracemap xtrace;
-
-    nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-    //maptrace
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    xfile.open(QIODevice::ReadWrite);
-
-    //maptrace_add
-    QFile xfile_1;
-    QString strpath_1;
-    strpath_1 = getenv("HOME");
-    strpath_1 = strpath_1 + "/map/mapadd.txt";
-    xfile_1.setFileName(strpath_1);
-    xfile_1.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::map::mappoint * pmappoint =  xtrace.add_point();
-        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
-                  gps_lon,gps_heading);
-        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
-                   gps_lat_avoidleft,gps_lon_avoidleft);
-        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
-                   gps_lat_avoidright,gps_lon_avoidright);
-
-        pmappoint->set_gps_lat(gps_lat);
-        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
-        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
-        pmappoint->set_gps_lng(gps_lon);
-        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
-        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
-        pmappoint->set_ins_heading_angle(gps_heading);
-
-        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
-
-        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
-        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
-        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
-
-        pmappoint->set_gps_x(gps_x);
-        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
-        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
-        pmappoint->set_gps_y(gps_y);
-        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
-        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
-
-        pmappoint->set_speed(xPlan[i].speed);
-        pmappoint->set_index(i);
-
-        pmappoint->set_roadori(xPlan[i].mnLaneori);
-        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
-        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
-        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
-        pmappoint->set_mflanewidth(xPlan[i].mWidth);
-        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
-        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
-
-
-        iv::GPSData data(new iv::GPS_INS);
-        data->gps_lat = gps_lat;
-        data->gps_lat_avoidleft = gps_lat_avoidleft;
-        data->gps_lat_avoidright = gps_lat_avoidright;
-        data->gps_lng = gps_lon;
-        data->gps_lng_avoidleft = gps_lon_avoidleft;
-        data->gps_lng_avoidright =  gps_lon_avoidright;
-        data->ins_heading_angle = gps_heading;
-        data->gps_x = gps_x;
-        data->gps_x_avoidleft = gps_x_avoidleft;
-        data->gps_x_avoidright = gps_x_avoidright;
-        data->gps_y = gps_y;
-        data->gps_y_avoidleft = gps_y_avoidleft;
-        data->gps_y_avoidright = gps_y_avoidright;
-        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
-        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
-
-
-//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-//                   data->gps_lng,data->ins_heading_angle);
-//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
-//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
-//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
-//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
-        data->index = i;
-        data->speed = xPlan[i].speed;
- //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
-//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
-
-        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-        data->roadOri = xPlan[i].mnLaneori;
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-
-
-        data->mbInLaneAvoid = xPlan[i].bInlaneAvoid;
-
-        if(xPlan[i].mbBoringRoad)
-        {
-            data->roadOri = 0;
-            data->roadSum = 2;
-            pmappoint->set_roadori(0);
-            pmappoint->set_roadsum(2);
-        }
-
-
-        data->mbnoavoid = xPlan[i].mbNoavoid;
-        if(data->mbnoavoid)
-        {
-            qDebug("no avoid i = %d",i);
-        }
-        data->mfCurvature = xPlan[i].mfCurvature;
-
-        data->mode2 = xPlan[i].nSignal;
-        if(data->mode2 == 3000)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
-            {
-                if(k<0)break;
-                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
-                {
-                 continue;
-                }
-                mapdata.at(k)->mode2 = 3000;
-            }
-         }
-        if(data->mode2 == 1000001)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
-            {
-                if(k<0)break;
-                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
-                {
-                 continue;
-                }
-                mapdata.at(k)->mode2 = 1000001;
-            }
-         }
-
-       pmappoint->set_mode2(data->mode2);
-       pmappoint->set_rel_x(xPlan[i].x);
-       pmappoint->set_rel_y(xPlan[i].y);
-       pmappoint->set_rel_z(0);
-       pmappoint->set_rel_yaw(xPlan[i].hdg);
-       pmappoint->set_laneid(-1);
-       pmappoint->set_roadid(xPlan[i].nRoadID);
-
-#ifdef BOAVOID
-    if(isboringroad(xPlan[i].nRoadID))
-    {
-        const int nrangeavoid = 300;
-         if((i+(nrangeavoid + 10))<nSize)
-         {
-             double fhdg1 = xPlan[i].hdg;
-             bool bavoid = true;
-//             int k;
-//             for(k=0;k<=10;k++)
-//             {
-//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
-//                 double fhdgdiff1 = fhdg5 - fhdg1;
-//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
-//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
-//                 {
-//                     bavoid = false;
-//                     break;
-//                 }
-
-//             }
-             if(bavoid)
-             {
-                 data->roadSum = 2;
-                 data->roadOri = 0;
-             }
-
-         }
-         else
-         {
-             int a = 1;
-             a++;
-         }
-    }
-#endif
-
-        mapdata.push_back(data);
-
-
-
-
-//        char strline[255];
-//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
-//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
-//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
-//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
-//        xfile.write(strline);
-    }
-
-
-    for(int j=0;j<nSize;j++)
-    {
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
-        xfile.write(strline);
-     //mapttrace1
-        char strline_1[255];
-        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
-                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
-        xfile_1.write(strline_1);
-    }
-
-    xfile.close();
-    xfile_1.close();
-    ShareMap(mapdata);
-
-    int nnewmapsize = xtrace.ByteSize();
-    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
-    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
-    {
-        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
-    }
-    else
-    {
-        std::cout<<" new trace map serialize fail."<<std::endl;
-    }
-
-
-
-
-    s = 1;
-}
-
-void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
-{
-
-    std::vector<PlanPoint> xPlan;
-
-    int i;
-
-    double fLastHdg = 0;
-
-    int ndeflane =nlane;
-
-    for(i=0;i<pxv2x->stgps_size();i++)
-    {
-
-        double x_src,y_src,x_dst,y_dst;
-
-        if(i==0)
-        {
-            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
-        }
-        else
-        {
-            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
-        }
-        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
-
-
-        std::vector<PlanPoint> xPlanPart;
-
-        double s;
-
-        //    x_src = -5;y_src = 6;
-        //    x_dst = -60;y_src = -150;
-        int nRtn = -1;
-        if(i==0)
-        {
-            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
-        }
-        else
-        {
-            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
-        }
-
-
-
-        if(nRtn < 0)
-        {
-            qDebug("plan fail.");
-            return;
-        }
-
-        int j;
-        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
-        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
-
-    }
-
-
-
-    int nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    xfile.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::GPSData data(new iv::GPS_INS);
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-                   data->gps_lng,data->ins_heading_angle);
-        data->index = i;
-        data->speed = xPlan[i].speed;
- //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-
-        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-//        data->roadSum = 1;
-//        data->roadMode = 0;
-//        data->roadOri = 0;
-
-//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
-        data->roadOri = xPlan[i].mnLaneori;
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-
-        data->mode2 = xPlan[i].nSignal;
-        if(data->mode2 == 3000)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
-            {
-                if(k<0)break;
-                mapdata.at(k)->mode2 = 3000;
-            }
-        }
-
-        mapdata.push_back(data);
-
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
-        xfile.write(strline);
-
-    }
-
-    xfile.close();
-
-    ShareMap(mapdata);
-
-}
-
-void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    if(nSize<sizeof(xodrobj))
-    {
-        std::cout<<"ListenCmd small."<<std::endl;
-        return;
-    }
-
-    xodrobj xo;
-    memcpy(&xo,strdata,sizeof(xodrobj));
-
-    givlog->debug("lat is %f", xo.flat);
-
-    xo.fhgdsrc = gsrc.fhgdsrc;
-    xo.flatsrc = gsrc.flatsrc;
-    xo.flonsrc = gsrc.flonsrc;
-
-    SetPlan(xo);
-
-
-}
-
-void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    iv::v2x::v2x xv2x;
-    if(!xv2x.ParseFromArray(strdata,nSize))
-    {
-        givlog->warn("ListernV2X Parse Error.");
-        std::cout<<"ListenV2X Parse Error."<<std::endl;
-        return;
-    }
-    if(xv2x.stgps_size()<1)
-    {
-        givlog->debug("ListenV2X no gps station.");
-        std::cout<<"ListenV2X no gps station."<<std::endl;
-        return;
-    }
-
-    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
-}
-
-void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    if(nSize<sizeof(xodrobj))
-    {
-        givlog->warn("ListenSrc small");
-        std::cout<<"ListenSrc small."<<std::endl;
-        return;
-    }
-
-    memcpy(&gsrc,strdata,sizeof(xodrobj));
-
-    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
-    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
-
-}
-
-void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    iv::gps::gpsimu  xgpsimu;
-    if(!xgpsimu.ParseFromArray(strdata,nSize))
-    {
-        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
-        return;
-    }
-
-    xodrobj xo;
-    xo.fhgdsrc = xgpsimu.heading();
-    xo.flatsrc = xgpsimu.lat();
-    xo.flonsrc = xgpsimu.lon();
-
-    gsrc = xo;
-
-    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
-    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
-}
-
-void ExitFunc()
-{
-//    gApp->quit();
-    iv::modulecomm::Unregister(gpasimple);
-    iv::modulecomm::Unregister(gpav2x);
-    iv::modulecomm::Unregister(gpagps);
-    iv::modulecomm::Unregister(gpasrc);
-    iv::modulecomm::Unregister(gpmap);
-    iv::modulecomm::Unregister(gpa);
-    std::cout<<"driver_map_xodrload exit."<<std::endl;
-    gApp->quit();
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
-}
-
-void signal_handler(int sig)
-{
-    if(sig == SIGINT)
-    {
-        ExitFunc();
-    }
-}
-
-void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
-{
-    (void)pstr_req;
-    (void)nreqsize;
-    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
-    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
-    {
-        std::shared_ptr<iv::roi::resultarray> pstr_roires;
-        gsrx.GetROI(pstr_roireq,pstr_roires);
-        int nbytesize = pstr_roires->ByteSize();
-        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
-        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
-        {
-            nressize = nbytesize;
-            std::cout<<"return res."<<std::endl;
-            return;
-        }
-        else
-        {
-            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
-        }
-    }
-    else
-    {
-        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
-        return;
-    }
-}
-
-int main(int argc, char *argv[])
-{
-    showversion("driver_map_xodrload");
-    QCoreApplication a(argc, argv);
-    gApp = &a;
-
-    RegisterIVBackTrace();
-
-    gfault = new iv::Ivfault("driver_map_xodrload");
-    givlog = new iv::Ivlog("driver_map_xodrload");
-
-    std::string strmapth,strparapath;
-    if(argc<3)
-    {
-//        strmapth = "./map.xodr";
-        strmapth = getenv("HOME");
-        strmapth = strmapth + "/map/map.xodr";
-//        strmapth = "/home/yuchuli/1226.xodr";
-        strparapath = "./driver_map_xodrload.xml";
-    }
-    else
-    {
-        strmapth = argv[1];
-        strparapath = argv[2];
-    }
-
-
-    iv::xmlparam::Xmlparam xp(strparapath);
-    xp.GetParam(std::string("he"),std::string("1"));
-    std::string strlat0 = xp.GetParam("lat0","39");
-    std::string strlon0 = xp.GetParam("lon0","117.0");
-    std::string strhdg0 = xp.GetParam("hdg0","360.0");
-
-    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
-
-    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
-
-    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
-
-    std::string strextendmap = xp.GetParam("extendmap","false");
-
-    std::string strsideenable = xp.GetParam("sideenable","false");
-
-
-    glat0 = atof(strlat0.data());
-    glon0 = atof(strlon0.data());
-    ghead0 = atof(strhdg0.data());
-
-    gvehiclewidth = atof(strvehiclewidth.data());
-
-    if(strextendmap == "true")
-    {
-        gbExtendMap = true;
-    }
-    else
-    {
-        gbExtendMap = false;
-    }
-
-    if(strsideenable == "true")
-    {
-        gbSideEnable = true;
-    }
-
-
-    LoadXODR(strmapth);
-
-    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
-    (void)serviceroi;
-
-    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
-    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
-    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
-
-
-    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
-    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
-    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
-    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
-
-
-
-
-    double x_src,y_src,x_dst,y_dst;
-
-    x_src = -26;y_src = 10;
-    x_dst = -50;y_dst = -220;
-
-    x_src = 0;y_src = 0;
-    x_dst = -23;y_dst = -18;
-    x_dst = 21;y_dst =-21;
-    x_dst =5;y_dst = 0;
-
-    x_src = -20; y_src = -1000;
-    x_dst = 900; y_dst = -630;
-
-//    x_dst = 450; y_dst = -640;
-//    x_dst = -190; y_dst = -690;
-
-//    x_src = 900; y_src = -610;
-//    x_dst = -100; y_dst = -680;
-    std::vector<PlanPoint> xPlan;
-    double s;
-//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);
-
-//    ToGPSTrace(xPlan);
-
-
-//    double lat = 39.1443880;
-//    double lon = 117.0812543;
-
-//    xodrobj xo;
-//    xo.fhgdsrc = 340;
-//    xo.flatsrc = lat; xo.flonsrc = lon;
-//    xo.flat = 39.1490196;
-//    xo.flon = 117.0806979;
-//    xo.lane = 1;
-//    SetPlan(xo);
-
-
-    void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
-
-    signal(SIGINT,signal_handler);
-
-    int nrc = a.exec();
-
-    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;
-
-    return nrc;
-}
+#include <QCoreApplication>
+#include <math.h>
+#include <string>
+#include <thread>
+
+#include <QFile>
+
+#include "OpenDrive/OpenDrive.h"
+#include "OpenDrive/OpenDriveXmlParser.h"
+
+#include "globalplan.h"
+
+#include "gpsimu.pb.h"
+
+#include "mapdata.pb.h"
+
+#include "v2x.pb.h"
+
+#include "modulecomm.h"
+
+#include "xmlparam.h"
+
+#include "gps_type.h"
+
+#include "xodrdijkstra.h"
+
+#include "gnss_coordinate_convert.h"
+
+#include "ivexit.h"
+
+#include "ivversion.h"
+
+#include "ivbacktrace.h"
+
+#include <signal.h>
+
+#include "service_roi_xodr.h"
+#include "ivservice.h"
+
+OpenDrive mxodr;
+xodrdijkstra * gpxd;
+bool gmapload = false;
+
+double glat0,glon0,ghead0;
+
+double gvehiclewidth = 2.0;
+
+bool gbExtendMap = true;
+
+static bool gbSideEnable = false;
+static bool gbSideLaneEnable = false;
+
+void * gpa;
+void * gpasrc;
+void * gpmap;
+void * gpagps;
+void * gpasimple;
+void * gpav2x;
+static void * gpanewtrace;
+
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+static QCoreApplication * gApp;
+
+service_roi_xodr gsrx;
+
+
+namespace iv {
+struct simpletrace
+{
+    double gps_lat = 0;//纬度
+    double gps_lng = 0;//经度
+
+    double gps_x = 0;
+    double gps_y = 0;
+    double gps_z = 0;
+
+
+    double ins_heading_angle = 0;	//航向角
+};
+
+}
+/**
+ *
+ *
+ *
+ *
+ * */
+bool LoadXODR(std::string strpath)
+{
+    OpenDriveXmlParser xp(&mxodr);
+    xp.ReadFile(strpath);
+    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
+    if(mxodr.GetRoadCount() < 1)
+    {
+        gmapload = true;
+        return false;
+    }
+
+
+    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
+    gpxd = pxd;
+//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);
+
+//    pxd->getgpspoint(10001,2,30012,2,xpath);
+
+    int i;
+    double  nlenth = 0;
+    int nroadsize = mxodr.GetRoadCount();
+    for(i=0;i<nroadsize;i++)
+    {
+        Road * px = mxodr.GetRoad(i);
+        nlenth = nlenth + px->GetRoadLength();
+        int bloksize = px->GetGeometryBlockCount();
+        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
+        {
+            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
+            double a = p->GetuA();
+            a = p->GetuB();
+            a = p->GetuC();
+            a = p->GetuD();
+            a = p->GetvA();
+        }
+
+
+    }
+
+//    void Header::GetAllParams(unsigned short int &revMajor, unsigned short int &revMinor, string &name, float &version, string &date,
+//                              double &north, double &south, double &east,double &west,double &lat0,double &lon0, double & hdg0)
+
+
+    unsigned short int revMajor,revMinor;
+    std::string name,date;
+    float version;
+    double north,south,east,west,lat0,lon0,hdg0;
+    if(mxodr.GetHeader() != 0)
+    {
+        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+        glat0 = lat0;
+        glon0 = lon0;
+    }
+
+
+
+    Road * proad1 = mxodr.GetRoad(0);
+    givlog->info("road name is %s",proad1->GetRoadName().data());
+    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
+
+    gsrx.SetXODR(&mxodr);
+}
+
+class roadx
+{
+  public:
+    roadx * para;
+    std::vector<roadx> child;
+    int nlen;
+};
+
+
+#define EARTH_RADIUS 6370856.0
+
+//从1到2的距离和方向
+int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
+{
+    double a,b;
+    double LonDis,LatDis;
+    double LonRadius;
+    double Dis;
+    double angle;
+    if((lat1 == lat2)&&(lon1 == lon2))return -1;
+
+    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
+    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
+    b = lat2 - lat1; b = b*100000.0;
+    LatDis = a*b;
+    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
+    b = lon2 - lon1; b = b*100000.0;
+    LonDis = a*b;
+    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
+    angle = acos(fabs(LonDis)/Dis);
+    angle = angle * 180.0/M_PI;
+
+    if(LonDis > 0)
+    {
+        if(LatDis > 0)angle = 90.0 - angle;
+        else angle= 90.0+angle;
+    }
+    else
+    {
+        if(LatDis > 0)angle = 270.0+angle;
+        else angle = 270.0-angle;
+    }
+
+    if(pLatDis != 0)*pLatDis = LatDis;
+    if(pLonDis != 0)*pLonDis = LonDis;
+    if(pDis != 0)*pDis = Dis;
+    if(pangle != 0)*pangle = angle;
+}
+
+
+
+//==========================================================
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+//void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+//{
+//    int ProjNo = 0; int ZoneWide; ////带宽
+//    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+//    double a, f, e2, ee, NN, T, C, A, M, iPI;
+//    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+//    ZoneWide = 6; ////6度带宽
+//    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+//                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+//    ProjNo = (int)(longitude / ZoneWide);
+//    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+//    longitude0 = longitude0 * iPI;
+//    latitude0 = 0;
+//    longitude1 = longitude * iPI; //经度转换为弧度
+//    latitude1 = latitude * iPI; //纬度转换为弧度
+//    e2 = 2 * f - f * f;
+//    ee = e2 * (1.0 - e2);
+//    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+//    T = tan(latitude1)*tan(latitude1);
+//    C = ee * cos(latitude1)*cos(latitude1);
+//    A = (longitude1 - longitude0)*cos(latitude1);
+//    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+//        *e2 / 1024)*sin(2 * latitude1)
+//        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+//    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+//    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+//        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+//    X0 = 1000000L * (ProjNo + 1) + 500000L;
+//    Y0 = 0;
+//    xval = xval + X0; yval = yval + Y0;
+//    *X = xval;
+//    *Y = yval;
+//}
+
+#include <math.h>
+static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
+{
+    double fxdiff,fydiff;
+
+    double xoff = y*(-1);
+    double yoff = x;
+
+    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
+    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South
+
+    double fEarthRadius = 6378245.0;
+
+    double ns1d = fEarthRadius*2*M_PI/360.0;
+    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
+    double ew1d = fewRadius * 2*M_PI/360.0;
+
+    fLat = fLat0 + fydiff/ns1d;
+    fLng = fLng0 + fxdiff/ew1d;
+
+
+}
+
+void CalcXY(const double lat0,const double lon0,const double hdg0,
+              const double lat,const double lon,
+              double & x,double & y)
+{
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(lon,lat,&x,&y);
+    x = x - x0;
+    y = y-  y0;
+
+//    double ang,dis;
+//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
+//    double xang = hdg0 - ang;
+//    while(xang<0)xang = xang + 360.0;
+//    x = dis * cos(xang * M_PI/180.0);
+//    y = dis * sin(xang * M_PI/180.0);
+}
+
+//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+//                const double x,const double y,const double xyhdg,
+//                double &lat,double & lon, double & hdg)
+//{
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+//    while(hdg < 0)hdg = hdg + 360;
+//    while(hdg >= 360)hdg = hdg - 360;
+//}
+
+
+void CalcLatLon(const double lat0,const double lon0,
+                const double x,const double y,
+                double &lat,double & lon)
+{
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+}
+
+
+void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+                const double x,const double y,const double xyhdg,
+                double &lat,double & lon, double & hdg)
+{
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+ //   hdg = hdg0 -xyhdg * 270/M_PI;
+    hdg = 90 -  xyhdg* 180.0/M_PI;
+
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+    while(hdg < 0)hdg = hdg + 360;
+    while(hdg >= 360)hdg = hdg - 360;
+}
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
+xodrobj gsrc;
+
+void ShareMap(std::vector<iv::GPSData> navigation_data)
+{
+    if(navigation_data.size()<1)return;
+    iv::GPS_INS x;
+    x = *(navigation_data.at(0));
+    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
+    int gpssize = sizeof(iv::GPS_INS);
+    int i;
+    for(i=0;i<navigation_data.size();i++)
+    {
+        x = *(navigation_data.at(i));
+        memcpy(data+i*gpssize,&x,gpssize);
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);
+
+
+    int nsize = 100;
+    int nstep = 1;
+    if(navigation_data.size() < 100)
+    {
+        nsize = navigation_data.size();
+
+    }
+    else
+    {
+        nstep = navigation_data.size()/100;
+    }
+
+    iv::simpletrace psim[100];
+    for(i=0;i<nsize;i++)
+    {
+        x = *(navigation_data.at(i*nstep));
+        psim[i].gps_lat = x.gps_lat;
+        psim[i].gps_lng = x.gps_lng;
+        psim[i].gps_z = x.gps_z;
+        psim[i].gps_x = x.gps_x;
+        psim[i].gps_y = x.gps_y;
+        psim[i].ins_heading_angle = x.ins_heading_angle;
+    }
+    if(navigation_data.size()>100)
+    {
+        int nlast = 99;
+        x = *(navigation_data.at(navigation_data.size()-1));
+        psim[nlast].gps_lat = x.gps_lat;
+        psim[nlast].gps_lng = x.gps_lng;
+        psim[nlast].gps_z = x.gps_z;
+        psim[nlast].gps_x = x.gps_x;
+        psim[nlast].gps_y = x.gps_y;
+        psim[nlast].ins_heading_angle = x.ins_heading_angle;
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));
+
+    delete data;
+}
+
+static void ToGPSTrace(std::vector<PlanPoint> xPlan)
+{
+//    double x_src,y_src,x_dst,y_dst;
+
+
+
+//    x_src = -26;y_src = 10;
+//    x_dst = -50;y_dst = -220;
+
+    int i;
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+
+        data->index = i;
+        data->speed = xPlan[i].speed;
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+
+        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->roadMode = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+        data->mnLaneChangeMark = xPlan[i].lanmp;
+
+        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+
+        mapdata.push_back(data);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,0,0);
+        if(bFileOpen)  xfile.write(strline);
+    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;
+
+    }
+    if(bFileOpen)xfile.close();
+
+    ShareMap(mapdata);
+}
+
+int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};
+
+inline bool isboringroad(int nroadid)
+{
+    int i;
+    bool brtn = false;
+    for(i=0;i<11;i++)
+    {
+        if(avoidroadid[i] == nroadid)
+        {
+            brtn = true;
+            break;
+        }
+    }
+    return brtn;
+}
+
+
+void CalcLaneSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+
+    int i;
+    int nsize = xPlan.size();
+
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+   //     double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+//        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
+void CalcSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+    bool bChange = true;
+    int i;
+    int nsize = xPlan.size();
+    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
+    {
+        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
+        {
+            std::cout<<" Because Lane is narrow, not change."<<std::endl;
+            bChange = false;
+            break;
+        }
+    }
+
+    if(bChange == false)return;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
+void SetPlan(xodrobj xo)
+{
+
+    double x_src,y_src,x_dst,y_dst;
+
+    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
+    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);
+
+
+    std::vector<PlanPoint> xPlan;
+
+    double s;
+
+//    x_src = -5;y_src = 6;
+//    x_dst = -60;y_src = -150;
+    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);
+
+    if(nRtn < 0)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+
+    int i;
+    int nSize = xPlan.size();
+
+    if(gbSideLaneEnable)
+    {
+        CalcLaneSide(xPlan);
+    }
+    else
+    {
+        if(gbSideEnable)
+        {
+            CalcSide(xPlan);
+        }
+    }
+
+    if(nSize<1)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+    PlanPoint xLastPoint = xPlan[nSize -1];
+    for(i=0;i<600;i++)
+    {
+        PlanPoint pp = xLastPoint;
+        double fdis = 0.1*(i+1);
+        pp.mS = pp.mS + i*0.1;
+        pp.x = pp.x + fdis * cos(pp.hdg);
+        pp.y = pp.y + fdis * sin(pp.hdg);
+        pp.nSignal = 23;
+        if(gbExtendMap)
+        {
+            xPlan.push_back(pp);
+        }
+
+    }
+    
+
+   iv::map::tracemap xtrace;
+
+    nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+    //maptrace
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    //maptrace_add
+    QFile xfile_1;
+    QString strpath_1;
+    strpath_1 = getenv("HOME");
+    strpath_1 = strpath_1 + "/map/mapadd.txt";
+    xfile_1.setFileName(strpath_1);
+    xfile_1.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::map::mappoint * pmappoint =  xtrace.add_point();
+        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+                   gps_lat_avoidleft,gps_lon_avoidleft);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+                   gps_lat_avoidright,gps_lon_avoidright);
+
+        pmappoint->set_gps_lat(gps_lat);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
+        pmappoint->set_gps_lng(gps_lon);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
+        pmappoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
+
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
+        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
+
+        pmappoint->set_gps_x(gps_x);
+        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
+        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
+        pmappoint->set_gps_y(gps_y);
+        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
+        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
+
+        pmappoint->set_speed(xPlan[i].speed);
+        pmappoint->set_index(i);
+
+        pmappoint->set_roadori(xPlan[i].mnLaneori);
+        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
+        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
+        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
+        pmappoint->set_mflanewidth(xPlan[i].mWidth);
+        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
+        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
+
+
+        iv::GPSData data(new iv::GPS_INS);
+        data->gps_lat = gps_lat;
+        data->gps_lat_avoidleft = gps_lat_avoidleft;
+        data->gps_lat_avoidright = gps_lat_avoidright;
+        data->gps_lng = gps_lon;
+        data->gps_lng_avoidleft = gps_lon_avoidleft;
+        data->gps_lng_avoidright =  gps_lon_avoidright;
+        data->ins_heading_angle = gps_heading;
+        data->gps_x = gps_x;
+        data->gps_x_avoidleft = gps_x_avoidleft;
+        data->gps_x_avoidright = gps_x_avoidright;
+        data->gps_y = gps_y;
+        data->gps_y_avoidleft = gps_y_avoidleft;
+        data->gps_y_avoidright = gps_y_avoidright;
+        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
+        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
+
+
+//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+//                   data->gps_lng,data->ins_heading_angle);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
+//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
+
+        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+
+        data->mbInLaneAvoid = xPlan[i].bInlaneAvoid;
+
+        if(xPlan[i].mbBoringRoad)
+        {
+            data->roadOri = 0;
+            data->roadSum = 2;
+            pmappoint->set_roadori(0);
+            pmappoint->set_roadsum(2);
+        }
+
+
+        data->mbnoavoid = xPlan[i].mbNoavoid;
+        if(data->mbnoavoid)
+        {
+            qDebug("no avoid i = %d",i);
+        }
+        data->mfCurvature = xPlan[i].mfCurvature;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 3000;
+            }
+         }
+        if(data->mode2 == 1000001)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 1000001;
+            }
+         }
+
+       pmappoint->set_mode2(data->mode2);
+       pmappoint->set_rel_x(xPlan[i].x);
+       pmappoint->set_rel_y(xPlan[i].y);
+       pmappoint->set_rel_z(0);
+       pmappoint->set_rel_yaw(xPlan[i].hdg);
+       pmappoint->set_laneid(-1);
+       pmappoint->set_roadid(xPlan[i].nRoadID);
+
+#ifdef BOAVOID
+    if(isboringroad(xPlan[i].nRoadID))
+    {
+        const int nrangeavoid = 300;
+         if((i+(nrangeavoid + 10))<nSize)
+         {
+             double fhdg1 = xPlan[i].hdg;
+             bool bavoid = true;
+//             int k;
+//             for(k=0;k<=10;k++)
+//             {
+//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
+//                 double fhdgdiff1 = fhdg5 - fhdg1;
+//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
+//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
+//                 {
+//                     bavoid = false;
+//                     break;
+//                 }
+
+//             }
+             if(bavoid)
+             {
+                 data->roadSum = 2;
+                 data->roadOri = 0;
+             }
+
+         }
+         else
+         {
+             int a = 1;
+             a++;
+         }
+    }
+#endif
+
+        mapdata.push_back(data);
+
+
+
+
+//        char strline[255];
+//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
+//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
+//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
+//        xfile.write(strline);
+    }
+
+
+    for(int j=0;j<nSize;j++)
+    {
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
+        xfile.write(strline);
+     //mapttrace1
+        char strline_1[255];
+        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
+        xfile_1.write(strline_1);
+    }
+
+    xfile.close();
+    xfile_1.close();
+    ShareMap(mapdata);
+
+    int nnewmapsize = xtrace.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
+    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
+    }
+    else
+    {
+        std::cout<<" new trace map serialize fail."<<std::endl;
+    }
+
+
+
+
+    s = 1;
+}
+
+void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
+{
+
+    std::vector<PlanPoint> xPlan;
+
+    int i;
+
+    double fLastHdg = 0;
+
+    int ndeflane =nlane;
+
+    for(i=0;i<pxv2x->stgps_size();i++)
+    {
+
+        double x_src,y_src,x_dst,y_dst;
+
+        if(i==0)
+        {
+            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
+        }
+        else
+        {
+            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
+        }
+        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
+
+
+        std::vector<PlanPoint> xPlanPart;
+
+        double s;
+
+        //    x_src = -5;y_src = 6;
+        //    x_dst = -60;y_src = -150;
+        int nRtn = -1;
+        if(i==0)
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+        else
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+
+
+
+        if(nRtn < 0)
+        {
+            qDebug("plan fail.");
+            return;
+        }
+
+        int j;
+        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
+        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
+
+    }
+
+
+
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
+
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
+
+        mapdata.push_back(data);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
+        xfile.write(strline);
+
+    }
+
+    xfile.close();
+
+    ShareMap(mapdata);
+
+}
+
+void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        std::cout<<"ListenCmd small."<<std::endl;
+        return;
+    }
+
+    xodrobj xo;
+    memcpy(&xo,strdata,sizeof(xodrobj));
+
+    givlog->debug("lat is %f", xo.flat);
+
+    xo.fhgdsrc = gsrc.fhgdsrc;
+    xo.flatsrc = gsrc.flatsrc;
+    xo.flonsrc = gsrc.flonsrc;
+
+    SetPlan(xo);
+
+
+}
+
+void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::v2x::v2x xv2x;
+    if(!xv2x.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ListernV2X Parse Error.");
+        std::cout<<"ListenV2X Parse Error."<<std::endl;
+        return;
+    }
+    if(xv2x.stgps_size()<1)
+    {
+        givlog->debug("ListenV2X no gps station.");
+        std::cout<<"ListenV2X no gps station."<<std::endl;
+        return;
+    }
+
+    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
+}
+
+void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        givlog->warn("ListenSrc small");
+        std::cout<<"ListenSrc small."<<std::endl;
+        return;
+    }
+
+    memcpy(&gsrc,strdata,sizeof(xodrobj));
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+
+}
+
+void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
+        return;
+    }
+
+    xodrobj xo;
+    xo.fhgdsrc = xgpsimu.heading();
+    xo.flatsrc = xgpsimu.lat();
+    xo.flonsrc = xgpsimu.lon();
+
+    gsrc = xo;
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+}
+
+void ExitFunc()
+{
+//    gApp->quit();
+    iv::modulecomm::Unregister(gpasimple);
+    iv::modulecomm::Unregister(gpav2x);
+    iv::modulecomm::Unregister(gpagps);
+    iv::modulecomm::Unregister(gpasrc);
+    iv::modulecomm::Unregister(gpmap);
+    iv::modulecomm::Unregister(gpa);
+    std::cout<<"driver_map_xodrload exit."<<std::endl;
+    gApp->quit();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
+}
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        ExitFunc();
+    }
+}
+
+void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
+{
+    (void)pstr_req;
+    (void)nreqsize;
+    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
+    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
+    {
+        std::shared_ptr<iv::roi::resultarray> pstr_roires;
+        gsrx.GetROI(pstr_roireq,pstr_roires);
+        int nbytesize = pstr_roires->ByteSize();
+        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
+        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
+        {
+            nressize = nbytesize;
+            std::cout<<"return res."<<std::endl;
+            return;
+        }
+        else
+        {
+            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
+        }
+    }
+    else
+    {
+        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
+        return;
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_map_xodrload");
+    QCoreApplication a(argc, argv);
+    gApp = &a;
+
+    RegisterIVBackTrace();
+
+    gfault = new iv::Ivfault("driver_map_xodrload");
+    givlog = new iv::Ivlog("driver_map_xodrload");
+
+    std::string strmapth,strparapath;
+    if(argc<3)
+    {
+//        strmapth = "./map.xodr";
+        strmapth = getenv("HOME");
+        strmapth = strmapth + "/map/map.xodr";
+//        strmapth = "/home/yuchuli/1226.xodr";
+        strparapath = "./driver_map_xodrload.xml";
+    }
+    else
+    {
+        strmapth = argv[1];
+        strparapath = argv[2];
+    }
+
+
+    iv::xmlparam::Xmlparam xp(strparapath);
+    xp.GetParam(std::string("he"),std::string("1"));
+    std::string strlat0 = xp.GetParam("lat0","39");
+    std::string strlon0 = xp.GetParam("lon0","117.0");
+    std::string strhdg0 = xp.GetParam("hdg0","360.0");
+
+    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+
+    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
+
+    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
+
+    std::string strextendmap = xp.GetParam("extendmap","false");
+
+    std::string strsideenable = xp.GetParam("sideenable","false");
+
+    std::string strsidelaneenable = xp.GetParam("sidelaneenable","false");
+
+
+    glat0 = atof(strlat0.data());
+    glon0 = atof(strlon0.data());
+    ghead0 = atof(strhdg0.data());
+
+    gvehiclewidth = atof(strvehiclewidth.data());
+
+    if(strextendmap == "true")
+    {
+        gbExtendMap = true;
+    }
+    else
+    {
+        gbExtendMap = false;
+    }
+
+    if(strsideenable == "true")
+    {
+        gbSideEnable = true;
+    }
+
+    if(strsidelaneenable == "true")
+    {
+        gbSideLaneEnable = true;
+    }
+
+
+    LoadXODR(strmapth);
+
+    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
+    (void)serviceroi;
+
+    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
+    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
+    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
+
+
+    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
+    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
+    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
+    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
+
+
+
+
+    double x_src,y_src,x_dst,y_dst;
+
+    x_src = -26;y_src = 10;
+    x_dst = -50;y_dst = -220;
+
+    x_src = 0;y_src = 0;
+    x_dst = -23;y_dst = -18;
+    x_dst = 21;y_dst =-21;
+    x_dst =5;y_dst = 0;
+
+    x_src = -20; y_src = -1000;
+    x_dst = 900; y_dst = -630;
+
+//    x_dst = 450; y_dst = -640;
+//    x_dst = -190; y_dst = -690;
+
+//    x_src = 900; y_src = -610;
+//    x_dst = -100; y_dst = -680;
+    std::vector<PlanPoint> xPlan;
+    double s;
+//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);
+
+//    ToGPSTrace(xPlan);
+
+
+//    double lat = 39.1443880;
+//    double lon = 117.0812543;
+
+//    xodrobj xo;
+//    xo.fhgdsrc = 340;
+//    xo.flatsrc = lat; xo.flonsrc = lon;
+//    xo.flat = 39.1490196;
+//    xo.flon = 117.0806979;
+//    xo.lane = 1;
+//    SetPlan(xo);
+
+
+    void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
+
+    signal(SIGINT,signal_handler);
+
+    int nrc = a.exec();
+
+    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;
+
+    return nrc;
+}

src/driver/driver_map_xodrload/mconf.h

@@ -1,199 +1,199 @@
-/*							mconf.h
- *
- *	Common include file for math routines
- *
- *
- *
- * SYNOPSIS:
- *
- * #include "mconf.h"
- *
- *
- *
- * DESCRIPTION:
- *
- * This file contains definitions for error codes that are
- * passed to the common error handling routine mtherr()
- * (which see).
- *
- * The file also includes a conditional assembly definition
- * for the type of computer arithmetic (IEEE, DEC, Motorola
- * IEEE, or UNKnown).
- * 
- * For Digital Equipment PDP-11 and VAX computers, certain
- * IBM systems, and others that use numbers with a 56-bit
- * significand, the symbol DEC should be defined.  In this
- * mode, most floating point constants are given as arrays
- * of octal integers to eliminate decimal to binary conversion
- * errors that might be introduced by the compiler.
- *
- * For little-endian computers, such as IBM PC, that follow the
- * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
- * Std 754-1985), the symbol IBMPC should be defined.  These
- * numbers have 53-bit significands.  In this mode, constants
- * are provided as arrays of hexadecimal 16 bit integers.
- *
- * Big-endian IEEE format is denoted MIEEE.  On some RISC
- * systems such as Sun SPARC, double precision constants
- * must be stored on 8-byte address boundaries.  Since integer
- * arrays may be aligned differently, the MIEEE configuration
- * may fail on such machines.
- *
- * To accommodate other types of computer arithmetic, all
- * constants are also provided in a normal decimal radix
- * which one can hope are correctly converted to a suitable
- * format by the available C language compiler.  To invoke
- * this mode, define the symbol UNK.
- *
- * An important difference among these modes is a predefined
- * set of machine arithmetic constants for each.  The numbers
- * MACHEP (the machine roundoff error), MAXNUM (largest number
- * represented), and several other parameters are preset by
- * the configuration symbol.  Check the file const.c to
- * ensure that these values are correct for your computer.
- *
- * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
- * may fail on many systems.  Verify that they are supposed
- * to work on your computer.
- */
-/*
-Cephes Math Library Release 2.3:  June, 1995
-Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
-*/
-
-
-/* Define if the `long double' type works.  */
-#define HAVE_LONG_DOUBLE 1
-
-/* Define as the return type of signal handlers (int or void).  */
-#define RETSIGTYPE void
-
-/* Define if you have the ANSI C header files.  */
-#define STDC_HEADERS 1
-
-/* Define if your processor stores words with the most significant
-   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
-/* #undef WORDS_BIGENDIAN */
-
-/* Define if floating point words are bigendian.  */
-/* #undef FLOAT_WORDS_BIGENDIAN */
-
-/* The number of bytes in a int.  */
-#define SIZEOF_INT 4
-
-/* Define if you have the <string.h> header file.  */
-#define HAVE_STRING_H 1
-
-/* Name of package */
-#define PACKAGE "cephes"
-
-/* Version number of package */
-#define VERSION "2.7"
-
-/* Constant definitions for math error conditions
- */
-
-//#define DOMAIN		1	/* argument domain error */
-//#define SING		2	/* argument singularity */
-//#define OVERFLOW	3	/* overflow range error */
-//#define UNDERFLOW	4	/* underflow range error */
-//#define TLOSS		5	/* total loss of precision */
-//#define PLOSS		6	/* partial loss of precision */
-
-#define EDOM		33
-#define ERANGE		34
-/* Complex numeral.  */
-typedef struct
-	{
-	double r;
-	double i;
-	} cmplx;
-
-#ifdef HAVE_LONG_DOUBLE
-/* Long double complex numeral.  */
-typedef struct
-	{
-	long double r;
-	long double i;
-	} cmplxl;
-#endif
-
-
-/* Type of computer arithmetic */
-
-/* PDP-11, Pro350, VAX:
- */
-/* #define DEC 1 */
-
-/* Intel IEEE, low order words come first:
- */
-/* #define IBMPC 1 */
-
-/* Motorola IEEE, high order words come first
- * (Sun 680x0 workstation):
- */
-/* #define MIEEE 1 */
-
-/* UNKnown arithmetic, invokes coefficients given in
- * normal decimal format.  Beware of range boundary
- * problems (MACHEP, MAXLOG, etc. in const.c) and
- * roundoff problems in pow.c:
- * (Sun SPARCstation)
- */
-#define UNK 1
-
-/* If you define UNK, then be sure to set BIGENDIAN properly. */
-#ifdef FLOAT_WORDS_BIGENDIAN
-#define BIGENDIAN 1
-#else
-#define BIGENDIAN 0
-#endif
-/* Define this `volatile' if your compiler thinks
- * that floating point arithmetic obeys the associative
- * and distributive laws.  It will defeat some optimizations
- * (but probably not enough of them).
- *
- * #define VOLATILE volatile
- */
-#define VOLATILE
-
-/* For 12-byte long doubles on an i386, pad a 16-bit short 0
- * to the end of real constants initialized by integer arrays.
- *
- * #define XPD 0,
- *
- * Otherwise, the type is 10 bytes long and XPD should be
- * defined blank (e.g., Microsoft C).
- *
- * #define XPD
- */
-#define XPD 0,
-
-/* Define to support tiny denormal numbers, else undefine. */
-#define DENORMAL 1
-
-/* Define to ask for infinity support, else undefine. */
-#define INFINITIES 1
-
-/* Define to ask for support of numbers that are Not-a-Number,
-   else undefine.  This may automatically define INFINITIES in some files. */
-#define NANS 1
-
-/* Define to distinguish between -0.0 and +0.0.  */
-#define MINUSZERO 1
-
-/* Define 1 for ANSI C atan2() function
-   See atan.c and clog.c. */
-#define ANSIC 1
-
-/* Get ANSI function prototypes, if you want them. */
-#if 1
-/* #ifdef __STDC__ */
-#define ANSIPROT 1
-int mtherr ( char *, int );
-#else
-int mtherr();
-#endif
-
-/* Variable for error reporting.  See mtherr.c.  */
-extern int merror;
+/*							mconf.h
+ *
+ *	Common include file for math routines
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * #include "mconf.h"
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains definitions for error codes that are
+ * passed to the common error handling routine mtherr()
+ * (which see).
+ *
+ * The file also includes a conditional assembly definition
+ * for the type of computer arithmetic (IEEE, DEC, Motorola
+ * IEEE, or UNKnown).
+ * 
+ * For Digital Equipment PDP-11 and VAX computers, certain
+ * IBM systems, and others that use numbers with a 56-bit
+ * significand, the symbol DEC should be defined.  In this
+ * mode, most floating point constants are given as arrays
+ * of octal integers to eliminate decimal to binary conversion
+ * errors that might be introduced by the compiler.
+ *
+ * For little-endian computers, such as IBM PC, that follow the
+ * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
+ * Std 754-1985), the symbol IBMPC should be defined.  These
+ * numbers have 53-bit significands.  In this mode, constants
+ * are provided as arrays of hexadecimal 16 bit integers.
+ *
+ * Big-endian IEEE format is denoted MIEEE.  On some RISC
+ * systems such as Sun SPARC, double precision constants
+ * must be stored on 8-byte address boundaries.  Since integer
+ * arrays may be aligned differently, the MIEEE configuration
+ * may fail on such machines.
+ *
+ * To accommodate other types of computer arithmetic, all
+ * constants are also provided in a normal decimal radix
+ * which one can hope are correctly converted to a suitable
+ * format by the available C language compiler.  To invoke
+ * this mode, define the symbol UNK.
+ *
+ * An important difference among these modes is a predefined
+ * set of machine arithmetic constants for each.  The numbers
+ * MACHEP (the machine roundoff error), MAXNUM (largest number
+ * represented), and several other parameters are preset by
+ * the configuration symbol.  Check the file const.c to
+ * ensure that these values are correct for your computer.
+ *
+ * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
+ * may fail on many systems.  Verify that they are supposed
+ * to work on your computer.
+ */
+/*
+Cephes Math Library Release 2.3:  June, 1995
+Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
+*/
+
+
+/* Define if the `long double' type works.  */
+#define HAVE_LONG_DOUBLE 1
+
+/* Define as the return type of signal handlers (int or void).  */
+#define RETSIGTYPE void
+
+/* Define if you have the ANSI C header files.  */
+#define STDC_HEADERS 1
+
+/* Define if your processor stores words with the most significant
+   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
+/* #undef WORDS_BIGENDIAN */
+
+/* Define if floating point words are bigendian.  */
+/* #undef FLOAT_WORDS_BIGENDIAN */
+
+/* The number of bytes in a int.  */
+#define SIZEOF_INT 4
+
+/* Define if you have the <string.h> header file.  */
+#define HAVE_STRING_H 1
+
+/* Name of package */
+#define PACKAGE "cephes"
+
+/* Version number of package */
+#define VERSION "2.7"
+
+/* Constant definitions for math error conditions
+ */
+
+//#define DOMAIN		1	/* argument domain error */
+//#define SING		2	/* argument singularity */
+//#define OVERFLOW	3	/* overflow range error */
+//#define UNDERFLOW	4	/* underflow range error */
+//#define TLOSS		5	/* total loss of precision */
+//#define PLOSS		6	/* partial loss of precision */
+
+#define EDOM		33
+#define ERANGE		34
+/* Complex numeral.  */
+typedef struct
+	{
+	double r;
+	double i;
+	} cmplx;
+
+#ifdef HAVE_LONG_DOUBLE
+/* Long double complex numeral.  */
+typedef struct
+	{
+	long double r;
+	long double i;
+	} cmplxl;
+#endif
+
+
+/* Type of computer arithmetic */
+
+/* PDP-11, Pro350, VAX:
+ */
+/* #define DEC 1 */
+
+/* Intel IEEE, low order words come first:
+ */
+/* #define IBMPC 1 */
+
+/* Motorola IEEE, high order words come first
+ * (Sun 680x0 workstation):
+ */
+/* #define MIEEE 1 */
+
+/* UNKnown arithmetic, invokes coefficients given in
+ * normal decimal format.  Beware of range boundary
+ * problems (MACHEP, MAXLOG, etc. in const.c) and
+ * roundoff problems in pow.c:
+ * (Sun SPARCstation)
+ */
+#define UNK 1
+
+/* If you define UNK, then be sure to set BIGENDIAN properly. */
+#ifdef FLOAT_WORDS_BIGENDIAN
+#define BIGENDIAN 1
+#else
+#define BIGENDIAN 0
+#endif
+/* Define this `volatile' if your compiler thinks
+ * that floating point arithmetic obeys the associative
+ * and distributive laws.  It will defeat some optimizations
+ * (but probably not enough of them).
+ *
+ * #define VOLATILE volatile
+ */
+#define VOLATILE
+
+/* For 12-byte long doubles on an i386, pad a 16-bit short 0
+ * to the end of real constants initialized by integer arrays.
+ *
+ * #define XPD 0,
+ *
+ * Otherwise, the type is 10 bytes long and XPD should be
+ * defined blank (e.g., Microsoft C).
+ *
+ * #define XPD
+ */
+#define XPD 0,
+
+/* Define to support tiny denormal numbers, else undefine. */
+#define DENORMAL 1
+
+/* Define to ask for infinity support, else undefine. */
+#define INFINITIES 1
+
+/* Define to ask for support of numbers that are Not-a-Number,
+   else undefine.  This may automatically define INFINITIES in some files. */
+#define NANS 1
+
+/* Define to distinguish between -0.0 and +0.0.  */
+#define MINUSZERO 1
+
+/* Define 1 for ANSI C atan2() function
+   See atan.c and clog.c. */
+#define ANSIC 1
+
+/* Get ANSI function prototypes, if you want them. */
+#if 1
+/* #ifdef __STDC__ */
+#define ANSIPROT 1
+int mtherr ( char *, int );
+#else
+int mtherr();
+#endif
+
+/* Variable for error reporting.  See mtherr.c.  */
+extern int merror;

src/driver/driver_map_xodrload/planpoint.h

@@ -1,41 +1,41 @@
-#ifndef PLANPOINT_H
-#define PLANPOINT_H
-
-class PlanPoint
-{
-public:
-    bool bInlaneAvoid = false;
-    double mx_left;
-    double my_left;
-    double mx_right;
-    double my_right;
-    double x;
-    double y;
-    double speed;
-    int lanmp; //left 1 right -1
-    double hdg;
-    double dis;
-    double mS;
-    double mWidth;  //Current Lane Width
-    double mLeftWidth[5];
-    double mRightWidth[5];
-
-    double mfRoadWidth;
-    double mfDisToRoadLeft;
-    double mfDisToLaneLeft;
-    int mnLaneori = 0;  //from Right 0
-    int mnLaneTotal = 1; //Lane Total
-
-    int nSignal = -1;   //if 0 no signal point
-
-    int nRoadID =-1;
-
-    double mfSecx;
-    double mfSecy;
-    int nlrchange; //1 left 2 right
-    bool mbBoringRoad = false;
-    bool mbNoavoid = false;
-    double mfCurvature = 0.0;
-};
-
-#endif // PLANPOINT_H
+#ifndef PLANPOINT_H
+#define PLANPOINT_H
+
+class PlanPoint
+{
+public:
+    bool bInlaneAvoid = false;
+    double mx_left;
+    double my_left;
+    double mx_right;
+    double my_right;
+    double x;
+    double y;
+    double speed;
+    int lanmp; //left 1 right -1
+    double hdg;
+    double dis;
+    double mS;
+    double mWidth;  //Current Lane Width
+    double mLeftWidth[5];
+    double mRightWidth[5];
+
+    double mfRoadWidth;
+    double mfDisToRoadLeft;
+    double mfDisToLaneLeft;
+    int mnLaneori = 0;  //from Right 0
+    int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
+
+    int nRoadID =-1;
+
+    double mfSecx;
+    double mfSecy;
+    int nlrchange; //1 left 2 right
+    bool mbBoringRoad = false;
+    bool mbNoavoid = false;
+    double mfCurvature = 0.0;
+};
+
+#endif // PLANPOINT_H

src/driver/driver_map_xodrload/polevl.c

@@ -1,97 +1,97 @@
-/*							polevl.c
- *							p1evl.c
- *
- *	Evaluate polynomial
- *
- *
- *
- * SYNOPSIS:
- *
- * int N;
- * double x, y, coef[N+1], polevl[];
- *
- * y = polevl( x, coef, N );
- *
- *
- *
- * DESCRIPTION:
- *
- * Evaluates polynomial of degree N:
- *
- *                     2          N
- * y  =  C  + C x + C x  +...+ C x
- *        0    1     2          N
- *
- * Coefficients are stored in reverse order:
- *
- * coef[0] = C  , ..., coef[N] = C  .
- *            N                   0
- *
- *  The function p1evl() assumes that coef[N] = 1.0 and is
- * omitted from the array.  Its calling arguments are
- * otherwise the same as polevl().
- *
- *
- * SPEED:
- *
- * In the interest of speed, there are no checks for out
- * of bounds arithmetic.  This routine is used by most of
- * the functions in the library.  Depending on available
- * equipment features, the user may wish to rewrite the
- * program in microcode or assembly language.
- *
- */
-
-
-/*
-Cephes Math Library Release 2.1:  December, 1988
-Copyright 1984, 1987, 1988 by Stephen L. Moshier
-Direct inquiries to 30 Frost Street, Cambridge, MA 02140
-*/
-
-
-double polevl( double x,  double *coef, int N )
-//double x;
-//double coef[];
-//int N;
-{
-double ans;
-int i;
-double *p;
-
-p = coef;
-ans = *p++;
-i = N;
-
-do
-	ans = ans * x  +  *p++;
-while( --i );
-
-return( ans );
-}
-
-/*							p1evl()	*/
-/*                                          N
- * Evaluate polynomial when coefficient of x  is 1.0.
- * Otherwise same as polevl.
- */
-
-double p1evl( double x, double *coef, int N )
-//double x;
-//double coef[];
-//int N;
-{
-double ans;
-double *p;
-int i;
-
-p = coef;
-ans = x + *p++;
-i = N-1;
-
-do
-	ans = ans * x  + *p++;
-while( --i );
-
-return( ans );
-}
+/*							polevl.c
+ *							p1evl.c
+ *
+ *	Evaluate polynomial
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * double x, y, coef[N+1], polevl[];
+ *
+ * y = polevl( x, coef, N );
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates polynomial of degree N:
+ *
+ *                     2          N
+ * y  =  C  + C x + C x  +...+ C x
+ *        0    1     2          N
+ *
+ * Coefficients are stored in reverse order:
+ *
+ * coef[0] = C  , ..., coef[N] = C  .
+ *            N                   0
+ *
+ *  The function p1evl() assumes that coef[N] = 1.0 and is
+ * omitted from the array.  Its calling arguments are
+ * otherwise the same as polevl().
+ *
+ *
+ * SPEED:
+ *
+ * In the interest of speed, there are no checks for out
+ * of bounds arithmetic.  This routine is used by most of
+ * the functions in the library.  Depending on available
+ * equipment features, the user may wish to rewrite the
+ * program in microcode or assembly language.
+ *
+ */
+
+
+/*
+Cephes Math Library Release 2.1:  December, 1988
+Copyright 1984, 1987, 1988 by Stephen L. Moshier
+Direct inquiries to 30 Frost Street, Cambridge, MA 02140
+*/
+
+
+double polevl( double x,  double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+int i;
+double *p;
+
+p = coef;
+ans = *p++;
+i = N;
+
+do
+	ans = ans * x  +  *p++;
+while( --i );
+
+return( ans );
+}
+
+/*							p1evl()	*/
+/*                                          N
+ * Evaluate polynomial when coefficient of x  is 1.0.
+ * Otherwise same as polevl.
+ */
+
+double p1evl( double x, double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+double *p;
+int i;
+
+p = coef;
+ans = x + *p++;
+i = N-1;
+
+do
+	ans = ans * x  + *p++;
+while( --i );
+
+return( ans );
+}

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -1,342 +1,342 @@
-#include "service_roi_xodr.h"
-
-#include <iostream>
-#include "gnss_coordinate_convert.h"
-
-service_roi_xodr::service_roi_xodr()
-{
-
-}
-
-void service_roi_xodr::SetXODR(OpenDrive *pxodr)
-{
-    mpxodr = pxodr;
-    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
-    updateroadarea();
-}
-
-
-#include <QTime>
-int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
-{
-
-    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
-    xres_ptr->set_lat(xreq_ptr->lat());
-    xres_ptr->set_lon(xreq_ptr->lon());
-    xres_ptr->set_heading(xreq_ptr->heading());
-    xres_ptr->set_range(xreq_ptr->range());
-    xres_ptr->set_gridsize(xreq_ptr->gridsize());
-
-    double x0,y0;
-    double x,y;
-    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
-    GaussProjCal(mlon0,mlat0,&x0,&y0);
-    x = x - x0;
-    y = y - y0;
-    unsigned int nroadsize = mvectorarea.size();
-    unsigned int i;
-    unsigned int j;
-    std::vector<iv::roi::area> xvectorarea;
-
-    QTime xTime;
-    xTime.start();
-
-    //Find Near ROI area
-    for(i=0;i<nroadsize;i++)
-    {
-        double fdis1;
-        iv::roi::roadarea * proadarea = &mvectorarea[i];
-        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
-        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
-        {
-            continue;
-        }
-        unsigned int nareasize = proadarea->mvectorarea.size();
-
-        for(j=0;j<nareasize;j++)
-        {
-            iv::roi::area * parea = &proadarea->mvectorarea[j];
-            double fd1,fd2,fd3,fd4;
-            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
-            if(fd1<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
-            if(fd2<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
-            if(fd3<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
-            if(fd4<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-            }
-        }
-    }
-
-    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
-
-    if(xreq_ptr->gridsize() <= 0.0)
-    {
-        return -1;  //grid size is error
-    }
-
-    double fgridsize = xreq_ptr->gridsize()/2.0;
-
-    std::vector<iv::roi::Point> xvectorpoint;
-    unsigned int nareasize = xvectorarea.size();
-    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
-    for(i=0;i<nareasize;i++)
-    {
-        int ix,iy;
-        int ny = xvectorarea[i].fsteplen/fgridsize;
-        int nxl = 0;
-        int nxr = 0;
-        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
-        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
-        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
- //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
-        double flaneoff = xvectorarea[i].flaneoff;
-        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
-        double frelxraw = xvectorarea[i].refPoint.x - x;
-        double frelyraw = xvectorarea[i].refPoint.y - y;
-        double fxrel,fyrel;
-//        fgpsyaw = 0;
-        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
-        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
-        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
-        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
-//        iv::roi::Point xpoint;
-//        xpoint.x = fxbase;
-//        xpoint.y = fybase;
-//        xvectorpoint.push_back(xpoint);
-//        continue;
-
-        fyaw = fyaw + 3.0*M_PI/2.0;
-        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
-        {
-            iv::roi::Point xpoint;
-            xpoint.x = fxbase;
-            xpoint.y = fybase;
-            xvectorpoint.push_back(xpoint);
-        }
-        else
-        {
-            if((nxl == 0)&&(nxr== 0))
-            {
-                for(iy=(-ny);iy<=0;iy++)
-                {
-                    iv::roi::Point xpoint;
-                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
-                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
-                    xvectorpoint.push_back(xpoint);
-                }
-            }
-            else
-            {
-                for(ix=(-nxl);ix<=nxr;ix++)
-                {
-                    for(iy=(-ny);iy<=0;iy++)
-                    {
-                        double fxlocal,fylocal;
-                        fxlocal = ix*fgridsize;
-                        fylocal = iy*fgridsize;
-                        iv::roi::Point xpoint;
-
-                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
-                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
-                        xvectorpoint.push_back(xpoint);
-                    }
-                }
-            }
-        }
-    }
-
-
-    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
-
-    for(i=0;i<xvectorpoint.size();i++)
-    {
-        iv::roi::resultunit * padd = xres_ptr->add_res();
-//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
-        padd->set_x(xvectorpoint[i].x);
-        padd->set_y(xvectorpoint[i].y);
-    }
-
-    return 0;
-}
-
-void service_roi_xodr::updateroadarea()
-{
-    OpenDrive * pxodr = mpxodr;
-    mvectorarea.clear();
-    unsigned int nroadcount = pxodr->GetRoadCount();
-    unsigned int i;
-    double fstep = GEOSAMPLESTEP;
-    for(i=0;i<nroadcount;i++)
-    {
-        Road * pRoad = pxodr->GetRoad(i);
-        if(pRoad == NULL)
-        {
-            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
-            continue;
-        }
-        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
-        mvectorarea.push_back(xroadarea);
-    }
-}
-
-iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
-{
-    iv::roi::roadarea xroadarea;
-    xroadarea.fRoadLen = pRoad->GetRoadLength();
-    double x1,y1,hdg1;
-    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
-    xroadarea.startPoint.x = x1;
-    xroadarea.startPoint.y = y1;
-    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
-    unsigned int i;
-    if(nSectionCount == 0)
-    {
-        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
-    }
-    double snow= 0;
-    for(i=0;i<nSectionCount;i++)
-    {
-        LaneSection * pLS = pRoad->GetLaneSection(i);
-        if(pLS == NULL)
-        {
-            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
-            continue;
-        }
-        snow = pLS->GetS();
-        double endS;
-        if(i == (nSectionCount -1))
-        {
-            endS = pRoad->GetRoadLength();
-        }
-        else
-        {
-            endS = pRoad->GetLaneSection(i+1)->GetS();
-        }
-
-        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
-        double frigthwidth = pRoad->GetRoadRightWidth(snow);
-        double x,y,yaw;
-        pRoad->GetGeometryCoords(snow,x,y,yaw);
-        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
-        double flaneoff = pRoad->GetLaneOffsetValue(snow);
-        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-        pointLeft_old = pointLeft;
-        pointRight_old = pointRight;
-        snow = snow + fstep;
-        while(snow < endS)
-        {
-            fleftwidth = pRoad->GetRoadLeftWidth(snow);
-            frigthwidth = pRoad->GetRoadRightWidth(snow);
-            pRoad->GetGeometryCoords(snow,x,y,yaw);
-            flaneoff = pRoad->GetLaneOffsetValue(snow);
-            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-            iv::roi::area xarea;
-            xarea.P1 = pointLeft_old;
-            xarea.P2 = pointLeft;
-            xarea.P3 = pointRight;
-            xarea.P4 = pointRight_old;
-            xarea.refPoint.x = x;
-            xarea.refPoint.y = y;
-            xarea.s = snow;
-            xarea.fmaxlen = fleftwidth + frigthwidth;
-            xarea.fhdg = yaw;
-            xarea.fleftwidth = fleftwidth;
-            xarea.frightwidth = frigthwidth;
-            xarea.flaneoff = flaneoff;
-            xarea.fsteplen = fstep;
-            xroadarea.mvectorarea.push_back(xarea);
-//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
-//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
-//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
-
-            pointLeft_old = pointLeft;
-            pointRight_old = pointRight;
-            snow = snow + fstep;
-
-        }
-        if((snow-fstep) < (endS - 0.000001))
-        {
-            double foldsnow = snow - fstep;
-            snow = endS - 0.000001;
-            fleftwidth = pRoad->GetRoadLeftWidth(snow);
-            frigthwidth = pRoad->GetRoadRightWidth(snow);
-            pRoad->GetGeometryCoords(snow,x,y,yaw);
-            flaneoff = pRoad->GetLaneOffsetValue(snow);
-            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-            iv::roi::area xarea;
-            xarea.P1 = pointLeft_old;
-            xarea.P2 = pointLeft;
-            xarea.P3 = pointRight;
-            xarea.P4 = pointRight_old;
-            xarea.refPoint.x = x;
-            xarea.refPoint.y = y;
-            xarea.s = snow;
-            xarea.fhdg = yaw;
-            xarea.fleftwidth = fleftwidth;
-            xarea.frightwidth = frigthwidth;
-            xarea.flaneoff = flaneoff;
-            xarea.fsteplen = endS - foldsnow;
-            xroadarea.mvectorarea.push_back(xarea);
-        }
-    }
-    return xroadarea;
-}
-
-bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
-{
-    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
-    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
-    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
-    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
-
-    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
-        return true;
-    }
-    return false;
-}
-
-bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
-{
-    unsigned int i;
-    unsigned nsize = xvectorroi.size();
-    for(i=0;i<nsize;i++)
-    {
-        iv::roi::area xarea = xvectorroi[i];
-        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
-        if(fdis>(xarea.fmaxlen+0.1))
-        {
-            continue;
-        }
-        if(CheckPointInArea(x,y,xarea))
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
+#include "service_roi_xodr.h"
+
+#include <iostream>
+#include "gnss_coordinate_convert.h"
+
+service_roi_xodr::service_roi_xodr()
+{
+
+}
+
+void service_roi_xodr::SetXODR(OpenDrive *pxodr)
+{
+    mpxodr = pxodr;
+    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
+    updateroadarea();
+}
+
+
+#include <QTime>
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
+{
+
+    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
+    xres_ptr->set_lat(xreq_ptr->lat());
+    xres_ptr->set_lon(xreq_ptr->lon());
+    xres_ptr->set_heading(xreq_ptr->heading());
+    xres_ptr->set_range(xreq_ptr->range());
+    xres_ptr->set_gridsize(xreq_ptr->gridsize());
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
+    GaussProjCal(mlon0,mlat0,&x0,&y0);
+    x = x - x0;
+    y = y - y0;
+    unsigned int nroadsize = mvectorarea.size();
+    unsigned int i;
+    unsigned int j;
+    std::vector<iv::roi::area> xvectorarea;
+
+    QTime xTime;
+    xTime.start();
+
+    //Find Near ROI area
+    for(i=0;i<nroadsize;i++)
+    {
+        double fdis1;
+        iv::roi::roadarea * proadarea = &mvectorarea[i];
+        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
+        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
+        {
+            continue;
+        }
+        unsigned int nareasize = proadarea->mvectorarea.size();
+
+        for(j=0;j<nareasize;j++)
+        {
+            iv::roi::area * parea = &proadarea->mvectorarea[j];
+            double fd1,fd2,fd3,fd4;
+            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
+            if(fd1<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
+            if(fd2<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
+            if(fd3<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
+            if(fd4<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+            }
+        }
+    }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    if(xreq_ptr->gridsize() <= 0.0)
+    {
+        return -1;  //grid size is error
+    }
+
+    double fgridsize = xreq_ptr->gridsize()/2.0;
+
+    std::vector<iv::roi::Point> xvectorpoint;
+    unsigned int nareasize = xvectorarea.size();
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
+    for(i=0;i<nareasize;i++)
+    {
+        int ix,iy;
+        int ny = xvectorarea[i].fsteplen/fgridsize;
+        int nxl = 0;
+        int nxr = 0;
+        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
+        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
+        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+ //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+        double flaneoff = xvectorarea[i].flaneoff;
+        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
+        double frelxraw = xvectorarea[i].refPoint.x - x;
+        double frelyraw = xvectorarea[i].refPoint.y - y;
+        double fxrel,fyrel;
+//        fgpsyaw = 0;
+        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
+        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
+        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
+        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
+//        iv::roi::Point xpoint;
+//        xpoint.x = fxbase;
+//        xpoint.y = fybase;
+//        xvectorpoint.push_back(xpoint);
+//        continue;
+
+        fyaw = fyaw + 3.0*M_PI/2.0;
+        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
+        {
+            iv::roi::Point xpoint;
+            xpoint.x = fxbase;
+            xpoint.y = fybase;
+            xvectorpoint.push_back(xpoint);
+        }
+        else
+        {
+            if((nxl == 0)&&(nxr== 0))
+            {
+                for(iy=(-ny);iy<=0;iy++)
+                {
+                    iv::roi::Point xpoint;
+                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
+                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
+                    xvectorpoint.push_back(xpoint);
+                }
+            }
+            else
+            {
+                for(ix=(-nxl);ix<=nxr;ix++)
+                {
+                    for(iy=(-ny);iy<=0;iy++)
+                    {
+                        double fxlocal,fylocal;
+                        fxlocal = ix*fgridsize;
+                        fylocal = iy*fgridsize;
+                        iv::roi::Point xpoint;
+
+                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
+                        xvectorpoint.push_back(xpoint);
+                    }
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
+
+    for(i=0;i<xvectorpoint.size();i++)
+    {
+        iv::roi::resultunit * padd = xres_ptr->add_res();
+//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
+        padd->set_x(xvectorpoint[i].x);
+        padd->set_y(xvectorpoint[i].y);
+    }
+
+    return 0;
+}
+
+void service_roi_xodr::updateroadarea()
+{
+    OpenDrive * pxodr = mpxodr;
+    mvectorarea.clear();
+    unsigned int nroadcount = pxodr->GetRoadCount();
+    unsigned int i;
+    double fstep = GEOSAMPLESTEP;
+    for(i=0;i<nroadcount;i++)
+    {
+        Road * pRoad = pxodr->GetRoad(i);
+        if(pRoad == NULL)
+        {
+            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
+            continue;
+        }
+        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
+        mvectorarea.push_back(xroadarea);
+    }
+}
+
+iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
+{
+    iv::roi::roadarea xroadarea;
+    xroadarea.fRoadLen = pRoad->GetRoadLength();
+    double x1,y1,hdg1;
+    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
+    xroadarea.startPoint.x = x1;
+    xroadarea.startPoint.y = y1;
+    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+    if(nSectionCount == 0)
+    {
+        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
+    }
+    double snow= 0;
+    for(i=0;i<nSectionCount;i++)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(pLS == NULL)
+        {
+            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
+            continue;
+        }
+        snow = pLS->GetS();
+        double endS;
+        if(i == (nSectionCount -1))
+        {
+            endS = pRoad->GetRoadLength();
+        }
+        else
+        {
+            endS = pRoad->GetLaneSection(i+1)->GetS();
+        }
+
+        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+        double frigthwidth = pRoad->GetRoadRightWidth(snow);
+        double x,y,yaw;
+        pRoad->GetGeometryCoords(snow,x,y,yaw);
+        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
+        double flaneoff = pRoad->GetLaneOffsetValue(snow);
+        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+        pointLeft_old = pointLeft;
+        pointRight_old = pointRight;
+        snow = snow + fstep;
+        while(snow < endS)
+        {
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fmaxlen = fleftwidth + frigthwidth;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = fstep;
+            xroadarea.mvectorarea.push_back(xarea);
+//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
+//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
+
+            pointLeft_old = pointLeft;
+            pointRight_old = pointRight;
+            snow = snow + fstep;
+
+        }
+        if((snow-fstep) < (endS - 0.000001))
+        {
+            double foldsnow = snow - fstep;
+            snow = endS - 0.000001;
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = endS - foldsnow;
+            xroadarea.mvectorarea.push_back(xarea);
+        }
+    }
+    return xroadarea;
+}
+
+bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
+{
+    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
+    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
+    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
+    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
+
+    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
+        return true;
+    }
+    return false;
+}
+
+bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
+{
+    unsigned int i;
+    unsigned nsize = xvectorroi.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::roi::area xarea = xvectorroi[i];
+        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
+        if(fdis>(xarea.fmaxlen+0.1))
+        {
+            continue;
+        }
+        if(CheckPointInArea(x,y,xarea))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -1,86 +1,86 @@
-#ifndef SERVICE_ROI_XODR_H
-#define SERVICE_ROI_XODR_H
-
-#include <vector>
-#include <OpenDrive/OpenDrive.h>
-#include "gpsimu.pb.h"
-#include "roireqest.pb.h"
-#include "roiresult.pb.h"
-
-namespace iv {
-
-namespace roi {
-
-struct Point
-{
-    double x;
-    double y;
-};
-
-struct area
-{
-
-    double s;
-    Point refPoint;
-    Point P1;
-    Point P2;
-    Point P3;
-    Point P4;
-    double fhdg;
-    double fleftwidth;
-    double frightwidth;
-    double fsteplen;
-    double flaneoff;
-    double fmaxlen;
-
-};
-
-struct roadarea
-{
-    int nroadid;
-    Point startPoint;
-    double fRoadLen;
-    std::vector<area> mvectorarea;
-};
-}
-}
-
-class service_roi_xodr
-{
-
-
-public:
-    service_roi_xodr();
-
-    /**
-     * @brief SetXODR Set OpenDrive Pointer to Service
-     * @param pxodr
-     */
-    void SetXODR(OpenDrive * pxodr);
-
-    /**
-     * @brief GetROI Get ROI From XODR Map
-     * @param xreq_ptr  now position and roi param
-     * @param xres_prt  result
-     * @return 0 Succeed -1 No Map  -2 Not in Map
-     */
-    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
-
-private:
-    void updateroadarea();
-    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
-
-    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
-
-    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
-
-private:
-    OpenDrive * mpxodr = NULL;
-    std::vector<iv::roi::roadarea> mvectorarea;
-    double mlon0;
-    double mlat0;
-    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
-
-};
-
-#endif // SERVICE_ROI_XODR_H
+#ifndef SERVICE_ROI_XODR_H
+#define SERVICE_ROI_XODR_H
+
+#include <vector>
+#include <OpenDrive/OpenDrive.h>
+#include "gpsimu.pb.h"
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+namespace iv {
+
+namespace roi {
+
+struct Point
+{
+    double x;
+    double y;
+};
+
+struct area
+{
+
+    double s;
+    Point refPoint;
+    Point P1;
+    Point P2;
+    Point P3;
+    Point P4;
+    double fhdg;
+    double fleftwidth;
+    double frightwidth;
+    double fsteplen;
+    double flaneoff;
+    double fmaxlen;
+
+};
+
+struct roadarea
+{
+    int nroadid;
+    Point startPoint;
+    double fRoadLen;
+    std::vector<area> mvectorarea;
+};
+}
+}
+
+class service_roi_xodr
+{
+
+
+public:
+    service_roi_xodr();
+
+    /**
+     * @brief SetXODR Set OpenDrive Pointer to Service
+     * @param pxodr
+     */
+    void SetXODR(OpenDrive * pxodr);
+
+    /**
+     * @brief GetROI Get ROI From XODR Map
+     * @param xreq_ptr  now position and roi param
+     * @param xres_prt  result
+     * @return 0 Succeed -1 No Map  -2 Not in Map
+     */
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
+
+private:
+    void updateroadarea();
+    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
+
+    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
+
+    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
+
+private:
+    OpenDrive * mpxodr = NULL;
+    std::vector<iv::roi::roadarea> mvectorarea;
+    double mlon0;
+    double mlat0;
+    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
+
+};
+
+#endif // SERVICE_ROI_XODR_H

src/driver/driver_map_xodrload/xodrplan.cpp

@@ -1,15 +1,15 @@
-#include "xodrplan.h"
-
-xodrplan::xodrplan()
-{
-
-}
-
-int xodrplan::MakePlan(xodrdijkstra *pxd, OpenDrive *pxodr, const double x_now,
-                       const double y_now, const double head, const double x_obj,
-                       const double y_obj, const double &obj_dis, const double srcnearthresh,
-                       const double dstnearthresh, const int nlanesel, std::vector<PlanPoint> &xPlan,
-                       const double fvehiclewidth)
-{
-
-}
+#include "xodrplan.h"
+
+xodrplan::xodrplan()
+{
+
+}
+
+int xodrplan::MakePlan(xodrdijkstra *pxd, OpenDrive *pxodr, const double x_now,
+                       const double y_now, const double head, const double x_obj,
+                       const double y_obj, const double &obj_dis, const double srcnearthresh,
+                       const double dstnearthresh, const int nlanesel, std::vector<PlanPoint> &xPlan,
+                       const double fvehiclewidth)
+{
+
+}

src/driver/driver_map_xodrload/xodrplan.h

@@ -1,26 +1,26 @@
-#ifndef XODRPLAN_H
-#define XODRPLAN_H
-
-#include "OpenDrive/OpenDrive.h"
-
-#include <vector>
-
-#include "xodrfunc.h"
-
-#include "xodrdijkstra.h"
-
-#include "planpoint.h"
-
-class xodrplan
-{
-public:
-    xodrplan();
-
-public:
-    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-                 const double x_obj,const double y_obj,const double & obj_dis,
-                 const double srcnearthresh,const double dstnearthresh,
-                 const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
-};
-
-#endif // XODRPLAN_H
+#ifndef XODRPLAN_H
+#define XODRPLAN_H
+
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class xodrplan
+{
+public:
+    xodrplan();
+
+public:
+    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+                 const double x_obj,const double y_obj,const double & obj_dis,
+                 const double srcnearthresh,const double dstnearthresh,
+                 const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+};
+
+#endif // XODRPLAN_H

src/v2x/obuUdpClient/udpreciver.cpp

@@ -158,8 +158,9 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
             QByteArray jsonData;
             int len = jsonSize;
             jsonData.resize(len);
-
-            memcpy(jsonData.data(), &(data.data()[5]),jsonSize);
+            data.remove(0, 5);
+            jsonData.resize(jsonSize);
+            memcpy(jsonData.data(), data,jsonSize);
             QJsonObject object = QJsonDocument::fromJson(jsonData).object();
 
             if(object.contains("LightsStates"))
@@ -239,7 +240,9 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
         else if(data[1] == 0x04)
         {
             QByteArray jsonData();
-            memcpy(jsonData, &data[5],jsonSize);
+            data.remove(0, 5);
+            jsonData.resize(jsonSize);
+            memcpy(jsonData.data(), data,jsonSize);
             QJsonObject object = QJsonDocument::fromJson(jsonData).object();
 
             if (object.contains("Obj_List")) {