diff options
Diffstat (limited to 'src/graphicsscene.cpp')
| -rw-r--r-- | src/graphicsscene.cpp | 232 |
1 files changed, 232 insertions, 0 deletions
diff --git a/src/graphicsscene.cpp b/src/graphicsscene.cpp new file mode 100644 index 0000000..52416d5 --- /dev/null +++ b/src/graphicsscene.cpp @@ -0,0 +1,232 @@ +#include "graphicsscene.h" + +// qt +#include <QGraphicsEllipseItem> +#include <QGraphicsLineItem> +#include <QGraphicsPolygonItem> + +namespace orphex +{ +namespace constants +{ +const QLineF laserPlane{QPointF{-1000, 0}, QPointF{1000, 0}}; +} +} // namespace orphex + +GraphicsScene::GraphicsScene(QObject* parent) + : QGraphicsScene{parent} +{ + setSceneRect(QRectF{QPointF{-1000, -1000}, QPointF{1000, 1000}}); + + m_lenseItem = addEllipse( + QRectF{QPointF{-1, -15}, QPointF{1, 15}}, + QPen{Qt::yellow, 0} + ); + m_lenseItem->setTransformOriginPoint(m_lenseItem->boundingRect().center()); + + m_sensorItem = new QGraphicsLineItem(QLineF{}, m_lenseItem); + m_sensorItem->setPen(QPen{Qt::red, 0}); + + m_lineOfActionItem = new QGraphicsLineItem{ + QLineF{QPointF{0, -50}, QPointF{0, 50}}, + m_lenseItem + }; + m_lineOfActionItem->setPen(QPen{Qt::gray, 0}); + + m_opticalAxisItem = new QGraphicsLineItem{ + QLineF{QPointF{-1000, 0}, QPointF{1000, 0}}, + m_lenseItem + }; + m_opticalAxisItem->setPen(QPen{Qt::gray, 0}); + + using namespace orphex::constants; + m_laserPlaneItem = addLine(laserPlane, QPen{Qt::black, 0}); + m_laserPlaneItem->stackBefore(m_lenseItem); + + m_reverseLaserPlaneItem = new QGraphicsLineItem(QLineF{}, m_lenseItem); + m_reverseLaserPlaneItem->setPen(QPen{Qt::magenta, 2}); + m_reverseLaserPlaneItem->setVisible(false); + + m_desiredLaserPlaneItem = new QGraphicsLineItem(QLineF{}, m_lenseItem); + m_desiredLaserPlaneItem->setPen(QPen{Qt::yellow, 0.1}); + m_desiredLaserPlaneItem->setOpacity(0.5); + + m_desiredImagePlaneItem = new QGraphicsLineItem(QLineF{}, m_lenseItem); + m_desiredImagePlaneItem->setPen(QPen{Qt::yellow, 0.15}); + m_desiredImagePlaneItem->stackBefore(m_sensorItem); + + m_desiredRangeAreaItem = new QGraphicsPolygonItem{m_lenseItem}; + m_desiredRangeAreaItem->setPen( + QPen{m_desiredLaserPlaneItem->pen().color(), 0} + ); + m_desiredRangeAreaItem->setBrush( + QBrush{m_desiredLaserPlaneItem->pen().color()} + ); + m_desiredRangeAreaItem->setOpacity(0.1); + + m_actualRangeItem = new QGraphicsLineItem(QLineF{}, m_lenseItem); + m_actualRangeItem->setPen(QPen{Qt::red, 0.1}); + m_actualRangeItem->setOpacity(0.5); + + m_actualRangeAreaItem = new QGraphicsPolygonItem{m_lenseItem}; + m_actualRangeAreaItem->setPen(QPen{m_sensorItem->pen().color(), 0}); + m_actualRangeAreaItem->setBrush(QBrush{m_sensorItem->pen().color()}); + m_actualRangeAreaItem->setOpacity(0.1); +} + +void GraphicsScene::update(OpticalDesign* design) +{ + if (!design) + { + qCritical() << Q_FUNC_INFO << "design is nullptr"; + return; + } + + using namespace orphex::constants; + + const auto F = design->get_focalDistanceMm(); + const auto H = design->get_lenseYPosMm(); + const auto w = design->get_sensorPixelsWidth(); + const auto h = design->get_sensorPixelsHeight(); + const auto wMm = design->get_sensorWidthMm(); + const auto hMm = design->get_sensorHeightMm(); + const auto oAngle = design->get_opticalAxisAngleDegrees(); + + m_lenseItem->setRotation(m_lenseItem->rotation()); + m_lenseItem->setRotation(-oAngle); + m_lenseItem->setPos(QPointF{0, -H}); + + const auto objectToImage = [F](const QPointF& objectPoint) -> QPointF { + // please note that object X is negative + const auto imagePointX = 1. / (1. / F - 1. / -objectPoint.x()); + const auto magnification = imagePointX / objectPoint.x(); + const auto imagePointY = magnification * objectPoint.y(); + + return QPointF{imagePointX, imagePointY}; + }; + + const auto imageToObject = [F](const QPointF& imagePoint) -> QPointF { + const auto objectPointX = 1. / (1. / imagePoint.x() - 1. / F); + const auto magnification = imagePoint.x() / objectPointX; + const auto objectPointY = imagePoint.y() / magnification; + + return QPointF{objectPointX, objectPointY}; + }; + + // calculate sensor plane + const auto& oai = m_opticalAxisItem; + const auto& oail = m_opticalAxisItem->line(); + const QLineF opticalAxisScene{ + oai->mapToScene(oail.p1()), + oai->mapToScene(oail.p2()) + }; + + QPointF intersection{}; + const auto type = + m_laserPlaneItem->line().intersects(opticalAxisScene, &intersection); + + if (type == QLineF::NoIntersection) + { + qCritical() << "no intersection between laser plane and optical axis"; + return; + } + + if (!qFuzzyCompare(intersection.y(), 0.)) + { + qCritical() << "laser plane/optical axis intersection point y != 0"; + return; + } + + // take 2 points - to the left and to the right from intersection point; + const auto leftP = intersection / 4; + // const auto rightP = intersection + leftP; + const auto rightP = intersection * 1.5; + + // create desired line on laser plane + m_desiredLaserPlaneItem->setLine( + {m_lenseItem->mapFromScene(QPointF{-design->get_zBaseMm(), 0}), + m_lenseItem->mapFromScene( + QPointF{-(design->get_zBaseMm() + design->get_zRangeMm()), 0} + )} + ); + + // calculate image plane + const auto debugL = m_desiredLaserPlaneItem->line(); + const QLineF debugImageL{ + objectToImage(debugL.p1()), + objectToImage(debugL.p2()) + }; + + m_desiredImagePlaneItem->setLine(debugImageL); + + // create sensor line on image plane + QPointF sensorCenterPos{}; + + if (m_opticalAxisItem->line().intersects( + m_desiredImagePlaneItem->line(), + &sensorCenterPos + ) == QLineF::NoIntersection) + { + qCritical() << "no intersection between sensor plane and optical axis"; + return; + } + + const auto sensorHalfHeight = design->get_sensorHeightMm() / 2.; + QLineF sensorLine{QPointF{0, 0}, QPointF{0, sensorHalfHeight}}; + sensorLine.translate(sensorCenterPos); + sensorLine.setAngle(m_desiredImagePlaneItem->line().angle()); + sensorLine.setP1(sensorCenterPos + (sensorCenterPos - sensorLine.p2())); + // at this momet sensor is centered and rotated. apply vertical offset + const auto offsetRatio = + design->get_sensorVerticalOffsetMm() / sensorLine.length(); + const auto offset = sensorLine.pointAt(1 + offsetRatio) - sensorLine.p2(); + sensorLine.setPoints(sensorLine.p1() + offset, sensorLine.p2() + offset); + + m_sensorItem->setLine(sensorLine); + + // find laser plane range which corresponds to sensor position + m_actualRangeItem->setLine({ + imageToObject(m_sensorItem->line().p1()), + imageToObject(m_sensorItem->line().p2()), + }); + + // not that object area has negative coords + design->set_actualZBaseMm( + -m_actualRangeItem->mapToScene(m_actualRangeItem->line().p1()).x() + ); + design->set_actualZRangeMm(m_actualRangeItem->line().length()); + + // convert debug image plane to laser plane just to verify they are the same + // TODO: check with code and log errors? + const QLineF reverseLaserPlaneLine{ + imageToObject(m_desiredImagePlaneItem->line().p1()), + imageToObject(m_desiredImagePlaneItem->line().p2()) + }; + m_reverseLaserPlaneItem->setLine(reverseLaserPlaneLine); + + // fill desired range area + m_desiredRangeAreaItem->setPolygon( + QPolygonF{ + {m_desiredImagePlaneItem->line().p1(), + m_desiredImagePlaneItem->line().p2(), + m_desiredLaserPlaneItem->line().p2(), + m_desiredLaserPlaneItem->line().p1()} + } + ); + + // fill actual range area + m_actualRangeAreaItem->setPolygon( + QPolygonF{ + {m_sensorItem->line().p1(), + m_sensorItem->line().p2(), + m_actualRangeItem->line().p2(), + m_actualRangeItem->line().p1()} + } + ); + + design->set_sensorLenseAngleDegrees( + m_desiredImagePlaneItem->line().angleTo(m_lineOfActionItem->line()) + ); + + // TODO: take back focal length into account +} |