Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1702 | - | 1 | /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
| 2 | // |
||
| 3 | // Unit tests for the AP_Math polygon code |
||
| 4 | // |
||
| 5 | |||
| 6 | #include <FastSerial.h> |
||
| 7 | #include <AP_Common.h> |
||
| 8 | #include <AP_Math.h> |
||
| 9 | |||
| 10 | FastSerialPort(Serial, 0); |
||
| 11 | |||
| 12 | /* |
||
| 13 | this is the boundary of the 2010 outback challenge |
||
| 14 | Note that the last point must be the same as the first for the |
||
| 15 | Polygon_outside() algorithm |
||
| 16 | */ |
||
| 17 | static const Vector2l OBC_boundary[] = { |
||
| 18 | Vector2l(-265695640, 1518373730), |
||
| 19 | Vector2l(-265699560, 1518394050), |
||
| 20 | Vector2l(-265768230, 1518411420), |
||
| 21 | Vector2l(-265773080, 1518403440), |
||
| 22 | Vector2l(-265815110, 1518419500), |
||
| 23 | Vector2l(-265784860, 1518474690), |
||
| 24 | Vector2l(-265994890, 1518528860), |
||
| 25 | Vector2l(-266092110, 1518747420), |
||
| 26 | Vector2l(-266454780, 1518820530), |
||
| 27 | Vector2l(-266435720, 1518303500), |
||
| 28 | Vector2l(-265875990, 1518344050), |
||
| 29 | Vector2l(-265695640, 1518373730) |
||
| 30 | }; |
||
| 31 | |||
| 32 | static const struct { |
||
| 33 | Vector2l point; |
||
| 34 | bool outside; |
||
| 35 | } test_points[] = { |
||
| 36 | { Vector2l(-266398870, 1518220000), true }, |
||
| 37 | { Vector2l(-266418700, 1518709260), false }, |
||
| 38 | { Vector2l(-350000000, 1490000000), true }, |
||
| 39 | { Vector2l(0, 0), true }, |
||
| 40 | { Vector2l(-265768150, 1518408250), false }, |
||
| 41 | { Vector2l(-265774060, 1518405860), true }, |
||
| 42 | { Vector2l(-266435630, 1518303440), true }, |
||
| 43 | { Vector2l(-266435650, 1518313540), false }, |
||
| 44 | { Vector2l(-266435690, 1518303530), false }, |
||
| 45 | { Vector2l(-266435690, 1518303490), true }, |
||
| 46 | { Vector2l(-265875990, 1518344049), true }, |
||
| 47 | { Vector2l(-265875990, 1518344051), false }, |
||
| 48 | { Vector2l(-266454781, 1518820530), true }, |
||
| 49 | { Vector2l(-266454779, 1518820530), true }, |
||
| 50 | { Vector2l(-266092109, 1518747420), true }, |
||
| 51 | { Vector2l(-266092111, 1518747420), false }, |
||
| 52 | { Vector2l(-266092110, 1518747421), true }, |
||
| 53 | { Vector2l(-266092110, 1518747419), false }, |
||
| 54 | { Vector2l(-266092111, 1518747421), true }, |
||
| 55 | { Vector2l(-266092109, 1518747421), true }, |
||
| 56 | { Vector2l(-266092111, 1518747419), false }, |
||
| 57 | }; |
||
| 58 | |||
| 59 | #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) |
||
| 60 | |||
| 61 | /* |
||
| 62 | polygon tests |
||
| 63 | */ |
||
| 64 | void setup(void) |
||
| 65 | { |
||
| 66 | unsigned i, count; |
||
| 67 | bool all_passed = true; |
||
| 68 | uint32_t start_time; |
||
| 69 | |||
| 70 | Serial.begin(115200); |
||
| 71 | Serial.println("polygon unit tests\n"); |
||
| 72 | |||
| 73 | if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) { |
||
| 74 | Serial.println("OBC boundary is not complete!"); |
||
| 75 | all_passed = false; |
||
| 76 | } |
||
| 77 | |||
| 78 | if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) { |
||
| 79 | Serial.println("Polygon_complete test failed"); |
||
| 80 | all_passed = false; |
||
| 81 | } |
||
| 82 | |||
| 83 | for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
||
| 84 | bool result; |
||
| 85 | result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
||
| 86 | Serial.printf_P(PSTR("%10f,%10f %s %s\n"), |
||
| 87 | 1.0e-7*test_points[i].point.x, |
||
| 88 | 1.0e-7*test_points[i].point.y, |
||
| 89 | result?"OUTSIDE":"INSIDE ", |
||
| 90 | result == test_points[i].outside?"PASS":"FAIL"); |
||
| 91 | if (result != test_points[i].outside) { |
||
| 92 | all_passed = false; |
||
| 93 | } |
||
| 94 | } |
||
| 95 | Serial.println(all_passed?"TEST PASSED":"TEST FAILED"); |
||
| 96 | |||
| 97 | Serial.println("Speed test:"); |
||
| 98 | start_time = micros(); |
||
| 99 | for (count=0; count<1000; count++) { |
||
| 100 | for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
||
| 101 | bool result; |
||
| 102 | result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
||
| 103 | if (result != test_points[i].outside) { |
||
| 104 | all_passed = false; |
||
| 105 | } |
||
| 106 | } |
||
| 107 | } |
||
| 108 | Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points)))); |
||
| 109 | Serial.println(all_passed?"ALL TESTS PASSED":"TEST FAILED"); |
||
| 110 | } |
||
| 111 | |||
| 112 | void |
||
| 113 | loop(void) |
||
| 114 | { |
||
| 115 | } |