Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/situation/Physics.hpp"
10 : : #include <algorithm>
11 : : #include <cmath>
12 : : #include <limits>
13 : : #include "ad/physics/Operation.hpp"
14 : :
15 : : namespace ad {
16 : :
17 : : using physics::Acceleration;
18 : : using physics::Distance;
19 : : using physics::Duration;
20 : : using physics::Speed;
21 : :
22 : : namespace rss {
23 : : namespace situation {
24 : :
25 : 4140 : bool calculateDistanceOffsetInAcceleratedMovement(Speed const speed,
26 : : Acceleration const acceleration,
27 : : Duration const duration,
28 : : Distance &distanceOffset)
29 : : {
30 [ + - + + ]: 4140 : if (duration < Duration(0.))
31 : : {
32 : : // time must not be negative
33 : 1 : return false;
34 : : }
35 : : // s(t) = (a/2) * t^2 + v0 * t
36 [ + - + - : 4139 : distanceOffset = (acceleration * 0.5 * duration * duration) + (speed * duration);
+ - + - ]
37 : 4139 : return true;
38 : : }
39 : :
40 : 66450 : bool calculateSpeedInAcceleratedMovement(Speed const speed,
41 : : Acceleration const acceleration,
42 : : Duration const duration,
43 : : Speed &resultingSpeed)
44 : : {
45 [ + - + + ]: 66450 : if (duration < Duration(0.))
46 : : {
47 : : // time must not be negative
48 : 2 : return false;
49 : : }
50 : : // v(t) =v0 + a * t
51 [ + - ]: 66448 : resultingSpeed = speed + acceleration * duration;
52 : 66448 : return true;
53 : : }
54 : :
55 : 115077 : bool calculateStoppingDistance(Speed const currentSpeed, Acceleration const deceleration, Distance &stoppingDistance)
56 : : {
57 [ + - + + ]: 115077 : if (deceleration == Acceleration(0.))
58 : : {
59 [ + - + - ]: 1 : if (currentSpeed != Speed(0.))
60 : : {
61 : : // not reaching zero speed
62 : 1 : return false;
63 : : }
64 : : else
65 : : {
66 : 0 : stoppingDistance = Distance(0.);
67 : 0 : return true;
68 : : }
69 : : }
70 [ + + ]: 115076 : if (std::signbit(static_cast<double>(deceleration)) == std::signbit(static_cast<double>(currentSpeed)))
71 : : {
72 : : // not reaching zero speed
73 : 3 : return false;
74 : : }
75 : :
76 : : // s = v^2 / (2 * -a)
77 [ + - + - : 115073 : stoppingDistance = (currentSpeed * currentSpeed) / (2.0 * -deceleration);
+ - ]
78 : 115073 : return true;
79 : : }
80 : :
81 : 62314 : bool calculateAcceleratedLimitedMovement(Speed const currentSpeed,
82 : : Speed const maxSpeedOnAcceleration,
83 : : Acceleration const acceleration,
84 : : Duration const duration,
85 : : Speed &resultingSpeed,
86 : : Distance &distanceOffset)
87 : : {
88 [ + - + + ]: 62314 : if (duration < Duration(0.))
89 : : {
90 : : // time must not be negative
91 : 2 : return false;
92 : : }
93 [ + - + + ]: 62312 : if (currentSpeed < Speed(0.))
94 : : {
95 : : // the speed has to be always >= 0.
96 : 2 : return false;
97 : : }
98 : :
99 : 62310 : auto const result = calculateSpeedInAcceleratedMovement(currentSpeed, acceleration, duration, resultingSpeed);
100 : :
101 [ + - ]: 62310 : if (result)
102 : : {
103 : : // Only deceleration till stop is allowed
104 [ + - ]: 62310 : resultingSpeed = std::max(Speed(0.), resultingSpeed);
105 : :
106 : : // The resultingSpeed is restricted by the maxSpeedOnAcceleration
107 [ + - ]: 62310 : if (maxSpeedOnAcceleration.isValid())
108 : : {
109 : : // in case the currentSpeed is already higher, the currentSpeed is the limit
110 [ + - ]: 62310 : auto const maxSpeedToAccelerate = std::max(maxSpeedOnAcceleration, currentSpeed);
111 [ + - ]: 62310 : resultingSpeed = std::min(maxSpeedToAccelerate, resultingSpeed);
112 : : }
113 : :
114 : 62310 : Duration accelDuration = Duration(0.);
115 [ + - + + ]: 62310 : if (acceleration != Acceleration(0.))
116 : : {
117 [ + - + - ]: 62277 : accelDuration = (resultingSpeed - currentSpeed) / acceleration;
118 : : }
119 : :
120 [ + - ]: 62310 : Duration resultingSpeedDuration = duration - accelDuration;
121 : :
122 [ + - ]: 62310 : distanceOffset = currentSpeed * duration;
123 [ + - + - : 62310 : distanceOffset += 0.5 * acceleration * accelDuration * accelDuration;
+ - + - ]
124 [ + - + - : 62310 : distanceOffset += (resultingSpeed - currentSpeed) * resultingSpeedDuration;
+ - ]
125 : : }
126 : :
127 : 62310 : return result;
128 : : }
129 : :
130 : 406 : Duration calculateRequiredTimeInAcceleratedMovement(Speed const currentSpeed,
131 : : Acceleration const acceleration,
132 : : Distance const distanceToCover)
133 : : {
134 : : // constant accelerated movement
135 : : // t = -v_0/a +- sqrt(v_0^2/a^2 + 2s/a)
136 [ + - + - ]: 406 : Duration const firstPart = -1. * currentSpeed / acceleration;
137 : :
138 [ + - + - : 406 : Duration const secondPart = std::sqrt((firstPart * firstPart) + (2. * distanceToCover / acceleration));
+ - + - +
- ]
139 : :
140 [ + - ]: 406 : Duration t1 = firstPart + secondPart;
141 [ + - ]: 406 : Duration t2 = firstPart - secondPart;
142 : :
143 : 406 : Duration requiredTime;
144 [ + - + + ]: 406 : if (t2 > Duration(0.))
145 : : {
146 : 290 : requiredTime = t2;
147 : : }
148 : : else
149 : : {
150 : 116 : requiredTime = t1;
151 : : }
152 : 406 : return requiredTime;
153 : : }
154 : :
155 : 416 : bool calculateTimeForDistance(Speed const currentSpeed,
156 : : Speed const maxSpeedOnAcceleration,
157 : : Acceleration const acceleration,
158 : : Distance const distanceToCover,
159 : : Duration &requiredTime)
160 : : {
161 [ + - + + ]: 416 : if (currentSpeed < Speed(0.))
162 : : {
163 : 1 : return false;
164 : : }
165 : :
166 : 415 : bool result = true;
167 : :
168 [ + - + + ]: 820 : if ((acceleration == Acceleration(0.)) ||
169 : : // actual accelerations > 0 are dismissed, if current speed already exceeds maxSpeedOnAcceleration
170 [ + - - + : 820 : ((currentSpeed >= maxSpeedOnAcceleration) && (acceleration > Acceleration(0.))))
- - - - +
+ ]
171 : : {
172 : : // non-accelerated constant movement:
173 : : // t = s/v
174 [ + - + + ]: 10 : if (currentSpeed == Speed(0.))
175 : : {
176 : 1 : requiredTime = std::numeric_limits<Duration>::max();
177 : : }
178 : : else
179 : : {
180 : 9 : requiredTime = distanceToCover / currentSpeed;
181 : : }
182 : : }
183 : : else
184 : : {
185 : : // first calculate requiredTime if acceleration part would not be limited
186 : 405 : requiredTime = calculateRequiredTimeInAcceleratedMovement(currentSpeed, acceleration, distanceToCover);
187 : :
188 : : // second, check if max speed is reached before distance is reached
189 [ + - + + ]: 405 : if (acceleration > Acceleration(0.))
190 : : {
191 [ + - + - ]: 115 : auto const accelerationDuration = (maxSpeedOnAcceleration - currentSpeed) / acceleration;
192 [ + - + + ]: 115 : if (requiredTime > accelerationDuration)
193 : : {
194 : : // split calculation into 1. accelerated movement and 2. maxSpeedOnAcceleration movement
195 : 1 : Distance distanceWhenReachingMaximalSpeed;
196 : 1 : Speed speed;
197 [ + - ]: 1 : result = calculateAcceleratedLimitedMovement(currentSpeed,
198 : : maxSpeedOnAcceleration,
199 : : acceleration,
200 : : accelerationDuration,
201 : : speed,
202 : : distanceWhenReachingMaximalSpeed);
203 [ + - + - : 1 : if ((speed != maxSpeedOnAcceleration) || (distanceWhenReachingMaximalSpeed > distanceToCover))
+ - - + -
+ ]
204 : : {
205 : : // in this case something went terribly wrong
206 [ # # ]: 0 : throw std::runtime_error("calculateTimeForDistance>> irregular calculation results");
207 : : }
208 : : else
209 : : {
210 : : // accelerated movement
211 : : requiredTime
212 [ + - ]: 1 : = calculateRequiredTimeInAcceleratedMovement(currentSpeed, acceleration, distanceWhenReachingMaximalSpeed);
213 : : // constant movement with maxSpeedOnAcceleration
214 [ + - + - : 1 : requiredTime += (distanceToCover - distanceWhenReachingMaximalSpeed) / maxSpeedOnAcceleration;
+ - ]
215 : : }
216 : : }
217 : : }
218 : : }
219 : 415 : return result;
220 : : }
221 : :
222 : 1002 : bool calculateTimeToCoverDistance(Speed const currentSpeed,
223 : : Speed const maxSpeedOnAcceleration,
224 : : Duration const responseTime,
225 : : Acceleration const aUntilResponseTime,
226 : : Acceleration const aAfterResponseTime,
227 : : Distance const distanceToCover,
228 : : Duration &requiredTime)
229 : : {
230 [ + - - + ]: 1002 : if (distanceToCover < Distance(0.))
231 : : {
232 : 0 : return false;
233 : : }
234 : :
235 : 1002 : Distance distanceAfterResponseTime;
236 : 1002 : Speed speedAfterResponseTime;
237 : :
238 [ + - ]: 1002 : auto result = calculateAcceleratedLimitedMovement(currentSpeed,
239 : : maxSpeedOnAcceleration,
240 : : aUntilResponseTime,
241 : : responseTime,
242 : : speedAfterResponseTime,
243 : : distanceAfterResponseTime);
244 : :
245 [ + - ]: 1002 : if (result)
246 : : {
247 [ + - + + ]: 1002 : if (distanceAfterResponseTime >= distanceToCover)
248 : : {
249 : : // already too far at responseTime
250 [ + - ]: 129 : result = calculateTimeForDistance(
251 : : currentSpeed, maxSpeedOnAcceleration, aUntilResponseTime, distanceToCover, requiredTime);
252 : : }
253 [ + - + + ]: 873 : else if (speedAfterResponseTime == Speed(0.))
254 : : {
255 : : // distance not reached at responseTime, but speed already zero. Will never reach the distance.
256 : 495 : requiredTime = std::numeric_limits<Duration>::max();
257 : : }
258 : : else
259 : : {
260 : : // distance not reached at responseTime, stopping afterwards
261 : 378 : Distance stoppingDistance;
262 [ + - ]: 378 : result = calculateStoppingDistance(speedAfterResponseTime, aAfterResponseTime, stoppingDistance);
263 : :
264 [ + - ]: 378 : if (result)
265 : : {
266 [ + - + - : 378 : if (distanceAfterResponseTime + stoppingDistance > distanceToCover)
+ + ]
267 : : {
268 : : // already too far after stopping
269 [ + - ]: 285 : Distance const remainingDistance = distanceToCover - distanceAfterResponseTime;
270 : :
271 : : // maxSpeedOnAcceleration not relevant anymore, because we are AFTER the response time
272 [ + - ]: 285 : result = calculateTimeForDistance(speedAfterResponseTime,
273 : : std::numeric_limits<Speed>::max(),
274 : : aAfterResponseTime,
275 : : remainingDistance,
276 : : requiredTime);
277 [ + - ]: 285 : requiredTime += responseTime;
278 : : }
279 : : else
280 : : {
281 : : // distance not reached after stopping. Will never reach the distance.
282 : 93 : requiredTime = std::numeric_limits<Duration>::max();
283 : : }
284 : : }
285 : : }
286 : : }
287 : :
288 : 1002 : return result;
289 : : }
290 : :
291 : 7 : bool calculateSpeedAndDistanceOffset(Duration const duration,
292 : : Speed const currentSpeed,
293 : : Duration const responseTime,
294 : : Speed const maxSpeedOnAcceleration,
295 : : Acceleration const aUntilResponseTime,
296 : : Acceleration const aAfterResponseTime,
297 : : Speed &resultingSpeed,
298 : : Distance &distanceOffset)
299 : : {
300 [ + - ]: 7 : auto const accelerationDurationUntilReponseTime = std::min(duration, responseTime);
301 [ + - ]: 7 : auto result = calculateAcceleratedLimitedMovement(currentSpeed,
302 : : maxSpeedOnAcceleration,
303 : : aUntilResponseTime,
304 : : accelerationDurationUntilReponseTime,
305 : : resultingSpeed,
306 : : distanceOffset);
307 : :
308 [ + - + - : 7 : if (result && (duration > responseTime))
+ + + + ]
309 : : {
310 [ + - ]: 4 : auto const accelerationDurationAfterReponseTime = duration - responseTime;
311 : 4 : Distance distanceAfterResponseTime;
312 [ + - ]: 4 : result &= calculateAcceleratedLimitedMovement(resultingSpeed,
313 : : maxSpeedOnAcceleration,
314 : : aAfterResponseTime,
315 : : accelerationDurationAfterReponseTime,
316 : : resultingSpeed,
317 : 4 : distanceAfterResponseTime);
318 [ + - ]: 4 : distanceOffset += distanceAfterResponseTime;
319 : : }
320 : 7 : return result;
321 : : }
322 : :
323 : 78 : bool calculateTimeToStop(Speed const currentSpeed,
324 : : Duration const responseTime,
325 : : Speed const maxSpeedOnAcceleration,
326 : : Acceleration const aUntilResponseTime,
327 : : Acceleration const aAfterResponseTime,
328 : : Duration &stopDuration)
329 : : {
330 : 78 : auto result = true;
331 [ + - + + : 78 : if ((aUntilResponseTime < Acceleration(0.)) && (currentSpeed <= Speed(0.)))
+ - + + +
+ ]
332 : : { // deceleration until response time, but no initial speed.
333 : 1 : stopDuration = Duration(0.);
334 : 1 : result = false;
335 : : }
336 : :
337 : 78 : Speed speedAtResponseTime;
338 [ + + ]: 78 : if (result)
339 : : {
340 : 77 : Distance distanceOffset;
341 [ + - ]: 77 : result = calculateAcceleratedLimitedMovement(
342 : : currentSpeed, maxSpeedOnAcceleration, aUntilResponseTime, responseTime, speedAtResponseTime, distanceOffset);
343 : : }
344 : :
345 [ + + + - : 78 : if (result && (aAfterResponseTime >= Acceleration(0.)) && (speedAtResponseTime > Speed(0.)))
+ + + - +
+ + + ]
346 : : { // continues to drive forever
347 : 1 : stopDuration = Duration(0.);
348 : 1 : result = false;
349 : : }
350 : :
351 [ + + ]: 78 : if (result)
352 : : {
353 : : // 0 = currentSpeed + a * t
354 : : // t = -currentSpeed/a
355 [ + - + + ]: 76 : if (speedAtResponseTime == Speed(0.))
356 : : {
357 : : // vehicle stopped before response time
358 [ + - + - ]: 1 : stopDuration = -currentSpeed / aUntilResponseTime;
359 : : }
360 : : else
361 : : {
362 [ + - + - ]: 75 : Duration timeToStop = -speedAtResponseTime / aAfterResponseTime;
363 [ + - ]: 75 : stopDuration = responseTime + timeToStop;
364 : : }
365 : : }
366 : 78 : return result;
367 : : }
368 : :
369 : : } // namespace situation
370 : : } // namespace rss
371 : : } // namespace ad
|
