src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp

   1 // Copyright (C) 2014-2023  CEA, EDF
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 #include <PlaneGCSSolver_Solver.h>
  21 #include <Events_LongOp.h>
  22
  23 // Multiplier to correlate IDs of SketchPlugin constraint and primitive PlaneGCS constraints
  24 static const int THE_CONSTRAINT_MULT = 100;
  25
  26
  27 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
  28   : myEquationSystem(new GCS::System),
  29     myDiagnoseBeforeSolve(false),
  30     myInitilized(false),
  31     myConfCollected(false),
  32     myDOF(0),
  33     myFictiveConstraint(0)
  34 {
  35 }
  36
  37 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
  38 {
  39   clear();
  40 }
  41
  42 void PlaneGCSSolver_Solver::clear()
  43 {
  44   myEquationSystem->clear();
  45   myParameters.clear();
  46   myConstraints.clear();
  47   myConflictingIDs.clear();
  48   myDOF = 0;
  49
  50   removeFictiveConstraint();
  51 }
  52
  53 void PlaneGCSSolver_Solver::addConstraint(const ConstraintID& theMultiConstraintID,
  54                                           const std::list<GCSConstraintPtr>& theConstraints)
  55 {
  56   int anID = theMultiConstraintID > CID_UNKNOWN ?
  57              theMultiConstraintID * THE_CONSTRAINT_MULT :
  58              theMultiConstraintID;
  59
  60   for (std::list<GCSConstraintPtr>::const_iterator anIt = theConstraints.begin();
  61        anIt != theConstraints.end(); ++anIt) {
  62     GCSConstraintPtr aConstraint = *anIt;
  63     aConstraint->setTag(anID);
  64     myEquationSystem->addConstraint(aConstraint.get());
  65
  66     if (anID > CID_UNKNOWN)
  67       ++anID;
  68   }
  69   myConstraints[theMultiConstraintID] = theConstraints;
  70
  71   if (theMultiConstraintID >= CID_UNKNOWN)
  72     myDOF = -1;
  73   myInitilized = false;
  74 }
  75
  76 void PlaneGCSSolver_Solver::removeConstraint(const ConstraintID& theID)
  77 {
  78   ConstraintMap::iterator aFound = myConstraints.find(theID);
  79   if (aFound != myConstraints.end()) {
  80     for (std::list<GCSConstraintPtr>::iterator anIt = aFound->second.begin();
  81          anIt != aFound->second.end(); ++anIt)
  82       myEquationSystem->clearByTag((*anIt)->getTag());
  83
  84     myConstraints.erase(aFound);
  85   }
  86
  87   if (myConstraints.empty()) {
  88     myEquationSystem->clear();
  89     myDOF = (int)myParameters.size();
  90   } else if (theID >= CID_UNKNOWN)
  91     myDOF = -1;
  92
  93   myInitilized = false;
  94 }
  95
  96 double* PlaneGCSSolver_Solver::createParameter()
  97 {
  98   double* aResult = new double(0);
  99   myParameters.push_back(aResult);
 100   if (myConstraints.empty() && myDOF >= 0)
 101     ++myDOF; // calculate DoF by hand if and only if there is no constraints yet
 102   else
 103     myDiagnoseBeforeSolve = true;
 104   return aResult;
 105 }
 106
 107 void PlaneGCSSolver_Solver::addParameters(const GCS::SET_pD& theParams)
 108 {
 109   GCS::SET_pD aParams(theParams);
 110   // leave new parameters only
 111   GCS::VEC_pD::iterator anIt = myParameters.begin();
 112   for (; anIt != myParameters.end(); ++anIt)
 113     if (aParams.find(*anIt) != aParams.end())
 114       aParams.erase(*anIt);
 115
 116   myParameters.insert(myParameters.end(), aParams.begin(), aParams.end());
 117   if (myConstraints.empty() && myDOF >=0)
 118     myDOF += (int)aParams.size(); // calculate DoF by hand only if there is no constraints yet
 119   else
 120     myDiagnoseBeforeSolve = true;
 121 }
 122
 123 void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams)
 124 {
 125   for (int i = (int)myParameters.size() - 1; i >= 0; --i)
 126     if (theParams.find(myParameters[i]) != theParams.end()) {
 127       myParameters.erase(myParameters.begin() + i);
 128       --myDOF;
 129     }
 130   if (!myConstraints.empty())
 131     myDiagnoseBeforeSolve = true;
 132 }
 133
 134 void PlaneGCSSolver_Solver::initialize()
 135 {
 136   Events_LongOp::start(this);
 137   addFictiveConstraintIfNecessary();
 138   if (myDiagnoseBeforeSolve)
 139     diagnose();
 140   myEquationSystem->declareUnknowns(myParameters);
 141   myEquationSystem->initSolution();
 142   Events_LongOp::end(this);
 143
 144   myInitilized = true;
 145 }
 146
 147 PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve()
 148 {
 149   // clear list of conflicting constraints
 150   if (myConfCollected) {
 151     myConflictingIDs.clear();
 152     myConfCollected = false;
 153   }
 154
 155   if (myParameters.empty())
 156     return myConstraints.empty() ? STATUS_OK : STATUS_INCONSISTENT;
 157
 158   GCS::SolveStatus aResult = GCS::Success;
 159   Events_LongOp::start(this);
 160   if (myInitilized) {
 161     aResult = (GCS::SolveStatus)myEquationSystem->solve();
 162   } else {
 163     addFictiveConstraintIfNecessary();
 164     diagnose();
 165     aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
 166   }
 167
 168   if (aResult == GCS::Failed) {
 169     // DogLeg solver failed without conflicting constraints, try to use Levenberg-Marquardt solver
 170     diagnose(GCS::LevenbergMarquardt);
 171     aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true,
 172                                                         GCS::LevenbergMarquardt);
 173     if (aResult == GCS::Failed) {
 174       diagnose(GCS::BFGS);
 175       aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true, GCS::BFGS);
 176     }
 177   }
 178   Events_LongOp::end(this);
 179
 180   // collect information about conflicting constraints every time,
 181   // sometimes solver reports about succeeded recalculation but has conflicting constraints
 182   // (for example, apply horizontal constraint for a copied feature)
 183   collectConflicting();
 184   if (!myConflictingIDs.empty())
 185     aResult = GCS::Failed;
 186
 187   SolveStatus aStatus;
 188   if (aResult == GCS::Failed)
 189     aStatus = STATUS_FAILED;
 190   else {
 191     myEquationSystem->applySolution();
 192     if (myDOF < 0)
 193       myDOF = myEquationSystem->dofsNumber();
 194     aStatus = STATUS_OK;
 195   }
 196
 197   removeFictiveConstraint();
 198   myInitilized = false;
 199   return aStatus;
 200 }
 201
 202 void PlaneGCSSolver_Solver::undo()
 203 {
 204   myEquationSystem->undoSolution();
 205 }
 206
 207 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 208 {
 209   if (!myConfCollected)
 210     const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
 211   return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
 212 }
 213
 214 void PlaneGCSSolver_Solver::collectConflicting(bool withRedundant)
 215 {
 216   GCS::VEC_I aConflict;
 217   myEquationSystem->getConflicting(aConflict);
 218   // convert PlaneGCS constraint IDs to SketchPlugin's ID
 219   for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt)
 220     myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT);
 221
 222   if (withRedundant) {
 223     myEquationSystem->getRedundant(aConflict);
 224     // convert PlaneGCS constraint IDs to SketchPlugin's ID
 225     for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt)
 226       myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT);
 227   }
 228
 229   myConfCollected = true;
 230 }
 231
 232 int PlaneGCSSolver_Solver::dof()
 233 {
 234   if (myDOF < 0 && !myConstraints.empty())
 235     diagnose();
 236   return myDOF;
 237 }
 238
 239 void PlaneGCSSolver_Solver::diagnose(const GCS::Algorithm& theAlgo)
 240 {
 241   myEquationSystem->declareUnknowns(myParameters);
 242   myDOF = myEquationSystem->diagnose(theAlgo);
 243   myDiagnoseBeforeSolve = false;
 244 }
 245
 246 void PlaneGCSSolver_Solver::getFreeParameters(GCS::SET_pD& theFreeParams)
 247 {
 248   if (myConstraints.empty())
 249     theFreeParams.insert(myParameters.begin(), myParameters.end());
 250   else {
 251     GCS::VEC_pD aParametersCopy = myParameters;
 252     ConstraintMap aConstraintCopy = myConstraints;
 253
 254     // clear the set of equations
 255     clear();
 256     // reset constraints
 257     myParameters = aParametersCopy;
 258     for (ConstraintMap::iterator anIt = aConstraintCopy.begin();
 259          anIt != aConstraintCopy.end(); ++anIt)
 260       addConstraint(anIt->first, anIt->second);
 261
 262     // parameters detection works for Dense QR only
 263     GCS::QRAlgorithm aQRAlgo = myEquationSystem->qrAlgorithm;
 264     myEquationSystem->qrAlgorithm = GCS::EigenDenseQR;
 265     diagnose();
 266     GCS::VEC_pD aFreeParams;
 267     myEquationSystem->getDependentParams(aFreeParams);
 268     theFreeParams.insert(aFreeParams.begin(), aFreeParams.end());
 269     // revert QR decomposition algorithm
 270     myEquationSystem->qrAlgorithm = aQRAlgo;
 271   }
 272
 273   if (theFreeParams.empty())
 274     return;
 275
 276   // find all equal parameters too
 277   struct EqualParameters
 278   {
 279     typedef std::map<double*, std::list<GCS::SET_pD>::iterator> MapParamGroup;
 280
 281     std::list<GCS::SET_pD> myEqualParams;
 282     MapParamGroup myGroups;
 283
 284     void add(double* theParam1, double* theParam2)
 285     {
 286       MapParamGroup::iterator aFound1 = myGroups.find(theParam1);
 287       MapParamGroup::iterator aFound2 = myGroups.find(theParam2);
 288
 289       if (aFound1 == myGroups.end()) {
 290         if (aFound2 == myGroups.end()) {
 291           // create new group
 292           myEqualParams.push_back(GCS::SET_pD());
 293           std::list<GCS::SET_pD>::iterator aGroup = --myEqualParams.end();
 294           aGroup->insert(theParam1);
 295           aGroup->insert(theParam2);
 296           myGroups[theParam1] = aGroup;
 297           myGroups[theParam2] = aGroup;
 298         }
 299         else {
 300           // add first parameter to the second group
 301           aFound2->second->insert(theParam1);
 302           myGroups[theParam1] = aFound2->second;
 303         }
 304       }
 305       else {
 306         if (aFound2 == myGroups.end()) {
 307           // add second parameter to the first group
 308           aFound1->second->insert(theParam2);
 309           myGroups[theParam2] = aFound1->second;
 310         }
 311         else if (aFound1 != aFound2) {
 312           // merge two groups
 313           GCS::SET_pD aCopy = *(aFound2->second);
 314           myEqualParams.erase(aFound2->second);
 315           for (GCS::SET_pD::iterator anIt = aCopy.begin(); anIt != aCopy.end(); ++anIt)
 316             myGroups[*anIt] = aFound1->second;
 317           aFound1->second->insert(aCopy.begin(), aCopy.end());
 318         }
 319       }
 320     }
 321   } anEqualParams;
 322
 323   for (ConstraintMap::iterator anIt = myConstraints.begin(); anIt != myConstraints.end(); ++anIt)
 324     for (std::list<GCSConstraintPtr>::iterator aCIt = anIt->second.begin();
 325          aCIt != anIt->second.end(); ++aCIt) {
 326       if ((*aCIt)->getTypeId() == GCS::Equal)
 327         anEqualParams.add((*aCIt)->params()[0], (*aCIt)->params()[1]);
 328     }
 329
 330   GCS::SET_pD aFreeParamsCopy = theFreeParams;
 331   for (GCS::SET_pD::iterator anIt = aFreeParamsCopy.begin();
 332        anIt != aFreeParamsCopy.end(); ++anIt) {
 333     EqualParameters::MapParamGroup::iterator aFound = anEqualParams.myGroups.find(*anIt);
 334     if (aFound != anEqualParams.myGroups.end())
 335       theFreeParams.insert(aFound->second->begin(), aFound->second->end());
 336   }
 337 }
 338
 339 void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary()
 340 {
 341   bool hasOnlyMovement = true;
 342   for (ConstraintMap::iterator anIt = myConstraints.begin();
 343        anIt != myConstraints.end() && hasOnlyMovement; ++anIt)
 344     hasOnlyMovement = anIt->first == CID_MOVEMENT;
 345   if (!hasOnlyMovement)
 346     return; // regular constraints are available too
 347
 348   if (myFictiveConstraint)
 349     return; // no need several fictive constraints
 350
 351   int aDOF = myDOF;
 352   double* aParam = createParameter();
 353   double* aFictiveParameter = new double(0.0);
 354
 355   myFictiveConstraint = new GCS::ConstraintEqual(aFictiveParameter, aParam);
 356   myFictiveConstraint->setTag(CID_FICTIVE);
 357   myEquationSystem->addConstraint(myFictiveConstraint);
 358   // DoF should not be changed when adding fictive constraint
 359   myDOF = aDOF;
 360 }
 361
 362 void PlaneGCSSolver_Solver::removeFictiveConstraint()
 363 {
 364   if (myFictiveConstraint) {
 365     myEquationSystem->clearByTag(myFictiveConstraint->getTag());
 366     myParameters.pop_back();
 367
 368     GCS::VEC_pD aParams = myFictiveConstraint->params();
 369     for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++anIt) {
 370       double* aPar = *anIt;
 371       delete aPar;
 372     }
 373     delete myFictiveConstraint;
 374     myFictiveConstraint = 0;
 375   }
 376 }