X-Git-Url: http://git.salome-platform.org/gitweb/?a=blobdiff_plain;f=src%2FSketchSolver%2FPlaneGCSSolver%2FPlaneGCSSolver_Solver.cpp;h=4baa58bdb7848ee1d472fec6b8326a0f69400c0a;hb=87447545000c44e455431ed0d401f850f373374e;hp=0c1584873293aa8b5ffe609da08dca24f87d31f0;hpb=884210338f7a0d6ea5442328609c841028503334;p=modules%2Fshaper.git diff --git a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp index 0c1584873..4baa58bdb 100644 --- a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp +++ b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp @@ -1,12 +1,28 @@ -// Copyright (C) 2014-20xx CEA/DEN, EDF R&D - -// File: PlaneGCSSolver_Solver.cpp -// Created: 14 Dec 2014 -// Author: Artem ZHIDKOV +// Copyright (C) 2014-2020 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 +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. +// +// 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 +// +// 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), @@ -34,23 +50,47 @@ 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); - myDOF = -1; + 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); + } else if (theID >= CID_UNKNOWN) myDOF = -1; - } + + myInitilized = false; } double* PlaneGCSSolver_Solver::createParameter() @@ -64,6 +104,22 @@ double* PlaneGCSSolver_Solver::createParameter() return aResult; } +void PlaneGCSSolver_Solver::addParameters(const GCS::SET_pD& theParams) +{ + GCS::SET_pD aParams(theParams); + // leave new parameters only + GCS::VEC_pD::iterator anIt = myParameters.begin(); + for (; anIt != myParameters.end(); ++anIt) + if (aParams.find(*anIt) != aParams.end()) + aParams.erase(*anIt); + + myParameters.insert(myParameters.end(), aParams.begin(), aParams.end()); + if (myConstraints.empty() && myDOF >=0) + myDOF += (int)aParams.size(); // calculate DoF by hand only if there is no constraints yet + else + myDiagnoseBeforeSolve = true; +} + void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams) { for (int i = (int)myParameters.size() - 1; i >= 0; --i) @@ -71,6 +127,8 @@ void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams) myParameters.erase(myParameters.begin() + i); --myDOF; } + if (!myConstraints.empty()) + myDiagnoseBeforeSolve = true; } void PlaneGCSSolver_Solver::initialize() @@ -95,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); @@ -103,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, @@ -144,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; } @@ -163,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; }