<commit_before>#include "mapQGraphicsView.h"
#include <QPointF>
#include <QReadLocker>
#include "poiQGraphicsEllipseItem.h"
#include "wallQGraphicsLineItem.h"
#include "atcQGraphicsRectItem.h"
#include "fleetManager.h"
#include <iostream>
#include "mainwindow.h"
#include <cmath>
#include <QDebug>
#include <QMessageBox>
#include "flogger.h"

MapQGraphicsView::MapQGraphicsView(FleetManager* fleetManager, QWidget* parent) :
    QGraphicsView(parent), wallToBeAddedStartPoint_(NULL), atcToBeAddedStartPoint_(NULL),
    mapScale_(1), traceShown_(true), fleetManager_(fleetManager),
    wallToBeAddedStartPointText_(NULL), wallToBeAddedEndPointText_(NULL),
    atcToBeAddedStartPointText_(NULL), atcToBeAddedEndPointText_(NULL)
{
    setRenderHints(QPainter::Antialiasing);
}

void MapQGraphicsView::mousePressEvent(QMouseEvent *event)
{
    if (event->button()==Qt::LeftButton)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::CURSOR)
        {
            scene()->clearSelection();
            if ( QGraphicsItem* item = itemAt(event->pos()) )
            {
                item->setSelected(true);
            }
            setDragMode(QGraphicsView::NoDrag);
            emit roombaSelected();
            qDebug() << "Draw a cursor!";
            (*flog.ts) << "Draw a cursor!" << endl;
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            setDragMode(QGraphicsView::NoDrag);
            qDebug() << "Start a wall!";
            wallToBeAdded_ = new WallQGraphicsLineItem
                    (fleetManager_, p.x(), p.y(), p.x(), p.y());
            wallToBeAddedStartPoint_ = new QPointF(p.x(), p.y());
            scene()->addItem(wallToBeAdded_);

            // Add textual coordinates to the beginning of the wall line
            wallToBeAddedStartPointText_ = new QGraphicsSimpleTextItem
                    ("X: " + QString::number(p.x()*Util::COORDCORRECTION, 'f', 0) +
                     " Y: " +  QString::number(p.y()*Util::COORDCORRECTION, 'f', 0));
            wallToBeAddedStartPointText_->setPos(p);
            wallToBeAddedStartPointText_->setZValue(5);
            QBrush wallToBeAddedStartPointBrush(Qt::GlobalColor::blue);
            wallToBeAddedStartPointText_->setBrush(wallToBeAddedStartPointBrush);
            scene()->addItem(wallToBeAddedStartPointText_);

            qDebug() << "Pos: " << p.x() << "y: "<< p.y();
            (*flog.ts)<< QString("Start a wall @ x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {

            setDragMode(QGraphicsView::NoDrag);
            qDebug() << "Draw a atc!";
            atcToBeAdded_ = new AtcQGraphicsRectItem
                    (fleetManager_, p.x(), p.y(), 0, 0);
            //                (0,0,0,0);

            atcToBeAddedStartPoint_ = new QPointF(p.x(), p.y());
            scene()->addItem(atcToBeAdded_);

            // Add textual coordinates to the Top Left corner point of the Rectangle
            atcToBeAddedStartPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x()*Util::COORDCORRECTION) +
                                                                      " Y: " +  QString::number(p.y()*Util::COORDCORRECTION));
            atcToBeAddedStartPointText_->setPos(p);
            atcToBeAddedStartPointText_->setZValue(5);
            QBrush atcToBeAddedStartPointBrush(Qt::GlobalColor::blue);
            atcToBeAddedStartPointText_->setBrush(atcToBeAddedStartPointBrush);
            scene()->addItem(atcToBeAddedStartPointText_);

            qDebug() << "Square corner X: " + QString::number(p.x()) + " Y: " +  QString::number(p.y());
            //        AtcQGraphicsRectItem* atc = new AtcQGraphicsRectItem
            //                (0.0-POIWIDTH/5.0-TRACEWIDTH/2.0, 0.0-POIWIDTH/5.0-TRACEWIDTH/2.0, 5*POIWIDTH, 5*POIWIDTH);

            //              (0.0-5*POIWIDTH/2.0, 0.0-5*POIWIDTH/2.0, 5*POIWIDTH, 5*POIWIDTH);
            //        (0.0, 0.0, 5*POIWIDTH, 5*POIWIDTH);

            //        atc->setPos(p);
            //        fleetManager_->pushATC(p);

            //        qDebug() << "pushATC(p) p.x(): " << p.x() << "p.y(): "<< p.y();

            //        atc->setFlag(QGraphicsItem::ItemIsSelectable,true);
            //        atc->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
            //        scene()->addItem(atc);
            //        fleetManager_->addAtc(atcToBeAdded_);

            //        qDebug() << "Adding scenePos().x(): " << atc->scenePos().x()
            //                 << " ,scenePos().y(): " << atc->scenePos().y();
            qDebug() << "squP.x(): " << p.x() << "P.y(): "<< p.y();
            (*flog.ts)<< QString("Draw a ATC, Adding ATC with x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;

            emit mapChanged();
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::POI)
        {
            if(fleetManager_->isBlocked(&p))
            {
                QMessageBox::warning
                        (parentWidget(), "", tr("POI must be inserted further away from wall!"));
            }
            else
            {
                qDebug() << "Draw a poi!";
                setDragMode(QGraphicsView::NoDrag);
                PoiQGraphicsEllipseItem* poi = new PoiQGraphicsEllipseItem
                        (fleetManager_, 0.0-Util::POIWIDTH/2.0, 0.0-Util::POIWIDTH/2.0, Util::POIWIDTH, Util::POIWIDTH);
                poi->setPos(p);
                poi->setFlag(QGraphicsItem::ItemIsSelectable,false);
                poi->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
                scene()->addItem(poi);
                fleetManager_->addPoi(poi);
                qDebug() << "Adding POI with x: " << poi->scenePos().x()
                         << " , y: " << poi->scenePos().y();
                emit mapChanged();
                (*flog.ts)<< QString("Draw a POI, Adding POI with x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;
            }
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::START ||
                 selectedPaintTool_ == Util::SelectedPaintTool::STARTVIRTUAL)
        {
            qDebug() << "Draw a start!";
            (*flog.ts) << "Draw a start" << endl;
            setDragMode(QGraphicsView::NoDrag);
            PoiQGraphicsEllipseItem *startPoint = new PoiQGraphicsEllipseItem
                    (fleetManager_, 0.0-Util::POIWIDTH*2.0/3.0, 0.0-Util::POIWIDTH*2.0/3.0,
                     Util::POIWIDTH*4.0/3.0, Util::POIWIDTH*4.0/3.0);
            startPoint->setPos(p);
            QBrush brush(Qt::GlobalColor::green);
            startPoint->setBrush(brush);
            startPoint->setFlag(QGraphicsItem::ItemIsSelectable,false);
            //movable needs additional logic before allowing
            startPoint->setFlag(QGraphicsItem::ItemIsMovable,false);
            //TODO: Add deleleting of startPoint when moving it
            scene()->addItem(startPoint);
            if(selectedPaintTool_ == Util::SelectedPaintTool::START)
            {
                fleetManager_->createRoomba(startPoint, false); //real roomba
            }
            else
            {
                fleetManager_->createRoomba(startPoint, true); //virtual roomba
            }
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mousePressEvent(event);
}

void MapQGraphicsView::mouseMoveEvent(QMouseEvent *event)
{
    // event->button() returns always Qt::NoButton for mouse move events, so button check is not needed
    if (wallToBeAddedStartPoint_ != NULL)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            wallToBeAdded_->setLine(wallToBeAddedStartPoint_->x(), wallToBeAddedStartPoint_->y(),
                                    p.x(), p.y());
        }
        // Add textual coordinates to the end of the wall line
        if (wallToBeAddedEndPointText_ == NULL)
        {
            wallToBeAddedEndPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x()) + " Y: " +  QString::number(p.y()));
            wallToBeAddedEndPointText_->setPos(p);
            QBrush wallToBeAddedEndPointTextBrush(Qt::GlobalColor::blue);
            wallToBeAddedEndPointText_->setBrush(wallToBeAddedEndPointTextBrush);
            scene()->addItem(wallToBeAddedEndPointText_);
            wallToBeAddedEndPointText_->setZValue(5);
        }
        // Update textual coordinates in the end of the wall line
        else
        {
            // Calculate the current wall length
            float deltaX = wallToBeAdded_->line().x2() - wallToBeAdded_->line().x1();
            float deltaY = wallToBeAdded_->line().y1() - wallToBeAdded_->line().y2();
            // TODO: Add pythagoras from here and iRoomba to some utility function
            float distance = sqrt(pow(deltaX,2)+pow(deltaY,2) )*Util::COORDCORRECTION;
            // Use offset to avoid colliding with cursor
            QPointF pointToDrawLength = p;
            pointToDrawLength.setY(pointToDrawLength.y()+Util::WALLLENGTHINDICATOROFFSET);
            wallToBeAddedEndPointText_->setPos(pointToDrawLength);
            wallToBeAddedEndPointText_->setText(QString::number(distance, 'f', 0) + "cm"); // Ignore decimals in wall length
        }
    }
    if (atcToBeAddedStartPoint_ != NULL)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {
            atcToBeAdded_->setRect(atcToBeAddedStartPoint_->x(), atcToBeAddedStartPoint_->y(),
                                   p.x()-atcToBeAddedStartPoint_->x(), p.y()-atcToBeAddedStartPoint_->y());

            scene()->update();

        }
        // Add textual coordinates to the end of the rectangle
        if (atcToBeAddedEndPointText_ == NULL)
        {
            atcToBeAddedEndPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x()) + " Y: " +  QString::number(p.y()));
            atcToBeAddedEndPointText_->setPos(p);
            QBrush atcToBeAddedEndPointTextBrush(Qt::GlobalColor::blue);
            atcToBeAddedEndPointText_->setBrush(atcToBeAddedEndPointTextBrush);
            scene()->addItem(atcToBeAddedEndPointText_);
            atcToBeAddedEndPointText_->setZValue(5);
        }
        // Update textual coordinates in the end of the rectangle
        // line width is 3, we have to add 3 for p.x() and p.y()

        else
        {

            atcToBeAddedEndPointText_->setPos(p);
            atcToBeAddedEndPointText_->setText("X: " + QString::number(p.x()*Util::COORDCORRECTION) + " Y: " +  QString::number(p.y()*Util::COORDCORRECTION)
                                               + "\nW " + QString::number((abs(atcToBeAdded_->rect().width())+3)*Util::COORDCORRECTION)
                                               + " H " + QString::number((abs(atcToBeAdded_->rect().height())+3)*Util::COORDCORRECTION) );
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mouseMoveEvent(event);
}

