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; } }
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); }
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; } }
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); }
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; }
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); }
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; }
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); }
/** * 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)); }
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 } }
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; }