Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
TrapezoidalVelocity.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_MATH_TRAPEZOIDALVELOCITY_H_
31 #define _RL_MATH_TRAPEZOIDALVELOCITY_H_
32 
33 #include <cassert>
34 
35 #include "Real.h"
36 
37 namespace rl
38 {
39  namespace math
40  {
41  template< typename T >
43  {
44  public:
46  am(0),
47  dm(0),
48  x0(0),
49  xe(0),
50  v0(0),
51  ve(0),
52  vm(0),
53  ah(0),
54  dh(0),
55  ta(::std::numeric_limits<Real>::max()),
56  tc(::std::numeric_limits<Real>::max()),
57  td(::std::numeric_limits<Real>::max())
58  {
59  }
60 
62  {
63  }
64 
65  T a(const Real& t) const
66  {
67  if (t < ta)
68  {
69  return ah;
70  }
71  else if (t < ta + tc)
72  {
73  return 0;
74  }
75  else if (t < ta + tc + td)
76  {
77  return -dh;
78  }
79  else
80  {
81  return 0;
82  }
83  }
84 
85  void interpolate()
86  {
87  T x = xe - x0;
88 
89  T ta1 = ( vm - v0) / am;
90  T ta2 = ( vm - v0) / am;
91  T ta3 = (-vm - v0) / am;
92  T ta4 = (-vm - v0) / am;
93  T ta5 = ( vm - v0) / -am;
94  T ta6 = ( vm - v0) / -am;
95  T ta7 = (-vm - v0) / -am;
96  T ta8 = (-vm - v0) / -am;
97 
98  T tc1 = ( dm * v0 * v0 - dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
99  T tc2 = ( dm * v0 * v0 - dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
100  T tc3 = -( dm * v0 * v0 - dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / dm / am / 2;
101  T tc4 = -( dm * v0 * v0 - dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
102  T tc5 = (-dm * v0 * v0 + dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
103  T tc6 = (-dm * v0 * v0 + dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
104  T tc7 = -(-dm * v0 * v0 + dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
105  T tc8 = -(-dm * v0 * v0 + dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
106 
107  T td1 = (ve - vm) / -dm;
108  T td2 = (ve - vm) / dm;
109  T td3 = (ve + vm) / -dm;
110  T td4 = (ve + vm) / dm;
111  T td5 = (ve - vm) / -dm;
112  T td6 = (ve - vm) / dm;
113  T td7 = (ve + vm) / -dm;
114  T td8 = (ve + vm) / dm;
115 
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;
124 
125  T t = ::std::numeric_limits<T>::max();
126 
127  T vh = vm;
128 
129  // am, vm, -dm
130  if (ta1 >= 0 && tc1 >= 0 && td1 >= 0 && ::std::abs(t1) < ::std::abs(t))
131  {
132  t = t1;
133  ta = ta1;
134  tc = tc1;
135  td = td1;
136  ah = am;
137  vh = vm;
138  dh = dm;
139  }
140  // am, vm, dm
141  if (ta2 >= 0 && tc2 >= 0 && td2 >= 0 && ::std::abs(t2) < ::std::abs(t))
142  {
143  t = t2;
144  ta = ta2;
145  tc = tc2;
146  td = td2;
147  ah = am;
148  vh = vm;
149  dh = -dm;
150  }
151  // am, -vm, -dm
152  if (ta3 >= 0 && tc3 >= 0 && td3 >= 0 && ::std::abs(t3) < ::std::abs(t))
153  {
154  t = t3;
155  ta = ta3;
156  tc = tc3;
157  td = td3;
158  ah = am;
159  vh = -vm;
160  dh = dm;
161  }
162  // am, -vm, dm
163  if (ta4 >= 0 && tc4 >= 0 && td4 >= 0 && ::std::abs(t4) < ::std::abs(t))
164  {
165  t = t4;
166  ta = ta4;
167  tc = tc4;
168  td = td4;
169  ah = am;
170  vh = -vm;
171  dh = -dm;
172  }
173  // -am, vm, -dm
174  if (ta5 >= 0 && tc5 >= 0 && td5 >= 0 && ::std::abs(t5) < ::std::abs(t))
175  {
176  t = t5;
177  ta = ta5;
178  tc = tc5;
179  td = td5;
180  ah = -am;
181  vh = vm;
182  dh = dm;
183  }
184  // -am, vm, dm
185  if (ta6 >= 0 && tc6 >= 0 && td6 >= 0 && ::std::abs(t6) < ::std::abs(t))
186  {
187  t = t6;
188  ta = ta6;
189  tc = tc6;
190  td = td6;
191  ah = -am;
192  vh = vm;
193  dh = -dm;
194  }
195  // -am, -vm, -dm
196  if (ta7 >= 0 && tc7 >= 0 && td7 >= 0 && ::std::abs(t7) < ::std::abs(t))
197  {
198  t = t7;
199  ta = ta7;
200  tc = tc7;
201  td = td7;
202  ah = -am;
203  vh = -vm;
204  dh = dm;
205  }
206  // -am, -vm, dm
207  if (ta8 >= 0 && tc8 >= 0 && td8 >= 0 && ::std::abs(t8) < ::std::abs(t))
208  {
209  t = t8;
210  ta = ta8;
211  tc = tc8;
212  td = td8;
213  ah = -am;
214  vh = -vm;
215  dh = -dm;
216  }
217 
218  T v1 = 1 / ( dm + am) * ::std::sqrt( ( dm + am) * ( dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
219  T v2 = 1 / (-dm + am) * ::std::sqrt(-(-dm + am) * ( dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
220  T v3 = 1 / ( dm + am) * ::std::sqrt( ( dm + am) * ( dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
221  T v4 = 1 / (-dm + am) * ::std::sqrt(-(-dm + am) * ( dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
222  T v5 = 1 / (-dm + am) * ::std::sqrt( (-dm + am) * (-dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
223  T v6 = 1 / ( dm + am) * ::std::sqrt(-( dm + am) * (-dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
224  T v7 = 1 / (-dm + am) * ::std::sqrt( (-dm + am) * (-dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
225  T v8 = 1 / ( dm + am) * ::std::sqrt(-( dm + am) * (-dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
226 
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;
243 
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;
260 
261  T t1a = ta1a + td1a;
262  T t1b = ta1b + td1b;
263  T t2a = ta2a + td2a;
264  T t2b = ta2b + td2b;
265  T t3a = ta3a + td3a;
266  T t3b = ta3b + td3b;
267  T t4a = ta4a + td4a;
268  T t4b = ta4b + td4b;
269  T t5a = ta5a + td5a;
270  T t5b = ta5b + td5b;
271  T t6a = ta6a + td6a;
272  T t6b = ta6b + td6b;
273  T t7a = ta7a + td7a;
274  T t7b = ta7b + td7b;
275  T t8a = ta8a + td8a;
276  T t8b = ta8b + td8b;
277 
278  // am, vm, -dm
279  if (ta1a >= 0 && td1a >= 0 && ::std::abs(t1a) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
280  {
281  t = t1a;
282  ta = ta1a;
283  tc = 0;
284  td = td1a;
285  ah = am;
286  vh = v1;
287  dh = dm;
288  }
289  if (ta1b >= 0 && td1b >= 0 && ::std::abs(t1b) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
290  {
291  t = t1b;
292  ta = ta1b;
293  tc = 0;
294  td = td1b;
295  ah = am;
296  vh = -v1;
297  dh = dm;
298  }
299  // am, vm, dm
300  if (ta2a >= 0 && td2a >= 0 && ::std::abs(t2a) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
301  {
302  t = t2a;
303  ta = ta2a;
304  tc = 0;
305  td = td2a;
306  ah = am;
307  vh = v2;
308  dh = -dm;
309  }
310  if (ta2b >= 0 && td2 >= 0 && ::std::abs(t2b) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
311  {
312  t = t2b;
313  ta = ta2b;
314  tc = 0;
315  td = td2b;
316  ah = am;
317  vh = -v2;
318  dh = -dm;
319  }
320  // am, -vm, -dm
321  if (ta3a >= 0 && td3a >= 0 && ::std::abs(t3a) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
322  {
323  t = t3a;
324  ta = ta3a;
325  tc = 0;
326  td = td3a;
327  ah = am;
328  vh = -v3;
329  dh = dm;
330  }
331  if (ta3b >= 0 && td3b >= 0 && ::std::abs(t3b) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
332  {
333  t = t3b;
334  ta = ta3b;
335  tc = 0;
336  td = td3b;
337  ah = am;
338  vh = v3;
339  dh = dm;
340  }
341  // am, -vm, dm
342  if (ta4a >= 0 && td4a >= 0 && ::std::abs(t4a) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
343  {
344  t = t4a;
345  ta = ta4a;
346  tc = 0;
347  td = td4a;
348  ah = am;
349  vh = -v4;
350  dh = -dm;
351  }
352  if (ta4b >= 0 && td4b >= 0 && ::std::abs(t4b) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
353  {
354  t = t4b;
355  ta = ta4b;
356  tc = 0;
357  td = td4b;
358  ah = am;
359  vh = v4;
360  dh = -dm;
361  }
362  // -am, vm, -dm
363  if (ta5a >= 0 && td5a >= 0 && ::std::abs(t5a) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
364  {
365  t = t5a;
366  ta = ta5a;
367  tc = 0;
368  td = td5a;
369  ah = -am;
370  vh = v5;
371  dh = dm;
372  }
373  if (ta5b >= 0 && td5b >= 0 && ::std::abs(t5b) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
374  {
375  t = t5b;
376  ta = ta5b;
377  tc = 0;
378  td = td5b;
379  ah = -am;
380  vh = -v5;
381  dh = dm;
382  }
383  // -am, vm, dm
384  if (ta6a >= 0 && td6a >= 0 && ::std::abs(t6a) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
385  {
386  t = t6a;
387  ta = ta6a;
388  tc = 0;
389  td = td6a;
390  ah = -am;
391  vh = v6;
392  dh = -dm;
393  }
394  if (ta6b >= 0 && td6b >= 0 && ::std::abs(t6b) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
395  {
396  t = t6b;
397  ta = ta6b;
398  tc = 0;
399  td = td6b;
400  ah = -am;
401  vh = -v6;
402  dh = -dm;
403  }
404  // -am, -vm, -dm
405  if (ta7a >= 0 && td7a >= 0 && ::std::abs(t7a) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
406  {
407  t = t7a;
408  ta = ta7a;
409  tc = 0;
410  td = td7a;
411  ah = -am;
412  vh = -v7;
413  dh = dm;
414  }
415  if (ta7b >= 0 && td7b >= 0 && ::std::abs(t7b) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
416  {
417  t = t7b;
418  ta = ta7b;
419  tc = 0;
420  td = td7b;
421  ah = -am;
422  vh = v7;
423  dh = dm;
424  }
425  // -am, -vm, dm
426  if (ta8a >= 0 && td8a >= 0 && ::std::abs(t8a) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
427  {
428  t = t8a;
429  ta = ta8a;
430  tc = 0;
431  td = td8a;
432  ah = -am;
433  vh = -v8;
434  dh = -dm;
435  }
436  if (ta8b >= 0 && td8b >= 0 && ::std::abs(t8b) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
437  {
438  t = t8b;
439  ta = ta8b;
440  tc = 0;
441  td = td8b;
442  ah = -am;
443  vh = v8;
444  dh = -dm;
445  }
446  }
447 
448  void interpolate(const Real& t)
449  {
450  T x = xe - x0;
451 
452  T v1a = (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);
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);
454  T v2a;
455  T v2b;
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);
458  T v4a;
459  T v4b;
460  T v5a;
461  T v5b;
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);
464  T v7a;
465  T v7b;
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);
468 
469  if (::std::abs(am - dm) <= ::std::numeric_limits<T>::epsilon())
470  {
471  v2a = ( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
472  v2b = ( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
473  v4a = -( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
474  v4b = -( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
475  v5a = -(-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
476  v5b = -(-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
477  v7a = (-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
478  v7b = (-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
479  }
480  else
481  {
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);
490  }
491 
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;
508 
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;
525 
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;
542 
543  T vh = vm;
544 
545  // am, vm, -dm
546  if (ta1a >= 0 && tc1a >= 0 && td1a >= 0 && ::std::abs(v1a) < ::std::abs(vh))
547  {
548  ta = ta1a;
549  tc = tc1a;
550  td = td1a;
551  ah = am;
552  vh = v1a;
553  dh = dm;
554  }
555  if (ta1b >= 0 && tc1b >= 0 && td1b >= 0 && ::std::abs(v1b) < ::std::abs(vh))
556  {
557  ta = ta1b;
558  tc = tc1b;
559  td = td1b;
560  ah = am;
561  vh = v1b;
562  dh = dm;
563  }
564  // am, vm, dm
565  if (ta2a >= 0 && tc2a >= 0 && td2a >= 0 && ::std::abs(v2a) < ::std::abs(vh))
566  {
567  ta = ta2a;
568  tc = tc2a;
569  td = td2a;
570  ah = am;
571  vh = v2a;
572  dh = -dm;
573  }
574  if (ta2b >= 0 && tc2b >= 0 && td2b >= 0 && ::std::abs(v2b) < ::std::abs(vh))
575  {
576  ta = ta2b;
577  tc = tc2b;
578  td = td2b;
579  ah = am;
580  vh = v2b;
581  dh = -dm;
582  }
583  // am, -vm, -dm
584  if (ta3a >= 0 && tc3a >= 0 && td3a >= 0 && ::std::abs(v3a) < ::std::abs(vh))
585  {
586  ta = ta3a;
587  tc = tc3a;
588  td = td3a;
589  ah = am;
590  vh = -v3a;
591  dh = dm;
592  }
593  if (ta3b >= 0 && tc3b >= 0 && td3b >= 0 && ::std::abs(v3b) < ::std::abs(vh))
594  {
595  ta = ta3b;
596  tc = tc3b;
597  td = td3b;
598  ah = am;
599  vh = -v3b;
600  dh = dm;
601  }
602  // am, -vm, dm
603  if (ta4a >= 0 && tc4a >= 0 && td4a >= 0 && ::std::abs(v4a) < ::std::abs(vh))
604  {
605  ta = ta4a;
606  tc = tc4a;
607  td = td4a;
608  ah = am;
609  vh = -v4a;
610  dh = -dm;
611  }
612  if (ta4b >= 0 && tc4b >= 0 && td4b >= 0 && ::std::abs(v4b) < ::std::abs(vh))
613  {
614  ta = ta4b;
615  tc = tc4b;
616  td = td4b;
617  ah = am;
618  vh = -v4b;
619  dh = -dm;
620  }
621  // -am, vm, -dm
622  if (ta5a >= 0 && tc5a >= 0 && td5a >= 0 && ::std::abs(v5a) < ::std::abs(vh))
623  {
624  ta = ta5a;
625  tc = tc5a;
626  td = td5a;
627  ah = -am;
628  vh = v5a;
629  dh = dm;
630  }
631  if (ta5b >= 0 && tc5b >= 0 && td5b >= 0 && ::std::abs(v5b) < ::std::abs(vh))
632  {
633  ta = ta5b;
634  tc = tc5b;
635  td = td5b;
636  ah = -am;
637  vh = v5b;
638  dh = dm;
639  }
640  // -am, vm, dm
641  if (ta6a >= 0 && tc6a >= 0 && td6a >= 0 && ::std::abs(v6a) < ::std::abs(vh))
642  {
643  ta = ta6a;
644  tc = tc6a;
645  td = td6a;
646  ah = -am;
647  vh = v6a;
648  dh = -dm;
649  }
650  if (ta6b >= 0 && tc6b >= 0 && td6b >= 0 && ::std::abs(v6b) < ::std::abs(vh))
651  {
652  ta = ta6b;
653  tc = tc6b;
654  td = td6b;
655  ah = -am;
656  vh = v6b;
657  dh = -dm;
658  }
659  // -am, -vm, -dm
660  if (ta7a >= 0 && tc7a >= 0 && td7a >= 0 && ::std::abs(v7a) < ::std::abs(vh))
661  {
662  ta = ta7a;
663  tc = tc7a;
664  td = td7a;
665  ah = -am;
666  vh = -v7a;
667  dh = dm;
668  }
669  if (ta7b >= 0 && tc7b >= 0 && td7b >= 0 && ::std::abs(v7b) < ::std::abs(vh))
670  {
671  ta = ta7b;
672  tc = tc7b;
673  td = td7b;
674  ah = -am;
675  vh = -v7b;
676  dh = dm;
677  }
678  // -am, -vm, dm
679  if (ta8a >= 0 && tc8a >= 0 && td8a >= 0 && ::std::abs(v8a) < ::std::abs(vh))
680  {
681  ta = ta8a;
682  tc = tc8a;
683  td = td8a;
684  ah = -am;
685  vh = -v8a;
686  dh = -dm;
687  }
688  if (ta8b >= 0 && tc8b >= 0 && td8b >= 0 && ::std::abs(v8b) < ::std::abs(vh))
689  {
690  ta = ta8b;
691  tc = tc8b;
692  td = td8b;
693  ah = -am;
694  vh = -v8b;
695  dh = -dm;
696  }
697  }
698 
699  T t() const
700  {
701  return ta + tc + td;
702  }
703 
704  T v(const Real& t) const
705  {
706  if (t < ta)
707  {
708  return v0 + ah * t;
709  }
710  else if (t < ta + tc)
711  {
712  return v0 + ah * ta;
713  }
714  else if (t < ta + tc + td)
715  {
716  return v0 + ah * ta - dh * t + dh * (ta + tc);
717  }
718  else
719  {
720  return ve;
721  }
722  }
723 
724  T x(const Real& t) const
725  {
726  if (t < ta)
727  {
728  return x0 + v0 * t + 0.5 * ah * ::std::pow(t, 2);
729  }
730  else if (t < ta + tc)
731  {
732  return x0 + v0 * t - 0.5 * ah * ::std::pow(ta, 2) + ah * ta * t;
733  }
734  else if (t < ta + tc + td)
735  {
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;
737  }
738  else
739  {
740  return xe + ve * (t - ta - tc - td);
741  }
742  }
743 
744  T am;
745 
746  T dm;
747 
748  T x0;
749 
750  T xe;
751 
752  T v0;
753 
754  T ve;
755 
756  T vm;
757 
758  protected:
759 
760  private:
761  T ah;
762 
763  T dh;
764 
766 
768 
770  };
771  }
772 }
773 
774 #endif // _RL_MATH_TRAPEZOIDALVELOCITY_H_