modularization/src/tool/map_lanetoxodr/xodrscenfunc.cpp

#include "xodrscenfunc.h"

xodrscenfunc::xodrscenfunc()
{

}

std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadParkingItem(RoadDigit *prd,std::vector<QGraphicsTextItem *> & xvectortext,
                                                                  std::vector<QGraphicsPathItem *>  & xvectormarkgraph)
{
    std::vector<QGraphicsPathItem *> xvectorgrapath;
    std::vector<iv::ParkingPoint> * pvectorpark = prd->GetParkingPoint();
    unsigned int lsize = pvectorpark->size();
    unsigned int i;
    for(i=0;i<lsize;i++)
    {
        std::vector<iv::ParkingXY> xvectorxy = pvectorpark->at(i).mParkingXY;
        QPainterPath xpath;
        xpath.moveTo(xvectorxy[0].mX,xvectorxy[0].mY*(-1));
        xpath.lineTo(xvectorxy[1].mX,xvectorxy[1].mY*(-1));
        xpath.lineTo(xvectorxy[2].mX,xvectorxy[2].mY*(-1));
        xpath.lineTo(xvectorxy[3].mX,xvectorxy[3].mY*(-1));
        xpath.closeSubpath();
        QGraphicsPathItem * pitem = new QGraphicsPathItem;
        pitem->setPath(xpath);
        pitem->setBrush(Qt::darkGray);
        pitem->setPen(QPen(Qt::darkGray,0.001));
        xvectorgrapath.push_back(pitem);

        QGraphicsTextItem * pitemtext = new QGraphicsTextItem;

        pitemtext->setPlainText(pvectorpark->at(i).strtext.data());
        QRectF xrect =  pitemtext->boundingRect();
        QFont font = pitemtext->font();
        font.setPixelSize(100.0);
//                font.setPixelSize(1);  // 像素大小
        //        font.setItalic(false);  // 斜体
        //        font.setUnderline(false);  // 下划线
        pitemtext->setFont(font);
  //      pitemtext->setFont(QFont("华文琥珀",10));
        pitemtext->setDefaultTextColor(QColor(255,255,255));
        pitemtext->setFlags(QGraphicsItem::ItemIsMovable|QGraphicsItem::ItemIsSelectable);
        double fScale = 0.01;
        pitemtext->setScale(fScale);
        xrect =  pitemtext->boundingRect();
        qDebug("rect height:  %f",xrect.height());
        double fwidth = xrect.width() * fScale;
        double fheight = xrect.height()*fScale;
        double ftemx = fwidth * (-0.5);
        double ftemy = fheight * 0.5;

        double fparkhdg = pvectorpark->at(i).mfHdg;
        double fposx = ftemx*cos(fparkhdg)-ftemy*sin(fparkhdg);
        double fposy = ftemx*sin(fparkhdg)+ftemy*cos(fparkhdg);


//        pitemtext->setDefaultTextColor(QColor(0, 160, 230));  // 文本色
        pitemtext->setDefaultTextColor(Qt::white);
        pitemtext->setRotation(pvectorpark->at(i).mfHdg *(-1.0)*180.0/M_PI);
        pitemtext->setPos(pvectorpark->at(i).mParkingCenterXY.mX + fposx,(pvectorpark->at(i).mParkingCenterXY.mY+fposy)*(-1));
        xvectortext.push_back(pitemtext);



    }

    for(i=0;i<lsize;i++)
    {
        std::vector<std::vector<iv::ParkingXY>> xvectormark = pvectorpark->at(i).mParkingMark;
        unsigned int j;
        for(j=0;j<xvectormark.size();j++)
        {
        QPainterPath xpath;
        if(xvectormark.at(j).size()<4)
        {
            std::cout<<"Park Mark size < 4"<<std::endl;
            continue;
        }
        xpath.moveTo(xvectormark.at(j).at(0).mX,xvectormark.at(j).at(0).mY*(-1));
        xpath.lineTo(xvectormark.at(j).at(1).mX,xvectormark.at(j).at(1).mY*(-1));
        xpath.lineTo(xvectormark.at(j).at(2).mX,xvectormark.at(j).at(2).mY*(-1));
        xpath.lineTo(xvectormark.at(j).at(3).mX,xvectormark.at(j).at(3).mY*(-1));
        xpath.closeSubpath();
        QGraphicsPathItem * pitem = new QGraphicsPathItem;
        pitem->setPath(xpath);
        pitem->setBrush(Qt::yellow);
        pitem->setPen(QPen(Qt::white,0.001));
        xvectormarkgraph.push_back(pitem);
        }
    }
    return xvectorgrapath;
}

