41 #ifndef FCL_CCD_TAYLOR_MODEL_INL_H
42 #define FCL_CCD_TAYLOR_MODEL_INL_H
81 time_interval_->setValue(l,
r);
88 time_interval_ = time_interval;
95 return time_interval_;
106 template <
typename S>
113 template <
typename S>
120 template <
typename S>
127 template <
typename S>
130 coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
134 template <
typename S>
141 template <
typename S>
149 template <
typename S>
161 template <
typename S>
173 template <
typename S>
176 return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
180 template <
typename S>
188 template <
typename S>
191 return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
195 template <
typename S>
203 template <
typename S>
211 template <
typename S>
219 template <
typename S>
223 coeffs_[0] += other.
coeffs_[0];
224 coeffs_[1] += other.
coeffs_[1];
225 coeffs_[2] += other.
coeffs_[2];
226 coeffs_[3] += other.
coeffs_[3];
232 template <
typename S>
236 coeffs_[0] -= other.
coeffs_[0];
237 coeffs_[1] -= other.
coeffs_[1];
238 coeffs_[2] -= other.
coeffs_[2];
239 coeffs_[3] -= other.
coeffs_[3];
258 template <
typename S>
267 template <
typename S>
270 return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_);
274 template <
typename S>
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;
289 S tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
290 remainder += time_interval_->t4_ * tempVal;
292 tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
293 remainder += time_interval_->t5_ * tempVal;
295 tempVal = coeffs_[3] * c3b;
296 remainder += time_interval_->t6_ * tempVal;
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_);
312 template <
typename S>
324 template <
typename S>
327 return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
331 template <
typename S>
334 std::cout << coeffs_[0] <<
"+" << coeffs_[1] <<
"*t+" << coeffs_[2] <<
"*t^2+" << coeffs_[3] <<
"*t^3+[" << r_[0] <<
"," << r_[1] <<
"]" << std::endl;
338 template <
typename S>
341 return Interval<S>(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
345 template <
typename S>
352 return Interval<S>(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
356 template <
typename S>
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];
363 template <
typename S>
366 if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
367 if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];
371 S a = -coeffs_[1] / (2 * coeffs_[2]);
373 if(a <= t1 && a >= t0)
375 S AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
377 S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
379 S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
381 S minQ = LQ, maxQ = RQ;
388 if(minQ > AQ) minQ = AQ;
389 if(maxQ < AQ) maxQ = AQ;
396 S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
398 S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
400 if(LQ > RQ) polybounds.
setValue(RQ, LQ);
404 return polybounds + r_;
409 S LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
411 S RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
422 S delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
426 S r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
427 S r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
429 if(r1 <= t1 && r1 >= t0)
431 S Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
433 else if(Q > RQ) RQ = Q;
436 if(r2 <= t1 && r2 >= t0)
438 S Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
440 else if(Q > RQ) RQ = Q;
448 template <
typename S>
451 return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
455 template <
typename S>
458 coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
463 template <
typename S>
476 template <
typename S>
483 template <
typename S>
490 template <
typename S>
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;
514 if(cosQL < cosQR) fddddBounds.
setValue(cosQL, cosQR);
515 else fddddBounds.
setValue(cosQR, cosQL);
518 fddddBounds[0] -= 1e-15;
519 fddddBounds[1] += 1e-15;
530 if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
533 if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
537 if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
540 if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
548 S midSize2 = midSize * midSize;
549 S midSize4 = midSize2 * midSize2;
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);
557 tm.
remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
561 template <
typename S>
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;
587 if(sinQL < sinQR) fddddBounds.
setValue(sinQL, sinQR);
588 else fddddBounds.
setValue(sinQR, sinQL);
591 fddddBounds[0] -= 1e-15;
592 fddddBounds[1] += 1e-15;
602 if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
605 if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
609 if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
612 if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
619 S midSize2 = midSize * midSize;
620 S midSize4 = midSize2 * midSize2;
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);
628 tm.
remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
633 template <
typename S>