Esempio n. 1
0
void
ScreenClosestPoint(const RasterPoint &p1, const RasterPoint &p2,
                   const RasterPoint &p3, RasterPoint *p4, int offset)
{
  int v12x, v12y, v13x, v13y;

  v12x = p2.x - p1.x;
  v12y = p2.y - p1.y;
  v13x = p3.x - p1.x;
  v13y = p3.y - p1.y;

  int mag12 = isqrt4(v12x * v12x + v12y * v12y);
  if (mag12 > 1) {
    // projection of v13 along v12 = v12.v13/|v12|
    int proj = (v12x * v13x + v12y * v13y) / mag12;
    // fractional distance
    double f;
    if (offset > 0) {
      if (offset * 2 < mag12) {
        proj = max(0, min(proj, mag12));
        proj = max(offset, min(mag12 - offset, proj + offset));
      } else {
        proj = mag12 / 2;
      }
    }
    f = min(1.0, max(0.0, (double)proj / mag12));

    // location of 'closest' point
    p4->x = lround(v12x * f) + p1.x;
    p4->y = lround(v12y * f) + p1.y;
  } else {
    p4->x = p1.x;
    p4->y = p1.y;
  }
}
Esempio n. 2
0
unsigned Distance(const POINT &p1, const POINT &p2)
{
  POINT d = p1;
  d.x-= p2.x;
  d.y-= p2.y;
  return isqrt4(d.x*d.x+d.y*d.y);
}
Esempio n. 3
0
void
ScreenClosestPoint(const RasterPoint &p1, const RasterPoint &p2,
                   const RasterPoint &p3, RasterPoint *p4, int offset)
{
  int v12x, v12y, v13x, v13y;

  v12x = p2.x - p1.x;
  v12y = p2.y - p1.y;
  v13x = p3.x - p1.x;
  v13y = p3.y - p1.y;

  const int mag = v12x * v12x + v12y * v12y;
  if (mag > 1) {
    const int mag12 = isqrt4(mag);
    // projection of v13 along v12 = v12.v13/|v12|
    int proj = (v12x * v13x + v12y * v13y) / mag12;
    // fractional distance
    if (offset > 0) {
      if (offset * 2 < mag12) {
        proj = std::max(0, std::min(proj, mag12));
        proj = std::max(offset, std::min(mag12 - offset, proj + offset));
      } else {
        proj = mag12 / 2;
      }
    }

    const fixed f = Clamp(fixed(proj) / mag12, fixed(0), fixed(1));
    // location of 'closest' point
    p4->x = iround(v12x * f) + p1.x;
    p4->y = iround(v12y * f) + p1.y;
  } else {
    p4->x = p1.x;
    p4->y = p1.y;
  }
}
Esempio n. 4
0
unsigned
FlatBoundingBox::distance(const FlatBoundingBox &f) const {
    long dx = max(0,min(f.bb_ll.Longitude-bb_ur.Longitude,
                        bb_ll.Longitude-f.bb_ur.Longitude));
    long dy = max(0,min(f.bb_ll.Latitude-bb_ur.Latitude,
                        bb_ll.Latitude-f.bb_ur.Latitude));
    return isqrt4(dx*dx+dy*dy);
}
Esempio n. 5
0
 int EstimateW1(int V_west_x, int V_west_y) {
   int v_tas_x = V_gps_x+V_west_x;
   int v_tas_y = V_gps_y+V_west_y;
   long vv = isqrt4(v_tas_x*v_tas_x+v_tas_y*v_tas_y);
   long vdiff = (long)V_tas_l-vv;
   int err = (1000*max(vdiff, -vdiff))/V_tas_l;
   // returns error in tenths of percent
   return err;
 }
