fixed
ProjectedDistance(GeoPoint loc1, GeoPoint loc2, GeoPoint loc3)
{
Angle dist_AD; Angle crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!positive(dist_AD.value_native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
Angle dist_AB; Angle crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!positive(dist_AB.value_native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
// 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();
const fixed XTD(earth_asin(sindist_AD * (crs_AD - crs_AB).sin())); // cross track distance
fixed sinXTD, cosXTD;
sin_cos(XTD, &sinXTD, &cosXTD);
// along track distance
const fixed ATD(earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD));
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return ATD * fixed_earth_r;
}
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 fixed cross_track_distance =
earth_asin(sindist_AD * (crs_AD - crs_AB).sin());
if (loc4) {
const auto sc = sin_cos(cross_track_distance);
const fixed sinXTD = sc.first, cosXTD = sc.second;
const fixed along_track_distance =
earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD);
*loc4 = IntermediatePoint(loc1, loc2, along_track_distance, dist_AB.Radians());
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return cross_track_distance * fixed_earth_r;
}
fixed
CrossTrackError(GeoPoint loc1, GeoPoint loc2, GeoPoint loc3, GeoPoint *loc4)
{
Angle dist_AD; Angle crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
Angle dist_AB; Angle 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 fixed XTD(earth_asin(sindist_AD * (crs_AD - crs_AB).sin()));
if (loc4) {
fixed sinXTD, cosXTD;
sin_cos(XTD, &sinXTD, &cosXTD);
// along track distance
const fixed ATD(earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD)
/ cosXTD));
*loc4 = IntermediatePoint(loc1, loc2, ATD, dist_AB.value_radians());
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
// units
return XTD * fixed_earth_r;
}
/**
* Calculates the location (loc_out) you would have, after being at
* a certain start location (loc) with a certain Bearing and going straight
* forward for a certain Distance.
* @param loc Current location
* @param Bearing Current bearing
* @param Distance Distance to predict
* @param loc_out Future location
*/
GeoPoint
FindLatitudeLongitude(GeoPoint loc, Angle Bearing,
fixed Distance)
{
assert(!negative(Distance));
if (!positive(Distance))
return loc;
GeoPoint loc_out;
Distance *= fixed_inv_earth_r;
fixed sinDistance, cosDistance;
sin_cos(Distance, &sinDistance, &cosDistance);
fixed sinBearing, cosBearing;
Bearing.sin_cos(sinBearing, cosBearing);
fixed sinLatitude, cosLatitude;
loc.Latitude.sin_cos(sinLatitude, cosLatitude);
loc_out.Latitude = Angle::radians(earth_asin(sinLatitude * cosDistance + cosLatitude
* sinDistance * cosBearing));
fixed result;
if (cosLatitude == fixed_zero)
result = loc.Longitude.value_radians();
else {
result = loc.Longitude.value_radians() +
earth_asin(sinBearing * sinDistance / cosLatitude);
}
loc_out.Longitude = Angle::radians(result);
loc_out.normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc_out;
}
/**
* 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)
{
fixed cos_lat1, sin_lat1;
loc1.Latitude.sin_cos(sin_lat1, cos_lat1);
fixed cos_lat2, sin_lat2;
loc2.Latitude.sin_cos(sin_lat2, cos_lat2);
const fixed dlon = (loc2.Longitude - loc1.Longitude).value_radians();
if (Distance) {
const fixed s1 = (loc2.Latitude - loc1.Latitude).accurate_half_sin();
const fixed s2 = accurate_half_sin(dlon);
const fixed a = sqr(s1) + cos_lat1 * cos_lat2 * sqr(s2);
fixed distance2 = earth_distance_function(a);
assert(!negative(distance2));
*Distance = Angle::radians(distance2);
}
if (Bearing) {
fixed sin_dlon, cos_dlon;
// speedup for fixed since this is one call
sin_cos(dlon, &sin_dlon, &cos_dlon);
const fixed y = sin_dlon * cos_lat2;
const fixed x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*Bearing = (x == fixed_zero && y == fixed_zero)
? Angle::native(fixed_zero)
: Angle::radians(atan2(y, x)).as_bearing();
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
}
/**
* 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)
{
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 fixed dlon = (loc2.longitude - loc1.longitude).Radians();
if (distance) {
const fixed s1 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const fixed s2 = accurate_half_sin(dlon);
const fixed a = sqr(s1) + cos_lat1 * cos_lat2 * sqr(s2);
fixed distance2 = earth_distance_function(a);
assert(!negative(distance2));
*distance = Angle::Radians(distance2);
}
if (bearing) {
// speedup for fixed since this is one call
const auto sc = sin_cos(dlon);
const fixed sin_dlon = sc.first, cos_dlon = sc.second;
const fixed y = sin_dlon * cos_lat2;
const fixed x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*bearing = (x == fixed_zero && y == fixed_zero)
? Angle::Zero()
: Angle::Radians(atan2(y, x)).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_zero;
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_zero;
// 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 fixed cross_track_distance =
earth_asin(sindist_AD * (crs_AD - crs_AB).sin());
const auto sc = sin_cos(cross_track_distance);
const fixed sinXTD = sc.first, cosXTD = sc.second;
// along track distance
const fixed along_track_distance =
earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return along_track_distance * fixed_earth_r;
}
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 helpers_master (void)
{
int r, f;
printf ("\nUsing %d helpers, vector size %u, repeating %d times\n",
helpers_num, size, rep);
if (trace==2)
{ helpers_trace(1);
}
/* Compute results "rep" times, by scheduling tasks. */
for (r = 1; r<=rep; r++)
{
if (trace==1 && r==rep)
{ helpers_trace(1);
}
if (disable && r==1)
{ helpers_disable(1);
}
if (no_pipelining && r==1)
{ helpers_no_pipelining(1);
}
if (no_multithreading)
{ if (r==(rep+2)/3)
{ helpers_no_multithreading(1);
}
if (r==rep+1-(rep+2)/3)
{ helpers_no_multithreading(0);
}
}
f = 0;
if (pipeline[0]) f |= HELPERS_PIPE_OUT;
if (master_only[0]) f |= HELPERS_MASTER_ONLY;
if (master_now[0]) f |= HELPERS_MASTER_NOW;
if (wait_unused) helpers_wait_until_not_in_use(A);
if (show_markers && r==rep)
{ printf("Markers for B before step (1): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
helpers_do_task (f, pipeline[0]?sequence_piped:sequence, 0, A, NULL, NULL);
if (show_markers && r==rep)
{ printf("Markers for B after step (1): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
f = 0;
if (pipeline[1]) f |= HELPERS_PIPE_IN1_OUT;
if (master_only[1]) f |= HELPERS_MASTER_ONLY;
if (master_now[1]) f |= HELPERS_MASTER_NOW;
if (wait_unused) helpers_wait_until_not_in_use(B);
if (show_markers && r==rep)
{ printf("Markers for B before step (2): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
helpers_do_task (f, pipeline[1] ? sin_cos_piped : sin_cos, 1, B, A, NULL);
if (show_markers && r==rep)
{ printf("Markers for B after step (2): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
f = 0;
if (pipeline[2]) f |= HELPERS_PIPE_IN1_OUT;
if (master_only[2]) f |= HELPERS_MASTER_ONLY;
if (master_now[2]) f |= HELPERS_MASTER_NOW;
if (wait_unused) helpers_wait_until_not_in_use(C);
if (show_markers && r==rep)
{ printf("Markers for B before step (3): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
helpers_do_task (f, pipeline[2] ? sin_cos_piped : sin_cos, 2, C, A, NULL);
if (show_markers && r==rep)
{ printf("Markers for B after step (3): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
f = 0;
if (pipeline[3]) f |= HELPERS_PIPE_IN12;
if (master_only[3]) f |= HELPERS_MASTER_ONLY;
if (master_now[3]) f |= HELPERS_MASTER_NOW;
if (wait_unused) helpers_wait_until_not_in_use(&D);
if (show_markers && r==rep)
{ printf("Markers for B before step (4): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
helpers_do_task (f, pipeline[3] ? ave_sqr_piped : ave_sqr, 0, &D, B, C);
if (show_markers && r==rep)
{ printf("Markers for B after step (4): in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
if (get_var_list && r==rep)
{ helpers_var_ptr *list;
printf("Variable list:");
for (list = helpers_var_list(0); *list != NULL; list++)
{ printf(" %s",helpers_var_name(*list));
}
printf("\n");
}
if (C_sum_computation)
{ helpers_size_t a;
double e1, e2;
helpers_start_computing_var(C);
HELPERS_WAIT_IN_VAR (C, a, size/3, size);
e1 = C[size/3];
if (a<=size/2) HELPERS_WAIT_IN_VAR (C, a, size/2, size);
e2 = C[size/2];
if (r==rep)
{ printf ("Sum of C[%u] and C[%u] is %f\n", size/3, size/2, e1+e2);
}
}
if (idle_numbers && r==rep)
{ int c;
for (c = 0; c<20; c++)
{ printf ("Numbers of idle helpers: %d\n",helpers_idle());
for (_x_x_ = 100000; _x_x_>0; _x_x_--) ;
}
}
if (wait_master_only)
{ helpers_wait_for_all_master_only();
}
if (wait_computed)
{ helpers_wait_until_not_being_computed (&D);
}
if (!wait_computed && !wait_unused)
{ helpers_wait_for_all();
}
}
helpers_wait_for_all();
if (show_markers)
{ printf("Markers for B at end: in use %d, being computed %d\n",
B_in_use, B_being_computed);
}
if (get_var_list && r==rep && *helpers_var_list(0)!=NULL)
{ printf("Variable list isn't empty at end!\n");
}
/* Do direct computation if -D specified. */
if (do_direct)
{ printf("Computing final values directly\n");
if (pipeline[0]) sequence_piped(0,A,NULL,NULL);else sequence(0,A,NULL,NULL);
if (pipeline[1]) sin_cos_piped(1,B,A,NULL); else sin_cos(1,B,A,NULL);
if (pipeline[2]) sin_cos_piped(0,C,A,NULL); else sin_cos(2,C,A,NULL);
if (pipeline[3]) ave_sqr_piped(0,&D,B,C); else ave_sqr(0,&D,B,C);
}
/* If -v, check results of last repetition (or direct computation, if -D)
by recomputing here. Recompute in backwards order, since that makes
it more likely we'll find a bug that gets us here before the last task
has finished. */
if (verify)
{ helpers_size_t i;
double sum = 0;
i = size;
while (i > 0)
{ double t;
int j;
i -= 1;
a = exp ((double)i / size);
for (j = 0; j<slow[0]; j++) a = log(exp(a));
b = sin(a);
for (j = 0; j<slow[1]; j++) b = log(exp(b));
c = cos(a);
for (j = 0; j<slow[2]; j++) c = log(exp(c));
if (A[i] != a || B[i] != b || C[i] != c)
{ printf ("Error in value at index %u: %f %f, %f %f, %f %f\n", i,
A[i], a, B[i], b, C[i], c);
break;
}
t = b*b + c*c;
for (j = 0; j<slow[3]; j++) t = log(exp(t));
sum += t;
}
d = sum/size;
if (D != d)
{ printf ("Error in final value: %f %f\n", D, d);
}
}
/* Print final value computed. */
printf ("\nFinal value of D: %.8f\n", D);
/* Print statistics, if -s used. */
if (stats) helpers_stats();
}
