Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : /**
9 : : * @file
10 : : */
11 : :
12 : : #include "TrajectoryPedestrian.hpp"
13 : : #include <ad/physics/Operation.hpp>
14 : : #include "ad/rss/situation/Physics.hpp"
15 : : #include "ad/rss/unstructured/DebugDrawing.hpp"
16 : :
17 : : /*!
18 : : * @brief namespace ad
19 : : */
20 : : namespace ad {
21 : : /*!
22 : : * @brief namespace rss
23 : : */
24 : : namespace rss {
25 : : /*!
26 : : * @brief namespace unstructured
27 : : */
28 : : namespace unstructured {
29 : :
30 : : const ad::physics::Distance TrajectoryPedestrian::maxRadius(1000.);
31 : : const ad::physics::Angle TrajectoryPedestrian::circleStepWidth(physics::c2PI / 20.);
32 : :
33 : 0 : bool TrajectoryPedestrian::calculateTrajectorySets(situation::VehicleState const &vehicleState,
34 : : Polygon &brakePolygon,
35 : : Polygon &continueForwardPolygon)
36 : : {
37 : 0 : ad::physics::Duration timeToStop;
38 [ # # ]: 0 : auto result = situation::calculateTimeToStop(vehicleState.objectState.speed,
39 : : vehicleState.dynamics.responseTime,
40 : : vehicleState.dynamics.maxSpeedOnAcceleration,
41 : : vehicleState.dynamics.alphaLon.accelMax,
42 : : vehicleState.dynamics.alphaLon.brakeMin,
43 : : timeToStop);
44 : :
45 [ # # # # ]: 0 : if (DEBUG_DRAWING_IS_ENABLED())
46 : : {
47 [ # # # # ]: 0 : auto vehicleLocation = TrafficParticipantLocation(TrajectoryPoint(vehicleState), vehicleState);
48 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", "pedestrian_initial_position");
# # # # #
# ]
49 : : }
50 [ # # ]: 0 : if (result)
51 : : {
52 [ # # # # ]: 0 : if (vehicleState.objectState.speed == ad::physics::Speed(0.))
53 : : {
54 [ # # ]: 0 : result = calculateTrajectorySetsStandingStill(vehicleState, timeToStop, brakePolygon, continueForwardPolygon);
55 : : }
56 : : else
57 : : {
58 [ # # ]: 0 : result = calculateTrajectorySetsMoving(vehicleState, timeToStop, brakePolygon, continueForwardPolygon);
59 : : }
60 : : }
61 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(brakePolygon, "red", "pedestrian_brake");
# # # # ]
62 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "pedestrian_continue_forward");
# # # # ]
63 : 0 : return result;
64 : : }
65 : :
66 : 0 : bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState const &vehicleState,
67 : : physics::Duration const &timeToStop,
68 : : Polygon &brakePolygon,
69 : : Polygon &continueForwardPolygon) const
70 : : {
71 : 0 : TrajectorySetStep responseTimeFrontSide;
72 : 0 : TrajectorySetStep responseTimeBackSide;
73 [ # # ]: 0 : auto result = getResponseTimeTrajectoryPoints(vehicleState, responseTimeFrontSide, responseTimeBackSide);
74 [ # # ]: 0 : if (!result)
75 : : {
76 [ # # ]: 0 : spdlog::debug(
77 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate reponse time trajectory points.");
78 : : }
79 : : else
80 : : {
81 [ # # ]: 0 : spdlog::trace("Trajectory points at response time: front left {}, front right {}, back left {}, back right {}",
82 : 0 : responseTimeFrontSide.left.size(),
83 : 0 : responseTimeFrontSide.right.size(),
84 : 0 : responseTimeBackSide.left.size(),
85 : 0 : responseTimeBackSide.right.size());
86 : : }
87 : :
88 [ # # ]: 0 : auto timeAfterResponseTime = timeToStop - vehicleState.dynamics.responseTime;
89 : :
90 : 0 : auto timeToStopBrakeMax = physics::Duration(0.);
91 [ # # # # ]: 0 : if (responseTimeBackSide.center.speed > physics::Speed(0.))
92 : : {
93 [ # # ]: 0 : result = situation::calculateTimeToStop(
94 : : responseTimeBackSide.center.speed,
95 : : timeAfterResponseTime, // this is the time to stop with brakemin, therefore sufficient here
96 : : vehicleState.dynamics.maxSpeedOnAcceleration,
97 : : vehicleState.dynamics.alphaLon.brakeMax,
98 : : vehicleState.dynamics.alphaLon.brakeMax,
99 : : timeToStopBrakeMax);
100 [ # # ]: 0 : if (!result)
101 : : {
102 [ # # ]: 0 : spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate time to stop. speed {}, "
103 : : "timeAfterResponseTime {}",
104 : : responseTimeBackSide.center.speed,
105 : : timeAfterResponseTime);
106 : : }
107 : : }
108 : :
109 : 0 : physics::Distance brakeMaxDistanceAfterResponseTime = physics::Distance(0.);
110 [ # # ]: 0 : if (result)
111 : : {
112 : 0 : physics::Speed unusedSpeed;
113 [ # # ]: 0 : result = situation::calculateAcceleratedLimitedMovement(responseTimeBackSide.center.speed,
114 : : vehicleState.dynamics.maxSpeedOnAcceleration,
115 : : vehicleState.dynamics.alphaLon.brakeMax,
116 : : timeToStopBrakeMax,
117 : : unusedSpeed,
118 : : brakeMaxDistanceAfterResponseTime);
119 : : }
120 : :
121 : 0 : physics::Distance brakeMinDistanceAfterResponseTime = physics::Distance(0.);
122 [ # # ]: 0 : if (result)
123 : : {
124 : 0 : physics::Speed unusedSpeed;
125 [ # # ]: 0 : result = situation::calculateAcceleratedLimitedMovement(responseTimeFrontSide.center.speed,
126 : : vehicleState.dynamics.maxSpeedOnAcceleration,
127 : : vehicleState.dynamics.alphaLon.brakeMin,
128 : : timeAfterResponseTime,
129 : : unusedSpeed,
130 : : brakeMinDistanceAfterResponseTime);
131 [ # # ]: 0 : if (!result)
132 : : {
133 [ # # ]: 0 : spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate speed and distance offset for "
134 : : "t={} and minBrake={}.",
135 : : timeAfterResponseTime,
136 : 0 : vehicleState.dynamics.alphaLon.brakeMin);
137 : : }
138 : : }
139 : :
140 : 0 : physics::Distance accelMaxDistanceAfterResponseTime = physics::Distance(0.);
141 [ # # ]: 0 : if (result)
142 : : {
143 : 0 : physics::Speed unusedSpeed;
144 [ # # ]: 0 : result = situation::calculateAcceleratedLimitedMovement(responseTimeFrontSide.center.speed,
145 : : vehicleState.dynamics.maxSpeedOnAcceleration,
146 : : vehicleState.dynamics.alphaLon.accelMax,
147 : : timeAfterResponseTime,
148 : : unusedSpeed,
149 : : accelMaxDistanceAfterResponseTime);
150 [ # # ]: 0 : if (!result)
151 : : {
152 [ # # ]: 0 : spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate speed and distance offset for "
153 : : "t={} and accelMax={}.",
154 : : timeAfterResponseTime,
155 : 0 : vehicleState.dynamics.alphaLon.accelMax);
156 : : }
157 : : }
158 : :
159 : : //=======================
160 : : // calculate brakePolygon
161 : : //=======================
162 : : //-------------
163 : : // brake front
164 : : //-------------
165 : 0 : TrajectorySetStep brakeFront;
166 [ # # ]: 0 : calculateTrajectoryPointsStraight(brakeMinDistanceAfterResponseTime, responseTimeFrontSide, brakeFront);
167 : :
168 : : //-------------
169 : : // brake sides
170 : : //-------------
171 : 0 : std::vector<TrajectorySetStep> brakeSideSteps;
172 : 0 : physics::Speed unusedSpeed;
173 : 0 : physics::Distance accelMaxDistanceAtResponseTime;
174 [ # # ]: 0 : result = situation::calculateAcceleratedLimitedMovement(vehicleState.objectState.speed,
175 : : vehicleState.dynamics.maxSpeedOnAcceleration,
176 : : vehicleState.dynamics.alphaLon.accelMax,
177 : : vehicleState.dynamics.responseTime,
178 : : unusedSpeed,
179 : : accelMaxDistanceAtResponseTime);
180 : : auto stepWidth = accelMaxDistanceAtResponseTime
181 [ # # ]: 0 : / (1 + vehicleState.dynamics.unstructuredSettings.pedestrianBrakeIntermediateAccelerationSteps);
182 [ # # # # : 0 : for (auto distance = stepWidth; distance < accelMaxDistanceAtResponseTime; distance += stepWidth)
# # ]
183 : : {
184 [ # # ]: 0 : auto left = TrajectoryPoint(vehicleState);
185 : 0 : auto right = left;
186 : 0 : auto center = left;
187 [ # # ]: 0 : calculateTrajectoryPoint(left, vehicleState.dynamics, distance, physics::RatioValue(1.0));
188 [ # # ]: 0 : calculateTrajectoryPoint(right, vehicleState.dynamics, distance, physics::RatioValue(-1.0));
189 [ # # ]: 0 : calculateTrajectoryPoint(center, vehicleState.dynamics, distance, physics::RatioValue(0.0));
190 : 0 : TrajectorySetStep step;
191 [ # # ]: 0 : step.left.push_back(left);
192 [ # # ]: 0 : step.right.push_back(right);
193 : 0 : step.center = center;
194 [ # # ]: 0 : brakeSideSteps.push_back(step);
195 : : }
196 : :
197 : : //-------------
198 : : // brake back
199 : : //-------------
200 : 0 : TrajectorySetStepVehicleLocation brakeMaxStepVehicleLocation;
201 [ # # ]: 0 : if (result)
202 : : {
203 : 0 : TrajectoryPoint finalRightMaxBrakeDistance;
204 [ # # ]: 0 : calculateTrajectoryPointStraight(
205 : 0 : responseTimeBackSide.right.front(), brakeMaxDistanceAfterResponseTime, finalRightMaxBrakeDistance);
206 : 0 : TrajectoryPoint finalLeftMaxBrakeDistance;
207 [ # # ]: 0 : calculateTrajectoryPointStraight(
208 : 0 : responseTimeBackSide.left.back(), brakeMaxDistanceAfterResponseTime, finalLeftMaxBrakeDistance);
209 : :
210 [ # # ]: 0 : auto finalRightMaxBrakeDistanceLocation = TrafficParticipantLocation(finalRightMaxBrakeDistance, vehicleState);
211 [ # # ]: 0 : auto finalLeftMaxBrakeDistanceLocation = TrafficParticipantLocation(finalLeftMaxBrakeDistance, vehicleState);
212 : :
213 : 0 : TrajectoryPoint finalCenterMaxBrakeDistance;
214 [ # # ]: 0 : calculateTrajectoryPointStraight(
215 : : responseTimeBackSide.center, brakeMaxDistanceAfterResponseTime, finalCenterMaxBrakeDistance);
216 [ # # ]: 0 : brakeMaxStepVehicleLocation.center = TrafficParticipantLocation(finalCenterMaxBrakeDistance, vehicleState);
217 : 0 : brakeMaxStepVehicleLocation.left = finalLeftMaxBrakeDistanceLocation;
218 : 0 : brakeMaxStepVehicleLocation.right = finalRightMaxBrakeDistanceLocation;
219 : :
220 : 0 : MultiPoint back;
221 [ # # ]: 0 : boost::geometry::append(back, finalRightMaxBrakeDistanceLocation.backRight);
222 [ # # ]: 0 : boost::geometry::append(back, finalRightMaxBrakeDistanceLocation.frontRight);
223 [ # # ]: 0 : boost::geometry::append(back, finalLeftMaxBrakeDistanceLocation.backLeft);
224 [ # # ]: 0 : boost::geometry::append(back, finalLeftMaxBrakeDistanceLocation.frontLeft);
225 [ # # ]: 0 : boost::geometry::convex_hull(back, brakePolygon);
226 : : }
227 : :
228 [ # # ]: 0 : if (result)
229 : : {
230 : 0 : TrajectorySetStepVehicleLocation unusedStepVehicleLocation;
231 [ # # # # ]: 0 : result = calculateFrontAndSidePolygon(vehicleState,
232 : : brakeMaxStepVehicleLocation,
233 : : brakeSideSteps,
234 : : brakeFront,
235 : : "pedestrian_brake",
236 : : brakePolygon,
237 : : unusedStepVehicleLocation);
238 [ # # ]: 0 : if (!result)
239 : : {
240 [ # # ]: 0 : spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate continueForward polygon.");
241 : : }
242 : : }
243 : :
244 : : //=================================
245 : : // calculate continueForwardPolygon
246 : : //=================================
247 [ # # ]: 0 : continueForwardPolygon = brakePolygon;
248 : : //-------------
249 : : // continueForward front
250 : : //-------------
251 : 0 : TrajectorySetStep continueForwardFront;
252 [ # # ]: 0 : if (result)
253 : : {
254 [ # # ]: 0 : calculateTrajectoryPointsStraight(accelMaxDistanceAfterResponseTime, responseTimeFrontSide, continueForwardFront);
255 : :
256 : : // max left
257 : : auto ratioDiff = physics::RatioValue(
258 : : 2.0
259 : 0 : / (2.0 * vehicleState.dynamics.unstructuredSettings.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps
260 : 0 : + 2.0));
261 [ # # # # : 0 : for (auto ratioValue = ratioDiff; ratioValue <= physics::RatioValue(1.0); ratioValue += ratioDiff)
# # ]
262 : : {
263 : 0 : auto pt = responseTimeFrontSide.left.back();
264 [ # # ]: 0 : calculateTrajectoryPoint(pt, vehicleState.dynamics, accelMaxDistanceAfterResponseTime, ratioValue);
265 [ # # ]: 0 : continueForwardFront.left.push_back(pt);
266 : : }
267 : :
268 : : // max right
269 : 0 : std::vector<TrajectoryPoint> right;
270 [ # # # # : 0 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue < physics::RatioValue(0.0)); ratioValue += ratioDiff)
# # ]
271 : : {
272 : 0 : auto pt = responseTimeFrontSide.right.front();
273 [ # # ]: 0 : calculateTrajectoryPoint(pt, vehicleState.dynamics, accelMaxDistanceAfterResponseTime, ratioValue);
274 [ # # ]: 0 : continueForwardFront.right.push_back(pt);
275 : : }
276 : 0 : auto previousRight = std::move(continueForwardFront.right);
277 [ # # ]: 0 : continueForwardFront.right = right;
278 [ # # ]: 0 : continueForwardFront.right.insert(continueForwardFront.right.end(), previousRight.begin(), previousRight.end());
279 : : }
280 : :
281 : : //-------------
282 : : // continueForward sides
283 : : //-------------
284 : 0 : std::vector<TrajectorySetStep> continueForwardSideSteps;
285 [ # # ]: 0 : if (result)
286 : : {
287 : 0 : stepWidth = (accelMaxDistanceAfterResponseTime - brakeMaxDistanceAfterResponseTime)
288 [ # # # # ]: 0 : / (1 + vehicleState.dynamics.unstructuredSettings.pedestrianContinueForwardIntermediateAccelerationSteps);
289 [ # # # # ]: 0 : for (auto distance = brakeMaxDistanceAfterResponseTime; distance <= accelMaxDistanceAfterResponseTime;
290 [ # # ]: 0 : distance += stepWidth)
291 : : {
292 : 0 : auto left = responseTimeFrontSide.left.back();
293 : 0 : auto right = responseTimeFrontSide.right.front();
294 : 0 : auto center = responseTimeFrontSide.center;
295 [ # # ]: 0 : calculateTrajectoryPoint(left, vehicleState.dynamics, distance, physics::RatioValue(1.0));
296 [ # # ]: 0 : calculateTrajectoryPoint(right, vehicleState.dynamics, distance, physics::RatioValue(-1.0));
297 [ # # ]: 0 : calculateTrajectoryPoint(center, vehicleState.dynamics, distance, physics::RatioValue(0.0));
298 : 0 : TrajectorySetStep step;
299 [ # # ]: 0 : step.left.push_back(left);
300 [ # # ]: 0 : step.right.push_back(right);
301 : 0 : step.center = center;
302 [ # # ]: 0 : continueForwardSideSteps.push_back(step);
303 : : }
304 : : }
305 [ # # ]: 0 : if (result)
306 : : {
307 : 0 : TrajectorySetStepVehicleLocation unusedStepVehicleLocation;
308 [ # # # # ]: 0 : result = calculateFrontAndSidePolygon(vehicleState,
309 : : brakeMaxStepVehicleLocation,
310 : : continueForwardSideSteps,
311 : : continueForwardFront,
312 : : "pedestrian_continue_forward",
313 : : continueForwardPolygon,
314 : : unusedStepVehicleLocation);
315 [ # # ]: 0 : if (!result)
316 : : {
317 [ # # ]: 0 : spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate continueForward polygon.");
318 : : }
319 : : }
320 : :
321 : 0 : return result;
322 : : }
323 : :
324 : 0 : bool TrajectoryPedestrian::calculateTrajectorySetsStandingStill(situation::VehicleState const &vehicleState,
325 : : physics::Duration const &timeToStop,
326 : : Polygon &brakePolygon,
327 : : Polygon &continueForwardPolygon) const
328 : : {
329 : : // If pedestrian is standing, he might start walking in any direction
330 : 0 : ad::physics::Speed speed;
331 : 0 : ad::physics::Distance brakeMinMaxDistance;
332 [ # # ]: 0 : auto result = situation::calculateSpeedAndDistanceOffset(timeToStop,
333 : : vehicleState.objectState.speed,
334 : : vehicleState.dynamics.responseTime,
335 : : vehicleState.dynamics.maxSpeedOnAcceleration,
336 : : vehicleState.dynamics.alphaLon.accelMax,
337 : : vehicleState.dynamics.alphaLon.brakeMin,
338 : : speed,
339 : : brakeMinMaxDistance);
340 [ # # # # ]: 0 : calculateCircleArc(toPoint(vehicleState.objectState.centerPoint),
341 : : brakeMinMaxDistance,
342 : 0 : ad::physics::Angle(0.),
343 : : ad::physics::c2PI,
344 : : circleStepWidth,
345 : : brakePolygon);
346 : :
347 : 0 : ad::physics::Distance accelMaxMaxDistance;
348 [ # # ]: 0 : if (result)
349 : : {
350 [ # # ]: 0 : result = situation::calculateSpeedAndDistanceOffset(timeToStop,
351 : : vehicleState.objectState.speed,
352 : : vehicleState.dynamics.responseTime,
353 : : vehicleState.dynamics.maxSpeedOnAcceleration,
354 : : vehicleState.dynamics.alphaLon.accelMax,
355 : : vehicleState.dynamics.alphaLon.accelMax,
356 : : speed,
357 : : accelMaxMaxDistance);
358 [ # # # # ]: 0 : calculateCircleArc(toPoint(vehicleState.objectState.centerPoint),
359 : : accelMaxMaxDistance,
360 : 0 : ad::physics::Angle(0.),
361 : : ad::physics::c2PI,
362 : : circleStepWidth,
363 : : continueForwardPolygon);
364 : : }
365 : :
366 : 0 : return result;
367 : : }
368 : :
369 : 0 : Polygon TrajectoryPedestrian::calculateSidePolygon(situation::VehicleState const &vehicleState,
370 : : TrajectoryPoint const &finalPointMin,
371 : : TrajectoryPoint const &finalPointMax) const
372 : : {
373 : 0 : MultiPoint side;
374 [ # # ]: 0 : boost::geometry::append(
375 [ # # ]: 0 : side, getVehicleCorner(finalPointMax, vehicleState.objectState.dimension, VehicleCorner::frontRight));
376 [ # # ]: 0 : boost::geometry::append(
377 [ # # ]: 0 : side, getVehicleCorner(finalPointMax, vehicleState.objectState.dimension, VehicleCorner::frontLeft));
378 [ # # ]: 0 : boost::geometry::append(side,
379 [ # # ]: 0 : getVehicleCorner(finalPointMin, vehicleState.objectState.dimension, VehicleCorner::backLeft));
380 [ # # ]: 0 : boost::geometry::append(
381 [ # # ]: 0 : side, getVehicleCorner(finalPointMin, vehicleState.objectState.dimension, VehicleCorner::backRight));
382 : :
383 [ # # ]: 0 : Polygon hull;
384 [ # # ]: 0 : boost::geometry::convex_hull(side, hull);
385 : 0 : return hull;
386 : : }
387 : :
388 : 0 : void TrajectoryPedestrian::calculateTrajectoryPointStraight(TrajectoryPoint const ¤tPoint,
389 : : physics::Distance const &distance,
390 : : TrajectoryPoint &resultPoint) const
391 : : {
392 : 0 : resultPoint = currentPoint;
393 [ # # # # ]: 0 : if (distance > physics::Distance(0.))
394 : : {
395 : 0 : resultPoint.position = currentPoint.position
396 [ # # # # : 0 : + toPoint(std::cos(currentPoint.angle) * distance, std::sin(currentPoint.angle) * distance);
# # # # #
# ]
397 : : }
398 : 0 : }
399 : :
400 : 0 : void TrajectoryPedestrian::calculateTrajectoryPointsStraight(physics::Distance const &distance,
401 : : TrajectorySetStep const &step,
402 : : TrajectorySetStep &resultStep) const
403 : : {
404 [ # # # # ]: 0 : if (distance > physics::Distance(0.))
405 : : {
406 [ # # ]: 0 : for (auto const &left : step.left)
407 : : {
408 : 0 : TrajectoryPoint pt;
409 [ # # ]: 0 : calculateTrajectoryPointStraight(left, distance, pt);
410 [ # # ]: 0 : resultStep.left.push_back(pt);
411 : : }
412 [ # # ]: 0 : for (auto const &right : step.right)
413 : : {
414 : 0 : TrajectoryPoint pt;
415 [ # # ]: 0 : calculateTrajectoryPointStraight(right, distance, pt);
416 [ # # ]: 0 : resultStep.right.push_back(pt);
417 : : }
418 : 0 : calculateTrajectoryPointStraight(step.center, distance, resultStep.center);
419 : : }
420 : : else
421 : : {
422 : 0 : resultStep = step;
423 : : }
424 : 0 : }
425 : :
426 : 0 : bool TrajectoryPedestrian::getResponseTimeTrajectoryPoints(situation::VehicleState const &vehicleState,
427 : : TrajectorySetStep &frontSide,
428 : : TrajectorySetStep &backSide) const
429 : : {
430 : 0 : auto result = true;
431 : : //-------------
432 : : // back
433 : : //-------------
434 : : auto ratioDiffBack = physics::RatioValue(
435 : 0 : 2.0 / (2.0 * vehicleState.dynamics.unstructuredSettings.pedestrianBackIntermediateHeadingChangeRatioSteps + 2.0));
436 [ # # # # : 0 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue <= physics::RatioValue(1.0)) && result;
# # # # ]
437 [ # # ]: 0 : ratioValue += ratioDiffBack)
438 : : {
439 [ # # ]: 0 : TrajectoryPoint pt(vehicleState);
440 : 0 : result = calculateTrajectoryPoint(pt,
441 : 0 : vehicleState.dynamics,
442 : 0 : vehicleState.dynamics.responseTime,
443 [ # # ]: 0 : vehicleState.dynamics.alphaLon.brakeMax,
444 : : ratioValue);
445 [ # # # # ]: 0 : if (ratioValue == physics::RatioValue(0.))
446 : : {
447 : 0 : backSide.center = pt;
448 : : }
449 [ # # # # ]: 0 : else if (ratioValue > physics::RatioValue(0.))
450 : : {
451 [ # # ]: 0 : backSide.left.push_back(pt);
452 : : }
453 : : else
454 : : {
455 [ # # ]: 0 : backSide.right.push_back(pt);
456 : : }
457 : : }
458 : :
459 : : //-------------
460 : : // front
461 : : //-------------
462 : : auto ratioDiffFront = physics::RatioValue(
463 : 0 : 2.0 / (2.0 * vehicleState.dynamics.unstructuredSettings.pedestrianFrontIntermediateHeadingChangeRatioSteps + 2.0));
464 [ # # # # : 0 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue <= physics::RatioValue(1.0)) && result;
# # # # ]
465 [ # # ]: 0 : ratioValue += ratioDiffFront)
466 : : {
467 [ # # ]: 0 : TrajectoryPoint pt(vehicleState);
468 : 0 : result = calculateTrajectoryPoint(pt,
469 : 0 : vehicleState.dynamics,
470 : 0 : vehicleState.dynamics.responseTime,
471 [ # # ]: 0 : vehicleState.dynamics.alphaLon.accelMax,
472 : : ratioValue);
473 [ # # # # ]: 0 : if (ratioValue == physics::RatioValue(0.))
474 : : {
475 : 0 : frontSide.center = pt;
476 : : }
477 [ # # # # ]: 0 : else if (ratioValue > physics::RatioValue(0.))
478 : : {
479 [ # # ]: 0 : frontSide.left.push_back(pt);
480 : : }
481 : : else
482 : : {
483 [ # # ]: 0 : frontSide.right.push_back(pt);
484 : : }
485 : : }
486 : :
487 : 0 : return result;
488 : : }
489 : :
490 : 0 : bool TrajectoryPedestrian::calculateTrajectoryPoint(TrajectoryPoint ¤tPoint,
491 : : world::RssDynamics const &dynamics,
492 : : physics::Duration const &duration,
493 : : ad::physics::Acceleration const &acceleration,
494 : : ad::physics::RatioValue const &angleChangeRatio) const
495 : : {
496 : 0 : ad::physics::Distance distance;
497 : 0 : auto result = situation::calculateAcceleratedLimitedMovement(
498 [ # # ]: 0 : currentPoint.speed, dynamics.maxSpeedOnAcceleration, acceleration, duration, currentPoint.speed, distance);
499 : :
500 [ # # ]: 0 : calculateTrajectoryPoint(currentPoint, dynamics, distance, angleChangeRatio);
501 : 0 : return result;
502 : : }
503 : :
504 : 0 : void TrajectoryPedestrian::calculateTrajectoryPoint(TrajectoryPoint ¤tPoint,
505 : : world::RssDynamics const &dynamics,
506 : : physics::Distance const &distance,
507 : : ad::physics::RatioValue const &angleChangeRatio) const
508 : : {
509 [ # # # # ]: 0 : if (distance == physics::Distance(0.))
510 : : {
511 : 0 : return;
512 : : }
513 : :
514 : 0 : if (static_cast<double>(std::fabs(angleChangeRatio))
515 [ # # # # ]: 0 : > dynamics.unstructuredSettings.pedestrianTurningRadius / maxRadius)
516 : : {
517 : : // move on circle
518 [ # # ]: 0 : auto radius = dynamics.unstructuredSettings.pedestrianTurningRadius / angleChangeRatio;
519 : :
520 [ # # ]: 0 : auto startingAngle = currentPoint.angle - ad::physics::cPI_2;
521 [ # # ]: 0 : auto circleOrigin = getCircleOrigin(currentPoint.position, radius, startingAngle);
522 : :
523 [ # # ]: 0 : auto angleChange = ad::physics::Angle(distance / radius);
524 : :
525 [ # # # # ]: 0 : currentPoint.position = getPointOnCircle(circleOrigin, radius, startingAngle + angleChange);
526 [ # # ]: 0 : currentPoint.angle += angleChange;
527 : : }
528 : : else
529 : : {
530 : : // straight line
531 : 0 : currentPoint.position = currentPoint.position
532 [ # # # # : 0 : + toPoint(std::cos(currentPoint.angle) * distance, std::sin(currentPoint.angle) * distance);
# # # # #
# ]
533 : : }
534 : : }
535 : :
536 : : } // namespace unstructured
537 : : } // namespace rss
538 : : } // namespace ad
|