void MapQGraphicsView::mouseReleaseEvent(QMouseEvent *event)
{
    if (event->button()==Qt::LeftButton)
    {
        if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            wallToBeAdded_->setFlag(QGraphicsItem::ItemIsSelectable,false);
            wallToBeAdded_->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
            fleetManager_->addWall(wallToBeAdded_);
            if(fleetManager_->removeBlockedPois())  //POIs blocked by the wall are removed
            {
                QMessageBox::warning
                        (parentWidget(), "", tr("POIs too close to the wall were removed"));
            }
            wallToBeAdded_ = NULL;
            delete wallToBeAddedStartPoint_;
            wallToBeAddedStartPoint_ = NULL;
            delete wallToBeAddedStartPointText_;
            wallToBeAddedStartPointText_ = NULL;
            delete wallToBeAddedEndPointText_;
            wallToBeAddedEndPointText_ = NULL;
            emit mapChanged();
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {
            atcToBeAdded_->setFlag(QGraphicsItem::ItemIsSelectable,false);
            atcToBeAdded_->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
            fleetManager_->addAtc(atcToBeAdded_);
            delete atcToBeAddedStartPoint_;
            atcToBeAddedStartPoint_ = NULL;
            delete atcToBeAddedStartPointText_;
            atcToBeAddedStartPointText_ = NULL;
            delete atcToBeAddedEndPointText_;
            atcToBeAddedEndPointText_ = NULL;
            emit mapChanged();
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mouseReleaseEvent(event);
}

void MapQGraphicsView::setSelectedPaintTool(Util::SelectedPaintTool tool)
{
    selectedPaintTool_ = tool;
}

//gives map's width in mm
unsigned int MapQGraphicsView::getmapScale()
{
    return mapScale_;
}

//give new width in mm.
void MapQGraphicsView::setmapScale(double scaleFactor)
{
    mapScale_ = scaleFactor;
    resetTransform();
    //MAP'S WIDTH IN PIXELS IS FIXED ATM
    scale(scaleFactor, scaleFactor);
}

MapQGraphicsView::~MapQGraphicsView()
{
}
<commit_msg>MAP: Adjusted precision of ATC's informative texts<commit_after>#include "mapQGraphicsView.h"
#include <QPointF>
#include <QReadLocker>
#include "poiQGraphicsEllipseItem.h"
#include "wallQGraphicsLineItem.h"
#include "atcQGraphicsRectItem.h"
#include "fleetManager.h"
#include <iostream>
#include "mainwindow.h"
#include <cmath>
#include <QDebug>
#include <QMessageBox>
#include "flogger.h"

MapQGraphicsView::MapQGraphicsView(FleetManager* fleetManager, QWidget* parent) :
    QGraphicsView(parent), wallToBeAddedStartPoint_(NULL), atcToBeAddedStartPoint_(NULL),
    mapScale_(1), traceShown_(true), fleetManager_(fleetManager),
    wallToBeAddedStartPointText_(NULL), wallToBeAddedEndPointText_(NULL),
    atcToBeAddedStartPointText_(NULL), atcToBeAddedEndPointText_(NULL)
{
    setRenderHints(QPainter::Antialiasing);
}

void MapQGraphicsView::mousePressEvent(QMouseEvent *event)
{
    if (event->button()==Qt::LeftButton)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::CURSOR)
        {
            scene()->clearSelection();
            if ( QGraphicsItem* item = itemAt(event->pos()) )
            {
                item->setSelected(true);
            }
            setDragMode(QGraphicsView::NoDrag);
            emit roombaSelected();
            qDebug() << "Draw a cursor!";
            (*flog.ts) << "Draw a cursor!" << endl;
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            setDragMode(QGraphicsView::NoDrag);
            qDebug() << "Start a wall!";
            wallToBeAdded_ = new WallQGraphicsLineItem
                    (fleetManager_, p.x(), p.y(), p.x(), p.y());
            wallToBeAddedStartPoint_ = new QPointF(p.x(), p.y());
            scene()->addItem(wallToBeAdded_);

            // Add textual coordinates to the beginning of the wall line
            wallToBeAddedStartPointText_ = new QGraphicsSimpleTextItem
                    ("X: " + QString::number(p.x()*Util::COORDCORRECTION, 'f', 0) +
                     " Y: " +  QString::number(p.y()*Util::COORDCORRECTION, 'f', 0));
            wallToBeAddedStartPointText_->setPos(p);
            wallToBeAddedStartPointText_->setZValue(5);
            QBrush wallToBeAddedStartPointBrush(Qt::GlobalColor::blue);
            wallToBeAddedStartPointText_->setBrush(wallToBeAddedStartPointBrush);
            scene()->addItem(wallToBeAddedStartPointText_);

            qDebug() << "Pos: " << p.x() << "y: "<< p.y();
            (*flog.ts)<< QString("Start a wall @ x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {

            setDragMode(QGraphicsView::NoDrag);
            qDebug() << "Draw a atc!";
            atcToBeAdded_ = new AtcQGraphicsRectItem
                    (fleetManager_, p.x(), p.y(), 0, 0);
            //                (0,0,0,0);

            atcToBeAddedStartPoint_ = new QPointF(p.x(), p.y());
            scene()->addItem(atcToBeAdded_);

            // Add textual coordinates to the Top Left corner point of the Rectangle
            atcToBeAddedStartPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x()*Util::COORDCORRECTION, 'f', 0) +
                                                                      " Y: " +  QString::number(p.y()*Util::COORDCORRECTION, 'f', 0));
            atcToBeAddedStartPointText_->setPos(p);
            atcToBeAddedStartPointText_->setZValue(5);
            QBrush atcToBeAddedStartPointBrush(Qt::GlobalColor::blue);
            atcToBeAddedStartPointText_->setBrush(atcToBeAddedStartPointBrush);
            scene()->addItem(atcToBeAddedStartPointText_);
            (*flog.ts)<< QString("Draw a ATC, Adding ATC with x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;
            emit mapChanged();
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::POI)
        {
            if(fleetManager_->isBlocked(&p))
            {
                QMessageBox::warning
                        (parentWidget(), "", tr("POI must be inserted further away from wall!"));
            }
            else
            {
                qDebug() << "Draw a poi!";
                setDragMode(QGraphicsView::NoDrag);
                PoiQGraphicsEllipseItem* poi = new PoiQGraphicsEllipseItem
                        (fleetManager_, 0.0-Util::POIWIDTH/2.0, 0.0-Util::POIWIDTH/2.0, Util::POIWIDTH, Util::POIWIDTH);
                poi->setPos(p);
                poi->setFlag(QGraphicsItem::ItemIsSelectable,false);
                poi->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
                scene()->addItem(poi);
                fleetManager_->addPoi(poi);
                qDebug() << "Adding POI with x: " << poi->scenePos().x()
                         << " , y: " << poi->scenePos().y();
                emit mapChanged();
                (*flog.ts)<< QString("Draw a POI, Adding POI with x: %1 y: %2").arg(p.x()).arg(p.y()) <<endl;
            }
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::START ||
                 selectedPaintTool_ == Util::SelectedPaintTool::STARTVIRTUAL)
        {
            qDebug() << "Draw a start!";
            (*flog.ts) << "Draw a start" << endl;
            setDragMode(QGraphicsView::NoDrag);
            PoiQGraphicsEllipseItem *startPoint = new PoiQGraphicsEllipseItem
                    (fleetManager_, 0.0-Util::POIWIDTH*2.0/3.0, 0.0-Util::POIWIDTH*2.0/3.0,
                     Util::POIWIDTH*4.0/3.0, Util::POIWIDTH*4.0/3.0);
            startPoint->setPos(p);
            QBrush brush(Qt::GlobalColor::green);
            startPoint->setBrush(brush);
            startPoint->setFlag(QGraphicsItem::ItemIsSelectable,false);
            //movable needs additional logic before allowing
            startPoint->setFlag(QGraphicsItem::ItemIsMovable,false);
            //TODO: Add deleleting of startPoint when moving it
            scene()->addItem(startPoint);
            if(selectedPaintTool_ == Util::SelectedPaintTool::START)
            {
                fleetManager_->createRoomba(startPoint, false); //real roomba
            }
            else
            {
                fleetManager_->createRoomba(startPoint, true); //virtual roomba
            }
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mousePressEvent(event);
}

void MapQGraphicsView::mouseMoveEvent(QMouseEvent *event)
{
    // event->button() returns always Qt::NoButton for mouse move events, so button check is not needed
    if (wallToBeAddedStartPoint_ != NULL)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            wallToBeAdded_->setLine(wallToBeAddedStartPoint_->x(), wallToBeAddedStartPoint_->y(),
                                    p.x(), p.y());
        }
        // Add textual coordinates to the end of the wall line
        if (wallToBeAddedEndPointText_ == NULL)
        {
            wallToBeAddedEndPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x()) + " Y: " +  QString::number(p.y()));
            wallToBeAddedEndPointText_->setPos(p);
            QBrush wallToBeAddedEndPointTextBrush(Qt::GlobalColor::blue);
            wallToBeAddedEndPointText_->setBrush(wallToBeAddedEndPointTextBrush);
            scene()->addItem(wallToBeAddedEndPointText_);
            wallToBeAddedEndPointText_->setZValue(5);
        }
        // Update textual coordinates in the end of the wall line
        else
        {
            // Calculate the current wall length
            float deltaX = wallToBeAdded_->line().x2() - wallToBeAdded_->line().x1();
            float deltaY = wallToBeAdded_->line().y1() - wallToBeAdded_->line().y2();
            // TODO: Add pythagoras from here and iRoomba to some utility function
            float distance = sqrt(pow(deltaX,2)+pow(deltaY,2) )*Util::COORDCORRECTION;
            // Use offset to avoid colliding with cursor
            QPointF pointToDrawLength = p;
            pointToDrawLength.setY(pointToDrawLength.y()+Util::WALLLENGTHINDICATOROFFSET);
            wallToBeAddedEndPointText_->setPos(pointToDrawLength);
            wallToBeAddedEndPointText_->setText(QString::number(distance, 'f', 0) + "cm"); // Ignore decimals in wall length
        }
    }
    if (atcToBeAddedStartPoint_ != NULL)
    {
        QPointF p = mapToScene(event->pos());
        if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {
            atcToBeAdded_->setRect(atcToBeAddedStartPoint_->x(), atcToBeAddedStartPoint_->y(),
                                   p.x()-atcToBeAddedStartPoint_->x(), p.y()-atcToBeAddedStartPoint_->y());

            scene()->update();

        }
        // Add textual coordinates to the end of the rectangle
        if (atcToBeAddedEndPointText_ == NULL)
        {
            atcToBeAddedEndPointText_ = new QGraphicsSimpleTextItem("X: " + QString::number(p.x(), 'f', 0) + " Y: " +  QString::number(p.y(), 'f', 0));
            atcToBeAddedEndPointText_->setPos(p);
            QBrush atcToBeAddedEndPointTextBrush(Qt::GlobalColor::blue);
            atcToBeAddedEndPointText_->setBrush(atcToBeAddedEndPointTextBrush);
            scene()->addItem(atcToBeAddedEndPointText_);
            atcToBeAddedEndPointText_->setZValue(5);
        }
        // Update textual coordinates in the end of the rectangle
        // line width is 3, we have to add 3 for p.x() and p.y()

        else
        {

            atcToBeAddedEndPointText_->setPos(p);
            atcToBeAddedEndPointText_->setText("X: " + QString::number(p.x()*Util::COORDCORRECTION, 'f', 0) + " Y: " +  QString::number(p.y()*Util::COORDCORRECTION, 'f', 0)
                                               + "\nW " + QString::number((abs(atcToBeAdded_->rect().width())+3)*Util::COORDCORRECTION, 'f', 0)
                                               + " H " + QString::number((abs(atcToBeAdded_->rect().height())+3)*Util::COORDCORRECTION, 'f', 0) );
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mouseMoveEvent(event);
}

void MapQGraphicsView::mouseReleaseEvent(QMouseEvent *event)
{
    if (event->button()==Qt::LeftButton)
    {
        if (selectedPaintTool_ == Util::SelectedPaintTool::WALL)
        {
            wallToBeAdded_->setFlag(QGraphicsItem::ItemIsSelectable,false);
            wallToBeAdded_->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
            fleetManager_->addWall(wallToBeAdded_);
            if(fleetManager_->removeBlockedPois())  //POIs blocked by the wall are removed
            {
                QMessageBox::warning
                        (parentWidget(), "", tr("POIs too close to the wall were removed"));
            }
            wallToBeAdded_ = NULL;
            delete wallToBeAddedStartPoint_;
            wallToBeAddedStartPoint_ = NULL;
            delete wallToBeAddedStartPointText_;
            wallToBeAddedStartPointText_ = NULL;
            delete wallToBeAddedEndPointText_;
            wallToBeAddedEndPointText_ = NULL;
            emit mapChanged();
        }
        else if (selectedPaintTool_ == Util::SelectedPaintTool::ATC)
        {
            atcToBeAdded_->setFlag(QGraphicsItem::ItemIsSelectable,false);
            atcToBeAdded_->setFlag(QGraphicsItem::ItemIsMovable,false); // Disabled so that the mapChanged signal works as expected
            fleetManager_->addAtc(atcToBeAdded_);
            delete atcToBeAddedStartPoint_;
            atcToBeAddedStartPoint_ = NULL;
            delete atcToBeAddedStartPointText_;
            atcToBeAddedStartPointText_ = NULL;
            delete atcToBeAddedEndPointText_;
            atcToBeAddedEndPointText_ = NULL;
            emit mapChanged();
        }
    }
    // Call the base class implementation to deliver the event for QGraphicsScene
    QGraphicsView::mouseReleaseEvent(event);
}

void MapQGraphicsView::setSelectedPaintTool(Util::SelectedPaintTool tool)
{
    selectedPaintTool_ = tool;
}

//gives map's width in mm
unsigned int MapQGraphicsView::getmapScale()
{
    return mapScale_;
}

//give new width in mm.
void MapQGraphicsView::setmapScale(double scaleFactor)
{
    mapScale_ = scaleFactor;
    resetTransform();
    //MAP'S WIDTH IN PIXELS IS FIXED ATM
    scale(scaleFactor, scaleFactor);
}

MapQGraphicsView::~MapQGraphicsView()
{
}
<|endoftext|>


<commit_before>/*
 * This file is part of the CN24 semantic segmentation software,
 * copyright (C) 2015 Clemens-Alexander Brust (ikosa dot de at gmail dot com).
 *
 * For licensing information, see the LICENSE file included with this project.
 */

#include <iomanip>

#include "Log.h"
#include "GradientTester.h"

#include "NetGraphNode.h"

namespace Conv {

void GradientTester::TestGradient ( NetGraph& graph, unsigned int skip_weights, bool fatal_fail ) {
  const double epsilon = 0.005;
  LOGDEBUG << "Testing gradient. FeedForward...";
	graph.FeedForward();
  LOGDEBUG << "Testing gradient. BackPropagate...";
  graph.BackPropagate();

	const datum initial_loss = graph.AggregateLoss();
	unsigned int global_okay = 0;
	unsigned int global_tolerable = 0;
	unsigned int global_failed = 0;
	unsigned int global_weights = 0;

  LOGDEBUG << "Initial loss: " << initial_loss;
  LOGDEBUG << "Using epsilon: " << epsilon;
  for(unsigned int l = 0; l < graph.GetNodes().size(); l++) {
		NetGraphNode* node = graph.GetNodes()[l];
		Layer* layer = node->layer;
    for(unsigned int p = 0; p < layer->parameters().size(); p++) {
			CombinedTensor* const param = layer->parameters()[p];
      LOGDEBUG << "Testing layer " << l << " (" << layer->GetLayerDescription() << "), parameter set " << p;
      LOGDEBUG << param->data;
      bool passed = true;
      unsigned int okay = 0;
      unsigned int tolerable = 0;
      unsigned int failed = 0;
      unsigned int total = 0;
      for(unsigned int e = 0; e < param->data.elements(); e+=(skip_weights + 1))
      {
        total++;
#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
	param->delta.MoveToCPU();
#endif
	const datum old_param = param->data(e);

	param->data[e] = old_param + epsilon;
	graph.FeedForward();
	const double plus_loss = graph.AggregateLoss();

#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
#endif
	param->data[e] = old_param - epsilon;
graph.FeedForward();
	const double minus_loss = graph.AggregateLoss();

	const double delta = param->delta[e];
	const double actual_delta = (plus_loss - minus_loss) / (2.0 * epsilon);

	const double ratio = actual_delta / delta;
	if(ratio > 1.02 || ratio < 0.98) {
	  if(ratio > 1.2 || ratio < 0.8) {
      if(passed) {
	      LOGWARN << "delta analytic: " << delta << ", numeric: " << actual_delta << ", ratio: " << ratio;
      }
	    passed = false;
	    // std::cout << "!" << std::flush;
			failed++;
	  } else {
	  // std::cout << "#" << std::flush;
		tolerable++;
	  }
	}
	else {
	  // std::cout << "." << std::flush;
	  okay++;
	}
#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
#endif
	param->data[e] = old_param;
      }
      // std::cout << "\n";
      if(passed) {
	LOGDEBUG << "Okay!";
      } else {
	LOGERROR << "Failed!";
      }
			LOGDEBUG << okay << " of " << total << " gradients okay (delta < 2%)";
			LOGDEBUG << tolerable << " of " << total << " gradients tolerable (delta < 20%)";
			LOGDEBUG << failed << " of " << total << " gradients failed (delta >= 20%)";
			global_okay += okay;
			global_tolerable += tolerable;
			global_failed += failed;
			global_weights += total;
    }
  }

	LOGRESULT << global_okay << " of " << global_weights << " tested gradients okay (delta < 2%)" << LOGRESULTEND;
	LOGRESULT << global_tolerable << " of " << global_weights << " tested gradients tolerable (delta < 20%)" << LOGRESULTEND;
	LOGRESULT << global_failed << " of " << global_weights << " tested gradients failed (delta >= 20%)" << LOGRESULTEND;

  if (global_failed > 0 && fatal_fail) {
    FATAL("Failed gradient check!");
  }

}

bool GradientTester::DoGradientTest(Conv::Layer* layer, Conv::Tensor& data, Conv::Tensor& delta, std::vector<Conv::CombinedTensor*>& outputs, Conv::datum epsilon, void (*WriteLossDeltas)(const std::vector<CombinedTensor*>&), datum (*CalculateLoss)(const std::vector<CombinedTensor*>&)) {
  layer->FeedForward();
  WriteLossDeltas(outputs);
  layer->BackPropagate();

  unsigned int elements = data.elements();
  unsigned int okay = 0;

  // Weight gradient test
  for (unsigned int w = 0; w < data.elements(); w++) {
#ifdef BUILD_OPENCL
    data.MoveToCPU();
    delta.MoveToCPU();
#endif
    const Conv::datum weight = data.data_ptr_const()[w];
    const Conv::datum gradient = delta.data_ptr_const()[w];

    // Using central diff
    data.data_ptr()[w] = weight + epsilon;
    layer->FeedForward();
    const Conv::datum forward_loss = CalculateLoss(outputs);

#ifdef BUILD_OPENCL
    data.MoveToCPU();
#endif
    data.data_ptr()[w] = weight - epsilon;
    layer->FeedForward();
    const Conv::datum backward_loss = CalculateLoss(outputs);

    const Conv::datum fd_gradient = (forward_loss - backward_loss) / (2.0 * epsilon);

#ifdef BUILD_OPENCL
    data.MoveToCPU();
#endif
    data.data_ptr()[w] = weight;

    const Conv::datum ratio = fd_gradient / gradient;
    if(ratio > 1.2 || ratio < 0.8) {
      LOGDEBUG << "BP Grad : " << gradient;
      LOGDEBUG << "FD Grad : " << fd_gradient;
      LOGDEBUG << "Ratio   : " << ratio;
      LOGDEBUG << "Diff    : " << gradient - fd_gradient;
    } else {
      okay++;
    }
  }
  if(okay != elements) {
    double success_rate = (double)okay/(double)elements;
    if(success_rate > 0.85)
      return true;
    else {
      LOGERROR << okay << " of " << elements << " gradients okay - " << std::setprecision(3) << 100.0 * (double)okay/(double)elements << "%";
      return false;
    }
  } else {
    return true;
  }
}

}
<commit_msg>GradientTest: Skip layers that are not gradient safe<commit_after>/*
 * This file is part of the CN24 semantic segmentation software,
 * copyright (C) 2015 Clemens-Alexander Brust (ikosa dot de at gmail dot com).
 *
 * For licensing information, see the LICENSE file included with this project.
 */

#include <iomanip>

#include "Log.h"
#include "GradientTester.h"

#include "NetGraphNode.h"

namespace Conv {

void GradientTester::TestGradient ( NetGraph& graph, unsigned int skip_weights, bool fatal_fail ) {
  const double epsilon = 0.005;
  LOGDEBUG << "Testing gradient. FeedForward...";
	graph.FeedForward();
  LOGDEBUG << "Testing gradient. BackPropagate...";
  graph.BackPropagate();

	const datum initial_loss = graph.AggregateLoss();
	unsigned int global_okay = 0;
	unsigned int global_tolerable = 0;
	unsigned int global_failed = 0;
	unsigned int global_weights = 0;

  LOGDEBUG << "Initial loss: " << initial_loss;
  LOGDEBUG << "Using epsilon: " << epsilon;
  for(unsigned int l = 0; l < graph.GetNodes().size(); l++) {
		NetGraphNode* node = graph.GetNodes()[l];
		Layer* layer = node->layer;
    if(layer->IsNotGradientSafe())
      continue;
    for(unsigned int p = 0; p < layer->parameters().size(); p++) {
			CombinedTensor* const param = layer->parameters()[p];
      LOGDEBUG << "Testing layer " << l << " (" << layer->GetLayerDescription() << "), parameter set " << p;
      LOGDEBUG << param->data;
      bool passed = true;
      unsigned int okay = 0;
      unsigned int tolerable = 0;
      unsigned int failed = 0;
      unsigned int total = 0;
      for(unsigned int e = 0; e < param->data.elements(); e+=(skip_weights + 1))
      {
        total++;
#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
	param->delta.MoveToCPU();
#endif
	const datum old_param = param->data(e);

	param->data[e] = old_param + epsilon;
	graph.FeedForward();
	const double plus_loss = graph.AggregateLoss();

#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
#endif
	param->data[e] = old_param - epsilon;
graph.FeedForward();
	const double minus_loss = graph.AggregateLoss();

	const double delta = param->delta[e];
	const double actual_delta = (plus_loss - minus_loss) / (2.0 * epsilon);

	const double ratio = actual_delta / delta;
	if(ratio > 1.02 || ratio < 0.98) {
	  if(ratio > 1.2 || ratio < 0.8) {
      if(passed) {
	      LOGWARN << "delta analytic: " << delta << ", numeric: " << actual_delta << ", ratio: " << ratio;
      }
	    passed = false;
	    // std::cout << "!" << std::flush;
			failed++;
	  } else {
	  // std::cout << "#" << std::flush;
		tolerable++;
	  }
	}
	else {
	  // std::cout << "." << std::flush;
	  okay++;
	}
#ifdef BUILD_OPENCL
	param->data.MoveToCPU();
#endif
	param->data[e] = old_param;
      }
      // std::cout << "\n";
      if(passed) {
	LOGDEBUG << "Okay!";
      } else {
	LOGERROR << "Failed!";
      }
			LOGDEBUG << okay << " of " << total << " gradients okay (delta < 2%)";
			LOGDEBUG << tolerable << " of " << total << " gradients tolerable (delta < 20%)";
			LOGDEBUG << failed << " of " << total << " gradients failed (delta >= 20%)";
			global_okay += okay;
			global_tolerable += tolerable;
			global_failed += failed;
			global_weights += total;
    }
  }

	LOGRESULT << global_okay << " of " << global_weights << " tested gradients okay (delta < 2%)" << LOGRESULTEND;
	LOGRESULT << global_tolerable << " of " << global_weights << " tested gradients tolerable (delta < 20%)" << LOGRESULTEND;
	LOGRESULT << global_failed << " of " << global_weights << " tested gradients failed (delta >= 20%)" << LOGRESULTEND;

  if (global_failed > 0 && fatal_fail) {
    FATAL("Failed gradient check!");
  }

}

bool GradientTester::DoGradientTest(Conv::Layer* layer, Conv::Tensor& data, Conv::Tensor& delta, std::vector<Conv::CombinedTensor*>& outputs, Conv::datum epsilon, void (*WriteLossDeltas)(const std::vector<CombinedTensor*>&), datum (*CalculateLoss)(const std::vector<CombinedTensor*>&)) {
  layer->FeedForward();
  WriteLossDeltas(outputs);
  layer->BackPropagate();

  unsigned int elements = data.elements();
  unsigned int okay = 0;

  // Weight gradient test
  for (unsigned int w = 0; w < data.elements(); w++) {
#ifdef BUILD_OPENCL
    data.MoveToCPU();
    delta.MoveToCPU();
#endif
    const Conv::datum weight = data.data_ptr_const()[w];
    const Conv::datum gradient = delta.data_ptr_const()[w];

    // Using central diff
    data.data_ptr()[w] = weight + epsilon;
    layer->FeedForward();
    const Conv::datum forward_loss = CalculateLoss(outputs);

#ifdef BUILD_OPENCL
    data.MoveToCPU();
#endif
    data.data_ptr()[w] = weight - epsilon;
    layer->FeedForward();
    const Conv::datum backward_loss = CalculateLoss(outputs);

    const Conv::datum fd_gradient = (forward_loss - backward_loss) / (2.0 * epsilon);

#ifdef BUILD_OPENCL
    data.MoveToCPU();
#endif
    data.data_ptr()[w] = weight;

    const Conv::datum ratio = fd_gradient / gradient;
    if(ratio > 1.2 || ratio < 0.8) {
      LOGDEBUG << "BP Grad : " << gradient;
      LOGDEBUG << "FD Grad : " << fd_gradient;
      LOGDEBUG << "Ratio   : " << ratio;
      LOGDEBUG << "Diff    : " << gradient - fd_gradient;
    } else {
      okay++;
    }
  }
  if(okay != elements) {
    double success_rate = (double)okay/(double)elements;
    if(success_rate > 0.85)
      return true;
    else {
      LOGERROR << okay << " of " << elements << " gradients okay - " << std::setprecision(3) << 100.0 * (double)okay/(double)elements << "%";
      return false;
    }
  } else {
    return true;
  }
}

}
<|endoftext|>


<commit_before>#pragma once

#include <vector>
#include <distributions/clustering.hpp>
#include <distributions/models/dd.hpp>
#include <distributions/models/dpd.hpp>
#include <distributions/models/nich.hpp>
#include <distributions/models/gp.hpp>
#include <distributions/mixture.hpp>
#include <distributions/io/protobuf.hpp>
#include "common.hpp"
#include "protobuf.hpp"

namespace loom
{

using distributions::rng_t;
using distributions::VectorFloat;

enum { DD_DIM = 256 };

struct ProductModel
{
    typedef protobuf::ProductModel::SparseValue Value;
    typedef distributions::Clustering<int>::PitmanYor Clustering;
    struct Mixture;

    protobuf::SparseValueSchema schema;
    Clustering clustering;
    std::vector<distributions::dirichlet_discrete::Model<DD_DIM>> dd;
    std::vector<distributions::dirichlet_process_discrete::Model> dpd;
    std::vector<distributions::gamma_poisson::Model> gp;
    std::vector<distributions::normal_inverse_chi_sq::Model> nich;

    void load (const protobuf::ProductModel & message);
};

struct ProductModel::Mixture
{
    ProductModel::Clustering::Mixture clustering;
    std::vector<distributions::dirichlet_discrete::Mixture<DD_DIM>> dd;
    std::vector<distributions::dirichlet_process_discrete::Mixture> dpd;
    std::vector<distributions::gamma_poisson::Mixture> gp;
    std::vector<distributions::normal_inverse_chi_sq::Mixture> nich;
    distributions::MixtureIdTracker id_tracker;

    void init_empty (
            const ProductModel & model,
            rng_t & rng,
            size_t empty_group_count = 1);
    void load (
            const ProductModel & model,
            const char * filename,
            rng_t & rng,
            size_t empty_roup_count = 1);
    void dump (const ProductModel & model, const char * filename);
    void add_value (
            const ProductModel & model,
            size_t groupid,
            const Value & value,
            rng_t & rng);
    void remove_value (
            const ProductModel & model,
            size_t groupid,
            const Value & value,
            rng_t & rng);
    void score (
            const ProductModel & model,
            const Value & value,
            VectorFloat & scores,
            rng_t & rng);
    void sample_value (
            const ProductModel & model,
            const VectorFloat & probs,
            Value & value,
            rng_t & rng);

private:

    void _validate (const ProductModel & model);

    template<class Mixture>
    void init_empty_factors (
            size_t empty_group_count,
            const std::vector<typename Mixture::Model> & models,
            std::vector<Mixture> & mixtures,
            rng_t & rng);

    template<class Fun>
    void apply_dense (const ProductModel & model, Fun & fun);

    template<class Fun>
    void apply_sparse (
            const ProductModel & model,
            Fun & fun,
            const Value & value);

    template<class Fun>
    void set_sparse (
            const ProductModel & model,
            Fun & fun,
            Value & value);

    struct validate_fun;
    struct load_group_fun;
    struct init_fun;
    struct dump_group_fun;
    struct add_group_fun;
    struct add_value_fun;
    struct remove_group_fun;
    struct remove_value_fun;
    struct score_fun;
    struct sample_fun;
};

template<class Fun>
inline void ProductModel::Mixture::apply_dense (
        const ProductModel & model,
        Fun & fun)
{
    //TODO("implement bb");
    for (size_t i = 0; i < dd.size(); ++i) {
        fun(i, model.dd[i], dd[i]);
    }
    for (size_t i = 0; i < dpd.size(); ++i) {
        fun(i, model.dpd[i], dpd[i]);
    }
    for (size_t i = 0; i < gp.size(); ++i) {
        fun(i, model.gp[i], gp[i]);
    }
    for (size_t i = 0; i < nich.size(); ++i) {
        fun(i, model.nich[i], nich[i]);
    }
}

template<class Fun>
inline void ProductModel::Mixture::apply_sparse (
        const ProductModel & model,
        Fun & fun,
        const Value & value)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        model.schema.validate(value);
    }

    size_t absolute_pos = 0;

    if (value.booleans_size()) {
        TODO("implement bb");
    } else {
        absolute_pos += 0;
    }

    if (value.counts_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < dd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.dd[i], dd[i], value.counts(packed_pos++));
            }
        }
        for (size_t i = 0; i < dpd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.dpd[i], dpd[i], value.counts(packed_pos++));
            }
        }
        for (size_t i = 0; i < gp.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.gp[i], gp[i], value.counts(packed_pos++));
            }
        }
    } else {
        absolute_pos += dd.size() + dpd.size() + gp.size();
    }

    if (value.reals_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < nich.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.nich[i], nich[i], value.reals(packed_pos++));
            }
        }
    }
}

