C++ DistanceBearing函数代码示例

// accurate method
int RasterMap::GetEffectivePixelSize(double *pixel_D,
                                     double latitude, double longitude)
{
  double terrain_step_x, terrain_step_y;
  double step_size = TerrainInfo.StepSize*sqrt(2.0);

  if ((*pixel_D<=0) || (step_size==0)) {
    *pixel_D = 1.0;
    return 1;
  }

  // how many steps are in the pixel size
  DistanceBearing(latitude, longitude, latitude+step_size, 
                  longitude, &terrain_step_x, NULL);
  terrain_step_x = fabs(terrain_step_x);

  DistanceBearing(latitude, longitude, latitude, 
                  longitude+step_size, &terrain_step_y, NULL);
  terrain_step_y = fabs(terrain_step_y);

  double rfact = max(terrain_step_x,terrain_step_y)/(*pixel_D);

  int epx = (int)(max(1.0,ceil(rfact)));
  //  *pixel_D = (*pixel_D)*rfact/epx;

  return epx;
}

double ScreenProjection::GetPixelSize() const {
    double lon0, lat0, lon1, lat1, dlon, dlat;
    
    Screen2LonLat(_Origin, lon0, lat0);

    Screen2LonLat({_Origin.x+1,_Origin.y}, lon1, lat1);
    DistanceBearing(lat0, lon0, lat1, lon1, &dlon, NULL);

    Screen2LonLat({ _Origin.x, _Origin.y+1 }, lon1, lat1);
    DistanceBearing(lat0, lon0, lat1, lon1, &dlat, NULL);

    return std::min(dlon, dlat);
}

// This is slow, careful!
int FindNearestWayPoint(double X, double Y, double MaxRange,
                        bool exhaustive)
{
  unsigned int i;
  int NearestIndex = -1;
  double NearestDistance, Dist;

  NearestDistance = MaxRange;

    for(i=RESWP_FIRST_MARKER;i<NumberOfWayPoints;i++) {

      // Consider only valid markers
      if ( (i<NUMRESWP)  &&  (WayPointCalc[i].WpType!=WPT_TURNPOINT) ) continue;

      DistanceBearing(Y,X,
                      WayPointList[i].Latitude, 
                      WayPointList[i].Longitude, &Dist, NULL);
      if(Dist < NearestDistance) {
        NearestIndex = i;
        NearestDistance = Dist;
      }
    }
   if(NearestIndex == -1) {
       return -1;
   }
   
	// now look at TAKEOFF... TODO check all virtuals too
	// Takeoff can be normally very closed to actual airport, but not the same point!
	DistanceBearing(Y,X, WayPointList[RESWP_TAKEOFF].Latitude, WayPointList[RESWP_TAKEOFF].Longitude, &Dist, NULL);
	if ( Dist<=NearestDistance ) {
		// takeoff is closer, and next wp is not even visible...maybe because of zoom
		if  (NearestIndex >RESWP_TAKEOFF) { //  100227 BUGFIX
			if ( WayPointList[NearestIndex].Visible == FALSE ) {
				NearestIndex = RESWP_TAKEOFF;
				NearestDistance = Dist;
			}
		} else { // else ok 100227
			NearestIndex = RESWP_TAKEOFF;
			NearestDistance = Dist;
		}
	}

  if(NearestDistance < MaxRange) {
    return NearestIndex;
  } else {
    return -1;
  }
}

void AATDistance::ShiftTargetOutside(double longitude, double latitude,
                                    int taskwaypoint) {
  // if no improvement possible, vector to outside
  double bearing;

  if (taskwaypoint>0) {
    DistanceBearing(latitude,
                    longitude,
                    WayPointList[Task[taskwaypoint+1].Index].Latitude,
                    WayPointList[Task[taskwaypoint+1].Index].Longitude,
                    NULL, &bearing);

    FindLatitudeLongitude(latitude, longitude,
                          bearing, 100.0,
                          &Task[taskwaypoint].AATTargetLat,
                          &Task[taskwaypoint].AATTargetLon);

    UpdateTargetAltitude(Task[taskwaypoint]);
    TargetModified = true;

  }

  //JMWAAT  Task[taskwaypoint].AATTargetOffsetRadial = bearing;

  // Move previous target to location that yields longest distance,
  // plus a little so optimal path vector points to next waypoint.
}

