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     myDuplicatedConstraint(false)
  34 {
  35 }
  36
  37 Slvs_hParam SketchSolver_Storage::addParameter(const Slvs_Param& theParam)
  38 {
  39   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
  40     // parameter is already used, rewrite it
  41     return updateParameter(theParam);
  42   }
  43
  44   Slvs_Param aParam = theParam;
  45   if (aParam.h > myParamMaxID)
  46     myParamMaxID = aParam.h;
  47   else
  48     aParam.h = ++myParamMaxID;
  49   myParameters.push_back(aParam);
  50   myNeedToResolve = true;
  51   return aParam.h;
  52 }
  53
  54 Slvs_hParam SketchSolver_Storage::updateParameter(const Slvs_Param& theParam)
  55 {
  56   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
  57     // parameter already used, rewrite it
  58     int aPos = Search(theParam.h, myParameters);
  59     if (aPos >= 0 && aPos < (int)myParameters.size()) {
  60       myNeedToResolve = myNeedToResolve || IsNotEqual(myParameters[aPos], theParam);
  61       myParameters[aPos] = theParam;
  62       return theParam.h;
  63     }
  64   }
  65
  66   // Parameter is not found, add new one
  67   Slvs_Param aParam = theParam;
  68   aParam.h = 0;
  69   return addParameter(aParam);
  70 }
  71
  72 bool SketchSolver_Storage::removeParameter(const Slvs_hParam& theParamID)
  73 {
  74   int aPos = Search(theParamID, myParameters);
  75   if (aPos >= 0 && aPos < (int)myParameters.size()) {
  76     // Firstly, search the parametes is not used elsewhere
  77     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
  78     for (; anEntIter != myEntities.end(); anEntIter++) {
  79       for (int i = 0; i < 4; i++)
  80         if (anEntIter->param[i] == theParamID)
  81           return false;
  82     }
  83     // Remove parameter
  84     myParameters.erase(myParameters.begin() + aPos);
  85     myParamMaxID = myParameters.empty() ? SLVS_E_UNKNOWN : myParameters.back().h;
  86     myNeedToResolve = true;
  87     myRemovedParameters.insert(theParamID);
  88     return true;
  89   }
  90   return false;
  91 }
  92
  93 const Slvs_Param& SketchSolver_Storage::getParameter(const Slvs_hParam& theParamID) const
  94 {
  95   int aPos = Search(theParamID, myParameters);
  96   if (aPos >= 0 && aPos < (int)myParameters.size())
  97     return myParameters[aPos];
  98
  99   // Parameter is not found, return empty object
 100   static Slvs_Param aDummy;
 101   aDummy.h = 0;
 102   return aDummy;
 103 }
 104
 105
 106 Slvs_hEntity SketchSolver_Storage::addEntity(const Slvs_Entity& theEntity)
 107 {
 108   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 109     // Entity is already used, rewrite it
 110     return updateEntity(theEntity);
 111   }
 112
 113   Slvs_Entity aEntity = theEntity;
 114   if (aEntity.h > myEntityMaxID)
 115     myEntityMaxID = aEntity.h;
 116   else
 117     aEntity.h = ++myEntityMaxID;
 118   myEntities.push_back(aEntity);
 119   myNeedToResolve = true;
 120   return aEntity.h;
 121 }
 122
 123 Slvs_hEntity SketchSolver_Storage::updateEntity(const Slvs_Entity& theEntity)
 124 {
 125   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 126     // Entity already used, rewrite it
 127     int aPos = Search(theEntity.h, myEntities);
 128     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 129       myNeedToResolve = myNeedToResolve || IsNotEqual(myEntities[aPos], theEntity);
 130       myEntities[aPos] = theEntity;
 131       return theEntity.h;
 132     }
 133   }
 134
 135   // Entity is not found, add new one
 136   Slvs_Entity aEntity = theEntity;
 137   aEntity.h = 0;
 138   return addEntity(aEntity);
 139 }
 140
 141 bool SketchSolver_Storage::removeEntity(const Slvs_hEntity& theEntityID)
 142 {
 143   bool aResult = true;
 144   int aPos = Search(theEntityID, myEntities);
 145   if (aPos >= 0 && aPos < (int)myEntities.size()) {
 146     // Firstly, check the entity and its attributes is not used elsewhere
 147     std::set<Slvs_hEntity> anEntAndSubs;
 148     anEntAndSubs.insert(theEntityID);
 149     for (int i = 0; i < 4; i++)
 150       if (myEntities[aPos].point[i] != SLVS_E_UNKNOWN)
 151         anEntAndSubs.insert(myEntities[aPos].point[i]);
 152
 153     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 154     for (; anEntIter != myEntities.end(); anEntIter++) {
 155       for (int i = 0; i < 4; i++)
 156         if (anEntAndSubs.find(anEntIter->point[i]) != anEntAndSubs.end())
 157           return false;
 158       if (anEntAndSubs.find(anEntIter->distance) != anEntAndSubs.end())
 159         return false;
 160     }
 161     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 162     for (; aConstrIter != myConstraints.end(); aConstrIter++) {
 163       Slvs_hEntity anEntIDs[6] = {aConstrIter->ptA, aConstrIter->ptB,
 164           aConstrIter->entityA, aConstrIter->entityB,
 165           aConstrIter->entityC, aConstrIter->entityD};
 166       for (int i = 0; i < 6; i++)
 167         if (anEntAndSubs.find(anEntIDs[i]) != anEntAndSubs.end())
 168           return false;
 169     }
 170     // The entity is not used, remove it and its parameters
 171     Slvs_Entity anEntity = myEntities[aPos];
 172     myEntities.erase(myEntities.begin() + aPos);
 173     myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 174     if (anEntity.distance != SLVS_E_UNKNOWN)
 175       aResult = aResult && removeParameter(anEntity.distance);
 176     for (int i = 0; i < 4; i++)
 177       if (anEntity.param[i] != SLVS_E_UNKNOWN)
 178         aResult = removeParameter(anEntity.param[i]) && aResult;
 179     for (int i = 0; i < 4; i++)
 180       if (anEntity.point[i] != SLVS_E_UNKNOWN)
 181         aResult = removeEntity(anEntity.point[i]) && aResult;
 182     myNeedToResolve = true;
 183     myRemovedEntities.insert(theEntityID);
 184     if (anEntity.type == SLVS_E_POINT_IN_2D || anEntity.type == SLVS_E_POINT_IN_3D)
 185       removeCoincidentPoint(theEntityID);
 186   }
 187   return aResult;
 188 }
 189
 190 const Slvs_Entity& SketchSolver_Storage::getEntity(const Slvs_hEntity& theEntityID) const
 191 {
 192   int aPos = Search(theEntityID, myEntities);
 193   if (aPos >= 0 && aPos < (int)myEntities.size())
 194     return myEntities[aPos];
 195
 196   // Entity is not found, return empty object
 197   static Slvs_Entity aDummy;
 198   aDummy.h = SLVS_E_UNKNOWN;
 199   return aDummy;
 200 }
 201
 202 Slvs_hEntity SketchSolver_Storage::copyEntity(const Slvs_hEntity& theCopied)
 203 {
 204   int aPos = Search(theCopied, myEntities);
 205   if (aPos < 0 || aPos >= (int)myEntities.size())
 206     return SLVS_E_UNKNOWN;
 207
 208   Slvs_Entity aCopy = myEntities[aPos];
 209   aCopy.h = SLVS_E_UNKNOWN;
 210   int i = 0;
 211   while (aCopy.point[i] != SLVS_E_UNKNOWN) {
 212     aCopy.point[i] = copyEntity(aCopy.point[i]);
 213     i++;
 214   }
 215   if (aCopy.param[0] != SLVS_E_UNKNOWN) {
 216     aPos = Search(aCopy.param[0], myParameters);
 217     i = 0;
 218     while (aCopy.param[i] != SLVS_E_UNKNOWN) {
 219       Slvs_Param aNewParam = myParameters[aPos];
 220       aNewParam.h = SLVS_E_UNKNOWN;
 221       aCopy.param[i] = addParameter(aNewParam);
 222       i++;
 223       aPos++;
 224     }
 225   }
 226   return addEntity(aCopy);
 227 }
 228
 229 void SketchSolver_Storage::copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo)
 230 {
 231   int aPosFrom = Search(theFrom, myEntities);
 232   int aPosTo = Search(theTo, myEntities);
 233   if (aPosFrom < 0 || aPosFrom >= (int)myEntities.size() ||
 234       aPosTo < 0 || aPosTo >= (int)myEntities.size())
 235     return;
 236
 237   Slvs_Entity aEntFrom = myEntities[aPosFrom];
 238   Slvs_Entity aEntTo = myEntities[aPosTo];
 239   int i = 0;
 240   while (aEntFrom.point[i] != SLVS_E_UNKNOWN) {
 241     copyEntity(aEntFrom.point[i], aEntTo.point[i]);
 242     i++;
 243   }
 244   if (aEntFrom.param[0] != SLVS_E_UNKNOWN) {
 245     aPosFrom = Search(aEntFrom.param[0], myParameters);
 246     aPosTo = Search(aEntTo.param[0], myParameters);
 247     i = 0;
 248     while (aEntFrom.param[i] != SLVS_E_UNKNOWN) {
 249       myParameters[aPosTo++].val = myParameters[aPosFrom++].val;
 250       i++;
 251     }
 252   }
 253 }
 254
 255
 256 bool SketchSolver_Storage::isPointFixed(
 257     const Slvs_hEntity& thePointID, Slvs_hConstraint& theFixed, bool theAccurate) const
 258 {
 259   // Search the set of coincident points
 260   std::set<Slvs_hEntity> aCoincident;
 261   aCoincident.insert(thePointID);
 262   std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 263   for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 264     if (aCPIter->find(thePointID) != aCPIter->end()) {
 265       aCoincident = *aCPIter;
 266       break;
 267     }
 268
 269   // Search the Rigid constraint
 270   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 271   for (; aConstrIter != myConstraints.end(); aConstrIter++)
 272     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 273         aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
 274       theFixed = aConstrIter->h;
 275       return true;
 276     }
 277
 278   if (theAccurate) {
 279     // Try to find the fixed entity which uses such point or its coincidence
 280     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 281     for (; anEntIter != myEntities.end(); anEntIter++) {
 282       for (int i = 0; i < 4; i++) {
 283         Slvs_hEntity aPt = anEntIter->point[i];
 284         if (aPt != SLVS_E_UNKNOWN &&
 285             (aPt == thePointID || aCoincident.find(aPt) != aCoincident.end())) {
 286           if (isEntityFixed(anEntIter->h, true))
 287             return true;
 288         }
 289       }
 290     }
 291   }
 292   return SLVS_E_UNKNOWN;
 293 }
 294
 295 bool SketchSolver_Storage::isEntityFixed(const Slvs_hEntity& theEntityID, bool theAccurate) const
 296 {
 297   int aPos = Search(theEntityID, myEntities);
 298   if (aPos < 0 || aPos >= (int)myEntities.size())
 299     return false;
 300
 301   // Firstly, find how many points are under Rigid constraint
 302   int aNbFixed = 0;
 303   for (int i = 0; i < 4; i++) {
 304     Slvs_hEntity aPoint = myEntities[aPos].point[i];
 305     if (aPoint == SLVS_E_UNKNOWN)
 306       continue;
 307
 308     std::set<Slvs_hEntity> aCoincident;
 309     aCoincident.insert(aPoint);
 310     std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 311     for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 312       if (aCPIter->find(aPoint) != aCPIter->end()) {
 313         aCoincident = *aCPIter;
 314         break;
 315       }
 316
 317     // Search the Rigid constraint
 318     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 319     for (; aConstrIter != myConstraints.end(); aConstrIter++)
 320       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 321           aCoincident.find(aConstrIter->ptA) != aCoincident.end())
 322         aNbFixed++;
 323   }
 324
 325   std::list<Slvs_Constraint> aList;
 326   std::list<Slvs_Constraint>::iterator anIt;
 327   Slvs_hConstraint aTempID; // used in isPointFixed() method
 328
 329   if (myEntities[aPos].type == SLVS_E_LINE_SEGMENT) {
 330     if (aNbFixed == 2)
 331       return true;
 332     else if (aNbFixed == 0 || !theAccurate)
 333       return false;
 334     // Additional check (the line may be fixed if it is used by different constraints):
 335     // 1. The line is used in Equal constraint, another entity is fixed and there is a fixed point on line
 336     aList = getConstraintsByType(SLVS_C_PT_ON_LINE);
 337     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 338       if (anIt->entityA == theEntityID && isPointFixed(anIt->ptA, aTempID))
 339         break;
 340     if (anIt != aList.end()) {
 341       aList = getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
 342       aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 343       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 344         if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 345           Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 346           if (isEntityFixed(anOther, false))
 347             return true;
 348         }
 349     }
 350     // 2. The line is used in Parallel/Perpendicular and Length constraints
 351     aList = getConstraintsByType(SLVS_C_PARALLEL);
 352     aList.splice(aList.end(), getConstraintsByType(SLVS_C_PERPENDICULAR));
 353     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 354       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 355         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 356         if (isEntityFixed(anOther, false))
 357           break;
 358       }
 359     if (anIt != aList.end()) {
 360       aList = getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 361       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 362         if ((anIt->ptA == myEntities[aPos].point[0] && anIt->ptB == myEntities[aPos].point[1]) ||
 363             (anIt->ptA == myEntities[aPos].point[1] && anIt->ptB == myEntities[aPos].point[0]))
 364           return true;
 365     }
 366     // 3. Another verifiers ...
 367   } else if (myEntities[aPos].type == SLVS_E_CIRCLE) {
 368     if (aNbFixed == 0)
 369       return false;
 370     // Search for Diameter constraint
 371     aList = getConstraintsByType(SLVS_C_DIAMETER);
 372     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 373       if (anIt->entityA == theEntityID)
 374         return true;
 375     if (!theAccurate)
 376       return false;
 377     // Additional check (the circle may be fixed if it is used by different constraints):
 378     // 1. The circle is used in Equal constraint and another entity is fixed
 379     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 380     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 381       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 382         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 383         if (isEntityFixed(anOther, false))
 384           return true;
 385       }
 386     // 2. Another verifiers ...
 387   } else if (myEntities[aPos].type == SLVS_E_ARC_OF_CIRCLE) {
 388     if (aNbFixed > 2)
 389       return true;
 390     else if (aNbFixed <= 1)
 391       return false;
 392     // Search for Diameter constraint
 393     aList = getConstraintsByType(SLVS_C_DIAMETER);
 394     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 395       if (anIt->entityA == theEntityID)
 396         return true;
 397     if (!theAccurate)
 398       return false;
 399     // Additional check (the arc may be fixed if it is used by different constraints):
 400     // 1. The arc is used in Equal constraint and another entity is fixed
 401     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 402     aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 403     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 404       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 405         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 406         if (isEntityFixed(anOther, false))
 407           return true;
 408       }
 409     // 2. Another verifiers ...
 410   }
 411   return false;
 412 }
 413
 414
 415 Slvs_hConstraint SketchSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 416 {
 417   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 418     // Constraint is already used, rewrite it
 419     return updateConstraint(theConstraint);
 420   }
 421
 422   Slvs_Constraint aConstraint = theConstraint;
 423
 424   // Find a constraint with same type uses same arguments to show user overconstraint situation
 425   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 426   for (; aCIt != myConstraints.end(); aCIt++) {
 427     if (aConstraint.type != aCIt->type)
 428       continue;
 429     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 430         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 431         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD)
 432       myDuplicatedConstraint = true;
 433   }
 434
 435   if (aConstraint.h > myConstrMaxID)
 436     myConstrMaxID = aConstraint.h;
 437   else
 438     aConstraint.h = ++myConstrMaxID;
 439   myConstraints.push_back(aConstraint);
 440   myNeedToResolve = true;
 441   if (aConstraint.type == SLVS_C_POINTS_COINCIDENT)
 442     addCoincidentPoints(aConstraint.ptA, aConstraint.ptB);
 443   return aConstraint.h;
 444 }
 445
 446 Slvs_hConstraint SketchSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 447 {
 448   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 449     // Constraint already used, rewrite it
 450     int aPos = Search(theConstraint.h, myConstraints);
 451     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 452       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 453       myConstraints[aPos] = theConstraint;
 454       if (theConstraint.type == SLVS_C_POINTS_COINCIDENT)
 455         addCoincidentPoints(theConstraint.ptA, theConstraint.ptB);
 456       return theConstraint.h;
 457     }
 458   }
 459
 460   // Constraint is not found, add new one
 461   Slvs_Constraint aConstraint = theConstraint;
 462   aConstraint.h = 0;
 463   return addConstraint(aConstraint);
 464 }
 465
 466 bool SketchSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 467 {
 468   bool aResult = true;
 469   int aPos = Search(theConstraintID, myConstraints);
 470   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 471     Slvs_Constraint aConstraint = myConstraints[aPos];
 472     myConstraints.erase(myConstraints.begin() + aPos);
 473     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 474     myNeedToResolve = true;
 475     myRemovedConstraints.insert(theConstraintID);
 476     // Remove all entities
 477     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
 478         aConstraint.entityA, aConstraint.entityB,
 479         aConstraint.entityC, aConstraint.entityD};
 480     for (int i = 0; i < 6; i++)
 481       if (anEntities[i] != SLVS_E_UNKNOWN)
 482         aResult = removeEntity(anEntities[i]) && aResult;
 483     // remove temporary fixed point, if available
 484     if (myFixed == theConstraintID)
 485       myFixed = SLVS_E_UNKNOWN;
 486     if (myDuplicatedConstraint) {
 487       // Check the duplicated constraints are still available
 488       myDuplicatedConstraint = false;
 489       std::vector<Slvs_Constraint>::const_iterator anIt1 = myConstraints.begin();
 490       std::vector<Slvs_Constraint>::const_iterator anIt2 = myConstraints.begin();
 491       for (; anIt1 != myConstraints.end() && !myDuplicatedConstraint; anIt1++)
 492         for (anIt2 = anIt1+1; anIt2 != myConstraints.end() && !myDuplicatedConstraint; anIt2++) {
 493           if (anIt1->type != anIt2->type)
 494             continue;
 495           if (anIt1->ptA == anIt2->ptA && anIt1->ptB == anIt2->ptB &&
 496               anIt1->entityA == anIt2->entityA && anIt1->entityB == anIt2->entityB &&
 497               anIt1->entityC == anIt2->entityC && anIt1->entityD == anIt2->entityD)
 498             myDuplicatedConstraint = true;
 499         }
 500     }
 501   }
 502   return aResult;
 503 }
 504
 505 const Slvs_Constraint& SketchSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
 506 {
 507   int aPos = Search(theConstraintID, myConstraints);
 508   if (aPos >= 0 && aPos < (int)myConstraints.size())
 509     return myConstraints[aPos];
 510
 511   // Constraint is not found, return empty object
 512   static Slvs_Constraint aDummy;
 513   aDummy.h = 0;
 514   return aDummy;
 515 }
 516
 517 std::list<Slvs_Constraint> SketchSolver_Storage::getConstraintsByType(int theConstraintType) const
 518 {
 519   std::list<Slvs_Constraint> aResult;
 520   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 521   for (; aCIter != myConstraints.end(); aCIter++)
 522     if (aCIter->type == theConstraintType)
 523       aResult.push_back(*aCIter);
 524   return aResult;
 525 }
 526
 527
 528 void SketchSolver_Storage::addConstraintWhereDragged(const Slvs_hConstraint& theConstraintID)
 529 {
 530   if (myFixed != SLVS_E_UNKNOWN)
 531     return; // the point is already fixed
 532   int aPos = Search(theConstraintID, myConstraints);
 533   if (aPos >= 0 && aPos < (int)myConstraints.size())
 534     myFixed = theConstraintID;
 535 }
 536
 537 void SketchSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
 538 {
 539   myTemporaryConstraints.insert(theConstraintID);
 540 }
 541
 542 void SketchSolver_Storage::removeTemporaryConstraints()
 543 {
 544   myTemporaryConstraints.clear();
 545 }
 546
 547 bool SketchSolver_Storage::isTemporary(const Slvs_hConstraint& theConstraintID) const
 548 {
 549   return myTemporaryConstraints.find(theConstraintID) != myTemporaryConstraints.end();
 550 }
 551
 552
 553 void SketchSolver_Storage::getRemoved(
 554     std::set<Slvs_hParam>& theParameters,
 555     std::set<Slvs_hEntity>& theEntities,
 556     std::set<Slvs_hConstraint>& theConstraints)
 557 {
 558   theParameters = myRemovedParameters;
 559   theEntities = myRemovedEntities;
 560   theConstraints = myRemovedConstraints;
 561
 562   myRemovedParameters.clear();
 563   myRemovedEntities.clear();
 564   myRemovedConstraints.clear();
 565 }
 566
 567 void SketchSolver_Storage::initializeSolver(SketchSolver_Solver& theSolver)
 568 {
 569   theSolver.setParameters(myParameters.data(), (int)myParameters.size());
 570   theSolver.setEntities(myEntities.data(), (int)myEntities.size());
 571
 572   // Copy constraints excluding the fixed one
 573   std::vector<Slvs_Constraint> aConstraints = myConstraints;
 574   if (myFixed != SLVS_E_UNKNOWN) {
 575     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
 576     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
 577     for (; anIt != aConstraints.end(); anIt++)
 578       if (anIt->h == myFixed) {
 579         aFixedPoint = anIt->ptA;
 580         aConstraints.erase(anIt);
 581         break;
 582       }
 583     // set dragged parameters
 584     int aPos = Search(aFixedPoint, myEntities);
 585     theSolver.setDraggedParameters(myEntities[aPos].param);
 586   }
 587   theSolver.setConstraints(aConstraints.data(), (int)aConstraints.size());
 588 }
 589
 590 void SketchSolver_Storage::addCoincidentPoints(
 591     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
 592 {
 593   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 594   std::vector< std::set<Slvs_hEntity> >::iterator aFoundIter = myCoincidentPoints.end(); // already found coincidence
 595   bool isFound = false;
 596   for (; aCIter != myCoincidentPoints.end(); aCIter++) {
 597     bool isFirstFound = aCIter->find(thePoint1) != aCIter->end();
 598     bool isSecondFound = aCIter->find(thePoint2) != aCIter->end();
 599     isFound = isFound || isFirstFound || isSecondFound;
 600     if (isFirstFound && isSecondFound)
 601       break; // already coincident
 602     else if (isFirstFound || isSecondFound) {
 603       if (aFoundIter != myCoincidentPoints.end()) {
 604         // merge two sets
 605         aFoundIter->insert(aCIter->begin(), aCIter->end());
 606         myCoincidentPoints.erase(aCIter);
 607         break;
 608       }
 609       aCIter->insert(thePoint1);
 610       aCIter->insert(thePoint2);
 611     }
 612   }
 613   // coincident points not found
 614   if (!isFound) {
 615     std::set<Slvs_hEntity> aNewSet;
 616     aNewSet.insert(thePoint1);
 617     aNewSet.insert(thePoint2);
 618     myCoincidentPoints.push_back(aNewSet);
 619   }
 620 }
 621
 622 void SketchSolver_Storage::removeCoincidentPoint(const Slvs_hEntity& thePoint)
 623 {
 624   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 625   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 626     if (aCIter->find(thePoint) != aCIter->end()) {
 627       aCIter->erase(thePoint);
 628       if (aCIter->size() <= 1)
 629         myCoincidentPoints.erase(aCIter);
 630       break;
 631     }
 632 }
 633
 634 bool SketchSolver_Storage::isCoincident(
 635     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
 636 {
 637   std::vector< std::set<Slvs_hEntity> >::const_iterator aCIter = myCoincidentPoints.begin();
 638   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 639     if (aCIter->find(thePoint1) != aCIter->end() && aCIter->find(thePoint2) != aCIter->end())
 640       return true;
 641   return false;
 642 }
 643
 644
 645
 646
 647 // ========================================================
 648 // =========      Auxiliary functions       ===============
 649 // ========================================================
 650
 651 template<typename T>
 652 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 653 {
 654   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 655   int aVecSize = theEntities.size();
 656   if (theEntities.empty())
 657     return 1;
 658   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 659     aResIndex--;
 660   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
 661     aResIndex++;
 662   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
 663     aResIndex = aVecSize;
 664   return aResIndex;
 665 }
 666
 667 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
 668 {
 669   return fabs(theParam1.val - theParam2.val) > tolerance;
 670 }
 671
 672 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
 673 {
 674   int i = 0;
 675   for (; theEntity1.param[i] != 0 && i < 4; i++)
 676     if (theEntity1.param[i] != theEntity2.param[i])
 677       return true;
 678   i = 0;
 679   for (; theEntity1.point[i] != 0 && i < 4; i++)
 680     if (theEntity1.point[i] != theEntity2.point[i])
 681       return true;
 682   return false;
 683 }
 684
 685 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
 686 {
 687   return theConstraint1.ptA != theConstraint2.ptA ||
 688          theConstraint1.ptB != theConstraint2.ptB ||
 689          theConstraint1.entityA != theConstraint2.entityA ||
 690          theConstraint1.entityB != theConstraint2.entityB ||
 691          theConstraint1.entityC != theConstraint2.entityC ||
 692          theConstraint1.entityD != theConstraint2.entityD ||
 693          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
 694 }