fixed
DoubleDistance(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3)
{
assert(loc1.IsValid());
assert(loc2.IsValid());
assert(loc3.IsValid());
const fixed cos_loc1_lat = loc1.latitude.cos();
const fixed cos_loc2_lat = loc2.latitude.cos();
const fixed cos_loc3_lat = loc3.latitude.cos();
const fixed s21 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const fixed sl21 = (loc2.longitude - loc1.longitude).accurate_half_sin();
const fixed s32 = (loc3.latitude - loc2.latitude).accurate_half_sin();
const fixed sl32 = (loc3.longitude - loc2.longitude).accurate_half_sin();
const fixed a12 = sqr(s21)
+ SmallMult(cos_loc1_lat, cos_loc2_lat) * sqr(sl21);
const fixed a23 = sqr(s32)
+ SmallMult(cos_loc2_lat, cos_loc3_lat) * sqr(sl32);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return (2 * REARTH) *
(EarthDistance(a12) + EarthDistance(a23)).Radians();
}
gcc_pure
static GeoPoint
IntermediatePoint(const GeoPoint &loc1, const GeoPoint &loc2,
Angle dthis, Angle dtotal)
{
assert(loc1.IsValid());
assert(loc2.IsValid());
if (loc1.longitude == loc2.longitude &&
loc1.latitude == loc2.latitude)
return loc1;
if (!positive(dtotal.Native()))
return loc1;
assert(dthis <= dtotal && !negative(dthis.Native()));
const fixed A = (dtotal - dthis).sin();
const fixed B = dthis.sin();
const auto sc1 = loc1.latitude.SinCos();
const fixed sin_loc1_lat = sc1.first, cos_loc1_lat = sc1.second;
const auto sc2 = loc2.latitude.SinCos();
const fixed sin_loc2_lat = sc2.first, cos_loc2_lat = sc2.second;
const auto sc3 = loc1.longitude.SinCos();
const fixed sin_loc1_lon = sc3.first, cos_loc1_lon = sc3.second;
const auto sc4 = loc2.longitude.SinCos();
const fixed sin_loc2_lon = sc4.first, cos_loc2_lon = sc4.second;
const fixed a_cos_loc1_lat = SmallMult(A, cos_loc1_lat);
const fixed b_cos_loc2_lat = SmallMult(B, cos_loc2_lat);
const fixed x = SmallMult(a_cos_loc1_lat, cos_loc1_lon)
+ SmallMult(b_cos_loc2_lat, cos_loc2_lon);
const fixed y = SmallMult(a_cos_loc1_lat, sin_loc1_lon)
+ SmallMult(b_cos_loc2_lat, sin_loc2_lon);
const fixed z = SmallMult(A, sin_loc1_lat) + SmallMult(B, sin_loc2_lat);
GeoPoint loc3;
loc3.latitude = Angle::FromXY(TinyHypot(x, y), z);
loc3.longitude = Angle::FromXY(x, y);
loc3.Normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc3;
}
GeoPoint
FindLatitudeLongitudeS(const GeoPoint &loc, const Angle bearing,
double distance)
{
assert(loc.IsValid());
assert(distance >= 0);
if (distance <= 0)
return loc;
const Angle distance_angle = FAISphere::EarthDistanceToAngle(distance);
const auto scd = distance_angle.SinCos();
const auto sin_distance = scd.first, cos_distance = scd.second;
const auto scb = bearing.SinCos();
const auto sin_bearing = scb.first, cos_bearing = scb.second;
const auto scl = loc.latitude.SinCos();
const auto sin_latitude = scl.first, cos_latitude = scl.second;
GeoPoint loc_out;
loc_out.latitude = Angle::asin(sin_latitude * cos_distance
+ cos_latitude * sin_distance * cos_bearing);
loc_out.longitude = loc.longitude +
Angle::FromXY(cos_distance - sin_latitude * loc_out.latitude.sin(),
sin_bearing * sin_distance * cos_latitude);
loc_out.Normalize(); // ensure longitude is within -180:180
return loc_out;
}
inline double
OrderedTask::ScanDistanceMin(const GeoPoint &location, bool full)
{
if (!full && location.IsValid() && last_min_location.IsValid() &&
DistanceIsSignificant(location, last_min_location)) {
const TaskWaypoint *active = GetActiveTaskPoint();
if (active != nullptr) {
const GeoPoint &target = active->GetWaypoint().location;
const unsigned last_distance =
(unsigned)last_min_location.Distance(target);
const unsigned cur_distance =
(unsigned)location.Distance(target);
/* do the full scan only if the distance to the active task
point has changed by more than 5%, otherwise we don't expect
any relevant changes */
if (last_distance < 2000 || cur_distance < 2000 ||
last_distance * 20 >= cur_distance * 21 ||
cur_distance * 20 >= last_distance * 21)
full = true;
}
}
if (full) {
RunDijsktraMin(location);
last_min_location = location;
}
return task_points.front()->ScanDistanceMin();
}
inline GeoVector
TaskLeg::GetRemainingVector(const GeoPoint &ref) const
{
switch (destination.GetActiveState()) {
case OrderedTaskPoint::AFTER_ACTIVE:
// this leg totally included
return GetPlannedVector();
case OrderedTaskPoint::CURRENT_ACTIVE: {
// this leg partially included
if (!ref.IsValid())
/* if we don't have a GPS fix yet, we fall back to the "planned"
vector unless this task leg has already been achieved */
return destination.HasEntered()
? GeoVector::Zero()
: GetPlannedVector();
return memo_remaining.calc(ref, destination.GetLocationRemaining());
}
case OrderedTaskPoint::BEFORE_ACTIVE:
// this leg not included
return GeoVector::Zero();
}
gcc_unreachable();
assert(false);
return GeoVector::Invalid();
}
static void
WriteEvent(JSON::ObjectWriter &object, const char *name,
const BrokenDateTime &time, const GeoPoint &location)
{
if (time.IsPlausible() || location.IsValid())
object.WriteElement(name, WriteEventAttributes, time, location);
}
GlideResult
TaskSolution::GlideSolutionRemaining(const GeoPoint &location,
const GeoPoint &target,
const fixed target_elevation,
const fixed altitude,
const SpeedVector &wind,
const GlideSettings &settings,
const GlidePolar &polar)
{
assert(location.IsValid());
assert(target.IsValid());
GlideState gs(location.DistanceBearing(target),
target_elevation, altitude, wind);
return MacCready::Solve(settings, polar, gs);
}
inline GeoVector
TaskLeg::GetTravelledVector(const GeoPoint &ref) const
{
switch (destination.GetActiveState()) {
case OrderedTaskPoint::BEFORE_ACTIVE:
if (!GetOrigin())
return GeoVector::Zero();
// this leg totally included
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
destination.GetLocationTravelled());
case OrderedTaskPoint::CURRENT_ACTIVE:
// this leg partially included
if (!GetOrigin())
return GeoVector(0,
ref.IsValid()
? ref.Bearing(destination.GetLocationRemaining())
: Angle::Zero());
if (destination.HasEntered())
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
destination.GetLocationTravelled());
else if (!ref.IsValid())
return GeoVector::Zero();
else
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(), ref);
case OrderedTaskPoint::AFTER_ACTIVE:
if (!GetOrigin())
return GeoVector::Zero();
// this leg may be partially included
if (GetOrigin()->HasEntered())
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
ref.IsValid()
? ref
: destination.GetLocationTravelled());
return GeoVector::Zero();
}
gcc_unreachable();
assert(false);
return GeoVector::Invalid();
}
/*
$GPRMB,<1>,,,,<5>,,,,,<10>,<11>,,<13>*hh<CR><LF>
<1> Position Valid (A = valid, V = invalid)
<5> Destination waypoint identifier, three digits
(leading zeros will be transmitted)
<10> Range from present position to distination waypoint, format XXXX.X,
nautical miles (leading zeros will be transmitted)
<11> Bearing from present position to destination waypoint, format XXX.X,
degrees true (leading zeros will be transmitted)
<13> Arrival flag <A = arrival, V = not arrival)
*/
static bool
FormatGPRMB(char *buffer, size_t buffer_size, const GeoPoint& here,
const AGeoPoint &destination)
{
if (!here.IsValid() || !destination.IsValid())
return false;
const GeoVector vector(here, destination);
const bool has_arrived = vector.distance < 1000; // < 1km ?
snprintf(buffer, buffer_size, "GPRMB,%c,,,,,,,,,%06.1f,%04.1f,%c",
here.IsValid() ? 'A' : 'V',
(double)Units::ToUserUnit(vector.distance, Unit::NAUTICAL_MILES),
(double)vector.bearing.Degrees(),
has_arrived ? 'A' : 'V');
return true;
}
fixed
UnorderedTask::ScanDistanceRemaining(const GeoPoint &location)
{
TaskPoint *tp = GetActiveTaskPoint();
if (tp == nullptr || !location.IsValid())
return fixed(0);
return tp->Distance(location);
}
inline fixed
TaskLeg::GetScoredDistance(const GeoPoint &ref) const
{
if (!GetOrigin())
return fixed(0);
switch (destination.GetActiveState()) {
case OrderedTaskPoint::BEFORE_ACTIVE:
// this leg totally included
return std::max(fixed(0),
GetOrigin()->GetLocationScored().Distance(destination.GetLocationScored())
- GetOrigin()->ScoreAdjustment()-destination.ScoreAdjustment());
case OrderedTaskPoint::CURRENT_ACTIVE:
// this leg partially included
if (destination.HasEntered()) {
return std::max(fixed(0),
GetOrigin()->GetLocationScored().Distance(destination.GetLocationScored())
- GetOrigin()->ScoreAdjustment()-destination.ScoreAdjustment());
} else if (ref.IsValid())
return std::max(fixed(0),
ref.ProjectedDistance(GetOrigin()->GetLocationScored(),
destination.GetLocationScored())
-GetOrigin()->ScoreAdjustment());
else
return fixed(0);
case OrderedTaskPoint::AFTER_ACTIVE:
// this leg may be partially included
if (GetOrigin()->HasEntered() && ref.IsValid()) {
return std::max(fixed(0),
memo_travelled.calc(GetOrigin()->GetLocationScored(),
ref).distance
-GetOrigin()->ScoreAdjustment());
}
return fixed(0);
}
gcc_unreachable();
assert(false);
return fixed(0);
}
/**
* Calculates the distance and bearing of two locations
* @param loc1 Location 1
* @param loc2 Location 2
* @param Distance Pointer to the distance variable
* @param Bearing Pointer to the bearing variable
*/
static void
DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2,
Angle *distance, Angle *bearing)
{
assert(loc1.IsValid());
assert(loc2.IsValid());
const auto sc1 = loc1.latitude.SinCos();
fixed sin_lat1 = sc1.first, cos_lat1 = sc1.second;
const auto sc2 = loc2.latitude.SinCos();
fixed sin_lat2 = sc2.first, cos_lat2 = sc2.second;
const Angle dlon = loc2.longitude - loc1.longitude;
if (distance) {
const fixed s1 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const fixed s2 = dlon.accurate_half_sin();
const fixed a = sqr(s1) + SmallMult(cos_lat1, cos_lat2) * sqr(s2);
Angle distance2 = EarthDistance(a);
assert(!negative(distance2.Native()));
*distance = distance2;
}
if (bearing) {
// speedup for fixed since this is one call
const auto sc = dlon.SinCos();
const fixed sin_dlon = sc.first, cos_dlon = sc.second;
const fixed y = SmallMult(sin_dlon, cos_lat2);
const fixed x = SmallMult(cos_lat1, sin_lat2)
- SmallMult(sin_lat1, cos_lat2, cos_dlon);
*bearing = (x == fixed(0) && y == fixed(0))
? Angle::Zero()
: Angle::FromXY(x, y).AsBearing();
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
}
void
ChartProjection::Set(const PixelRect &rc, const OrderedTask &task,
const GeoPoint &fallback_loc)
{
GeoPoint center = task.GetTaskCenter();
if (!center.IsValid())
center = fallback_loc;
const fixed radius = std::max(fixed(10000), task.GetTaskRadius());
Set(rc, center, radius);
}
void
FlatProjection::SetCenter(const GeoPoint &_center)
{
assert(_center.IsValid());
center = _center;
cos = center.latitude.fastcosine() * fixed_scale;
r_cos = 1. / cos;
approx_scale = Unproject(FlatGeoPoint(0,-1)).DistanceS(Unproject(FlatGeoPoint(0,1))) / 2;
}
void
DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2,
Angle *distance, Angle *bearing)
{
assert(loc1.IsValid());
assert(loc2.IsValid());
const auto sc1 = loc1.latitude.SinCos();
auto sin_lat1 = sc1.first, cos_lat1 = sc1.second;
const auto sc2 = loc2.latitude.SinCos();
auto sin_lat2 = sc2.first, cos_lat2 = sc2.second;
const Angle dlon = loc2.longitude - loc1.longitude;
if (distance) {
const auto s1 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const auto s2 = dlon.accurate_half_sin();
const auto a = Square(s1) + cos_lat1 * cos_lat2 * Square(s2);
Angle distance2 = EarthDistance(a);
assert(!distance2.IsNegative());
*distance = distance2;
}
if (bearing) {
// speedup for fixed since this is one call
const auto sc = dlon.SinCos();
const auto sin_dlon = sc.first, cos_dlon = sc.second;
const auto y = sin_dlon * cos_lat2;
const auto x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*bearing = (x == fixed(0) && y == fixed(0))
? Angle::Zero()
: Angle::FromXY(x, y).AsBearing();
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
}
void Push(lua_State *L, GeoPoint value) {
if (value.IsValid()) {
lua_newtable(L);
lua_newtable(L);
SetField(L, -2, "__tostring", l_GeoPoint_tostring);
lua_setmetatable(L, -2);
SetField(L, -2, "longitude", value.longitude);
SetField(L, -2, "latitude", value.latitude);
} else
lua_pushnil(L);
}
void
GeoBounds::Extend(const GeoPoint pt)
{
if (!pt.IsValid())
return;
if (IsValid()) {
longitude.Extend(pt.longitude);
latitude.Extend(pt.latitude);
} else {
*this = GeoBounds(pt);
}
}
void
DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2,
Angle *distance, Angle *bearing)
{
assert(loc1.IsValid());
assert(loc2.IsValid());
const auto sc1 = loc1.latitude.SinCos();
auto sin_lat1 = sc1.first, cos_lat1 = sc1.second;
const auto sc2 = loc2.latitude.SinCos();
auto sin_lat2 = sc2.first, cos_lat2 = sc2.second;
const Angle dlon = loc2.longitude - loc1.longitude;
if (distance) {
const auto s1 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const auto s2 = dlon.accurate_half_sin();
const auto a = Square(s1) + cos_lat1 * cos_lat2 * Square(s2);
Angle distance2 = EarthDistance(a);
assert(!distance2.IsNegative());
*distance = distance2;
}
if (bearing) {
const auto sc = dlon.SinCos();
const auto sin_dlon = sc.first, cos_dlon = sc.second;
const auto y = sin_dlon * cos_lat2;
const auto x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*bearing = (x == 0 && y == 0)
? Angle::Zero()
: Angle::FromXY(x, y).AsBearing();
}
}
static void
WriteEventAttributes(TextWriter &writer,
const BrokenDateTime &time, const GeoPoint &location)
{
JSON::ObjectWriter object(writer);
if (time.IsPlausible()) {
NarrowString<64> buffer;
FormatISO8601(buffer.buffer(), time);
object.WriteElement("time", JSON::WriteString, buffer);
}
if (location.IsValid())
JSON::WriteGeoPointAttributes(object, location);
}
static void
Main()
{
GeoPoint value = GeoPoint(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
if (!GeoPointEntryDialog(_T("The caption"), value, format, true))
return;
if (value.IsValid())
_tprintf(_T("%s\n"),
FormatGeoPoint(value, CoordinateFormat::DDMMSS).c_str());
else
printf("invalid\n");
}
static void
Main()
{
GeoPoint value = GeoPoint(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
if (!GeoPointEntryDialog(_T("The caption"), value, true))
return;
if (value.IsValid()) {
TCHAR buffer[64];
_tprintf(_T("%s\n"), FormatGeoPoint(value, buffer, ARRAY_SIZE(buffer),
CoordinateFormat::DDMMSS));
} else
printf("invalid\n");
}
bool
TargetMapWindow::isClickOnTarget(const PixelPoint pc) const
{
if (task == nullptr)
return false;
ProtectedTaskManager::Lease task_manager(*task);
const GeoPoint t = task_manager->GetLocationTarget(target_index);
if (!t.IsValid())
return false;
const GeoPoint gp = projection.ScreenToGeo(pc.x, pc.y);
if (projection.GeoToScreenDistance(gp.DistanceS(t)) < Layout::GetHitRadius())
return true;
return false;
}
FlatGeoPoint
RoutePolars::ReachIntercept(const int index, const AFlatGeoPoint &flat_origin,
const GeoPoint &origin,
const RasterMap* map,
const FlatProjection &proj) const
{
const bool valid = map && map->IsDefined();
const int altitude = flat_origin.altitude - GetSafetyHeight();
const FlatGeoPoint flat_dest = MSLIntercept(index, flat_origin,
altitude, proj);
if (!valid)
return flat_dest;
const GeoPoint dest = proj.Unproject(flat_dest);
const GeoPoint p = map->Intersection(origin, altitude,
altitude, dest, height_min_working);
if (!p.IsValid())
return flat_dest;
FlatGeoPoint fp = proj.ProjectInteger(p);
/* when there's an obstacle very nearby and our intersection is
right next to our origin, the intersection may be deformed due to
terrain raster rounding errors; the following code applies
clipping to avoid degenerate polygons */
FlatGeoPoint delta1 = flat_dest - (FlatGeoPoint)flat_origin;
FlatGeoPoint delta2 = fp - (FlatGeoPoint)flat_origin;
if (delta1.x * delta2.x < 0)
/* intersection is on the wrong horizontal side */
fp.x = flat_origin.x;
if (delta1.y * delta2.y < 0)
/* intersection is on the wrong vertical side */
fp.x = flat_origin.y;
return fp;
}
GeoPoint
FindLatitudeLongitude(const GeoPoint &loc, const Angle bearing,
fixed distance)
{
assert(loc.IsValid());
assert(!negative(distance));
if (!positive(distance))
return loc;
GeoPoint loc_out;
const Angle distance_angle = EarthDistanceToAngle(distance);
const auto scd = distance_angle.SinCos();
const fixed sin_distance = scd.first, cos_distance = scd.second;
const auto scb = bearing.SinCos();
const fixed sin_bearing = scb.first, cos_bearing = scb.second;
const auto scl = loc.latitude.SinCos();
const fixed sin_latitude = scl.first, cos_latitude = scl.second;
loc_out.latitude = EarthASin(SmallMult(sin_latitude, cos_distance)
+ SmallMult(cos_latitude, sin_distance,
cos_bearing));
loc_out.longitude = loc.longitude +
Angle::FromXY(cos_distance - SmallMult(sin_latitude,
loc_out.latitude.sin()),
SmallMult(sin_bearing, sin_distance, cos_latitude));
loc_out.Normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc_out;
}
bool IsValid() const {
return center.IsValid();
}
gcc_pure
bool IsValid() const {
return location.IsValid();
}
PyObject* xcsoar_Airspaces_addPolygon(Pyxcsoar_Airspaces *self, PyObject *args) {
PyObject *py_points = nullptr,
*py_name = nullptr,
*py_as_class = nullptr,
*py_base_ref = nullptr,
*py_top_ref = nullptr;
double base_alt, top_alt;
if (!PyArg_ParseTuple(args, "OOOdOdO", &py_points, &py_name, &py_as_class,
&base_alt, &py_base_ref,
&top_alt, &py_top_ref)) {
PyErr_SetString(PyExc_AttributeError, "Error reading attributes.");
return nullptr;
}
/* Parse points */
std::vector<GeoPoint> points;
if (!PySequence_Check(py_points)) {
PyErr_SetString(PyExc_ValueError, "First argument is no sequence");
return nullptr;
}
Py_ssize_t num_items = PySequence_Fast_GET_SIZE(py_points);
for (Py_ssize_t i = 0; i < num_items; ++i) {
PyObject *py_location = PySequence_Fast_GET_ITEM(py_points, i);
GeoPoint location = Python::ReadLonLat(py_location);
if (!location.IsValid()) {
if (PyErr_Occurred() == nullptr)
PyErr_SetString(PyExc_RuntimeError, "Unknown error while parsing location");
return nullptr;
}
points.push_back(location);
}
if (points.size() < 3) {
PyErr_SetString(PyExc_ValueError, "Polygon has not enough points");
return nullptr;
}
/* Parse airspace name */
tstring name;
if (!Python::PyStringToString(py_name, name)) {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace name.");
return nullptr;
}
/* Parse airspace class */
tstring as_class;
AirspaceClass type = AirspaceClass::OTHER;
if (!Python::PyStringToString(py_as_class, as_class)) {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace class.");
return nullptr;
}
for (unsigned i = 0; i < ARRAY_SIZE(airspace_class_strings); i++) {
if (as_class.compare(airspace_class_strings[i].string) == 0)
type = airspace_class_strings[i].type;
}
/* Parse airspace base and top */
tstring base_ref, top_ref;
AirspaceAltitude base, top;
if (!Python::PyStringToString(py_base_ref, base_ref)) {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace base reference.");
return nullptr;
}
if (!Python::PyStringToString(py_top_ref, top_ref)) {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace top reference.");
return nullptr;
}
if (base_ref.compare("MSL") == 0) {
base.reference = AltitudeReference::MSL;
base.altitude = base_alt;
} else if (base_ref.compare("FL") == 0) {
base.reference = AltitudeReference::STD;
base.flight_level = base_alt;
} else if (base_ref.compare("AGL") == 0) {
base.reference = AltitudeReference::AGL;
base.altitude_above_terrain = base_alt;
} else {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace base.");
return nullptr;
}
if (top_ref.compare("MSL") == 0) {
top.reference = AltitudeReference::MSL;
top.altitude = top_alt;
} else if (top_ref.compare("FL") == 0) {
top.reference = AltitudeReference::STD;
top.flight_level = top_alt;
} else if (top_ref.compare("AGL") == 0) {
top.reference = AltitudeReference::AGL;
top.altitude_above_terrain = top_alt;
} else {
PyErr_SetString(PyExc_ValueError, "Can't parse airspace top.");
return nullptr;
}
/* Create airspace and save it into the database */
AbstractAirspace *as = new AirspacePolygon(points);
as->SetProperties(std::move(name), type, base, top);
self->airspace_database->Add(as);
Py_RETURN_NONE;
}
bool IsValid() const {
return geo_location.IsValid();
}
