30 #ifndef _RL_MATH_TRAPEZOIDALVELOCITY_H_
31 #define _RL_MATH_TRAPEZOIDALVELOCITY_H_
41 template<
typename T >
55 ta(::std::numeric_limits<
Real>::max()),
56 tc(::std::numeric_limits<
Real>::max()),
57 td(::std::numeric_limits<
Real>::max())
116 T t1 = ta1 + tc1 + td1;
117 T t2 = ta2 + tc2 + td2;
118 T t3 = ta3 + tc3 + td3;
119 T t4 = ta4 + tc4 + td4;
120 T t5 = ta5 + tc5 + td5;
121 T t6 = ta6 + tc6 + td6;
122 T t7 = ta7 + tc7 + td7;
123 T t8 = ta8 + tc8 + td8;
125 T
t = ::std::numeric_limits<T>::max();
130 if (ta1 >= 0 && tc1 >= 0 && td1 >= 0 && ::std::abs(t1) < ::std::abs(t))
141 if (ta2 >= 0 && tc2 >= 0 && td2 >= 0 && ::std::abs(t2) < ::std::abs(t))
152 if (ta3 >= 0 && tc3 >= 0 && td3 >= 0 && ::std::abs(t3) < ::std::abs(t))
163 if (ta4 >= 0 && tc4 >= 0 && td4 >= 0 && ::std::abs(t4) < ::std::abs(t))
174 if (ta5 >= 0 && tc5 >= 0 && td5 >= 0 && ::std::abs(t5) < ::std::abs(t))
185 if (ta6 >= 0 && tc6 >= 0 && td6 >= 0 && ::std::abs(t6) < ::std::abs(t))
196 if (ta7 >= 0 && tc7 >= 0 && td7 >= 0 && ::std::abs(t7) < ::std::abs(t))
207 if (ta8 >= 0 && tc8 >= 0 && td8 >= 0 && ::std::abs(t8) < ::std::abs(t))
227 T ta1a = ( v1 -
v0) /
am;
228 T ta1b = (-v1 -
v0) /
am;
229 T ta2a = ( v2 -
v0) /
am;
230 T ta2b = (-v2 -
v0) /
am;
231 T ta3a = (-v3 -
v0) /
am;
232 T ta3b = ( v3 -
v0) /
am;
233 T ta4a = (-v4 -
v0) /
am;
234 T ta4b = ( v4 -
v0) /
am;
235 T ta5a = ( v5 -
v0) / -
am;
236 T ta5b = (-v5 -
v0) / -
am;
237 T ta6a = ( v6 -
v0) / -
am;
238 T ta6b = (-v6 -
v0) / -
am;
239 T ta7a = (-v7 -
v0) / -
am;
240 T ta7b = ( v7 -
v0) / -
am;
241 T ta8a = (-v8 -
v0) / -
am;
242 T ta8b = ( v8 -
v0) / -
am;
244 T td1a = (
ve - v1) / -
dm;
245 T td1b = (
ve + v1) / -
dm;
246 T td2a = (
ve - v2) /
dm;
247 T td2b = (
ve + v2) /
dm;
248 T td3a = (
ve + v3) / -
dm;
249 T td3b = (
ve - v3) / -
dm;
250 T td4a = (
ve + v4) /
dm;
251 T td4b = (
ve - v4) /
dm;
252 T td5a = (
ve - v5) / -
dm;
253 T td5b = (
ve + v5) / -
dm;
254 T td6a = (
ve - v6) /
dm;
255 T td6b = (
ve + v6) /
dm;
256 T td7a = (
ve + v7) / -
dm;
257 T td7b = (
ve - v7) / -
dm;
258 T td8a = (
ve + v8) /
dm;
259 T td8b = (
ve - v8) /
dm;
279 if (ta1a >= 0 && td1a >= 0 && ::std::abs(t1a) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
289 if (ta1b >= 0 && td1b >= 0 && ::std::abs(t1b) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
300 if (ta2a >= 0 && td2a >= 0 && ::std::abs(t2a) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
310 if (ta2b >= 0 && td2 >= 0 && ::std::abs(t2b) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
321 if (ta3a >= 0 && td3a >= 0 && ::std::abs(t3a) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
331 if (ta3b >= 0 && td3b >= 0 && ::std::abs(t3b) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
342 if (ta4a >= 0 && td4a >= 0 && ::std::abs(t4a) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
352 if (ta4b >= 0 && td4b >= 0 && ::std::abs(t4b) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
363 if (ta5a >= 0 && td5a >= 0 && ::std::abs(t5a) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
373 if (ta5b >= 0 && td5b >= 0 && ::std::abs(t5b) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
384 if (ta6a >= 0 && td6a >= 0 && ::std::abs(t6a) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
394 if (ta6b >= 0 && td6b >= 0 && ::std::abs(t6b) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
405 if (ta7a >= 0 && td7a >= 0 && ::std::abs(t7a) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
415 if (ta7b >= 0 && td7b >= 0 && ::std::abs(t7b) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
426 if (ta8a >= 0 && td8a >= 0 && ::std::abs(t8a) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
436 if (ta8b >= 0 && td8b >= 0 && ::std::abs(t8b) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
453 T v1b = (dm *
v0 +
am *
ve + t *
am * dm - ::std::sqrt( 2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm - dm *
am *
ve *
ve - 2 * x *
am * dm * dm -
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm)) / ( dm +
am);
456 T v3a = -(dm *
v0 +
am *
ve + t *
am * dm - ::std::sqrt( 2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm -
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm - dm *
am *
ve *
ve - 2 * x *
am * dm * dm)) / (
am + dm);
457 T v3b = -(dm *
v0 +
am *
ve + t *
am * dm + ::std::sqrt( 2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm -
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm - dm *
am *
ve *
ve - 2 * x *
am * dm * dm)) / (
am + dm);
462 T v6a = (dm *
v0 +
am *
ve - t *
am * dm + ::std::sqrt( 2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm - dm *
am *
ve *
ve + 2 * x *
am * dm * dm -
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm)) / ( dm +
am);
463 T v6b = (dm *
v0 +
am *
ve - t *
am * dm - ::std::sqrt( 2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm - dm *
am *
ve *
ve + 2 * x *
am * dm * dm -
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm)) / ( dm +
am);
466 T v8a = -(dm *
v0 +
am *
ve - t *
am * dm - ::std::sqrt( 2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm -
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm - dm *
am *
ve *
ve + 2 * x *
am * dm * dm)) / (
am + dm);
467 T v8b = -(dm *
v0 +
am *
ve - t *
am * dm + ::std::sqrt( 2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm -
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm - dm *
am *
ve *
ve + 2 * x *
am * dm * dm)) / (
am + dm);
469 if (::std::abs(
am - dm) <= ::std::numeric_limits<T>::epsilon())
482 v2a = -(dm *
v0 -
am *
ve + t *
am * dm - ::std::sqrt(-2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm + dm *
am *
ve *
ve - 2 * x *
am * dm * dm +
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm)) / (-dm +
am);
483 v2b = -(dm *
v0 -
am *
ve + t *
am * dm + ::std::sqrt(-2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm + dm *
am *
ve *
ve - 2 * x *
am * dm * dm +
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm)) / (-dm +
am);
484 v4a = (dm *
v0 -
am *
ve + t *
am * dm + ::std::sqrt(-2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm +
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm + dm *
am *
ve *
ve - 2 * x *
am * dm * dm)) / (
am - dm);
485 v4b = (dm *
v0 -
am *
ve + t *
am * dm - ::std::sqrt(-2 * dm *
v0 *
am *
ve + 2 * dm * dm *
v0 * t *
am - 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm +
am * dm *
v0 *
v0 + 2 * x *
am *
am * dm + dm *
am *
ve *
ve - 2 * x *
am * dm * dm)) / (
am - dm);
486 v5a = -(dm *
v0 -
am *
ve - t *
am * dm - ::std::sqrt(-2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm + dm *
am *
ve *
ve + 2 * x *
am * dm * dm +
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm)) / (-dm +
am);
487 v5b = -(dm *
v0 -
am *
ve - t *
am * dm + ::std::sqrt(-2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm + dm *
am *
ve *
ve + 2 * x *
am * dm * dm +
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm)) / (-dm +
am);
488 v7a = (dm *
v0 -
am *
ve - t *
am * dm + ::std::sqrt(-2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm +
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm + dm *
am *
ve *
ve + 2 * x *
am * dm * dm)) / (
am - dm);
489 v7b = (dm *
v0 -
am *
ve - t *
am * dm - ::std::sqrt(-2 * dm *
v0 *
am *
ve - 2 * dm * dm *
v0 * t *
am + 2 *
am *
am *
ve * t * dm + t * t *
am *
am * dm * dm +
am * dm *
v0 *
v0 - 2 * x *
am *
am * dm + dm *
am *
ve *
ve + 2 * x *
am * dm * dm)) / (
am - dm);
492 T ta1a = ( v1a -
v0) /
am;
493 T ta1b = ( v1b -
v0) /
am;
494 T ta2a = ( v2a -
v0) /
am;
495 T ta2b = ( v2b -
v0) /
am;
496 T ta3a = (-v3a -
v0) /
am;
497 T ta3b = (-v3b -
v0) /
am;
498 T ta4a = (-v4a -
v0) /
am;
499 T ta4b = (-v4b -
v0) /
am;
500 T ta5a = ( v5a -
v0) / -
am;
501 T ta5b = ( v5b -
v0) / -
am;
502 T ta6a = ( v6a -
v0) / -
am;
503 T ta6b = ( v6b -
v0) / -
am;
504 T ta7a = (-v7a -
v0) / -
am;
505 T ta7b = (-v7b -
v0) / -
am;
506 T ta8a = (-v8a -
v0) / -
am;
507 T ta8b = (-v8b -
v0) / -
am;
509 T td1a = (
ve - v1a) / -dm;
510 T td1b = (
ve - v1b) / -dm;
511 T td2a = (
ve - v2a) / dm;
512 T td2b = (
ve - v2b) / dm;
513 T td3a = (
ve + v3a) / -dm;
514 T td3b = (
ve + v3b) / -dm;
515 T td4a = (
ve + v4a) / dm;
516 T td4b = (
ve + v4b) / dm;
517 T td5a = (
ve - v5a) / -dm;
518 T td5b = (
ve - v5b) / -dm;
519 T td6a = (
ve - v6a) / dm;
520 T td6b = (
ve - v6b) / dm;
521 T td7a = (
ve + v7a) / -dm;
522 T td7b = (
ve + v7b) / -dm;
523 T td8a = (
ve + v8a) / dm;
524 T td8b = (
ve + v8b) / dm;
526 T tc1a = t - ta1a - td1a;
527 T tc1b = t - ta1b - td1b;
528 T tc2a = t - ta2a - td2a;
529 T tc2b = t - ta2b - td2b;
530 T tc3a = t - ta3a - td3a;
531 T tc3b = t - ta3b - td3b;
532 T tc4a = t - ta4a - td4a;
533 T tc4b = t - ta4b - td4b;
534 T tc5a = t - ta5a - td5a;
535 T tc5b = t - ta5b - td5b;
536 T tc6a = t - ta6a - td6a;
537 T tc6b = t - ta6b - td6b;
538 T tc7a = t - ta7a - td7a;
539 T tc7b = t - ta7b - td7b;
540 T tc8a = t - ta8a - td8a;
541 T tc8b = t - ta8b - td8b;
546 if (ta1a >= 0 && tc1a >= 0 && td1a >= 0 && ::std::abs(v1a) < ::std::abs(vh))
555 if (ta1b >= 0 && tc1b >= 0 && td1b >= 0 && ::std::abs(v1b) < ::std::abs(vh))
565 if (ta2a >= 0 && tc2a >= 0 && td2a >= 0 && ::std::abs(v2a) < ::std::abs(vh))
574 if (ta2b >= 0 && tc2b >= 0 && td2b >= 0 && ::std::abs(v2b) < ::std::abs(vh))
584 if (ta3a >= 0 && tc3a >= 0 && td3a >= 0 && ::std::abs(v3a) < ::std::abs(vh))
593 if (ta3b >= 0 && tc3b >= 0 && td3b >= 0 && ::std::abs(v3b) < ::std::abs(vh))
603 if (ta4a >= 0 && tc4a >= 0 && td4a >= 0 && ::std::abs(v4a) < ::std::abs(vh))
612 if (ta4b >= 0 && tc4b >= 0 && td4b >= 0 && ::std::abs(v4b) < ::std::abs(vh))
622 if (ta5a >= 0 && tc5a >= 0 && td5a >= 0 && ::std::abs(v5a) < ::std::abs(vh))
631 if (ta5b >= 0 && tc5b >= 0 && td5b >= 0 && ::std::abs(v5b) < ::std::abs(vh))
641 if (ta6a >= 0 && tc6a >= 0 && td6a >= 0 && ::std::abs(v6a) < ::std::abs(vh))
650 if (ta6b >= 0 && tc6b >= 0 && td6b >= 0 && ::std::abs(v6b) < ::std::abs(vh))
660 if (ta7a >= 0 && tc7a >= 0 && td7a >= 0 && ::std::abs(v7a) < ::std::abs(vh))
669 if (ta7b >= 0 && tc7b >= 0 && td7b >= 0 && ::std::abs(v7b) < ::std::abs(vh))
679 if (ta8a >= 0 && tc8a >= 0 && td8a >= 0 && ::std::abs(v8a) < ::std::abs(vh))
688 if (ta8b >= 0 && tc8b >= 0 && td8b >= 0 && ::std::abs(v8b) < ::std::abs(vh))
710 else if (t <
ta +
tc)
714 else if (t <
ta +
tc +
td)
728 return x0 +
v0 * t + 0.5 *
ah * ::std::pow(t, 2);
730 else if (t <
ta +
tc)
732 return x0 +
v0 * t - 0.5 *
ah * ::std::pow(
ta, 2) +
ah *
ta *
t;
734 else if (t <
ta +
tc +
td)
736 return x0 +
v0 * t - 0.5 *
ah * ::std::pow(
ta, 2) +
ah *
ta * t - 0.5 *
dh * ::std::pow(
ta +
tc, 2) - 0.5 *
dh * ::std::pow(t, 2) +
dh * (
ta +
tc) * t;
774 #endif // _RL_MATH_TRAPEZOIDALVELOCITY_H_