// Calculate distance and bearing using Geodetic Datums
void au_uav_ros::CSimulation::GeodeticCalculation(double lat1,double lat2,
double long1,double long2,
double& newLat,double& newLong,
double& actualBearing,
double& bearing,
double& distanceToDestination,
double& groundSpeed)
{
GeographicLib::Math::real azi1, azi2, s12, m12, a12, M12, M21, S12;
// Using WGS84 a,f (radius and flattening values)
GeographicLib::Math::real
a = GeographicLib::Constants::WGS84_a<GeographicLib::Math::real>(),
f = GeographicLib::Constants::WGS84_f<GeographicLib::Math::real>();
const GeographicLib::Geodesic geod(a, f);
// All calculation done in degrees
double lat1_deg,lat2_deg,long1_deg,long2_deg;
lat1_deg = lat1*RADIANS_TO_DEGREES;
lat2_deg = lat2*RADIANS_TO_DEGREES;
long1_deg = long1*RADIANS_TO_DEGREES;
long2_deg = long2*RADIANS_TO_DEGREES;
// Calculate Bearing
a12 = geod.Inverse(lat1_deg, long1_deg,
lat2_deg, long2_deg,
s12, azi1, azi2,
m12, M12, M21, S12);
bearing = azi1*DEGREES_TO_RADIANS;
// Distance to Destination Returned to the caller
distanceToDestination = s12;
// DIfference between course and bearing
actualBearing = CheckTurningRadius(actualBearing*RADIANS_TO_DEGREES,bearing*RADIANS_TO_DEGREES);
double distance = MPS_SPEED;
// Apply Environment and GPS effects
au_uav_ros::CSimulation::GetInstance().UpdateSimulatedValues(actualBearing,distance, groundSpeed);
// Geodesic Line along which the UAV is moving
GeographicLib::GeodesicLine l;
double temp_actualBearing = actualBearing*RADIANS_TO_DEGREES;
l = geod.Line(lat1_deg, long1_deg,temp_actualBearing );
// Obtain next location along geodesic line
a12 = l.Position(distance, newLat, newLong, temp_actualBearing, m12, M12, M21, S12);
newLat = newLat*DEGREES_TO_RADIANS;
newLong = newLong*DEGREES_TO_RADIANS;
}
void Trajectory::calcLengthPath(void){
try {
const Geodesic& geod = Geodesic::WGS84();
double a12 = geod.Inverse(this->startPoint.getDecLatitude(), this->startPoint.getDecLongitude(),this->endPoint.getDecLatitude(), this->endPoint.getDecLongitude(), this->distanceLengthStartToEnd, this->azimuthStartEndPointFromStart,this->azimuthStartEndPointFormEnd);
const GeographicLib::GeodesicLine line(geod,this->startPoint.getDecLatitude(),this->startPoint.getDecLongitude(), this->azimuthStartEndPointFromStart);
this->numberOfInervals = int(ceil(this->distanceLengthStartToEnd / this->quantumStep));
this->distanceStartToNthPoint=this->distanceLengthStartToEnd/this->numberOfInervals;
for (int i = 0; i <=this->numberOfInervals; ++i) {
double lat,lon;
line.Position(i*this->distanceStartToNthPoint,lat,lon);
this->nThPoint.setDecLatitude(lat);
this->nThPoint.setDecLongitude(lon);
this->pathLength.push_back(this->nThPoint);
}
vypis(this->pathLength);
} catch (const exception& e) {
cerr << "Caught exception: " << e.what() << "\n";
//return 1;
}
}