Esempio n. 6
0
int CalculateWaypointApproxDistance(int scx_aircraft, int scy_aircraft,
                                    int i) {

  // Do preliminary fast search, by converting to screen coordinates
  int sc_x, sc_y;
  LatLon2Flat(WayPointList[i].Longitude, 
              WayPointList[i].Latitude, &sc_x, &sc_y);
  int dx, dy;
  dx = scx_aircraft-sc_x;
  dy = scy_aircraft-sc_y;

  return isqrt4(dx*dx+dy*dy);
}
Esempio n. 7
0
bool DoTarget(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
   double x0, y0, etas, ttgo=0;
   double bearing, distance;

   if (LKTargetIndex<0 || LKTargetIndex>=MAXTRAFFIC) return false;

   // DoTarget is called from MapWindow, in real time. We have enough CPU power there now

   #if 0
   if (  LastDoTarget > Basic->Time ) LastDoTarget=Basic->Time;
   // We calculate in real time, because PFLAA sentences are calculated too in real time
   if ( Basic->Time < (LastDoTarget+3.0) ) { 
	return false;
   }
   LastDoTarget=Basic->Time;
   #endif

   #ifdef DEBUG_LKT
   StartupStore(_T("... DoTarget Copy LKTraffic\n"));
   #endif
   //LockFlightData();
   memcpy(LKTraffic, Basic->FLARM_Traffic, sizeof(LKTraffic));
   //UnlockFlightData();

   if (LKTARG.ID <=0) return false;

   DistanceBearing(Basic->Latitude, Basic->Longitude, 
		LKTARG.Latitude, LKTARG.Longitude,
		&distance, &bearing);
   LKTARG.Distance=distance;
   LKTARG.Bearing=bearing;

   if (LKTARG.Speed>1) {
	x0 = fastsine(LKTARG.TrackBearing)*LKTARG.Speed;
	y0 = fastcosine(LKTARG.TrackBearing)*LKTARG.Speed;
	x0 += fastsine(Calculated->WindBearing)*Calculated->WindSpeed;
	y0 += fastcosine(Calculated->WindBearing)*Calculated->WindSpeed;
	// LKTARG.Heading = AngleLimit360(atan2(x0,y0)*RAD_TLK_DEG); // 101210 check
	etas = isqrt4((unsigned long)(x0*x0*100+y0*y0*100))/10.0;
	LKASSERT(AirDensityRatio(LKTARG.Altitude)!=0);
	LKTARG.EIAS = etas/AirDensityRatio(LKTARG.Altitude);
   } else {
	LKTARG.EIAS=0;
   }


   //double height_above_target = Calculated->NavAltitude - LKTARG.Altitude;

   // We DONT use EnergyHeight here because we are not considering the Target's TE either
   LKTARG.AltArriv = Calculated->NavAltitude - GlidePolar::MacCreadyAltitude(MACCREADY,
	distance,
	bearing,
	Calculated->WindSpeed, Calculated->WindBearing,
	0, 0,
	// final glide, use wind
	true, 
	&ttgo) - LKTARG.Altitude;
	
   // We CANNOT use RelativeAltitude because when ghost or zombie, it wont be updated in real time in respect
   // to OUR real position!! Lets use the last target altitude known.
   double relalt=Calculated->NavAltitude - LKTARG.Altitude;
   if (relalt==0)
	LKTARG.GR=999;
   else {
	// we need thus to invert the relative altitude
	LKTARG.GR=distance/(relalt);
   }


   return true;
}
Esempio n. 8
0
int slow_norm3(const int k, const int x, const int y, const int z) {
  const long mag = (long)x*x+(long)y*y+(long)x*x;
  return (1<<NORMALISE_BITS)*k/isqrt4(mag);
}
Esempio n. 9
0
 /**
  * Return accumulated distance.
  * Typically this expects all dimensions to be added
  * before calculating the distance.
  *
  * @return Absolute value (accumulated) distance
  */
 gcc_pure
 BBDist sqrt() const {
   return BBDist((int)isqrt4(d));
 }
