Commit 7271b853 authored by Valentin Platzgummer's avatar Valentin Platzgummer

circular survey optimized, some bugs though

parent a22db7f4
This diff is collapsed.
......@@ -29,7 +29,9 @@ DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += SNAKE_DEBUG
#DEFINES += SNAKE_DEBUG
DEFINES += SHOW_CIRCULAR_SURVEY_TIME
DEFINES += DEBUG_CIRCULAR_SURVEY
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
......@@ -411,6 +413,8 @@ FORMS += \
#
HEADERS += \
src/Wima/CircularSurvey.h \
src/Wima/Geometry/GenericCircle.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
......@@ -473,9 +477,7 @@ HEADERS += \
src/Wima/WimaPlanData.h \
src/Wima/Geometry/WimaJoinedArea.h \
src/Wima/Geometry/WimaJoinedAreaData.h \
src/Wima/CircularSurveyComplexItem.h \
src/Wima/Geometry/PlanimetryCalculus.h \
src/Wima/Geometry/Circle.h \
src/Wima/Geometry/PolygonCalculus.h \
src/Wima/OptimisationTools.h \
src/Wima/Geometry/GeoUtilities.h \
......@@ -500,6 +502,7 @@ HEADERS += \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h
SOURCES += \
src/Wima/CircularSurvey.cc \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
......@@ -548,9 +551,7 @@ SOURCES += \
src/Wima/Geometry/WimaMeasurementAreaData.cc \
src/Wima/Geometry/WimaJoinedArea.cc \
src/Wima/Geometry/WimaJoinedAreaData.cc \
src/Wima/CircularSurveyComplexItem.cc \
src/Wima/Geometry/PlanimetryCalculus.cc \
src/Wima/Geometry/Circle.cc \
src/Wima/OptimisationTools.cc \
src/Wima/Geometry/GeoUtilities.cc \
src/Wima/Geometry/PolygonCalculus.cc \
......
......@@ -31,7 +31,7 @@
#include "KML.h"
#include "QGCCorePlugin.h"
#include "src/Wima/CircularSurveyComplexItem.h"
#include "src/Wima/CircularSurvey.h"
#ifndef __mobile__
#include "MainWindow.h"
......@@ -544,7 +544,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
} else if (itemName == patternCorridorScanName) {
newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
} else if (itemName == _circularSurveyMissionItemName) {
newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
newItem = new CircularSurvey(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
} else {
qWarning() << "Internal error: Unknown complex item:" << itemName;
return sequenceNumber;
......@@ -560,7 +560,7 @@ int MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QS
if (itemName == _surveyMissionItemName) {
newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == _circularSurveyMissionItemName) {
newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
newItem = new CircularSurvey(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == patternStructureScanName) {
newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
} else if (itemName == patternCorridorScanName) {
......@@ -578,7 +578,7 @@ int MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* compl
int sequenceNumber = _nextSequenceNumber();
bool surveyStyleItem = qobject_cast<SurveyComplexItem*>(complexItem)
|| qobject_cast<CorridorScanComplexItem*>(complexItem)
|| qobject_cast<CircularSurveyComplexItem*>(complexItem);
|| qobject_cast<CircularSurvey*>(complexItem);
if (surveyStyleItem) {
bool rollSupported = false;
......@@ -633,7 +633,7 @@ void MissionController::removeMissionItem(int index)
bool removeSurveyStyle = _visualItems->value<SurveyComplexItem*>(index)
|| _visualItems->value<CorridorScanComplexItem*>(index)
|| _visualItems->value<CircularSurveyComplexItem*>(index);
|| _visualItems->value<CircularSurvey*>(index);
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
_deinitVisualItem(item);
......@@ -645,7 +645,7 @@ void MissionController::removeMissionItem(int index)
for (int i=1; i<_visualItems->count(); i++) {
if ( _visualItems->value<SurveyComplexItem*>(i)
|| _visualItems->value<CorridorScanComplexItem*>(i)
|| _visualItems->value<CircularSurveyComplexItem*>(i)) {
|| _visualItems->value<CircularSurvey*>(i)) {
foundSurvey = true;
break;
}
......@@ -905,9 +905,9 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec
nextSequenceNumber = corridorItem->lastSequenceNumber() + 1;
qCDebug(MissionControllerLog) << "Corridor Scan load complete: nextSequenceNumber" << nextSequenceNumber;
visualItems->append(corridorItem);
} else if (complexItemType == CircularSurveyComplexItem::jsonComplexItemTypeValue) {
} else if (complexItemType == CircularSurvey::CircularSurveyName) {
qCDebug(MissionControllerLog) << "Loading Circular Survey: nextSequenceNumber" << nextSequenceNumber;
CircularSurveyComplexItem* circularSurvey = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
CircularSurvey* circularSurvey = new CircularSurvey(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
if (!circularSurvey->load(itemObject, nextSequenceNumber++, errorString)) {
return false;
}
......
......@@ -26,7 +26,7 @@
#include "QmlObjectListModel.h"
class SurveyComplexItem;
class CircularSurveyComplexItem;
class CircularSurvey;
class SimpleMissionItem;
class MissionController;
#ifdef UNITTEST_BUILD
......@@ -153,7 +153,7 @@ private:
static const char* _jsonParam4Key;
friend class SurveyComplexItem;
friend class CircularSurveyComplexItem;
friend class CircularSurvey;
friend class SimpleMissionItem;
friend class MissionController;
#ifdef UNITTEST_BUILD
......
......@@ -137,11 +137,6 @@ Rectangle {
spacing: _margin
visible: transectsHeader.checked
FactCheckBox {
text: qsTr("Fixed Direction")
fact: missionItem.fixedDirection
}
QGCCheckBox {
id: relAlt
text: qsTr("Relative altitude")
......
This diff is collapsed.
#pragma once
#include <QFutureWatcher>
#include <QVector>
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
class CircularSurvey : public TransectStyleComplexItem {
Q_OBJECT
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile,
QObject *parent);
Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY
refPointChanged)
Q_PROPERTY(Fact *deltaR READ deltaR CONSTANT)
Q_PROPERTY(Fact *deltaAlpha READ deltaAlpha CONSTANT)
Q_PROPERTY(Fact *transectMinLength READ transectMinLength CONSTANT)
Q_PROPERTY(Fact *reverse READ reverse CONSTANT)
Q_PROPERTY(Fact *maxWaypoints READ maxWaypoints CONSTANT)
Q_PROPERTY(bool isInitialized READ isInitialized WRITE setIsInitialized NOTIFY
isInitializedChanged)
Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged)
Q_INVOKABLE void resetReference(void);
// Property setters
void setRefPoint(const QGeoCoordinate &refPt);
// Set this to true if survey was automatically generated, prevents
// initialisation from gui.
void setIsInitialized(bool isInitialized);
// Property getters
QGeoCoordinate refPoint() const;
Fact *deltaR();
Fact *deltaAlpha();
Fact *transectMinLength();
Fact *reverse();
Fact *maxWaypoints();
bool calculating();
// Is true if survey was automatically generated, prevents initialisation from
// gui.
bool isInitialized();
// Overrides from ComplexMissionItem
bool load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) final;
QString mapVisualQML(void) const final;
// Overrides from TransectStyleComplexItem
void save(QJsonArray &planItems) final;
bool specifiesCoordinate(void) const final;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) final;
void applyNewAltitude(double newAltitude) final;
double timeBetweenShots(void) final;
// Overrides from VisualMissionionItem
QString commandDescription(void) const final;
QString commandName(void) const final;
QString abbreviation(void) const final;
bool readyForSave(void) const final;
double additionalTimeDelay(void) const final;
static const char *settingsGroup;
static const char *deltaRName;
static const char *deltaAlphaName;
static const char *transectMinLengthName;
static const char *reverseName;
static const char *maxWaypointsName;
static const char *CircularSurveyName;
static const char *refPointLongitudeName;
static const char *refPointLatitudeName;
static const char *refPointAltitudeName;
signals:
void refPointChanged();
void isInitializedChanged();
void calculatingChanged();
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
void _deferUpdate();
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
// center of the circular lanes, e.g. base station
QGeoCoordinate _referencePoint;
QMap<QString, FactMetaData *> _metaDataMap;
// distance between two neighbour circles
SettingsFact _deltaR;
// angle discretisation of the circles
SettingsFact _deltaAlpha;
// minimal transect lenght, transects are rejected if they are shorter than
// this value
SettingsFact _minLength;
// reverses the _transects path
SettingsFact _reverse;
// the maximum number of waypoints _transects (TransectStyleComplexItem) can
// contain (to avoid performance hits)
SettingsFact _maxWaypoints;
// Timer to defer recalc
QTimer _timer;
// indicates if the polygon and refpoint etc. are initialized, prevents
// reinitialisation from gui and execution of _rebuildTransectsPhase1 during
// init from gui
bool _isInitialized;
using PtrTransects = std::shared_ptr<Transects>;
using Watcher = QFutureWatcher<PtrTransects>;
Watcher _watcher;
bool _calculating;
bool _cancle;
};
This diff is collapsed.
# pragma once
#include "TransectStyleComplexItem.h"
#include "Geometry/PolygonCalculus.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/GeoUtilities.h"
#include "QVector"
#include "Geometry/Circle.h"
#include "SettingsFact.h"
class CircularSurveyComplexItem :public TransectStyleComplexItem
{
Q_OBJECT
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default polygon
CircularSurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent);
Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged)
Q_PROPERTY(Fact* deltaR READ deltaR CONSTANT)
Q_PROPERTY(Fact* deltaAlpha READ deltaAlpha CONSTANT)
Q_PROPERTY(Fact* transectMinLength READ transectMinLength CONSTANT)
Q_PROPERTY(Fact* fixedDirection READ fixedDirection CONSTANT)
Q_PROPERTY(Fact* reverse READ reverse CONSTANT)
Q_PROPERTY(Fact* maxWaypoints READ maxWaypoints CONSTANT)
Q_PROPERTY(bool isInitialized READ isInitialized WRITE setIsInitialized NOTIFY isInitializedChanged)
Q_INVOKABLE void resetReference(void);
Q_INVOKABLE void comprehensiveUpdate(void); // triggers a slow recalculation of the transects
// Property setters
void setRefPoint(const QGeoCoordinate &refPt);
// Set this to true if survey was automatically generated, prevents initialisation from gui.
void setIsInitialized(bool isInitialized);
// Property getters
QGeoCoordinate refPoint() const;
Fact *deltaR();
Fact *deltaAlpha();
Fact *transectMinLength();
Fact *fixedDirection();
Fact *reverse();
Fact *maxWaypoints();
// Is true if survey was automatically generated, prevents initialisation from gui.
bool isInitialized();
bool referencePointBeingChanged(); // returns true if the referencepoint is being changed (dragged by user)
// Overrides from ComplexMissionItem
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
QString mapVisualQML (void) const final { return QStringLiteral("CircularSurveyMapVisual.qml"); }
// Overrides from TransectStyleComplexItem
void save (QJsonArray& planItems) final;
bool specifiesCoordinate (void) const final { return true; }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double timeBetweenShots (void) final;
// Overrides from VisualMissionionItem
QString commandDescription (void) const final { return tr("Circular Survey"); }
QString commandName (void) const final { return tr("Circular Survey"); }
QString abbreviation (void) const final { return tr("C.S."); }
bool readyForSave (void) const final;
double additionalTimeDelay (void) const final;
static const char* settingsGroup;
static const char* deltaRName;
static const char* deltaAlphaName;
static const char* transectMinLengthName;
static const char* fixedDirectionName;
static const char* reverseName;
static const char* maxWaypointsName;
static const char* jsonComplexItemTypeValue;
static const char* jsonDeltaRKey;
static const char* jsonDeltaAlphaKey;
static const char* jsonTransectMinLengthKey;
static const char* jsonfixedDirectionKey;
static const char* jsonReverseKey;
static const char* jsonReferencePointLongKey;
static const char* jsonReferencePointLatKey;
static const char* jsonReferencePointAltKey;
static const long triggerTime = 50; // trigger time (ms) for _triggerSlowRecalcTimer
signals:
void refPointChanged();
void isInitializedChanged();
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1 (void) final; // calls _rebuildTransectsFast or _rebuildTransectsSlow depending on _fastRecalc
void _rebuildTransectsFast (void);
void _rebuildTransectsSlow (void); // the slow version of _rebuildTransectsFast which properly connects the _transects
void _triggerSlowRecalc (void);
bool _generateTransectPath (QVector<QVector<QPointF>> &transectPath, const QPolygonF &surveyPolygon);
bool _rebuildTransectsInputCheck(QPolygonF &poly);
void _rebuildTransectsToGeo (const QVector<QPointF> &path, const QGeoCoordinate &reference);
void _recalcComplexDistance (void) final;
void _recalcCameraShots (void) final;
void _reverseTransects (void);
bool _shortestPath (const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &shortestPath);
signals:
private:
void _appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
QGeoCoordinate _referencePoint; // center of the circular lanes, e.g. base station
QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _deltaR; // distance between two neighbour circles
SettingsFact _deltaAlpha; // angle discretisation of the circles
SettingsFact _transectMinLength; // minimal transect lenght, transects are rejected if they are shorter than this value
SettingsFact _fixedDirection; // bool value, determining if transects have fixed direction or not
SettingsFact _reverse; // reverses the _transects path
SettingsFact _maxWaypoints; // the maximum number of waypoints _transects (TransectStyleComplexItem) can contain (to avoid performance hits)
QTimer _triggerSlowRecalcTimer;
bool _isInitialized; // indicates if the polygon and refpoint etc. are initialized, prevents reinitialisation from gui and execution of _rebuildTransectsPhase1 during init from gui
bool _reverseOnly; // if this is true _rebuildTransectsPhase1() will reverse the path only, _rebuildTransectsPhase1() resets _reverseOnly
bool _doFastRecalc; // fast recalc of transects if set, see _rebuildTransectsPhase1 for furhter explanation
};
#include "Circle.h"
Circle::Circle(QObject *parent)
: QObject (parent)
, _circleRadius(0)
, _circleOrigin(QPointF(0,0))
{
}
Circle::Circle(double radius, QObject *parent)
: QObject (parent)
, _circleRadius(radius >= 0 ? radius : 0)
, _circleOrigin(QPointF(0,0))
{
}
Circle::Circle(double radius, QPointF origin, QObject *parent)
: QObject (parent)
, _circleRadius(radius >= 0 ? radius : 0)
, _circleOrigin(origin)
{
}
Circle::Circle(const Circle &other, QObject *parent)
: QObject (parent)
{
*this = other;
}
Circle &Circle::operator=(const Circle &other)
{
this->setRadius(other.radius());
this->setOrigin(other.origin());
return *this;
}
/*!
* \fn void Circle::setRadius(double radius)
* Sets the radius of the circle to \a radius
*/
void Circle::setRadius(double radius)
{
if ( radius >= 0 && !qFuzzyCompare(_circleRadius, radius) ) {
_circleRadius = radius;
emit radiusChanged(_circleRadius);
}
}
/*!
* \fn void Circle::setOrigin(const QPointF &origin)
* Sets the origin of the circle to \a origin
*
* \sa QPointF
*/
void Circle::setOrigin(const QPointF &origin)
{
if (origin != _circleOrigin) {
_circleOrigin = origin;
emit originChanged(_circleOrigin);
}
}
/*!
* \fn double Circle::radius() const
* Returns the radius of the circle.
*/
double Circle::radius() const
{
return _circleRadius;
}
/*!
* \fn QPointF Circle::origin() const
* Returns the origin of the circle.
*
* \sa QPointF
*/
QPointF Circle::origin() const
{
return _circleOrigin;
}
/*!
* \fn QVector<QPointF> Circle::approximate(int numberOfCorners)
* Returns a polygon with \a numberOfCorners corners which approximates the circle.
*
* \sa QPointF
*/
QVector<QPointF> Circle::approximate(int numberOfCorners) const
{
if ( numberOfCorners < 3)
return QVector<QPointF>();
return approximateSektor(numberOfCorners+1, 0, 2*M_PI);
}
QVector<QPointF> Circle::approximate(double angleDiscretisation) const
{
return approximateSektor(angleDiscretisation, 0, 2*M_PI);
}
QVector<QPointF> Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const
{
if ( numberOfCorners < 2) {
qWarning("numberOfCorners < 2");
return QVector<QPointF>();
}
return approximateSektor(PlanimetryCalculus::truncateAngle(alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2);
}
QVector<QPointF> Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const
{
using namespace PlanimetryCalculus;
// check if input is valid
if ( qFuzzyCompare(alpha1, alpha2) )
return QVector<QPointF>();
alpha1 = truncateAngle(alpha1);
alpha2 = truncateAngle(alpha2);
double deltaAlpha = fabs(alpha1 - alpha2);
if (signum(angleDiscretisation*(alpha1 - alpha2)) == 1) {
deltaAlpha = 2*M_PI - deltaAlpha;
}
// check if input is valid
if ( qFuzzyIsNull(angleDiscretisation))
return QVector<QPointF>();
QVector<QPointF> sector;
double currentAngle = alpha1;
// how many nodes?
int j = floor(fabs(deltaAlpha/angleDiscretisation));
// rotate the vertex j+1 times add the origin and append to the sector.
do {
sector.append(toCoordinate(currentAngle));
currentAngle = truncateAngle(currentAngle + angleDiscretisation);
}while(j--);
// append last point if necessarry
QPointF vertex = toCoordinate(alpha2);
if ( !qFuzzyIsNull(PlanimetryCalculus::distance(sector.first(), vertex))
&& !qFuzzyIsNull(PlanimetryCalculus::distance(sector.last(), vertex )) ){
sector.append(vertex);
}
return sector;
}
/*!
* \fn void Circle::toCoordinate(QPointF &coordinate, double alpha) const
* Calculates the coordinates of a point on the circle with angle \a alpha.
* Stores the result in \a coordiante.
*
* \sa QPointF
*/
void Circle::toCoordinate(QPointF &coordinate, double alpha) const
{
using namespace PlanimetryCalculus;
coordinate = QPointF(_circleRadius,0);
rotateReference(coordinate, alpha);
coordinate += _circleOrigin;
}
/*!
* \overload QPointF Circle::toCoordinate(double alpha) const
* Returns the coordinates of a point on the circle with angle \a alpha.
*
* \sa QPointF
*/
QPointF Circle::toCoordinate(double alpha) const
{
QPointF coordinate;
toCoordinate(coordinate, alpha);
return coordinate;
}
bool Circle::isNull() const
{
return _circleRadius <= 0 ? true : false;
}
/*!
* \class Circle
* \brief Provies a circle with radius and origin.
*
* \sa QPointF
*/
#pragma once
#include <QObject>
#include <QPointF>
#include <QPolygonF>
#include <cmath>
#include "PlanimetryCalculus.h"
class Circle : public QObject
{
Q_OBJECT
public:
Circle(QObject *parent = nullptr);
Circle(double radius = 0, QObject *parent = nullptr);
Circle(double radius = 0, QPointF origin = QPointF(0,0), QObject *parent = nullptr);
Circle(const Circle &other, QObject *parent = nullptr);
Circle &operator=(const Circle &other);
// Property setters
void setRadius(double radius);
void setOrigin(const QPointF &origin);
// Property getters
double radius() const;
QPointF origin() const;
// Member methodes
QVector<QPointF> approximate (int numberOfCorners) const;
QVector<QPointF> approximate (double angleDiscretisation) const;
QVector<QPointF> approximateSektor(int numberOfCorners, double alpha1, double alpha2) const;
QVector<QPointF> approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const;
void toCoordinate (QPointF &toCoordinate, double alpha) const;
QPointF toCoordinate (double alpha) const;
bool isNull() const;
signals:
void radiusChanged(double radius);
void originChanged(QPointF origin);
private:
double _circleRadius;
QPointF _circleOrigin;
};
#pragma once
#include <QObject>
#include <QPointF>
#include <cmath>
#include "boost/units/systems/angle/degrees.hpp"
#include "boost/units/systems/si.hpp"
template <class Point, int k> auto get(Point &p);
namespace bu = boost::units;
namespace qty {
using Length = bu::quantity<bu::si::length>;
using Angle = bu::quantity<bu::si::plane_angle>;
using Degree = bu::quantity<bu::degree::plane_angle>;
using Radian = bu::quantity<bu::si::plane_angle>;
} // namespace qty
template <class NumberType, class Point> class GenericCircle {
public:
GenericCircle();
GenericCircle(NumberType rad, Point ori);
// Property setters
void setRadius(NumberType rad);
void setOrigin(const Point &ori);
// Property getters
NumberType radius() const;
const Point &origin() const;
Point &origin();
private:
NumberType _r;
Point _origin;
};
// Free functions.
template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
Container &c);
template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c);
// Impl.
template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle() : _r(0), _origin(0, 0) {}
template <class NumberType, class Point>
GenericCircle<NumberType, Point>::GenericCircle(NumberType rad, Point ori)
: _r(rad), _origin(ori) {}
/*!
* Returns a polygon with \a n corners which approximates the
* circle.
*
* \sa Point
*/
template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setRadius(NumberType rad) {
if (rad >= 0) {
this->_r = rad;
}
}
template <class NumberType, class Point>
void GenericCircle<NumberType, Point>::setOrigin(const Point &ori) {
this->_origin = ori;
}
template <class NumberType, class Point>
NumberType GenericCircle<NumberType, Point>::radius() const {
return this->_r;
}
template <class NumberType, class Point>
const Point &GenericCircle<NumberType, Point>::origin() const {
return this->_origin;
}
template <class NumberType, class Point>
Point &GenericCircle<NumberType, Point>::origin() {
return this->_origin;
}
template <class Circle, class Container>
void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
Container &c) {
auto clipp = [](double angle) {
while (angle < 0) {
angle += 2 * M_PI;
}
while (angle > 2 * M_PI) {
angle -= 2 * M_PI;
}
return angle;
};
double a1 = clipp(alpha1.value());
double a2 = clipp(alpha2.value());
double angleDisc = 0;
if (n > 0) {
angleDisc = (a2 - a1) / (n - 1);
} else {
angleDisc = (a2 - a1) / (n + 1);
n = n * (-1);
}
double a = a1;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) {
auto x = NumberType(x0 + r * std::cos(a));
auto y = NumberType(y0 + r * std::sin(a));
c.push_back(Point(x, y));
a += angleDisc;
}
}
template <class Circle, class Container>
void approximate(Circle &circ, long n, Container &c) {
using namespace bu;
double angleDisc = 0;
if (n > 0) {
angleDisc = 2 * M_PI / (n - 1);
} else {
angleDisc = 2 * M_PI / (n + 1);
n = n * (-1);
}
double a = 0;
auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin());
auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) {
auto x = NumberType(x0 + r * std::cos(a));
auto y = NumberType(y0 + r * std::sin(a));
c.push_back(Point(x, y));
a += angleDisc;
}
}
This diff is collapsed.
......@@ -4,97 +4,121 @@
#include <QPointF>
#include <QPolygonF>
#include <QtMath>
#include <QLineF>
#include "GenericCircle.h"
#include "PolygonCalculus.h"
class Circle;
using Circle = GenericCircle<qreal, QPointF>;
namespace PlanimetryCalculus {
enum IntersectType{InsideNoIntersection, InsideTouching, InsideIntersection,
OutsideIntersection, OutsideTouching, OutsideNoIntersection,
CirclesEqual, //Circle Circle intersection
Tangent, Secant, // Circle Line Intersetion
EdgeCornerIntersection, EdgeEdgeIntersection, CornerCornerIntersection,
LinesParallel, LinesEqual, // Line Line intersection
Interior, // Polygon contains
NoIntersection, Error // general
};
typedef QVector<QPair<int, int>> NeighbourVector;
typedef QVector<QPointF> QPointFVector;
typedef QVector<IntersectType> IntersectVector;
void rotateReference(QPointF &point, double alpha);
void rotateReference(QPointFVector &points, double alpha);
void rotateReference(QLineF &line, double alpha);
//void rotateReference(QPolygonF &polygon, double alpha);
QPointF rotateReturn(QPointF point, double alpha);
QPointFVector rotateReturn(QPointFVector points, double alpha);
QLineF rotateReturn(QLineF line, double alpha);
//QPolygonF rotateReturn(QPolygonF &polygon, double alpha);
bool intersects(const Circle &circle1, const Circle &circle2);
bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type);
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints);
bool intersects(const Circle &circle1, const Circle &circle2, QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line);
bool intersects(const Circle &circle, const QLineF &line, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints);
bool intersects(const Circle &circle, const QLineF &line, QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QPolygonF &polygon);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, IntersectVector &typeList);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList);
bool intersects(const Circle &circle, const QPolygonF &polygon, QVector<QPointFVector> &intersectionPoints, NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersects(const QLineF &line1, const QLineF &line2);
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt);
bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type);
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, IntersectVector &typeList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList);
bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFVector &intersectionList, NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersectsFast(const QPolygonF &polygon, const QLineF &line);
bool contains(const QLineF &line, const QPointF &point);
bool contains(const QLineF &line, const QPointF &point, IntersectType &type);
bool contains(const QPolygonF &polygon, const QPointF &point);
bool contains(const QPolygonF &polygon, const QPointF &point, IntersectType &type);
double distance(const QPointF &p1, const QPointF p2);
double norm(double x, double y);
double norm(const QPointF &p);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1);
double angle(const QLineF &line);
double angleDegree(const QPointF &p1, const QPointF p2);
double truncateAngle(double angle);
double truncateAngleDegree(double angle);
/*!
* \fntemplate <typename T> int signum(T val)
* Returns the signum of a value \a val.
*
* \sa QPair, QVector
*/
template <typename T> int signum(T val)
{
return (T(0) < val) - (val < T(0));
}
}
enum IntersectType {
InsideNoIntersection,
InsideTouching,
InsideIntersection,
OutsideIntersection,
OutsideTouching,
OutsideNoIntersection,
CirclesEqual, // Circle Circle intersection
Tangent,
Secant, // Circle Line Intersetion
EdgeCornerIntersection,
EdgeEdgeIntersection,
CornerCornerIntersection,
LinesParallel,
LinesEqual, // Line Line intersection
Interior, // Polygon contains
NoIntersection,
Error // general
};
typedef QVector<QPair<int, int>> NeighbourVector;
typedef QVector<QPointF> QPointFVector;
typedef QVector<IntersectType> IntersectVector;
void rotateReference(QPointF &point, double alpha);
void rotateReference(QPointFVector &points, double alpha);
void rotateReference(QLineF &line, double alpha);
// void rotateReference(QPolygonF &polygon, double alpha);
QPointF rotateReturn(QPointF point, double alpha);
QPointFVector rotateReturn(QPointFVector points, double alpha);
QLineF rotateReturn(QLineF line, double alpha);
// QPolygonF rotateReturn(QPolygonF &polygon, double alpha);
bool intersects(const Circle &circle1, const Circle &circle2);
bool intersects(const Circle &circle1, const Circle &circle2,
IntersectType &type);
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints);
bool intersects(const Circle &circle1, const Circle &circle2,
QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line);
bool intersects(const Circle &circle, const QLineF &line, IntersectType &type);
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints);
bool intersects(const Circle &circle, const QLineF &line,
QPointFVector &intersectionPoints, IntersectType &type);
bool intersects(const Circle &circle, const QPolygonF &polygon);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
IntersectVector &typeList);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList);
bool intersects(const Circle &circle, const QPolygonF &polygon,
QVector<QPointFVector> &intersectionPoints,
NeighbourVector &neighbourList, IntersectVector &typeList);
bool intersects(const QLineF &line1, const QLineF &line2);
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt);
bool intersects(const QLineF &line1, const QLineF &line2,
QPointF &intersectionPt, IntersectType &type);
// bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF
// &intersectionPt, IntersectType &type);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, IntersectVector &typeList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList,
NeighbourVector &neighbourList);
bool intersects(const QPolygonF &polygon, const QLineF &line,
QPointFVector &intersectionList, NeighbourVector &neighbourList,
IntersectVector &typeList);
bool intersectsFast(const QPolygonF &polygon, const QLineF &line);
bool contains(const QLineF &line, const QPointF &point);
bool contains(const QLineF &line, const QPointF &point, IntersectType &type);
bool contains(const QPolygonF &polygon, const QPointF &point);
bool contains(const QPolygonF &polygon, const QPointF &point,
IntersectType &type);
double distance(const QPointF &p1, const QPointF p2);
double norm(double x, double y);
double norm(const QPointF &p);
double angle(const QPointF &p1, const QPointF p2);
double angle(const QPointF &p1);
double angle(const QLineF &line);
double angleDegree(const QPointF &p1, const QPointF p2);
double truncateAngle(double angle);
double truncateAngleDegree(double angle);
/*!
* \fntemplate <typename T> int signum(T val)
* Returns the signum of a value \a val.
*
* \sa QPair, QVector
*/
template <typename T> int signum(T val) { return (T(0) < val) - (val < T(0)); }
} // namespace PlanimetryCalculus
......@@ -397,6 +397,8 @@ void SnakeThread::run() {
waypointsValid = false;
}
}
} else {
waypointsValid = false;
}
UniqueLock lk(this->pImpl->output.mutex);
......
......@@ -167,7 +167,7 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth,
using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = vector<BoostPoint>;
using Route = BoostLineString;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const BoostPolygon &mArea,
......
This diff is collapsed.
This diff is collapsed.
......@@ -113,10 +113,6 @@ Item {
borderColor: "black"
interiorColor: "green"
interiorOpacity: 0.5
onDragStop: {
_missionItem.comprehensiveUpdate()
}
}
// Transect lines
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment