diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index 9dff30f..28c497c 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -902,6 +902,8 @@ int Sketch::addDistanceYConstraint(int geoId1, PointPos pos1, int geoId2, PointP } // horizontal line constraint +// geoId identifies a line. +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addHorizontalConstraint(int geoId) { geoId = checkGeoId(geoId); @@ -910,12 +912,22 @@ int Sketch::addHorizontalConstraint(int geoId) return -1; GCS::Line &l = Lines[Geoms[geoId].index]; + + // decide whether the direction is right or left + double a = (*l.p1.x <= *l.p2.x) ? 0. : M_PI; + + // add the fixed parameter for the angle + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintHorizontal(l, tag); + GCSsys.addConstraintP2PAngle(l.p1, l.p2, angle, tag); return ConstraintsCounter; } // two points on a horizontal line constraint +// (geoId1,pos1) and (geoId2,pos2) identify two points. +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addHorizontalConstraint(int geoId1, PointPos pos1, int geoId2, PointPos pos2) { geoId1 = checkGeoId(geoId1); @@ -928,14 +940,23 @@ int Sketch::addHorizontalConstraint(int geoId1, PointPos pos1, int geoId2, Point pointId2 >= 0 && pointId2 < int(Points.size())) { GCS::Point &p1 = Points[pointId1]; GCS::Point &p2 = Points[pointId2]; + + // decide whether the direction is right or left + double a = (*p1.x <= *p2.x) ? 0. : M_PI; + + // add the fixed parameter for the angle + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintHorizontal(p1, p2, tag); + GCSsys.addConstraintP2PAngle(p1, p2, angle, tag); return ConstraintsCounter; } return -1; } // vertical line constraint +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addVerticalConstraint(int geoId) { geoId = checkGeoId(geoId); @@ -944,12 +965,21 @@ int Sketch::addVerticalConstraint(int geoId) return -1; GCS::Line &l = Lines[Geoms[geoId].index]; + + // decide whether the direction is up or down + double a = (*l.p1.y <= *l.p2.y) ? M_PI/2 : -M_PI/2; + + // add the fixed parameter for the angle + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintVertical(l, tag); + GCSsys.addConstraintP2PAngle(l.p1, l.p2, angle, tag); return ConstraintsCounter; } // two points on a vertical line constraint +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addVerticalConstraint(int geoId1, PointPos pos1, int geoId2, PointPos pos2) { geoId1 = checkGeoId(geoId1); @@ -962,8 +992,16 @@ int Sketch::addVerticalConstraint(int geoId1, PointPos pos1, int geoId2, PointPo pointId2 >= 0 && pointId2 < int(Points.size())) { GCS::Point &p1 = Points[pointId1]; GCS::Point &p2 = Points[pointId2]; + + // decide whether the direction is up or down + double a = (*p1.y <= *p2.y) ? M_PI/2 : -M_PI/2; + + // add the fixed parameter for the angle + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintVertical(p1, p2, tag); + GCSsys.addConstraintP2PAngle(p1, p2, angle, tag); return ConstraintsCounter; } return -1; @@ -988,6 +1026,8 @@ int Sketch::addPointCoincidentConstraint(int geoId1, PointPos pos1, int geoId2, return -1; } +// parallel constraint +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addParallelConstraint(int geoId1, int geoId2) { geoId1 = checkGeoId(geoId1); @@ -999,12 +1039,26 @@ int Sketch::addParallelConstraint(int geoId1, int geoId2) GCS::Line &l1 = Lines[Geoms[geoId1].index]; GCS::Line &l2 = Lines[Geoms[geoId2].index]; + + // decide whether they're going to be parallel or antiparallel + double dx1 = *l1.p2.x - *l1.p1.x, dy1 = *l1.p2.y - *l1.p1.y; + double dx2 = *l2.p2.x - *l2.p1.x, dy2 = *l2.p2.y - *l2.p1.y; + double a1 = atan2(dy1, dx1), a2 = atan2(dy2, dx2); + double da = fmod(a2 - a1 + M_PI/2, M_PI*2); // [0,pi] => parallel + // [pi,2pi] => antiparallel + double a = (da < M_PI) ? 0. : M_PI; + + // add the fixed parameter for the angle between the lines + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintParallel(l1, l2, tag); + GCSsys.addConstraintL2LAngle(l1, l2, angle, tag); return ConstraintsCounter; } // simple perpendicularity constraint +// [JAF] oriented in a specific direction (was previously ignoring direction) int Sketch::addPerpendicularConstraint(int geoId1, int geoId2) { // accepts the following combinations: @@ -1018,8 +1072,17 @@ int Sketch::addPerpendicularConstraint(int geoId1, int geoId2) if (Geoms[geoId1].type == Line) { GCS::Line &l1 = Lines[Geoms[geoId1].index]; GCS::Line &l2 = Lines[Geoms[geoId2].index]; + + // decide whether the perpendicularity is left- or right-handed + // ### TODO... + double a = M_PI/2; + + // add the fixed parameter for the angle between the lines + FixParameters.push_back(new double(a)); + double *angle = FixParameters[FixParameters.size() - 1]; + int tag = ++ConstraintsCounter; - GCSsys.addConstraintPerpendicular(l1, l2, tag); + GCSsys.addConstraintL2LAngle(l1, l2, angle, tag); return ConstraintsCounter; } else diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 28c5c57..d28a9b0 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -560,60 +560,6 @@ double ConstraintPointOnPerpBisector::grad(double *param) return scale * deriv; } -// Parallel -ConstraintParallel::ConstraintParallel(Line &l1, Line &l2) -{ - pvec.push_back(l1.p1.x); - pvec.push_back(l1.p1.y); - pvec.push_back(l1.p2.x); - pvec.push_back(l1.p2.y); - pvec.push_back(l2.p1.x); - pvec.push_back(l2.p1.y); - pvec.push_back(l2.p2.x); - pvec.push_back(l2.p2.y); - origpvec = pvec; - rescale(); -} - -ConstraintType ConstraintParallel::getTypeId() -{ - return Parallel; -} - -void ConstraintParallel::rescale(double coef) -{ - double dx1 = (*l1p1x() - *l1p2x()); - double dy1 = (*l1p1y() - *l1p2y()); - double dx2 = (*l2p1x() - *l2p2x()); - double dy2 = (*l2p1y() - *l2p2y()); - scale = coef / sqrt((dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2)); -} - -double ConstraintParallel::error() -{ - double dx1 = (*l1p1x() - *l1p2x()); - double dy1 = (*l1p1y() - *l1p2y()); - double dx2 = (*l2p1x() - *l2p2x()); - double dy2 = (*l2p1y() - *l2p2y()); - return scale * (dx1*dy2 - dy1*dx2); -} - -double ConstraintParallel::grad(double *param) -{ - double deriv=0.; - if (param == l1p1x()) deriv += (*l2p1y() - *l2p2y()); // = dy2 - if (param == l1p2x()) deriv += -(*l2p1y() - *l2p2y()); // = -dy2 - if (param == l1p1y()) deriv += -(*l2p1x() - *l2p2x()); // = -dx2 - if (param == l1p2y()) deriv += (*l2p1x() - *l2p2x()); // = dx2 - - if (param == l2p1x()) deriv += -(*l1p1y() - *l1p2y()); // = -dy1 - if (param == l2p2x()) deriv += (*l1p1y() - *l1p2y()); // = dy1 - if (param == l2p1y()) deriv += (*l1p1x() - *l1p2x()); // = dx1 - if (param == l2p2y()) deriv += -(*l1p1x() - *l1p2x()); // = -dx1 - - return scale * deriv; -} - // Perpendicular ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2) { @@ -727,15 +673,24 @@ void ConstraintL2LAngle::rescale(double coef) double ConstraintL2LAngle::error() { + // The method of calculation is chosen to avoid discontinuities and + // ensure the resulting angular error (before scaling) is centred + // around zero with a range [-pi,pi]. + double dx1 = (*l1p2x() - *l1p1x()); double dy1 = (*l1p2y() - *l1p1y()); double dx2 = (*l2p2x() - *l2p1x()); double dy2 = (*l2p2y() - *l2p1y()); + // a := (angle of line1 + controlled angle), which is supposed to be + // the angle of line2 but expressed in a range that may exceed [-pi,pi]. double a = atan2(dy1,dx1) + *angle(); double ca = cos(a); double sa = sin(a); + // (x2,y2) := line2 direction vector rotated by (-a) double x2 = dx2*ca + dy2*sa; double y2 = -dx2*sa + dy2*ca; + // result := scale * angle_of(x2,y2) + // so is equivalent to (direction(l2) - direction(l1) - controlled_angle) return scale * atan2(y2,x2); } @@ -752,6 +707,7 @@ double ConstraintL2LAngle::grad(double *param) if (param == l1p2x()) deriv += dy1/r2; if (param == l1p2y()) deriv += -dx1/r2; } + // ### Why treat l2 differently from l1? if (param == l2p1x() || param == l2p1y() || param == l2p2x() || param == l2p2y()) { double dx1 = (*l1p2x() - *l1p1x()); diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index a47aaf9..d5c7700 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -74,7 +74,7 @@ namespace GCS protected: VEC_pD origpvec; // is used only as a reference for redirecting and reverting pvec VEC_pD pvec; - double scale; + double scale; // ### ... For a 'parallel' constraint: inverse square of geometric-mean of the two lengths. int tag; bool pvecChangedFlag; //indicates that pvec has changed and saved pointers must be reconstructed (currently used only in AngleViaPoint) public: @@ -222,26 +222,6 @@ namespace GCS virtual double grad(double *); }; - // Parallel - class ConstraintParallel : public Constraint - { - private: - inline double* l1p1x() { return pvec[0]; } - inline double* l1p1y() { return pvec[1]; } - inline double* l1p2x() { return pvec[2]; } - inline double* l1p2y() { return pvec[3]; } - inline double* l2p1x() { return pvec[4]; } - inline double* l2p1y() { return pvec[5]; } - inline double* l2p2x() { return pvec[6]; } - inline double* l2p2y() { return pvec[7]; } - public: - ConstraintParallel(Line &l1, Line &l2); - virtual ConstraintType getTypeId(); - virtual void rescale(double coef=1.); - virtual double error(); - virtual double grad(double *); - }; - // Perpendicular class ConstraintPerpendicular : public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index b193737..6d25197 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -343,7 +343,7 @@ int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, int System::addConstraintP2PAngle(Point &p1, Point &p2, double *angle, int tagId) { - return addConstraintP2PAngle(p1, p2, angle, 0.); + return addConstraintP2PAngle(p1, p2, angle, 0., tagId); } int System::addConstraintP2LDistance(Point &p, Line &l, double *distance, int tagId) @@ -381,20 +381,6 @@ int System::addConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2, i return addConstraint(constr); } -int System::addConstraintParallel(Line &l1, Line &l2, int tagId) -{ - Constraint *constr = new ConstraintParallel(l1, l2); - constr->setTag(tagId); - return addConstraint(constr); -} - -int System::addConstraintPerpendicular(Line &l1, Line &l2, int tagId) -{ - Constraint *constr = new ConstraintPerpendicular(l1, l2); - constr->setTag(tagId); - return addConstraint(constr); -} - int System::addConstraintPerpendicular(Point &l1p1, Point &l1p2, Point &l2p1, Point &l2p2, int tagId) { @@ -456,26 +442,6 @@ int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId) return addConstraintEqual(p1.y, p2.y, tagId); } -int System::addConstraintHorizontal(Line &l, int tagId) -{ - return addConstraintEqual(l.p1.y, l.p2.y, tagId); -} - -int System::addConstraintHorizontal(Point &p1, Point &p2, int tagId) -{ - return addConstraintEqual(p1.y, p2.y, tagId); -} - -int System::addConstraintVertical(Line &l, int tagId) -{ - return addConstraintEqual(l.p1.x, l.p2.x, tagId); -} - -int System::addConstraintVertical(Point &p1, Point &p2, int tagId) -{ - return addConstraintEqual(p1.x, p2.x, tagId); -} - int System::addConstraintCoordinateX(Point &p, double *x, int tagId) { return addConstraintEqual(p.x, x, tagId); @@ -1258,7 +1224,7 @@ int System::solve_LM(SubSystem* subsys) k++; } - if (k > 50) { + if (k > 50) { // ### How could it be > 50? Do we mean >= 50? stop = 7; break; } @@ -1936,9 +1902,6 @@ void free(std::vector &constrvec) case PointOnLine: delete static_cast(*constr); break; - case Parallel: - delete static_cast(*constr); - break; case Perpendicular: delete static_cast(*constr); break; diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 4baa403..7012582 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -110,8 +110,6 @@ namespace GCS int addConstraintPointOnLine(Point &p, Point &lp1, Point &lp2, int tagId=0); int addConstraintPointOnPerpBisector(Point &p, Line &l, int tagId=0); int addConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2, int tagId=0); - int addConstraintParallel(Line &l1, Line &l2, int tagId=0); - int addConstraintPerpendicular(Line &l1, Line &l2, int tagId=0); int addConstraintPerpendicular(Point &l1p1, Point &l1p2, Point &l2p1, Point &l2p2, int tagId=0); int addConstraintL2LAngle(Line &l1, Line &l2, double *angle, int tagId=0); @@ -127,10 +125,6 @@ namespace GCS // derived constraints int addConstraintP2PCoincident(Point &p1, Point &p2, int tagId=0); - int addConstraintHorizontal(Line &l, int tagId=0); - int addConstraintHorizontal(Point &p1, Point &p2, int tagId=0); - int addConstraintVertical(Line &l, int tagId=0); - int addConstraintVertical(Point &p1, Point &p2, int tagId=0); int addConstraintCoordinateX(Point &p, double *x, int tagId=0); int addConstraintCoordinateY(Point &p, double *y, int tagId=0); int addConstraintArcRules(Arc &a, int tagId=0);