Commit 6cee3dfc authored by Gus Grubba's avatar Gus Grubba

Compute bounding cube instead of "box"

parent ba55604f
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#define ComplexMissionItem_H #define ComplexMissionItem_H
#include "VisualMissionItem.h" #include "VisualMissionItem.h"
#include "QGCGeo.h"
class ComplexMissionItem : public VisualMissionItem class ComplexMissionItem : public VisualMissionItem
{ {
...@@ -22,15 +23,14 @@ public: ...@@ -22,15 +23,14 @@ public:
const ComplexMissionItem& operator=(const ComplexMissionItem& other); const ComplexMissionItem& operator=(const ComplexMissionItem& other);
Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged)
Q_PROPERTY(QRectF boundingBox READ boundingBox NOTIFY boundingBoxChanged)
/// @return The distance covered the complex mission item in meters. /// @return The distance covered the complex mission item in meters.
/// Signals complexDistanceChanged /// Signals complexDistanceChanged
virtual double complexDistance(void) const = 0; virtual double complexDistance(void) const = 0;
/// @return The item bounding box (QRectf(West, Nort, East, South)) /// @return The item bounding cube
/// Signals boundingBoxChanged /// Signals boundingCubeChanged
virtual QRectF boundingBox(void) const { return QRectF(); } virtual QGCGeoBoundingCube boundingCube(void) const { return QGCGeoBoundingCube(); }
/// @return Amount of additional time delay in seconds needed to fly the complex item /// @return Amount of additional time delay in seconds needed to fly the complex item
virtual double additionalTimeDelay(void) const { return 0; } virtual double additionalTimeDelay(void) const { return 0; }
...@@ -53,7 +53,7 @@ public: ...@@ -53,7 +53,7 @@ public:
signals: signals:
void complexDistanceChanged (void); void complexDistanceChanged (void);
void boundingBoxChanged (void); void boundingCubeChanged (void);
void greatestDistanceToChanged (void); void greatestDistanceToChanged (void);
void additionalTimeDelayChanged (void); void additionalTimeDelayChanged (void);
}; };
......
...@@ -440,7 +440,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate ...@@ -440,7 +440,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
//-- Keep track of bounding box changes in complex items //-- Keep track of bounding box changes in complex items
if(!newItem->isSimpleItem()) { if(!newItem->isSimpleItem()) {
connect(newItem, &ComplexMissionItem::boundingBoxChanged, this, &MissionController::_complexBoundingBoxChanged); connect(newItem, &ComplexMissionItem::boundingCubeChanged, this, &MissionController::_complexBoundingBoxChanged);
} }
newItem->setSequenceNumber(sequenceNumber); newItem->setSequenceNumber(sequenceNumber);
_initVisualItem(newItem); _initVisualItem(newItem);
...@@ -1997,11 +1997,13 @@ void MissionController::setCurrentPlanViewIndex(int sequenceNumber, bool force) ...@@ -1997,11 +1997,13 @@ void MissionController::setCurrentPlanViewIndex(int sequenceNumber, bool force)
void MissionController::_updateTimeout() void MissionController::_updateTimeout()
{ {
QRectF boundingBox; QGCGeoBoundingCube boundingCube;
double north = 0.0; double north = 0.0;
double south = 180.0; double south = 180.0;
double east = 0.0; double east = 0.0;
double west = 360.0; double west = 360.0;
double minAlt = QGCGeoBoundingCube::MaxAlt;
double maxAlt = QGCGeoBoundingCube::MinAlt;
for (int i = 1; i < _visualItems->count(); i++) { for (int i = 1; i < _visualItems->count(); i++) {
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i)); VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
if(item->isSimpleItem()) { if(item->isSimpleItem()) {
...@@ -2014,10 +2016,13 @@ void MissionController::_updateTimeout() ...@@ -2014,10 +2016,13 @@ void MissionController::_updateTimeout()
if(pSimpleItem->coordinate().isValid()) { if(pSimpleItem->coordinate().isValid()) {
double lat = pSimpleItem->coordinate().latitude() + 90.0; double lat = pSimpleItem->coordinate().latitude() + 90.0;
double lon = pSimpleItem->coordinate().longitude() + 180.0; double lon = pSimpleItem->coordinate().longitude() + 180.0;
double alt = pSimpleItem->coordinate().altitude();
north = fmax(north, lat); north = fmax(north, lat);
south = fmin(south, lat); south = fmin(south, lat);
east = fmax(east, lon); east = fmax(east, lon);
west = fmin(west, lon); west = fmin(west, lon);
minAlt = fmin(minAlt, alt);
maxAlt = fmax(maxAlt, alt);
} }
break; break;
default: default:
...@@ -2027,20 +2032,24 @@ void MissionController::_updateTimeout() ...@@ -2027,20 +2032,24 @@ void MissionController::_updateTimeout()
} else { } else {
ComplexMissionItem* pComplexItem = qobject_cast<ComplexMissionItem*>(item); ComplexMissionItem* pComplexItem = qobject_cast<ComplexMissionItem*>(item);
if(pComplexItem) { if(pComplexItem) {
QRectF bb = pComplexItem->boundingBox(); QGCGeoBoundingCube bc = pComplexItem->boundingCube();
if(!bb.isNull()) { if(bc.isValid()) {
north = fmax(north, bb.y() + 90.0); north = fmax(north, bc.pointNW.latitude() + 90.0);
south = fmin(south, bb.height() + 90.0); south = fmin(south, bc.pointSE.latitude() + 90.0);
east = fmax(east, bb.x() + 180.0); east = fmax(east, bc.pointNW.longitude() + 180.0);
west = fmin(west, bb.width() + 180.0); west = fmin(west, bc.pointSE.longitude() + 180.0);
minAlt = fmin(minAlt, bc.pointNW.altitude());
maxAlt = fmax(maxAlt, bc.pointSE.altitude());
} }
} }
} }
} }
boundingBox = QRectF(east - 180.0, north - 90.0, west - 180.0, south - 90.0); boundingCube = QGCGeoBoundingCube(
if(_boundingBox != boundingBox) { QGeoCoordinate(north - 90.0, west - 180.0, minAlt),
_boundingBox = boundingBox; QGeoCoordinate(south - 90.0, east - 180.0, maxAlt));
qCDebug(MissionControllerLog) << "Bounding box:" << _boundingBox.y() << _boundingBox.x() << _boundingBox.height() << _boundingBox.width(); if(_travelBoundingCube != boundingCube) {
_travelBoundingCube = boundingCube;
qCDebug(MissionControllerLog) << "Bounding cube:" << _travelBoundingCube.pointNW << _travelBoundingCube.pointSE;
} }
} }
......
...@@ -16,6 +16,7 @@ ...@@ -16,6 +16,7 @@
#include "Vehicle.h" #include "Vehicle.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include "MavlinkQmlSingleton.h" #include "MavlinkQmlSingleton.h"
#include "QGCGeo.h"
#include <QHash> #include <QHash>
...@@ -261,7 +262,7 @@ private: ...@@ -261,7 +262,7 @@ private:
int _currentPlanViewIndex; int _currentPlanViewIndex;
VisualMissionItem* _currentPlanViewItem; VisualMissionItem* _currentPlanViewItem;
QTimer _updateTimer; QTimer _updateTimer;
QRectF _boundingBox; QGCGeoBoundingCube _travelBoundingCube;
static const char* _settingsGroup; static const char* _settingsGroup;
......
...@@ -163,11 +163,11 @@ void SurveyMissionItem::_setSurveyDistance(double surveyDistance) ...@@ -163,11 +163,11 @@ void SurveyMissionItem::_setSurveyDistance(double surveyDistance)
} }
} }
void SurveyMissionItem::_setBoundingBox(QRectF bb) void SurveyMissionItem::_setBoundingCube(QGCGeoBoundingCube bc)
{ {
if (bb != _boundingBox) { if (bc != _boundingCube) {
_boundingBox = bb; _boundingCube = bc;
emit boundingBoxChanged(); emit boundingCubeChanged();
} }
} }
...@@ -668,6 +668,28 @@ int SurveyMissionItem::_calcMissionCommandCount(QList<QList<QGeoCoordinate>>& tr ...@@ -668,6 +668,28 @@ int SurveyMissionItem::_calcMissionCommandCount(QList<QList<QGeoCoordinate>>& tr
return missionCommandCount; return missionCommandCount;
} }
void SurveyMissionItem::_calcBoundingCube()
{
// Calc bounding cube
double north = 0.0;
double south = 180.0;
double east = 0.0;
double west = 360.0;
for (int i = 0; i < _simpleGridPoints.count(); i++) {
QGeoCoordinate coord = _simpleGridPoints[i].value<QGeoCoordinate>();
double lat = coord.latitude() + 90.0;
double lon = coord.longitude() + 180.0;
north = fmax(north, lat);
south = fmin(south, lat);
east = fmax(east, lon);
west = fmin(west, lon);
}
_setBoundingCube(QGCGeoBoundingCube(
QGeoCoordinate(north - 90.0, west - 180.0, _gridAltitudeFact.rawValue().toDouble()),
QGeoCoordinate(south - 90.0, east - 180.0, _gridAltitudeFact.rawValue().toDouble())));
}
void SurveyMissionItem::_generateGrid(void) void SurveyMissionItem::_generateGrid(void)
{ {
if (_ignoreRecalc) { if (_ignoreRecalc) {
...@@ -733,28 +755,14 @@ void SurveyMissionItem::_generateGrid(void) ...@@ -733,28 +755,14 @@ void SurveyMissionItem::_generateGrid(void)
// Calc survey distance // Calc survey distance
double surveyDistance = 0.0; double surveyDistance = 0.0;
for (int i=1; i<_simpleGridPoints.count(); i++) { for (int i = 1; i < _simpleGridPoints.count(); i++) {
QGeoCoordinate coord1 = _simpleGridPoints[i-1].value<QGeoCoordinate>(); QGeoCoordinate coord1 = _simpleGridPoints[i-1].value<QGeoCoordinate>();
QGeoCoordinate coord2 = _simpleGridPoints[i].value<QGeoCoordinate>(); QGeoCoordinate coord2 = _simpleGridPoints[i].value<QGeoCoordinate>();
surveyDistance += coord1.distanceTo(coord2); surveyDistance += coord1.distanceTo(coord2);
} }
_setSurveyDistance(surveyDistance); _setSurveyDistance(surveyDistance);
// Calc bounding cube
// Calc bounding box _calcBoundingCube();
double north = 0.0;
double south = 180.0;
double east = 0.0;
double west = 360.0;
for (int i = 0; i < _simpleGridPoints.count(); i++) {
QGeoCoordinate coord = _simpleGridPoints[i].value<QGeoCoordinate>();
double lat = coord.latitude() + 90.0;
double lon = coord.longitude() + 180.0;
north = fmax(north, lat);
south = fmin(south, lat);
east = fmax(east, lon);
west = fmin(west, lon);
}
_setBoundingBox(QRectF(east - 180.0, north - 90.0, west - 180.0, south - 90.0));
if (cameraShots == 0 && _triggerCamera()) { if (cameraShots == 0 && _triggerCamera()) {
cameraShots = (int)floor(surveyDistance / _triggerDistance()); cameraShots = (int)floor(surveyDistance / _triggerDistance());
...@@ -1315,6 +1323,7 @@ void SurveyMissionItem::applyNewAltitude(double newAltitude) ...@@ -1315,6 +1323,7 @@ void SurveyMissionItem::applyNewAltitude(double newAltitude)
{ {
_fixedValueIsAltitudeFact.setRawValue(true); _fixedValueIsAltitudeFact.setRawValue(true);
_gridAltitudeFact.setRawValue(newAltitude); _gridAltitudeFact.setRawValue(newAltitude);
_calcBoundingCube();
} }
void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees) void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees)
......
...@@ -96,7 +96,7 @@ public: ...@@ -96,7 +96,7 @@ public:
// Overrides from ComplexMissionItem // Overrides from ComplexMissionItem
double complexDistance (void) const final { return _surveyDistance; } double complexDistance (void) const final { return _surveyDistance; }
QRectF boundingBox (void) const final { return _boundingBox; } QGCGeoBoundingCube boundingCube (void) const final { return _boundingCube; }
double additionalTimeDelay (void) const final { return _additionalFlightDelaySeconds; } double additionalTimeDelay (void) const final { return _additionalFlightDelaySeconds; }
int lastSequenceNumber (void) const final; int lastSequenceNumber (void) const final;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
...@@ -200,7 +200,7 @@ private: ...@@ -200,7 +200,7 @@ private:
void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines); void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines); void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines);
void _setSurveyDistance(double surveyDistance); void _setSurveyDistance(double surveyDistance);
void _setBoundingBox(QRectF bb); void _setBoundingCube(QGCGeoBoundingCube bc);
void _setCameraShots(int cameraShots); void _setCameraShots(int cameraShots);
void _setCoveredArea(double coveredArea); void _setCoveredArea(double coveredArea);
void _cameraValueChanged(void); void _cameraValueChanged(void);
...@@ -226,6 +226,7 @@ private: ...@@ -226,6 +226,7 @@ private:
bool _gridAngleIsNorthSouthTransects(); bool _gridAngleIsNorthSouthTransects();
double _clampGridAngle90(double gridAngle); double _clampGridAngle90(double gridAngle);
int _calcMissionCommandCount(QList<QList<QGeoCoordinate>>& transectSegments); int _calcMissionCommandCount(QList<QList<QGeoCoordinate>>& transectSegments);
void _calcBoundingCube();
int _sequenceNumber; int _sequenceNumber;
bool _dirty; bool _dirty;
...@@ -243,12 +244,12 @@ private: ...@@ -243,12 +244,12 @@ private:
bool _ignoreRecalc; bool _ignoreRecalc;
double _surveyDistance; double _surveyDistance;
QRectF _boundingBox;
int _cameraShots; int _cameraShots;
double _coveredArea; double _coveredArea;
double _timeBetweenShots; double _timeBetweenShots;
double _cruiseSpeed; double _cruiseSpeed;
QGCGeoBoundingCube _boundingCube;
QMap<QString, FactMetaData*> _metaDataMap; QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _manualGridFact; SettingsFact _manualGridFact;
......
...@@ -102,3 +102,10 @@ void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, ...@@ -102,3 +102,10 @@ void convertUTMToGeo(double easting, double northing, int zone, bool southhemi,
coord.setLatitude(RadToDeg(latRadians)); coord.setLatitude(RadToDeg(latRadians));
coord.setLongitude(RadToDeg(lonRadians)); coord.setLongitude(RadToDeg(lonRadians));
} }
double QGCGeoBoundingCube::MaxAlt = 1000000.0;
double QGCGeoBoundingCube::MinAlt = -1000000.0;
double QGCGeoBoundingCube::MaxNorth = 90.0;
double QGCGeoBoundingCube::MaxSouth = -90.0;
double QGCGeoBoundingCube::MaxWest = -180.0;
double QGCGeoBoundingCube::MaxEast = 180.0;
...@@ -81,4 +81,46 @@ int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northi ...@@ -81,4 +81,46 @@ int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northi
// The function does not return a value. // The function does not return a value.
void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord); void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord);
// A bounding cube
class QGCGeoBoundingCube {
public:
QGCGeoBoundingCube()
: pointNW(QGeoCoordinate(MaxSouth, MaxEast, MaxAlt))
, pointSE(QGeoCoordinate(MaxNorth, MaxWest, MinAlt))
{
}
QGCGeoBoundingCube(QGeoCoordinate p1, QGeoCoordinate p2)
: pointNW(p1)
, pointSE(p2)
{
}
QGeoCoordinate pointNW;
QGeoCoordinate pointSE;
inline bool operator ==(const QGCGeoBoundingCube& other)
{
return pointNW == other.pointNW && pointSE == other.pointSE;
}
inline bool operator !=(const QGCGeoBoundingCube& other)
{
return !(*this == other);
}
inline QGCGeoBoundingCube operator =(const QGCGeoBoundingCube& other)
{
pointNW = other.pointNW;
pointSE = other.pointSE;
return *this;
}
inline bool isValid()
{
return pointNW.isValid() && pointSE.isValid() && pointNW.latitude() != MaxSouth && pointSE.latitude() != MaxNorth && \
pointNW.longitude() != MaxEast && pointSE.longitude() != MaxWest && pointNW.altitude() < MaxAlt and pointSE.altitude() > MinAlt;
}
static double MaxAlt;
static double MinAlt;
static double MaxNorth;
static double MaxSouth;
static double MaxWest;
static double MaxEast;
};
#endif // QGCGEO_H #endif // QGCGEO_H
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