mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 23:48:31 -04:00
6efa2e53cb
using sign checking and 64 bit integer math only when needed results in an algorithm that is just as fast as the floating point version, but has perfect results for any representable lat/lng
116 lines
3.7 KiB
Plaintext
116 lines
3.7 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
//
|
|
// Unit tests for the AP_Meta_class and AP_Var classes.
|
|
//
|
|
|
|
#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)
|
|
{
|
|
}
|