1 // Copyright (C) 2014-2020 CEA/DEN, EDF R&D
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.
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.
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
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
20 #include <PlaneGCSSolver_Solver.h>
21 #include <Events_LongOp.h>
23 // Multiplier to correlate IDs of SketchPlugin constraint and primitive PlaneGCS constraints
24 static const int THE_CONSTRAINT_MULT = 100;
27 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
28 : myEquationSystem(new GCS::System),
29 myDiagnoseBeforeSolve(false),
31 myConfCollected(false),
33 myFictiveConstraint(0)
37 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
42 void PlaneGCSSolver_Solver::clear()
44 myEquationSystem->clear();
46 myConstraints.clear();
47 myConflictingIDs.clear();
50 removeFictiveConstraint();
53 void PlaneGCSSolver_Solver::addConstraint(const ConstraintID& theMultiConstraintID,
54 const std::list<GCSConstraintPtr>& theConstraints)
56 int anID = theMultiConstraintID > CID_UNKNOWN ?
57 theMultiConstraintID * THE_CONSTRAINT_MULT :
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());
66 if (anID > CID_UNKNOWN)
69 myConstraints[theMultiConstraintID] = theConstraints;
71 if (theMultiConstraintID >= CID_UNKNOWN)
76 void PlaneGCSSolver_Solver::removeConstraint(const ConstraintID& theID)
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());
84 myConstraints.erase(aFound);
87 if (myConstraints.empty()) {
88 myEquationSystem->clear();
89 myDOF = (int)myParameters.size();
90 } else if (theID >= CID_UNKNOWN)
96 double* PlaneGCSSolver_Solver::createParameter()
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
103 myDiagnoseBeforeSolve = true;
107 void PlaneGCSSolver_Solver::addParameters(const GCS::SET_pD& theParams)
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);
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
120 myDiagnoseBeforeSolve = true;
123 void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams)
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);
130 if (!myConstraints.empty())
131 myDiagnoseBeforeSolve = true;
134 void PlaneGCSSolver_Solver::initialize()
136 Events_LongOp::start(this);
137 addFictiveConstraintIfNecessary();
138 if (myDiagnoseBeforeSolve)
140 myEquationSystem->declareUnknowns(myParameters);
141 myEquationSystem->initSolution();
142 Events_LongOp::end(this);
147 PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve()
149 // clear list of conflicting constraints
150 if (myConfCollected) {
151 myConflictingIDs.clear();
152 myConfCollected = false;
155 if (myParameters.empty())
156 return myConstraints.empty() ? STATUS_OK : STATUS_INCONSISTENT;
158 GCS::SolveStatus aResult = GCS::Success;
159 Events_LongOp::start(this);
161 aResult = (GCS::SolveStatus)myEquationSystem->solve();
163 addFictiveConstraintIfNecessary();
165 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
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) {
175 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters, true, GCS::BFGS);
178 Events_LongOp::end(this);
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;
188 if (aResult == GCS::Failed)
189 aStatus = STATUS_FAILED;
191 myEquationSystem->applySolution();
193 myDOF = myEquationSystem->dofsNumber();
197 removeFictiveConstraint();
198 myInitilized = false;
202 void PlaneGCSSolver_Solver::undo()
204 myEquationSystem->undoSolution();
207 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
209 if (!myConfCollected)
210 const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
211 return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
214 void PlaneGCSSolver_Solver::collectConflicting(bool withRedundant)
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);
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);
229 myConfCollected = true;
232 int PlaneGCSSolver_Solver::dof()
234 if (myDOF < 0 && !myConstraints.empty())
239 void PlaneGCSSolver_Solver::diagnose(const GCS::Algorithm& theAlgo)
241 myEquationSystem->declareUnknowns(myParameters);
242 myDOF = myEquationSystem->diagnose(theAlgo);
243 myDiagnoseBeforeSolve = false;
246 void PlaneGCSSolver_Solver::getFreeParameters(GCS::SET_pD& theFreeParams)
248 if (myConstraints.empty())
249 theFreeParams.insert(myParameters.begin(), myParameters.end());
251 GCS::VEC_pD aParametersCopy = myParameters;
252 ConstraintMap aConstraintCopy = myConstraints;
254 // clear the set of equations
257 myParameters = aParametersCopy;
258 for (ConstraintMap::iterator anIt = aConstraintCopy.begin();
259 anIt != aConstraintCopy.end(); ++anIt)
260 addConstraint(anIt->first, anIt->second);
262 // parameters detection works for Dense QR only
263 GCS::QRAlgorithm aQRAlgo = myEquationSystem->qrAlgorithm;
264 myEquationSystem->qrAlgorithm = GCS::EigenDenseQR;
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;
273 if (theFreeParams.empty())
276 // find all equal parameters too
277 struct EqualParameters
279 typedef std::map<double*, std::list<GCS::SET_pD>::iterator> MapParamGroup;
281 std::list<GCS::SET_pD> myEqualParams;
282 MapParamGroup myGroups;
284 void add(double* theParam1, double* theParam2)
286 MapParamGroup::iterator aFound1 = myGroups.find(theParam1);
287 MapParamGroup::iterator aFound2 = myGroups.find(theParam2);
289 if (aFound1 == myGroups.end()) {
290 if (aFound2 == myGroups.end()) {
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;
300 // add first parameter to the second group
301 aFound2->second->insert(theParam1);
302 myGroups[theParam1] = aFound2->second;
306 if (aFound2 == myGroups.end()) {
307 // add second parameter to the first group
308 aFound1->second->insert(theParam2);
309 myGroups[theParam2] = aFound1->second;
311 else if (aFound1 != aFound2) {
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());
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]);
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());
339 void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary()
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
348 if (myFictiveConstraint)
349 return; // no need several fictive constraints
352 double* aParam = createParameter();
353 double* aFictiveParameter = new double(0.0);
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
362 void PlaneGCSSolver_Solver::removeFictiveConstraint()
364 if (myFictiveConstraint) {
365 myEquationSystem->clearByTag(myFictiveConstraint->getTag());
366 myParameters.pop_back();
368 GCS::VEC_pD aParams = myFictiveConstraint->params();
369 for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++anIt) {
370 double* aPar = *anIt;
373 delete myFictiveConstraint;
374 myFictiveConstraint = 0;