std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadRefGeoItem(RoadDigit *prd)
{
    std::vector<QGraphicsPathItem *> xvectorgrapath;
    std::vector<iv::RoadDigitUnit> * pvectorrdu =  prd->GetRDU();
    int nsize = pvectorrdu->size();
    int i;
    double fgeowidth = 0.3;
    double fmarklen = 3.0;
    for(i=0;i<(nsize-1);i++)
    {
        QPainterPath xpath;
        double fx1,fx2,fx3,fx4,fy1,fy2,fy3,fy4;

        fx1 = pvectorrdu->at(i).mX + 0.5*fgeowidth * cos(pvectorrdu->at(i).mfHdg - M_PI/2.0);
        fy1 = (pvectorrdu->at(i).mY + 0.5* fgeowidth * sin(pvectorrdu->at(i).mfHdg - M_PI/2.0))*(-1.0);
        fx2 = pvectorrdu->at(i+1).mX+ 0.5*fgeowidth * cos(pvectorrdu->at(i+1).mfHdg - M_PI/2.0);
        fy2 = (pvectorrdu->at(i+1).mY +0.5*fgeowidth * sin(pvectorrdu->at(i+1).mfHdg - M_PI/2.0))*(-1.0);
        fx3 = pvectorrdu->at(i+1).mX + 0.5*fgeowidth * cos(pvectorrdu->at(i+1).mfHdg + M_PI/2.0);
        fy3 = (pvectorrdu->at(i+1).mY+0.5*fgeowidth * sin(pvectorrdu->at(i+1).mfHdg + M_PI/2.0))*(-1.0);
        fx4 = pvectorrdu->at(i).mX+ 0.5*fgeowidth * cos(pvectorrdu->at(i).mfHdg + M_PI/2.0);
        fy4 = (pvectorrdu->at(i).mY+0.5*fgeowidth * sin(pvectorrdu->at(i).mfHdg + M_PI/2.0))*(-1.0);
        xpath.moveTo(fx1,fy1);
        xpath.lineTo(fx2,fy2);
        xpath.lineTo(fx3,fy3);
        xpath.lineTo(fx4,fy4);
        xpath.closeSubpath();

        QGraphicsPathItem * pitem = new QGraphicsPathItem;
        pitem->setPath(xpath);
        QColor brushcolor = Qt::red;

        pitem->setBrush(brushcolor);
        pitem->setPen(QPen(brushcolor,0.001));
        xvectorgrapath.push_back(pitem);
    }

    if(nsize>0)
    {
        QPainterPath xpath;
        double fx1,fx2,fx3,fx4,fy1,fy2,fy3,fy4;
        fx1 = pvectorrdu->at(0).mX + 0.5*fmarklen * cos(pvectorrdu->at(0).mfHdg - M_PI/2.0);
        fy1 = (pvectorrdu->at(0).mY + 0.5* fmarklen * sin(pvectorrdu->at(0).mfHdg - M_PI/2.0))*(-1.0);
        fx2 = pvectorrdu->at(0).mX+ 0.5*fmarklen * cos(pvectorrdu->at(0).mfHdg + M_PI/2.0);
        fy2 = (pvectorrdu->at(0).mY+0.5*fmarklen * sin(pvectorrdu->at(0).mfHdg + M_PI/2.0))*(-1.0);
//        fx3 = pvectorrdu->at(0).mX+ 0.65*fmarklen * cos(pvectorrdu->at(0).mfHdg );
//        fy3 = (pvectorrdu->at(0).mY+0.65*fmarklen * sin(pvectorrdu->at(0).mfHdg ))*(-1.0);
        fx3 = fx2+ 0.1*fmarklen * cos(pvectorrdu->at(0).mfHdg);
        fy3 = (fy2*(-1.0)+0.1*fmarklen * sin(pvectorrdu->at(0).mfHdg ))*(-1.0);
        fx4 = fx1+ 0.1*fmarklen * cos(pvectorrdu->at(0).mfHdg );
        fy4 = (fy1*(-1.0)+0.1*fmarklen * sin(pvectorrdu->at(0).mfHdg ))*(-1.0);
        xpath.moveTo(fx1,fy1);
        xpath.lineTo(fx2,fy2);
        xpath.lineTo(fx3,fy3);
        xpath.lineTo(fx4,fy4);
        xpath.closeSubpath();

        QGraphicsPathItem * pitem = new QGraphicsPathItem;
        pitem->setPath(xpath);
        QColor brushcolor = Qt::green;

        pitem->setBrush(brushcolor);
        pitem->setPen(QPen(brushcolor,0.001));
        xvectorgrapath.push_back(pitem);
    }

    return xvectorgrapath;
}

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(xvepre[k].mnlane == 0)
                    {
                        int ooo = 0;
                        ooo++;
                    }
                    if((nmarktype == 0)||(IsDrawMark(xvepre[k].mS)))
                    {
                        if(xvepre[k].mnlane == 0)
                        {
                            int ooo = 0;
                            ooo++;
                        }
                        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;
    }
}