diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index a86d8afcee..315676f99c 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -3206,7 +3206,12 @@ int Sketch::addAngleAtPointConstraint(int geoId1, if (e2e) { tag = ++ConstraintsCounter; GCSsys.addConstraintP2PCoincident(p, *p2, tag, driving); - GCSsys.addConstraintAngleViaPoint(*crv1, *crv2, p, angle, tag, driving); + if (Geoms[geoId1].type == BSpline && Geoms[geoId2].type == BSpline) { + GCSsys.addConstraintAngleViaTwoPoints(*crv1, *crv2, p, *p2, angle, tag, driving); + } + else { + GCSsys.addConstraintAngleViaPoint(*crv1, *crv2, p, angle, tag, driving); + } } if (avp) { tag = ++ConstraintsCounter; diff --git a/src/Mod/Sketcher/App/SketchObject.cpp b/src/Mod/Sketcher/App/SketchObject.cpp index ba972818a1..2f0bf3934f 100644 --- a/src/Mod/Sketcher/App/SketchObject.cpp +++ b/src/Mod/Sketcher/App/SketchObject.cpp @@ -3840,7 +3840,7 @@ int SketchObject::split(int GeoId, const Base::Vector3d& point) return -1; } -int SketchObject::join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketcher::PointPos posId2) +int SketchObject::join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketcher::PointPos posId2, int continuity) { // No need to check input data validity as this is an sketchobject managed operation @@ -3901,7 +3901,25 @@ int SketchObject::join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketch else if (bsp2->getDegree() < bsp1->getDegree()) bsp2->increaseDegree(bsp1->getDegree()); - // TODO: set up vectors for new poles, knots, mults + // TODO: Check for tangent constraint here + bool makeC1Continuous = (continuity >= 1); + + // TODO: Rescale one or both sections to fulfill some purpose. + // This could include making param between [0,1], and/or making + // C1 continuity possible. + if (makeC1Continuous) { + // We assume here that there is already G1 continuity. + // Just scale parameters to get C1. + Base::Vector3d slope1 = bsp1->firstDerivativeAtParameter(bsp1->getLastParameter()); + Base::Vector3d slope2 = bsp2->firstDerivativeAtParameter(bsp2->getFirstParameter()); + // TODO: slope2 can technically be a zero vector + // But that seems not possible unless the spline is trivial. + // Prove or account for the possibility. + double scale = slope2.Length() / slope1.Length(); + bsp2->scaleKnotsToBounds(0, scale * (bsp2->getLastParameter() - bsp2->getFirstParameter())); + } + + // set up vectors for new poles, knots, mults std::vector poles1 = bsp1->getPoles(); std::vector weights1 = bsp1->getWeights(); std::vector knots1 = bsp1->getKnots(); @@ -3916,13 +3934,18 @@ int SketchObject::join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketch std::vector newKnots(std::move(knots1)); std::vector newMults(std::move(mults1)); + poles2.erase(poles2.begin()); + if (makeC1Continuous) + newPoles.erase(newPoles.end()-1); newPoles.insert(newPoles.end(), std::make_move_iterator(poles2.begin()), std::make_move_iterator(poles2.end())); // TODO: Weights might need to be scaled weights2.erase(weights2.begin()); + if (makeC1Continuous) + newWeights.erase(newWeights.end()-1); newWeights.insert(newWeights.end(), std::make_move_iterator(weights2.begin()), std::make_move_iterator(weights2.end())); @@ -3938,7 +3961,10 @@ int SketchObject::join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketch // end knots can have a multiplicity of (degree + 1) if (bsp1->getDegree() < newMults.back()) - newMults.back() = bsp1->getDegree(); + if (makeC1Continuous) + newMults.back() = bsp1->getDegree()-1; + else + newMults.back() = bsp1->getDegree(); mults2.erase(mults2.begin()); newMults.insert(newMults.end(), std::make_move_iterator(mults2.begin()), @@ -8331,6 +8357,20 @@ void SketchObject::getDirectlyCoincidentPoints(int GeoId, PointPos PosId, PosIdList.push_back((*it)->FirstPos); } } + if ((*it)->Type == Sketcher::Tangent) { + if ((*it)->First == GeoId && (*it)->FirstPos == PosId && + ((*it)->SecondPos == Sketcher::PointPos::start || + (*it)->SecondPos == Sketcher::PointPos::end)) { + GeoIdList.push_back((*it)->Second); + PosIdList.push_back((*it)->SecondPos); + } + if ((*it)->Second == GeoId && (*it)->SecondPos == PosId && + ((*it)->FirstPos == Sketcher::PointPos::start || + (*it)->FirstPos == Sketcher::PointPos::end)) { + GeoIdList.push_back((*it)->First); + PosIdList.push_back((*it)->FirstPos); + } + } } if (GeoIdList.size() == 1) { GeoIdList.clear(); diff --git a/src/Mod/Sketcher/App/SketchObject.h b/src/Mod/Sketcher/App/SketchObject.h index 8a72250b67..d0f3b99ba1 100644 --- a/src/Mod/Sketcher/App/SketchObject.h +++ b/src/Mod/Sketcher/App/SketchObject.h @@ -354,7 +354,11 @@ public: \param geoId1, posId1, geoId2, posId2: the end points to join \retval - 0 on success, -1 on failure */ - int join(int geoId1, Sketcher::PointPos posId1, int geoId2, Sketcher::PointPos posId2); + int join(int geoId1, + Sketcher::PointPos posId1, + int geoId2, + Sketcher::PointPos posId2, + int continuity = 0); /// adds symmetric geometric elements with respect to the refGeoId (line or point) int addSymmetric(const std::vector& geoIdList, diff --git a/src/Mod/Sketcher/App/SketchObjectPyImp.cpp b/src/Mod/Sketcher/App/SketchObjectPyImp.cpp index ee192870f5..631e90b8f8 100644 --- a/src/Mod/Sketcher/App/SketchObjectPyImp.cpp +++ b/src/Mod/Sketcher/App/SketchObjectPyImp.cpp @@ -1336,15 +1336,17 @@ PyObject* SketchObjectPy::join(PyObject* args) int GeoId1(Sketcher::GeoEnum::GeoUndef), GeoId2(Sketcher::GeoEnum::GeoUndef); int PosId1 = static_cast(Sketcher::PointPos::none), PosId2 = static_cast(Sketcher::PointPos::none); + int continuity = 0; - if (!PyArg_ParseTuple(args, "iiii", &GeoId1, &PosId1, &GeoId2, &PosId2)) { + if (!PyArg_ParseTuple(args, "iiii|i", &GeoId1, &PosId1, &GeoId2, &PosId2, &continuity)) { return nullptr; } if (this->getSketchObjectPtr()->join(GeoId1, (Sketcher::PointPos)PosId1, GeoId2, - (Sketcher::PointPos)PosId2)) { + (Sketcher::PointPos)PosId2, + continuity)) { std::stringstream str; str << "Not able to join the curves with end points: (" << GeoId1 << ", " << PosId1 << "), (" << GeoId2 << ", " << PosId2 << ")"; diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index f99bfbb69d..5a66be5bee 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -2650,6 +2650,108 @@ double ConstraintAngleViaPoint::grad(double* param) return scale * deriv; } +// -------------------------------------------------------- +// ConstraintAngleViaTwoPoints +ConstraintAngleViaTwoPoints::ConstraintAngleViaTwoPoints(Curve& acrv1, + Curve& acrv2, + Point p1, + Point p2, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaTwoPoints::~ConstraintAngleViaTwoPoints() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaTwoPoints::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa1.x = pvec[cnt]; + cnt++; + poa1.y = pvec[cnt]; + cnt++; + poa2.x = pvec[cnt]; + cnt++; + poa2.y = pvec[cnt]; + cnt++; + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaTwoPoints::getTypeId() +{ + return AngleViaTwoPoints; +} + +void ConstraintAngleViaTwoPoints::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaTwoPoints::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(poa1); + DeriVector2 n2 = crv2->CalculateNormal(poa2); + + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); + + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). + return scale * err; +} + +double ConstraintAngleViaTwoPoints::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(poa1, param); + DeriVector2 n2 = crv2->CalculateNormal(poa2, param); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); + + return scale * deriv; +} + // -------------------------------------------------------- // ConstraintAngleViaPointAndParam ConstraintAngleViaPointAndParam::ConstraintAngleViaPointAndParam(Curve& acrv1, diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index d092789e96..017165ca6f 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -80,7 +80,8 @@ enum ConstraintType C2LDistance = 31, P2CDistance = 32, AngleViaPointAndParam = 33, - AngleViaPointAndTwoParams = 34 + AngleViaPointAndTwoParams = 34, + AngleViaTwoPoints = 35 }; enum InternalAlignmentType @@ -1126,6 +1127,41 @@ public: double grad(double*) override; }; +class ConstraintAngleViaTwoPoints: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The points + // are easily shallow-copied by C++, so no pointer type here and no delete is necessary. We use + // two points in this method as a workaround for B-splines (and friends). There, normals at + // general points are not implemented, just at their stored start/end points. + Point poa1; + Point poa2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + +public: + ConstraintAngleViaTwoPoints(Curve& acrv1, Curve& acrv2, Point p1, Point p2, double* angle); + ~ConstraintAngleViaTwoPoints() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + // snell's law angles constrainer. Point needs to lie on all three curves to be constraied. class ConstraintSnell: public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index c7e9a19d96..b5b7600189 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -774,6 +774,20 @@ int System::addConstraintAngleViaPoint(Curve& crv1, return addConstraint(constr); } +int System::addConstraintAngleViaTwoPoints(Curve& crv1, + Curve& crv2, + Point& p1, + Point& p2, + double* angle, + int tagId, + bool driving) +{ + Constraint* constr = new ConstraintAngleViaTwoPoints(crv1, crv2, p1, p2, angle); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + int System::addConstraintAngleViaPointAndParam(Curve& crv1, Curve& crv2, Point& p, diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 68175e3e18..5d941a9f91 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -317,6 +317,13 @@ public: double* angle, int tagId = 0, bool driving = true); + int addConstraintAngleViaTwoPoints(Curve& crv1, + Curve& crv2, + Point& p1, + Point& p2, + double* angle, + int tagId = 0, + bool driving = true); int addConstraintAngleViaPointAndParam(Curve& crv1, Curve& crv2, Point& p, diff --git a/src/Mod/Sketcher/Gui/CommandSketcherBSpline.cpp b/src/Mod/Sketcher/Gui/CommandSketcherBSpline.cpp index 3fa34fcbed..0ff88432d7 100644 --- a/src/Mod/Sketcher/Gui/CommandSketcherBSpline.cpp +++ b/src/Mod/Sketcher/Gui/CommandSketcherBSpline.cpp @@ -1052,16 +1052,30 @@ void CmdSketcherJoinCurves::activated(int iMsg) } } + // TODO: Check for tangent constraints between these the two points. + // These need to be explicit: indirect tangency because poles are collinear will not work. + bool tangentConstraintExists = false; + for (const auto& constr : Obj->Constraints.getValues()) { + if (constr->Type == Sketcher::ConstraintType::Tangent + && ((constr->First == GeoIds[0] && constr->FirstPos == PosIds[0] + && constr->Second == GeoIds[1] && constr->SecondPos == PosIds[1]) + || (constr->First == GeoIds[1] && constr->FirstPos == PosIds[1] + && constr->Second == GeoIds[0] && constr->SecondPos == PosIds[0]))) { + tangentConstraintExists = true; + } + } + Gui::Command::openCommand(QT_TRANSLATE_NOOP("Command", "Join Curves")); bool applied = false; try { Gui::cmdAppObjectArgs(selection[0].getObject(), - "join(%d, %d, %d, %d) ", + "join(%d, %d, %d, %d, %d) ", GeoIds[0], static_cast(PosIds[0]), GeoIds[1], - static_cast(PosIds[1])); + static_cast(PosIds[1]), + tangentConstraintExists ? 1 : 0); applied = true; // Warning: GeoId list will have changed