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 "GeometryStore.hpp"
10 : : #include <memory>
11 : : #include "ad/map/access/Logging.hpp"
12 : : #include "ad/map/access/Store.hpp"
13 : : #include "ad/map/lane/LaneOperation.hpp"
14 : : #include "ad/map/point/GeometryOperation.hpp"
15 : : #include "ad/map/serialize/SerializeGeneratedTypes.hpp"
16 : :
17 : : namespace ad {
18 : : namespace map {
19 : : namespace access {
20 : :
21 : 5 : GeometryStore::GeometryStore()
22 : : {
23 : 5 : store_ = nullptr;
24 : 5 : points3d_ = 0;
25 : 5 : capacity3d_ = 0;
26 : 5 : }
27 : :
28 : 5 : GeometryStore::~GeometryStore()
29 : : {
30 : 5 : destroy();
31 : 5 : }
32 : :
33 : : /////////////
34 : : // Operations
35 : :
36 : 602 : bool GeometryStore::store(lane::Lane::ConstPtr lane)
37 : : {
38 [ + + ]: 602 : if (lane)
39 : : {
40 : 601 : lane::LaneId id = lane->id;
41 [ + - ]: 601 : auto it = lane_items_.find(id);
42 [ + - ]: 601 : if (it == lane_items_.end())
43 : : {
44 : 601 : uint32_t offset_left = 0, size_left = 0;
45 [ + - + - ]: 601 : if (store(lane, lane::ContactLocation::LEFT, offset_left, size_left))
46 : : {
47 : 601 : uint32_t offset_right = 0, size_right = 0;
48 [ + - + - ]: 601 : if (store(lane, lane::ContactLocation::RIGHT, offset_right, size_right))
49 : : {
50 : 601 : GeometryStoreItem item;
51 : 601 : item.leftEdgeOffset = offset_left;
52 : 601 : item.leftEdgePoints = size_left;
53 : 601 : item.rightEdgeOffset = offset_right;
54 : 601 : item.rightEdgePoints = size_right;
55 [ + - ]: 601 : lane_items_[id] = item;
56 : 601 : return true;
57 : : }
58 : : }
59 : : }
60 : : else
61 : : {
62 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane already in Store?! {}", id);
63 [ # # ]: 0 : throw std::runtime_error("GeometryStore: Lane already in Store?! ");
64 : : }
65 : : }
66 : : else
67 : : {
68 [ + - ]: 1 : throw std::runtime_error("GeometryStore: Lane invalid");
69 : : }
70 : 0 : return false;
71 : : }
72 : :
73 : 302 : bool GeometryStore::restore(lane::Lane::Ptr lane) const
74 : : {
75 [ + + ]: 302 : if (lane)
76 : : {
77 : 301 : lane::LaneId id = lane->id;
78 [ + - ]: 301 : auto it = lane_items_.find(id);
79 [ + - ]: 301 : if (it != lane_items_.end())
80 : : {
81 : 301 : const GeometryStoreItem &item = it->second;
82 : 301 : point::ECEFEdge left;
83 [ + - + - ]: 301 : if (restore(left, item.leftEdgeOffset, item.leftEdgePoints))
84 : : {
85 : 301 : point::ECEFEdge right;
86 [ + - + - ]: 301 : if (restore(right, item.rightEdgeOffset, item.rightEdgePoints))
87 : : {
88 [ + - ]: 301 : lane->edgeLeft = point::createGeometry(left, false);
89 [ + - ]: 301 : lane->edgeRight = point::createGeometry(right, false);
90 : 301 : return true;
91 : : }
92 : : else
93 : : {
94 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane right edge not in Store?! {}", id);
95 : : }
96 : : }
97 : : else
98 : : {
99 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane left edge not in Store?! {}", id);
100 : : }
101 : : }
102 : : else
103 : : {
104 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane not in Store?! {}", id);
105 : : }
106 : : }
107 : : else
108 : : {
109 [ + - ]: 1 : throw std::runtime_error("GeometryStore: Lane invalid");
110 : : }
111 : 0 : return false;
112 : : }
113 : :
114 : 302 : bool GeometryStore::check(lane::Lane::ConstPtr lane) const
115 : : {
116 [ + + ]: 302 : if (lane)
117 : : {
118 : 301 : lane::LaneId id = lane->id;
119 [ + - ]: 301 : auto it = lane_items_.find(id);
120 [ + - ]: 301 : if (it != lane_items_.end())
121 : : {
122 : 301 : const GeometryStoreItem &item = it->second;
123 : 301 : point::ECEFEdge left;
124 [ + - + - ]: 301 : if (restore(left, item.leftEdgeOffset, item.leftEdgePoints))
125 : : {
126 : 301 : point::ECEFEdge right;
127 [ + - + - ]: 301 : if (restore(right, item.rightEdgeOffset, item.rightEdgePoints))
128 : : {
129 [ + - + - : 301 : if (lane->edgeLeft.ecefEdge == left && lane->edgeRight.ecefEdge == right)
+ - + - +
- ]
130 : : {
131 : 301 : return true;
132 : : }
133 : : else
134 : : {
135 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane geometry mismatch?! {}", id);
136 : : }
137 : : }
138 : : else
139 : : {
140 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane right edge not in Store?! {}", id);
141 : : }
142 : : }
143 : : else
144 : : {
145 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane left edge not in Store?! {}", id);
146 : : }
147 : : }
148 : : else
149 : : {
150 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Lane not in Store?! {}", id);
151 : : }
152 : : }
153 : : else
154 : : {
155 [ + - ]: 1 : throw std::runtime_error("GeometryStore: Lane invalid");
156 : : }
157 : 0 : return false;
158 : : }
159 : :
160 : : ///////////////
161 : : // Aux Methods
162 : :
163 : 1202 : bool GeometryStore::store(lane::Lane::ConstPtr lane, lane::ContactLocation location, uint32_t &offs3d, uint32_t &size)
164 : : {
165 [ + + - + ]: 1202 : if ((location != lane::ContactLocation::LEFT) && (location != lane::ContactLocation::RIGHT))
166 : : {
167 [ # # ]: 0 : throw std::runtime_error("Location must be LEFT or RIGHT");
168 : : }
169 : : const point::ECEFEdge &ecef
170 [ + + ]: 1202 : = (location == lane::ContactLocation::LEFT) ? lane->edgeLeft.ecefEdge : lane->edgeRight.ecefEdge;
171 : 1202 : size = static_cast<uint32_t>(ecef.size());
172 [ + - ]: 2404 : lane::ContactLaneList contact_lanes = lane::getContactLanes(*lane, location);
173 [ + + + - ]: 1771 : for (auto contact_lane : contact_lanes)
174 : : {
175 : 953 : lane::LaneId contact_lane_id = contact_lane.toLane;
176 [ + - ]: 953 : auto it = lane_items_.find(contact_lane_id);
177 [ + + ]: 953 : if (it != lane_items_.end())
178 : : {
179 [ + - ]: 476 : lane::Lane::ConstPtr clane = lane::getLanePtr(contact_lane_id);
180 [ + - ]: 476 : if (clane)
181 : : {
182 : : const point::ECEFEdge &ecef1
183 [ + + ]: 476 : = (location == lane::ContactLocation::LEFT) ? clane->edgeRight.ecefEdge : clane->edgeLeft.ecefEdge;
184 [ + - + + ]: 476 : if (ecef == ecef1)
185 : : {
186 [ + + ]: 384 : offs3d = (location == lane::ContactLocation::LEFT) ? it->second.rightEdgeOffset : it->second.leftEdgeOffset;
187 : 384 : return true;
188 : : }
189 : : }
190 : : }
191 : : }
192 [ + - ]: 818 : return store(ecef, offs3d);
193 : : }
194 : :
195 : 827 : bool GeometryStore::store(const point::ECEFEdge &ecef, uint32_t &offset3d)
196 : : {
197 [ + + ]: 827 : while (points3d_ + ecef.size() >= capacity3d_)
198 : : {
199 [ - + ]: 9 : if (!expand())
200 : : {
201 : 0 : return false;
202 : : }
203 : : }
204 : 818 : offset3d = points3d_;
205 [ + + ]: 7868 : for (auto pt : ecef)
206 : : {
207 : 7050 : uint32_t index = (points3d_++) * 3;
208 : 7050 : store_[index++] = static_cast<double>(pt.x);
209 : 7050 : store_[index++] = static_cast<double>(pt.y);
210 : 7050 : store_[index++] = static_cast<double>(pt.z);
211 : : }
212 : 818 : return true;
213 : : }
214 : :
215 : 1204 : bool GeometryStore::restore(point::ECEFEdge &ecef, uint32_t offset3d, uint32_t points3d) const
216 : : {
217 [ - + ]: 1204 : if (!ecef.empty())
218 : : {
219 [ # # ]: 0 : throw std::runtime_error("ecef not empty");
220 : : }
221 [ + - ]: 1204 : if (offset3d + points3d <= capacity3d_)
222 : : {
223 [ + + ]: 10542 : for (uint32_t index = offset3d * 3; points3d != 0; points3d--)
224 : : {
225 [ + - ]: 9338 : point::ECEFPoint pt;
226 : 9338 : pt.x = point::ECEFCoordinate(store_[index++]);
227 : 9338 : pt.y = point::ECEFCoordinate(store_[index++]);
228 : 9338 : pt.z = point::ECEFCoordinate(store_[index++]);
229 [ + - ]: 9338 : ecef.push_back(pt);
230 : : }
231 : 1204 : return true;
232 : : }
233 : : else
234 : : {
235 : 0 : return false;
236 : : }
237 : : }
238 : :
239 : : //////////////
240 : : // Aux Methods
241 : :
242 : 10 : void GeometryStore::destroy()
243 : : {
244 [ + + ]: 10 : if (store_ != nullptr)
245 : : {
246 : 5 : free(store_);
247 : 5 : store_ = nullptr;
248 : 5 : points3d_ = 0;
249 : 5 : capacity3d_ = 0;
250 [ + - ]: 5 : access::getLogger()->debug("GeometryStore: Destroyed.");
251 : : }
252 : 10 : }
253 : :
254 : 9 : bool GeometryStore::expand()
255 : : {
256 [ + + ]: 9 : if (store_ == nullptr)
257 : : {
258 : 3 : return create(SIZE_INCREMENT);
259 : : }
260 : : else
261 : : {
262 : 6 : size_t bytes = (capacity3d_ + SIZE_INCREMENT) * 3 * sizeof(double);
263 : 6 : double *store = static_cast<double *>(std::realloc(store_, bytes));
264 [ + - ]: 6 : if (store != nullptr)
265 : : {
266 : 6 : store_ = store;
267 : 6 : capacity3d_ += SIZE_INCREMENT;
268 : 6 : return true;
269 : : }
270 : : else
271 : : {
272 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Cannot expand to {} bytes.", bytes);
273 : 0 : return false;
274 : : }
275 : : }
276 : : }
277 : :
278 : 5 : bool GeometryStore::create(uint32_t capacity3d)
279 : : {
280 [ + - ]: 5 : destroy();
281 : 5 : size_t bytes = capacity3d * 3 * sizeof(double);
282 : 5 : store_ = static_cast<double *>(std::malloc(bytes));
283 [ + - ]: 5 : if (store_ != nullptr)
284 : : {
285 : 5 : points3d_ = 0;
286 : 5 : capacity3d_ = capacity3d;
287 : 5 : return true;
288 : : }
289 : : else
290 : : {
291 [ # # # # ]: 0 : access::getLogger()->error("GeometryStore: Cannot allocate {} bytes.", bytes);
292 : 0 : return false;
293 : : }
294 : : }
295 : :
296 : 4 : bool GeometryStore::serialize(serialize::ISerializer &serializer)
297 : : {
298 [ + - ]: 4 : bool ok = serializer.serialize(serialize::SerializeableMagic::GeometryStore)
299 [ + - + - : 4 : && serializer.serializeObjectMap(lane_items_) && serializer.serialize(points3d_);
+ - + - +
- ]
300 : :
301 : : /*Todo the two lines below will be deleted after the map are generated again without use_zfp_*/
302 : 4 : bool use_zfp_ = false;
303 [ + - + - : 4 : ok = ok && serializer.serialize(use_zfp_);
+ - ]
304 : :
305 [ + - ]: 4 : if (ok)
306 : : {
307 [ + + ]: 4 : if (!serializer.isStoring())
308 : : {
309 [ + - + - ]: 2 : if (create(points3d_))
310 : : {
311 : 2 : points3d_ = capacity3d_;
312 : : }
313 : : else
314 : : {
315 : 0 : return false;
316 : : }
317 [ + - ]: 2 : ok = serializer.read(store_, points3d_ * 3 * sizeof(double));
318 : : }
319 : : else
320 : : {
321 [ + - ]: 2 : ok = serializer.write(store_, points3d_ * 3 * sizeof(double));
322 : : }
323 : : }
324 : 4 : return ok;
325 : : }
326 : :
327 : : } // namespace access
328 : : } // namespace map
329 : : } // namespace ad
|
