Skip to content
This repository was archived by the owner on Feb 22, 2024. It is now read-only.

Commit ae1de73

Browse files
joseviccruzjoseviccruz
committed
Merge branch 'fuzzy_compare' into main
2 parents 69dfb5b + 09fdc47 commit ae1de73

7 files changed

Lines changed: 1056 additions & 0 deletions

File tree

‎robocin/utility/CMakeLists.txt‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,3 +26,24 @@ robocin_cpp_library(
2626
CONFIGS epsilon.h.in
2727
DEPS type_traits concepts
2828
)
29+
30+
robocin_cpp_test(
31+
NAME fuzzy_compare_test
32+
HDRS internal/test/epsilon_injector.h
33+
SRCS fuzzy_compare_test.cpp
34+
DEPS fuzzy_compare
35+
)
36+
37+
robocin_cpp_library(
38+
NAME angular
39+
HDRS angular.h
40+
SRCS angular.cpp
41+
DEPS concepts
42+
)
43+
44+
robocin_cpp_test(
45+
NAME angular_test
46+
HDRS internal/test/epsilon_injector.h
47+
SRCS angular_test.cpp
48+
DEPS angular
49+
)

‎robocin/utility/README.md‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,27 @@ A collection of utility functions and classes.
44

55
## Table of Contents
66

