Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include <ad/map/access/Operation.hpp>
10 : : #include <ad/map/point/ENUOperation.hpp>
11 : : #include <ad/map/point/Operation.hpp>
12 : : #include <ad/map/point/Transform.hpp>
13 : : #include <gtest/gtest.h>
14 : :
15 : : double const epsilon = 0.00001;
16 : :
17 : : using namespace ::ad;
18 : : using namespace ::ad::map;
19 : : using namespace ::ad::map::point;
20 : :
21 : : struct PointOperationTest : ::testing::Test
22 : : {
23 : 7 : PointOperationTest()
24 : 7 : {
25 : 7 : }
26 : :
27 : 7 : virtual void SetUp()
28 : : {
29 : 7 : }
30 : :
31 : 7 : virtual void TearDown()
32 : : {
33 : 7 : access::cleanup();
34 : 7 : }
35 : : };
36 : :
37 : 2 : TEST_F(PointOperationTest, dot_product)
38 : : {
39 : : std::vector<std::pair<double, std::pair<ENUPoint, ENUPoint>>> testVectors{
40 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(10., 0., 0.), createENUPoint(0., 10., 0.))),
41 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(10., 0., 0.), createENUPoint(0., 0., 10.))),
42 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(0., 10., 0.), createENUPoint(0., 0., 10.))),
43 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(0., 10., 0.), createENUPoint(10., 0., 0.))),
44 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(0., 0., 10.), createENUPoint(10., 0., 0.))),
45 [ + - + - ]: 1 : std::make_pair(0., std::make_pair(createENUPoint(0., 0., 10.), createENUPoint(0., 10., 0.))),
46 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(10., 0., 0.), createENUPoint(10., 0., 0.))),
47 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(0., 10., 0.), createENUPoint(0., 10., 0.))),
48 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(0., 0., 10.), createENUPoint(0., 0., 10.))),
49 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(10., 0., 0.), createENUPoint(-10., 0., 0.))),
50 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(0., 10., 0.), createENUPoint(0., -10., 0.))),
51 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(0., 0., 10.), createENUPoint(0., 0., -10.))),
52 [ + - + - ]: 1 : std::make_pair(300., std::make_pair(createENUPoint(10., 10., 10.), createENUPoint(10., 10., 10.))),
53 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(-10., 10., 10.), createENUPoint(10., 10., 10.))),
54 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(10., -10., 10.), createENUPoint(10., 10., 10.))),
55 [ + - + - ]: 1 : std::make_pair(100., std::make_pair(createENUPoint(10., 10., -10.), createENUPoint(10., 10., 10.))),
56 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(-10., -10., 10.), createENUPoint(10., 10., 10.))),
57 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(10., -10., -10.), createENUPoint(10., 10., 10.))),
58 [ + - + - ]: 1 : std::make_pair(-100., std::make_pair(createENUPoint(-10., 10., -10.), createENUPoint(10., 10., 10.))),
59 [ + - + - : 2 : std::make_pair(-300., std::make_pair(createENUPoint(-10., -10., -10.), createENUPoint(10., 10., 10.)))};
+ - ]
60 : :
61 [ + + ]: 21 : for (auto entry : testVectors)
62 : : {
63 : 20 : auto result = vectorDotProduct(entry.second.first, entry.second.second);
64 [ + - - + : 20 : EXPECT_NEAR(entry.first, result, epsilon);
- - - - -
- ]
65 [ + - ]: 20 : result = entry.second.first * entry.second.second;
66 [ + - - + : 20 : EXPECT_NEAR(entry.first, result, epsilon);
- - - - -
- ]
67 : : }
68 : 1 : }
69 : :
70 : 2 : TEST_F(PointOperationTest, cross_product)
71 : : {
72 : : std::vector<std::pair<ENUPoint, std::pair<ENUPoint, ENUPoint>>> testVectors{
73 [ + - + - ]: 1 : std::make_pair(createENUPoint(0., 0., 1.), std::make_pair(createENUPoint(1., 0., 0.), createENUPoint(0., 1., 0.))),
74 [ + - + - ]: 1 : std::make_pair(createENUPoint(1., 0., 0.), std::make_pair(createENUPoint(0., 1., 0.), createENUPoint(0., 0., 1.))),
75 [ + - + - ]: 1 : std::make_pair(createENUPoint(0., 1., 0.), std::make_pair(createENUPoint(0., 0., 1.), createENUPoint(1., 0., 0.))),
76 [ + - ]: 2 : };
77 : :
78 [ + + ]: 4 : for (auto entry : testVectors)
79 : : {
80 [ + - ]: 3 : auto result = vectorCrossProduct(entry.second.first, entry.second.second);
81 [ + - - + : 3 : EXPECT_EQ(entry.first, result);
- - - - -
- ]
82 [ + - ]: 3 : result = vectorCrossProduct(entry.second.second, entry.second.first);
83 [ + - + - : 3 : EXPECT_EQ(-1. * entry.first, result);
- + - - -
- - - ]
84 : : }
85 : 1 : }
86 : :
87 : 2 : TEST_F(PointOperationTest, enu_heading_normalization)
88 : : {
89 [ + - ]: 2 : std::vector<double> numbers{0., 22., 85.6, 90., 101., 130., 179., 179.99};
90 [ + + ]: 9 : for (auto number : numbers)
91 : : {
92 [ + - ]: 8 : ENUHeading a = createENUHeading(degree2radians(number));
93 [ + - - + : 8 : EXPECT_EQ(a, ENUHeading(degree2radians(number)));
- - - - -
- ]
94 : :
95 [ + - ]: 8 : ENUHeading b = createENUHeading(degree2radians(-number));
96 [ + - - + : 8 : EXPECT_EQ(b, ENUHeading(degree2radians(-number)));
- - - - -
- ]
97 : :
98 [ + - ]: 8 : ENUHeading a1 = createENUHeading(degree2radians(number + 360.));
99 [ + - - + : 8 : EXPECT_EQ(a1, ENUHeading(degree2radians(number)));
- - - - -
- ]
100 : :
101 [ + - ]: 8 : ENUHeading a2 = createENUHeading(degree2radians(number - 360.));
102 [ + - - + : 8 : EXPECT_EQ(a2, ENUHeading(degree2radians(number)));
- - - - -
- ]
103 : :
104 [ + - ]: 8 : ENUHeading b1 = createENUHeading(degree2radians(-number + 360.));
105 [ + - - + : 8 : EXPECT_EQ(b1, ENUHeading(degree2radians(-number)));
- - - - -
- ]
106 : :
107 [ + - ]: 8 : ENUHeading b2 = createENUHeading(degree2radians(-number - 360.));
108 [ + - - + : 8 : EXPECT_EQ(b2, ENUHeading(degree2radians(-number)));
- - - - -
- ]
109 : : }
110 : :
111 [ + - ]: 1 : ENUHeading d = createENUHeading(degree2radians(-180.));
112 [ + - - + : 1 : EXPECT_EQ(d, ENUHeading(degree2radians(180)));
- - - - -
- ]
113 : 1 : }
114 : :
115 : 2 : TEST_F(PointOperationTest, enu_heading_construction)
116 : : {
117 : 1 : GeoPoint enuReferencePoint;
118 : 1 : enuReferencePoint.longitude = Longitude(8.4399515);
119 : 1 : enuReferencePoint.latitude = Latitude(49.0189305);
120 : 1 : enuReferencePoint.altitude = Altitude(0.);
121 : :
122 [ + - + - ]: 1 : access::init("test_files/TPK.adm.txt");
123 : :
124 [ + - ]: 1 : access::setENUReferencePoint(enuReferencePoint);
125 [ + - ]: 1 : ECEFPoint enuReferencePointEcef = toECEF(enuReferencePoint);
126 : :
127 [ + - ]: 1 : std::vector<std::pair<ENUPoint, double>> pointPairs{std::make_pair(createENUPoint(1., 0., 0.), 0.),
128 [ + - ]: 1 : std::make_pair(createENUPoint(0., 1., 0.), 90.),
129 [ + - ]: 1 : std::make_pair(createENUPoint(-1., 0., 0.), 180.),
130 [ + - ]: 1 : std::make_pair(createENUPoint(0., -1., 0.), -90.),
131 [ + - ]: 1 : std::make_pair(createENUPoint(5., 0., 0.), 0.),
132 [ + - ]: 1 : std::make_pair(createENUPoint(5., 5., 0.), 45.),
133 [ + - ]: 1 : std::make_pair(createENUPoint(0., 5., 0.), 90.),
134 [ + - ]: 1 : std::make_pair(createENUPoint(-5., 5., 0.), 135.),
135 [ + - ]: 1 : std::make_pair(createENUPoint(-5., 0., 0.), 180.),
136 [ + - ]: 1 : std::make_pair(createENUPoint(-5., -5., 0.), -135.),
137 [ + - ]: 1 : std::make_pair(createENUPoint(0., -5., 0.), -90.),
138 [ + - + - ]: 2 : std::make_pair(createENUPoint(5., -5., 0.), -45.)};
139 : :
140 [ + + ]: 13 : for (auto pairElement : pointPairs)
141 : : {
142 [ + - + - ]: 12 : ECEFHeading heading = createECEFHeading(enuReferencePointEcef, toECEF(pairElement.first));
143 [ + - ]: 12 : ENUHeading a = createENUHeading(heading, enuReferencePoint);
144 [ + - - + : 12 : EXPECT_EQ(a, ENUHeading(degree2radians(pairElement.second)));
- - - - -
- ]
145 : : }
146 : 1 : }
147 : :
148 : 2 : TEST_F(PointOperationTest, NearestPointOnLine)
149 : : {
150 : : {
151 : 1 : ECEFPoint pt0 = createECEFPoint(1, 1, 0);
152 : 1 : ECEFPoint pt1 = createECEFPoint(3, 1, 0);
153 : 1 : ECEFPoint pt = createECEFPoint(2, 2, 0);
154 [ + - ]: 1 : auto t = findNearestPointOnEdge(pt, pt0, pt1);
155 [ + - - + : 1 : ASSERT_EQ(t, physics::RatioValue(0.5));
- - - - -
- ]
156 : : }
157 : : }
158 : :
159 : 2 : TEST_F(PointOperationTest, NearestPointOnSegment)
160 : : {
161 : 1 : ECEFPoint pt0 = createECEFPoint(1, 1, 0);
162 : 1 : ECEFPoint pt1 = createECEFPoint(3, 1, 0);
163 : : {
164 : 1 : ECEFPoint pt = createECEFPoint(2, 2, 0);
165 [ + - ]: 1 : auto t = findNearestPointOnSegment(pt, pt0, pt1);
166 [ + - - + : 1 : ASSERT_EQ(t, physics::ParametricValue(0.5));
- - - - -
- ]
167 : : }
168 : : {
169 : 1 : ECEFPoint pt = createECEFPoint(0, 2, 0);
170 [ + - ]: 1 : auto t = findNearestPointOnSegment(pt, pt0, pt1);
171 [ + - - + : 1 : ASSERT_EQ(t, physics::ParametricValue(0.));
- - - - -
- ]
172 : : }
173 : : {
174 : 1 : ECEFPoint pt = createECEFPoint(7, 3, 7);
175 [ + - ]: 1 : auto t = findNearestPointOnSegment(pt, pt0, pt1);
176 [ + - - + : 1 : ASSERT_EQ(t, physics::ParametricValue(1.));
- - - - -
- ]
177 : : }
178 : : }
179 : :
180 : 2 : TEST_F(PointOperationTest, CalcENUDistance)
181 : : {
182 : 1 : ENUPoint pt1 = point::createENUPoint(1., 2., 3.);
183 : 1 : ENUPoint pt2 = point::createENUPoint(2., 3., 4.);
184 : 1 : ENUEdge edge;
185 [ + - ]: 1 : edge.push_back(pt1);
186 [ + - ]: 1 : edge.push_back(pt2);
187 [ + - ]: 1 : auto ret = point::calcLength(edge);
188 [ + - - + : 1 : ASSERT_DOUBLE_EQ(static_cast<double>(ret), std::sqrt(3.));
- - - - -
- ]
189 [ + - + - ]: 1 : ENUHeading heading = normalizeENUHeading(point::createENUHeading(M_PI_2));
190 [ + - + - : 1 : ASSERT_EQ(heading, createENUHeading(M_PI_2));
- + - - -
- - - ]
191 : : }
|
