modularization/src/tool/map_lanetoxodr/roaddigit.cpp

#include "roaddigit.h"

#include <math.h>
#include "xodrfunc.h"


RoadDigit::RoadDigit(Road * pRoad,double fspace)
{
    mpRoad = pRoad;
    UpdateSpace(fspace);
}

std::vector<iv::RoadDigitUnit> * RoadDigit::GetRDU()
{
    return &mvectorRDU;
}

void RoadDigit::UpdateSpace(double fspace)
{
    if(mpRoad == 0)return;
    CalcLine(fspace);
    CalcLane();
}

int RoadDigit::GetSectionIndex(Road *pRoad, double s)
{
    int nrtn = 0;
    int nsectioncount = pRoad->GetLaneSectionCount();
    if(nsectioncount == 1)return 0;
    int i;
    for(i=0;i<(nsectioncount-1);i++)
    {
        if(pRoad->GetLaneSection(i+1)->GetS()>s)
        {
            break;
        }
    }
    nrtn = i;
    return nrtn;
}

void RoadDigit::CalcLine(double fspace)
{
    unsigned int j;
    iv::RoadDigitUnit rdu;

    bool bLastGeo = false;
    for(j=0;j<mpRoad->GetGeometryBlockCount();j++)
    {

        GeometryBlock * pgeob = mpRoad->GetGeometryBlock(j);
        double x,y;
        double x_center,y_center;
        double R;
        RoadGeometry * pg;
        GeometryArc * parc;
        GeometryParamPoly3 * ppp3;
        GeometrySpiral *pSpiral;
        GeometryPoly3 *ppoly;
        double rel_x,rel_y,rel_hdg;
        pg = pgeob->GetGeometryAt(0);
        x = pg->GetX();
        y = pg->GetY();

        if(j == (mpRoad->GetGeometryBlockCount() -1))
        {
            bLastGeo = true;
        }

        switch (pg->GetGeomType()) {
        case 0:
        {

            rdu.mS = pg->GetS();
            rdu.mX = pg->GetX();
            rdu.mY = pg->GetY();
            rdu.mfHdg = pg->GetHdg();
            mvectorRDU.push_back(rdu);
            int ncount = pg->GetLength() /fspace;
//            int nsec1 = GetSectionIndex(mpRoad,pg->GetS());
//            int nsec2 = GetSectionIndex(mpRoad,pg->GetS() + pg->GetLength());
//            if(nsec1 != nsec2)
//            {
//                if((nsec2-nsec1)>1)
//                {
//                    ncount = pg->GetLength()/0.5;
//                }
//                else
//                {
//                    LaneSection * psec1 = mpRoad->GetLaneSection(nsec1);
//                    LaneSection * psec2 = mpRoad->GetLaneSection(nsec2);
//                    if(psec1->GetLaneCount() != psec2->GetLaneCount())
//                    {
//                        ncount = pg->GetLength()/0.1;
//                    }
//                }
//            }

            if(ncount<2)ncount = 2;
            double fstep;
            if(ncount > 0)fstep = pg->GetLength()/ncount;
            int i;
            if(bLastGeo )ncount = ncount+1;
            double xtem,ytem;
            for(i=1;i<ncount;i++)
            {

                xtem = x + (i*fstep)*cos(pg->GetHdg());
                ytem = y + (i*fstep)*sin(pg->GetHdg());

                rdu.mS = pg->GetS() + i*fstep;
                rdu.mX = xtem;
                rdu.mY = ytem;
                rdu.mfHdg = pg->GetHdg();
                mvectorRDU.push_back(rdu);

            }
//            if(bLastGeo == false)
//            {
//                double fx = ncount*fstep -0.1;
//                if((fx < pg->GetLength())&&(fx> ((ncount -1 )*fstep)))
//                {
//                    xtem = x + fx*cos(pg->GetHdg());
//                    ytem = y + fx*sin(pg->GetHdg());

//                    rdu.mS = pg->GetS() + fx;
//                    rdu.mX = xtem;
//                    rdu.mY = ytem;
//                    rdu.mfHdg = pg->GetHdg();
//                    mvectorRDU.push_back(rdu);
//                }
//            }
        }
            break;

        case 1:
            pSpiral = (GeometrySpiral * )pg;
            {
               int ncount = pSpiral->GetLength()/fspace;
               if(ncount < 5)ncount = 5;
               double sstep = pSpiral->GetLength()/((double)ncount);
               int k;
               double x0,y0,hdg0,s0;
               x0 = pSpiral->GetX();
               y0 = pSpiral->GetY();
               s0 = pSpiral->GetS();
               hdg0 = pSpiral->GetHdg() ;

               rdu.mS = s0;
               rdu.mX = x0;
               rdu.mY = y0;
               rdu.mfHdg = hdg0;
               mvectorRDU.push_back(rdu);


               pSpiral->GetCoords(s0,rel_x,rel_y,rel_hdg);

               if(bLastGeo )ncount = ncount+1;
               for(k=1;k<ncount;k++)
               {
                   pSpiral->GetCoords(s0+sstep*k,rel_x,rel_y,rel_hdg);

                   rdu.mS = s0 + sstep * k;
                   rdu.mX = rel_x;
                   rdu.mY = rel_y;
                   rdu.mfHdg = rel_hdg;
                   mvectorRDU.push_back(rdu);

            }
        }

            break;
        case 2:
            {
            parc = (GeometryArc *)pg;
            R = abs(1.0/parc->GetCurvature());
            if(parc->GetCurvature() > 0)
            {
                x_center = pg->GetX() + R *cos(pg->GetHdg() + M_PI/2.0);
                y_center = pg->GetY() + R * sin(pg->GetHdg() + M_PI/2.0);
            }
            else
            {
                x_center = pg->GetX() + R *cos(pg->GetHdg() -M_PI/2.0);
                y_center = pg->GetY() + R * sin(pg->GetHdg() - M_PI/2.0);
            }

            rdu.mS = pg->GetS();
            rdu.mX = pg->GetX();
            rdu.mY = pg->GetY();
            rdu.mfHdg = pg->GetHdg();
            mvectorRDU.push_back(rdu);

            int k;
            int ncount = parc->GetLength() /fspace;
            if(ncount< 5)ncount = 5;
            double curv = parc->GetCurvature();
            double arcrange = parc->GetLength()/R;
            if((arcrange/0.1)>ncount)ncount = arcrange/0.1;
            double hdgstep;
            double hdg0 = parc->GetHdg();
            double hdgnow = parc->GetHdg();
            if(ncount > 0) hdgstep= (parc->GetLength()/R)/ncount;

            double x_draw,y_draw;

            if(curv > 0)
            {
                hdgnow =  hdg0 ;
                x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
                y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
            }
            else
            {
                hdgnow = hdg0;
                x_draw = x_center + R *cos(hdgnow  + M_PI/2.0);
                y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
            }


            if(bLastGeo )ncount = ncount+1;
            for(k=1;k<ncount;k++)
            {


                if(curv > 0)
                {
                    hdgnow =  hdg0 + k*hdgstep;
                    x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
                    y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
                }
                else
                {
                    hdgnow = hdg0 - k * hdgstep;
                    x_draw = x_center + R *cos(hdgnow  + M_PI/2.0);
                    y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
                }

                rdu.mS = pg->GetS() + hdgstep * k* R;
                rdu.mX = x_draw;
                rdu.mY = y_draw;
                rdu.mfHdg = hdgnow;
                mvectorRDU.push_back(rdu);



            }
            }
            break;
        case 3:
            {
            ppoly = (GeometryPoly3 *)pg;
            x = pg->GetX();
            y = pg->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;
            rdu.mS = pg->GetS();
            rdu.mX = pg->GetX();
            rdu.mY = pg->GetY();
            rdu.mfHdg = pg->GetHdg();
            mvectorRDU.push_back(rdu);
            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;
                rdu.mS = pg->GetS() + flen;
                rdu.mX = x;
                rdu.mY = y;
                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(oldx,oldy),QPointF(x,y));

                mvectorRDU.push_back(rdu);
                oldx = x;
                oldy = y;
            }
            }
            break;
        case 4:
            {
            ppp3 = (GeometryParamPoly3 * )pg;
            int ncount = ppp3->GetLength() /fspace;
            if(ncount < 5)ncount = 5;
            double sstep;
            double arclength = ppp3->GetLength();
            if(ncount > 0)sstep = ppp3->GetLength()/ncount;
            else sstep = 10000.0;
            double s = 0;
            double xtem,ytem;
            double pr;

            xtem = ppp3->GetuA() +  ppp3->GetuB() * s +  ppp3->GetuC() * s*s +  ppp3->GetuD() * s*s*s;
            ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
            x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
            y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
            s = s+ sstep;

            rdu.mS = pg->GetS();
            rdu.mX = pg->GetX();
            rdu.mY = pg->GetY();
            rdu.mfHdg = pg->GetHdg();
            mvectorRDU.push_back(rdu);

            double flastx = pg->GetX();
            double flasty = pg->GetY();
            while(s <= ppp3->GetLength())
            {
                double pr= 1.0;
                if(arclength > 0)pr = s/arclength;
                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();

                rdu.mS = pg->GetS() + s;
                rdu.mX = x;
                rdu.mY = y;
                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
                mvectorRDU.push_back(rdu);

                s = s+ sstep;


            }
            }
            break;
        default:
            break;
        }

//         painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));

    }

}

void RoadDigit::CalcLane()
{
    int i;
    int ncount = mvectorRDU.size();
    for(i=0;i<ncount;i++)
    {

        std::vector<iv::LanePoint> xvectorlp2 = xodrfunc::GetAllLanePoint(mpRoad,mvectorRDU[i].mS,
                                                                          mvectorRDU[i].mX,mvectorRDU[i].mY,mvectorRDU[i].mfHdg);
        mvectorRDU[i].mvectorLanePoint = xvectorlp2;
    }
}