Beispiel #1
0
 /* This callback is called everytime this node's simulation starts or restarts.
  This is different from initialize() above. */
 virtual int64_t onInitialization() override {
     // Initial state and output
     x.setZero();
     velocity = 0.0;
     std::cout << "At " << currentSimulationTime() << " INIT" << std::endl;
     return 0;
 }
Beispiel #2
0
double BlockConstraint::_basicCostGrad(Eigen::Vector2d& grad_c, const Vector2d &config)
{
    Eigen::Vector2d p = _tfinv*(config-_center);
//    std::cout << p.transpose() << "\t|\t" << scales[0] << ", " << scales[1] << std::endl;
    
    assert( buffer > 0 && "We only support positive-valued buffer sizes!");
    
    double c = INFINITY;
    for(size_t i=0; i<2; ++i)
    {
        if(fabs(p[i]) >= fabs(scales[i])+fabs(buffer))
        {
            grad_c.setZero();
            return 0;
        }
        else if(fabs(p[i]) >= fabs(scales[i]))
        {
            double dist = buffer-(fabs(p[i])-fabs(scales[i]));
            double ctemp = dist*dist/(2*buffer);
            if(ctemp < c)
            {
                c = ctemp;
                grad_c.setZero();
                grad_c[i] = -(fabs(p[i])-fabs(scales[i])-buffer)/buffer*sign(p[i]);
            }
        }
        else
        {
            double ctemp = scales[i]-p[i] + buffer/2;
            if(ctemp < c)
            {
                c = ctemp;
                grad_c.setZero();
                grad_c[i] = -sign(p[i]);
            }
        }
    }
    
    return c;
}
bool mesh_core::LineSegment2D::intersect(
      const LineSegment2D& other,
      Eigen::Vector2d& intersection,
      bool& parallel) const
{
  const LineSegment2D& a = *this;
  const LineSegment2D& b = other;

  if (acorn_debug_ear_state)
  {
    logInform("Intersect");
    logInform("     a=%s",a.str().c_str());
    logInform("     b=%s",b.str().c_str());
  }

  if (a.vertical_)
  {
    if (b.vertical_)
    {
      parallel = true;
      intersection = a.pt_[0];
      if (a.pt_[0].x() != b.pt_[0].x())
        return false;

      double aymin = std::min(a.pt_[0].y(), a.pt_[1].y());
      double aymax = std::max(a.pt_[0].y(), a.pt_[1].y());
      double bymin = std::min(b.pt_[0].y(), b.pt_[1].y());
      double bymax = std::max(b.pt_[0].y(), b.pt_[1].y());

      if (acorn_debug_ear_state)
      {
        logInform("     both vertical aymin=%f aymax=%f bymin=%f bymax\%f",
          aymin, aymax, bymin, bymax);
      }

      if (bymax < aymin)
        return false;
      if (bymin > aymax)
        return false;

      if (bymax <= aymax)
        intersection.y() = bymax;
      else if (bymin >= aymin)
        intersection.y() = bymin;
      else
        intersection.y() = aymin;

      if (acorn_debug_ear_state)
      {
        logInform("     INTERSECTS");
      }
      return true;
    }
    parallel = false;
    intersection.x() = a.pt_[0].x();
    intersection.y() = b.slope_ * intersection.x() + b.y_intercept_;

    double tb = (intersection.x() - b.pt_[0].x()) * b.inv_dx_;
    if (acorn_debug_ear_state)
    {
      logInform("     a vertical pt=(%8.4f, %8.4f) tb=%f",
        intersection.x(),
        intersection.y(),
        tb);
    }
    if (tb > 1.0 || tb < 0.0)
      return false;

    if (b.inv_dx_ == 0.0)
    {
      if (intersection.y() != a.pt_[0].y())
        return false;
    }
    else
    {
      double ta = (intersection.y() - a.pt_[0].y()) * a.inv_dx_;
      if (acorn_debug_ear_state)
      {
        logInform("       ta=%f", ta);
      }
      if (ta > 1.0 || ta < 0.0)
        return false;
    }

    if (acorn_debug_ear_state)
    {
      logInform("     INTERSECTS");
    }
    return true;
  }
  else if (b.vertical_)
  {
    return b.intersect(a, intersection, parallel);
  }
  else
  {
    double bottom = a.slope_ - b.slope_;
    if (std::abs(bottom) < std::numeric_limits<double>::epsilon())
    {
      parallel = true;
      intersection.setZero();
      return false;
    }

    parallel = false;
    intersection.x() = (b.y_intercept_ - a.y_intercept_) / bottom;
    intersection.y() = a.slope_ * intersection.x() + a.y_intercept_;

    double ta = (intersection.x() - a.pt_[0].x()) * a.inv_dx_;
    if (acorn_debug_ear_state)
    {
      logInform("     not vertical pt=(%8.4f, %8.4f) ta=%f",
        intersection.x(),
        intersection.y(),
        ta);
    }
    if (ta > 1.0 || ta < 0.0)
      return false;

    double tb = (intersection.x() - b.pt_[0].x()) * b.inv_dx_;
    if (acorn_debug_ear_state)
    {
      logInform("       tb=%f", tb);
    }
    if (tb > 1.0 || tb < 0.0)
      return false;

    if (acorn_debug_ear_state)
    {
      logInform("     INTERSECTS");
    }
    return true;
  }
}