test/src/TestAATPoint.cpp

   1 /* Copyright_License {
   2
   3   XCSoar Glide Computer - http://www.xcsoar.org/
   4   Copyright (C) 2000-2013 The XCSoar Project
   5   A detailed list of copyright holders can be found in the file "AUTHORS".
   6
   7   This program is free software; you can redistribute it and/or
   8   modify it under the terms of the GNU General Public License
   9   as published by the Free Software Foundation; either version 2
  10   of the License, or (at your option) any later version.
  11
  12   This program is distributed in the hope that it will be useful,
  13   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15   GNU General Public License for more details.
  16
  17   You should have received a copy of the GNU General Public License
  18   along with this program; if not, write to the Free Software
  19   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  20 }
  21 */
  22
  23 #include "Engine/GlideSolvers/GlidePolar.hpp"
  24 #include "Engine/Task/Ordered/OrderedTask.hpp"
  25 #include "Engine/Task/Ordered/OrderedTaskBehaviour.hpp"
  26 #include "Engine/Task/Ordered/Points/AATPoint.hpp"
  27 #include "Engine/Task/Ordered/Points/StartPoint.hpp"
  28 #include "Engine/Task/Ordered/Points/FinishPoint.hpp"
  29 #include "Engine/Task/ObservationZones/CylinderZone.hpp"
  30 #include "Geo/Flat/TaskProjection.hpp"
  31 #include "TestUtil.hpp"
  32
  33 static TaskBehaviour task_behaviour;
  34 static OrderedTaskBehaviour ordered_task_behaviour;
  35 static GlidePolar glide_polar(fixed(0));
  36
  37 static GeoPoint
  38 MakeGeoPoint(double longitude, double latitude)
  39 {
  40   return GeoPoint(Angle::Degrees(longitude),
  41                   Angle::Degrees(latitude));
  42 }
  43
  44 static Waypoint
  45 MakeWaypoint(Waypoint wp, double altitude)
  46 {
  47   wp.elevation = fixed(altitude);
  48   return wp;
  49 }
  50
  51 static Waypoint
  52 MakeWaypoint(double longitude, double latitude, double altitude)
  53 {
  54   return MakeWaypoint(Waypoint(MakeGeoPoint(longitude, latitude)), altitude);
  55 }
  56
  57 static const Waypoint wp1 = MakeWaypoint(0, 45, 50);
  58 static const Waypoint wp2 = MakeWaypoint(0, 45.3, 50);
  59 static const Waypoint wp3 = MakeWaypoint(0, 46, 50);
  60
  61 static void
  62 TestAATPoint()
  63 {
  64   OrderedTask task(task_behaviour);
  65   task.Append(StartPoint(new CylinderZone(wp1.location, fixed(500)), wp1,
  66                          task_behaviour,
  67                          ordered_task_behaviour.start_constraints));
  68   task.Append(AATPoint(new CylinderZone(wp2.location, fixed(10000)), wp2,
  69                        task_behaviour));
  70   task.Append(FinishPoint(new CylinderZone(wp3.location, fixed(500)), wp3,
  71                           task_behaviour,
  72                           ordered_task_behaviour.finish_constraints));
  73   task.SetActiveTaskPoint(1);
  74   ok1(task.CheckTask());
  75
  76   AircraftState aircraft;
  77   aircraft.Reset();
  78   aircraft.location = wp1.location;
  79   aircraft.altitude = fixed(2500);
  80   task.Update(aircraft, aircraft, glide_polar);
  81
  82   AATPoint &ap = (AATPoint &)task.GetPoint(1);
  83
  84   ok1(!ap.IsTargetLocked());
  85   ok1(equals(ap.GetTargetLocation(), wp2.location));
  86   ap.LockTarget(true);
  87   ok1(ap.IsTargetLocked());
  88   ok1(equals(ap.GetTargetLocation(), wp2.location));
  89
  90   GeoPoint target = MakeGeoPoint(0, 45.31);
  91   ap.SetTarget(target);
  92   ok1(ap.IsTargetLocked());
  93   ok1(equals(ap.GetTargetLocation(), wp2.location));
  94
  95   ap.SetTarget(target, true);
  96   ok1(ap.IsTargetLocked());
  97   ok1(equals(ap.GetTargetLocation(), target));
  98
  99   RangeAndRadial rar = ap.GetTargetRangeRadial();
 100   ok1(equals(rar.range, fixed(0.1112), 1000));
 101   ok1(equals(rar.radial.Degrees(), fixed(0), 200));
 102
 103   target = MakeGeoPoint(0, 45.29);
 104   ap.SetTarget(target, true);
 105   rar = ap.GetTargetRangeRadial();
 106   ok1(equals(rar.range, fixed(-0.1112), 1000));
 107   ok1(equals(rar.radial.Degrees(), fixed(180), 200) ||
 108       equals(rar.radial.Degrees(), fixed(-180), 200));
 109
 110   target = MakeGeoPoint(-0.05, 45.3);
 111   ap.SetTarget(target, true);
 112   rar = ap.GetTargetRangeRadial();
 113   ok1(equals(rar.range, 0.39107));
 114   ok1(equals(rar.radial.Degrees(), -89.98));
 115
 116   target = MakeGeoPoint(0.05, 45.3);
 117   ap.SetTarget(target, true);
 118   rar = ap.GetTargetRangeRadial();
 119   ok1(equals(rar.range, 0.39107));
 120   ok1(equals(rar.radial.Degrees(), 89.98));
 121
 122   for (int radial = -170; radial <= 170; radial += 10) {
 123     const Angle radial2 = Angle::Degrees(radial);
 124
 125     for (int range = 10; range <= 100; range += 10) {
 126       const fixed range2(fixed(radial >= -90 && radial <= 90
 127                                ? range : -range) / 100);
 128
 129       ap.SetTarget(RangeAndRadial{range2, radial2}, task.GetTaskProjection());
 130       rar = ap.GetTargetRangeRadial();
 131       ok1(equals(rar.range, range2, 100));
 132       ok1(equals(rar.radial.Degrees(), radial2.Degrees(), 100));
 133     }
 134   }
 135 }
 136
 137 static void
 138 TestAll()
 139 {
 140   TestAATPoint();
 141 }
 142
 143 int main(int argc, char **argv)
 144 {
 145   plan_tests(717);
 146
 147   task_behaviour.SetDefaults();
 148   ordered_task_behaviour.SetDefaults();
 149
 150   TestAll();
 151
 152   return exit_status();
 153 }