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();
}

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

            }
        }
            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 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 4:
            {
            ppp3 = (GeometryParamPoly3 * )pg;
            int ncount = ppp3->GetLength() /fspace;
            if(ncount < 5)ncount = 5;
            double sstep;
            if(ncount > 0)sstep = ppp3->GetLength()/ncount;
            else sstep = 10000.0;
            double s = 0;
            double xtem,ytem;
            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())
            {

                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();

                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;
    }
}