template<class Fun>
inline void ProductModel::Mixture::set_sparse (
        const ProductModel & model,
        Fun & fun,
        Value & value)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        model.schema.validate(value);
    }

    size_t absolute_pos = 0;

    if (value.booleans_size()) {
        TODO("implement bb");
    } else {
        absolute_pos += 0;
    }

    if (value.counts_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < dd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.dd[i], dd[i]));
            }
        }
        for (size_t i = 0; i < dpd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.dpd[i], dpd[i]));
            }
        }
        for (size_t i = 0; i < gp.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.gp[i], gp[i]));
            }
        }
    } else {
        absolute_pos += dd.size() + dpd.size() + gp.size();
    }

    if (value.reals_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < nich.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_reals(packed_pos++, fun(model.nich[i], nich[i]));
            }
        }
    }
}

struct ProductModel::Mixture::validate_fun
{
    const size_t group_count;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model &,
            const Mixture & mixture)
    {
        LOOM_ASSERT_EQ(mixture.groups.size(), group_count);
    }
};

inline void ProductModel::Mixture::_validate (
        const ProductModel & model)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        const size_t group_count = clustering.counts().size();
        validate_fun fun = {group_count};
        apply_dense(model, fun);
        LOOM_ASSERT_EQ(id_tracker.packed_size(), group_count);
    }
}

