src/SketchSolver/SketchSolver_Storage.cpp

   1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
   2
   3 // File:    SketchSolver_Storage.cpp
   4 // Created: 18 Mar 2015
   5 // Author:  Artem ZHIDKOV
   6
   7 #include <SketchSolver_Storage.h>
   8
   9 #include <math.h>
  10
  11 /** \brief Search the entity/parameter with specified ID in the list of elements
  12  *  \param[in] theEntityID unique ID of the element
  13  *  \param[in] theEntities list of elements
  14  *  \return position of the found element or -1 if the element is not found
  15  */
  16 template<typename T>
  17 static int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities);
  18
  19 /// \brief Compare two parameters to be different
  20 static bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2);
  21 /// \brief Compare two entities to be different
  22 static bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2);
  23 /// \brief Compare two constriants to be different
  24 static bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2);
  25
  26
  27 SketchSolver_Storage::SketchSolver_Storage()
  28   : myParamMaxID(SLVS_E_UNKNOWN),
  29     myEntityMaxID(SLVS_E_UNKNOWN),
  30     myConstrMaxID(SLVS_C_UNKNOWN),
  31     myFixed(SLVS_E_UNKNOWN),
  32     myNeedToResolve(false)
  33 {
  34 }
  35
  36 Slvs_hParam SketchSolver_Storage::addParameter(const Slvs_Param& theParam)
  37 {
  38   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
  39     // parameter is already used, rewrite it
  40     return updateParameter(theParam);
  41   }
  42
  43   Slvs_Param aParam = theParam;
  44   if (aParam.h > myParamMaxID)
  45     myParamMaxID = aParam.h;
  46   else
  47     aParam.h = ++myParamMaxID;
  48   myParameters.push_back(aParam);
  49   myNeedToResolve = true;
  50   return aParam.h;
  51 }
  52
  53 Slvs_hParam SketchSolver_Storage::updateParameter(const Slvs_Param& theParam)
  54 {
  55   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
  56     // parameter already used, rewrite it
  57     int aPos = Search(theParam.h, myParameters);
  58     if (aPos >= 0 && aPos < (int)myParameters.size()) {
  59       myNeedToResolve = myNeedToResolve || IsNotEqual(myParameters[aPos], theParam);
  60       myParameters[aPos] = theParam;
  61       return theParam.h;
  62     }
  63   }
  64
  65   // Parameter is not found, add new one
  66   Slvs_Param aParam = theParam;
  67   aParam.h = 0;
  68   return addParameter(aParam);
  69 }
  70
  71 bool SketchSolver_Storage::removeParameter(const Slvs_hParam& theParamID)
  72 {
  73   int aPos = Search(theParamID, myParameters);
  74   if (aPos >= 0 && aPos < (int)myParameters.size()) {
  75     // Firstly, search the parametes is not used elsewhere
  76     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
  77     for (; anEntIter != myEntities.end(); anEntIter++) {
  78       for (int i = 0; i < 4; i++)
  79         if (anEntIter->param[i] == theParamID)
  80           return false;
  81     }
  82     // Remove parameter
  83     myParameters.erase(myParameters.begin() + aPos);
  84     myParamMaxID = myParameters.empty() ? SLVS_E_UNKNOWN : myParameters.back().h;
  85     myNeedToResolve = true;
  86     myRemovedParameters.insert(theParamID);
  87     return true;
  88   }
  89   return false;
  90 }
  91
  92 const Slvs_Param& SketchSolver_Storage::getParameter(const Slvs_hParam& theParamID) const
  93 {
  94   int aPos = Search(theParamID, myParameters);
  95   if (aPos >= 0 && aPos < (int)myParameters.size())
  96     return myParameters[aPos];
  97
  98   // Parameter is not found, return empty object
  99   static Slvs_Param aDummy;
 100   aDummy.h = 0;
 101   return aDummy;
 102 }
 103
 104
 105 Slvs_hEntity SketchSolver_Storage::addEntity(const Slvs_Entity& theEntity)
 106 {
 107   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 108     // Entity is already used, rewrite it
 109     return updateEntity(theEntity);
 110   }
 111
 112   Slvs_Entity aEntity = theEntity;
 113   if (aEntity.h > myEntityMaxID)
 114     myEntityMaxID = aEntity.h;
 115   else
 116     aEntity.h = ++myEntityMaxID;
 117   myEntities.push_back(aEntity);
 118   myNeedToResolve = true;
 119   return aEntity.h;
 120 }
 121
 122 Slvs_hEntity SketchSolver_Storage::updateEntity(const Slvs_Entity& theEntity)
 123 {
 124   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 125     // Entity already used, rewrite it
 126     int aPos = Search(theEntity.h, myEntities);
 127     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 128       myNeedToResolve = myNeedToResolve || IsNotEqual(myEntities[aPos], theEntity);
 129       myEntities[aPos] = theEntity;
 130       return theEntity.h;
 131     }
 132   }
 133
 134   // Entity is not found, add new one
 135   Slvs_Entity aEntity = theEntity;
 136   aEntity.h = 0;
 137   return addEntity(aEntity);
 138 }
 139
 140 bool SketchSolver_Storage::removeEntity(const Slvs_hEntity& theEntityID)
 141 {
 142   bool aResult = true;
 143   int aPos = Search(theEntityID, myEntities);
 144   if (aPos >= 0 && aPos < (int)myEntities.size()) {
 145     // Firstly, check the entity is not used elsewhere
 146     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 147     for (; anEntIter != myEntities.end(); anEntIter++) {
 148       for (int i = 0; i < 4; i++)
 149         if (anEntIter->point[i] == theEntityID)
 150           return false;
 151       if (anEntIter->distance == theEntityID)
 152         return false;
 153     }
 154     std::set<Slvs_hEntity> anEntAndSubs;
 155     anEntAndSubs.insert(theEntityID);
 156     for (int i = 0; i < 4; i++)
 157       if (myEntities[aPos].point[i] != SLVS_E_UNKNOWN)
 158         anEntAndSubs.insert(myEntities[aPos].point[i]);
 159
 160     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 161     for (; aConstrIter != myConstraints.end(); aConstrIter++) {
 162       Slvs_hEntity anEntIDs[6] = {aConstrIter->ptA, aConstrIter->ptB,
 163           aConstrIter->entityA, aConstrIter->entityB,
 164           aConstrIter->entityC, aConstrIter->entityD};
 165       for (int i = 0; i < 6; i++)
 166         if (anEntAndSubs.find(anEntIDs[i]) != anEntAndSubs.end())
 167           return false;
 168     }
 169     // The entity is not used, remove it and its parameters
 170     Slvs_Entity anEntity = myEntities[aPos];
 171     myEntities.erase(myEntities.begin() + aPos);
 172     myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 173     if (anEntity.distance != SLVS_E_UNKNOWN)
 174       aResult = aResult && removeParameter(anEntity.distance);
 175     for (int i = 0; i < 4; i++)
 176       if (anEntity.param[i] != SLVS_E_UNKNOWN)
 177         aResult = removeParameter(anEntity.param[i]) && aResult;
 178     for (int i = 0; i < 4; i++)
 179       if (anEntity.point[i] != SLVS_E_UNKNOWN)
 180         aResult = removeEntity(anEntity.point[i]) && aResult;
 181     myNeedToResolve = true;
 182     myRemovedEntities.insert(theEntityID);
 183     if (anEntity.type == SLVS_E_POINT_IN_2D || anEntity.type == SLVS_E_POINT_IN_3D)
 184       removeCoincidentPoint(theEntityID);
 185   }
 186   return aResult;
 187 }
 188
 189 const Slvs_Entity& SketchSolver_Storage::getEntity(const Slvs_hEntity& theEntityID) const
 190 {
 191   int aPos = Search(theEntityID, myEntities);
 192   if (aPos >= 0 && aPos < (int)myEntities.size())
 193     return myEntities[aPos];
 194
 195   // Entity is not found, return empty object
 196   static Slvs_Entity aDummy;
 197   aDummy.h = 0;
 198   return aDummy;
 199 }
 200
 201 Slvs_hConstraint SketchSolver_Storage::isPointFixed(const Slvs_hEntity& thePointID) const
 202 {
 203   // Search the set of coincident points
 204   std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 205   for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 206     if (aCPIter->find(thePointID) != aCPIter->end())
 207       break;
 208   if (aCPIter == myCoincidentPoints.end()) {
 209     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 210     for (; aConstrIter != myConstraints.end(); aConstrIter++)
 211       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 212           aConstrIter->ptA == thePointID)
 213         return aConstrIter->h;
 214     return SLVS_E_UNKNOWN;
 215   }
 216
 217   // Search the Rigid constraint
 218   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 219   for (; aConstrIter != myConstraints.end(); aConstrIter++)
 220     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 221         aCPIter->find(aConstrIter->ptA) != aCPIter->end())
 222       return aConstrIter->h;
 223   return SLVS_E_UNKNOWN;
 224 }
 225
 226
 227 Slvs_hConstraint SketchSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 228 {
 229   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 230     // Constraint is already used, rewrite it
 231     return updateConstraint(theConstraint);
 232   }
 233
 234   Slvs_Constraint aConstraint = theConstraint;
 235
 236   // Find a constraint with same type uses same arguments
 237   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 238   for (; aCIt != myConstraints.end(); aCIt++) {
 239     if (aConstraint.type != aCIt->type)
 240       continue;
 241     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 242         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 243         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD) {
 244       aConstraint.h = aCIt->h;
 245       return updateConstraint(aConstraint);
 246     }
 247   }
 248
 249   if (aConstraint.h > myConstrMaxID)
 250     myConstrMaxID = aConstraint.h;
 251   else
 252     aConstraint.h = ++myConstrMaxID;
 253   myConstraints.push_back(aConstraint);
 254   myNeedToResolve = true;
 255   if (aConstraint.type == SLVS_C_POINTS_COINCIDENT)
 256     addCoincidentPoints(aConstraint.ptA, aConstraint.ptB);
 257   return aConstraint.h;
 258 }
 259
 260 Slvs_hConstraint SketchSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 261 {
 262   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 263     // Constraint already used, rewrite it
 264     int aPos = Search(theConstraint.h, myConstraints);
 265     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 266       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 267       myConstraints[aPos] = theConstraint;
 268       if (theConstraint.type == SLVS_C_POINTS_COINCIDENT)
 269         addCoincidentPoints(theConstraint.ptA, theConstraint.ptB);
 270       return theConstraint.h;
 271     }
 272   }
 273
 274   // Constraint is not found, add new one
 275   Slvs_Constraint aConstraint = theConstraint;
 276   aConstraint.h = 0;
 277   return addConstraint(aConstraint);
 278 }
 279
 280 bool SketchSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 281 {
 282   bool aResult = true;
 283   int aPos = Search(theConstraintID, myConstraints);
 284   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 285     Slvs_Constraint aConstraint = myConstraints[aPos];
 286     myConstraints.erase(myConstraints.begin() + aPos);
 287     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 288     myNeedToResolve = true;
 289     myRemovedConstraints.insert(theConstraintID);
 290     // Remove all entities
 291     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
 292         aConstraint.entityA, aConstraint.entityB,
 293         aConstraint.entityC, aConstraint.entityD};
 294     for (int i = 0; i < 6; i++)
 295       if (anEntities[i] != SLVS_E_UNKNOWN)
 296         aResult = removeEntity(anEntities[i]) && aResult;
 297     // remove temporary fixed point, if available
 298     if (myFixed == theConstraintID)
 299       myFixed = SLVS_E_UNKNOWN;
 300   }
 301   return aResult;
 302 }
 303
 304 const Slvs_Constraint& SketchSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
 305 {
 306   int aPos = Search(theConstraintID, myConstraints);
 307   if (aPos >= 0 && aPos < (int)myConstraints.size())
 308     return myConstraints[aPos];
 309
 310   // Constraint is not found, return empty object
 311   static Slvs_Constraint aDummy;
 312   aDummy.h = 0;
 313   return aDummy;
 314 }
 315
 316 std::list<Slvs_Constraint> SketchSolver_Storage::getConstraintsByType(int theConstraintType) const
 317 {
 318   std::list<Slvs_Constraint> aResult;
 319   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 320   for (; aCIter != myConstraints.end(); aCIter++)
 321     if (aCIter->type == theConstraintType)
 322       aResult.push_back(*aCIter);
 323   return aResult;
 324 }
 325
 326
 327 void SketchSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
 328 {
 329   if (myFixed != SLVS_E_UNKNOWN)
 330     return; // the point is already fixed
 331   int aPos = Search(theConstraintID, myConstraints);
 332   if (aPos >= 0 && aPos < (int)myConstraints.size())
 333     myFixed = theConstraintID;
 334 }
 335
 336 void SketchSolver_Storage::getRemoved(
 337     std::set<Slvs_hParam>& theParameters,
 338     std::set<Slvs_hEntity>& theEntities,
 339     std::set<Slvs_hConstraint>& theConstraints)
 340 {
 341   theParameters = myRemovedParameters;
 342   theEntities = myRemovedEntities;
 343   theConstraints = myRemovedConstraints;
 344
 345   myRemovedParameters.clear();
 346   myRemovedEntities.clear();
 347   myRemovedConstraints.clear();
 348 }
 349
 350 void SketchSolver_Storage::initializeSolver(SketchSolver_Solver& theSolver)
 351 {
 352   theSolver.setParameters(myParameters.data(), (int)myParameters.size());
 353   theSolver.setEntities(myEntities.data(), (int)myEntities.size());
 354
 355   // Copy constraints excluding the fixed one
 356   std::vector<Slvs_Constraint> aConstraints = myConstraints;
 357   if (myFixed != SLVS_E_UNKNOWN) {
 358     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
 359     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
 360     for (; anIt != aConstraints.end(); anIt++)
 361       if (anIt->h == myFixed) {
 362         aFixedPoint = anIt->ptA;
 363         aConstraints.erase(anIt);
 364         break;
 365       }
 366     // set dragged parameters
 367     int aPos = Search(aFixedPoint, myEntities);
 368     theSolver.setDraggedParameters(myEntities[aPos].param);
 369   }
 370   theSolver.setConstraints(aConstraints.data(), (int)aConstraints.size());
 371 }
 372
 373 void SketchSolver_Storage::addCoincidentPoints(
 374     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
 375 {
 376   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 377   std::vector< std::set<Slvs_hEntity> >::iterator aFoundIter = myCoincidentPoints.end(); // already found coincidence
 378   bool isFound = false;
 379   for (; aCIter != myCoincidentPoints.end(); aCIter++) {
 380     bool isFirstFound = aCIter->find(thePoint1) != aCIter->end();
 381     bool isSecondFound = aCIter->find(thePoint2) != aCIter->end();
 382     isFound = isFound || isFirstFound || isSecondFound;
 383     if (isFirstFound && isSecondFound)
 384       break; // already coincident
 385     else if (isFirstFound || isSecondFound) {
 386       if (aFoundIter != myCoincidentPoints.end()) {
 387         // merge two sets
 388         aFoundIter->insert(aCIter->begin(), aCIter->end());
 389         myCoincidentPoints.erase(aCIter);
 390         break;
 391       }
 392       aCIter->insert(thePoint1);
 393       aCIter->insert(thePoint2);
 394     }
 395   }
 396   // coincident points not found
 397   if (!isFound) {
 398     std::set<Slvs_hEntity> aNewSet;
 399     aNewSet.insert(thePoint1);
 400     aNewSet.insert(thePoint2);
 401     myCoincidentPoints.push_back(aNewSet);
 402   }
 403 }
 404
 405 void SketchSolver_Storage::removeCoincidentPoint(const Slvs_hEntity& thePoint)
 406 {
 407   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 408   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 409     if (aCIter->find(thePoint) != aCIter->end()) {
 410       aCIter->erase(thePoint);
 411       if (aCIter->size() <= 1)
 412         myCoincidentPoints.erase(aCIter);
 413       break;
 414     }
 415 }
 416
 417 bool SketchSolver_Storage::isCoincident(
 418     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
 419 {
 420   std::vector< std::set<Slvs_hEntity> >::const_iterator aCIter = myCoincidentPoints.begin();
 421   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 422     if (aCIter->find(thePoint1) != aCIter->end() && aCIter->find(thePoint2) != aCIter->end())
 423       return true;
 424   return false;
 425 }
 426
 427
 428
 429
 430 // ========================================================
 431 // =========      Auxiliary functions       ===============
 432 // ========================================================
 433
 434 template<typename T>
 435 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 436 {
 437   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 438   int aVecSize = theEntities.size();
 439   if (theEntities.empty())
 440     return 1;
 441   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 442     aResIndex--;
 443   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
 444     aResIndex++;
 445   if (aResIndex == -1)
 446     aResIndex = aVecSize;
 447   return aResIndex;
 448 }
 449
 450 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
 451 {
 452   return fabs(theParam1.val - theParam2.val) > tolerance;
 453 }
 454
 455 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
 456 {
 457   int i = 0;
 458   for (; theEntity1.param[i] != 0 && i < 4; i++)
 459     if (theEntity1.param[i] != theEntity2.param[i])
 460       return true;
 461   i = 0;
 462   for (; theEntity1.point[i] != 0 && i < 4; i++)
 463     if (theEntity1.point[i] != theEntity2.point[i])
 464       return true;
 465   return false;
 466 }
 467
 468 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
 469 {
 470   return theConstraint1.ptA != theConstraint2.ptA ||
 471          theConstraint1.ptB != theConstraint2.ptB ||
 472          theConstraint1.entityA != theConstraint2.entityA ||
 473          theConstraint1.entityB != theConstraint2.entityB ||
 474          theConstraint1.entityC != theConstraint2.entityC ||
 475          theConstraint1.entityD != theConstraint2.entityD ||
 476          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
 477 }