mirror of https://github.com/ArduPilot/ardupilot
AP_Common: added uint16_t sorting code
also added test suite
This commit is contained in:
Andrew Tridgell
parent
6e83633a12
commit
19661fe981
|
|
@ -0,0 +1,109 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include "sorting.h"
|
||||
|
||||
/*
|
||||
in-place insertion sort for small arrays of data. This is O(n) if
|
||||
already sorted and O(n^2) for worst case (elements are reversed)
|
||||
sort order is smallest first
|
||||
*/
|
||||
void insertion_sort_uint16(uint16_t *data, uint16_t n)
|
||||
{
|
||||
for (uint16_t i=1; i<n; i++) {
|
||||
uint16_t temp = data[i];
|
||||
int16_t j = i - 1;
|
||||
|
||||
while (j >= 0 && data[j] > temp) {
|
||||
data[j+1] = data[j];
|
||||
j--;
|
||||
}
|
||||
data[j+1] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
remove duplicates from a sorted uint16_t array, returning the new
|
||||
count
|
||||
*/
|
||||
uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n)
|
||||
{
|
||||
uint16_t removed = 0;
|
||||
for (uint16_t i=1; i<n; i++) {
|
||||
if (data[i-(1+removed)] == data[i]) {
|
||||
removed++;
|
||||
} else if (removed != 0) {
|
||||
data[i-removed] = data[i];
|
||||
}
|
||||
}
|
||||
return n - removed;
|
||||
}
|
||||
|
||||
/*
|
||||
bisection search on a sorted uint16_t array to find an element
|
||||
return true if found
|
||||
*/
|
||||
bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value)
|
||||
{
|
||||
if (n == 0) {
|
||||
return false;
|
||||
}
|
||||
uint16_t low=0, high=n-1;
|
||||
while (low < high) {
|
||||
uint16_t mid = (low+high)/2;
|
||||
if (value < data[mid]) {
|
||||
high = mid;
|
||||
continue;
|
||||
}
|
||||
if (value > data[mid]) {
|
||||
low = mid+1;
|
||||
continue;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return data[low] == value;
|
||||
}
|
||||
|
||||
/*
|
||||
remove elements in a 2nd sorted array from a sorted uint16_t array
|
||||
return the number of remaining elements
|
||||
*/
|
||||
uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
|
||||
{
|
||||
uint16_t removed = 0;
|
||||
for (uint16_t i=0; i<n; i++) {
|
||||
if (bisect_search_uint16(rem, n2, data[i])) {
|
||||
removed++;
|
||||
} else if (removed != 0) {
|
||||
data[i-removed] = data[i];
|
||||
}
|
||||
}
|
||||
return n - removed;
|
||||
}
|
||||
|
||||
/*
|
||||
return number of common elements between two sorted uint16_t lists
|
||||
*/
|
||||
uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *data2, uint16_t n2)
|
||||
{
|
||||
uint16_t common = 0;
|
||||
for (uint8_t i=0; i<n2; i++) {
|
||||
if (bisect_search_uint16(data, n, data2[i])) {
|
||||
common++;
|
||||
}
|
||||
}
|
||||
return common;
|
||||
}
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
in-place insertion sort for small arrays of data. This is O(n) if
|
||||
already sorted and O(n^2) for worst case (elements are reversed)
|
||||
sort order is smallest first
|
||||
*/
|
||||
void insertion_sort_uint16(uint16_t *data, uint16_t n);
|
||||
|
||||
/*
|
||||
remove duplicates from a sorted uint16_t array, returning the new
|
||||
count
|
||||
*/
|
||||
uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n);
|
||||
|
||||
/*
|
||||
bisection search on a sorted uint16_t array to find an element
|
||||
return true if found
|
||||
*/
|
||||
bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value);
|
||||
|
||||
/*
|
||||
remove elements in a 2nd sorted array from a sorted uint16_t array
|
||||
return the number of remaining elements
|
||||
*/
|
||||
uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);
|
||||
|
||||
/*
|
||||
return number of common elements between two sorted uint16_t lists
|
||||
*/
|
||||
uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);
|
||||
|
|
@ -0,0 +1,156 @@
|
|||
#include <AP_gtest.h>
|
||||
|
||||
/*
|
||||
tests for AP_Common/sorting.cpp
|
||||
*/
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Common/sorting.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
|
||||
static int comp16(const uint16_t *v1, const uint16_t *v2) {
|
||||
return int32_t(*v1) - int32_t(*v2);
|
||||
}
|
||||
|
||||
static void check_equal(const uint16_t *a1, const uint16_t *a2, uint16_t n)
|
||||
{
|
||||
for (uint8_t j=0; j<n; j++) {
|
||||
EXPECT_EQ(a1[j], a2[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Sorting, sort)
|
||||
{
|
||||
for (uint16_t i=0; i<10000; i++) {
|
||||
const uint8_t maxval = 100;
|
||||
uint16_t n = 1 + (unsigned(random()) % 100);
|
||||
uint16_t a1[n];
|
||||
uint16_t a2[n];
|
||||
for (uint8_t j=0; j<n; j++) {
|
||||
a1[j] = a2[j] = unsigned(random()) % maxval;
|
||||
}
|
||||
insertion_sort_uint16(a1, n);
|
||||
qsort(a2, n, sizeof(uint16_t), (__compar_fn_t)comp16);
|
||||
check_equal(a1, a2, n);
|
||||
}
|
||||
}
|
||||
|
||||
// a dumb version of remove_duplicates_uint16() for testing
|
||||
static uint16_t dumb_unique(uint16_t *data, uint16_t n)
|
||||
{
|
||||
uint16_t a2[n];
|
||||
uint16_t ret = 0;
|
||||
a2[0] = data[0];
|
||||
for (uint16_t i=1; i<n; i++) {
|
||||
if (data[i] != a2[ret]) {
|
||||
a2[++ret] = data[i];
|
||||
}
|
||||
}
|
||||
ret++;
|
||||
memcpy(data, a2, ret*sizeof(uint16_t));
|
||||
return ret;
|
||||
}
|
||||
|
||||
TEST(Sorting, unique)
|
||||
{
|
||||
for (uint16_t i=0; i<10000; i++) {
|
||||
const uint8_t maxval = 30;
|
||||
uint16_t n = 1 + (unsigned(random()) % 100);
|
||||
uint16_t a1[n];
|
||||
uint16_t a2[n];
|
||||
for (uint8_t j=0; j<n; j++) {
|
||||
a1[j] = a2[j] = unsigned(random()) % maxval;
|
||||
}
|
||||
insertion_sort_uint16(a1, n);
|
||||
insertion_sort_uint16(a2, n);
|
||||
uint16_t n1 = remove_duplicates_uint16(a1, n);
|
||||
uint16_t n2 = dumb_unique(a2, n);
|
||||
EXPECT_EQ(n1, n2);
|
||||
check_equal(a1, a2, n1);
|
||||
}
|
||||
}
|
||||
|
||||
// a dumb version of bisect_search_uint16()
|
||||
static bool dumb_search(uint16_t *data, uint16_t n, uint16_t value)
|
||||
{
|
||||
for (uint16_t i=0; i<n; i++) {
|
||||
if (data[i] == value) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
TEST(Sorting, bisect)
|
||||
{
|
||||
for (uint16_t i=0; i<1000; i++) {
|
||||
const uint8_t maxval = 100;
|
||||
uint16_t n = 1 + (unsigned(random()) % 100);
|
||||
uint16_t a1[n];
|
||||
for (uint8_t j=0; j<n; j++) {
|
||||
a1[j] = unsigned(random()) % maxval;
|
||||
}
|
||||
insertion_sort_uint16(a1, n);
|
||||
for (uint8_t j=0; j<10; j++) {
|
||||
uint16_t v = unsigned(random()) % maxval;
|
||||
bool b1 = dumb_search(a1, n, v);
|
||||
bool b2 = bisect_search_uint16(a1, n, v);
|
||||
EXPECT_EQ(b1, b2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// a dumb version of bisect_search_uint16()
|
||||
static uint16_t dumb_remove_list(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
|
||||
{
|
||||
uint16_t a[n];
|
||||
uint16_t ret = 0;
|
||||
for (uint16_t i=0; i<n; i++) {
|
||||
bool found = false;
|
||||
for (uint16_t j=0; j<n2; j++) {
|
||||
if (rem[j] == data[i]) {
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
a[ret] = data[i];
|
||||
ret++;
|
||||
}
|
||||
}
|
||||
memcpy(data, a, ret*sizeof(uint16_t));
|
||||
return ret;
|
||||
}
|
||||
|
||||
TEST(Sorting, remove)
|
||||
{
|
||||
for (uint16_t i=0; i<1000; i++) {
|
||||
const uint8_t maxval = 100;
|
||||
uint16_t n = 1 + (unsigned(random()) % 100);
|
||||
uint16_t n2 = 1 + (unsigned(random()) % 100);
|
||||
uint16_t a1[n];
|
||||
uint16_t a2[n];
|
||||
uint16_t a3[n2];
|
||||
for (uint8_t j=0; j<n; j++) {
|
||||
a2[j] = a1[j] = unsigned(random()) % maxval;
|
||||
}
|
||||
for (uint8_t j=0; j<n2; j++) {
|
||||
a3[j] = unsigned(random()) % maxval;
|
||||
}
|
||||
insertion_sort_uint16(a1, n);
|
||||
insertion_sort_uint16(a2, n);
|
||||
insertion_sort_uint16(a3, n2);
|
||||
uint16_t r1 = remove_list_uint16(a1, n, a3, n2);
|
||||
uint16_t r2 = dumb_remove_list(a2, n, a3, n2);
|
||||
EXPECT_EQ(r1, r2);
|
||||
for (uint8_t j=0; j<r1; j++) {
|
||||
EXPECT_EQ(a1[j], a2[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
AP_GTEST_MAIN()
|
||||
|
||||
#endif // HAL_SITL or HAL_LINUX
|
||||
Loading…
Reference in New Issue