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