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(); }