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