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