From 27a76afa94dc2839b0d884767cfb8217f46a6eb6 Mon Sep 17 00:00:00 2001 From: Abdullah Tahiri Date: Tue, 12 Jan 2016 15:52:14 +0100 Subject: [PATCH] In Part:Geometry: - Fixing Hyperbola classes to get CCW emulation (like Ellipse classes). In Sketcher: - The Sketcher representation deals with the right branch of the Hyperbola only. - Solver model is: Center, Focus1 (focus of the right branch), minor radius (b). - HyperbolicArcRangeToEndPoints code is the one of Ellipse <= Awaiting DeepSOIC help ;) - ConstraintPointOnHyperbola solver constraint is now implemented and should be working. - No InternalAligment constraints implemented yet. --- src/Mod/Part/App/Geometry.cpp | 77 ++++++- src/Mod/Part/App/Geometry.h | 7 +- src/Mod/Sketcher/App/Sketch.cpp | 32 ++- src/Mod/Sketcher/App/SketchObject.cpp | 10 +- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 212 ++++++++++++++++++ src/Mod/Sketcher/App/planegcs/Constraints.h | 44 +++- src/Mod/Sketcher/App/planegcs/GCS.cpp | 24 ++ src/Mod/Sketcher/App/planegcs/GCS.h | 3 + src/Mod/Sketcher/App/planegcs/Geo.cpp | 102 ++++++--- src/Mod/Sketcher/App/planegcs/Geo.h | 28 ++- src/Mod/Sketcher/Gui/ViewProviderSketch.cpp | 4 +- 11 files changed, 478 insertions(+), 65 deletions(-) diff --git a/src/Mod/Part/App/Geometry.cpp b/src/Mod/Part/App/Geometry.cpp index fa692e40c..cc773a375 100644 --- a/src/Mod/Part/App/Geometry.cpp +++ b/src/Mod/Part/App/Geometry.cpp @@ -2103,15 +2103,86 @@ void GeomArcOfHyperbola::setAngleXU(double angle) } } -void GeomArcOfHyperbola::getRange(double& u, double& v) const +/*! + * \brief GeomArcOfHyperbola::getMajorAxisDir + * \return the direction vector (unit-length) of major axis of the hyperbola. The + * direction also points to the first focus. + */ +Base::Vector3d GeomArcOfHyperbola::getMajorAxisDir() const +{ + Handle_Geom_Hyperbola c = Handle_Geom_Hyperbola::DownCast( myCurve->BasisCurve() ); + assert(!c.IsNull()); + gp_Dir xdir = c->XAxis().Direction(); + return Base::Vector3d(xdir.X(), xdir.Y(), xdir.Z()); +} + +/*! + * \brief GeomArcOfHyperbola::setMajorAxisDir Rotates the hyperbola in its plane, so + * that its major axis is as close as possible to the provided direction. + * \param newdir [in] is the new direction. If the vector is small, the + * orientation of the ellipse will be preserved. If the vector is not small, + * but its projection onto plane of the ellipse is small, an exception will be + * thrown. + */ +void GeomArcOfHyperbola::setMajorAxisDir(Base::Vector3d newdir) +{ + Handle_Geom_Hyperbola c = Handle_Geom_Hyperbola::DownCast( myCurve->BasisCurve() ); + assert(!c.IsNull()); + #if OCC_VERSION_HEX >= 0x060504 + if (newdir.Sqr() < Precision::SquareConfusion()) + #else + if (newdir.Length() < Precision::Confusion()) + #endif + return;//zero vector was passed. Keep the old orientation. + + try { + gp_Ax2 pos = c->Position(); + pos.SetXDirection(gp_Dir(newdir.x, newdir.y, newdir.z));//OCC should keep the old main Direction (Z), and change YDirection to accomodate the new XDirection. + c->SetPosition(pos); + } + catch (Standard_Failure) { + Handle_Standard_Failure e = Standard_Failure::Caught(); + throw Base::Exception(e->GetMessageString()); + } +} + +/*! + * \brief GeomArcOfHyperbola::isReversedInXY tests if an arc that lies in XY plane is reversed + * (i.e. drawn from startpoint to endpoint in CW direction instead of CCW.) + * \return Returns True if the arc is CW and false if CCW. + */ +bool GeomArcOfHyperbola::isReversedInXY() const +{ + Handle_Geom_Hyperbola c = Handle_Geom_Hyperbola::DownCast( myCurve->BasisCurve() ); + assert(!c.IsNull()); + return c->Axis().Direction().Z() < 0; +} + +void GeomArcOfHyperbola::getRange(double& u, double& v, bool emulateCCWXY) const { u = myCurve->FirstParameter(); v = myCurve->LastParameter(); + if(emulateCCWXY){ + if(isReversedInXY()){ + std::swap(u,v); + u = -u; v = -v; + if (v < u) + v += 2*M_PI; + if (v-u > 2*M_PI) + v -= 2*M_PI; + } + } } -void GeomArcOfHyperbola::setRange(double u, double v) +void GeomArcOfHyperbola::setRange(double u, double v, bool emulateCCWXY) { try { + if(emulateCCWXY){ + if(isReversedInXY()){ + std::swap(u,v); + u = -u; v = -v; + } + } myCurve->SetTrim(u, v); } catch (Standard_Failure) { @@ -2120,6 +2191,8 @@ void GeomArcOfHyperbola::setRange(double u, double v) } } + + // Persistence implementer unsigned int GeomArcOfHyperbola::getMemSize (void) const { diff --git a/src/Mod/Part/App/Geometry.h b/src/Mod/Part/App/Geometry.h index 0453316ca..b876e5493 100644 --- a/src/Mod/Part/App/Geometry.h +++ b/src/Mod/Part/App/Geometry.h @@ -399,9 +399,12 @@ public: void setMinorRadius(double Radius); double getAngleXU(void) const; void setAngleXU(double angle); + Base::Vector3d getMajorAxisDir() const; + void setMajorAxisDir(Base::Vector3d newdir); + bool isReversedInXY() const; - void getRange(double& u, double& v) const; - void setRange(double u, double v); + void getRange(double& u, double& v, bool emulateCCWXY) const; + void setRange(double u, double v, bool emulateCCWXY); // Persistence implementer --------------------- virtual unsigned int getMemSize(void) const; diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index eb1e2e62b..25c07c38a 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -490,14 +490,14 @@ int Sketch::addArcOfHyperbola(const Part::GeomArcOfHyperbola &hyperbolaSegment, Base::Vector3d endPnt = aoh->getEndPoint(); double radmaj = aoh->getMajorRadius(); double radmin = aoh->getMinorRadius(); - double phi = aoh->getAngleXU(); + Base::Vector3d radmajdir = aoh->getMajorAxisDir(); double dist_C_F = sqrt(radmaj*radmaj+radmin*radmin); // solver parameters - Base::Vector3d focus1 = center+dist_C_F*Vector3d(cos(phi), sin(phi),0); //+x + Base::Vector3d focus1 = center+dist_C_F*radmajdir; //+x double startAngle, endAngle; - aoh->getRange(startAngle, endAngle); + aoh->getRange(startAngle, endAngle,/*emulateCCW=*/true); GCS::Point p1, p2, p3; @@ -529,8 +529,8 @@ int Sketch::addArcOfHyperbola(const Part::GeomArcOfHyperbola &hyperbolaSegment, Points.push_back(p3); // add the radius parameters - params.push_back(new double(radmaj)); - double *rmaj = params[params.size()-1]; + params.push_back(new double(radmin)); + double *rmin = params[params.size()-1]; params.push_back(new double(startAngle)); double *a1 = params[params.size()-1]; params.push_back(new double(endAngle)); @@ -541,9 +541,9 @@ int Sketch::addArcOfHyperbola(const Part::GeomArcOfHyperbola &hyperbolaSegment, a.start = p1; a.end = p2; a.center = p3; - a.focus.x = f1X; - a.focus.y = f1Y; - a.radmaj = rmaj; + a.focus1.x = f1X; + a.focus1.y = f1Y; + a.radmin = rmin; a.startAngle = a1; a.endAngle = a2; def.index = ArcsOfHyperbola.size(); @@ -553,8 +553,8 @@ int Sketch::addArcOfHyperbola(const Part::GeomArcOfHyperbola &hyperbolaSegment, Geoms.push_back(def); // arcs require an ArcRules constraint for the end points - /*if (!fixed) - GCSsys.addConstraintArcOfHyperbolaRules(a);*/ + if (!fixed) + GCSsys.addConstraintArcOfHyperbolaRules(a); // return the position of the newly added geometry return Geoms.size()-1; @@ -2167,13 +2167,11 @@ bool Sketch::updateGeometry() GeomArcOfHyperbola *aoh = dynamic_cast(it->geo); Base::Vector3d center = Vector3d(*Points[it->midPointId].x, *Points[it->midPointId].y, 0.0); - Base::Vector3d f1 = Vector3d(*myArc.focus.x, *myArc.focus.y, 0.0); - double radmaj = *myArc.radmaj; + Base::Vector3d f1 = Vector3d(*myArc.focus1.x, *myArc.focus1.y, 0.0); + double radmin = *myArc.radmin; Base::Vector3d fd=f1-center; - double radmin = sqrt(fd*fd-radmaj*radmaj); - - double phi = atan2(fd.y,fd.x); + double radmaj = sqrt(fd*fd-radmin*radmin); aoh->setCenter(center); if ( radmaj >= aoh->getMinorRadius() ){ @@ -2183,8 +2181,8 @@ bool Sketch::updateGeometry() aoh->setMinorRadius(radmin); aoh->setMajorRadius(radmaj); } - aoh->setAngleXU(phi); - aoh->setRange(*myArc.startAngle, *myArc.endAngle); + aoh->setMajorAxisDir(fd); + aoh->setRange(*myArc.startAngle, *myArc.endAngle, /*emulateCCW=*/true); } } catch (Base::Exception e) { Base::Console().Error("Updating geometry: Error build geometry(%d): %s\n", diff --git a/src/Mod/Sketcher/App/SketchObject.cpp b/src/Mod/Sketcher/App/SketchObject.cpp index 9466ba520..dbd29096b 100644 --- a/src/Mod/Sketcher/App/SketchObject.cpp +++ b/src/Mod/Sketcher/App/SketchObject.cpp @@ -1731,7 +1731,7 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point) const Part::GeomArcOfHyperbola *aoh = dynamic_cast(geo); Base::Vector3d center = aoh->getCenter(); double startAngle, endAngle; - aoh->getRange(startAngle, endAngle); + aoh->getRange(startAngle, endAngle, /*emulateCCW=*/true); double dir = (startAngle < endAngle) ? 1 : -1; // this is always == 1 double arcLength = (endAngle - startAngle)*dir; double theta0 = Base::fmod( @@ -1762,8 +1762,8 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point) Part::GeomArcOfHyperbola *aoh1 = dynamic_cast(geomlist[GeoId]); Part::GeomArcOfHyperbola *aoh2 = dynamic_cast(geomlist[newGeoId]); - aoh1->setRange(startAngle, startAngle + theta1); - aoh2->setRange(startAngle + theta2, endAngle); + aoh1->setRange(startAngle, startAngle + theta1, /*emulateCCW=*/true); + aoh2->setRange(startAngle + theta2, endAngle, /*emulateCCW=*/true); // constrain the trimming points on the corresponding geometries Sketcher::Constraint *newConstr = new Sketcher::Constraint(); @@ -1862,7 +1862,7 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point) if (theta1 > theta0) { // trim arc start delConstraintOnPoint(GeoId, start, false); Part::GeomArcOfHyperbola *aoe1 = dynamic_cast(geomlist[GeoId]); - aoe1->setRange(startAngle + theta1, endAngle); + aoe1->setRange(startAngle + theta1, endAngle, /*emulateCCW=*/true); // constrain the trimming point on the corresponding geometry Sketcher::Constraint *newConstr = new Sketcher::Constraint(); newConstr->Type = constrType; @@ -1880,7 +1880,7 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point) else { // trim arc end delConstraintOnPoint(GeoId, end, false); Part::GeomArcOfHyperbola *aoe1 = dynamic_cast(geomlist[GeoId]); - aoe1->setRange(startAngle, startAngle + theta1); + aoe1->setRange(startAngle, startAngle + theta1, /*emulateCCW=*/true); Sketcher::Constraint *newConstr = new Sketcher::Constraint(); newConstr->Type = constrType; newConstr->First = GeoId; diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index e63533954..62a00c1d8 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -1423,6 +1423,218 @@ double ConstraintEllipticalArcRangeToEndPoints::maxStep(MAP_pD_D &dir, double li return lim; } +// HyperbolicArcRangeToEndPoints +ConstraintHyperbolicArcRangeToEndPoints::ConstraintHyperbolicArcRangeToEndPoints(Point &p, ArcOfHyperbola &a, double *angle_t) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(angle_t); + e = a; + e.PushOwnParams(pvec); + origpvec = pvec; + rescale(); +} + +void ConstraintHyperbolicArcRangeToEndPoints::ReconstructGeomPointers() +{ + int i=0; + p.x=pvec[i]; i++; + p.y=pvec[i]; i++; + i++;//we have an inline function for the angle + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintHyperbolicArcRangeToEndPoints::getTypeId() +{ + return HyperbolicArcRangeToEndPoints; +} + +void ConstraintHyperbolicArcRangeToEndPoints::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintHyperbolicArcRangeToEndPoints::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + + DeriVector2 c(e.center, param); + DeriVector2 f1(e.focus1, param); + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + double b, db; + b = *e.radmin; db = e.radmin==param ? 1.0 : 0.0; + double a, da; + a = e.getRadMaj(c,f1,b,db,da); + + DeriVector2 multimaj = emaj.multD(b, db);//a vector to muptiply pc by to yield an x for atan2. This is a minor radius drawn along major axis. + DeriVector2 multimin = emin.multD(a, da);//to yield y for atan2 + + DeriVector2 pv(p, param); + DeriVector2 pc = pv.subtr(c); //point referenced to ellipse's center + + double x, dx, y, dy;//distorted coordinates of point in ellipse's coordinates, to be fed to atan2 to yiels a t-parameter (called "angle" here) + x = pc.scalarProd(multimaj, &dx); + y = pc.scalarProd(multimin, &dy); + double xylen2 = x*x + y*y ;//square of length of (x,y) + + double si, co; + si = sin(*angle()); co = cos(*angle()); + + double dAngle = param==angle() ? 1.0 : 0.0; + + if (err) + *err = atan2(-si*x+co*y, co*x+si*y);//instead of calculating atan2(y,x) and subtracting angle, we rotate (x,y) by -angle and calculate atan2 of the result. Hopefully, this will not force angles to zero when x,y happen to be zero. Plus, one atan2 is cheaper to compute than two atan2's. + if (grad) + *grad = -dAngle + ( -dx*y / xylen2 + dy*x / xylen2 ); + +} + +double ConstraintHyperbolicArcRangeToEndPoints::error() +{ + double err; + errorgrad(&err,0,0); + return scale * err; +} + +double ConstraintHyperbolicArcRangeToEndPoints::grad(double *param) +{ + //first of all, check that we need to compute anything. + if ( findParamInPvec(param) == -1 ) return 0.0; + + double deriv; + errorgrad(0, &deriv, param); + + return deriv*scale; +} + +double ConstraintHyperbolicArcRangeToEndPoints::maxStep(MAP_pD_D &dir, double lim) +{ + // step(angle()) <= pi/18 = 10° + MAP_pD_D::iterator it = dir.find(angle()); + if (it != dir.end()) { + double step = std::abs(it->second); + if (step > M_PI/18.) + lim = std::min(lim, (M_PI/18.) / step); + } + return lim; +} + +// ConstraintPointOnHyperbola +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(e.center.x); + pvec.push_back(e.center.y); + pvec.push_back(e.focus1.x); + pvec.push_back(e.focus1.y); + pvec.push_back(e.radmin); + origpvec = pvec; + rescale(); +} + +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, ArcOfHyperbola &e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(e.center.x); + pvec.push_back(e.center.y); + pvec.push_back(e.focus1.x); + pvec.push_back(e.focus1.y); + pvec.push_back(e.radmin); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnHyperbola::getTypeId() +{ + return PointOnHyperbola; +} + +void ConstraintPointOnHyperbola::rescale(double coef) +{ + scale = coef * 1; +} + +double ConstraintPointOnHyperbola::error() +{ + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + // Full sage worksheet at: + // http://forum.freecadweb.org/viewtopic.php?f=10&t=8038&p=110447#p110447 + // + // Err = |PF2| - |PF1| - 2*a + // sage code: + // C = vector([X_c,Y_c]) + // F2 = C+(C-F1) + // X_F2 = F2[0] + // Y_F2 = F2[1] + // a = sqrt((F1-C)*(F1-C)-b*b); + // show(a) + // DM=sqrt((P-F2)*(P-F2))-sqrt((P-F1)*(P-F1))-2*a + // show(DM.simplify_radical()) + double err=-sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + sqrt(pow(X_0 + + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - 2*Y_c, 2)) - 2*sqrt(-pow(b, 2) + + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + return scale * err; +} + +double ConstraintPointOnHyperbola::grad(double *param) +{ + double deriv=0.; + if (param == p1x() || param == p1y() || + param == f1x() || param == f1y() || + param == cx() || param == cy() || + param == rmin()) { + + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + if (param == p1x()) + deriv += -(X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - + 2*Y_c, 2)); + if (param == p1y()) + deriv += -(Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - + 2*Y_c, 2)); + if (param == f1x()) + deriv += (X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - + 2*(X_F1 - X_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, + 2)) + (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + + Y_F1 - 2*Y_c, 2)); + if (param == f1y()) + deriv +=(Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - + 2*(Y_F1 - Y_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, + 2)) + (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + + Y_F1 - 2*Y_c, 2)); + if (param == cx()) + deriv += 2*(X_F1 - X_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 + - Y_c, 2)) - 2*(X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + + pow(Y_0 + Y_F1 - 2*Y_c, 2)); + if (param == cy()) + deriv +=2*(Y_F1 - Y_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 + - Y_c, 2)) - 2*(Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + + pow(Y_0 + Y_F1 - 2*Y_c, 2)); + if (param == rmin()) + deriv += 2*b/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c,2)); + } + return scale * deriv; +} + // ConstraintAngleViaPoint ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve &acrv1, Curve &acrv2, Point p, double* angle) { diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index 7dd94a6be..17a771cfe 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -61,7 +61,9 @@ namespace GCS EqualMajorAxesEllipse = 16, EllipticalArcRangeToEndPoints = 17, AngleViaPoint = 18, - Snell = 19 + Snell = 19, + HyperbolicArcRangeToEndPoints = 20, + PointOnHyperbola = 21 }; enum InternalAlignmentType { @@ -452,6 +454,46 @@ namespace GCS virtual double maxStep(MAP_pD_D &dir, double lim=1.); }; + class ConstraintHyperbolicArcRangeToEndPoints : public Constraint + { + private: + inline double* angle() { return pvec[2]; } + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + Hyperbola e; + Point p; + public: + ConstraintHyperbolicArcRangeToEndPoints(Point &p, ArcOfHyperbola &a, double *angle_t); + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + virtual double maxStep(MAP_pD_D &dir, double lim=1.); + }; + + // PointOnHyperbola + class ConstraintPointOnHyperbola : public Constraint + { + private: + inline double* p1x() { return pvec[0]; } + inline double* p1y() { return pvec[1]; } + inline double* cx() { return pvec[2]; } + inline double* cy() { return pvec[3]; } + inline double* f1x() { return pvec[4]; } + inline double* f1y() { return pvec[5]; } + inline double* rmin() { return pvec[6]; } + public: + ConstraintPointOnHyperbola(Point &p, Hyperbola &e); + ConstraintPointOnHyperbola(Point &p, ArcOfHyperbola &a); + #ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnHyperbola(){} + #endif + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + }; + class ConstraintAngleViaPoint : public Constraint { private: diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 187ebe121..466cfc23d 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -577,6 +577,13 @@ int System::addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId) return addConstraint(constr); } +int System::addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId) +{ + Constraint *constr = new ConstraintPointOnHyperbola(p, e); + constr->setTag(tagId); + return addConstraint(constr); +} + int System::addConstraintEllipticalArcRangeToEndPoints(Point &p, ArcOfEllipse &a, double *angle, int tagId) { Constraint *constr = new ConstraintEllipticalArcRangeToEndPoints(p,a,angle); @@ -594,6 +601,23 @@ int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId) return addConstraintPointOnEllipse(a.end, a, tagId); } +int System::addConstraintHyperbolicArcRangeToEndPoints(Point &p, ArcOfHyperbola &a, double *angle, int tagId) +{ + Constraint *constr = new ConstraintHyperbolicArcRangeToEndPoints(p,a,angle); + constr->setTag(tagId); + return addConstraint(constr); +} + + +int System::addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId) +{ + addConstraintHyperbolicArcRangeToEndPoints(a.start,a,a.startAngle, tagId); + addConstraintHyperbolicArcRangeToEndPoints(a.end,a,a.endAngle, tagId); + + addConstraintPointOnHyperbolicArc(a.start, a, tagId); + return addConstraintPointOnHyperbolicArc(a.end, a, tagId); +} + int System::addConstraintPointOnArc(Point &p, Arc &a, int tagId) { return addConstraintP2PDistance(p, a.center, a.rad, tagId); diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index d9969985a..28a04e571 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -180,8 +180,11 @@ namespace GCS int addConstraintArcRules(Arc &a, int tagId=0); int addConstraintPointOnCircle(Point &p, Circle &c, int tagId=0); int addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId=0); + int addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId=0); int addConstraintEllipticalArcRangeToEndPoints(Point &p, ArcOfEllipse &a, double *angle, int tagId=0); int addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId=0); + int addConstraintHyperbolicArcRangeToEndPoints(Point &p, ArcOfHyperbola &a, double *angle, int tagId=0); + int addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId=0); int addConstraintPointOnArc(Point &p, Arc &a, int tagId=0); int addConstraintPerpendicularLine2Arc(Point &p1, Point &p2, Arc &a, int tagId=0); diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index adca4308f..2f7c765bc 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -301,17 +301,82 @@ ArcOfEllipse* ArcOfEllipse::Copy() return crv; } -//---------------arc of hyperbola +//---------------hyperbola + +//this function is exposed to allow reusing pre-filled derivectors in constraints code +double Hyperbola::getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj) +{ + double cf, dcf; + cf = f1.subtr(center).length(dcf); + DeriVector2 hack (b, cf, + db, dcf);//hack = a nonsense vector to calculate major radius with derivatives, useful just because the calculation formula is the same as vector length formula + return hack.length(ret_dRadMaj); +} + +//returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Hyperbola::getRadMaj(double *derivparam, double &ret_dRadMaj) +{ + DeriVector2 c(center, derivparam); + DeriVector2 f1(focus1, derivparam); + return getRadMaj(c, f1, *radmin, radmin==derivparam ? 1.0 : 0.0, ret_dRadMaj); +} + +//returns the major radius (plain value, no derivatives) +double Hyperbola::getRadMaj() +{ + double dradmaj;//dummy + return getRadMaj(0,dradmaj); +} + +DeriVector2 Hyperbola::CalculateNormal(Point &p, double* derivparam) +{ + //fill some vectors in + DeriVector2 cv (center, derivparam); + DeriVector2 f1v (focus1, derivparam); + DeriVector2 pv (p, derivparam); + + //calculation. + //focus2: + DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0); // 2*cv - f1v + + //pf1, pf2 = vectors from p to focus1,focus2 + DeriVector2 pf1 = f1v.subtr(pv).mult(-1.0); // <--- differs from ellipse normal calculation code by inverting this vector + DeriVector2 pf2 = f2v.subtr(pv); + //return sum of normalized pf2, pf2 + DeriVector2 ret = pf1.getNormalized().sum(pf2.getNormalized()); + + return ret; +} + +int Hyperbola::PushOwnParams(VEC_pD &pvec) +{ + int cnt=0; + pvec.push_back(center.x); cnt++; + pvec.push_back(center.y); cnt++; + pvec.push_back(focus1.x); cnt++; + pvec.push_back(focus1.y); cnt++; + pvec.push_back(radmin); cnt++; + return cnt; +} +void Hyperbola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) +{ + center.x=pvec[cnt]; cnt++; + center.y=pvec[cnt]; cnt++; + focus1.x=pvec[cnt]; cnt++; + focus1.y=pvec[cnt]; cnt++; + radmin=pvec[cnt]; cnt++; +} +Hyperbola* Hyperbola::Copy() +{ + Hyperbola* crv = new Hyperbola(*this); + return crv; +} + +//--------------- arc of hyperbola int ArcOfHyperbola::PushOwnParams(VEC_pD &pvec) { int cnt=0; - // hyperbola - pvec.push_back(center.x); cnt++; - pvec.push_back(center.y); cnt++; - pvec.push_back(focus.x); cnt++; - pvec.push_back(focus.y); cnt++; - pvec.push_back(radmaj); cnt++; - // arc + cnt += Hyperbola::PushOwnParams(pvec); pvec.push_back(start.x); cnt++; pvec.push_back(start.y); cnt++; pvec.push_back(end.x); cnt++; @@ -323,13 +388,7 @@ int ArcOfHyperbola::PushOwnParams(VEC_pD &pvec) } void ArcOfHyperbola::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) { - // hyperbola - center.x=pvec[cnt]; cnt++; - center.y=pvec[cnt]; cnt++; - focus.x=pvec[cnt]; cnt++; - focus.y=pvec[cnt]; cnt++; - radmaj=pvec[cnt]; cnt++; - //arc + Hyperbola::ReconstructOnNewPvec(pvec,cnt); start.x=pvec[cnt]; cnt++; start.y=pvec[cnt]; cnt++; end.x=pvec[cnt]; cnt++; @@ -343,17 +402,4 @@ ArcOfHyperbola* ArcOfHyperbola::Copy() return crv; } -DeriVector2 ArcOfHyperbola::CalculateNormal(Point &p, double* derivparam) -{ - //fill some vectors in - DeriVector2 cv (center, derivparam); - DeriVector2 fv (focus, derivparam); - DeriVector2 pv (p, derivparam); - - DeriVector2 ret = fv; - - return ret; -} - - }//namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index d3de7ea8a..c4abca272 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -183,26 +183,38 @@ namespace GCS virtual void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt); virtual ArcOfEllipse* Copy(); }; - - class ArcOfHyperbola: public Curve + + class Hyperbola: public Curve { public: - ArcOfHyperbola(){startAngle=0;endAngle=0;radmaj = 0;} + Hyperbola(){ radmin = 0;} + virtual ~Hyperbola(){} + Point center; + Point focus1; + double *radmin; + double getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj); + double getRadMaj(double* derivparam, double &ret_dRadMaj); + double getRadMaj(); + DeriVector2 CalculateNormal(Point &p, double* derivparam = 0); + virtual int PushOwnParams(VEC_pD &pvec); + virtual void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt); + virtual Hyperbola* Copy(); + }; + + class ArcOfHyperbola: public Hyperbola + { + public: + ArcOfHyperbola(){startAngle=0;endAngle=0;radmin = 0;} virtual ~ArcOfHyperbola(){} // parameters double *startAngle; double *endAngle; - double *radmaj; Point start; Point end; - Point center; - Point focus; // interface helpers virtual int PushOwnParams(VEC_pD &pvec); virtual void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt); virtual ArcOfHyperbola* Copy(); - // Curve interface - DeriVector2 CalculateNormal(Point &p, double* derivparam = 0); }; } //namespace GCS diff --git a/src/Mod/Sketcher/Gui/ViewProviderSketch.cpp b/src/Mod/Sketcher/Gui/ViewProviderSketch.cpp index a101ab5c8..d09971586 100644 --- a/src/Mod/Sketcher/Gui/ViewProviderSketch.cpp +++ b/src/Mod/Sketcher/Gui/ViewProviderSketch.cpp @@ -2216,7 +2216,7 @@ void ViewProviderSketch::doBoxSelection(const SbVec2s &startPos, const SbVec2s & if (pnt0Inside && pnt1Inside) { double startangle, endangle; - aoh->getRange(startangle, endangle); + aoh->getRange(startangle, endangle, /*emulateCCW=*/true); if (startangle > endangle) // if arc is reversed std::swap(startangle, endangle); @@ -3191,7 +3191,7 @@ void ViewProviderSketch::draw(bool temp) Handle_Geom_TrimmedCurve curve = Handle_Geom_TrimmedCurve::DownCast(aoh->handle()); double startangle, endangle; - aoh->getRange(startangle, endangle); + aoh->getRange(startangle, endangle, /*emulateCCW=*/true); if (startangle > endangle) // if arc is reversed std::swap(startangle, endangle);