7+
- [angular](#angular)
78
- [concepts](#concepts)
89
- [epsilon](#epsilon)
910
- [fuzzy_compare](#fuzzy_compare)
1011
- [type_traits](#type_traits)
1112

13+
<a name="angular"></a>
14+
15+
## [`angular`](angular.h)
16+
17+
The [angular](angular.h) header provides a set of functions to perform operations on angular values.
18+
19+
- `degreesToRadians`: convert degrees to radians;
20+
- `radiansToDegrees`: convert radians to degrees;
21+
- `normalizeAngle`: normalize an angle to the range [-pi, pi];
22+
- `smallestAngleDiff`: calculate the smallest angle difference between two angles;
23+
- `absSmallestAngleDiff`: calculate the absolute value of the smallest angle difference between two angles;
24+
25+
> **Note**: As in the standard library, additional overloads are provided for all integer types, which are treated
26+
> as `double`.
27+
1228
<a name="concepts"></a>
1329

1430
## [`concepts`](concepts.h)

‎robocin/utility/angular.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
//
2+
// Created by José Cruz <joseviccruz> on 24/02/23.
3+
// Copyright (c) 2023 RobôCIn.
4+
//
5+
6+
#include "robocin/utility/angular.h"

‎robocin/utility/angular.h‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
//
2+
// Created by José Cruz <joseviccruz> on 24/02/23.
3+
// Copyright (c) 2023 RobôCIn.
4+
//
5+
6+
#ifndef ROBOCIN_UTILITY_ANGULAR_H
7+
#define ROBOCIN_UTILITY_ANGULAR_H
8+
9+
#include <cmath>
10+
#include <numbers>
11+
12+
#include "robocin/utility/concepts.h"
13+
14+
namespace robocin {
15+
16+
template <arithmetic T>
17+
constexpr auto degreesToRadians(T degrees) {
18+
using F = std::conditional_t<std::floating_point<T>, T, double>;
19+
20+
constexpr F kDegreesToRadiansFactor = std::numbers::pi_v<F> / 180;
21+
22+
return degrees * kDegreesToRadiansFactor;
23+
}
24+
25+
template <arithmetic T>
26+
constexpr auto radiansToDegrees(T radians) {
27+
using F = std::conditional_t<std::floating_point<T>, T, double>;
28+
29+
constexpr F kRadiansToDegreesFactor = 180 / std::numbers::pi_v<F>;
30+
31+
return radians * kRadiansToDegreesFactor;
32+
}
33+
34+
template <arithmetic T>
35+
constexpr auto normalizeAngle(T angle) {
36+
using F = std::conditional_t<std::floating_point<T>, T, double>;
37+
38+
constexpr F kPi = std::numbers::pi_v<F>;
39+
constexpr F k2Pi = 2 * kPi;
40+
41+
if (-kPi <= angle && angle <= kPi) {
42+
return static_cast<F>(angle);
43+
}
44+
45+
F result = std::fmod(static_cast<F>(angle), k2Pi);
46+
if (result < -kPi) {
47+
result += k2Pi;
48+
} else if (result > kPi) {
49+
result -= k2Pi;
50+
}
51+
return result;
52+
}
53+
54+
template <arithmetic T, arithmetic U>
55+
constexpr auto smallestAngleDiff(T lhs, U rhs) {
56+
using F = std::conditional_t<std::floating_point<std::common_type_t<T, U>>, T, double>;
57+
58+
return normalizeAngle<F>(rhs - lhs);
59+
}
60+
61+
template <arithmetic T, arithmetic U>
62+
constexpr auto absSmallestAngleDiff(T lhs, U rhs) {
63+
return std::abs(smallestAngleDiff(lhs, rhs));
64+
}
65+
66+
} // namespace robocin
67+
68+
#endif // ROBOCIN_UTILITY_ANGULAR_H

‎robocin/utility/angular_test.cpp‎

Lines changed: 170 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,170 @@
1+
//
2+
// Created by José Cruz <joseviccruz> on 04/04/23.
3+
// Copyright (c) 2023 RobôCIn.
4+
//
5+
6+
#include "robocin/utility/angular.h"
7+
8+
#include <numbers>
9+
#include <ranges>
10+
11+
#include <gtest/gtest.h>
12+
13+
#include "robocin/utility/internal/test/epsilon_injector.h"
14+
15+
namespace robocin {
16+
namespace {
17+
18+
using ::testing::Test;
19+
using ::testing::Types;
20+
21+
using FloatingPointTestTypes = Types<float, double, long double>;
22+
23+
template <class>
24+
class FloatingPointTest : public Test {};
25+
TYPED_TEST_SUITE(FloatingPointTest, FloatingPointTestTypes);
26+
27+
// degreesToRadians --------------------------------------------------------------------------------
28+
TYPED_TEST(FloatingPointTest, DegreesToRadiansGivenPiFractions) {
29+
using T = TypeParam;
30+
31+
static constexpr T kEpsilon = epsilon_v<T>;
32+
static constexpr T kPi = std::numbers::pi_v<T>;
33+
static constexpr T kPiInDegrees = 180;
34+
35+
for (const T kRatio : std::views::iota(0, 7)) {
36+
EXPECT_NEAR(degreesToRadians<T>(kRatio * kPiInDegrees), kRatio * kPi, kEpsilon);
37+
}
38+
}
39+
40+
// radiansToDegrees --------------------------------------------------------------------------------
41+
TYPED_TEST(FloatingPointTest, RadiansToDegreesGivenPiFractions) {
42+
using T = TypeParam;
43+
44+
static constexpr T kEpsilon = epsilon_v<T>;
45+
static constexpr T kPi = std::numbers::pi_v<T>;
46+
static constexpr T kPiInDegrees = 180;
47+
48+
for (const T kRatio : std::views::iota(0, 7)) {
49+
EXPECT_NEAR(radiansToDegrees<T>(kRatio * kPi), kRatio * kPiInDegrees, kEpsilon);
50+
}
51+
}
52+
53+
// normalizeAngle ----------------------------------------------------------------------------------
54+
TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesBetweenPiAndMinusPi) {
55+
using T = TypeParam;
56+
57+
static constexpr T kEpsilon = epsilon_v<T>;
58+
static constexpr T kPi = std::numbers::pi_v<T>;
59+
60+
EXPECT_NEAR(normalizeAngle<T>(0.0), 0.0, kEpsilon); // 0.0 degrees
61+
EXPECT_NEAR(normalizeAngle<T>(kPi / 2), kPi / 2, kEpsilon); // 90.0 degrees
62+
EXPECT_NEAR(normalizeAngle<T>(-kPi / 2), -kPi / 2, kEpsilon); // -90.0 degrees
63+
EXPECT_NEAR(normalizeAngle<T>(kPi), kPi, kEpsilon); // 180.0 degrees
64+
EXPECT_NEAR(normalizeAngle<T>(-kPi), -kPi, kEpsilon); // -180.0 degrees
65+
}
66+
67+
TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesGreaterThanPi) {
68+
using T = TypeParam;
69+
70+
static constexpr T kEpsilon = epsilon_v<T>;
71+
static constexpr T kPi = std::numbers::pi_v<T>;
72+
73+
EXPECT_NEAR(normalizeAngle<T>(5 * kPi / 4), -3 * kPi / 4, kEpsilon); // 225.0 degrees
74+
EXPECT_NEAR(normalizeAngle<T>(-5 * kPi / 4), 3 * kPi / 4, kEpsilon); // -225.0 degrees
75+
EXPECT_NEAR(normalizeAngle<T>(3 * kPi / 2), -kPi / 2, kEpsilon); // 270.0 degrees
76+
EXPECT_NEAR(normalizeAngle<T>(-3 * kPi / 2), kPi / 2, kEpsilon); // -270.0 degrees
77+
EXPECT_NEAR(normalizeAngle<T>(7 * kPi / 3), kPi / 3, kEpsilon); // 420.0 degrees
78+
EXPECT_NEAR(normalizeAngle<T>(-7 * kPi / 3), -kPi / 3, kEpsilon); // -420.0 degrees
79+
}
80+
81+
TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesLessThanMinusPi) {
82+
using T = TypeParam;
83+
84+
static constexpr T kEpsilon = epsilon_v<T>;
85+
static constexpr T kPi = std::numbers::pi_v<T>;
86+
87+
EXPECT_NEAR(normalizeAngle<T>(-9 * kPi / 4), -kPi / 4, kEpsilon); // -405.0 degrees
88+
EXPECT_NEAR(normalizeAngle<T>(9 * kPi / 4), kPi / 4, kEpsilon); // 405.0 degrees
89+
EXPECT_NEAR(normalizeAngle<T>(-5 * kPi / 2), -kPi / 2, kEpsilon); // -450.0 degrees
90+
EXPECT_NEAR(normalizeAngle<T>(5 * kPi / 2), kPi / 2, kEpsilon); // 450.0 degrees
91+
EXPECT_NEAR(normalizeAngle<T>(-11 * kPi / 3), kPi / 3, kEpsilon); // -660.0 degrees
92+
EXPECT_NEAR(normalizeAngle<T>(11 * kPi / 3), -kPi / 3, kEpsilon); // 660.0 degrees
93+
}
94+
95+
// smallestAngleDiff -------------------------------------------------------------------------------
96+
TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesBetweenPiAndMinusPi) {
97+
using T = TypeParam;
98+
99+
static constexpr T kEpsilon = epsilon_v<T>;
100+
static constexpr T kPi = std::numbers::pi_v<T>;
101+
102+
// 0.0 and 90.0 degrees
103+
EXPECT_NEAR((smallestAngleDiff<T, T>(0, kPi / 2)), kPi / 2, kEpsilon);
104+
// 90.0 and 0.0 degrees
105+
EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 2, 0)), -kPi / 2, kEpsilon);
106+
// -45.0 and 45.0 degrees
107+
EXPECT_NEAR((smallestAngleDiff<T, T>(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon);
108+
// 45.0 and -45.0 degrees
109+
EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 4, -kPi / 4)), -kPi / 2, kEpsilon);
110+
// -45.0 and 135.0 degrees
111+
EXPECT_NEAR((smallestAngleDiff<T, T>(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon);
112+
// 135.0 and -45.0 degrees
113+
EXPECT_NEAR((smallestAngleDiff<T, T>(kPi / 2, -kPi / 4)), -3 * kPi / 4, kEpsilon);
114+
}
115+
116+
TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesOutsidePiAndMinusPi) {
117+
using T = TypeParam;
118+
119+
static constexpr T kEpsilon = epsilon_v<T>;
120+
static constexpr T kPi = std::numbers::pi_v<T>;
121+
122+
// -450.0 to 360.0 degrees
123+
EXPECT_NEAR((smallestAngleDiff<T, T>(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon);
124+
// 360.0 to -450.0 degrees
125+
EXPECT_NEAR((smallestAngleDiff<T, T>(2 * kPi, -5 * kPi / 2)), -kPi / 2, kEpsilon);
126+
// -450.0 to 270.0 degrees
127+
EXPECT_NEAR((smallestAngleDiff<T, T>(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon);
128+
// -450.0 to 270.0 degrees
129+
EXPECT_NEAR((smallestAngleDiff<T, T>(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon);
130+
}
131+
132+
// absSmallestAngleDiff ----------------------------------------------------------------------------
133+
TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesBetweenPiAndMinusPi) {
134+
using T = TypeParam;
135+
136+
static constexpr T kEpsilon = epsilon_v<T>;
137+
static constexpr T kPi = std::numbers::pi_v<T>;
138+
139+
// 0.0 and 90.0 degrees
140+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(0, kPi / 2)), kPi / 2, kEpsilon);
141+
// 90.0 and 0.0 degrees
142+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 2, 0)), kPi / 2, kEpsilon);
143+
// -45.0 and 45.0 degrees
144+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon);
145+
// 45.0 and -45.0 degrees
146+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 4, -kPi / 4)), kPi / 2, kEpsilon);
147+
// -45.0 and 135.0 degrees
148+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon);
149+
// 135.0 and -45.0 degrees
150+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(kPi / 2, -kPi / 4)), 3 * kPi / 4, kEpsilon);
151+
}
152+
153+
TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesOutsidePiAndMinusPi) {
154+
using T = TypeParam;
155+
156+
static constexpr T kEpsilon = epsilon_v<T>;
157+
static constexpr T kPi = std::numbers::pi_v<T>;
158+
159+
// -450.0 to 360.0 degrees
160+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon);
161+
// 360.0 to -450.0 degrees
162+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(2 * kPi, -5 * kPi / 2)), kPi / 2, kEpsilon);
163+
// -450.0 to 270.0 degrees
164+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon);
165+
// -450.0 to 270.0 degrees
166+
EXPECT_NEAR((absSmallestAngleDiff<T, T>(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon);
167+
}
168+
169+
} // namespace
170+
} // namespace robocin

0 commit comments

Comments
 (0)