struct ProductModel::Mixture::add_group_fun
{
    rng_t & rng;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model & model,
            Mixture & mixture)
    {
        mixture.add_group(model, rng);
    }
};

struct ProductModel::Mixture::add_value_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.add_value(model, groupid, value, rng);
    }
};

inline void ProductModel::Mixture::add_value (
        const ProductModel & model,
        size_t groupid,
        const Value & value,
        rng_t & rng)
{
    bool add_group = clustering.add_value(model.clustering, groupid);
    add_value_fun fun = {groupid, rng};
    apply_sparse(model, fun, value);

    if (LOOM_UNLIKELY(add_group)) {
        add_group_fun fun = {rng};
        apply_dense(model, fun);
        id_tracker.add_group();
        _validate(model);
    }
}

struct ProductModel::Mixture::remove_group_fun
{
    const size_t groupid;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model & model,
            Mixture & mixture)
    {
        mixture.remove_group(model, groupid);
    }
};

struct ProductModel::Mixture::remove_value_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.remove_value(model, groupid, value, rng);
    }
};

inline void ProductModel::Mixture::remove_value (
        const ProductModel & model,
        size_t groupid,
        const Value & value,
        rng_t & rng)
{
    bool remove_group = clustering.remove_value(model.clustering, groupid);
    remove_value_fun fun = {groupid, rng};
    apply_sparse(model, fun, value);

    if (LOOM_UNLIKELY(remove_group)) {
        remove_group_fun fun = {groupid};
        apply_dense(model, fun);
        id_tracker.remove_group(groupid);
        _validate(model);
    }
}

