C++ Interval::setValue方法代碼示例

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));
  }
}

Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
{
  if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
  if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];

  if(coeffs_[3] == 0)
  {
    register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
    Interval polybounds;
    if(a <= t1 && a >= t0)
    {
      FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
      register FCL_REAL t = t0;
      FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
      t = t1;
      FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);

      FCL_REAL minQ = LQ, maxQ = RQ;
      if(LQ > RQ)
      {
        minQ = RQ;
        maxQ = LQ;
      }

      if(minQ > AQ) minQ = AQ;
      if(maxQ < AQ) maxQ = AQ;

      polybounds.setValue(minQ, maxQ);
    }
    else
    {
      register FCL_REAL t = t0;
      FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
      t = t1;
      FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);

      if(LQ > RQ) polybounds.setValue(RQ, LQ);
      else polybounds.setValue(LQ, RQ);
    }

    return polybounds + r_;
  }
  else
  {
    register FCL_REAL t = t0;
    FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
    t = t1;
    FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));

    if(LQ > RQ)
    {
      FCL_REAL tmp = LQ;
      LQ = RQ;
      RQ = tmp;
    }

    // derivative: c1+2*c2*t+3*c3*t^2

    FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
    if(delta < 0)
      return Interval(LQ, RQ) + r_;

    FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
    FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);

    if(r1 <= t1 && r1 >= t0)
    {
      FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
      if(Q < LQ) LQ = Q;
      else if(Q > RQ) RQ = Q;
    }

    if(r2 <= t1 && r2 >= t0)
    {
      FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
      if(Q < LQ) LQ = Q;
      else if(Q > RQ) RQ = Q;
    }

    return Interval(LQ, RQ) + r_;
  }
}