X-Git-Url: http://git.salome-platform.org/gitweb/?a=blobdiff_plain;f=src%2FSketchSolver%2FPlaneGCSSolver%2FPlaneGCSSolver_Solver.cpp;h=47b03a01cd61570aa52253ced5ed8906ba091475;hb=650367c68c1543143d9cd3f7fee54daa39f20bb6;hp=9c4cad57d2a7488cc3fba558c3f5ab04b63cbd6a;hpb=a94fc319f2aa64b43c9a73b5ff7063923648faec;p=modules%2Fshaper.git diff --git a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp index 9c4cad57d..47b03a01c 100644 --- a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp +++ b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp @@ -1,4 +1,4 @@ -// Copyright (C) 2014-2017 CEA/DEN, EDF R&D +// Copyright (C) 2014-2023 CEA, EDF // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public @@ -12,15 +12,17 @@ // // You should have received a copy of the GNU Lesser General Public // License along with this library; if not, write to the Free Software -// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA // -// See http://www.salome-platform.org/ or -// email : webmaster.salome@opencascade.com +// See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com // #include #include +// Multiplier to correlate IDs of SketchPlugin constraint and primitive PlaneGCS constraints +static const int THE_CONSTRAINT_MULT = 100; + PlaneGCSSolver_Solver::PlaneGCSSolver_Solver() : myEquationSystem(new GCS::System), @@ -48,26 +50,46 @@ void PlaneGCSSolver_Solver::clear() removeFictiveConstraint(); } -void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint) +void PlaneGCSSolver_Solver::addConstraint(const ConstraintID& theMultiConstraintID, + const std::list& theConstraints) { - myEquationSystem->addConstraint(theConstraint.get()); - myConstraints[theConstraint->getTag()].insert(theConstraint); - if (theConstraint->getTag() >= 0) + int anID = theMultiConstraintID > CID_UNKNOWN ? + theMultiConstraintID * THE_CONSTRAINT_MULT : + theMultiConstraintID; + + for (std::list::const_iterator anIt = theConstraints.begin(); + anIt != theConstraints.end(); ++anIt) { + GCSConstraintPtr aConstraint = *anIt; + aConstraint->setTag(anID); + myEquationSystem->addConstraint(aConstraint.get()); + + if (anID > CID_UNKNOWN) + ++anID; + } + myConstraints[theMultiConstraintID] = theConstraints; + + if (theMultiConstraintID >= CID_UNKNOWN) myDOF = -1; myInitilized = false; } -void PlaneGCSSolver_Solver::removeConstraint(ConstraintID theID) +void PlaneGCSSolver_Solver::removeConstraint(const ConstraintID& theID) { - myConstraints.erase(theID); + ConstraintMap::iterator aFound = myConstraints.find(theID); + if (aFound != myConstraints.end()) { + for (std::list::iterator anIt = aFound->second.begin(); + anIt != aFound->second.end(); ++anIt) + myEquationSystem->clearByTag((*anIt)->getTag()); + + myConstraints.erase(aFound); + } + if (myConstraints.empty()) { myEquationSystem->clear(); myDOF = (int)myParameters.size(); - } else { - myEquationSystem->clearByTag(theID); - if (theID >= 0) - myDOF = -1; - } + } else if (theID >= CID_UNKNOWN) + myDOF = -1; + myInitilized = false; } @@ -131,7 +153,7 @@ PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve() } if (myParameters.empty()) - return STATUS_INCONSISTENT; + return myConstraints.empty() ? STATUS_OK : STATUS_INCONSISTENT; GCS::SolveStatus aResult = GCS::Success; Events_LongOp::start(this); @@ -139,11 +161,20 @@ PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve() aResult = (GCS::SolveStatus)myEquationSystem->solve(); } else { addFictiveConstraintIfNecessary(); - - if (myDiagnoseBeforeSolve) - diagnose(); + diagnose(); aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters); } + + if (aResult == GCS::Failed) { + // DogLeg solver failed without conflicting constraints, try to use Levenberg-Marquardt solver + diagnose(GCS::LevenbergMarquardt); + aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true, + GCS::LevenbergMarquardt); + if (aResult == GCS::Failed) { + diagnose(GCS::BFGS); + aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true, GCS::BFGS); + } + } Events_LongOp::end(this); // collect information about conflicting constraints every time, @@ -180,14 +211,20 @@ bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) con return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end(); } -void PlaneGCSSolver_Solver::collectConflicting() +void PlaneGCSSolver_Solver::collectConflicting(bool withRedundant) { GCS::VEC_I aConflict; myEquationSystem->getConflicting(aConflict); - myConflictingIDs.insert(aConflict.begin(), aConflict.end()); - - myEquationSystem->getRedundant(aConflict); - myConflictingIDs.insert(aConflict.begin(), aConflict.end()); + // convert PlaneGCS constraint IDs to SketchPlugin's ID + for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt) + myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT); + + if (withRedundant) { + myEquationSystem->getRedundant(aConflict); + // convert PlaneGCS constraint IDs to SketchPlugin's ID + for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt) + myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT); + } myConfCollected = true; } @@ -199,39 +236,140 @@ int PlaneGCSSolver_Solver::dof() return myDOF; } -void PlaneGCSSolver_Solver::diagnose() +void PlaneGCSSolver_Solver::diagnose(const GCS::Algorithm& theAlgo) { myEquationSystem->declareUnknowns(myParameters); - myDOF = myEquationSystem->diagnose(); + myDOF = myEquationSystem->diagnose(theAlgo); myDiagnoseBeforeSolve = false; } -void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary() +void PlaneGCSSolver_Solver::getFreeParameters(GCS::SET_pD& theFreeParams) { - if (!myConstraints.empty() && - myConstraints.find(CID_MOVEMENT) == myConstraints.end()) + if (myConstraints.empty()) + theFreeParams.insert(myParameters.begin(), myParameters.end()); + else { + GCS::VEC_pD aParametersCopy = myParameters; + ConstraintMap aConstraintCopy = myConstraints; + + // clear the set of equations + clear(); + // reset constraints + myParameters = aParametersCopy; + for (ConstraintMap::iterator anIt = aConstraintCopy.begin(); + anIt != aConstraintCopy.end(); ++anIt) + addConstraint(anIt->first, anIt->second); + + // parameters detection works for Dense QR only + GCS::QRAlgorithm aQRAlgo = myEquationSystem->qrAlgorithm; + myEquationSystem->qrAlgorithm = GCS::EigenDenseQR; + diagnose(); + GCS::VEC_pD aFreeParams; + myEquationSystem->getDependentParams(aFreeParams); + theFreeParams.insert(aFreeParams.begin(), aFreeParams.end()); + // revert QR decomposition algorithm + myEquationSystem->qrAlgorithm = aQRAlgo; + } + + if (theFreeParams.empty()) return; + // find all equal parameters too + struct EqualParameters + { + typedef std::map::iterator> MapParamGroup; + + std::list myEqualParams; + MapParamGroup myGroups; + + void add(double* theParam1, double* theParam2) + { + MapParamGroup::iterator aFound1 = myGroups.find(theParam1); + MapParamGroup::iterator aFound2 = myGroups.find(theParam2); + + if (aFound1 == myGroups.end()) { + if (aFound2 == myGroups.end()) { + // create new group + myEqualParams.push_back(GCS::SET_pD()); + std::list::iterator aGroup = --myEqualParams.end(); + aGroup->insert(theParam1); + aGroup->insert(theParam2); + myGroups[theParam1] = aGroup; + myGroups[theParam2] = aGroup; + } + else { + // add first parameter to the second group + aFound2->second->insert(theParam1); + myGroups[theParam1] = aFound2->second; + } + } + else { + if (aFound2 == myGroups.end()) { + // add second parameter to the first group + aFound1->second->insert(theParam2); + myGroups[theParam2] = aFound1->second; + } + else if (aFound1 != aFound2) { + // merge two groups + GCS::SET_pD aCopy = *(aFound2->second); + myEqualParams.erase(aFound2->second); + for (GCS::SET_pD::iterator anIt = aCopy.begin(); anIt != aCopy.end(); ++anIt) + myGroups[*anIt] = aFound1->second; + aFound1->second->insert(aCopy.begin(), aCopy.end()); + } + } + } + } anEqualParams; + + for (ConstraintMap::iterator anIt = myConstraints.begin(); anIt != myConstraints.end(); ++anIt) + for (std::list::iterator aCIt = anIt->second.begin(); + aCIt != anIt->second.end(); ++aCIt) { + if ((*aCIt)->getTypeId() == GCS::Equal) + anEqualParams.add((*aCIt)->params()[0], (*aCIt)->params()[1]); + } + + GCS::SET_pD aFreeParamsCopy = theFreeParams; + for (GCS::SET_pD::iterator anIt = aFreeParamsCopy.begin(); + anIt != aFreeParamsCopy.end(); ++anIt) { + EqualParameters::MapParamGroup::iterator aFound = anEqualParams.myGroups.find(*anIt); + if (aFound != anEqualParams.myGroups.end()) + theFreeParams.insert(aFound->second->begin(), aFound->second->end()); + } +} + +void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary() +{ + bool hasOnlyMovement = true; + for (ConstraintMap::iterator anIt = myConstraints.begin(); + anIt != myConstraints.end() && hasOnlyMovement; ++anIt) + hasOnlyMovement = anIt->first == CID_MOVEMENT; + if (!hasOnlyMovement) + return; // regular constraints are available too + if (myFictiveConstraint) return; // no need several fictive constraints + int aDOF = myDOF; double* aParam = createParameter(); double* aFictiveParameter = new double(0.0); myFictiveConstraint = new GCS::ConstraintEqual(aFictiveParameter, aParam); myFictiveConstraint->setTag(CID_FICTIVE); myEquationSystem->addConstraint(myFictiveConstraint); + // DoF should not be changed when adding fictive constraint + myDOF = aDOF; } void PlaneGCSSolver_Solver::removeFictiveConstraint() { if (myFictiveConstraint) { - myEquationSystem->removeConstraint(myFictiveConstraint); + myEquationSystem->clearByTag(myFictiveConstraint->getTag()); myParameters.pop_back(); GCS::VEC_pD aParams = myFictiveConstraint->params(); - for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++ anIt) - delete *anIt; + for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++anIt) { + double* aPar = *anIt; + delete aPar; + } delete myFictiveConstraint; myFictiveConstraint = 0; }