gcc_pure
static fixed
SimpleDistance(const GeoPoint &a, const GeoPoint &b,
const fixed latitude_cos)
{
return TinyHypot((a.longitude - b.longitude).AsDelta().Native(),
(a.latitude - b.latitude).AsDelta().Native() * latitude_cos);
}
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;
}
static void
TestTinyHypot()
{
for (unsigned i = 0; i < ARRAY_SIZE(tiny_hypot_test_values); i++) {
double dx = tiny_hypot_test_values[i][0];
double dy = tiny_hypot_test_values[i][1];
double d = hypot(dx, dy);
fixed fdx(dx);
fixed fdy(dy);
fixed fd(TinyHypot(fdx, fdy));
ok(fabs(fd - fixed(d)) < fixed(1.0e-3), "hypot(dx, dy)", 0);
}
}
