modularization/src/tool/map_lanetoxodr/xodrscenfunc.cpp

#include "xodrscenfunc.h"

xodrscenfunc::xodrscenfunc()
{

}

std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadLaneItem(RoadDigit *prd)
{
    std::vector<QGraphicsPathItem *> xvectorgrapath;
    std::vector<iv::RoadDigitUnit> * pvectorrdu =  prd->GetRDU();
    int nsize = pvectorrdu->size();
    int i;
    for(i=0;i<(nsize-1);i++)
    {
        std::vector<iv::LanePoint> xvepre = pvectorrdu->at(i).mvectorLanePoint;
        std::vector<iv::LanePoint> xvenxt = pvectorrdu->at(i+1).mvectorLanePoint;
        if((xvepre.size()<2)||(xvenxt.size()<2))
        {
            continue;
        }

        unsigned int k;
        for(k=0;k<(xvepre.size()-1);k++)
        {
            QPainterPath xpath;
            if((xvenxt.size() != xvepre.size()))
            {
                double fdis = sqrt(pow(xvenxt.at(0).mfGeoX-xvepre.at(0).mfGeoX,2)
                                   +pow(xvenxt.at(0).mfGeoY - xvepre.at(0).mfGeoY,2));
                double fhdg = xvepre.at(0).mfhdg;
                xpath.moveTo(xvepre.at(k).mfX,xvepre.at(k).mfY*(-1));
                xpath.lineTo(xvepre.at(k).mfX + fdis*cos(fhdg),(xvepre.at(k).mfY + fdis*sin(fhdg))*(-1));
                xpath.lineTo(xvepre.at(k+1).mfX + fdis*cos(fhdg),(xvepre.at(k+1).mfY + fdis*sin(fhdg))*(-1));
                xpath.lineTo(xvepre.at(k+1).mfX,xvepre.at(k+1).mfY*(-1));

            }
            else
            {
                xpath.moveTo(xvepre.at(k).mfX,xvepre.at(k).mfY*(-1));
                xpath.lineTo(xvenxt.at(k).mfX,xvenxt.at(k).mfY*(-1));
                xpath.lineTo(xvenxt.at(k+1).mfX,xvenxt.at(k+1).mfY*(-1));
                xpath.lineTo(xvepre.at(k+1).mfX,xvepre.at(k+1).mfY*(-1));
            }
            xpath.closeSubpath();
            QGraphicsPathItem * pitem = new QGraphicsPathItem;
            pitem->setPath(xpath);
//            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
            int nlanetype = xvepre.at(k).mnlanetype;
            if(xvepre.at(k).mnlane<=0)nlanetype = xvepre.at(k+1).mnlanetype;
            QColor brushcolor = Qt::darkGray;
            switch (nlanetype) {
            case 0:
                brushcolor = QColor(0x66,0xBF,0x00);
                break;
            case 1:
                brushcolor = Qt::darkGreen;
                break;
            case 2:
                brushcolor = Qt::darkGray;
                break;
            case 8:
                brushcolor = Qt::red;
                break;
            case 9:
                brushcolor = QColor(0xB2,0xB2,0xD6);
                break;
            default:
                brushcolor = Qt::yellow;//Qt::darkGreen;
                break;
            }
            pitem->setBrush(brushcolor);
            pitem->setPen(QPen(brushcolor,0.001));
  //          mpscene->addItem(pitem);
            xvectorgrapath.push_back(pitem);
        }
    }

    return xvectorgrapath;


}