struct ProductModel::Mixture::score_fun
{
    VectorFloat & scores;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        const Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.score_value(model, value, scores, rng);
    }
};

inline void ProductModel::Mixture::score (
        const ProductModel & model,
        const Value & value,
        VectorFloat & scores,
        rng_t & rng)
{
    scores.resize(clustering.counts().size());
    clustering.score(model.clustering, scores);
    score_fun fun = {scores, rng};
    apply_sparse(model, fun, value);
}

struct ProductModel::Mixture::sample_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    typename Mixture::Value operator() (
        const typename Mixture::Model & model,
        const Mixture & mixture)
    {
        return model.sample_value(mixture.groups[groupid], rng);
    }
};

inline void ProductModel::Mixture::sample_value (
        const ProductModel & model,
        const VectorFloat & probs,
        Value & value,
        rng_t & rng)
{
    size_t groupid = distributions::sample_from_probs(rng, probs);
    sample_fun fun = {groupid, rng};
    set_sparse(model, fun, value);
}

} // namespace loom
<commit_msg>Call sample_value from distributions namespace.<commit_after>#pragma once

#include <vector>
#include <distributions/clustering.hpp>
#include <distributions/models/dd.hpp>
#include <distributions/models/dpd.hpp>
#include <distributions/models/nich.hpp>
#include <distributions/models/gp.hpp>
#include <distributions/mixture.hpp>
#include <distributions/io/protobuf.hpp>
#include "common.hpp"
#include "protobuf.hpp"

namespace loom
{

using distributions::rng_t;
using distributions::VectorFloat;

enum { DD_DIM = 256 };

struct ProductModel
{
    typedef protobuf::ProductModel::SparseValue Value;
    typedef distributions::Clustering<int>::PitmanYor Clustering;
    struct Mixture;

    protobuf::SparseValueSchema schema;
    Clustering clustering;
    std::vector<distributions::dirichlet_discrete::Model<DD_DIM>> dd;
    std::vector<distributions::dirichlet_process_discrete::Model> dpd;
    std::vector<distributions::gamma_poisson::Model> gp;
    std::vector<distributions::normal_inverse_chi_sq::Model> nich;

    void load (const protobuf::ProductModel & message);
};

struct ProductModel::Mixture
{
    ProductModel::Clustering::Mixture clustering;
    std::vector<distributions::dirichlet_discrete::Mixture<DD_DIM>> dd;
    std::vector<distributions::dirichlet_process_discrete::Mixture> dpd;
    std::vector<distributions::gamma_poisson::Mixture> gp;
    std::vector<distributions::normal_inverse_chi_sq::Mixture> nich;
    distributions::MixtureIdTracker id_tracker;

