Skip to content

Commit 19661fe

Browse files
tridgetridge
committed
AP_Common: added uint16_t sorting code
also added test suite
1 parent 6e83633 commit 19661fe

File tree

3 files changed

+309
-0
lines changed

3 files changed

+309
-0
lines changed

‎libraries/AP_Common/sorting.cpp

+109
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
/*
2+
This program is free software: you can redistribute it and/or modify
3+
it under the terms of the GNU General Public License as published by
4+
the Free Software Foundation, either version 3 of the License, or
5+
(at your option) any later version.
6+
7+
This program is distributed in the hope that it will be useful,
8+
but WITHOUT ANY WARRANTY; without even the implied warranty of
9+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10+
GNU General Public License for more details.
11+
12+
You should have received a copy of the GNU General Public License
13+
along with this program. If not, see <http://www.gnu.org/licenses/>.
14+
*/
15+
16+
#include <stdint.h>
17+
#include "sorting.h"
18+
19+
/*
20+
in-place insertion sort for small arrays of data. This is O(n) if
21+
already sorted and O(n^2) for worst case (elements are reversed)
22+
sort order is smallest first
23+
*/
24+
void insertion_sort_uint16(uint16_t *data, uint16_t n)
25+
{
26+
for (uint16_t i=1; i<n; i++) {
27+
uint16_t temp = data[i];
28+
int16_t j = i - 1;
29+
30+
while (j >= 0 && data[j] > temp) {
31+
data[j+1] = data[j];
32+
j--;
33+
}
34+
data[j+1] = temp;
35+
}
36+
}
37+
38+
/*
39+
remove duplicates from a sorted uint16_t array, returning the new
40+
count
41+
*/
42+
uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n)
43+
{
44+
uint16_t removed = 0;
45+
for (uint16_t i=1; i<n; i++) {
46+
if (data[i-(1+removed)] == data[i]) {
47+
removed++;
48+
} else if (removed != 0) {
49+
data[i-removed] = data[i];
50+
}
51+
}
52+
return n - removed;
53+
}
54+
55+
/*
56+
bisection search on a sorted uint16_t array to find an element
57+
return true if found
58+
*/
59+
bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value)
60+
{
61+
if (n == 0) {
62+
return false;
63+
}
64+
uint16_t low=0, high=n-1;
65+
while (low < high) {
66+
uint16_t mid = (low+high)/2;
67+
if (value < data[mid]) {
68+
high = mid;
69+
continue;
70+
}
71+
if (value > data[mid]) {
72+
low = mid+1;
73+
continue;
74+
}
75+
return true;
76+
}
77+
return data[low] == value;
78+
}
79+
80+
/*
81+
remove elements in a 2nd sorted array from a sorted uint16_t array
82+
return the number of remaining elements
83+
*/
84+
uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
85+
{
86+
uint16_t removed = 0;
87+
for (uint16_t i=0; i<n; i++) {
88+
if (bisect_search_uint16(rem, n2, data[i])) {
89+
removed++;
90+
} else if (removed != 0) {
91+
data[i-removed] = data[i];
92+
}
93+
}
94+
return n - removed;
95+
}
96+
97+
/*
98+
return number of common elements between two sorted uint16_t lists
99+
*/
100+
uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *data2, uint16_t n2)
101+
{
102+
uint16_t common = 0;
103+
for (uint8_t i=0; i<n2; i++) {
104+
if (bisect_search_uint16(data, n, data2[i])) {
105+
common++;
106+
}
107+
}
108+
return common;
109+
}

‎libraries/AP_Common/sorting.h

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
/*
2+
This program is free software: you can redistribute it and/or modify
3+
it under the terms of the GNU General Public License as published by
4+
the Free Software Foundation, either version 3 of the License, or
5+
(at your option) any later version.
6+
7+
This program is distributed in the hope that it will be useful,
8+
but WITHOUT ANY WARRANTY; without even the implied warranty of
9+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10+
GNU General Public License for more details.
11+
12+
You should have received a copy of the GNU General Public License
13+
along with this program. If not, see <http://www.gnu.org/licenses/>.
14+
*/
15+
16+
/*
17+
in-place insertion sort for small arrays of data. This is O(n) if
18+
already sorted and O(n^2) for worst case (elements are reversed)
19+
sort order is smallest first
20+
*/
21+
void insertion_sort_uint16(uint16_t *data, uint16_t n);
22+
23+
/*
24+
remove duplicates from a sorted uint16_t array, returning the new
25+
count
26+
*/
27+
uint16_t remove_duplicates_uint16(uint16_t *data, uint16_t n);
28+
29+
/*
30+
bisection search on a sorted uint16_t array to find an element
31+
return true if found
32+
*/
33+
bool bisect_search_uint16(const uint16_t *data, uint16_t n, uint16_t value);
34+
35+
/*
36+
remove elements in a 2nd sorted array from a sorted uint16_t array
37+
return the number of remaining elements
38+
*/
39+
uint16_t remove_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);
40+
41+
/*
42+
return number of common elements between two sorted uint16_t lists
43+
*/
44+
uint16_t common_list_uint16(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2);

‎libraries/AP_Common/tests/test_sorting.cpp