std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadMarkItem(RoadDigit *prd)
{
    std::vector<QGraphicsPathItem *> xvectorgrapath;
    std::vector<iv::RoadDigitUnit> * pvectorrdu =  prd->GetRDU();
    int nsize = pvectorrdu->size();
    int i;
    double flmw = 0.15;

    for(i=0;i<(nsize-1);i++)
    {
        std::vector<iv::LanePoint> xvepre = pvectorrdu->at(i).mvectorLanePoint;
        std::vector<iv::LanePoint> xvenxt = pvectorrdu->at(i+1).mvectorLanePoint;
        if(xvenxt.size() != xvepre.size())
        {
            continue;
        }
        unsigned int k;
        int noldmarktype = -1;
        double foldx1,foldy1,foldx2,foldy2;
//        double foldx1_2,foldy1_2,foldx2_2,foldy2_2;
        double fx1,fx2,fx3,fx4,fy1,fy2,fy3,fy4;
        for(k=0;k<(xvepre.size());k++)
        {
            QPainterPath xpath;
            int ncolor = -3;
            int nmarktype = xvepre[k].mnlanemarktype;
            if(nmarktype >= 0)
            {
                if(nmarktype<2)
                {
                    if((nmarktype == 0)||(IsDrawMark(xvepre[k].mS)))
                    {
                        fx1 = xvepre[k].mfX + 0.5*flmw * cos(xvepre[k].mfhdg - M_PI/2.0);
                        fy1 = (xvepre[k].mfY+0.5*flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0);
                        fx2 = xvenxt[k].mfX+ 0.5*flmw * cos(xvenxt[k].mfhdg - M_PI/2.0);
                        fy2 = (xvenxt[k].mfY+0.5*flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0);
                        fx3 = xvenxt[k].mfX + 0.5*flmw * cos(xvenxt[k].mfhdg + M_PI/2.0);
                        fy3 = (xvenxt[k].mfY+0.5*flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0);
                        fx4 = xvepre[k].mfX+ 0.5*flmw * cos(xvepre[k].mfhdg + M_PI/2.0);
                        fy4 = (xvepre[k].mfY+0.5*flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0);
                        if(noldmarktype != nmarktype)
                        {
                        xpath.moveTo(fx1,fy1);
                        xpath.lineTo(fx2,fy2);
                        xpath.lineTo(fx3,fy3);
                        xpath.lineTo(fx4,fy4);
                        xpath.closeSubpath();
                        }
                        else
                        {
                            xpath.moveTo(foldx1,foldy1);
                            xpath.lineTo(fx2,fy2);
                            xpath.lineTo(fx3,fy3);
                            xpath.lineTo(foldx2,foldy2);
                            xpath.closeSubpath();
                            foldx1 = fx2;
                            foldy1 = fy2;
                            foldx2 = fx3;
                            foldy2 = fy3;
                            noldmarktype = nmarktype;
                        }
                        ncolor = xvepre[k].mnlanecolor;


                    }
                }
                else
                {
                    if((nmarktype == 2)||(nmarktype == 3)||(IsDrawMark(xvepre[k].mS)))
                    {
                        xpath.moveTo(xvepre[k].mfX + flmw * cos(xvenxt[k].mfhdg + M_PI/2.0),
                                     (xvepre[k].mfY+flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0));
                        xpath.lineTo(xvenxt[k].mfX+ flmw * cos(xvepre[k].mfhdg + M_PI/2.0),
                                     (xvenxt[k].mfY+flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0));

                        xpath.lineTo(xvenxt[k].mfX + 2*flmw * cos(xvenxt[k].mfhdg + M_PI/2.0),
                                     (xvenxt[k].mfY+2*flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0));
                        xpath.lineTo(xvepre[k].mfX+ 2*flmw * cos(xvepre[k].mfhdg + M_PI/2.0),
                                     (xvepre[k].mfY+2*flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0));
                        xpath.closeSubpath();
                        ncolor = xvepre[k].mnlanecolor;
                    }
                    if((nmarktype == 2)||(nmarktype == 4)||(IsDrawMark(xvepre[k].mS)))
                    {
                        xpath.moveTo(xvepre[k].mfX + flmw * cos(xvenxt[k].mfhdg - M_PI/2.0),
                                     (xvepre[k].mfY+flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0));
                        xpath.lineTo(xvenxt[k].mfX+ flmw * cos(xvepre[k].mfhdg - M_PI/2.0),
                                     (xvenxt[k].mfY+flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0));

                        xpath.lineTo(xvenxt[k].mfX + 2*flmw * cos(xvenxt[k].mfhdg - M_PI/2.0),
                                     (xvenxt[k].mfY+2*flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0));
                        xpath.lineTo(xvepre[k].mfX+ 2*flmw * cos(xvepre[k].mfhdg - M_PI/2.0),
                                     (xvepre[k].mfY+2*flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0));
                        xpath.closeSubpath();
                        ncolor = xvepre[k].mnlanecolor;
                    }

                }
            }

            if(ncolor != -3)
            {
                QGraphicsPathItem * pitem = new QGraphicsPathItem;
                pitem->setPath(xpath);
                QColor brushcolor;
                switch (ncolor) {
                case 0:
                    brushcolor = Qt::white;
                    break;
                case 1:
                    brushcolor = Qt::blue;
                    break;
                case 2:
                    brushcolor = Qt::green;
                    break;
                case 3:
                    brushcolor = Qt::red;
                    break;
                case 4:
                    brushcolor = Qt::white;
                    break;
                case 5:
                    brushcolor = Qt::yellow;
                    break;
                case 6:
                    brushcolor = Qt::yellow; //orange use yellow replace
                    break;
                default:
                    brushcolor = Qt::white;
                    break;
                }
                pitem->setBrush(brushcolor);
                pitem->setPen(QPen(brushcolor,0.001));
//                pitem->setPos(mfViewMoveX + VIEW_WIDTH/2,-mfViewMoveY +VIEW_HEIGHT/2);
//                mpscene->addItem(pitem);
                xvectorgrapath.push_back(pitem);
            }
            else
            {
                noldmarktype = -1;
            }


        }

    }

    return xvectorgrapath;


}

bool xodrscenfunc::IsDrawMark(double s)
{
    const double dotdis = 10.0;
    const double dotlen = 5.0;

    double  y = fmod(s,dotdis);
    if(y>dotlen)return true;
    else
    {
        return false;
    }
}