void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v) { tm.coeff(0) = p; tm.coeff(1) = v; tm.coeff(2) = 0; tm.coeff(3) = 0; tm.remainder()[0] = 0; tm.remainder()[1] = 0; }
TVector3 operator * (const Vec3f& v, const TaylorModel& a) { TVector3 res(a.getTimeInterval()); res[0] = a * v[0]; res[1] = a * v[1]; res[2] = a * v[2]; return res; }
TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a) { TMatrix3 res(a.getTimeInterval()); res(0, 0) = a * m(0, 0); res(0, 1) = a * m(0, 1); res(0, 1) = a * m(0, 2); res(1, 0) = a * m(1, 0); res(1, 1) = a * m(1, 1); res(1, 1) = a * m(1, 2); res(2, 0) = a * m(2, 0); res(2, 1) = a * m(2, 1); res(2, 1) = a * m(2, 2); return res; }
void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) { FCL_REAL a = tm.getTimeInterval()->t_.center(); FCL_REAL t = w * a + q0; FCL_REAL w2 = w * w; FCL_REAL fa = sin(t); FCL_REAL fda = w*cos(t); FCL_REAL fdda = -w2*fa; FCL_REAL fddda = -w2*fda; tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; tm.coeff(2) = 0.5*(fdda-a*fddda); tm.coeff(3) = 1.0/6.0*fddda; // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1] Interval fddddBounds; if(w == 0) fddddBounds.setValue(0); else { FCL_REAL sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0); FCL_REAL sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0); if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR); else fddddBounds.setValue(sinQR, sinQL); // enlarge to handle round-off errors fddddBounds[0] -= 1e-15; fddddBounds[1] += 1e-15; // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi]; // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi] FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25; FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25; if(w > 0) { if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; } else { if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; } FCL_REAL w4 = w2 * w2; fddddBounds *= w4; FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); FCL_REAL midSize2 = midSize * midSize; FCL_REAL midSize4 = midSize2 * midSize2; // [0, midSize4] * fdddBounds if(fddddBounds[0] > 0) tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); else if(fddddBounds[0] < 0) tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); else tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); } }