1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
3 // File: PlaneGCSSolver_Solver.cpp
4 // Created: 14 Dec 2014
5 // Author: Artem ZHIDKOV
7 #include "PlaneGCSSolver_Solver.h"
8 #include <Events_LongOp.h>
13 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
14 : myEquationSystem(new GCS::System),
15 myConfCollected(false)
19 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
24 void PlaneGCSSolver_Solver::clear()
26 myEquationSystem->clear();
27 myConstraints.clear();
31 void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint,
32 const SketchSolver_ConstraintType theType)
34 GCS::Constraint* aConstraint = theConstraint.get();
35 if (myConstraints.find(aConstraint) != myConstraints.end())
36 return; // constraint already exists, no need to add it again
38 myEquationSystem->addConstraint(aConstraint);
39 myConstraints[aConstraint] = theType;
42 void PlaneGCSSolver_Solver::removeConstraint(GCSConstraintPtr theConstraint)
44 GCS::Constraint* aConstraint = theConstraint.get();
45 removeConstraint(aConstraint);
48 void PlaneGCSSolver_Solver::removeConstraint(GCS::Constraint* theConstraint)
50 if (myConstraints.find(theConstraint) == myConstraints.end())
51 return; // no constraint, no need to remove it
53 myEquationSystem->removeConstraint(theConstraint);
54 myConstraints.erase(theConstraint);
57 SketchSolver_SolveStatus PlaneGCSSolver_Solver::solve()
59 // clear list of conflicting constraints
60 if (myConfCollected) {
61 myConflictingIDs.clear();
62 myConfCollected = false;
65 if (myConstraints.empty())
66 return STATUS_EMPTYSET;
67 if (myParameters.empty())
68 return STATUS_INCONSISTENT;
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())
82 if (aPIt == aParams.end() && aConstrIt->first->getTag() > 0) {
83 myConflictingIDs.insert(aConstrIt->first->getTag());
84 myConfCollected = true;
85 aResult = GCS::Failed;
89 if (aResult == GCS::Success)
90 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
92 GCS::VEC_I aRedundantID;
94 // Workaround: the system with tangent constraint
95 // may fail if the tangent entities are connected smoothly.
96 // Investigate this situation and move constraints to redundant list
97 if (aResult == GCS::Failed && !myTangent.empty()) {
98 GCS::VEC_I aConflictingID;
99 myEquationSystem->getConflicting(aConflictingID);
100 GCS::VEC_I::iterator aCIt = aConflictingID.begin();
101 for (; aCIt != aConflictingID.end(); ++ aCIt) {
102 if (myTangent.find(*aCIt) == myTangent.end())
104 if (isTangentTruth(*aCIt))
105 aRedundantID.push_back(*aCIt);
108 if (!aRedundantID.empty())
109 aResult = GCS::Success; // check redundant constraints
112 // Additionally check redundant constraints
113 if (aResult == GCS::Success || aResult == GCS::Converged) {
114 GCS::VEC_I aRedundantLocal;
115 myEquationSystem->getRedundant(aRedundantLocal);
116 aRedundantID.insert(aRedundantID.end(), aRedundantLocal.begin(), aRedundantLocal.end());
117 // Workaround: remove all point-point coincidences from list of redundant
118 if (!aRedundantID.empty()) {
119 ConstraintMap::const_iterator aCIt = myConstraints.begin();
120 for (; aCIt != myConstraints.end(); ++aCIt) {
121 if (aCIt->second != CONSTRAINT_PT_PT_COINCIDENT)
123 GCS::VEC_I::iterator aRIt = aRedundantID.begin();
124 for (; aRIt != aRedundantID.end(); ++aRIt)
125 if (aCIt->first->getTag() == *aRIt) {
126 aRedundantID.erase(aRIt);
131 // The system with tangent constraints may show redundant constraints
132 // if the entities are coupled smoothly.
133 // Sometimes tangent constraints are fall to both conflicting and redundant constraints.
134 // Need to check if there are redundant constraints without these tangencies.
135 if (!aRedundantID.empty())
136 aResult = myTangent.empty() ? GCS::Failed : solveWithoutTangent();
138 aResult = GCS::Success;
140 Events_LongOp::end(this);
142 SketchSolver_SolveStatus aStatus;
143 if (aResult == GCS::Success) {
144 myEquationSystem->applySolution();
147 aStatus = STATUS_FAILED;
152 GCS::SolveStatus PlaneGCSSolver_Solver::solveWithoutTangent()
154 std::shared_ptr<GCS::System> aSystemWithoutTangent(new GCS::System);
156 // Remove tangency which leads to redundant or conflicting constraints
157 GCS::VEC_I aConflicting, aRedundant;
158 myEquationSystem->getRedundant(aRedundant);
159 size_t aNbRemove = myTangent.size(); // number of tangent constraints which can be removed
160 myEquationSystem->getConflicting(aConflicting);
161 aRedundant.insert(aRedundant.end(), aConflicting.begin(), aConflicting.end());
163 GCS::SET_I aTangentToRemove;
164 GCS::VEC_I::iterator aCIt = aRedundant.begin();
165 for (; aCIt != aRedundant.end() && aNbRemove > 0; ++aCIt)
166 if (myTangent.find(*aCIt) != myTangent.end()) {
167 aTangentToRemove.insert(*aCIt);
171 std::set<GCS::Constraint*> aRemovedTangent;
172 ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
173 while (aConstrIt != myConstraints.end()) {
174 GCS::Constraint* aConstraint = aConstrIt->first;
175 int anID = aConstraint->getTag();
177 if (aTangentToRemove.find(anID) == aTangentToRemove.end())
178 aSystemWithoutTangent->addConstraint(aConstraint);
180 aRemovedTangent.insert(aConstraint);
184 GCS::SolveStatus aResult = (GCS::SolveStatus)aSystemWithoutTangent->solve(myParameters);
185 if (aResult == GCS::Success) {
186 GCS::VEC_I aRedundant;
187 aSystemWithoutTangent->getRedundant(aRedundant);
188 if (!aRedundant.empty())
189 aResult = GCS::Failed;
192 // additional check that removed constraints are still correct
193 if (aResult == GCS::Success) {
194 aSystemWithoutTangent->applySolution();
195 std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
196 for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
197 if (!isTangentTruth(*aRemIt))
199 if (aRemIt != aRemovedTangent.end())
200 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
203 if (aResult == GCS::Success)
204 myEquationSystem = aSystemWithoutTangent;
206 // Add IDs of removed tangent to the list of conflicting constraints
207 std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
208 for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
209 myConflictingIDs.insert((*aRemIt)->getTag());
215 bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
217 if (theTangent->getTypeId() == GCS::TangentCircumf) {
218 static const double aTol = 1e-4;
219 GCS::VEC_pD aParams = theTangent->params();
220 double dx = *(aParams[2]) - *(aParams[0]);
221 double dy = *(aParams[3]) - *(aParams[1]);
222 double aDist2 = dx * dx + dy * dy;
223 double aRadSum = *(aParams[4]) + *(aParams[5]);
224 double aRadDiff = *(aParams[4]) - *(aParams[5]);
225 double aTol2 = aTol * aRadSum;
227 return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 ||
228 fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2;
230 if (theTangent->getTypeId() == GCS::P2LDistance) {
231 static const double aTol2 = 1e-10;
232 GCS::VEC_pD aParams = theTangent->params();
233 double aDist2 = *(aParams[6]) * *(aParams[6]);
234 // orthogonal line direction
235 double aDirX = *(aParams[5]) - *(aParams[3]);
236 double aDirY = *(aParams[2]) - *(aParams[4]);
237 double aLen2 = aDirX * aDirX + aDirY * aDirY;
238 // vector from line's start to point
239 double aVecX = *(aParams[0]) - *(aParams[2]);
240 double aVecY = *(aParams[1]) - *(aParams[3]);
242 double aDot = aVecX * aDirX + aVecY * aDirY;
243 return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
248 bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
250 ConstraintMap::const_iterator anIt = myConstraints.begin();
251 for (; anIt != myConstraints.end(); ++anIt)
252 if (anIt->first->getTag() == theTagID)
253 return isTangentTruth(anIt->first);
257 void PlaneGCSSolver_Solver::undo()
259 myEquationSystem->undoSolution();
262 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
264 if (!myConfCollected)
265 const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
266 return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
269 void PlaneGCSSolver_Solver::collectConflicting()
271 GCS::VEC_I aConflict;
272 myEquationSystem->getConflicting(aConflict);
273 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
275 myEquationSystem->getRedundant(aConflict);
276 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
278 myConfCollected = true;
281 int PlaneGCSSolver_Solver::dof() const
283 return const_cast<PlaneGCSSolver_Solver*>(this)->myEquationSystem->dofsNumber();