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=5b34fd59a0f5abcd3b2b0baf3e97d3e661bf4d15;hpb=a500a52a60724971fa61a3871979c54a1ce9c1d8;p=modules%2Fshaper.git diff --git a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp index 5b34fd59a..8fcf4aa27 100644 --- a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp +++ b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp @@ -1,17 +1,36 @@ -// Copyright (C) 2014-20xx CEA/DEN, EDF R&D - -// File: PlaneGCSSolver_Solver.cpp -// Created: 14 Dec 2014 -// Author: Artem ZHIDKOV +// 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 +// 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), + myDiagnoseBeforeSolve(false), + myInitilized(false), myConfCollected(false), - myDOF(0) + myDOF(0), + myFictiveConstraint(0) { } @@ -27,46 +46,80 @@ void PlaneGCSSolver_Solver::clear() myConstraints.clear(); myConflictingIDs.clear(); myDOF = 0; + + removeFictiveConstraint(); } -void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint, - const SketchSolver_ConstraintType theType) +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()); - // Workaround: avoid tangent constraint in the list of redundant - if (theType == CONSTRAINT_TANGENT_CIRCLE_LINE || - theType == CONSTRAINT_TANGENT_CIRCLE_CIRCLE || - theType == CONSTRAINT_PT_PT_COINCIDENT || - theType == CONSTRAINT_PT_ON_CIRCLE || - theType == CONSTRAINT_PT_ON_LINE) - myConstraintIDsNotRedundant.insert(theConstraint->getTag()); + 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); - myConstraintIDsNotRedundant.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() { double* aResult = new double(0); myParameters.push_back(aResult); - if (myDOF >= 0) - ++myDOF; + if (myConstraints.empty() && myDOF >= 0) + ++myDOF; // calculate DoF by hand if and only if there is no constraints yet + else + myDiagnoseBeforeSolve = true; 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) @@ -74,6 +127,21 @@ void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams) myParameters.erase(myParameters.begin() + i); --myDOF; } + if (!myConstraints.empty()) + myDiagnoseBeforeSolve = true; +} + +void PlaneGCSSolver_Solver::initialize() +{ + Events_LongOp::start(this); + addFictiveConstraintIfNecessary(); + if (myDiagnoseBeforeSolve) + diagnose(); + myEquationSystem->declareUnknowns(myParameters); + myEquationSystem->initSolution(); + Events_LongOp::end(this); + + myInitilized = true; } PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve() @@ -85,30 +153,49 @@ 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); - // solve equations - GCS::SolveStatus aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters); - Events_LongOp::end(this); + if (myInitilized) { + aResult = (GCS::SolveStatus)myEquationSystem->solve(); + } else { + addFictiveConstraintIfNecessary(); + diagnose(); + aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters); + } - GCS::VEC_I aRedundant; - myEquationSystem->getRedundant(aRedundant); - if (!aRedundant.empty()) { - collectConflicting(); - if (!myConflictingIDs.empty()) - aResult = GCS::Failed; + 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, + // sometimes solver reports about succeeded recalculation but has conflicting constraints + // (for example, apply horizontal constraint for a copied feature) + collectConflicting(); + if (!myConflictingIDs.empty()) + aResult = GCS::Failed; SolveStatus aStatus; - if (aResult == GCS::Success) { + if (aResult == GCS::Failed) + aStatus = STATUS_FAILED; + else { myEquationSystem->applySolution(); if (myDOF < 0) myDOF = myEquationSystem->dofsNumber(); aStatus = STATUS_OK; - } else - aStatus = STATUS_FAILED; + } + removeFictiveConstraint(); + myInitilized = false; return aStatus; } @@ -124,20 +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); - // Workaround: avoid conflicting tangent constraints - GCS::VEC_I aTemp = aConflict; - aConflict.clear(); - for (GCS::VEC_I::iterator anIt = aTemp.begin(); anIt != aTemp.end(); ++anIt) - if (myConstraintIDsNotRedundant.find(*anIt) == myConstraintIDsNotRedundant.end()) - aConflict.push_back(*anIt); - 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; } @@ -145,6 +232,145 @@ void PlaneGCSSolver_Solver::collectConflicting() int PlaneGCSSolver_Solver::dof() { if (myDOF < 0 && !myConstraints.empty()) - solve(); + diagnose(); return myDOF; } + +void PlaneGCSSolver_Solver::diagnose(const GCS::Algorithm& theAlgo) +{ + myEquationSystem->declareUnknowns(myParameters); + 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; + } +}