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