    void init_empty (
            const ProductModel & model,
            rng_t & rng,
            size_t empty_group_count = 1);
    void load (
            const ProductModel & model,
            const char * filename,
            rng_t & rng,
            size_t empty_roup_count = 1);
    void dump (const ProductModel & model, const char * filename);
    void add_value (
            const ProductModel & model,
            size_t groupid,
            const Value & value,
            rng_t & rng);
    void remove_value (
            const ProductModel & model,
            size_t groupid,
            const Value & value,
            rng_t & rng);
    void score (
            const ProductModel & model,
            const Value & value,
            VectorFloat & scores,
            rng_t & rng);
    void sample_value (
            const ProductModel & model,
            const VectorFloat & probs,
            Value & value,
            rng_t & rng);

private:

    void _validate (const ProductModel & model);

    template<class Mixture>
    void init_empty_factors (
            size_t empty_group_count,
            const std::vector<typename Mixture::Model> & models,
            std::vector<Mixture> & mixtures,
            rng_t & rng);

    template<class Fun>
    void apply_dense (const ProductModel & model, Fun & fun);

    template<class Fun>
    void apply_sparse (
            const ProductModel & model,
            Fun & fun,
            const Value & value);

    template<class Fun>
    void set_sparse (
            const ProductModel & model,
            Fun & fun,
            Value & value);

    struct validate_fun;
    struct load_group_fun;
    struct init_fun;
    struct dump_group_fun;
    struct add_group_fun;
    struct add_value_fun;
    struct remove_group_fun;
    struct remove_value_fun;
    struct score_fun;
    struct sample_fun;
};

template<class Fun>
inline void ProductModel::Mixture::apply_dense (
        const ProductModel & model,
        Fun & fun)
{
    //TODO("implement bb");
    for (size_t i = 0; i < dd.size(); ++i) {
        fun(i, model.dd[i], dd[i]);
    }
    for (size_t i = 0; i < dpd.size(); ++i) {
        fun(i, model.dpd[i], dpd[i]);
    }
    for (size_t i = 0; i < gp.size(); ++i) {
        fun(i, model.gp[i], gp[i]);
    }
    for (size_t i = 0; i < nich.size(); ++i) {
        fun(i, model.nich[i], nich[i]);
    }
}

template<class Fun>
inline void ProductModel::Mixture::apply_sparse (
        const ProductModel & model,
        Fun & fun,
        const Value & value)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        model.schema.validate(value);
    }

    size_t absolute_pos = 0;

    if (value.booleans_size()) {
        TODO("implement bb");
    } else {
        absolute_pos += 0;
    }

    if (value.counts_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < dd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.dd[i], dd[i], value.counts(packed_pos++));
            }
        }
        for (size_t i = 0; i < dpd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.dpd[i], dpd[i], value.counts(packed_pos++));
            }
        }
        for (size_t i = 0; i < gp.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.gp[i], gp[i], value.counts(packed_pos++));
            }
        }
    } else {
        absolute_pos += dd.size() + dpd.size() + gp.size();
    }

    if (value.reals_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < nich.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                fun(model.nich[i], nich[i], value.reals(packed_pos++));
            }
        }
    }
}

template<class Fun>
inline void ProductModel::Mixture::set_sparse (
        const ProductModel & model,
        Fun & fun,
        Value & value)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        model.schema.validate(value);
    }

    size_t absolute_pos = 0;

    if (value.booleans_size()) {
        TODO("implement bb");
    } else {
        absolute_pos += 0;
    }

    if (value.counts_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < dd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.dd[i], dd[i]));
            }
        }
        for (size_t i = 0; i < dpd.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.dpd[i], dpd[i]));
            }
        }
        for (size_t i = 0; i < gp.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_counts(packed_pos++, fun(model.gp[i], gp[i]));
            }
        }
    } else {
        absolute_pos += dd.size() + dpd.size() + gp.size();
    }

    if (value.reals_size()) {
        size_t packed_pos = 0;
        for (size_t i = 0; i < nich.size(); ++i) {
            if (value.observed(absolute_pos++)) {
                value.set_reals(packed_pos++, fun(model.nich[i], nich[i]));
            }
        }
    }
}

struct ProductModel::Mixture::validate_fun
{
    const size_t group_count;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model &,
            const Mixture & mixture)
    {
        LOOM_ASSERT_EQ(mixture.groups.size(), group_count);
    }
};

inline void ProductModel::Mixture::_validate (
        const ProductModel & model)
{
    if (LOOM_DEBUG_LEVEL >= 2) {
        const size_t group_count = clustering.counts().size();
        validate_fun fun = {group_count};
        apply_dense(model, fun);
        LOOM_ASSERT_EQ(id_tracker.packed_size(), group_count);
    }
}

struct ProductModel::Mixture::add_group_fun
{
    rng_t & rng;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model & model,
            Mixture & mixture)
    {
        mixture.add_group(model, rng);
    }
};

struct ProductModel::Mixture::add_value_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.add_value(model, groupid, value, rng);
    }
};

inline void ProductModel::Mixture::add_value (
        const ProductModel & model,
        size_t groupid,
        const Value & value,
        rng_t & rng)
{
    bool add_group = clustering.add_value(model.clustering, groupid);
    add_value_fun fun = {groupid, rng};
    apply_sparse(model, fun, value);

    if (LOOM_UNLIKELY(add_group)) {
        add_group_fun fun = {rng};
        apply_dense(model, fun);
        id_tracker.add_group();
        _validate(model);
    }
}

struct ProductModel::Mixture::remove_group_fun
{
    const size_t groupid;

    template<class Mixture>
    void operator() (
            size_t,
            const typename Mixture::Model & model,
            Mixture & mixture)
    {
        mixture.remove_group(model, groupid);
    }
};

struct ProductModel::Mixture::remove_value_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.remove_value(model, groupid, value, rng);
    }
};

inline void ProductModel::Mixture::remove_value (
        const ProductModel & model,
        size_t groupid,
        const Value & value,
        rng_t & rng)
{
    bool remove_group = clustering.remove_value(model.clustering, groupid);
    remove_value_fun fun = {groupid, rng};
    apply_sparse(model, fun, value);

    if (LOOM_UNLIKELY(remove_group)) {
        remove_group_fun fun = {groupid};
        apply_dense(model, fun);
        id_tracker.remove_group(groupid);
        _validate(model);
    }
}

struct ProductModel::Mixture::score_fun
{
    VectorFloat & scores;
    rng_t & rng;

    template<class Mixture>
    void operator() (
        const typename Mixture::Model & model,
        const Mixture & mixture,
        const typename Mixture::Value & value)
    {
        mixture.score_value(model, value, scores, rng);
    }
};

inline void ProductModel::Mixture::score (
        const ProductModel & model,
        const Value & value,
        VectorFloat & scores,
        rng_t & rng)
{
    scores.resize(clustering.counts().size());
    clustering.score(model.clustering, scores);
    score_fun fun = {scores, rng};
    apply_sparse(model, fun, value);
}

struct ProductModel::Mixture::sample_fun
{
    const size_t groupid;
    rng_t & rng;

    template<class Mixture>
    typename Mixture::Value operator() (
        const typename Mixture::Model & model,
        const Mixture & mixture)
    {
        return distributions::sample_value(model, mixture.groups[groupid], rng);
    }
};

inline void ProductModel::Mixture::sample_value (
        const ProductModel & model,
        const VectorFloat & probs,
        Value & value,
        rng_t & rng)
{
    size_t groupid = distributions::sample_from_probs(rng, probs);
    sample_fun fun = {groupid, rng};
    set_sparse(model, fun, value);
}

} // namespace loom
<|endoftext|>


<commit_before>//Copyright (c) 2020 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include <algorithm>

#include "ExtrusionLine.h"
#include "linearAlg2D.h"