Esempio n. 10
0
void Heading(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
  double x0, y0, mag=0;
  static double LastTime = 0;
  static double lastHeading = 0;
  static double lastSpeed = 0;

  if (DoInit[MDI_HEADING]) {
	LastTime = 0;
	lastHeading = 0;
	DoInit[MDI_HEADING]=false;
  }

  if ((Basic->Speed>0)||(Calculated->WindSpeed>0)) {

    x0 = fastsine(Basic->TrackBearing)*Basic->Speed;
    y0 = fastcosine(Basic->TrackBearing)*Basic->Speed;
    x0 += fastsine(Calculated->WindBearing)*Calculated->WindSpeed;
    y0 += fastcosine(Calculated->WindBearing)*Calculated->WindSpeed;

    Calculated->Heading = AngleLimit360(atan2(x0,y0)*RAD_TO_DEG);

    if (!Calculated->Flying) {
      // don't take wind into account when on ground
      Calculated->Heading = Basic->TrackBearing;
    }

    // calculate turn rate in wind coordinates
    if(Basic->Time > LastTime) {
      double dT = Basic->Time - LastTime;

      LKASSERT(dT!=0);
      Calculated->TurnRateWind = AngleLimit180(Calculated->Heading
                                               - lastHeading)/dT;

      lastHeading = Calculated->Heading;
    }

    if (ISCAR) {
	// On ground, TAS is GS. Wind gradient irrilevant, normally.
	Calculated->TrueAirspeedEstimated = Basic->Speed;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = Basic->Speed/AirDensityRatio(Calculated->NavAltitude);
    } else {
	// calculate estimated true airspeed
	mag = isqrt4((unsigned long)(x0*x0*100+y0*y0*100))/10.0;
	Calculated->TrueAirspeedEstimated = mag;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = mag/AirDensityRatio(Calculated->NavAltitude);
    }
    // estimate bank angle (assuming balanced turn)
    double angle = atan(DEG_TO_RAD*Calculated->TurnRateWind*
			Calculated->TrueAirspeedEstimated/9.81);

    Calculated->BankAngle = RAD_TO_DEG*angle;

    if (ISCAR) {
	if(Basic->Time > LastTime) {
		Calculated->Gload = ((Basic->Speed - lastSpeed) / (Basic->Time-LastTime))/9.81;
		lastSpeed=Basic->Speed;
	} else {
		Calculated->Gload = 0;
	}
    } else {
	Calculated->Gload = 1.0/max(0.001,fabs(cos(angle)));
    }

    LastTime = Basic->Time;

    // estimate pitch angle (assuming balanced turn)
/*
    Calculated->PitchAngle = RAD_TO_DEG*
      atan2(Calculated->GPSVario-Calculated->Vario,
           Calculated->TrueAirspeedEstimated);
*/
	// should be used as here only when no real vario available
    Calculated->PitchAngle = RAD_TO_DEG*	
      atan2(Calculated->Vario,
           Calculated->TrueAirspeedEstimated);

    // update zigzag wind
    if (  ((AutoWindMode==D_AUTOWIND_ZIGZAG) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG))
        && (!ReplayLogger::IsEnabled()) ) {
      double zz_wind_speed;
      double zz_wind_bearing;
      int quality=0;
      quality = WindKalmanUpdate(Basic, Calculated, &zz_wind_speed, &zz_wind_bearing);

      if (quality>0) {

        SetWindEstimate(zz_wind_speed, zz_wind_bearing);
	Calculated->WindSpeed   = zz_wind_speed;
	Calculated->WindBearing = zz_wind_bearing;

/* 100118 redundant!! removed. TOCHECK *
        Vector v_wind;
        v_wind.x = zz_wind_speed*cos(zz_wind_bearing*3.1415926/180.0);
        v_wind.y = zz_wind_speed*sin(zz_wind_bearing*3.1415926/180.0);
        LockFlightData();
        if (windanalyser) {
	  windanalyser->slot_newEstimate(Basic, Calculated, v_wind, quality);
        }
        UnlockFlightData();
*/
      }
    }
  // else basic speed is 0 and there is no wind.. 
  } else { 
    Calculated->Heading = Basic->TrackBearing;
    Calculated->TrueAirspeedEstimated = 0; // BUGFIX 100318
    Calculated->IndicatedAirspeedEstimated = 0; // BUGFIX 100318
  }

}
Esempio n. 11
0
rectObj MapWindow::CalculateScreenBounds(double scale, const RECT& rc) {
    // compute lat lon extents of visible screen
    rectObj sb;

    if (scale>= 1.0) {
        POINT screen_center;
        LatLon2Screen(PanLongitude,
                      PanLatitude,
                      screen_center);

        sb.minx = sb.maxx = PanLongitude;
        sb.miny = sb.maxy = PanLatitude;

        unsigned int dx, dy;
        unsigned int maxsc=0;
        dx = screen_center.x-rc.right;
        dy = screen_center.y-rc.top;
        maxsc = max(maxsc, dx*dx+dy*dy);
        dx = screen_center.x-rc.left;
        dy = screen_center.y-rc.top;
        maxsc = max(maxsc, dx*dx+dy*dy);
        dx = screen_center.x-rc.left;
        dy = screen_center.y-rc.bottom;
        maxsc = max(maxsc, dx*dx+dy*dy);
        dx = screen_center.x-rc.right;
        dy = screen_center.y-rc.bottom;
        maxsc = max(maxsc, dx*dx+dy*dy);
        maxsc = isqrt4(maxsc);

        for (int i=0; i<10; i++) {
            double ang = i*360.0/10;
            POINT p;
            double X, Y;
            p.x = screen_center.x + iround(fastcosine(ang)*maxsc*scale);
            p.y = screen_center.y + iround(fastsine(ang)*maxsc*scale);
            Screen2LatLon(p.x, p.y, X, Y);
            sb.minx = min(X, sb.minx);
            sb.miny = min(Y, sb.miny);
            sb.maxx = max(X, sb.maxx);
            sb.maxy = max(Y, sb.maxy);
        }

    } else {

        double xmin, xmax, ymin, ymax;
        int x, y;
        double X, Y;

        x = rc.left;
        y = rc.top;
        Screen2LatLon(x, y, X, Y);
        xmin = X;
        xmax = X;
        ymin = Y;
        ymax = Y;

        x = rc.right;
        y = rc.top;
        Screen2LatLon(x, y, X, Y);
        xmin = min(xmin, X);
        xmax = max(xmax, X);
        ymin = min(ymin, Y);
        ymax = max(ymax, Y);

        x = rc.right;
        y = rc.bottom;
        Screen2LatLon(x, y, X, Y);
        xmin = min(xmin, X);
        xmax = max(xmax, X);
        ymin = min(ymin, Y);
        ymax = max(ymax, Y);

        x = rc.left;
        y = rc.bottom;
        Screen2LatLon(x, y, X, Y);
        xmin = min(xmin, X);
        xmax = max(xmax, X);
        ymin = min(ymin, Y);
        ymax = max(ymax, Y);


        sb.minx = xmin;
        sb.maxx = xmax;
        sb.miny = ymin;
        sb.maxy = ymax;

    }

    return sb;
}