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
  95   // may fail if the tangent entities are connected smoothly.
  96   // Investigate this situation and move constraints to redundant list
  97   if (aResult == GCS::Failed && !myTangent.empty()) {
  98     GCS::VEC_I aConflictingID;
  99     myEquationSystem->getConflicting(aConflictingID);
 100     GCS::VEC_I::iterator aCIt = aConflictingID.begin();
 101     for (; aCIt != aConflictingID.end(); ++ aCIt) {
 102       if (myTangent.find(*aCIt) == myTangent.end())
 103         continue;
 104       if (isTangentTruth(*aCIt))
 105         aRedundantID.push_back(*aCIt);
 106     }
 107
 108     if (!aRedundantID.empty())
 109       aResult = GCS::Success; // check redundant constraints
 110   }
 111
 112   // Additionally check redundant constraints
 113   if (aResult == GCS::Success || aResult == GCS::Converged) {
 114     GCS::VEC_I aRedundantLocal;
 115     myEquationSystem->getRedundant(aRedundantLocal);
 116     aRedundantID.insert(aRedundantID.end(), aRedundantLocal.begin(), aRedundantLocal.end());
 117     // Workaround: remove all point-point coincidences from list of redundant
 118     if (!aRedundantID.empty()) {
 119       ConstraintMap::const_iterator aCIt = myConstraints.begin();
 120       for (; aCIt != myConstraints.end(); ++aCIt) {
 121         if (aCIt->second != CONSTRAINT_PT_PT_COINCIDENT)
 122           continue;
 123         GCS::VEC_I::iterator aRIt = aRedundantID.begin();
 124         for (; aRIt != aRedundantID.end(); ++aRIt)
 125           if (aCIt->first->getTag() == *aRIt) {
 126             aRedundantID.erase(aRIt);
 127             break;
 128           }
 129       }
 130     }
 131     // The system with tangent constraints may show redundant constraints
 132     // if the entities are coupled smoothly.
 133     // Sometimes tangent constraints are fall to both conflicting and redundant constraints.
 134     // Need to check if there are redundant constraints without these tangencies.
 135     if (!aRedundantID.empty())
 136       aResult = myTangent.empty() ? GCS::Failed : solveWithoutTangent();
 137     else
 138       aResult = GCS::Success;
 139   }
 140   Events_LongOp::end(this);
 141
 142   SketchSolver_SolveStatus aStatus;
 143   if (aResult == GCS::Success) {
 144     myEquationSystem->applySolution();
 145     aStatus = STATUS_OK;
 146   } else
 147     aStatus = STATUS_FAILED;
 148
 149   return aStatus;
 150 }
 151
 152 GCS::SolveStatus PlaneGCSSolver_Solver::solveWithoutTangent()
 153 {
 154   std::shared_ptr<GCS::System> aSystemWithoutTangent(new GCS::System);
 155
 156   // Remove tangency which leads to redundant or conflicting constraints
 157   GCS::VEC_I aConflicting, aRedundant;
 158   myEquationSystem->getRedundant(aRedundant);
 159   size_t aNbRemove = myTangent.size(); // number of tangent constraints which can be removed
 160   myEquationSystem->getConflicting(aConflicting);
 161   aRedundant.insert(aRedundant.end(), aConflicting.begin(), aConflicting.end());
 162
 163   GCS::SET_I aTangentToRemove;
 164   GCS::VEC_I::iterator aCIt = aRedundant.begin();
 165   for (; aCIt != aRedundant.end() && aNbRemove > 0; ++aCIt)
 166     if (myTangent.find(*aCIt) != myTangent.end()) {
 167       aTangentToRemove.insert(*aCIt);
 168       --aNbRemove;
 169     }
 170
 171   std::set<GCS::Constraint*> aRemovedTangent;
 172   ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
 173   while (aConstrIt != myConstraints.end()) {
 174     GCS::Constraint* aConstraint = aConstrIt->first;
 175     int anID = aConstraint->getTag();
 176     ++aConstrIt;
 177     if (aTangentToRemove.find(anID) == aTangentToRemove.end())
 178       aSystemWithoutTangent->addConstraint(aConstraint);
 179     else
 180       aRemovedTangent.insert(aConstraint);
 181   }
 182
 183   myTangent.clear();
 184   GCS::SolveStatus aResult = (GCS::SolveStatus)aSystemWithoutTangent->solve(myParameters);
 185   if (aResult == GCS::Success) {
 186     GCS::VEC_I aRedundant;
 187     aSystemWithoutTangent->getRedundant(aRedundant);
 188     if (!aRedundant.empty())
 189       aResult = GCS::Failed;
 190   }
 191
 192   // additional check that removed constraints are still correct
 193   if (aResult == GCS::Success) {
 194     aSystemWithoutTangent->applySolution();
 195     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
 196     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
 197       if (!isTangentTruth(*aRemIt))
 198         break;
 199     if (aRemIt != aRemovedTangent.end())
 200       aResult = (GCS::SolveStatus)myEquationSystem->solve(myParameters);
 201   }
 202
 203   if (aResult == GCS::Success)
 204       myEquationSystem = aSystemWithoutTangent;
 205   else {
 206     // Add IDs of removed tangent to the list of conflicting constraints
 207     std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
 208     for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
 209       myConflictingIDs.insert((*aRemIt)->getTag());
 210   }
 211
 212   return aResult;
 213 }
 214
 215 bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
 216 {
 217   if (theTangent->getTypeId() == GCS::TangentCircumf) {
 218     static const double aTol = 1e-4;
 219     GCS::VEC_pD aParams = theTangent->params();
 220     double dx = *(aParams[2]) - *(aParams[0]);
 221     double dy = *(aParams[3]) - *(aParams[1]);
 222     double aDist2 = dx * dx + dy * dy;
 223     double aRadSum  = *(aParams[4]) + *(aParams[5]);
 224     double aRadDiff = *(aParams[4]) - *(aParams[5]);
 225     double aTol2 = aTol * aRadSum;
 226     aTol2 *= aTol2;
 227     return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 ||
 228            fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2;
 229   }
 230   if (theTangent->getTypeId() == GCS::P2LDistance) {
 231     static const double aTol2 = 1e-10;
 232     GCS::VEC_pD aParams = theTangent->params();
 233     double aDist2 = *(aParams[6]) * *(aParams[6]);
 234     // orthogonal line direction
 235     double aDirX = *(aParams[5]) - *(aParams[3]);
 236     double aDirY = *(aParams[2]) - *(aParams[4]);
 237     double aLen2 = aDirX * aDirX + aDirY * aDirY;
 238     // vector from line's start to point
 239     double aVecX = *(aParams[0]) - *(aParams[2]);
 240     double aVecY = *(aParams[1]) - *(aParams[3]);
 241
 242     double aDot = aVecX * aDirX + aVecY * aDirY;
 243     return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
 244   }
 245   return false;
 246 }
 247
 248 bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
 249 {
 250   ConstraintMap::const_iterator anIt = myConstraints.begin();
 251   for (; anIt != myConstraints.end(); ++anIt)
 252     if (anIt->first->getTag() == theTagID)
 253       return isTangentTruth(anIt->first);
 254   return false;
 255 }
 256
 257 void PlaneGCSSolver_Solver::undo()
 258 {
 259   myEquationSystem->undoSolution();
 260 }
 261
 262 bool PlaneGCSSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 263 {
 264   if (!myConfCollected)
 265     const_cast<PlaneGCSSolver_Solver*>(this)->collectConflicting();
 266   return myConflictingIDs.find((int)theConstraint) != myConflictingIDs.end();
 267 }
 268
 269 void PlaneGCSSolver_Solver::collectConflicting()
 270 {
 271   GCS::VEC_I aConflict;
 272   myEquationSystem->getConflicting(aConflict);
 273   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
 274
 275   myEquationSystem->getRedundant(aConflict);
 276   myConflictingIDs.insert(aConflict.begin(), aConflict.end());
 277
 278   myConfCollected = true;
 279 }
 280
 281 int PlaneGCSSolver_Solver::dof() const
 282 {
 283   return const_cast<PlaneGCSSolver_Solver*>(this)->myEquationSystem->dofsNumber();
 284 }