1 // Copyright (C) 2014-2017 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
18 // email : webmaster.salome@opencascade.com<mailto:webmaster.salome@opencascade.com>
21 #include <PlaneGCSSolver_Solver.h>
22 #include <Events_LongOp.h>
25 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
26 : myEquationSystem(new GCS::System),
27 myDiagnoseBeforeSolve(false),
29 myConfCollected(false),
31 myFictiveConstraint(0)
35 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
40 void PlaneGCSSolver_Solver::clear()
42 myEquationSystem->clear();
44 myConstraints.clear();
45 myConflictingIDs.clear();
48 removeFictiveConstraint();
51 void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint)
53 myEquationSystem->addConstraint(theConstraint.get());
54 myConstraints[theConstraint->getTag()].insert(theConstraint);
55 if (theConstraint->getTag() >= 0)
60 void PlaneGCSSolver_Solver::removeConstraint(ConstraintID theID)
62 myConstraints.erase(theID);
63 if (myConstraints.empty()) {
64 myEquationSystem->clear();
65 myDOF = (int)myParameters.size();
67 myEquationSystem->clearByTag(theID);
74 double* PlaneGCSSolver_Solver::createParameter()
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
81 myDiagnoseBeforeSolve = true;
85 void PlaneGCSSolver_Solver::addParameters(const GCS::SET_pD& theParams)
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())
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
98 myDiagnoseBeforeSolve = true;
101 void PlaneGCSSolver_Solver::removeParameters(const GCS::SET_pD& theParams)
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);
108 if (!myConstraints.empty())
109 myDiagnoseBeforeSolve = true;
112 void PlaneGCSSolver_Solver::initialize()
114 Events_LongOp::start(this);
115 addFictiveConstraintIfNecessary();
116 if (myDiagnoseBeforeSolve)
118 myEquationSystem->declareUnknowns(myParameters);
119 myEquationSystem->initSolution();
120 Events_LongOp::end(this);
125 PlaneGCSSolver_Solver::SolveStatus PlaneGCSSolver_Solver::solve()
127 // clear list of conflicting constraints
128 if (myConfCollected) {
129 myConflictingIDs.clear();
130 myConfCollected = false;
133 if (myParameters.empty())
134 return STATUS_INCONSISTENT;
136 GCS::SolveStatus aResult = GCS::Success;
137 Events_LongOp::start(this);
139 aResult = (GCS::SolveStatus)myEquationSystem->solve();
141 addFictiveConstraintIfNecessary();
143 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
145 Events_LongOp::end(this);
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)
160 std::set<GCSConstraintPtr>::const_iterator anIt = aCIt->second.begin();
161 for (; anIt != aCIt->second.end(); ++anIt)
162 if ((*anIt)->getTypeId() == GCS::P2LDistance)
164 if (anIt != aCIt->second.end())
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;
179 if (aResult == GCS::Failed)
180 aStatus = STATUS_FAILED;
182 myEquationSystem->applySolution();
184 myDOF = myEquationSystem->dofsNumber();
188 removeFictiveConstraint();
189 myInitilized = false;
193 void PlaneGCSSolver_Solver::undo()
195 myEquationSystem->undoSolution();
198 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
200 if (!myConfCollected)
201 const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
202 return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
205 void PlaneGCSSolver_Solver::collectConflicting()
207 GCS::VEC_I aConflict;
208 myEquationSystem->getConflicting(aConflict);
209 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
211 myEquationSystem->getRedundant(aConflict);
212 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
214 myConfCollected = true;
217 int PlaneGCSSolver_Solver::dof()
219 if (myDOF < 0 && !myConstraints.empty())
224 void PlaneGCSSolver_Solver::diagnose()
226 myEquationSystem->declareUnknowns(myParameters);
227 myDOF = myEquationSystem->diagnose();
228 myDiagnoseBeforeSolve = false;
231 void PlaneGCSSolver_Solver::addFictiveConstraintIfNecessary()
233 if (!myConstraints.empty() &&
234 myConstraints.find(CID_MOVEMENT) == myConstraints.end())
237 if (myFictiveConstraint)
238 return; // no need several fictive constraints
240 double* aParam = createParameter();
241 double* aFictiveParameter = new double(0.0);
243 myFictiveConstraint = new GCS::ConstraintEqual(aFictiveParameter, aParam);
244 myFictiveConstraint->setTag(CID_FICTIVE);
245 myEquationSystem->addConstraint(myFictiveConstraint);
248 void PlaneGCSSolver_Solver::removeFictiveConstraint()
250 if (myFictiveConstraint) {
251 myEquationSystem->removeConstraint(myFictiveConstraint);
252 myParameters.pop_back();
254 GCS::VEC_pD aParams = myFictiveConstraint->params();
255 for (GCS::VEC_pD::iterator anIt = aParams.begin(); anIt != aParams.end(); ++ anIt)
257 delete myFictiveConstraint;
258 myFictiveConstraint = 0;