Salome HOME
10e453599057adbcbb54e41aeb738a4a850e9684
[modules/shaper.git] / 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 }