00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/ccd/taylor_model.h"
00038 #include <cassert>
00039 #include <iostream>
00040 #include <cmath>
00041 #include <boost/math/constants/constants.hpp>
00042
00043
00044 namespace fcl
00045 {
00046
00047 TaylorModel::TaylorModel()
00048 {
00049 coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
00050 }
00051
00052 TaylorModel::TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
00053 {
00054 coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
00055 }
00056
00057 TaylorModel::TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
00058 {
00059 coeffs_[0] = coeff;
00060 coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
00061 }
00062
00063 TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
00064 {
00065 coeffs_[0] = coeffs[0];
00066 coeffs_[1] = coeffs[1];
00067 coeffs_[2] = coeffs[2];
00068 coeffs_[3] = coeffs[3];
00069
00070 r_ = r;
00071 }
00072
00073 TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
00074 {
00075 coeffs_[0] = c0;
00076 coeffs_[1] = c1;
00077 coeffs_[2] = c2;
00078 coeffs_[3] = c3;
00079
00080 r_ = r;
00081 }
00082
00083 void TaylorModel::setTimeInterval(FCL_REAL l, FCL_REAL r)
00084 {
00085 time_interval_->t_.setValue(l, r);
00086 time_interval_->t2_.setValue(l * time_interval_->t_[0], r * time_interval_->t_[1]);
00087 time_interval_->t3_.setValue(l * time_interval_->t2_[0], r * time_interval_->t2_[1]);
00088 time_interval_->t4_.setValue(l * time_interval_->t3_[0], r * time_interval_->t3_[1]);
00089 time_interval_->t5_.setValue(l * time_interval_->t4_[0], r * time_interval_->t4_[1]);
00090 time_interval_->t6_.setValue(l * time_interval_->t5_[0], r * time_interval_->t5_[1]);
00091 }
00092
00093 TaylorModel TaylorModel::operator + (FCL_REAL d) const
00094 {
00095 return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
00096 }
00097
00098 TaylorModel& TaylorModel::operator += (FCL_REAL d)
00099 {
00100 coeffs_[0] += d;
00101 }
00102
00103 TaylorModel TaylorModel::operator + (const TaylorModel& other) const
00104 {
00105 assert(other.time_interval_ == time_interval_);
00106 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_);
00107 }
00108
00109 TaylorModel TaylorModel::operator - (const TaylorModel& other) const
00110 {
00111 assert(other.time_interval__ == time_interval_);
00112 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_);
00113 }
00114 TaylorModel& TaylorModel::operator += (const TaylorModel& other)
00115 {
00116 assert(other.time_interval_ == time_interval_);
00117 coeffs_[0] += other.coeffs_[0];
00118 coeffs_[1] += other.coeffs_[1];
00119 coeffs_[2] += other.coeffs_[2];
00120 coeffs_[3] += other.coeffs_[3];
00121 r_ += other.r_;
00122 return *this;
00123 }
00124
00125 TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
00126 {
00127 assert(other.time_interval_ == time_interval_);
00128 coeffs_[0] -= other.coeffs_[0];
00129 coeffs_[1] -= other.coeffs_[1];
00130 coeffs_[2] -= other.coeffs_[2];
00131 coeffs_[3] -= other.coeffs_[3];
00132 r_ -= other.r_;
00133 return *this;
00134 }
00135
00151 TaylorModel TaylorModel::operator * (const TaylorModel& other) const
00152 {
00153 TaylorModel res(*this);
00154 res *= other;
00155 return res;
00156 }
00157
00158 TaylorModel TaylorModel::operator * (FCL_REAL d) const
00159 {
00160 return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_);
00161 }
00162
00163 TaylorModel& TaylorModel::operator *= (const TaylorModel& other)
00164 {
00165 assert(other.time_interval_ == time_interval_);
00166 register FCL_REAL c0, c1, c2, c3;
00167 register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
00168
00169 const Interval& rb = other.r_;
00170
00171 c0 = coeffs_[0] * c0b;
00172 c1 = coeffs_[0] * c1b + coeffs_[1] * c0b;
00173 c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b;
00174 c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;
00175
00176 Interval remainder(r_ * rb);
00177 register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
00178 remainder += time_interval_->t4_ * tempVal;
00179
00180 tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
00181 remainder += time_interval_->t5_ * tempVal;
00182
00183 tempVal = coeffs_[3] * c3b;
00184 remainder += time_interval_->t6_ * tempVal;
00185
00186 remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb +
00187 (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_);
00188
00189 coeffs_[0] = c0;
00190 coeffs_[1] = c1;
00191 coeffs_[2] = c2;
00192 coeffs_[3] = c3;
00193
00194 r_ = remainder;
00195
00196 return *this;
00197 }
00198
00199 TaylorModel& TaylorModel::operator *= (FCL_REAL d)
00200 {
00201 coeffs_[0] *= d;
00202 coeffs_[1] *= d;
00203 coeffs_[2] *= d;
00204 coeffs_[3] *= d;
00205 r_ *= d;
00206 return *this;
00207 }
00208
00209
00210 TaylorModel TaylorModel::operator - () const
00211 {
00212 return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
00213 }
00214
00215 void TaylorModel::print() const
00216 {
00217 std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
00218 }
00219
00220 Interval TaylorModel::getBound(FCL_REAL t) const
00221 {
00222 return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
00223 }
00224
00225 Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const
00226 {
00227 Interval t(t0, t1);
00228 Interval t2(t0 * t0, t1 * t1);
00229 Interval t3(t0 * t2[0], t1 * t2[1]);
00230
00231 return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
00232 }
00233
00234 Interval TaylorModel::getBound() const
00235 {
00236 return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3];
00237 }
00238
00239 Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
00240 {
00241 if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
00242 if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];
00243
00244 if(coeffs_[3] == 0)
00245 {
00246 register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
00247 Interval polybounds;
00248 if(a <= t1 && a >= t0)
00249 {
00250 FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
00251 register FCL_REAL t = t0;
00252 FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
00253 t = t1;
00254 FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
00255
00256 FCL_REAL minQ = LQ, maxQ = RQ;
00257 if(LQ > RQ)
00258 {
00259 minQ = RQ;
00260 maxQ = LQ;
00261 }
00262
00263 if(minQ > AQ) minQ = AQ;
00264 if(maxQ < AQ) maxQ = AQ;
00265
00266 polybounds.setValue(minQ, maxQ);
00267 }
00268 else
00269 {
00270 register FCL_REAL t = t0;
00271 FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
00272 t = t1;
00273 FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
00274
00275 if(LQ > RQ) polybounds.setValue(RQ, LQ);
00276 else polybounds.setValue(LQ, RQ);
00277 }
00278
00279 return polybounds + r_;
00280 }
00281 else
00282 {
00283 register FCL_REAL t = t0;
00284 FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
00285 t = t1;
00286 FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
00287
00288 if(LQ > RQ)
00289 {
00290 FCL_REAL tmp = LQ;
00291 LQ = RQ;
00292 RQ = tmp;
00293 }
00294
00295
00296
00297 FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
00298 if(delta < 0)
00299 return Interval(LQ, RQ) + r_;
00300
00301 FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
00302 FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
00303
00304 if(r1 <= t1 && r1 >= t0)
00305 {
00306 FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
00307 if(Q < LQ) LQ = Q;
00308 else if(Q > RQ) RQ = Q;
00309 }
00310
00311 if(r2 <= t1 && r2 >= t0)
00312 {
00313 FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
00314 if(Q < LQ) LQ = Q;
00315 else if(Q > RQ) RQ = Q;
00316 }
00317
00318 return Interval(LQ, RQ) + r_;
00319 }
00320 }
00321
00322 Interval TaylorModel::getTightBound() const
00323 {
00324 return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
00325 }
00326
00327 void TaylorModel::setZero()
00328 {
00329 coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
00330 r_.setValue(0);
00331 }
00332
00333
00334 void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
00335 {
00336 FCL_REAL a = tm.time_interval_->t_.center();
00337 FCL_REAL t = w * a + q0;
00338 FCL_REAL w2 = w * w;
00339 FCL_REAL fa = cos(t);
00340 FCL_REAL fda = -w*sin(t);
00341 FCL_REAL fdda = -w2*fa;
00342 FCL_REAL fddda = -w2*fda;
00343
00344 tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
00345 tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
00346 tm.coeffs_[2] = 0.5*(fdda-a*fddda);
00347 tm.coeffs_[3] = 1.0/6.0*fddda;
00348
00349
00350 Interval fddddBounds;
00351 if(w == 0) fddddBounds.setValue(0);
00352 else
00353 {
00354 FCL_REAL cosQL = cos(tm.time_interval_->t_[0] * w + q0);
00355 FCL_REAL cosQR = cos(tm.time_interval_->t_[1] * w + q0);
00356
00357 if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
00358 else fddddBounds.setValue(cosQR, cosQL);
00359
00360
00361 fddddBounds[0] -= 1e-15;
00362 fddddBounds[1] += 1e-15;
00363
00364
00365
00366
00367 FCL_REAL k1 = (tm.time_interval_->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
00368 FCL_REAL k2 = (tm.time_interval_->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
00369
00370
00371 if(w > 0)
00372 {
00373 if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
00374 k1 -= 0.5;
00375 k2 -= 0.5;
00376 if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
00377 }
00378 else
00379 {
00380 if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
00381 k1 -= 0.5;
00382 k2 -= 0.5;
00383 if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
00384 }
00385 }
00386
00387 FCL_REAL w4 = w2 * w2;
00388 fddddBounds *= w4;
00389
00390 FCL_REAL midSize = 0.5 * (tm.time_interval_->t_[1] - tm.time_interval_->t_[0]);
00391 FCL_REAL midSize2 = midSize * midSize;
00392 FCL_REAL midSize4 = midSize2 * midSize2;
00393
00394
00395 if(fddddBounds[0] > 0)
00396 tm.r_.setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
00397 else if(fddddBounds[0] < 0)
00398 tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
00399 else
00400 tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
00401 }
00402
00403 void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
00404 {
00405 FCL_REAL a = tm.time_interval_->t_.center();
00406 FCL_REAL t = w * a + q0;
00407 FCL_REAL w2 = w * w;
00408 FCL_REAL fa = sin(t);
00409 FCL_REAL fda = w*cos(t);
00410 FCL_REAL fdda = -w2*fa;
00411 FCL_REAL fddda = -w2*fda;
00412
00413 tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
00414 tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
00415 tm.coeffs_[2] = 0.5*(fdda-a*fddda);
00416 tm.coeffs_[3] = 1.0/6.0*fddda;
00417
00418
00419
00420 Interval fddddBounds;
00421
00422 if(w == 0) fddddBounds.setValue(0);
00423 else
00424 {
00425 FCL_REAL sinQL = sin(w * tm.time_interval_->t_[0] + q0);
00426 FCL_REAL sinQR = sin(w * tm.time_interval_->t_[1] + q0);
00427
00428 if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
00429 else fddddBounds.setValue(sinQR, sinQL);
00430
00431
00432 fddddBounds[0] -= 1e-15;
00433 fddddBounds[1] += 1e-15;
00434
00435
00436
00437
00438 FCL_REAL k1 = (tm.time_interval_->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
00439 FCL_REAL k2 = (tm.time_interval_->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
00440
00441 if(w > 0)
00442 {
00443 if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
00444 k1 -= 0.5;
00445 k2 -= 0.5;
00446 if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
00447 }
00448 else
00449 {
00450 if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
00451 k1 -= 0.5;
00452 k2 -= 0.5;
00453 if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
00454 }
00455
00456 FCL_REAL w4 = w2 * w2;
00457 fddddBounds *= w4;
00458
00459 FCL_REAL midSize = 0.5 * (tm.time_interval_->t_[1] - tm.time_interval_->t_[0]);
00460 FCL_REAL midSize2 = midSize * midSize;
00461 FCL_REAL midSize4 = midSize2 * midSize2;
00462
00463
00464 if(fddddBounds[0] > 0)
00465 tm.r_.setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
00466 else if(fddddBounds[0] < 0)
00467 tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
00468 else
00469 tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
00470 }
00471 }
00472
00473 void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v)
00474 {
00475 tm.coeffs_[0] = p;
00476 tm.coeffs_[1] = v;
00477 tm.coeffs_[2] = tm.coeffs_[3] = tm.r_[0] = tm.r_[1] = 0;
00478 }
00479
00480 }