namespace cura
{

ExtrusionLine::ExtrusionLine(const size_t inset_idx, const bool is_odd)
: inset_idx(inset_idx)
, is_odd(is_odd)
, is_closed(false)
{}

coord_t ExtrusionLine::getLength() const
{
    if (junctions.empty())
    {
        return 0;
    }
    coord_t len = 0;
    ExtrusionJunction prev = junctions.front();
    for (const ExtrusionJunction& next : junctions)
    {
        len += vSize(next.p - prev.p);
        prev = next;
    }
    if (is_closed)
    {
        len += vSize(front().p - back().p);
    }
    return len;
}

coord_t ExtrusionLine::getMinimalWidth() const
{
    return std::min_element(junctions.cbegin(), junctions.cend(),
                            [](const ExtrusionJunction& l, const ExtrusionJunction& r)
                            {
                                return l.w < r.w;
                            })->w;
}

void ExtrusionLine::simplify(const coord_t smallest_line_segment_squared, const coord_t allowed_error_distance_squared, const coord_t maximum_extrusion_area_deviation)
{
    const size_t min_path_size = is_closed ? 3 : 2;
    if (junctions.size() <= min_path_size)
    {
        return;
    }

    // TODO: allow for the first point to be removed in case of simplifying closed Extrusionlines.

    /* ExtrusionLines are treated as (open) polylines, so in case an ExtrusionLine is actually a closed polygon, its
     * starting and ending points will be equal (or almost equal). Therefore, the simplification of the ExtrusionLine
     * should not touch the first and last points. As a result, start simplifying from point at index 1.
     * */
    std::vector<ExtrusionJunction> new_junctions;
    // Starting junction should always exist in the simplified path
    new_junctions.emplace_back(junctions.front());

    /* Initially, previous_previous is always the same as previous because, for open ExtrusionLines the last junction
     * cannot be taken into consideration when checking the points at index 1. For closed ExtrusionLines, the first and
     * last junctions are anyway the same.
     * */
    ExtrusionJunction previous_previous = junctions.front();
    ExtrusionJunction previous = junctions.front();

    /* When removing a vertex, we check the height of the triangle of the area
     being removed from the original polygon by the simplification. However,
     when consecutively removing multiple vertices the height of the previously
     removed vertices w.r.t. the shortcut path changes.
     In order to not recompute the new height value of previously removed
     vertices we compute the height of a representative triangle, which covers
     the same amount of area as the area being cut off. We use the Shoelace
     formula to accumulate the area under the removed segments. This works by
     computing the area in a 'fan' where each of the blades of the fan go from
     the origin to one of the segments. While removing vertices the area in
     this fan accumulates. By subtracting the area of the blade connected to
     the short-cutting segment we obtain the total area of the cutoff region.
     From this area we compute the height of the representative triangle using
     the standard formula for a triangle area: A = .5*b*h
     */
    const ExtrusionJunction& initial = junctions.at(1);
    coord_t accumulated_area_removed = previous.p.X * initial.p.Y - previous.p.Y * initial.p.X; // Twice the Shoelace formula for area of polygon per line segment.

    for (size_t point_idx = 1; point_idx < junctions.size() - 1; point_idx++)
    {
        const ExtrusionJunction& current = junctions[point_idx];

        // Spill over in case of overflow, unless the [next] vertex will then be equal to [previous].
        const bool spill_over = point_idx + 1 == junctions.size() && new_junctions.size() > 1;
        ExtrusionJunction& next = spill_over ? new_junctions[0] : junctions[point_idx + 1];

        const coord_t removed_area_next = current.p.X * next.p.Y - current.p.Y * next.p.X; // Twice the Shoelace formula for area of polygon per line segment.
        const coord_t negative_area_closing = next.p.X * previous.p.Y - next.p.Y * previous.p.X; // Area between the origin and the short-cutting segment
        accumulated_area_removed += removed_area_next;

        const coord_t length2 = vSize2(current - previous);
        if (length2 < 25)
        {
            // We're allowed to always delete segments of less than 5 micron. The width in this case doesn't matter that much.
            continue;
        }

        const coord_t area_removed_so_far = accumulated_area_removed + negative_area_closing; // Close the shortcut area polygon
        const coord_t base_length_2 = vSize2(next - previous);

        if (base_length_2 == 0) // Two line segments form a line back and forth with no area.
        {
            continue; // Remove the junction (vertex).
        }
        //We want to check if the height of the triangle formed by previous, current and next vertices is less than allowed_error_distance_squared.
        //1/2 L = A           [actual area is half of the computed shoelace value] // Shoelace formula is .5*(...) , but we simplify the computation and take out the .5
        //A = 1/2 * b * h     [triangle area formula]
        //L = b * h           [apply above two and take out the 1/2]
        //h = L / b           [divide by b]
        //h^2 = (L / b)^2     [square it]
        //h^2 = L^2 / b^2     [factor the divisor]
        const coord_t height_2 = area_removed_so_far * area_removed_so_far / base_length_2;
        coord_t weighted_average_width;
        const coord_t extrusion_area_error = calculateExtrusionAreaDeviationError(previous, current, next, weighted_average_width);
        if ((height_2 <= 1 //Almost exactly colinear (barring rounding errors).
             && LinearAlg2D::getDistFromLine(current.p, previous.p, next.p) <= 1) // Make sure that height_2 is not small because of cancellation of positive and negative areas
            // We shouldn't remove middle junctions of colinear segments if the area changed for the C-P segment is exceeding the maximum allowed
             && extrusion_area_error <= maximum_extrusion_area_deviation)
        {
            // Adjust the width of the entire P-N line as a weighted average of the widths of the P-C and C-N lines and
            // then remove the current junction (vertex).
            next.w = weighted_average_width;
            continue;
        }

        if (length2 < smallest_line_segment_squared
            && height_2 <= allowed_error_distance_squared) // Removing the junction (vertex) doesn't introduce too much error.
        {
            const coord_t next_length2 = vSize2(current - next);
            if (next_length2 > smallest_line_segment_squared)
            {
                // Special case; The next line is long. If we were to remove this, it could happen that we get quite noticeable artifacts.
                // We should instead move this point to a location where both edges are kept and then remove the previous point that we wanted to keep.
                // By taking the intersection of these two lines, we get a point that preserves the direction (so it makes the corner a bit more pointy).
                // We just need to be sure that the intersection point does not introduce an artifact itself.
                Point intersection_point;
                bool has_intersection = LinearAlg2D::lineLineIntersection(previous_previous.p, previous.p, current.p, next.p, intersection_point);
                if (!has_intersection
                    || LinearAlg2D::getDist2FromLine(intersection_point, previous.p, current.p) > allowed_error_distance_squared
                    || vSize2(intersection_point - previous.p) > smallest_line_segment_squared  // The intersection point is way too far from the 'previous'
                    || vSize2(intersection_point - next.p) > smallest_line_segment_squared)     // and 'next' points, so it shouldn't replace 'current'
                {
                    // We can't find a better spot for it, but the size of the line is more than 5 micron.
                    // So the only thing we can do here is leave it in...
                }
                else
                {
                    // New point seems like a valid one.
                    const ExtrusionJunction new_to_add = ExtrusionJunction(intersection_point, current.w, current.perimeter_index);
                    // If there was a previous point added, remove it.
                    if(!new_junctions.empty())
                    {
                        new_junctions.pop_back();
                        previous = previous_previous;
                    }

                    // The junction (vertex) is replaced by the new one.
                    accumulated_area_removed = removed_area_next; // So that in the next iteration it's the area between the origin, [previous] and [current]
                    previous_previous = previous;
                    previous = new_to_add; // Note that "previous" is only updated if we don't remove the junction (vertex).
                    new_junctions.push_back(new_to_add);
                    continue;
                }
            }
            else
            {
                continue; // Remove the junction (vertex).
            }
        }
        // The junction (vertex) isn't removed.
        accumulated_area_removed = removed_area_next; // So that in the next iteration it's the area between the origin, [previous] and [current]
        previous_previous = previous;
        previous = current; // Note that "previous" is only updated if we don't remove the junction (vertex).
        new_junctions.push_back(current);
    }

    // Ending junction (vertex) should always exist in the simplified path
    new_junctions.emplace_back(junctions.back());

    /* In case this is a closed polygon (instead of a poly-line-segments), the invariant that the first and last points are the same should be enforced.
     * Since one of them didn't move, and the other can't have been moved further than the constraints, if originally equal, they can simply be equated.
     */
    if (vSize2(junctions.front().p - junctions.back().p) == 0)
    {
        new_junctions.back().p = junctions.front().p;
    }

    junctions = new_junctions;
}

coord_t ExtrusionLine::calculateExtrusionAreaDeviationError(ExtrusionJunction A, ExtrusionJunction B, ExtrusionJunction C, coord_t& weighted_average_width)
{
    /*
     * A             B                          C              A                                        C
     * ---------------                                         **************
     * |             |                                         ------------------------------------------
     * |             |--------------------------|  B removed   |            |***************************|
     * |             |                          |  --------->  |            |                           |
     * |             |--------------------------|              |            |***************************|
     * |             |                                         ------------------------------------------
     * ---------------             ^                           **************
     *       ^                    C.w                                             ^
     *      B.w                                                     new_width = weighted_average_width
     *
     *
     * ******** denote the total extrusion area deviation error in the consecutive segments as a result of using the
     * weighted-average width for the entire extrusion line.
     *
     * */
    const coord_t ab_length = vSize(B - A);
    const coord_t bc_length = vSize(C - B);
    const coord_t width_diff = llabs(B.w - A.w);
    if (width_diff > 1)
    {
        // Adjust the width only if there is a difference, or else the rounding errors may produce the wrong
        // weighted average value.
        weighted_average_width = (ab_length * A.w + bc_length * B.w) / vSize(C - A);
        return llabs(A.w - weighted_average_width) * ab_length + llabs(B.w - weighted_average_width) * bc_length;
    }
    else
    {
        // If the width difference is very small, then select the width of the segment that is longer
        weighted_average_width = ab_length > bc_length ? A.w : B.w;
        return ab_length > bc_length ? width_diff * bc_length : width_diff * ab_length;
    }
}

}
<commit_msg>Replace 'llabs' with more standard 'std::abs'.<commit_after>//Copyright (c) 2020 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include <algorithm>

#include "ExtrusionLine.h"
#include "linearAlg2D.h"

