Created
August 11, 2017 18:10
-
-
Save fernandoc1/94a164331017a2d73574991c7fa20ac5 to your computer and use it in GitHub Desktop.
This code calculates intersection between arcs in the sphere surface and between arcs and a route.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "geo_helper.hpp" | |
#include <cmath> | |
//It assumes radius as 1. | |
QVector3D GeoHelper::QGeoCoordinate2QVector3D(QGeoCoordinate coord) | |
{ | |
double radlat = qDegreesToRadians(coord.latitude()); | |
double radlon = qDegreesToRadians(coord.longitude()); | |
return QVector3D( | |
qCos(radlat)*qCos(radlon), | |
qCos(radlat)*qSin(radlon), | |
qSin(radlat) | |
); | |
} | |
QGeoCoordinate GeoHelper::QVector3D2QGeoCoordinate(QVector3D vec) | |
{ | |
double lat = qAsin(vec.z()); | |
double tmp = qCos(lat); | |
double sign = qAsin(vec.y() / tmp); | |
double lon = qAcos(vec.x() / tmp); | |
if(sign < 0) | |
{ | |
lon = qAbs(lon) * (-1); | |
} | |
else | |
{ | |
lon = qAbs(lon); | |
} | |
return QGeoCoordinate(qRadiansToDegrees(lat), qRadiansToDegrees(lon)); | |
} | |
QGeoCoordinate GeoHelper::KmlbaseVec3ToQGeoCoordinate(kmlbase::Vec3 vec) | |
{ | |
return QGeoCoordinate(vec.get_latitude(), vec.get_longitude()); | |
} | |
void GeoHelper::printKmlbaseVec3(kmlbase::Vec3 vec) | |
{ | |
std::cout << vec.get_latitude() << " " << vec.get_longitude() << " " << vec.get_altitude() << std::endl; | |
} | |
void GeoHelper::printKmldomPlacemark(kmldom::PlacemarkPtr placemark) | |
{ | |
kmldom::GeometryPtr geometry = placemark->get_geometry(); | |
kmldom::PointPtr point = kmldom::AsPoint(geometry); | |
if(point) | |
{ | |
kmldom::CoordinatesPtr coordinates = point->get_coordinates(); | |
int size = coordinates->get_coordinates_array_size(); | |
if(size < 1) | |
{ | |
throw std::runtime_error("GeoHelper::printKmldomPlacemark: there are no coordinates to print"); | |
} | |
for(int i = 0; i < size; i++) | |
{ | |
printKmlbaseVec3(coordinates->get_coordinates_array_at(i)); | |
} | |
} | |
else | |
{ | |
throw std::runtime_error("GeoHelper::printKmldomPlacemark: there is no point in placemark."); | |
} | |
} | |
Arc::Arc(double lng1, double lat1, double lng2, double lat2) | |
: a(lat1, lng1) | |
, b(lat2, lng2) | |
{ | |
//qInfo() <<"Arc::Arc: double: QGeoCoordinate a: " << a; | |
//qInfo() <<"Arc::Arc: double: QGeoCoordinate b: " << b; | |
} | |
Arc::Arc(kmlbase::Vec3 p1, kmlbase::Vec3 p2) | |
: a(p1.get_latitude(), p1.get_longitude()) | |
, b(p2.get_latitude(), p2.get_longitude()) | |
{ | |
//qInfo() <<"Arc::Arc: kmlbase::Vec3: QGeoCoordinate a: " << a; | |
//qInfo() <<"Arc::Arc: kmlbase::Vec3: QGeoCoordinate b: " << b; | |
} | |
Arc::Arc(QGeoCoordinate a, QGeoCoordinate b) | |
: a(a) | |
, b(b) | |
{ | |
//qInfo() <<"Arc::Arc: QGeoCoordinate a: " << a.latitude() << " " << a.longitude(); | |
//qInfo() <<"Arc::Arc: QGeoCoordinate b: " << b.latitude() << " " << b.longitude(); | |
} | |
QString Arc::toString() | |
{ | |
QString output; | |
QPair<QGeoCoordinate, QGeoCoordinate> arcPair = this->getCoordinates(); | |
QGeoCoordinate a = arcPair.first; | |
QGeoCoordinate b = arcPair.second; | |
output.append(a.toString(QGeoCoordinate::Degrees)); | |
output.append(" "); | |
output.append(b.toString(QGeoCoordinate::Degrees)); | |
return output; | |
} | |
QPair<QGeoCoordinate, QGeoCoordinate> Arc::getCoordinates() | |
{ | |
return QPair<QGeoCoordinate, QGeoCoordinate>(a, b); | |
} | |
double Arc::getLength() | |
{ | |
return this->a.distanceTo(b); | |
} | |
bool Arc::isCoordinateInArc(QGeoCoordinate coord) | |
{ | |
double total = this->getLength(); | |
double lengthToA = a.distanceTo(coord); | |
double lengthToB = b.distanceTo(coord); | |
double test = total - (lengthToA + lengthToB); | |
qInfo() << "Arc::isCoordinateInArc: Test value: " << test; | |
return (qAbs(test) < 3.000); | |
} | |
QVector3D Arc::getGreatCirclePlaneNormal() | |
{ | |
QVector3D p1 = GeoHelper::QGeoCoordinate2QVector3D(a); | |
QVector3D p2 = GeoHelper::QGeoCoordinate2QVector3D(b); | |
p1.normalize(); | |
p2.normalize(); | |
return QVector3D::crossProduct(p1, p2).normalized(); | |
} | |
bool Arc::intersects(Arc& other) | |
{ | |
qInfo() << "Arc::intersects: Testing intersection for: " << this->toString() << " " << other.toString(); | |
QVector3D thisGreatCircleNormal = this->getGreatCirclePlaneNormal(); | |
QVector3D otherGreatCircleNormal = other.getGreatCirclePlaneNormal(); | |
QVector3D intersectionPointA = QVector3D::crossProduct(thisGreatCircleNormal, otherGreatCircleNormal).normalized(); | |
QVector3D intersectionPointB = QVector3D::crossProduct(otherGreatCircleNormal, thisGreatCircleNormal).normalized(); | |
QGeoCoordinate intersectionCoordA = GeoHelper::QVector3D2QGeoCoordinate(intersectionPointA); | |
QGeoCoordinate intersectionCoordB = GeoHelper::QVector3D2QGeoCoordinate(intersectionPointB); | |
if( | |
(intersectionCoordA.latitude() == 0) | |
&& (intersectionCoordA.longitude() == 0) | |
&& (intersectionCoordB.latitude() == 0) | |
&& (intersectionCoordB.longitude() == 0) | |
) | |
{ | |
return false; | |
} | |
qInfo() << "Arc::intersects: " << intersectionCoordA << " " << intersectionCoordB; | |
return (this->isCoordinateInArc(intersectionCoordA) || this->isCoordinateInArc(intersectionCoordB)) && | |
(other.isCoordinateInArc(intersectionCoordA) || other.isCoordinateInArc(intersectionCoordB)); | |
} | |
Route::Route(kmldom::LineStringPtr linestring) | |
{ | |
if(!linestring) | |
{ | |
throw std::runtime_error("Route::Route: linestring is NULL"); | |
} | |
kmldom::KmlFactory* factory(kmldom::KmlFactory::GetFactory()); | |
kmldom::CoordinatesPtr coordinates = linestring->get_coordinates(); | |
for(int i = 0; i < coordinates->get_coordinates_array_size(); i++) | |
{ | |
kmldom::CoordinatesPtr singleCoordinate(factory->CreateCoordinates()); | |
kmlbase::Vec3 vec = coordinates->get_coordinates_array_at(i); | |
singleCoordinate->add_vec3(vec); | |
kmldom::PointPtr point(factory->CreatePoint()); | |
point->set_coordinates(singleCoordinate); | |
kmldom::GeometryPtr geometry = kmldom::AsGeometry(point); | |
if(!geometry) | |
{ | |
throw std::runtime_error("Route::Route: failed to convert point to geometry"); | |
} | |
kmldom::PlacemarkPtr placemark(factory->CreatePlacemark()); | |
placemark->set_geometry(geometry); | |
this->addPlacemark(placemark); | |
} | |
} | |
void Route::addPlacemark(kmldom::PlacemarkPtr placemark) | |
{ | |
//std::cout << "Route::addPlacemark: "; GeoHelper::printKmldomPlacemark(placemark); | |
this->featureList.PushBack(placemark); | |
kmldom::GeometryPtr geometry = placemark->get_geometry(); | |
kmldom::PointPtr point = kmldom::AsPoint(geometry); | |
if(point) | |
{ | |
this->placemarkVector.push_back(placemark); | |
} | |
else | |
{ | |
throw std::runtime_error("Route::addPlacemark: placemark do not contain a point"); | |
} | |
} | |
kmlbase::Vec3 Route::getCoordinate(int index) | |
{ | |
kmldom::PlacemarkPtr placemark = this->placemarkVector[index]; | |
kmldom::GeometryPtr geometry = placemark->get_geometry(); | |
kmldom::PointPtr point = kmldom::AsPoint(geometry); | |
if(point) | |
{ | |
kmldom::CoordinatesPtr coordinates = point->get_coordinates(); | |
if(coordinates->get_coordinates_array_size() == 1) | |
{ | |
kmlbase::Vec3 coord = coordinates->get_coordinates_array_at(0); | |
return coord; | |
} | |
else | |
{ | |
std::string error("Route::getCoordinates: unexpected number of coordinates "); | |
error += std::to_string(coordinates->get_coordinates_array_size()); | |
throw std::runtime_error(error); | |
} | |
} | |
std::string error("Route::getCoordinates: index "); | |
error += std::to_string(index); | |
error += " do not contain a point"; | |
throw std::runtime_error(error); | |
} | |
QGeoCoordinate Route::getQGeoCoordinate(int index) | |
{ | |
kmlbase::Vec3 coord = this->getCoordinate(index); | |
return QGeoCoordinate(coord.get_latitude(), coord.get_longitude()); | |
} | |
kmldom::TimeStampPtr Route::getTimestamp(int index) | |
{ | |
kmldom::PlacemarkPtr placemark = this->placemarkVector[index]; | |
kmldom::TimePrimitivePtr time = placemark->get_timeprimitive(); | |
kmldom::TimeStampPtr timestamp = kmldom::AsTimeStamp(time); | |
if(timestamp) | |
{ | |
return timestamp; | |
} | |
std::string error("Route::getTimestamp: index "); | |
error += std::to_string(index); | |
error += " do not contain a timestamp"; | |
throw std::runtime_error(error); | |
} | |
kmlengine::Bbox Route::getBoundingBox() | |
{ | |
kmlengine::Bbox bbox; | |
this->featureList.ComputeBoundingBox(&bbox); | |
return bbox; | |
} | |
int Route::getPlacemarksAmount() | |
{ | |
return this->placemarkVector.size(); | |
} | |
bool Route::intersects(Arc& arc) | |
{ | |
for(int i = 1; i < this->placemarkVector.size(); i++) | |
{ | |
QGeoCoordinate a = GeoHelper::KmlbaseVec3ToQGeoCoordinate(this->getCoordinate(i - 1)); | |
QGeoCoordinate b = GeoHelper::KmlbaseVec3ToQGeoCoordinate(this->getCoordinate(i)); | |
qInfo() << "Route::intersects: Route arc for checking: " << a << " " << b; | |
Arc currentArc(a, b); | |
qInfo() << "Route::intersects: Arc length: " << currentArc.getLength(); | |
if(arc.intersects(currentArc)) | |
{ | |
return true; | |
} | |
} | |
return false; | |
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#ifndef __GEO_HELPER_HPP__ | |
#define __GEO_HELPER_HPP__ | |
#include <algorithm> | |
#include <iostream> | |
#include <kml/base/file.h> | |
#include <kml/dom.h> | |
#include <kml/engine.h> | |
#include <kml/dom/feature.h> | |
#include <kml/convenience/convenience.h> | |
#include <kml/convenience/feature_list.h> | |
#include <string> | |
#include <vector> | |
#include <math.h> | |
#include <QDebug> | |
#include <QGeoCoordinate> | |
#include <QVector3D> | |
#include <QtMath> | |
/* | |
* Reference: | |
https://www.boeing-727.com/Data/fly%20odds/distance.html | |
*/ | |
namespace GeoHelper | |
{ | |
QVector3D QGeoCoordinate2QVector3D(QGeoCoordinate coord); | |
QGeoCoordinate QVector3D2QGeoCoordinate(QVector3D vec); | |
QGeoCoordinate KmlbaseVec3ToQGeoCoordinate(kmlbase::Vec3 vec); | |
void printKmlbaseVec3(kmlbase::Vec3 vec); | |
void printKmldomPlacemark(kmldom::PlacemarkPtr placemark); | |
} | |
class Arc | |
{ | |
QGeoCoordinate a; | |
QGeoCoordinate b; | |
public: | |
Arc(QGeoCoordinate a, QGeoCoordinate b); | |
Arc(double lng1, double lat1, double lng2, double lat2); | |
Arc(kmlbase::Vec3 p1, kmlbase::Vec3 p2); | |
QString toString(); | |
QPair<QGeoCoordinate, QGeoCoordinate> getCoordinates(); | |
QVector3D getGreatCirclePlaneNormal(); | |
bool intersects(Arc& other); | |
bool isCoordinateInArc(QGeoCoordinate coord); | |
double getLength(); | |
}; | |
class Route | |
{ | |
private: | |
kmlconvenience::FeatureList featureList; | |
std::vector<kmldom::PlacemarkPtr> placemarkVector; | |
public: | |
Route(){} | |
Route(kmldom::LineStringPtr linestring); | |
void addPlacemark(kmldom::PlacemarkPtr placemark); | |
int getPlacemarksAmount(); | |
kmlbase::Vec3 getCoordinate(int index); | |
QGeoCoordinate getQGeoCoordinate(int index); | |
kmldom::TimeStampPtr getTimestamp(int index); | |
kmlengine::Bbox getBoundingBox(); | |
bool intersects(Arc& arc); | |
}; | |
#endif |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
This code depends on Qt5 and libkml.