src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp

   1 // Copyright (C) 2014-2017  CEA/DEN, EDF R&D
   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
  18 // email : webmaster.salome@opencascade.com<mailto:webmaster.salome@opencascade.com>
  19 //
  20
  21 #include <PlaneGCSSolver_Solver.h>
  22 #include <Events_LongOp.h>
  23
  24 // Multiplier to correlate IDs of SketchPlugin constraint and primitive PlaneGCS constraints
  25 static const int THE_CONSTRAINT_MULT = 10;
  26
  27
  28 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
  29   : myEquationSystem(new GCS::System),
  30     myDiagnoseBeforeSolve(false),
  31     myInitilized(false),
  32     myConfCollected(false),
  33     myDOF(0),
  34     myFictiveConstraint(0)
  35 {
  36 }
  37
  38 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
  39 {
  40   clear();
  41 }
  42
  43 void PlaneGCSSolver_Solver::clear()
  44 {
  45   myEquationSystem->clear();
  46   myParameters.clear();
  47   myConstraints.clear();
  48   myConflictingIDs.clear();
  49   myDOF = 0;
  50
  51   removeFictiveConstraint();
  52 }
  53
  54 void PlaneGCSSolver_Solver::addConstraint(const ConstraintID& theMultiConstraintID,
  55                                           const std::list<GCSConstraintPtr>& theConstraints)
  56 {
  57   int anID = theMultiConstraintID > CID_UNKNOWN ?
  58              theMultiConstraintID * THE_CONSTRAINT_MULT :
  59              theMultiConstraintID;
  60
  61   for (std::list<GCSConstraintPtr>::const_iterator anIt = theConstraints.begin();
  62        anIt != theConstraints.end(); ++anIt) {
  63     GCSConstraintPtr aConstraint = *anIt;
  64     aConstraint->setTag(anID);
  65     myEquationSystem->addConstraint(aConstraint.get());
  66     myConstraints[theMultiConstraintID].insert(aConstraint);
  67
  68     if (anID > CID_UNKNOWN)
  69       ++anID;
  70   }
  71
  72   if (theMultiConstraintID >= CID_UNKNOWN)
  73     myDOF = -1;
  74   myInitilized = false;
  75 }
  76
  77 void PlaneGCSSolver_Solver::removeConstraint(const ConstraintID& theID)
  78 {
  79   ConstraintMap::iterator aFound = myConstraints.find(theID);
  80   if (aFound != myConstraints.end()) {
  81     for (std::set<GCSConstraintPtr>::iterator anIt = aFound->second.begin();
  82          anIt != aFound->second.end(); ++anIt)
  83       myEquationSystem->clearByTag((*anIt)->getTag());
  84
  85     myConstraints.erase(aFound);
  86   }
  87
  88   if (myConstraints.empty()) {
  89     myEquationSystem->clear();
  90     myDOF = (int)myParameters.size();
  91   } else if (theID >= CID_UNKNOWN)
  92     myDOF = -1;
  93
  94   myInitilized = false;
  95 }
  96
  97 double* PlaneGCSSolver_Solver::createParameter()
  98 {
  99   double* aResult = new double(0);
 100   myParameters.push_back(aResult);
 101   if (myConstraints.empty() && myDOF >= 0)
 102     ++myDOF; // calculate DoF by hand if and only if there is no constraints yet
 103   else
 104     myDiagnoseBeforeSolve = true;
 105   return aResult;
 106 }
 107
 108 void PlaneGCSSolver_Solver::addParameters(const GCS::SET_pD& theParams)
 109 {
 110   GCS::SET_pD aParams(theParams);
 111   // leave new parameters only
 112   GCS::VEC_pD::iterator anIt = myParameters.begin();
 113   for (; anIt != myParameters.end(); ++anIt)
 114     if (aParams.find(*anIt) != aParams.end())
 115       aParams.erase(*anIt);
 116
 117   myParameters.insert(myParameters.end(), aParams.begin(), aParams.end());
 118   if (myConstraints.empty() && myDOF >=0)
 119     myDOF += (int)aParams.size(); // calculate DoF by hand only if there is no constraints yet
 120   else
 121     myDiagnoseBeforeSolve = true;
 122 }
 123
 124 void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams)
 125 {
 126   for (int i = (int)myParameters.size() - 1; i >= 0; --i)
 127     if (theParams.find(myParameters[i]) != theParams.end()) {
 128       myParameters.erase(myParameters.begin() + i);
 129       --myDOF;
 130     }
 131   if (!myConstraints.empty())
 132     myDiagnoseBeforeSolve = true;
 133 }
 134
 135 void PlaneGCSSolver_Solver::initialize()
 136 {
 137   Events_LongOp::start(this);
 138   addFictiveConstraintIfNecessary();
 139   if (myDiagnoseBeforeSolve)
 140     diagnose();
 141   myEquationSystem->declareUnknowns(myParameters);
 142   myEquationSystem->initSolution();
 143   Events_LongOp::end(this);
 144
 145   myInitilized = true;
 146 }
 147
 148 PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve()
 149 {
 150   // clear list of conflicting constraints
 151   if (myConfCollected) {
 152     myConflictingIDs.clear();
 153     myConfCollected = false;
 154   }
 155
 156   if (myParameters.empty())
 157     return myConstraints.empty() ? STATUS_OK : STATUS_INCONSISTENT;
 158
 159   GCS::SolveStatus aResult = GCS::Success;
 160   Events_LongOp::start(this);
 161   if (myInitilized) {
 162     aResult = (GCS::SolveStatus)myEquationSystem->solve();
 163   } else {
 164     addFictiveConstraintIfNecessary();
 165     diagnose();
 166     aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
 167   }
 168
 169   if (aResult == GCS::Failed) {
 170     // DogLeg solver failed without conflicting constraints, try to use Levenberg-Marquardt solver
 171     diagnose(GCS::LevenbergMarquardt);
 172     aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true,
 173                                                         GCS::LevenbergMarquardt);
 174     if (aResult == GCS::Failed) {
 175       diagnose(GCS::BFGS);
 176       aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true, GCS::BFGS);
 177     }
 178   }
 179   Events_LongOp::end(this);
 180
 181   // collect information about conflicting constraints every time,
 182   // sometimes solver reports about succeeded recalculation but has conflicting constraints
 183   // (for example, apply horizontal constraint for a copied feature)
 184   collectConflicting();
 185   if (!myConflictingIDs.empty())
 186     aResult = GCS::Failed;
 187
 188   SolveStatus aStatus;
 189   if (aResult == GCS::Failed)
 190     aStatus = STATUS_FAILED;
 191   else {
 192     myEquationSystem->applySolution();
 193     if (myDOF < 0)
 194       myDOF = myEquationSystem->dofsNumber();
 195     aStatus = STATUS_OK;
 196   }
 197
 198   removeFictiveConstraint();
 199   myInitilized = false;
 200   return aStatus;
 201 }
 202
 203 void PlaneGCSSolver_Solver::undo()
 204 {
 205   myEquationSystem->undoSolution();
 206 }
 207
 208 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 209 {
 210   if (!myConfCollected)
 211     const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
 212   return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
 213 }
 214
 215 void PlaneGCSSolver_Solver::collectConflicting(bool withRedundant)
 216 {
 217   GCS::VEC_I aConflict;
 218   myEquationSystem->getConflicting(aConflict);
 219   // convert PlaneGCS constraint IDs to SketchPlugin's ID
 220   for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt)
 221     myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT);
 222
 223   if (withRedundant) {
 224     myEquationSystem->getRedundant(aConflict);
 225     // convert PlaneGCS constraint IDs to SketchPlugin's ID
 226     for (GCS::VEC_I::const_iterator anIt = aConflict.begin(); anIt != aConflict.end(); ++anIt)
 227       myConflictingIDs.insert((*anIt) / THE_CONSTRAINT_MULT);
 228   }
 229
 230   myConfCollected = true;
 231 }
 232
 233 int PlaneGCSSolver_Solver::dof()
 234 {
 235   if (myDOF < 0 && !myConstraints.empty())
 236     diagnose();
 237   return myDOF;
 238 }
 239
 240 void PlaneGCSSolver_Solver::diagnose(const GCS::Algorithm& theAlgo)
 241 {
 242   myEquationSystem->declareUnknowns(myParameters);
 243   myDOF = myEquationSystem->diagnose(theAlgo);
 244   myDiagnoseBeforeSolve = false;
 245 }
 246
 247 void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary()
 248 {
 249   if (!myConstraints.empty() &&
 250       myConstraints.find(CID_MOVEMENT) == myConstraints.end())
 251     return;
 252
 253   if (myFictiveConstraint)
 254     return; // no need several fictive constraints
 255
 256   int aDOF = myDOF;
 257   double* aParam = createParameter();
 258   double* aFictiveParameter = new double(0.0);
 259
 260   myFictiveConstraint = new GCS::ConstraintEqual(aFictiveParameter, aParam);
 261   myFictiveConstraint->setTag(CID_FICTIVE);
 262   myEquationSystem->addConstraint(myFictiveConstraint);
 263   // DoF should not be changed when adding fictive constraint
 264   myDOF = aDOF;
 265 }
 266
 267 void PlaneGCSSolver_Solver::removeFictiveConstraint()
 268 {
 269   if (myFictiveConstraint) {
 270     myEquationSystem->removeConstraint(myFictiveConstraint);
 271     myParameters.pop_back();
 272
 273     GCS::VEC_pD aParams = myFictiveConstraint->params();
 274     for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++ anIt)
 275       delete *anIt;
 276     delete myFictiveConstraint;
 277     myFictiveConstraint = 0;
 278   }
 279 }