Skip to content

Instantly share code, notes, and snippets.

@fernandoc1
Created August 11, 2017 18:10
Show Gist options
  • Save fernandoc1/94a164331017a2d73574991c7fa20ac5 to your computer and use it in GitHub Desktop.
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.
#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;
}
#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
@fernandoc1
Copy link
Author

This code depends on Qt5 and libkml.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment