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       aResult = GCS::Failed;
 188   }
 189
 190   // additional check that removed constraints are still correct
 191   if (aResult == GCS::Success) {
 192     aSystemWithoutTangent->applySolution();
 193     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
 194     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
 195       if (!isTangentTruth(*aRemIt))
 196         break;
 197     if (aRemIt != aRemovedTangent.end())
 198       aResult = GCS::Failed;
 199   }
 200
 201   if (aResult == GCS::Success)
 202       myEquationSystem = aSystemWithoutTangent;
 203   else {
 204     // Add IDs of removed tangent to the list of conflicting constraints
 205     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
 206     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
 207       myConflictingIDs.insert((*aRemIt)->getTag());
 208   }
 209
 210   return aResult;
 211 }
 212
 213 bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
 214 {
 215   if (theTangent->getTypeId() == GCS::TangentCircumf) {
 216     static const double aTol = 1e-4;
 217     GCS::VEC_pD aParams = theTangent->params();
 218     double dx = *(aParams[2]) - *(aParams[0]);
 219     double dy = *(aParams[3]) - *(aParams[1]);
 220     double aDist2 = dx * dx + dy * dy;
 221     double aRadSum  = *(aParams[4]) + *(aParams[5]);
 222     double aRadDiff = *(aParams[4]) - *(aParams[5]);
 223     double aTol2 = aTol * aRadSum;
 224     aTol2 *= aTol2;
 225     return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 ||
 226            fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2;
 227   }
 228   if (theTangent->getTypeId() == GCS::P2LDistance) {
 229     static const double aTol2 = 1e-12;
 230     GCS::VEC_pD aParams = theTangent->params();
 231     double aDist2 = *(aParams[6]) * *(aParams[6]);
 232     // orthogonal line direction
 233     double aDirX = *(aParams[5]) - *(aParams[3]);
 234     double aDirY = *(aParams[2]) - *(aParams[4]);
 235     double aLen2 = aDirX * aDirX + aDirY * aDirY;
 236     // vector from line's start to point
 237     double aVecX = *(aParams[0]) - *(aParams[2]);
 238     double aVecY = *(aParams[1]) - *(aParams[3]);
 239
 240     double aDot = aVecX * aDirX + aVecY * aDirY;
 241     return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
 242   }
 243   return false;
 244 }
 245
 246 bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
 247 {
 248   ConstraintMap::const_iterator anIt = myConstraints.begin();
 249   for (; anIt != myConstraints.end(); ++anIt)
 250     if (anIt->first->getTag() == theTagID)
 251       return isTangentTruth(anIt->first);
 252   return false;
 253 }
 254
 255 void PlaneGCSSolver_Solver::undo()
 256 {
 257   myEquationSystem->undoSolution();
 258 }
 259
 260 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 261 {
 262   if (!myConfCollected)
 263     const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
 264   return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
 265 }
 266
 267 void PlaneGCSSolver_Solver::collectConflicting()
 268 {
 269   GCS::VEC_I aConflict;
 270   myEquationSystem->getConflicting(aConflict);
 271   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
 272
 273   myEquationSystem->getRedundant(aConflict);
 274   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
 275
 276   myConfCollected = true;
 277 }
 278
 279 int PlaneGCSSolver_Solver::dof() const
 280 {
 281   return const_cast<PlaneGCSSolver_Solver*>(this)->myEquationSystem->dofsNumber();
 282 }