Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Common: added uint16_t sorting code

Browse Source 
also added test suite
This commit is contained in:
Andrew Tridgell 2022-02-14 12:32:34 +11:00
parent 6e83633a12
commit 19661fe981
3 changed files with 309 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
libraries/AP_Common/sorting.cpp Normal file
View File

@ -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;
}

44
libraries/AP_Common/sorting.h Normal file
View File

@ -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);

156
libraries/AP_Common/tests/test_sorting.cpp Normal file
View File

@ -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