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 and its attributes is not used elsewhere
 146     std::set<Slvs_hEntity> anEntAndSubs;
 147     anEntAndSubs.insert(theEntityID);
 148     for (int i = 0; i < 4; i++)
 149       if (myEntities[aPos].point[i] != SLVS_E_UNKNOWN)
 150         anEntAndSubs.insert(myEntities[aPos].point[i]);
 151
 152     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 153     for (; anEntIter != myEntities.end(); anEntIter++) {
 154       for (int i = 0; i < 4; i++)
 155         if (anEntAndSubs.find(anEntIter->point[i]) != anEntAndSubs.end())
 156           return false;
 157       if (anEntAndSubs.find(anEntIter->distance) != anEntAndSubs.end())
 158         return false;
 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 = SLVS_E_UNKNOWN;
 198   return aDummy;
 199 }
 200
 201 Slvs_hEntity SketchSolver_Storage::copyEntity(const Slvs_hEntity& theCopied)
 202 {
 203   int aPos = Search(theCopied, myEntities);
 204   if (aPos < 0 || aPos >= (int)myEntities.size())
 205     return SLVS_E_UNKNOWN;
 206
 207   Slvs_Entity aCopy = myEntities[aPos];
 208   aCopy.h = SLVS_E_UNKNOWN;
 209   int i = 0;
 210   while (aCopy.point[i] != SLVS_E_UNKNOWN) {
 211     aCopy.point[i] = copyEntity(aCopy.point[i]);
 212     i++;
 213   }
 214   if (aCopy.param[0] != SLVS_E_UNKNOWN) {
 215     aPos = Search(aCopy.param[0], myParameters);
 216     i = 0;
 217     while (aCopy.param[i] != SLVS_E_UNKNOWN) {
 218       Slvs_Param aNewParam = myParameters[aPos];
 219       aNewParam.h = SLVS_E_UNKNOWN;
 220       aCopy.param[i] = addParameter(aNewParam);
 221       i++;
 222       aPos++;
 223     }
 224   }
 225   return addEntity(aCopy);
 226 }
 227
 228 void SketchSolver_Storage::copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo)
 229 {
 230   int aPosFrom = Search(theFrom, myEntities);
 231   int aPosTo = Search(theTo, myEntities);
 232   if (aPosFrom < 0 || aPosFrom >= (int)myEntities.size() ||
 233       aPosTo < 0 || aPosTo >= (int)myEntities.size())
 234     return;
 235
 236   Slvs_Entity aEntFrom = myEntities[aPosFrom];
 237   Slvs_Entity aEntTo = myEntities[aPosTo];
 238   int i = 0;
 239   while (aEntFrom.point[i] != SLVS_E_UNKNOWN) {
 240     copyEntity(aEntFrom.point[i], aEntTo.point[i]);
 241     i++;
 242   }
 243   if (aEntFrom.param[0] != SLVS_E_UNKNOWN) {
 244     aPosFrom = Search(aEntFrom.param[0], myParameters);
 245     aPosTo = Search(aEntTo.param[0], myParameters);
 246     i = 0;
 247     while (aEntFrom.param[i] != SLVS_E_UNKNOWN) {
 248       myParameters[aPosTo++].val = myParameters[aPosFrom++].val;
 249       i++;
 250     }
 251   }
 252 }
 253
 254
 255 bool SketchSolver_Storage::isPointFixed(
 256     const Slvs_hEntity& thePointID, Slvs_hConstraint& theFixed, bool theAccurate) const
 257 {
 258   // Search the set of coincident points
 259   std::set<Slvs_hEntity> aCoincident;
 260   aCoincident.insert(thePointID);
 261   std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 262   for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 263     if (aCPIter->find(thePointID) != aCPIter->end()) {
 264       aCoincident = *aCPIter;
 265       break;
 266     }
 267
 268   // Search the Rigid constraint
 269   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 270   for (; aConstrIter != myConstraints.end(); aConstrIter++)
 271     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 272         aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
 273       theFixed = aConstrIter->h;
 274       return true;
 275     }
 276
 277   if (theAccurate) {
 278     // Try to find the fixed entity which uses such point or its coincidence
 279     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 280     for (; anEntIter != myEntities.end(); anEntIter++) {
 281       for (int i = 0; i < 4; i++) {
 282         Slvs_hEntity aPt = anEntIter->point[i];
 283         if (aPt != SLVS_E_UNKNOWN &&
 284             (aPt == thePointID || aCoincident.find(aPt) != aCoincident.end())) {
 285           if (isEntityFixed(anEntIter->h, true))
 286             return true;
 287         }
 288       }
 289     }
 290   }
 291   return SLVS_E_UNKNOWN;
 292 }
 293
 294 bool SketchSolver_Storage::isEntityFixed(const Slvs_hEntity& theEntityID, bool theAccurate) const
 295 {
 296   int aPos = Search(theEntityID, myEntities);
 297   if (aPos < 0 || aPos >= (int)myEntities.size())
 298     return false;
 299
 300   // Firstly, find how many points are under Rigid constraint
 301   int aNbFixed = 0;
 302   for (int i = 0; i < 4; i++) {
 303     Slvs_hEntity aPoint = myEntities[aPos].point[i];
 304     if (aPoint == SLVS_E_UNKNOWN)
 305       continue;
 306
 307     std::set<Slvs_hEntity> aCoincident;
 308     aCoincident.insert(aPoint);
 309     std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 310     for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 311       if (aCPIter->find(aPoint) != aCPIter->end()) {
 312         aCoincident = *aCPIter;
 313         break;
 314       }
 315
 316     // Search the Rigid constraint
 317     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 318     for (; aConstrIter != myConstraints.end(); aConstrIter++)
 319       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 320           aCoincident.find(aConstrIter->ptA) != aCoincident.end())
 321         aNbFixed++;
 322   }
 323
 324   std::list<Slvs_Constraint> aList;
 325   std::list<Slvs_Constraint>::iterator anIt;
 326   Slvs_hConstraint aTempID; // used in isPointFixed() method
 327
 328   if (myEntities[aPos].type == SLVS_E_LINE_SEGMENT) {
 329     if (aNbFixed == 2)
 330       return true;
 331     else if (aNbFixed == 0 || !theAccurate)
 332       return false;
 333     // Additional check (the line may be fixed if it is used by different constraints):
 334     // 1. The line is used in Equal constraint, another entity is fixed and there is a fixed point on line
 335     aList = getConstraintsByType(SLVS_C_PT_ON_LINE);
 336     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 337       if (anIt->entityA == theEntityID && isPointFixed(anIt->ptA, aTempID))
 338         break;
 339     if (anIt != aList.end()) {
 340       aList = getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
 341       aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 342       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 343         if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 344           Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 345           if (isEntityFixed(anOther, false))
 346             return true;
 347         }
 348     }
 349     // 2. The line is used in Parallel/Perpendicular and Length constraints
 350     aList = getConstraintsByType(SLVS_C_PARALLEL);
 351     aList.splice(aList.end(), getConstraintsByType(SLVS_C_PERPENDICULAR));
 352     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 353       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 354         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 355         if (isEntityFixed(anOther, false))
 356           break;
 357       }
 358     if (anIt != aList.end()) {
 359       aList = getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 360       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 361         if ((anIt->ptA == myEntities[aPos].point[0] && anIt->ptB == myEntities[aPos].point[1]) ||
 362             (anIt->ptA == myEntities[aPos].point[1] && anIt->ptB == myEntities[aPos].point[0]))
 363           return true;
 364     }
 365     // 3. Another verifiers ...
 366   } else if (myEntities[aPos].type == SLVS_E_CIRCLE) {
 367     if (aNbFixed == 0)
 368       return false;
 369     // Search for Diameter constraint
 370     aList = getConstraintsByType(SLVS_C_DIAMETER);
 371     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 372       if (anIt->entityA == theEntityID)
 373         return true;
 374     if (!theAccurate)
 375       return false;
 376     // Additional check (the circle may be fixed if it is used by different constraints):
 377     // 1. The circle is used in Equal constraint and another entity is fixed
 378     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 379     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 380       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 381         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 382         if (isEntityFixed(anOther, false))
 383           return true;
 384       }
 385     // 2. Another verifiers ...
 386   } else if (myEntities[aPos].type == SLVS_E_ARC_OF_CIRCLE) {
 387     if (aNbFixed > 2)
 388       return true;
 389     else if (aNbFixed <= 1)
 390       return false;
 391     // Search for Diameter constraint
 392     aList = getConstraintsByType(SLVS_C_DIAMETER);
 393     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 394       if (anIt->entityA == theEntityID)
 395         return true;
 396     if (!theAccurate)
 397       return false;
 398     // Additional check (the arc may be fixed if it is used by different constraints):
 399     // 1. The arc is used in Equal constraint and another entity is fixed
 400     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 401     aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 402     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 403       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 404         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 405         if (isEntityFixed(anOther, false))
 406           return true;
 407       }
 408     // 2. Another verifiers ...
 409   }
 410   return false;
 411 }
 412
 413
 414 Slvs_hConstraint SketchSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 415 {
 416   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 417     // Constraint is already used, rewrite it
 418     return updateConstraint(theConstraint);
 419   }
 420
 421   Slvs_Constraint aConstraint = theConstraint;
 422
 423   // Find a constraint with same type uses same arguments
 424   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 425   for (; aCIt != myConstraints.end(); aCIt++) {
 426     if (aConstraint.type != aCIt->type)
 427       continue;
 428     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 429         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 430         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD) {
 431       aConstraint.h = aCIt->h;
 432       return updateConstraint(aConstraint);
 433     }
 434   }
 435
 436   if (aConstraint.h > myConstrMaxID)
 437     myConstrMaxID = aConstraint.h;
 438   else
 439     aConstraint.h = ++myConstrMaxID;
 440   myConstraints.push_back(aConstraint);
 441   myNeedToResolve = true;
 442   if (aConstraint.type == SLVS_C_POINTS_COINCIDENT)
 443     addCoincidentPoints(aConstraint.ptA, aConstraint.ptB);
 444   return aConstraint.h;
 445 }
 446
 447 Slvs_hConstraint SketchSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 448 {
 449   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 450     // Constraint already used, rewrite it
 451     int aPos = Search(theConstraint.h, myConstraints);
 452     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 453       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 454       myConstraints[aPos] = theConstraint;
 455       if (theConstraint.type == SLVS_C_POINTS_COINCIDENT)
 456         addCoincidentPoints(theConstraint.ptA, theConstraint.ptB);
 457       return theConstraint.h;
 458     }
 459   }
 460
 461   // Constraint is not found, add new one
 462   Slvs_Constraint aConstraint = theConstraint;
 463   aConstraint.h = 0;
 464   return addConstraint(aConstraint);
 465 }
 466
 467 bool SketchSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 468 {
 469   bool aResult = true;
 470   int aPos = Search(theConstraintID, myConstraints);
 471   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 472     Slvs_Constraint aConstraint = myConstraints[aPos];
 473     myConstraints.erase(myConstraints.begin() + aPos);
 474     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 475     myNeedToResolve = true;
 476     myRemovedConstraints.insert(theConstraintID);
 477     // Remove all entities
 478     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
 479         aConstraint.entityA, aConstraint.entityB,
 480         aConstraint.entityC, aConstraint.entityD};
 481     for (int i = 0; i < 6; i++)
 482       if (anEntities[i] != SLVS_E_UNKNOWN)
 483         aResult = removeEntity(anEntities[i]) && aResult;
 484     // remove temporary fixed point, if available
 485     if (myFixed == theConstraintID)
 486       myFixed = SLVS_E_UNKNOWN;
 487   }
 488   return aResult;
 489 }
 490
 491 const Slvs_Constraint& SketchSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
 492 {
 493   int aPos = Search(theConstraintID, myConstraints);
 494   if (aPos >= 0 && aPos < (int)myConstraints.size())
 495     return myConstraints[aPos];
 496
 497   // Constraint is not found, return empty object
 498   static Slvs_Constraint aDummy;
 499   aDummy.h = 0;
 500   return aDummy;
 501 }
 502
 503 std::list<Slvs_Constraint> SketchSolver_Storage::getConstraintsByType(int theConstraintType) const
 504 {
 505   std::list<Slvs_Constraint> aResult;
 506   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 507   for (; aCIter != myConstraints.end(); aCIter++)
 508     if (aCIter->type == theConstraintType)
 509       aResult.push_back(*aCIter);
 510   return aResult;
 511 }
 512
 513
 514 void SketchSolver_Storage::addConstraintWhereDragged(const Slvs_hConstraint& theConstraintID)
 515 {
 516   if (myFixed != SLVS_E_UNKNOWN)
 517     return; // the point is already fixed
 518   int aPos = Search(theConstraintID, myConstraints);
 519   if (aPos >= 0 && aPos < (int)myConstraints.size())
 520     myFixed = theConstraintID;
 521 }
 522
 523 void SketchSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
 524 {
 525   myTemporaryConstraints.insert(theConstraintID);
 526 }
 527
 528 void SketchSolver_Storage::removeTemporaryConstraints()
 529 {
 530   myTemporaryConstraints.clear();
 531 }
 532
 533 bool SketchSolver_Storage::isTemporary(const Slvs_hConstraint& theConstraintID) const
 534 {
 535   return myTemporaryConstraints.find(theConstraintID) != myTemporaryConstraints.end();
 536 }
 537
 538
 539 void SketchSolver_Storage::getRemoved(
 540     std::set<Slvs_hParam>& theParameters,
 541     std::set<Slvs_hEntity>& theEntities,
 542     std::set<Slvs_hConstraint>& theConstraints)
 543 {
 544   theParameters = myRemovedParameters;
 545   theEntities = myRemovedEntities;
 546   theConstraints = myRemovedConstraints;
 547
 548   myRemovedParameters.clear();
 549   myRemovedEntities.clear();
 550   myRemovedConstraints.clear();
 551 }
 552
 553 void SketchSolver_Storage::initializeSolver(SketchSolver_Solver& theSolver)
 554 {
 555   theSolver.setParameters(myParameters.data(), (int)myParameters.size());
 556   theSolver.setEntities(myEntities.data(), (int)myEntities.size());
 557
 558   // Copy constraints excluding the fixed one
 559   std::vector<Slvs_Constraint> aConstraints = myConstraints;
 560   if (myFixed != SLVS_E_UNKNOWN) {
 561     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
 562     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
 563     for (; anIt != aConstraints.end(); anIt++)
 564       if (anIt->h == myFixed) {
 565         aFixedPoint = anIt->ptA;
 566         aConstraints.erase(anIt);
 567         break;
 568       }
 569     // set dragged parameters
 570     int aPos = Search(aFixedPoint, myEntities);
 571     theSolver.setDraggedParameters(myEntities[aPos].param);
 572   }
 573   theSolver.setConstraints(aConstraints.data(), (int)aConstraints.size());
 574 }
 575
 576 void SketchSolver_Storage::addCoincidentPoints(
 577     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
 578 {
 579   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 580   std::vector< std::set<Slvs_hEntity> >::iterator aFoundIter = myCoincidentPoints.end(); // already found coincidence
 581   bool isFound = false;
 582   for (; aCIter != myCoincidentPoints.end(); aCIter++) {
 583     bool isFirstFound = aCIter->find(thePoint1) != aCIter->end();
 584     bool isSecondFound = aCIter->find(thePoint2) != aCIter->end();
 585     isFound = isFound || isFirstFound || isSecondFound;
 586     if (isFirstFound && isSecondFound)
 587       break; // already coincident
 588     else if (isFirstFound || isSecondFound) {
 589       if (aFoundIter != myCoincidentPoints.end()) {
 590         // merge two sets
 591         aFoundIter->insert(aCIter->begin(), aCIter->end());
 592         myCoincidentPoints.erase(aCIter);
 593         break;
 594       }
 595       aCIter->insert(thePoint1);
 596       aCIter->insert(thePoint2);
 597     }
 598   }
 599   // coincident points not found
 600   if (!isFound) {
 601     std::set<Slvs_hEntity> aNewSet;
 602     aNewSet.insert(thePoint1);
 603     aNewSet.insert(thePoint2);
 604     myCoincidentPoints.push_back(aNewSet);
 605   }
 606 }
 607
 608 void SketchSolver_Storage::removeCoincidentPoint(const Slvs_hEntity& thePoint)
 609 {
 610   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 611   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 612     if (aCIter->find(thePoint) != aCIter->end()) {
 613       aCIter->erase(thePoint);
 614       if (aCIter->size() <= 1)
 615         myCoincidentPoints.erase(aCIter);
 616       break;
 617     }
 618 }
 619
 620 bool SketchSolver_Storage::isCoincident(
 621     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
 622 {
 623   std::vector< std::set<Slvs_hEntity> >::const_iterator aCIter = myCoincidentPoints.begin();
 624   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 625     if (aCIter->find(thePoint1) != aCIter->end() && aCIter->find(thePoint2) != aCIter->end())
 626       return true;
 627   return false;
 628 }
 629
 630
 631
 632
 633 // ========================================================
 634 // =========      Auxiliary functions       ===============
 635 // ========================================================
 636
 637 template<typename T>
 638 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 639 {
 640   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 641   int aVecSize = theEntities.size();
 642   if (theEntities.empty())
 643     return 1;
 644   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 645     aResIndex--;
 646   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
 647     aResIndex++;
 648   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
 649     aResIndex = aVecSize;
 650   return aResIndex;
 651 }
 652
 653 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
 654 {
 655   return fabs(theParam1.val - theParam2.val) > tolerance;
 656 }
 657
 658 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
 659 {
 660   int i = 0;
 661   for (; theEntity1.param[i] != 0 && i < 4; i++)
 662     if (theEntity1.param[i] != theEntity2.param[i])
 663       return true;
 664   i = 0;
 665   for (; theEntity1.point[i] != 0 && i < 4; i++)
 666     if (theEntity1.point[i] != theEntity2.point[i])
 667       return true;
 668   return false;
 669 }
 670
 671 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
 672 {
 673   return theConstraint1.ptA != theConstraint2.ptA ||
 674          theConstraint1.ptB != theConstraint2.ptB ||
 675          theConstraint1.entityA != theConstraint2.entityA ||
 676          theConstraint1.entityB != theConstraint2.entityB ||
 677          theConstraint1.entityC != theConstraint2.entityC ||
 678          theConstraint1.entityD != theConstraint2.entityD ||
 679          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
 680 }