bool InAATTurnSector(const double longitude, const double latitude,
                    const int the_turnpoint)
{
  double AircraftBearing;
  bool retval = false;

  if (!ValidTaskPoint(the_turnpoint)) {
    return false;
  }

  double distance;
  LockTaskData();
  DistanceBearing(WayPointList[Task[the_turnpoint].Index].Latitude,
                  WayPointList[Task[the_turnpoint].Index].Longitude,
                  latitude,
                  longitude,
                  &distance, &AircraftBearing);

  if(Task[the_turnpoint].AATType ==  CIRCLE) {
    if(distance < Task[the_turnpoint].AATCircleRadius) {
      retval = true;
    }
  } else if(distance < Task[the_turnpoint].AATSectorRadius) {
    if (AngleInRange(Task[the_turnpoint].AATStartRadial,
                     Task[the_turnpoint].AATFinishRadial,
                     AngleLimit360(AircraftBearing), true)) {
      retval = true;
    }
  }

  UnlockTaskData();
  return retval;
}

/**
 * Calculates the bearing between two locations
 * @param loc1 Location 1
 * @param loc2 Location 2
 * @return The bearing
 */
Angle
Bearing(GeoPoint loc1, GeoPoint loc2)
{
  Angle retval;
  DistanceBearing(loc1, loc2, NULL, &retval);
  return retval;
}

Angle
Bearing(const GeoPoint &loc1, const GeoPoint &loc2)
{
  Angle bearing;
  DistanceBearing(loc1, loc2, nullptr, &bearing);
  return bearing;
}

int SearchStation(double Freq)
{
int i;
TCHAR	szFreq[8] ;
_stprintf(szFreq,  _T("%7.3f"),Freq);
	double minDist =9999999;
	int minIdx=0;
  //  LKASSERT(numvalidwp<=NumberOfWayPoints);
	double fDist, fBear;
	for (i=0; i<(int)WayPointList.size(); i++)
	{
                      LKASSERT(ValidWayPointFast(i));
	 //   LKASSERT(numvalidwp<=NumberOfWayPoints);

	    if (WayPointList[i].Latitude!=RESWP_INVALIDNUMBER)
	    {

	      DistanceBearing(GPS_INFO.Latitude,
	                      GPS_INFO.Longitude,
	                      WayPointList[i].Latitude,
	                      WayPointList[i].Longitude,
	                      &fDist,
	                      &fBear);
	      if(fabs(Freq -   StrToDouble(WayPointList[i].Freq,NULL)) < 0.001)
	        if(fDist < minDist)
	        {
	    	  minDist = fDist;
	    	  minIdx =i;
	        }
	    }
	}

	return minIdx;
}

/**
 * Calculates the distance between two locations
 * @param loc1 Location 1
 * @param loc2 Location 2
 * @return The distance
 */
fixed
Distance(GeoPoint loc1, GeoPoint loc2)
{
  fixed retval;
  DistanceBearing(loc1, loc2, &retval, NULL);
  return retval;
}

fixed
Distance(const GeoPoint &loc1, const GeoPoint &loc2)
{
  fixed distance;
  DistanceBearing(loc1, loc2, &distance, nullptr);
  return distance;
}

void
MapWindow::DrawProjectedTrack(Canvas &canvas)
{
  if (task == NULL || !task->Valid() || !task->getSettings().AATEnabled ||
      task->getActiveIndex() ==0)
    return;

  if (Calculated().Circling || task->TaskIsTemporary()) {
    // don't display in various modes
    return;
  }

  // TODO feature: maybe have this work even if no task?
  // TODO feature: draw this also when in target pan mode

  GEOPOINT start = Basic().Location;
  GEOPOINT previous_loc = task->getTargetLocation(task->getActiveIndex() - 1);

  double distance_from_previous, bearing;
  DistanceBearing(previous_loc, start,
		  &distance_from_previous,
		  &bearing);

  if (distance_from_previous < 100.0) {
    bearing = Basic().TrackBearing;
    // too short to have valid data
  }
  POINT pt[2] = {{0,-75},{0,-400}};
  if (SettingsMap().TargetPan) {
    double screen_range = GetScreenDistanceMeters();
    double f_low = 0.4;
    double f_high = 1.5;
    screen_range = max(screen_range, Calculated().WaypointDistance);

    GEOPOINT p1, p2;
    FindLatitudeLongitude(start,
			  bearing, f_low*screen_range,
			  &p1);
    FindLatitudeLongitude(start,
			  bearing, f_high*screen_range,
			  &p2);
    LonLat2Screen(p1, pt[0]);
    LonLat2Screen(p2, pt[1]);
  } else if (fabs(bearing-Calculated().WaypointBearing)<10) {
    // too small an error to bother
    return;
  } else {
    pt[1].y = (long)(-max(MapRectBig.right-MapRectBig.left,
			  MapRectBig.bottom-MapRectBig.top)*1.2);
    PolygonRotateShift(pt, 2, Orig_Aircraft.x, Orig_Aircraft.y,
		       bearing-DisplayAngle);
  }

  Pen dash_pen(Pen::DASH, IBLSCALE(2), Color(0, 0, 0));
  canvas.select(dash_pen);
  canvas.line(pt[0], pt[1]);
}