+156
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
1+
#include <AP_gtest.h>
2+
3+
/*
4+
tests for AP_Common/sorting.cpp
5+
*/
6+
7+
#include <AP_Common/AP_Common.h>
8+
#include <AP_Common/sorting.h>
9+
#include <stdlib.h>
10+
11+
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
12+
13+
static int comp16(const uint16_t *v1, const uint16_t *v2) {
14+
return int32_t(*v1) - int32_t(*v2);
15+
}
16+
17+
static void check_equal(const uint16_t *a1, const uint16_t *a2, uint16_t n)
18+
{
19+
for (uint8_t j=0; j<n; j++) {
20+
EXPECT_EQ(a1[j], a2[j]);
21+
}
22+
}
23+
24+
TEST(Sorting, sort)
25+
{
26+
for (uint16_t i=0; i<10000; i++) {
27+
const uint8_t maxval = 100;
28+
uint16_t n = 1 + (unsigned(random()) % 100);
29+
uint16_t a1[n];
30+
uint16_t a2[n];
31+
for (uint8_t j=0; j<n; j++) {
32+
a1[j] = a2[j] = unsigned(random()) % maxval;
33+
}
34+
insertion_sort_uint16(a1, n);
35+
qsort(a2, n, sizeof(uint16_t), (__compar_fn_t)comp16);
36+
check_equal(a1, a2, n);
37+
}
38+
}
39+
40+
// a dumb version of remove_duplicates_uint16() for testing
41+
static uint16_t dumb_unique(uint16_t *data, uint16_t n)
42+
{
43+
uint16_t a2[n];
44+
uint16_t ret = 0;
45+
a2[0] = data[0];
46+
for (uint16_t i=1; i<n; i++) {
47+
if (data[i] != a2[ret]) {
48+
a2[++ret] = data[i];
49+
}
50+
}
51+
ret++;
52+
memcpy(data, a2, ret*sizeof(uint16_t));
53+
return ret;
54+
}
55+
56+
TEST(Sorting, unique)
57+
{
58+
for (uint16_t i=0; i<10000; i++) {
59+
const uint8_t maxval = 30;
60+
uint16_t n = 1 + (unsigned(random()) % 100);
61+
uint16_t a1[n];
62+
uint16_t a2[n];
63+
for (uint8_t j=0; j<n; j++) {
64+
a1[j] = a2[j] = unsigned(random()) % maxval;
65+
}
66+
insertion_sort_uint16(a1, n);
67+
insertion_sort_uint16(a2, n);
68+
uint16_t n1 = remove_duplicates_uint16(a1, n);
69+
uint16_t n2 = dumb_unique(a2, n);
70+
EXPECT_EQ(n1, n2);
71+
check_equal(a1, a2, n1);
72+
}
73+
}
74+
75+
// a dumb version of bisect_search_uint16()
76+
static bool dumb_search(uint16_t *data, uint16_t n, uint16_t value)
77+
{
78+
for (uint16_t i=0; i<n; i++) {
79+
if (data[i] == value) {
80+
return true;
81+
}
82+
}
83+
return false;
84+
}
85+
86+
TEST(Sorting, bisect)
87+
{
88+
for (uint16_t i=0; i<1000; i++) {
89+
const uint8_t maxval = 100;
90+
uint16_t n = 1 + (unsigned(random()) % 100);
91+
uint16_t a1[n];
92+
for (uint8_t j=0; j<n; j++) {
93+
a1[j] = unsigned(random()) % maxval;
94+
}
95+
insertion_sort_uint16(a1, n);
96+
for (uint8_t j=0; j<10; j++) {
97+
uint16_t v = unsigned(random()) % maxval;
98+
bool b1 = dumb_search(a1, n, v);
99+
bool b2 = bisect_search_uint16(a1, n, v);
100+
EXPECT_EQ(b1, b2);
101+
}
102+
}
103+
}
104+
105+
// a dumb version of bisect_search_uint16()
106+
static uint16_t dumb_remove_list(uint16_t *data, uint16_t n, const uint16_t *rem, uint16_t n2)
107+
{
108+
uint16_t a[n];
109+
uint16_t ret = 0;
110+
for (uint16_t i=0; i<n; i++) {
111+
bool found = false;
112+
for (uint16_t j=0; j<n2; j++) {
113+
if (rem[j] == data[i]) {
114+
found = true;
115+
break;
116+
}
117+
}
118+
if (!found) {
119+
a[ret] = data[i];
120+
ret++;
121+
}
122+
}
123+
memcpy(data, a, ret*sizeof(uint16_t));
124+
return ret;
125+
}
126+
127+
TEST(Sorting, remove)
128+
{
129+
for (uint16_t i=0; i<1000; i++) {
130+
const uint8_t maxval = 100;
131+
uint16_t n = 1 + (unsigned(random()) % 100);
132+
uint16_t n2 = 1 + (unsigned(random()) % 100);
133+
uint16_t a1[n];
134+
uint16_t a2[n];
135+
uint16_t a3[n2];
136+
for (uint8_t j=0; j<n; j++) {
137+
a2[j] = a1[j] = unsigned(random()) % maxval;
138+
}
139+
for (uint8_t j=0; j<n2; j++) {
140+
a3[j] = unsigned(random()) % maxval;
141+
}
142+
insertion_sort_uint16(a1, n);
143+
insertion_sort_uint16(a2, n);
144+
insertion_sort_uint16(a3, n2);
145+
uint16_t r1 = remove_list_uint16(a1, n, a3, n2);
146+
uint16_t r2 = dumb_remove_list(a2, n, a3, n2);
147+
EXPECT_EQ(r1, r2);
148+
for (uint8_t j=0; j<r1; j++) {
149+
EXPECT_EQ(a1[j], a2[j]);
150+
}
151+
}
152+
}
153+
154+
AP_GTEST_MAIN()
155+
156+
#endif // HAL_SITL or HAL_LINUX

0 commit comments

Comments
 (0)