- double user_latitude_rad = _user_latitude_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
- double user_longitude_rad = _user_longitude_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
-
- double earth_rad_m = _earth_radius_node->getDoubleValue() * SG_FEET_TO_METER;
- if( earth_rad_m < SG_EPSILON )
- earth_rad_m = SG_EARTH_RAD * 1000;
-
- // Placing the probes
-
- for (int i = 0; i < sizeof(probe_lat_rad)/sizeof(probe_lat_rad[0]); i++) {
- double probe_radius_ratio = dist_probe_m[i]/earth_rad_m;
- double sin_probe_radius_ratio = sin(probe_radius_ratio);
-
- probe_lat_rad[i] = asin(sin(user_latitude_rad)*cos(probe_radius_ratio)
- +cos(user_latitude_rad)*sin_probe_radius_ratio*cos(ground_wind_from_rad));
- if (probe_lat_rad[i] < SG_EPSILON ) {
- probe_lon_rad[i] = user_latitude_rad; // probe on a pole
- } else {
- probe_lon_rad[i] = fmod((user_longitude_rad+asin(sin(ground_wind_from_rad)
- *sin_probe_radius_ratio/cos(probe_lat_rad[i]))+SG_PI)
- ,SGD_2PI)-SG_PI;
- }
- probe_lat_deg[i]= probe_lat_rad[i] * SG_RADIANS_TO_DEGREES;
- probe_lon_deg[i]= probe_lon_rad[i] * SG_RADIANS_TO_DEGREES;
- }
-
- for (int i = 0; i < sizeof(probe_elev_m)/sizeof(probe_elev_m[0]); i++) {
- double elevation = 0;
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(
- SGGeod::fromRad(probe_lon_rad[i],probe_lat_rad[i]), 20000), elevation, 0)) {
- if ( elevation > 0.1 ) {
- probe_elev_m[i] = elevation;
- } else {
- probe_elev_m[i] = 0.1 ;
- }
- } else {
- probe_elev_m[i] = 0.1;
+ // probe0 is current position
+ probe_lat_deg[0] = _user_latitude_node->getDoubleValue();
+ probe_lon_deg[0] = _user_longitude_node->getDoubleValue();
+ probe_elev_m[0] = _ground_elev_node->getDoubleValue() * SG_FEET_TO_METER;
+
+ // position is geodetic, need geocentric for advanceRadM
+ SGGeod myGeodPos = SGGeod::fromDegM( probe_lon_deg[0], probe_lat_deg[0], 20000.0 );
+ SGGeoc myGeocPos = SGGeoc::fromGeod( myGeodPos );
+ double ground_wind_from_rad = _surface_wind_from_deg_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
+
+ // compute the remaining probes
+ for (unsigned i = 1; i < sizeof(probe_elev_m)/sizeof(probe_elev_m[0]); i++) {
+ SGGeoc probe = myGeocPos.advanceRadM( ground_wind_from_rad, dist_probe_m[i] );
+ // convert to geodetic position for ground level computation
+ SGGeod probeGeod = SGGeod::fromGeoc( probe );
+ probe_lat_deg[i] = probeGeod.getLatitudeDeg();
+ probe_lon_deg[i] = probeGeod.getLongitudeDeg();
+ if (!globals->get_scenery()->get_elevation_m( probeGeod, probe_elev_m[i], NULL )) {
+ // no ground found? use elevation of previous probe :-(
+ probe_elev_m[i] = probe_elev_m[i-1];