namespace cura
{

ExtrusionLine::ExtrusionLine(const size_t inset_idx, const bool is_odd)
: inset_idx(inset_idx)
, is_odd(is_odd)
, is_closed(false)
{}

coord_t ExtrusionLine::getLength() const
{
    if (junctions.empty())
    {
        return 0;
    }
    coord_t len = 0;
    ExtrusionJunction prev = junctions.front();
    for (const ExtrusionJunction& next : junctions)
    {
        len += vSize(next.p - prev.p);
        prev = next;
    }
    if (is_closed)
    {
        len += vSize(front().p - back().p);
    }
    return len;
}

coord_t ExtrusionLine::getMinimalWidth() const
{
    return std::min_element(junctions.cbegin(), junctions.cend(),
                            [](const ExtrusionJunction& l, const ExtrusionJunction& r)
                            {
                                return l.w < r.w;
                            })->w;
}

void ExtrusionLine::simplify(const coord_t smallest_line_segment_squared, const coord_t allowed_error_distance_squared, const coord_t maximum_extrusion_area_deviation)
{
    const size_t min_path_size = is_closed ? 3 : 2;
    if (junctions.size() <= min_path_size)
    {
        return;
    }

    // TODO: allow for the first point to be removed in case of simplifying closed Extrusionlines.

    /* ExtrusionLines are treated as (open) polylines, so in case an ExtrusionLine is actually a closed polygon, its
     * starting and ending points will be equal (or almost equal). Therefore, the simplification of the ExtrusionLine
     * should not touch the first and last points. As a result, start simplifying from point at index 1.
     * */
    std::vector<ExtrusionJunction> new_junctions;
    // Starting junction should always exist in the simplified path
    new_junctions.emplace_back(junctions.front());

    /* Initially, previous_previous is always the same as previous because, for open ExtrusionLines the last junction
     * cannot be taken into consideration when checking the points at index 1. For closed ExtrusionLines, the first and
     * last junctions are anyway the same.
     * */
    ExtrusionJunction previous_previous = junctions.front();
    ExtrusionJunction previous = junctions.front();

    /* When removing a vertex, we check the height of the triangle of the area
     being removed from the original polygon by the simplification. However,
     when consecutively removing multiple vertices the height of the previously
     removed vertices w.r.t. the shortcut path changes.
     In order to not recompute the new height value of previously removed
     vertices we compute the height of a representative triangle, which covers
     the same amount of area as the area being cut off. We use the Shoelace
     formula to accumulate the area under the removed segments. This works by
     computing the area in a 'fan' where each of the blades of the fan go from
     the origin to one of the segments. While removing vertices the area in
     this fan accumulates. By subtracting the area of the blade connected to
     the short-cutting segment we obtain the total area of the cutoff region.
     From this area we compute the height of the representative triangle using
     the standard formula for a triangle area: A = .5*b*h
     */
    const ExtrusionJunction& initial = junctions.at(1);
    coord_t accumulated_area_removed = previous.p.X * initial.p.Y - previous.p.Y * initial.p.X; // Twice the Shoelace formula for area of polygon per line segment.

    for (size_t point_idx = 1; point_idx < junctions.size() - 1; point_idx++)
    {
        const ExtrusionJunction& current = junctions[point_idx];

        // Spill over in case of overflow, unless the [next] vertex will then be equal to [previous].
        const bool spill_over = point_idx + 1 == junctions.size() && new_junctions.size() > 1;
        ExtrusionJunction& next = spill_over ? new_junctions[0] : junctions[point_idx + 1];

        const coord_t removed_area_next = current.p.X * next.p.Y - current.p.Y * next.p.X; // Twice the Shoelace formula for area of polygon per line segment.
        const coord_t negative_area_closing = next.p.X * previous.p.Y - next.p.Y * previous.p.X; // Area between the origin and the short-cutting segment
        accumulated_area_removed += removed_area_next;

        const coord_t length2 = vSize2(current - previous);
        if (length2 < 25)
        {
            // We're allowed to always delete segments of less than 5 micron. The width in this case doesn't matter that much.
            continue;
        }

        const coord_t area_removed_so_far = accumulated_area_removed + negative_area_closing; // Close the shortcut area polygon
        const coord_t base_length_2 = vSize2(next - previous);

        if (base_length_2 == 0) // Two line segments form a line back and forth with no area.
        {
            continue; // Remove the junction (vertex).
        }
        //We want to check if the height of the triangle formed by previous, current and next vertices is less than allowed_error_distance_squared.
        //1/2 L = A           [actual area is half of the computed shoelace value] // Shoelace formula is .5*(...) , but we simplify the computation and take out the .5
        //A = 1/2 * b * h     [triangle area formula]
        //L = b * h           [apply above two and take out the 1/2]
        //h = L / b           [divide by b]
        //h^2 = (L / b)^2     [square it]
        //h^2 = L^2 / b^2     [factor the divisor]
        const coord_t height_2 = area_removed_so_far * area_removed_so_far / base_length_2;
        coord_t weighted_average_width;
        const coord_t extrusion_area_error = calculateExtrusionAreaDeviationError(previous, current, next, weighted_average_width);
        if ((height_2 <= 1 //Almost exactly colinear (barring rounding errors).
             && LinearAlg2D::getDistFromLine(current.p, previous.p, next.p) <= 1) // Make sure that height_2 is not small because of cancellation of positive and negative areas
            // We shouldn't remove middle junctions of colinear segments if the area changed for the C-P segment is exceeding the maximum allowed
             && extrusion_area_error <= maximum_extrusion_area_deviation)
        {
            // Adjust the width of the entire P-N line as a weighted average of the widths of the P-C and C-N lines and
            // then remove the current junction (vertex).
            next.w = weighted_average_width;
            continue;
        }

        if (length2 < smallest_line_segment_squared
            && height_2 <= allowed_error_distance_squared) // Removing the junction (vertex) doesn't introduce too much error.
        {
            const coord_t next_length2 = vSize2(current - next);
            if (next_length2 > smallest_line_segment_squared)
            {
                // Special case; The next line is long. If we were to remove this, it could happen that we get quite noticeable artifacts.
                // We should instead move this point to a location where both edges are kept and then remove the previous point that we wanted to keep.
                // By taking the intersection of these two lines, we get a point that preserves the direction (so it makes the corner a bit more pointy).
                // We just need to be sure that the intersection point does not introduce an artifact itself.
                Point intersection_point;
                bool has_intersection = LinearAlg2D::lineLineIntersection(previous_previous.p, previous.p, current.p, next.p, intersection_point);
                if (!has_intersection
                    || LinearAlg2D::getDist2FromLine(intersection_point, previous.p, current.p) > allowed_error_distance_squared
                    || vSize2(intersection_point - previous.p) > smallest_line_segment_squared  // The intersection point is way too far from the 'previous'
                    || vSize2(intersection_point - next.p) > smallest_line_segment_squared)     // and 'next' points, so it shouldn't replace 'current'
                {
                    // We can't find a better spot for it, but the size of the line is more than 5 micron.
                    // So the only thing we can do here is leave it in...
                }
                else
                {
                    // New point seems like a valid one.
                    const ExtrusionJunction new_to_add = ExtrusionJunction(intersection_point, current.w, current.perimeter_index);
                    // If there was a previous point added, remove it.
                    if(!new_junctions.empty())
                    {
                        new_junctions.pop_back();
                        previous = previous_previous;
                    }

                    // The junction (vertex) is replaced by the new one.
                    accumulated_area_removed = removed_area_next; // So that in the next iteration it's the area between the origin, [previous] and [current]
                    previous_previous = previous;
                    previous = new_to_add; // Note that "previous" is only updated if we don't remove the junction (vertex).
                    new_junctions.push_back(new_to_add);
                    continue;
                }
            }
            else
            {
                continue; // Remove the junction (vertex).
            }
        }
        // The junction (vertex) isn't removed.
        accumulated_area_removed = removed_area_next; // So that in the next iteration it's the area between the origin, [previous] and [current]
        previous_previous = previous;
        previous = current; // Note that "previous" is only updated if we don't remove the junction (vertex).
        new_junctions.push_back(current);
    }

    // Ending junction (vertex) should always exist in the simplified path
    new_junctions.emplace_back(junctions.back());

    /* In case this is a closed polygon (instead of a poly-line-segments), the invariant that the first and last points are the same should be enforced.
     * Since one of them didn't move, and the other can't have been moved further than the constraints, if originally equal, they can simply be equated.
     */
    if (vSize2(junctions.front().p - junctions.back().p) == 0)
    {
        new_junctions.back().p = junctions.front().p;
    }

    junctions = new_junctions;
}

coord_t ExtrusionLine::calculateExtrusionAreaDeviationError(ExtrusionJunction A, ExtrusionJunction B, ExtrusionJunction C, coord_t& weighted_average_width)
{
    /*
     * A             B                          C              A                                        C
     * ---------------                                         **************
     * |             |                                         ------------------------------------------
     * |             |--------------------------|  B removed   |            |***************************|
     * |             |                          |  --------->  |            |                           |
     * |             |--------------------------|              |            |***************************|
     * |             |                                         ------------------------------------------
     * ---------------             ^                           **************
     *       ^                    C.w                                             ^
     *      B.w                                                     new_width = weighted_average_width
     *
     *
     * ******** denote the total extrusion area deviation error in the consecutive segments as a result of using the
     * weighted-average width for the entire extrusion line.
     *
     * */
    const coord_t ab_length = vSize(B - A);
    const coord_t bc_length = vSize(C - B);
    const coord_t width_diff = std::abs(B.w - A.w);
    if (width_diff > 1)
    {
        // Adjust the width only if there is a difference, or else the rounding errors may produce the wrong
        // weighted average value.
        weighted_average_width = (ab_length * A.w + bc_length * B.w) / vSize(C - A);
        return std::abs(A.w - weighted_average_width) * ab_length + std::abs(B.w - weighted_average_width) * bc_length;
    }
    else
    {
        // If the width difference is very small, then select the width of the segment that is longer
        weighted_average_width = ab_length > bc_length ? A.w : B.w;
        return ab_length > bc_length ? width_diff * bc_length : width_diff * ab_length;
    }
}

}
<|endoftext|>