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