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;
}
static void
DrawLandableRunway(Canvas &canvas, const PixelPoint &pt,
const Angle angle, double radius, double width)
{
if (radius <= 0)
return;
const auto sc = angle.SinCos();
const auto x = sc.first, y = sc.second;
int lx = iround(2 * x * radius) & ~0x1; // make it a even number
int ly = iround(2 * y * radius) & ~0x1;
int wx = iround(-y * width);
int wy = iround(x * width);
BulkPixelPoint runway[4];
runway[0].x = pt.x - (lx / 2) + (wx / 2);
runway[0].y = pt.y + (ly / 2) - (wy / 2);
runway[1].x = runway[0].x - wx;
runway[1].y = runway[0].y + wy;
runway[2].x = runway[1].x + lx;
runway[2].y = runway[1].y - ly;
runway[3].x = runway[2].x + wx;
runway[3].y = runway[2].y - wy;
canvas.DrawTriangleFan(runway, ARRAY_SIZE(runway));
}
fixed
CrossTrackError(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3, GeoPoint *loc4)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
// cross track distance
const Angle cross_track_distance =
EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));
if (loc4) {
const auto sc = cross_track_distance.SinCos();
const fixed sinXTD = sc.first, cosXTD = sc.second;
const Angle along_track_distance =
EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);
*loc4 = IntermediatePoint(loc1, loc2, along_track_distance, dist_AB);
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return AngleToEarthDistance(cross_track_distance);
}
void
FlatPoint::Rotate(const Angle angle)
{
const fixed _x = x;
const fixed _y = y;
const auto sc = angle.SinCos();
const fixed sa = sc.first, ca = sc.second;
x = _x * ca - _y * sa;
y = _x * sa + _y * ca;
}
void Rotate(const Angle alpha) {
const auto sc = alpha.SinCos();
const fixed sin = sc.first, cos = sc.second;
#ifdef FIXED_MATH
long s = sin.as_glfixed();
long c = cos.as_glfixed();
#else
long s = sin * (1<<16);
long c = cos * (1<<16);
#endif
Rotatex(s, c);
}
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;
}
FlatPoint
FlatEllipse::Parametric(const fixed t) const
{
const Angle at = (Angle::FullCircle() * t + theta_initial).AsDelta();
const auto sc = at.SinCos();
fixed sat = sc.first, cat = sc.second;
FlatPoint res(a * cat, b * sat);
res.Rotate(theta);
res.Add(p);
return res;
}
bool
FlatEllipse::IntersectExtended(const FlatPoint &pe, FlatPoint &i1,
FlatPoint &i2) const
{
const FlatLine l_f1p(f1, pe);
const FlatLine l_pf2(pe, f2);
const Angle ang = l_f1p.angle();
const fixed d = l_pf2.d() + std::max(a, b); // max line length
const auto sc = ang.SinCos();
fixed san = sc.first, can = sc.second;
FlatLine e_l(pe, FlatPoint(pe.x + d * can, pe.y + d * san));
// e_l is the line extended from p in direction of f1-p
return Intersect(e_l, i1, i2);
}
/**
* 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
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
}
fixed
ProjectedDistance(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!positive(dist_AD.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed(0);
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!positive(dist_AB.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed(0);
// The "along track distance", along_track_distance, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
const Angle cross_track_distance =
EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));
const auto sc = cross_track_distance.SinCos();
const fixed sinXTD = sc.first, cosXTD = sc.second;
// along track distance
const Angle along_track_distance =
EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return AngleToEarthDistance(along_track_distance);
}
GeoPoint
FindLatitudeLongitude(const GeoPoint loc, const Angle bearing,
fixed distance)
{
assert(!negative(distance));
if (!positive(distance))
return loc;
GeoPoint loc_out;
distance *= fixed_inv_earth_r;
const auto scd = sin_cos(distance);
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 = Angle::Radians(earth_asin(
sin_latitude * cos_distance + cos_latitude * sin_distance * cos_bearing));
fixed result = loc.longitude.Radians();
if (cos_latitude != fixed_zero)
result += earth_asin(sin_bearing * sin_distance / cos_latitude);
loc_out.longitude = Angle::Radians(result);
loc_out.Normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc_out;
}
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();
}
}
Vector(Angle bearing, fixed norm) {
auto sc = bearing.SinCos();
x = sc.second * norm;
y = sc.first * norm;
}
ZZX(const fixed mag, const Angle ang) {
const auto sc = ang.SinCos();
gps_east = sc.first * mag;
gps_north = sc.second * mag;
}