You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
115 lines
3.7 KiB
115 lines
3.7 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
// |
|
// Unit tests for the AP_Math polygon code |
|
// |
|
|
|
#include <FastSerial.h> |
|
#include <AP_Common.h> |
|
#include <AP_Math.h> |
|
|
|
FastSerialPort(Serial, 0); |
|
|
|
/* |
|
* this is the boundary of the 2010 outback challenge |
|
* Note that the last point must be the same as the first for the |
|
* Polygon_outside() algorithm |
|
*/ |
|
static const Vector2l OBC_boundary[] = { |
|
Vector2l(-265695640, 1518373730), |
|
Vector2l(-265699560, 1518394050), |
|
Vector2l(-265768230, 1518411420), |
|
Vector2l(-265773080, 1518403440), |
|
Vector2l(-265815110, 1518419500), |
|
Vector2l(-265784860, 1518474690), |
|
Vector2l(-265994890, 1518528860), |
|
Vector2l(-266092110, 1518747420), |
|
Vector2l(-266454780, 1518820530), |
|
Vector2l(-266435720, 1518303500), |
|
Vector2l(-265875990, 1518344050), |
|
Vector2l(-265695640, 1518373730) |
|
}; |
|
|
|
static const struct { |
|
Vector2l point; |
|
bool outside; |
|
} test_points[] = { |
|
{ Vector2l(-266398870, 1518220000), true }, |
|
{ Vector2l(-266418700, 1518709260), false }, |
|
{ Vector2l(-350000000, 1490000000), true }, |
|
{ Vector2l(0, 0), true }, |
|
{ Vector2l(-265768150, 1518408250), false }, |
|
{ Vector2l(-265774060, 1518405860), true }, |
|
{ Vector2l(-266435630, 1518303440), true }, |
|
{ Vector2l(-266435650, 1518313540), false }, |
|
{ Vector2l(-266435690, 1518303530), false }, |
|
{ Vector2l(-266435690, 1518303490), true }, |
|
{ Vector2l(-265875990, 1518344049), true }, |
|
{ Vector2l(-265875990, 1518344051), false }, |
|
{ Vector2l(-266454781, 1518820530), true }, |
|
{ Vector2l(-266454779, 1518820530), true }, |
|
{ Vector2l(-266092109, 1518747420), true }, |
|
{ Vector2l(-266092111, 1518747420), false }, |
|
{ Vector2l(-266092110, 1518747421), true }, |
|
{ Vector2l(-266092110, 1518747419), false }, |
|
{ Vector2l(-266092111, 1518747421), true }, |
|
{ Vector2l(-266092109, 1518747421), true }, |
|
{ Vector2l(-266092111, 1518747419), false }, |
|
}; |
|
|
|
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) |
|
|
|
/* |
|
* polygon tests |
|
*/ |
|
void setup(void) |
|
{ |
|
unsigned i, count; |
|
bool all_passed = true; |
|
uint32_t start_time; |
|
|
|
Serial.begin(115200); |
|
Serial.println("polygon unit tests\n"); |
|
|
|
if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) { |
|
Serial.println("OBC boundary is not complete!"); |
|
all_passed = false; |
|
} |
|
|
|
if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) { |
|
Serial.println("Polygon_complete test failed"); |
|
all_passed = false; |
|
} |
|
|
|
for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
|
bool result; |
|
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
|
Serial.printf_P(PSTR("%10f,%10f %s %s\n"), |
|
1.0e-7*test_points[i].point.x, |
|
1.0e-7*test_points[i].point.y, |
|
result ? "OUTSIDE" : "INSIDE ", |
|
result == test_points[i].outside ? "PASS" : "FAIL"); |
|
if (result != test_points[i].outside) { |
|
all_passed = false; |
|
} |
|
} |
|
Serial.println(all_passed ? "TEST PASSED" : "TEST FAILED"); |
|
|
|
Serial.println("Speed test:"); |
|
start_time = micros(); |
|
for (count=0; count<1000; count++) { |
|
for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
|
bool result; |
|
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
|
if (result != test_points[i].outside) { |
|
all_passed = false; |
|
} |
|
} |
|
} |
|
Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points)))); |
|
Serial.println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED"); |
|
} |
|
|
|
void |
|
loop(void) |
|
{ |
|
}
|
|
|