double ProjectedDistance(GEOPOINT loc1, GEOPOINT loc2, GEOPOINT loc3)
{
  GEOPOINT loc4;

  CrossTrackError(loc1, loc2, loc3, &loc4);
  double tmpd;
  DistanceBearing(loc1, loc4, &tmpd, NULL);
  return tmpd;
}

// finds cross track error in meters and closest point p4 between p3 and
// desired track p1-p2.
// very slow function!
double CrossTrackError(double lon1, double lat1,
                       double lon2, double lat2,
                       double lon3, double lat3,
                       double *lon4, double *lat4) {

  double dist_AD, crs_AD;
  DistanceBearing(lat1, lon1, lat3, lon3, &dist_AD, &crs_AD);
  dist_AD/= (RAD_TO_DEG * 111194.9267); crs_AD*= DEG_TO_RAD;

  double dist_AB, crs_AB;
  DistanceBearing(lat1, lon1, lat2, lon2, &dist_AB, &crs_AB);
  dist_AB/= (RAD_TO_DEG * 111194.9267); crs_AB*= DEG_TO_RAD;

  lat1 *= DEG_TO_RAD;
  lat2 *= DEG_TO_RAD;
  lat3 *= DEG_TO_RAD;
  lon1 *= DEG_TO_RAD;
  lon2 *= DEG_TO_RAD;
  lon3 *= DEG_TO_RAD;

  double XTD; // cross track distance
  double ATD; // along track distance
  //  The "along track distance", ATD, the distance from A along the
  //  course towards B to the point abeam D

  double sindist_AD = sin(dist_AD);

  XTD = asin(sindist_AD*sin(crs_AD-crs_AB));

  double sinXTD = sin(XTD);
  ATD = asin(sqrt( sindist_AD*sindist_AD - sinXTD*sinXTD )/cos(XTD));
  
  if (lon4 && lat4) {
    IntermediatePoint(lon1, lat1, lon2, lat2, ATD, dist_AB,
		      lon4, lat4);
  }

  // units
  XTD *= (RAD_TO_DEG * 111194.9267);

  return XTD;
}

  double GetSpeed(double time) {
    if (p[1].t == p[0].t ) return 0.0;
    if (p[2].t == p[1].t ) return 0.0;
    if (Ready()) {
      double u= (time-p[1].t)/(p[2].t-p[1].t);
      double s0;
      DistanceBearing(p[0].lat, p[0].lon,
                      p[1].lat, p[1].lon, &s0, NULL);

      s0/= (p[1].t-p[0].t);
      double s1;
      DistanceBearing(p[1].lat, p[1].lon,
                      p[2].lat, p[2].lon, &s1, NULL);
      s1/= (p[2].t-p[1].t);
      u = max(0.0,min(1.0,u));
      return s1*u+s0*(1.0-u);
    } else {
      return 0.0;
    }
  }

    void UpdateToStartSector() {
        if (mIdx > 0) {
            const WAYPOINT *CurrPt = TaskWayPoint(mIdx);
            const WAYPOINT *StartPt = TaskWayPoint(0);
            // bearing to prev
            DistanceBearing(CurrPt->Latitude, CurrPt->Longitude,
                    StartPt->Latitude, StartPt->Longitude, NULL, &mA12);

            UpdateFixedSector();
        }
    }