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