double ConstraintPointOnLine::error()
{
double x0=*p0x(), x1=*p1x(), x2=*p2x();
double y0=*p0y(), y1=*p1y(), y2=*p2y();
double dx = x2-x1;
double dy = y2-y1;
double d = sqrt(dx*dx+dy*dy);
double area = -x0*dy+y0*dx+x1*y2-x2*y1; // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area)
return scale * area/d;
}
double ConstraintP2LDistance::error()
{
double x0=*p0x(), x1=*p1x(), x2=*p2x();
double y0=*p0y(), y1=*p1y(), y2=*p2y();
double dist = *distance();
double dx = x2-x1;
double dy = y2-y1;
double d = sqrt(dx*dx+dy*dy);
double area = std::abs(-x0*dy+y0*dx+x1*y2-x2*y1); // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area)
return scale * (area/d - dist);
}
double ConstraintP2PAngle::error()
{
double dx = (*p2x() - *p1x());
double dy = (*p2y() - *p1y());
double a = *angle() + da;
double ca = cos(a);
double sa = sin(a);
double x = dx*ca + dy*sa;
double y = -dx*sa + dy*ca;
return scale * atan2(y,x);
}
double ConstraintP2LDistance::maxStep(MAP_pD_D &dir, double lim)
{
MAP_pD_D::iterator it;
// distance() >= 0
it = dir.find(distance());
if (it != dir.end()) {
if (it->second < 0.)
lim = std::min(lim, -(*distance()) / it->second);
}
// restrict actual area change
double darea=0.;
double x0=*p0x(), x1=*p1x(), x2=*p2x();
double y0=*p0y(), y1=*p1y(), y2=*p2y();
it = dir.find(p0x());
if (it != dir.end()) darea += (y1-y2) * it->second;
it = dir.find(p0y());
if (it != dir.end()) darea += (x2-x1) * it->second;
it = dir.find(p1x());
if (it != dir.end()) darea += (y2-y0) * it->second;
it = dir.find(p1y());
if (it != dir.end()) darea += (x0-x2) * it->second;
it = dir.find(p2x());
if (it != dir.end()) darea += (y0-y1) * it->second;
it = dir.find(p2y());
if (it != dir.end()) darea += (x1-x0) * it->second;
darea = std::abs(darea);
if (darea > 0.) {
double dx = x2-x1;
double dy = y2-y1;
double area = 0.3*(*distance())*sqrt(dx*dx+dy*dy);
if (darea > area) {
area = std::max(area, 0.3*std::abs(-x0*dy+y0*dx+x1*y2-x2*y1));
if (darea > area)
lim = std::min(lim, area/darea);
}
}
return lim;
}
double ConstraintPointOnPerpBisector::grad(double *param)
{
double deriv=0.;
if (param == p0x() || param == p0y() ||
param == p1x() || param == p1y()) {
double dx1 = *p1x() - *p0x();
double dy1 = *p1y() - *p0y();
if (param == p0x()) deriv -= dx1/sqrt(dx1*dx1+dy1*dy1);
if (param == p0y()) deriv -= dy1/sqrt(dx1*dx1+dy1*dy1);
if (param == p1x()) deriv += dx1/sqrt(dx1*dx1+dy1*dy1);
if (param == p1y()) deriv += dy1/sqrt(dx1*dx1+dy1*dy1);
}
if (param == p0x() || param == p0y() ||
param == p2x() || param == p2y()) {
double dx2 = *p2x() - *p0x();
double dy2 = *p2y() - *p0y();
if (param == p0x()) deriv += dx2/sqrt(dx2*dx2+dy2*dy2);
if (param == p0y()) deriv += dy2/sqrt(dx2*dx2+dy2*dy2);
if (param == p2x()) deriv -= dx2/sqrt(dx2*dx2+dy2*dy2);
if (param == p2y()) deriv -= dy2/sqrt(dx2*dx2+dy2*dy2);
}
return scale * deriv;
}
double ConstraintP2PAngle::grad(double *param)
{
double deriv=0.;
if (param == p1x() || param == p1y() ||
param == p2x() || param == p2y()) {
double dx = (*p2x() - *p1x());
double dy = (*p2y() - *p1y());
double a = *angle() + da;
double ca = cos(a);
double sa = sin(a);
double x = dx*ca + dy*sa;
double y = -dx*sa + dy*ca;
double r2 = dx*dx+dy*dy;
dx = -y/r2;
dy = x/r2;
if (param == p1x()) deriv += (-ca*dx + sa*dy);
if (param == p1y()) deriv += (-sa*dx - ca*dy);
if (param == p2x()) deriv += ( ca*dx - sa*dy);
if (param == p2y()) deriv += ( sa*dx + ca*dy);
}
if (param == angle()) deriv += -1;
return scale * deriv;
}
