void PolygonRotateShift(RasterPoint *poly, const int n, const PixelScalar xs, const PixelScalar ys, Angle angle, const int scale) { static Angle lastangle = Angle::Native(fixed(-1)); static int cost = 1024, sint = 0; static int last_scale = 0; angle = angle.AsBearing(); if ((angle != lastangle) || (last_scale != scale)) { lastangle = angle; last_scale = scale; if (scale == 100) { cost = FastScale(angle.ifastcosine()); sint = FastScale(angle.ifastsine()); } else { cost = FastScale(angle.ifastcosine() * scale) / 100; sint = FastScale(angle.ifastsine() * scale) / 100; } } RasterPoint *p = poly; const RasterPoint *pe = poly + n; while (p < pe) { int x = p->x; int y = p->y; p->x = roundshift(x * cost - y * sint) + xs; p->y = roundshift(y * cost + x * sint) + ys; p++; } }
/** * Shifts and rotates the given polygon and also sizes it via FastScale() * @param poly Points specifying the polygon * @param n Number of points of the polygon * @param xs Pixels to shift in the x-direction * @param ys Pixels to shift in the y-direction * @param angle Angle of rotation */ void PolygonRotateShift(RasterPoint *poly, const int n, const int xs, const int ys, Angle angle, const bool scale) { static Angle lastangle = Angle::native(-fixed_one); static int cost = 1024, sint = 0; static bool last_scale = false; angle = angle.as_bearing(); if ((angle != lastangle) || (last_scale != scale)) { lastangle = angle; last_scale = scale; if (scale) { cost = Layout::FastScale(angle.ifastcosine()); sint = Layout::FastScale(angle.ifastsine()); } else { cost = Layout::FastScale(angle.ifastcosine()/2); sint = Layout::FastScale(angle.ifastsine()/2); } } RasterPoint *p = poly; const RasterPoint *pe = poly + n; while (p < pe) { int x = p->x; int y = p->y; p->x = roundshift(x * cost - y * sint ) + xs; p->y = roundshift(y * cost + x * sint ) + ys; p++; } }
/** * This function creates some simulated traffic for FLARM debugging * @param GPS_INFO Pointer to the NMEA_INFO struct */ void NMEAParser::TestRoutine(NMEA_INFO *GPS_INFO) { static int i = 90; i++; if (i > 255) i = 0; if (i > 80) return; const Angle angle = Angle::degrees(fixed((i * 360) / 255)).as_bearing(); // PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>, // <RelativeVertical>,<RelativeDistance>(,<ID>) int h1; int n1; int e1; int t1; unsigned l; h1 = (angle.ifastsine()) / 7; n1 = (angle.ifastsine()) / 2 - 200; e1 = (angle.ifastcosine()) / 1.5; t1 = -angle.as_bearing().value_degrees(); l = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1)); int h2; int n2; int e2; int t2; Angle dangle = (angle + Angle::degrees(fixed(120))).as_bearing(); Angle hangle = dangle; hangle.flip(); hangle = hangle.as_bearing(); h2 = (angle.ifastcosine()) / 10; n2 = (dangle.ifastsine()) / 1.20 + 300; e2 = (dangle.ifastcosine()) + 500; t2 = hangle.value_degrees(); // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> char t_laa1[50]; sprintf(t_laa1, "%d,%d,%d,%d,2,DDA85C,%d,0,0,0,1", l, n1, e1, h1, t1); char t_laa2[50]; sprintf(t_laa2, "0,%d,%d,%d,2,AA9146,%d,0,0,0,1", n2, e2, h2, t2); char t_lau[50]; sprintf(t_lau, "2,1,2,1,%d", l); GPS_INFO->flarm.FLARM_Available = true; NMEAInputLine line(t_lau); PFLAU(line, GPS_INFO->flarm); line = NMEAInputLine(t_laa1); PFLAA(line, GPS_INFO); line = NMEAInputLine(t_laa2); PFLAA(line, GPS_INFO); }
/** * This function creates some simulated traffic for FLARM debugging * @param GPS_INFO Pointer to the NMEA_INFO struct */ void Simulator::GenerateFLARMTraffic(NMEAInfo &basic) { static int i = 90; i++; if (i > 255) i = 0; if (i > 80) return; const Angle angle = Angle::degrees(fixed((i * 360) / 255)).as_bearing(); Angle dangle = (angle + Angle::degrees(fixed(120))).as_bearing(); Angle hangle = dangle.flipped().as_bearing(); int alt = (angle.ifastsine()) / 7; int north = (angle.ifastsine()) / 2 - 200; int east = (angle.ifastcosine()) / 1.5; int track = -angle.as_bearing().value_degrees(); unsigned alarm_level = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1)); NMEAParser parser; char buffer[50]; // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,%d,%d,%d,%d,2,DDA85C,%d,0,35,0,1", alarm_level, north, east, alt, track); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); alt = (angle.ifastcosine()) / 10; north = (dangle.ifastsine()) / 1.20 + 300; east = (dangle.ifastcosine()) + 500; track = hangle.value_degrees(); // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,0,%d,%d,%d,2,AA9146,,,,,1", north, east, alt); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); // PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>, // <RelativeVertical>,<RelativeDistance>(,<ID>) sprintf(buffer, "$PFLAU,2,1,2,1,%d", alarm_level); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); }
/** * Shifts and rotates the given polygon and also sizes it via FastScale() * @param poly Points specifying the polygon * @param n Number of points of the polygon * @param xs Pixels to shift in the x-direction * @param ys Pixels to shift in the y-direction * @param angle Angle of rotation */ void PolygonRotateShift(RasterPoint *poly, const int n, const int xs, const int ys, Angle angle) { static Angle lastangle = Angle::native(-fixed_one); static int cost = 1024, sint = 0; angle = angle.as_bearing(); if (angle != lastangle) { lastangle = angle; cost = Layout::FastScale(angle.ifastcosine()); sint = Layout::FastScale(angle.ifastsine()); } const int xxs = (xs << 10) + 512; const int yys = (ys << 10) + 512; RasterPoint *p = poly; const RasterPoint *pe = poly + n; while (p < pe) { int x = p->x; int y = p->y; p->x = (x * cost - y * sint + xxs) >> 10; p->y = (y * cost + x * sint + yys) >> 10; p++; } }
/** * This function creates some simulated traffic for FLARM debugging * @param GPS_INFO Pointer to the NMEA_INFO struct */ void Simulator::GenerateFLARMTraffic(NMEAInfo &basic) { static int i = 90; i++; if (i > 255) i = 0; if (i > 80) return; const Angle angle = Angle::FullCircle() * i / 255; Angle dangle = (angle + Angle::Degrees(120)).AsBearing(); Angle hangle = dangle.Flipped().AsBearing(); int alt = (angle.ifastsine()) / 7; int north = (angle.ifastsine()) / 2 - 200; int east = (angle.ifastcosine()) / 1.5; int track = -(int)angle.AsBearing().Degrees(); unsigned alarm_level = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1)); NMEAParser parser(true); char buffer[50]; // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,%d,%d,%d,%d,2,DDA85C,%d,0,35,0,1", alarm_level, north, east, alt, track); parser.ParseLine(buffer, basic); alt = (angle.ifastcosine()) / 10; north = (dangle.ifastsine()) / 1.20 + 300; east = (dangle.ifastcosine()) + 500; track = (int)hangle.Degrees(); // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,0,%d,%d,%d,2,AA9146,,,,,1", north, east, alt); parser.ParseLine(buffer, basic); // PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>, // <RelativeVertical>,<RelativeDistance>(,<ID>) sprintf(buffer, "$PFLAU,2,1,2,1,%d", alarm_level); parser.ParseLine(buffer, basic); }