taylor_model-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 // This code is based on code developed by Stephane Redon at UNC and Inria for
37 // the CATCH library: http://graphics.ewha.ac.kr/CATCH/
38 
41 #ifndef FCL_CCD_TAYLOR_MODEL_INL_H
42 #define FCL_CCD_TAYLOR_MODEL_INL_H
43 
44 #include "fcl/math/motion/taylor_model/taylor_model.h"
45 
46 namespace fcl
47 {
48 
49 //==============================================================================
50 extern template
51 class FCL_EXPORT TaylorModel<double>;
52 
53 //==============================================================================
54 extern template
55 TaylorModel<double> operator * (double d, const TaylorModel<double>& a);
56 
57 //==============================================================================
58 extern template
59 TaylorModel<double> operator + (double d, const TaylorModel<double>& a);
60 
61 //==============================================================================
62 extern template
63 TaylorModel<double> operator - (double d, const TaylorModel<double>& a);
64 
65 //==============================================================================
66 extern template
67 void generateTaylorModelForCosFunc(TaylorModel<double>& tm, double w, double q0);
68 
69 //==============================================================================
70 extern template
71 void generateTaylorModelForSinFunc(TaylorModel<double>& tm, double w, double q0);
72 
73 //==============================================================================
74 extern template
75 void generateTaylorModelForLinearFunc(TaylorModel<double>& tm, double p, double v);
76 
77 //==============================================================================
78 template <typename S>
79 void TaylorModel<S>::setTimeInterval(S l, S r)
80 {
81  time_interval_->setValue(l, r);
82 }
83 
84 //==============================================================================
85 template <typename S>
86 void TaylorModel<S>::setTimeInterval(const std::shared_ptr<TimeInterval<S> >& time_interval)
87 {
88  time_interval_ = time_interval;
89 }
90 
91 //==============================================================================
92 template <typename S>
93 const std::shared_ptr<TimeInterval<S> >&TaylorModel<S>::getTimeInterval() const
94 {
95  return time_interval_;
96 }
97 
98 //==============================================================================
99 template <typename S>
100 S TaylorModel<S>::coeff(std::size_t i) const
101 {
102  return coeffs_[i];
103 }
104 
105 //==============================================================================
106 template <typename S>
107 S&TaylorModel<S>::coeff(std::size_t i)
108 {
109  return coeffs_[i];
110 }
111 
112 //==============================================================================
113 template <typename S>
114 const Interval<S>&TaylorModel<S>::remainder() const
115 {
116  return r_;
117 }
118 
119 //==============================================================================
120 template <typename S>
121 Interval<S>&TaylorModel<S>::remainder()
122 {
123  return r_;
124 }
125 
126 //==============================================================================
127 template <typename S>
128 TaylorModel<S>::TaylorModel()
129 {
130  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
131 }
132 
133 //==============================================================================
134 template <typename S>
135 TaylorModel<S>::TaylorModel(const std::shared_ptr<TimeInterval<S>>& time_interval) : time_interval_(time_interval)
136 {
137  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
138 }
139 
140 //==============================================================================
141 template <typename S>
142 TaylorModel<S>::TaylorModel(S coeff, const std::shared_ptr<TimeInterval<S>>& time_interval) : time_interval_(time_interval)
143 {
144  coeffs_[0] = coeff;
145  coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
146 }
147 
148 //==============================================================================
149 template <typename S>
150 TaylorModel<S>::TaylorModel(S coeffs[3], const Interval<S>& r, const std::shared_ptr<TimeInterval<S>>& time_interval) : time_interval_(time_interval)
151 {
152  coeffs_[0] = coeffs[0];
153  coeffs_[1] = coeffs[1];
154  coeffs_[2] = coeffs[2];
155  coeffs_[3] = coeffs[3];
156 
157  r_ = r;
158 }
159 
160 //==============================================================================
161 template <typename S>
162 TaylorModel<S>::TaylorModel(S c0, S c1, S c2, S c3, const Interval<S>& r, const std::shared_ptr<TimeInterval<S>>& time_interval) : time_interval_(time_interval)
163 {
164  coeffs_[0] = c0;
165  coeffs_[1] = c1;
166  coeffs_[2] = c2;
167  coeffs_[3] = c3;
168 
169  r_ = r;
170 }
171 
172 //==============================================================================
173 template <typename S>
174 TaylorModel<S> TaylorModel<S>::operator + (S d) const
175 {
176  return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
177 }
178 
179 //==============================================================================
180 template <typename S>
181 TaylorModel<S>& TaylorModel<S>::operator += (S d)
182 {
183  coeffs_[0] += d;
184  return *this;
185 }
186 
187 //==============================================================================
188 template <typename S>
189 TaylorModel<S> TaylorModel<S>::operator - (S d) const
190 {
191  return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
192 }
193 
194 //==============================================================================
195 template <typename S>
196 TaylorModel<S>& TaylorModel<S>::operator -= (S d)
197 {
198  coeffs_[0] -= d;
199  return *this;
200 }
201 
202 //==============================================================================
203 template <typename S>
204 TaylorModel<S> TaylorModel<S>::operator + (const TaylorModel<S>& other) const
205 {
206  assert(other.time_interval_ == time_interval_);
207  return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_);
208 }
209 
210 //==============================================================================
211 template <typename S>
212 TaylorModel<S> TaylorModel<S>::operator - (const TaylorModel<S>& other) const
213 {
214  assert(other.time_interval_ == time_interval_);
215  return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_);
216 }
217 
218 //==============================================================================
219 template <typename S>
220 TaylorModel<S>& TaylorModel<S>::operator += (const TaylorModel<S>& other)
221 {
222  assert(other.time_interval_ == time_interval_);
223  coeffs_[0] += other.coeffs_[0];
224  coeffs_[1] += other.coeffs_[1];
225  coeffs_[2] += other.coeffs_[2];
226  coeffs_[3] += other.coeffs_[3];
227  r_ += other.r_;
228  return *this;
229 }
230 
231 //==============================================================================
232 template <typename S>
233 TaylorModel<S>& TaylorModel<S>::operator -= (const TaylorModel<S>& other)
234 {
235  assert(other.time_interval_ == time_interval_);
236  coeffs_[0] -= other.coeffs_[0];
237  coeffs_[1] -= other.coeffs_[1];
238  coeffs_[2] -= other.coeffs_[2];
239  coeffs_[3] -= other.coeffs_[3];
240  r_ -= other.r_;
241  return *this;
242 }
243 
244 //==============================================================================
258 template <typename S>
259 TaylorModel<S> TaylorModel<S>::operator * (const TaylorModel<S>& other) const
260 {
261  TaylorModel res(*this);
262  res *= other;
263  return res;
264 }
265 
266 //==============================================================================
267 template <typename S>
268 TaylorModel<S> TaylorModel<S>::operator * (S d) const
269 {
270  return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_);
271 }
272 
273 //==============================================================================
274 template <typename S>
275 TaylorModel<S>& TaylorModel<S>::operator *= (const TaylorModel<S>& other)
276 {
277  assert(other.time_interval_ == time_interval_);
278  S c0, c1, c2, c3;
279  S c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
280 
281  const Interval<S>& rb = other.r_;
282 
283  c0 = coeffs_[0] * c0b;
284  c1 = coeffs_[0] * c1b + coeffs_[1] * c0b;
285  c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b;
286  c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;
287 
288  Interval<S> remainder(r_ * rb);
289  S tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
290  remainder += time_interval_->t4_ * tempVal;
291 
292  tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
293  remainder += time_interval_->t5_ * tempVal;
294 
295  tempVal = coeffs_[3] * c3b;
296  remainder += time_interval_->t6_ * tempVal;
297 
298  remainder += ((Interval<S>(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb +
299  (Interval<S>(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_);
300 
301  coeffs_[0] = c0;
302  coeffs_[1] = c1;
303  coeffs_[2] = c2;
304  coeffs_[3] = c3;
305 
306  r_ = remainder;
307 
308  return *this;
309 }
310 
311 //==============================================================================
312 template <typename S>
313 TaylorModel<S>& TaylorModel<S>::operator *= (S d)
314 {
315  coeffs_[0] *= d;
316  coeffs_[1] *= d;
317  coeffs_[2] *= d;
318  coeffs_[3] *= d;
319  r_ *= d;
320  return *this;
321 }
322 
323 //==============================================================================
324 template <typename S>
325 TaylorModel<S> TaylorModel<S>::operator - () const
326 {
327  return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
328 }
329 
330 //==============================================================================
331 template <typename S>
332 void TaylorModel<S>::print() const
333 {
334  std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
335 }
336 
337 //==============================================================================
338 template <typename S>
339 Interval<S> TaylorModel<S>::getBound(S t) const
340 {
341  return Interval<S>(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
342 }
343 
344 //==============================================================================
345 template <typename S>
346 Interval<S> TaylorModel<S>::getBound(S t0, S t1) const
347 {
348  Interval<S> t(t0, t1);
349  Interval<S> t2(t0 * t0, t1 * t1);
350  Interval<S> t3(t0 * t2[0], t1 * t2[1]);
351 
352  return Interval<S>(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
353 }
354 
355 //==============================================================================
356 template <typename S>
357 Interval<S> TaylorModel<S>::getBound() const
358 {
359  return Interval<S>(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3];
360 }
361 
362 //==============================================================================
363 template <typename S>
364 Interval<S> TaylorModel<S>::getTightBound(S t0, S t1) const
365 {
366  if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
367  if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];
368 
369  if(coeffs_[3] == 0)
370  {
371  S a = -coeffs_[1] / (2 * coeffs_[2]);
372  Interval<S> polybounds;
373  if(a <= t1 && a >= t0)
374  {
375  S AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
376  S t = t0;
377  S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
378  t = t1;
379  S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
380 
381  S minQ = LQ, maxQ = RQ;
382  if(LQ > RQ)
383  {
384  minQ = RQ;
385  maxQ = LQ;
386  }
387 
388  if(minQ > AQ) minQ = AQ;
389  if(maxQ < AQ) maxQ = AQ;
390 
391  polybounds.setValue(minQ, maxQ);
392  }
393  else
394  {
395  S t = t0;
396  S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
397  t = t1;
398  S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
399 
400  if(LQ > RQ) polybounds.setValue(RQ, LQ);
401  else polybounds.setValue(LQ, RQ);
402  }
403 
404  return polybounds + r_;
405  }
406  else
407  {
408  S t = t0;
409  S LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
410  t = t1;
411  S RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
412 
413  if(LQ > RQ)
414  {
415  S tmp = LQ;
416  LQ = RQ;
417  RQ = tmp;
418  }
419 
420  // derivative: c1+2*c2*t+3*c3*t^2
421 
422  S delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
423  if(delta < 0)
424  return Interval<S>(LQ, RQ) + r_;
425 
426  S r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
427  S r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
428 
429  if(r1 <= t1 && r1 >= t0)
430  {
431  S Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
432  if(Q < LQ) LQ = Q;
433  else if(Q > RQ) RQ = Q;
434  }
435 
436  if(r2 <= t1 && r2 >= t0)
437  {
438  S Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
439  if(Q < LQ) LQ = Q;
440  else if(Q > RQ) RQ = Q;
441  }
442 
443  return Interval<S>(LQ, RQ) + r_;
444  }
445 }
446 
447 //==============================================================================
448 template <typename S>
449 Interval<S> TaylorModel<S>::getTightBound() const
450 {
451  return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
452 }
453 
454 //==============================================================================
455 template <typename S>
456 void TaylorModel<S>::setZero()
457 {
458  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
459  r_.setValue(0);
460 }
461 
462 //==============================================================================
463 template <typename S>
464 TaylorModel<S> operator * (S d, const TaylorModel<S>& a)
465 {
466  TaylorModel<S> res(a);
467  res.coeff(0) *= d;
468  res.coeff(1) *= d;
469  res.coeff(2) *= d;
470  res.coeff(3) *= d;
471  res.remainder() *= d;
472  return res;
473 }
474 
475 //==============================================================================
476 template <typename S>
477 TaylorModel<S> operator + (S d, const TaylorModel<S>& a)
478 {
479  return a + d;
480 }
481 
482 //==============================================================================
483 template <typename S>
484 TaylorModel<S> operator - (S d, const TaylorModel<S>& a)
485 {
486  return -a + d;
487 }
488 
489 //==============================================================================
490 template <typename S>
491 void generateTaylorModelForCosFunc(TaylorModel<S>& tm, S w, S q0)
492 {
493  S a = tm.getTimeInterval()->t_.center();
494  S t = w * a + q0;
495  S w2 = w * w;
496  S fa = cos(t);
497  S fda = -w*sin(t);
498  S fdda = -w2*fa;
499  S fddda = -w2*fda;
500 
501  tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
502  tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
503  tm.coeff(2) = 0.5*(fdda-a*fddda);
504  tm.coeff(3) = 1.0/6.0*fddda;
505 
506  // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1]
507  Interval<S> fddddBounds;
508  if(w == 0) fddddBounds.setValue(0);
509  else
510  {
511  S cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0);
512  S cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0);
513 
514  if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
515  else fddddBounds.setValue(cosQR, cosQL);
516 
517  // enlarge to handle round-off errors
518  fddddBounds[0] -= 1e-15;
519  fddddBounds[1] += 1e-15;
520 
521  // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi];
522  // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi]
523 
524  S k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants<S>::pi());
525  S k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants<S>::pi());
526 
527 
528  if(w > 0)
529  {
530  if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
531  k1 -= 0.5;
532  k2 -= 0.5;
533  if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
534  }
535  else
536  {
537  if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
538  k1 -= 0.5;
539  k2 -= 0.5;
540  if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
541  }
542  }
543 
544  S w4 = w2 * w2;
545  fddddBounds *= w4;
546 
547  S midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
548  S midSize2 = midSize * midSize;
549  S midSize4 = midSize2 * midSize2;
550 
551  // [0, midSize4] * fdddBounds
552  if(fddddBounds[0] > 0)
553  tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
554  else if(fddddBounds[0] < 0)
555  tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
556  else
557  tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
558 }
559 
560 //==============================================================================
561 template <typename S>
562 void generateTaylorModelForSinFunc(TaylorModel<S>& tm, S w, S q0)
563 {
564  S a = tm.getTimeInterval()->t_.center();
565  S t = w * a + q0;
566  S w2 = w * w;
567  S fa = sin(t);
568  S fda = w*cos(t);
569  S fdda = -w2*fa;
570  S fddda = -w2*fda;
571 
572  tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
573  tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
574  tm.coeff(2) = 0.5*(fdda-a*fddda);
575  tm.coeff(3) = 1.0/6.0*fddda;
576 
577  // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1]
578 
579  Interval<S> fddddBounds;
580 
581  if(w == 0) fddddBounds.setValue(0);
582  else
583  {
584  S sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0);
585  S sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0);
586 
587  if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
588  else fddddBounds.setValue(sinQR, sinQL);
589 
590  // enlarge to handle round-off errors
591  fddddBounds[0] -= 1e-15;
592  fddddBounds[1] += 1e-15;
593 
594  // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi];
595  // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi]
596 
597  S k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants<S>::pi()) - 0.25;
598  S k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants<S>::pi()) - 0.25;
599 
600  if(w > 0)
601  {
602  if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
603  k1 -= 0.5;
604  k2 -= 0.5;
605  if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
606  }
607  else
608  {
609  if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
610  k1 -= 0.5;
611  k2 -= 0.5;
612  if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
613  }
614 
615  S w4 = w2 * w2;
616  fddddBounds *= w4;
617 
618  S midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
619  S midSize2 = midSize * midSize;
620  S midSize4 = midSize2 * midSize2;
621 
622  // [0, midSize4] * fdddBounds
623  if(fddddBounds[0] > 0)
624  tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
625  else if(fddddBounds[0] < 0)
626  tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
627  else
628  tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
629  }
630 }
631 
632 //==============================================================================
633 template <typename S>
634 void generateTaylorModelForLinearFunc(TaylorModel<S>& tm, S p, S v)
635 {
636  tm.coeff(0) = p;
637  tm.coeff(1) = v;
638  tm.coeff(2) = 0;
639  tm.coeff(3) = 0;
640  tm.remainder()[0] = 0;
641  tm.remainder()[1] = 0;
642 }
643 
644 } // namespace fcl
645 
646 #endif
fcl::TaylorModel::print
void print() const
Definition: taylor_model-inl.h:332
fcl::TaylorModel::getBound
Interval< S > getBound() const
Definition: taylor_model-inl.h:357
fcl::TaylorModel::operator-
TaylorModel operator-() const
Definition: taylor_model-inl.h:325
fcl::generateTaylorModelForLinearFunc
template void generateTaylorModelForLinearFunc(TaylorModel< double > &tm, double p, double v)
fcl::operator-
template TMatrix3< double > operator-(const Matrix3< double > &m1, const TMatrix3< double > &m2)
fcl::TaylorModel::TaylorModel
TaylorModel()
Definition: taylor_model-inl.h:128
fcl::TaylorModel::r_
Interval< S > r_
interval remainder
Definition: taylor_model.h:67
fcl::TaylorModel::setTimeInterval
void setTimeInterval(S l, S r)
Definition: taylor_model-inl.h:79
fcl::TaylorModel::operator+
TaylorModel operator+(const TaylorModel &other) const
Definition: taylor_model-inl.h:204
fcl::generateTaylorModelForSinFunc
template void generateTaylorModelForSinFunc(TaylorModel< double > &tm, double w, double q0)
fcl::TaylorModel::time_interval_
std::shared_ptr< TimeInterval< S > > time_interval_
time interval
Definition: taylor_model.h:61
fcl::TaylorModel::coeffs_
S coeffs_[4]
Coefficients of the cubic polynomial approximation.
Definition: taylor_model.h:64
fcl::TaylorModel< double >
template class FCL_EXPORT TaylorModel< double >
fcl::TaylorModel::coeff
S coeff(std::size_t i) const
Definition: taylor_model-inl.h:100
fcl::TaylorModel::operator-=
TaylorModel & operator-=(const TaylorModel &other)
Definition: taylor_model-inl.h:233
fcl::TimeInterval
Definition: time_interval.h:50
fcl::TaylorModel::getTightBound
Interval< S > getTightBound() const
Definition: taylor_model-inl.h:449
fcl::TaylorModel::getTimeInterval
const std::shared_ptr< TimeInterval< S > > & getTimeInterval() const
Definition: taylor_model-inl.h:93
fcl::TaylorModel::operator+=
TaylorModel & operator+=(const TaylorModel &other)
Definition: taylor_model-inl.h:220
fcl::generateTaylorModelForCosFunc
template void generateTaylorModelForCosFunc(TaylorModel< double > &tm, double w, double q0)
r
S r
Definition: test_sphere_box.cpp:171
fcl::operator*
template TMatrix3< double > operator*(const Matrix3< double > &m, const TaylorModel< double > &a)
fcl::TaylorModel
TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a t...
Definition: taylor_model.h:58
fcl::Interval
Interval class for [a, b].
Definition: interval.h:50
fcl::TaylorModel::operator*
TaylorModel operator*(const TaylorModel &other) const
Taylor model multiplication: f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b] g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,...
Definition: taylor_model-inl.h:259
fcl::Interval::setValue
void setValue(S a, S b)
construct interval [left, right]
Definition: interval-inl.h:82
fcl::constants
Definition: constants.h:129
fcl::TaylorModel::setZero
void setZero()
Definition: taylor_model-inl.h:456
fcl::TaylorModel::remainder
const Interval< S > & remainder() const
Definition: taylor_model-inl.h:114
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::TaylorModel::operator*=
TaylorModel & operator*=(const TaylorModel &other)
Definition: taylor_model-inl.h:275
taylor_model.h
fcl::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)

fcl
Author(s):
autogenerated on Fri Mar 14 2025 02:38:18