]> SALOME platform Git repositories - modules/shaper.git/blob - src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Solver.cpp
Salome HOME
059e6b005c379dd8aaa4f712bd729b9bd445c455
[modules/shaper.git] / src / SketchSolver / PlaneGCSSolver / PlaneGCSSolver_Solver.cpp
1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
2
3 // File:    PlaneGCSSolver_Solver.cpp
4 // Created: 14 Dec 2014
5 // Author:  Artem ZHIDKOV
6
7 #include "PlaneGCSSolver_Solver.h"
8 #include <Events_LongOp.h>
9
10 #include <cmath>
11
12
13 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
14   : myEquationSystem(new GCS::System),
15   myConfCollected(false)
16 {
17 }
18
19 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
20 {
21   clear();
22 }
23
24 void PlaneGCSSolver_Solver::clear()
25 {
26   myEquationSystem->clear();
27   myConstraints.clear();
28   myParameters.clear();
29 }
30
31 void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint,
32     const SketchSolver_ConstraintType theType)
33 {
34   GCS::Constraint* aConstraint = theConstraint.get();
35   if (myConstraints.find(aConstraint) != myConstraints.end())
36     return; // constraint already exists, no need to add it again
37
38   myEquationSystem->addConstraint(aConstraint);
39   myConstraints[aConstraint] = theType;
40 }
41
42 void PlaneGCSSolver_Solver::removeConstraint(GCSConstraintPtr theConstraint)
43 {
44   GCS::Constraint* aConstraint = theConstraint.get();
45   removeConstraint(aConstraint);
46 }
47
48 void PlaneGCSSolver_Solver::removeConstraint(GCS::Constraint* theConstraint)
49 {
50   if (myConstraints.find(theConstraint) == myConstraints.end())
51     return; // no constraint, no need to remove it
52
53   myEquationSystem->removeConstraint(theConstraint);
54   myConstraints.erase(theConstraint);
55 }
56
57 SketchSolver_SolveStatus PlaneGCSSolver_Solver::solve()
58 {
59   // clear list of conflicting constraints
60   if (myConfCollected) {
61     myConflictingIDs.clear();
62     myConfCollected = false;
63   }
64
65   if (myConstraints.empty())
66     return STATUS_EMPTYSET;
67   if (myParameters.empty())
68     return STATUS_INCONSISTENT;
69
70   Events_LongOp::start(this);
71   GCS::SolveStatus aResult = GCS::Success;
72   // if there is a constraint with all attributes constant, set fail status
73   GCS::SET_pD aParameters;
74   aParameters.insert(myParameters.begin(), myParameters.end());
75   ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
76   for (; aConstrIt != myConstraints.end(); ++aConstrIt) {
77     GCS::VEC_pD aParams = aConstrIt->first->params();
78     GCS::VEC_pD::const_iterator aPIt = aParams.begin();
79     for (; aPIt != aParams.end(); ++aPIt)
80       if (aParameters.find(*aPIt) != aParameters.end())
81         break;
82     if (aPIt == aParams.end() && aConstrIt->first->getTag() > 0) {
83       myConflictingIDs.insert(aConstrIt->first->getTag());
84       myConfCollected = true;
85       aResult = GCS::Failed;
86     }
87   }
88   // solve equations
89   if (aResult == GCS::Success)
90     aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
91
92   GCS::VEC_I aRedundantID;
93
94   // Workaround: the system with tangent constraint may fail if the tangent entities are connected smoothly.
95   // Investigate this situation and move constraints to redundant list
96   if (aResult == GCS::Failed && !myTangent.empty()) {
97     GCS::VEC_I aConflictingID;
98     myEquationSystem->getConflicting(aConflictingID);
99     GCS::VEC_I::iterator aCIt = aConflictingID.begin();
100     for (; aCIt != aConflictingID.end(); ++ aCIt) {
101       if (myTangent.find(*aCIt) == myTangent.end())
102         continue;
103       if (isTangentTruth(*aCIt))
104         aRedundantID.push_back(*aCIt);
105     }
106
107     if (!aRedundantID.empty())
108       aResult = GCS::Success; // check redundant constraints
109   }
110
111   // Additionally check redundant constraints
112   if (aResult == GCS::Success || aResult == GCS::Converged) {
113     GCS::VEC_I aRedundantLocal;
114     myEquationSystem->getRedundant(aRedundantLocal);
115     aRedundantID.insert(aRedundantID.end(), aRedundantLocal.begin(), aRedundantLocal.end());
116     // Workaround: remove all point-point coincidences from list of redundant
117     if (!aRedundantID.empty()) {
118       ConstraintMap::const_iterator aCIt = myConstraints.begin();
119       for (; aCIt != myConstraints.end(); ++aCIt) {
120         if (aCIt->second != CONSTRAINT_PT_PT_COINCIDENT)
121           continue;
122         GCS::VEC_I::iterator aRIt = aRedundantID.begin();
123         for (; aRIt != aRedundantID.end(); ++aRIt)
124           if (aCIt->first->getTag() == *aRIt) {
125             aRedundantID.erase(aRIt);
126             break;
127           }
128       }
129     }
130     // The system with tangent constraints may show redundant constraints if the entities are coupled smoothly.
131     // Sometimes tangent constraints are fall to both conflicting and redundant constraints.
132     // Need to check if there are redundant constraints without these tangencies.
133     if (!aRedundantID.empty())
134       aResult = myTangent.empty() ? GCS::Failed : solveWithoutTangent();
135     else
136       aResult = GCS::Success;
137   }
138   Events_LongOp::end(this);
139
140   SketchSolver_SolveStatus aStatus;
141   if (aResult == GCS::Success) {
142     myEquationSystem->applySolution();
143     aStatus = STATUS_OK;
144   } else
145     aStatus = STATUS_FAILED;
146
147   return aStatus;
148 }
149
150 GCS::SolveStatus PlaneGCSSolver_Solver::solveWithoutTangent()
151 {
152   std::shared_ptr<GCS::System> aSystemWithoutTangent(new GCS::System);
153
154   // Remove tangency which leads to redundant or conflicting constraints
155   GCS::VEC_I aConflicting, aRedundant;
156   myEquationSystem->getRedundant(aRedundant);
157   size_t aNbRemove = myTangent.size(); // number of tangent constraints which can be removed
158   myEquationSystem->getConflicting(aConflicting);
159   aRedundant.insert(aRedundant.end(), aConflicting.begin(), aConflicting.end());
160
161   GCS::SET_I aTangentToRemove;
162   GCS::VEC_I::iterator aCIt = aRedundant.begin();
163   for (; aCIt != aRedundant.end() && aNbRemove > 0; ++aCIt)
164     if (myTangent.find(*aCIt) != myTangent.end()) {
165       aTangentToRemove.insert(*aCIt);
166       --aNbRemove;
167     }
168
169   std::set<GCS::Constraint*> aRemovedTangent;
170   ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
171   while (aConstrIt != myConstraints.end()) {
172     GCS::Constraint* aConstraint = aConstrIt->first;
173     int anID = aConstraint->getTag();
174     ++aConstrIt;
175     if (aTangentToRemove.find(anID) == aTangentToRemove.end())
176       aSystemWithoutTangent->addConstraint(aConstraint);
177     else
178       aRemovedTangent.insert(aConstraint);
179   }
180
181   myTangent.clear();
182   GCS::SolveStatus aResult = (GCS::SolveStatus)aSystemWithoutTangent->solve(myParameters);
183   if (aResult == GCS::Success) {
184     GCS::VEC_I aRedundant;
185     aSystemWithoutTangent->getRedundant(aRedundant);
186     if (aRedundant.empty())
187       myEquationSystem = aSystemWithoutTangent;
188     else
189       aResult = GCS::Failed;
190   }
191
192   // additional check that removed constraints are still correct
193   if (aResult == GCS::Success) {
194     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
195     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
196       if (!isTangentTruth(*aRemIt))
197         break;
198     if (aRemIt != aRemovedTangent.end())
199       aResult = GCS::Failed;
200   }
201
202   // Add IDs of removed tangent to the list of conflicting constraints
203   if (aResult == GCS::Failed) {
204     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
205     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
206       myConflictingIDs.insert((*aRemIt)->getTag());
207   }
208
209   return aResult;
210 }
211
212 bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
213 {
214   static const double aTol = 1e-5;
215   static const double aTol2 = aTol *aTol;
216
217   if (theTangent->getTypeId() == GCS::TangentCircumf) {
218     GCS::VEC_pD aParams = theTangent->params();
219     double dx = *(aParams[2]) - *(aParams[0]);
220     double dy = *(aParams[3]) - *(aParams[1]);
221     double aDist2 = dx * dx + dy * dy;
222     double aRadSum  = *(aParams[4]) + *(aParams[5]);
223     double aRadDiff = *(aParams[4]) - *(aParams[5]);
224     return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 * aDist2 ||
225            fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2 * aDist2;
226   }
227   if (theTangent->getTypeId() == GCS::P2LDistance) {
228     GCS::VEC_pD aParams = theTangent->params();
229     double aDist2 = *(aParams[6]) * *(aParams[6]);
230     // orthogonal line direction
231     double aDirX = *(aParams[5]) - *(aParams[3]);
232     double aDirY = *(aParams[2]) - *(aParams[4]);
233     double aLen2 = aDirX * aDirX + aDirY * aDirY;
234     // vector from line's start to point
235     double aVecX = *(aParams[0]) - *(aParams[2]);
236     double aVecY = *(aParams[1]) - *(aParams[3]);
237
238     double aDot = aVecX * aDirX + aVecY * aDirY;
239     return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
240   }
241   return false;
242 }
243
244 bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
245 {
246   ConstraintMap::const_iterator anIt = myConstraints.begin();
247   for (; anIt != myConstraints.end(); ++anIt)
248     if (anIt->first->getTag() == theTagID)
249       return isTangentTruth(anIt->first);
250   return false;
251 }
252
253 void PlaneGCSSolver_Solver::undo()
254 {
255   myEquationSystem->undoSolution();
256 }
257
258 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
259 {
260   if (!myConfCollected)
261     const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
262   return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
263 }
264
265 void PlaneGCSSolver_Solver::collectConflicting()
266 {
267   GCS::VEC_I aConflict;
268   myEquationSystem->getConflicting(aConflict);
269   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
270
271   myEquationSystem->getRedundant(aConflict);
272   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
273
274   myConfCollected = true;
275 }
276
277 int PlaneGCSSolver_Solver::dof() const
278 {
279   return const_cast<PlaneGCSSolver_Solver*>(this)->myEquationSystem->dofsNumber();
280 }