/* 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;
}
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;
}
}