X-Git-Url: http://git.salome-platform.org/gitweb/?a=blobdiff_plain;ds=sidebyside;f=src%2FSketchSolver%2FPlaneGCSSolver%2FPlaneGCSSolver_Solver.cpp;h=8fcf4aa27b720492f16e5d8e44968ccc7a2d14f6;hb=6c9733606d54a0813c3b7d5522dd74a3af989917;hp=38b60f7fef31009e8b4725e8c251fb53af5c7466;hpb=2f9cc8a307aeb75f6ee8c2c5c97f90acd5f3d53e;p=modules%2Fshaper.git diff --git a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp index 38b60f7fe..8fcf4aa27 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-2022 CEA/DEN, EDF R&D // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public @@ -12,22 +12,25 @@ // // 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), myDiagnoseBeforeSolve(false), myInitilized(false), myConfCollected(false), - myDOF(0) + myDOF(0), + myFictiveConstraint(0) { } @@ -43,28 +46,50 @@ void PlaneGCSSolver_Solver::clear() myConstraints.clear(); myConflictingIDs.clear(); myDOF = 0; + + 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; } @@ -109,6 +134,7 @@ void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams) void PlaneGCSSolver_Solver::initialize() { Events_LongOp::start(this); + addFictiveConstraintIfNecessary(); if (myDiagnoseBeforeSolve) diagnose(); myEquationSystem->declareUnknowns(myParameters); @@ -127,17 +153,28 @@ 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); if (myInitilized) { aResult = (GCS::SolveStatus)myEquationSystem->solve(); } else { - if (myDiagnoseBeforeSolve) - diagnose(); + addFictiveConstraintIfNecessary(); + 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, @@ -157,6 +194,7 @@ PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve() aStatus = STATUS_OK; } + removeFictiveConstraint(); myInitilized = false; return aStatus; } @@ -173,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()); + // 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); - myEquationSystem->getRedundant(aConflict); - myConflictingIDs.insert(aConflict.begin(), aConflict.end()); + 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; } @@ -192,9 +236,141 @@ 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::getFreeParameters(GCS::SET_pD& theFreeParams) +{ + 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->clearByTag(myFictiveConstraint->getTag()); + myParameters.pop_back(); + + GCS::VEC_pD aParams = myFictiveConstraint->params(); + for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++anIt) { + double* aPar = *anIt; + delete aPar; + } + delete myFictiveConstraint; + myFictiveConstraint = 0; + } +}