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>
12 // remove indices of all point-point coincidences from the vector
13 static void removePtPtCoincidences(const ConstraintMap& theConstraints, GCS::VEC_I& theVecToClear)
15 ConstraintMap::const_iterator aCIt = theConstraints.begin();
16 for (; aCIt != theConstraints.end(); ++aCIt) {
17 if (aCIt->second != CONSTRAINT_PT_PT_COINCIDENT)
19 GCS::VEC_I::iterator aRIt = theVecToClear.begin();
20 for (; aRIt != theVecToClear.end(); ++aRIt)
21 if (aCIt->first->getTag() == *aRIt) {
22 theVecToClear.erase(aRIt);
29 PlaneGCSSolver_Solver::PlaneGCSSolver_Solver()
30 : myEquationSystem(new GCS::System),
31 myConfCollected(false)
35 PlaneGCSSolver_Solver::~PlaneGCSSolver_Solver()
40 void PlaneGCSSolver_Solver::clear()
42 myEquationSystem->clear();
43 myConstraints.clear();
47 void PlaneGCSSolver_Solver::addConstraint(GCSConstraintPtr theConstraint,
48 const SketchSolver_ConstraintType theType)
50 GCS::Constraint* aConstraint = theConstraint.get();
51 if (myConstraints.find(aConstraint) != myConstraints.end())
52 return; // constraint already exists, no need to add it again
54 myEquationSystem->addConstraint(aConstraint);
55 myConstraints[aConstraint] = theType;
58 void PlaneGCSSolver_Solver::removeConstraint(GCSConstraintPtr theConstraint)
60 GCS::Constraint* aConstraint = theConstraint.get();
61 removeConstraint(aConstraint);
64 void PlaneGCSSolver_Solver::removeConstraint(GCS::Constraint* theConstraint)
66 if (myConstraints.find(theConstraint) == myConstraints.end())
67 return; // no constraint, no need to remove it
69 myEquationSystem->removeConstraint(theConstraint);
70 myConstraints.erase(theConstraint);
73 SketchSolver_SolveStatus PlaneGCSSolver_Solver::solve()
75 // clear list of conflicting constraints
76 if (myConfCollected) {
77 myConflictingIDs.clear();
78 myConfCollected = false;
81 if (myConstraints.empty())
82 return STATUS_EMPTYSET;
83 if (myParameters.empty())
84 return STATUS_INCONSISTENT;
86 Events_LongOp::start(this);
87 GCS::SolveStatus aResult = GCS::Success;
88 // if there is a constraint with all attributes constant, set fail status
89 GCS::SET_pD aParameters;
90 aParameters.insert(myParameters.begin(), myParameters.end());
91 ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
92 for (; aConstrIt != myConstraints.end(); ++aConstrIt) {
93 GCS::VEC_pD aParams = aConstrIt->first->params();
94 GCS::VEC_pD::const_iterator aPIt = aParams.begin();
95 for (; aPIt != aParams.end(); ++aPIt)
96 if (aParameters.find(*aPIt) != aParameters.end())
98 if (aPIt == aParams.end() && aConstrIt->first->getTag() > 0) {
99 myConflictingIDs.insert(aConstrIt->first->getTag());
100 myConfCollected = true;
101 aResult = GCS::Failed;
105 if (aResult == GCS::Success)
106 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
108 GCS::VEC_I aRedundantID;
110 // Workaround: the system with tangent constraint
111 // may fail if the tangent entities are connected smoothly.
112 // Investigate this situation and move constraints to redundant list
113 if (aResult == GCS::Failed && !myTangent.empty()) {
114 GCS::VEC_I aConflictingID;
115 myEquationSystem->getConflicting(aConflictingID);
116 GCS::VEC_I::iterator aCIt = aConflictingID.begin();
117 for (; aCIt != aConflictingID.end(); ++ aCIt) {
118 if (myTangent.find(*aCIt) == myTangent.end())
120 if (isTangentTruth(*aCIt))
121 aRedundantID.push_back(*aCIt);
124 if (!aRedundantID.empty())
125 aResult = GCS::Success; // check redundant constraints
128 // Additionally check redundant constraints
129 if (aResult == GCS::Success || aResult == GCS::Converged) {
130 bool isSolveWithoutTangent = !aRedundantID.empty();
131 GCS::VEC_I aRedundantLocal;
132 myEquationSystem->getRedundant(aRedundantLocal);
133 aRedundantID.insert(aRedundantID.end(), aRedundantLocal.begin(), aRedundantLocal.end());
134 // Workaround: remove all point-point coincidences from list of redundant
135 if (!aRedundantID.empty())
136 removePtPtCoincidences(myConstraints, aRedundantID);
137 // The system with tangent constraints may show redundant constraints
138 // if the entities are coupled smoothly.
139 // Sometimes tangent constraints are fall to both conflicting and redundant constraints.
140 // Need to check if there are redundant constraints without these tangencies.
141 if (!aRedundantID.empty() && !isSolveWithoutTangent) {
142 GCS::VEC_I::iterator aCIt = aRedundantID.begin();
143 for (; aCIt != aRedundantID.end(); ++ aCIt)
144 if (myTangent.find(*aCIt) != myTangent.end()) {
145 isSolveWithoutTangent = true;
149 if (isSolveWithoutTangent)
150 aResult = myTangent.empty() ? GCS::Failed : solveWithoutTangent();
152 aResult = GCS::Success;
154 Events_LongOp::end(this);
156 SketchSolver_SolveStatus aStatus;
157 if (aResult == GCS::Success) {
158 myEquationSystem->applySolution();
161 aStatus = STATUS_FAILED;
166 GCS::SolveStatus PlaneGCSSolver_Solver::solveWithoutTangent()
168 std::shared_ptr<GCS::System> aSystemWithoutTangent(new GCS::System);
170 // Remove tangency which leads to redundant or conflicting constraints
171 GCS::VEC_I aConflicting, aRedundant;
172 myEquationSystem->getRedundant(aRedundant);
173 size_t aNbRemove = myTangent.size(); // number of tangent constraints which can be removed
174 myEquationSystem->getConflicting(aConflicting);
175 aRedundant.insert(aRedundant.end(), aConflicting.begin(), aConflicting.end());
177 GCS::SET_I aTangentToRemove;
178 GCS::VEC_I::iterator aCIt = aRedundant.begin();
179 for (; aCIt != aRedundant.end() && aNbRemove > 0; ++aCIt)
180 if (myTangent.find(*aCIt) != myTangent.end()) {
181 aTangentToRemove.insert(*aCIt);
185 std::set<GCS::Constraint*> aRemovedTangent;
186 ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
187 while (aConstrIt != myConstraints.end()) {
188 GCS::Constraint* aConstraint = aConstrIt->first;
189 int anID = aConstraint->getTag();
191 if (aTangentToRemove.find(anID) == aTangentToRemove.end())
192 aSystemWithoutTangent->addConstraint(aConstraint);
194 aRemovedTangent.insert(aConstraint);
198 GCS::SolveStatus aResult = (GCS::SolveStatus)aSystemWithoutTangent->solve(myParameters);
199 if (aResult == GCS::Success) {
200 GCS::VEC_I aRedundant;
201 aSystemWithoutTangent->getRedundant(aRedundant);
202 if (!aRedundant.empty()) {
203 removePtPtCoincidences(myConstraints, aRedundant);
204 if (!aRedundant.empty())
205 aResult = GCS::Failed;
209 // additional check that removed constraints are still correct
210 if (aResult == GCS::Success) {
211 aSystemWithoutTangent->applySolution();
212 std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
213 for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
214 if (!isTangentTruth(*aRemIt))
216 if (aRemIt != aRemovedTangent.end()) {
217 aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
218 if (aResult != GCS::Failed) {
219 aSystemWithoutTangent = myEquationSystem;
220 aResult = GCS::Success;
225 if (aResult == GCS::Success)
226 myEquationSystem = aSystemWithoutTangent;
228 // Add IDs of removed tangent to the list of conflicting constraints
229 std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
230 for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
231 myConflictingIDs.insert((*aRemIt)->getTag());
237 bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
239 if (theTangent->getTypeId() == GCS::TangentCircumf) {
240 static const double aTol = 1e-4;
241 GCS::VEC_pD aParams = theTangent->params();
242 double dx = *(aParams[2]) - *(aParams[0]);
243 double dy = *(aParams[3]) - *(aParams[1]);
244 double aDist2 = dx * dx + dy * dy;
245 double aRadSum = *(aParams[4]) + *(aParams[5]);
246 double aRadDiff = *(aParams[4]) - *(aParams[5]);
247 double aTol2 = aTol * aRadSum;
249 return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 ||
250 fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2;
252 if (theTangent->getTypeId() == GCS::P2LDistance) {
253 static const double aTol2 = 1e-10;
254 GCS::VEC_pD aParams = theTangent->params();
255 double aDist2 = *(aParams[6]) * *(aParams[6]);
256 // orthogonal line direction
257 double aDirX = *(aParams[5]) - *(aParams[3]);
258 double aDirY = *(aParams[2]) - *(aParams[4]);
259 double aLen2 = aDirX * aDirX + aDirY * aDirY;
260 // vector from line's start to point
261 double aVecX = *(aParams[0]) - *(aParams[2]);
262 double aVecY = *(aParams[1]) - *(aParams[3]);
264 double aDot = aVecX * aDirX + aVecY * aDirY;
265 return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
270 bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
272 ConstraintMap::const_iterator anIt = myConstraints.begin();
273 for (; anIt != myConstraints.end(); ++anIt)
274 if (anIt->first->getTag() == theTagID)
275 return isTangentTruth(anIt->first);
279 void PlaneGCSSolver_Solver::undo()
281 myEquationSystem->undoSolution();
284 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
286 if (!myConfCollected)
287 const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
288 return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
291 void PlaneGCSSolver_Solver::collectConflicting()
293 GCS::VEC_I aConflict;
294 myEquationSystem->getConflicting(aConflict);
295 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
297 myEquationSystem->getRedundant(aConflict);
298 myConflictingIDs.insert(aConflict.begin(), aConflict.end());
300 myConfCollected = true;
303 int PlaneGCSSolver_Solver::dof() const
305 return const_cast<PlaneGCSSolver_Solver*>(this)->myEquationSystem->dofsNumber();