void
MapWindow::set_terrain(RasterTerrain *_terrain)
{
terrain = _terrain;
terrain_center = GeoPoint(Angle::native(fixed_zero),
Angle::native(fixed_zero));
m_background.set_terrain(_terrain);
}
GeoPoint
GeoBounds::GetCenter() const
{
if (!IsValid())
return GeoPoint::Invalid();
return GeoPoint(longitude.GetMiddle().AsDelta(), latitude.GetMiddle());
}
GeoPoint
RasterMap::Intersection(const GeoPoint& origin,
const int h_origin,
const int h_glide,
const GeoPoint& destination) const
{
return GeoPoint(Angle::Zero(), Angle::Zero());
}
GeoPoint
UTM::ToGeoPoint() const
{
// remove longitude offset
double x = (double)easting - 500000;
double y = (double)northing;
// if southern hemisphere
if (zone_letter < 'N')
y -= 10000000.0;
double m = y / k0;
double mu = m / (r * (1 - e / 4 - 3 * e2 / 64 - 5 * e3 / 256));
double _e_sqrt = sqrt(1 - e);
double _e = (1 - _e_sqrt) / (1 + _e_sqrt);
double _e2 = _e * _e;
double _e3 = _e * _e2;
double _e4 = _e * _e3;
double _e5 = _e * _e4;
Angle phi1rad = Angle::Radians(fixed(mu +
(3. / 2 * _e - 27. / 32 * _e3 + 269. / 512 * _e5) * sin(2 * mu) +
(21. / 16 * _e2 - 55. / 32 * _e4) * sin(4 * mu) +
(151. / 96 * _e3 - 417. / 128 * _e5) * sin(6 * mu) +
(1097. / 512 * _e4) * sin(8 * mu)));
double _sin = (double)phi1rad.sin();
double sin2 = _sin * _sin;
double _cos = (double)phi1rad.cos();
double _tan = _sin / _cos;
double tan2 = _tan * _tan;
double tan4 = tan2 * tan2;
double _e_sin2_sqrt = sqrt(1 - e * sin2);
double _e_sin2_sqrt3 = _e_sin2_sqrt * _e_sin2_sqrt * _e_sin2_sqrt;
double n = r / _e_sin2_sqrt;
double c = e * _cos * _cos;
double c2 = c * c;
double _r = r * (1 - e) / _e_sin2_sqrt3;
double d = x / (n * k0);
double d2 = d * d;
double d3 = d * d2;
double d4 = d * d3;
double d5 = d * d4;
double d6 = d * d5;
double latitude = (double)phi1rad.Radians() -
(n * _tan / _r) * (d2 / 2 -
d4 / 24 * (5 + 3 * tan2 + 10 * c - 4 * c2 - 9 * e_p2) +
d6 / 720 * (61 + 90 * tan2 + 298 * c + 45 * tan4 - 252 * e_p2 - 3 * c2));
double longitude = (d - d3 / 6 * (1 + 2 * tan2 + c) +
d5 / 120 * (5 - 2 * c + 28 * tan2 - 3 * c2 + 8 * e_p2 + 24 * tan4)) / _cos;
return GeoPoint(Angle::Radians(fixed(longitude)) + GetCentralMeridian(zone_number),
Angle::Radians(fixed(latitude)));
}
const GeoPoint
AirspacePolygon::get_center() const
{
if (m_border.empty()) {
return GeoPoint();
} else {
return m_border[0].get_location();
}
}
FlatBoundingBox
OrderedTask::GetBoundingBox(const GeoBounds &bounds) const
{
if (!TaskSize()) {
// undefined!
return FlatBoundingBox(FlatGeoPoint(0,0),FlatGeoPoint(0,0));
}
FlatGeoPoint ll = task_projection.project(GeoPoint(bounds.west, bounds.south));
FlatGeoPoint lr = task_projection.project(GeoPoint(bounds.east, bounds.south));
FlatGeoPoint ul = task_projection.project(GeoPoint(bounds.west, bounds.north));
FlatGeoPoint ur = task_projection.project(GeoPoint(bounds.east, bounds.north));
FlatGeoPoint fmin(min(ll.Longitude,ul.Longitude), min(ll.Latitude,lr.Latitude));
FlatGeoPoint fmax(max(lr.Longitude,ur.Longitude), max(ul.Latitude,ur.Latitude));
// note +/- 1 to ensure rounding keeps bb valid
fmin.Longitude-= 1; fmin.Latitude-= 1;
fmax.Longitude+= 1; fmax.Latitude+= 1;
return FlatBoundingBox (fmin, fmax);
}
static void
test_simple()
{
Projection prj;
prj.SetGeoLocation(GeoPoint::Zero());
TestGeoScreenCouple(prj, GeoPoint(Angle::Zero(),
Angle::Zero()), 0, 0);
}
GeoPoint CWaypoints::GetPointByIDApprox(int iId)
{
for (std::list<CPoint>::iterator it = m_Points.begin(); it != m_Points.end(); ++it)
{
if (it->GetID() == iId)
return it->GetApproximatePos();
}
return GeoPoint(0,0);
}
void
OZPreviewRenderer::Draw(Canvas &canvas, const ObservationZonePoint &oz,
const RasterPoint pt, unsigned radius,
const TaskLook &look,
const AirspaceRendererSettings &airspace_settings,
const AirspaceLook &airspace_look)
{
fixed scale;
GeoPoint center;
if (IsAncientHardware()) {
scale = fixed(radius) / ((const CylinderZone &)oz).GetRadius();
center = oz.GetReference();
} else {
OZBoundary boundary = oz.GetBoundary();
auto it = boundary.begin();
GeoBounds bounds(*it);
for (auto it_end = boundary.end(); it != it_end; ++it)
bounds.Extend(*it);
center = bounds.GetCenter();
fixed geo_heigth = GeoPoint(center.longitude, bounds.north).Distance(
GeoPoint(center.longitude, bounds.south));
fixed geo_width = GeoPoint(bounds.west, center.latitude).Distance(
GeoPoint(bounds.east, center.latitude));
scale = fixed(radius * 2) / std::max(geo_heigth, geo_width);
}
WindowProjection projection;
projection.SetScreenSize(radius * 2, radius * 2);
projection.SetScreenOrigin(pt.x, pt.y);
projection.SetGeoLocation(center);
projection.SetScale(scale);
projection.SetScreenAngle(Angle::Zero());
projection.UpdateScreenBounds();
OZRenderer ozv(look, airspace_look, airspace_settings);
ozv.Draw(canvas, OZRenderer::LAYER_SHADE, projection, oz, 1);
ozv.Draw(canvas, OZRenderer::LAYER_INACTIVE, projection, oz, 1);
ozv.Draw(canvas, OZRenderer::LAYER_ACTIVE, projection, oz, 1);
}
GeoPoint
FlatProjection::Unproject(const FlatGeoPoint &fp) const
{
assert(IsValid());
return GeoPoint(Angle::Native(fp.x * r_cos)
+ center.longitude,
Angle::Native(fp.y * inv_scale)
+ center.latitude);
}
static bool
test_airspace(const unsigned n_airspaces)
{
TestFlightComponents components;
components.airspaces = new Airspaces;
setup_airspaces(*components.airspaces, GeoPoint(Angle::Degrees(fixed_half), Angle::Degrees(fixed_half)), n_airspaces);
bool fine = test_flight(components, 4, 0);
delete components.airspaces;
return fine;
}
static bool
test_nearest_landable(const Waypoints& waypoints)
{
const Waypoint *r = waypoints.get_nearest_landable(
GeoPoint(Angle::degrees(fixed(0.99)), Angle::degrees(fixed(1.1))), 50000);
if (!r)
return false;
return r->id == 3;
}
static Waypoint
MakeWaypoint(const TCHAR *name, int altitude,
double longitude, double latitude)
{
Waypoint wp(GeoPoint(Angle::Degrees(longitude),
Angle::Degrees(latitude)));
wp.name = name;
wp.elevation = fixed(altitude);
return wp;
}
int main(int argc, char **argv)
{
const TCHAR *path = _T("output/test/test.igc");
File::Delete(path);
static const GeoPoint home(Angle::degrees(fixed(7.7061111111111114)),
Angle::degrees(fixed(51.051944444444445)));
static const GeoPoint tp(Angle::degrees(fixed(10.726111111111111)),
Angle::degrees(fixed(50.632222222222225)));
static NMEA_INFO i;
i.DateTime.year = 2010;
i.DateTime.month = 9;
i.DateTime.day = 4;
i.DateTime.hour = 11;
i.DateTime.minute = 22;
i.DateTime.second = 33;
i.Location = home;
i.GPSAltitude = fixed(487);
i.BaroAltitude = fixed(490);
IGCWriter writer(path, i);
writer.header(i.DateTime, _T("Pilot Name"), _T("ASK-21"), _T("D-1234"),
_T("foo"), _T("bar"));
writer.StartDeclaration(i.DateTime, 3);
writer.AddDeclaration(home, _T("Bergneustadt"));
writer.AddDeclaration(tp, _T("Suhl"));
writer.AddDeclaration(home, _T("Bergneustadt"));
writer.EndDeclaration();
i.DateTime.second += 5;
writer.LogPoint(i);
i.DateTime.second += 5;
writer.LogEvent(i, "my_event");
i.DateTime.second += 5;
writer.LoggerNote(_T("my_note"));
i.DateTime.second += 5;
i.Location = GeoPoint(Angle::degrees(fixed(-7.7061111111111114)),
Angle::degrees(fixed(-51.051944444444445)));
writer.LogPoint(i);
writer.finish(i);
writer.sign();
CheckTextFile(path, expect);
GRecord grecord;
grecord.Init();
grecord.SetFileName(path);
assert(grecord.VerifyGRecordInFile());
return 0;
}
gcc_const
static GeoPoint
clip_latitude(const GeoPoint origin, const GeoPoint pt, Angle at)
{
Angle dx = pt.longitude - origin.longitude;
Angle dy = pt.latitude - origin.latitude;
Angle ey = at - origin.latitude;
Angle ex = ey * (dx.Native() / dy.Native());
return GeoPoint(origin.longitude + ex, at);
}
gcc_const
static GeoPoint
clip_longitude(const GeoPoint origin, const GeoPoint pt, Angle at)
{
Angle dx = pt.longitude - origin.longitude;
Angle dy = pt.latitude - origin.latitude;
Angle ex = at - origin.longitude;
Angle ey = ex * (dy.Native() / dx.Native());
return GeoPoint(at, origin.latitude + ey);
}
GeoBounds
SearchPointVector::CalculateGeoBounds() const
{
if (empty())
return GeoBounds(GeoPoint(Angle::zero(), Angle::zero()));
GeoBounds bb((*this)[0].get_location());
for (const_iterator v = begin(); v != end(); ++v)
bb.extend(v->get_location());
return bb;
}
static GeoPoint
TiffPixelToGeoPoint(GTIF >if, GTIFDefn &defn, double x, double y)
{
if (!GTIFImageToPCS(>if, &x, &y))
return GeoPoint::Invalid();
if (defn.Model != ModelTypeGeographic &&
!GTIFProj4ToLatLong(&defn, 1, &x, &y))
return GeoPoint::Invalid();
return GeoPoint(Angle::Degrees(x), Angle::Degrees(y));
}
fixed
RasterProjection::pixel_distance(const GeoPoint &location, unsigned pixels) const
{
enum {
/**
* This factor is used to reduce fixed point rounding errors.
* x_scale and y_scale are quite large numbers, and building their
* reciprocals may lose a lot of precision.
*/
FACTOR = 256,
};
Angle distance = width_to_angle(fixed_sqrt_two * FACTOR * pixels);
GeoPoint p = GeoPoint(location.longitude + distance, location.latitude);
fixed x = location.Distance(p);
distance = height_to_angle(fixed_sqrt_two * FACTOR * pixels);
p = GeoPoint(location.longitude, location.latitude + distance);
fixed y = location.Distance(p);
return max(x, y) / FACTOR;
}
fixed
RasterProjection::FinePixelDistance(const GeoPoint &location,
unsigned pixels) const
{
enum {
/**
* This factor is used to reduce fixed point rounding errors.
* x_scale and y_scale are quite large numbers, and building their
* reciprocals may lose a lot of precision.
*/
FACTOR = 256,
};
Angle distance = WidthToAngle(M_SQRT2 * FACTOR * pixels);
GeoPoint p = GeoPoint(location.longitude + distance, location.latitude);
auto x = location.DistanceS(p);
distance = HeightToAngle(M_SQRT2 * FACTOR * pixels);
p = GeoPoint(location.longitude, location.latitude + distance);
auto y = location.DistanceS(p);
return std::max(x, y) / FACTOR;
}
static void
DrawPolygon(Canvas &canvas, const AirspacePolygon &airspace,
const RasterPoint pt, unsigned radius)
{
if (IsAncientHardware()) {
canvas.Rectangle(pt.x - radius, pt.y - radius,
pt.x + radius, pt.y + radius);
return;
}
GeoBounds bounds = airspace.GetGeoBounds();
GeoPoint center = bounds.GetCenter();
fixed geo_heigth = GeoPoint(center.longitude, bounds.north).Distance(
GeoPoint(center.longitude, bounds.south));
fixed geo_width = GeoPoint(bounds.west, center.latitude).Distance(
GeoPoint(bounds.east, center.latitude));
fixed geo_size = std::max(geo_heigth, geo_width);
WindowProjection projection;
projection.SetScreenSize(radius * 2, radius * 2);
projection.SetScreenOrigin(pt.x, pt.y);
projection.SetGeoLocation(center);
projection.SetScale(fixed(radius * 2) / geo_size);
projection.SetScreenAngle(Angle::Zero());
projection.UpdateScreenBounds();
const SearchPointVector &border = airspace.GetPoints();
std::vector<RasterPoint> pts;
pts.reserve(border.size());
for (auto it = border.begin(), it_end = border.end(); it != it_end; ++it)
pts.push_back(projection.GeoToScreen(it->get_location()));
canvas.polygon(&pts[0], (unsigned)pts.size());
}
static void
Main()
{
GeoPoint value = GeoPoint(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
if (!GeoPointEntryDialog(_T("The caption"), value, format, true))
return;
if (value.IsValid())
_tprintf(_T("%s\n"),
FormatGeoPoint(value, CoordinateFormat::DDMMSS).c_str());
else
printf("invalid\n");
}
virtual void run() override
{
auto clock = std::make_shared<SteppableClock>(3E-3f);
ClockFactory::get(clock);
std::unique_ptr<MultiRotorParams> params = MultiRotorParamsFactory::createConfig("SimpleFlight");
MultiRotor vehicle;
std::unique_ptr<Environment> environment;
vehicle.initialize(params.get(), Pose(),
GeoPoint(), environment);
std::vector<UpdatableObject*> vehicles = { &vehicle };
std::unique_ptr<PhysicsEngineBase> physics_engine(new FastPhysicsEngine());
PhysicsWorld physics_world(physics_engine.get(), vehicles,
static_cast<uint64_t>(clock->getStepSize() * 1E9));
DroneControllerBase* controller = params->getController();
testAssert(controller != nullptr, "Controller was null");
std::string message;
testAssert(controller->isAvailable(message), message);
clock->sleep_for(0.04f);
Utils::getSetMinLogLevel(true, 100);
DirectCancelableBase cancellable(controller, &vehicle);
controller->enableApiControl(true);
controller->armDisarm(true, cancellable);
controller->takeoff(10, cancellable);
clock->sleep_for(2.0f);
Utils::getSetMinLogLevel(true);
controller->moveToPosition(-5, -5, -5, 5, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0, cancellable);
clock->sleep_for(2.0f);
while (true) {
clock->sleep_for(0.1f);
controller->getStatusMessages(messages_);
for (const auto& status_message : messages_) {
std::cout << status_message << std::endl;
}
messages_.clear();
}
}
static void
Main()
{
GeoPoint value = GeoPoint(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
if (!GeoPointEntryDialog(_T("The caption"), value, true))
return;
if (value.IsValid()) {
TCHAR buffer[64];
_tprintf(_T("%s\n"), FormatGeoPoint(value, buffer, ARRAY_SIZE(buffer),
CoordinateFormat::DDMMSS));
} else
printf("invalid\n");
}
int main(int argc, char **argv)
{
TestProjection projection;
GeoPoint gp = GeoPoint(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
long x = 0, y = 0;
for (unsigned i = 64 * 1024 * 1024; i-- > 0;) {
RasterPoint rp = projection.GeoToScreen(gp);
/* prevent gcc from optimizing this loop away */
x += rp.x;
y += rp.y;
}
return x + y;
}
GeoPoint
Volkslogger::Waypoint::GetLocation() const
{
uint32_t ll = ((latitude[0] & 0x7f) << 16) |
(latitude[1] << 8) | latitude[2];
fixed lat = fixed(ll) / 60000;
if (latitude[0] & 0x80)
lat = -lat;
ll = (longitude[0] << 16) |
(longitude[1] << 8) | longitude[2];
fixed lon = fixed(ll) / 60000;
if (type_and_longitude_sign & 0x80)
lon = -lon;
return GeoPoint(Angle::Degrees(lon), Angle::Degrees(lat));
}
static void
AddSpiralWaypoints(Waypoints &waypoints,
const GeoPoint ¢er = GeoPoint(Angle::Degrees(51.4),
Angle::Degrees(7.85)),
Angle angle_start = Angle::Degrees(0),
Angle angle_step = Angle::Degrees(15),
fixed distance_start = fixed(0),
fixed distance_step = fixed(1000),
fixed distance_max = fixed(150000))
{
assert(positive(distance_step));
for (unsigned i = 0;; ++i) {
GeoVector vector;
vector.distance = distance_start + distance_step * i;
if (vector.distance > distance_max)
break;
vector.bearing = angle_start + angle_step * i;
Waypoint waypoint;
waypoint.location = vector.EndPoint(center);
waypoint.original_id = i;
waypoint.elevation = fixed(i * 10 - 500);
StaticString<256> buffer;
if (i % 7 == 0) {
buffer = _T("Airfield");
waypoint.type = Waypoint::Type::AIRFIELD;
} else if (i % 3 == 0) {
buffer = _T("Field");
waypoint.type = Waypoint::Type::OUTLANDING;
} else
buffer = _T("Waypoint");
buffer.AppendFormat(_T(" #%d"), i + 1);
waypoint.name = buffer;
waypoints.Append(std::move(waypoint));
}
waypoints.Optimise();
}
int main(int argc, char** argv)
{
// default arguments
autopilot_parms.ideal();
if (!parse_args(argc,argv)) {
return 0;
}
plan_tests(3);
ok(test_airspace(20),"airspace 20",0);
ok(test_airspace(100),"airspace 100",0);
Airspaces airspaces;
setup_airspaces(airspaces, GeoPoint(Angle::Zero(), Angle::Zero()), 20);
ok(test_airspace_extra(airspaces),"airspace extra",0);
return exit_status();
}
void
TrackLineRenderer::DrawProjected(Canvas &canvas,
const WindowProjection &projection,
const NMEAInfo &basic,
const DerivedInfo &calculated,
const MapSettings &settings,
bool wind_relative)
{
// projection.GetMapScale() <= 6000;
GeoPoint traildrift;
if (calculated.wind_available && !wind_relative) {
GeoPoint tp1 = FindLatitudeLongitude(basic.location,
calculated.wind.bearing,
calculated.wind.norm);
traildrift = basic.location - tp1;
} else {
traildrift = GeoPoint(Angle::Zero(), Angle::Zero());
}
auto dt = ARC_SWEEP/ARC_STEPS/
std::max(MIN_RATE,calculated.turn_rate_heading_smoothed.Absolute());
Angle heading = basic.attitude.heading;
GeoPoint loc = basic.location;
RasterPoint pts[ARC_STEPS+1];
pts[0] = projection.GeoToScreen(loc);
int i = 1;
while (i <= ARC_STEPS) {
GeoVector v(basic.true_airspeed*dt, heading);
loc = v.EndPoint(loc.Parametric(traildrift, dt));
pts[i] = projection.GeoToScreen(loc);
heading += calculated.turn_rate_heading_smoothed*dt;
i++;
}
canvas.Select(look.track_line_pen);
canvas.DrawPolyline(pts, i);
}
/**
* Initialises waypoints with random and non-random waypoints
* for testing
*
* @param waypoints waypoints class to add waypoints to
*/
bool setup_waypoints(Waypoints &waypoints, const unsigned n)
{
Waypoint wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed_zero)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed_one)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one),
Angle::Degrees(fixed_one)));
wp.name = _T("Hello");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed_half;
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(0.8)),
Angle::Degrees(fixed(0.5))));
wp.name = _T("Unk");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one),
Angle::Degrees(fixed_zero)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed(0.23))));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
for (unsigned i=0; i<(unsigned)std::max((int)n-6,0); i++) {
int x = rand()%1200-100;
int y = rand()%1200-100;
double z = rand()% std::max(terrain_height,1);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(x/1000.0)),
Angle::Degrees(fixed(y/1000.0))));
wp.type = Waypoint::Type::NORMAL;
wp.elevation = fixed(z);
waypoints.Append(wp);
}
waypoints.Optimise();
if (verbose) {
std::ofstream fin("results/res-wp-in.txt");
for (unsigned i=1; i<=waypoints.size(); i++) {
const Waypoint *wp = waypoints.LookupId(i);
if (wp != NULL)
fin << *wp;
}
}
return true;
}
