1 /*
2 * Copyright (c) 2024 Embeint Inc
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include <math.h>
7
8 #include <zephyr/ztest.h>
9
10 #ifdef __clang__
11 /*
12 * Floating-point contraction merges an operation of the form (a*b + c) from
13 * two operations (fmul, fadd) into a single operation (fmadd). This can change
14 * the value of the resulting LSB, causing problems when the expected value is
15 * some multiple of 0.5f and the rounding functions are used. Disable
16 * contraction for the tests to ensure this doesn't occur.
17 */
18 #pragma STDC FP_CONTRACT OFF
19 #endif
20
21 #include <zephyr/math/interpolation.h>
22
ZTEST(interpolation,test_interpolation_oob)23 ZTEST(interpolation, test_interpolation_oob)
24 {
25 int32_t x_axis[] = {10, 20, 30, 40, 50};
26 int32_t y_axis[] = {20, 22, 24, 28, 36};
27 uint8_t len = ARRAY_SIZE(x_axis);
28
29 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
30
31 zassert_equal(y_axis[0], linear_interpolate(x_axis, y_axis, len, INT32_MIN));
32 zassert_equal(y_axis[0], linear_interpolate(x_axis, y_axis, len, -1));
33 zassert_equal(y_axis[0], linear_interpolate(x_axis, y_axis, len, 0));
34 zassert_equal(y_axis[0], linear_interpolate(x_axis, y_axis, len, x_axis[0] - 1));
35
36 zassert_equal(y_axis[4], linear_interpolate(x_axis, y_axis, len, x_axis[4] + 1));
37 zassert_equal(y_axis[4], linear_interpolate(x_axis, y_axis, len, 100));
38 zassert_equal(y_axis[4], linear_interpolate(x_axis, y_axis, len, INT32_MAX));
39 }
40
ZTEST(interpolation,test_interpolation_on_boundary)41 ZTEST(interpolation, test_interpolation_on_boundary)
42 {
43 int32_t x_axis[] = {10, 20, 30, 40, 50};
44 int32_t y_axis[] = {20, 22, 24, 28, 36};
45 uint8_t len = ARRAY_SIZE(x_axis);
46
47 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
48
49 /* Looking up x_axis values should return corresponding y_axis */
50 for (int i = 0; i < ARRAY_SIZE(x_axis); i++) {
51 zassert_equal(y_axis[i], linear_interpolate(x_axis, y_axis, len, x_axis[i]));
52 }
53 }
54
ZTEST(interpolation,test_interpolation_rounding)55 ZTEST(interpolation, test_interpolation_rounding)
56 {
57 int32_t y_axis[] = {0, 1, 2};
58 int32_t x_axis[] = {0, 10, 20};
59 uint8_t len = ARRAY_SIZE(x_axis);
60
61 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
62
63 /* 0 to 4 -> 0 */
64 for (int i = 0; i < 5; i++) {
65 zassert_equal(0, linear_interpolate(x_axis, y_axis, len, i));
66 }
67 /* 5 to 14 -> 1 */
68 for (int i = 5; i < 15; i++) {
69 zassert_equal(1, linear_interpolate(x_axis, y_axis, len, i));
70 }
71 /* 15 to N -> 2 */
72 for (int i = 15; i <= 20; i++) {
73 zassert_equal(2, linear_interpolate(x_axis, y_axis, len, i));
74 }
75 }
76
ZTEST(interpolation,test_interpolation_simple)77 ZTEST(interpolation, test_interpolation_simple)
78 {
79 int32_t y_axis[] = {0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
80 int32_t x_axis[] = {2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700, 2800, 2900, 3000};
81 uint8_t len = ARRAY_SIZE(x_axis);
82 int32_t expected;
83
84 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
85
86 /* y = (x - 2000) / 10 */
87 for (int i = x_axis[0]; i <= x_axis[1]; i++) {
88 expected = round((i - 2000.0) / 10.0);
89 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
90 }
91 }
92
ZTEST(interpolation,test_interpolation_negative_y)93 ZTEST(interpolation, test_interpolation_negative_y)
94 {
95 int32_t y_axis[] = {-100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0};
96 int32_t x_axis[] = {2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700, 2800, 2900, 3000};
97 uint8_t len = ARRAY_SIZE(x_axis);
98 int32_t expected;
99
100 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
101
102 /* y = ((x - 2000) / 10) - 100 */
103 for (int i = x_axis[0]; i <= x_axis[len - 1]; i++) {
104 expected = round((i - 2000.0) / 10.0 - 100.0);
105 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
106 }
107 }
108
ZTEST(interpolation,test_interpolation_negative_x)109 ZTEST(interpolation, test_interpolation_negative_x)
110 {
111 int32_t y_axis[] = {0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
112 int32_t x_axis[] = {-3000, -2900, -2800, -2700, -2600, -2500,
113 -2400, -2300, -2200, -2100, -2000};
114 uint8_t len = ARRAY_SIZE(x_axis);
115 int32_t expected;
116
117 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
118
119 /* y = ((x + 3000) / 10) */
120 for (int i = x_axis[0]; i <= x_axis[len - 1]; i++) {
121 expected = round((i + 3000.0) / 10.0);
122 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
123 }
124 }
125
ZTEST(interpolation,test_interpolation_negative_xy)126 ZTEST(interpolation, test_interpolation_negative_xy)
127 {
128 int32_t y_axis[] = {-100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0};
129 int32_t x_axis[] = {-3000, -2900, -2800, -2700, -2600, -2500,
130 -2400, -2300, -2200, -2100, -2000};
131 uint8_t len = ARRAY_SIZE(x_axis);
132 int32_t expected;
133
134 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
135
136 /* y = ((x + 3000) / 10) - 100 */
137 for (int i = x_axis[0]; i <= x_axis[len - 1]; i++) {
138 expected = round((i + 3000.0) / 10.0 - 100.0);
139 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
140 }
141 }
142
ZTEST(interpolation,test_interpolation_piecewise)143 ZTEST(interpolation, test_interpolation_piecewise)
144 {
145 int32_t y_axis[] = {10, 30, 110, 40, 0};
146 int32_t x_axis[] = {100, 150, 200, 250, 300};
147 uint8_t len = ARRAY_SIZE(x_axis);
148 int32_t expected;
149
150 zassert_equal(ARRAY_SIZE(x_axis), ARRAY_SIZE(y_axis));
151
152 /* First line segment, y = 0.4x - 30 */
153 for (int i = x_axis[0]; i <= x_axis[1]; i++) {
154 expected = round(0.4 * i - 30.0);
155 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
156 }
157
158 /* Second line segment, y = 1.6x - 210 */
159 for (int i = x_axis[1]; i <= x_axis[2]; i++) {
160 expected = round(1.6 * i - 210.0);
161 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
162 }
163
164 /* Third line segment, y = 390 - 1.4x */
165 for (int i = x_axis[2]; i <= x_axis[3]; i++) {
166 expected = round(390.0 - 1.4 * i);
167 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
168 }
169
170 /* Fourth line segment, y = 240 - 0.8x */
171 for (int i = x_axis[3]; i <= x_axis[4]; i++) {
172 expected = round(240.0 - 0.8 * i);
173 zassert_equal(expected, linear_interpolate(x_axis, y_axis, len, i));
174 }
175 }
176
177 ZTEST_SUITE(interpolation, NULL, NULL, NULL, NULL, NULL);
178
