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 void SketchSolver_Storage::removeUnusedEntities()
 191 {
 192   std::set<Slvs_hEntity> anUnusedEntities;
 193   std::vector<Slvs_Entity>::const_iterator aEIt = myEntities.begin();
 194   for (; aEIt != myEntities.end(); ++aEIt) {
 195     if (aEIt->h == aEIt->wrkpl) {
 196       // don't remove workplane
 197       anUnusedEntities.erase(aEIt->point[0]);
 198       anUnusedEntities.erase(aEIt->normal);
 199       continue;
 200     }
 201     anUnusedEntities.insert(aEIt->h);
 202   }
 203
 204   std::vector<Slvs_Constraint>::const_iterator aCIt = myConstraints.begin();
 205   for (; aCIt != myConstraints.end(); ++aCIt) {
 206     Slvs_hEntity aSubs[6] = {
 207         aCIt->entityA, aCIt->entityB,
 208         aCIt->entityC, aCIt->entityD,
 209         aCIt->ptA,     aCIt->ptB};
 210     for (int i = 0; i < 6; i++) {
 211       if (aSubs[i] != SLVS_E_UNKNOWN) {
 212         anUnusedEntities.erase(aSubs[i]);
 213         int aPos = Search(aSubs[i], myEntities);
 214         if (aPos >= 0 && aPos < (int)myEntities.size()) {
 215           for (int j = 0; j < 4; j++)
 216             if (myEntities[aPos].point[j] != 0)
 217               anUnusedEntities.erase(myEntities[aPos].point[j]);
 218         }
 219       }
 220     }
 221   }
 222
 223   std::set<Slvs_hEntity>::const_iterator anEntIt = anUnusedEntities.begin();
 224   for (; anEntIt != anUnusedEntities.end(); ++anEntIt) {
 225     int aPos = Search(*anEntIt, myEntities);
 226     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 227       // Remove entity and its parameters
 228       Slvs_Entity anEntity = myEntities[aPos];
 229       myEntities.erase(myEntities.begin() + aPos);
 230       myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 231       if (anEntity.distance != SLVS_E_UNKNOWN)
 232         removeParameter(anEntity.distance);
 233       for (int i = 0; i < 4; i++)
 234         if (anEntity.param[i] != SLVS_E_UNKNOWN)
 235           removeParameter(anEntity.param[i]);
 236       for (int i = 0; i < 4; i++)
 237         if (anEntity.point[i] != SLVS_E_UNKNOWN)
 238           removeEntity(anEntity.point[i]);
 239       myRemovedEntities.insert(*anEntIt);
 240       if (anEntity.type == SLVS_E_POINT_IN_2D || anEntity.type == SLVS_E_POINT_IN_3D)
 241         removeCoincidentPoint(*anEntIt);
 242     }
 243   }
 244
 245   if (!anUnusedEntities.empty())
 246     myNeedToResolve = true;
 247 }
 248
 249 const Slvs_Entity& SketchSolver_Storage::getEntity(const Slvs_hEntity& theEntityID) const
 250 {
 251   int aPos = Search(theEntityID, myEntities);
 252   if (aPos >= 0 && aPos < (int)myEntities.size())
 253     return myEntities[aPos];
 254
 255   // Entity is not found, return empty object
 256   static Slvs_Entity aDummy;
 257   aDummy.h = SLVS_E_UNKNOWN;
 258   return aDummy;
 259 }
 260
 261 Slvs_hEntity SketchSolver_Storage::copyEntity(const Slvs_hEntity& theCopied)
 262 {
 263   int aPos = Search(theCopied, myEntities);
 264   if (aPos < 0 || aPos >= (int)myEntities.size())
 265     return SLVS_E_UNKNOWN;
 266
 267   Slvs_Entity aCopy = myEntities[aPos];
 268   aCopy.h = SLVS_E_UNKNOWN;
 269   int i = 0;
 270   while (aCopy.point[i] != SLVS_E_UNKNOWN) {
 271     aCopy.point[i] = copyEntity(aCopy.point[i]);
 272     i++;
 273   }
 274   if (aCopy.param[0] != SLVS_E_UNKNOWN) {
 275     aPos = Search(aCopy.param[0], myParameters);
 276     i = 0;
 277     while (aCopy.param[i] != SLVS_E_UNKNOWN) {
 278       Slvs_Param aNewParam = myParameters[aPos];
 279       aNewParam.h = SLVS_E_UNKNOWN;
 280       aCopy.param[i] = addParameter(aNewParam);
 281       i++;
 282       aPos++;
 283     }
 284   }
 285   return addEntity(aCopy);
 286 }
 287
 288 void SketchSolver_Storage::copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo)
 289 {
 290   int aPosFrom = Search(theFrom, myEntities);
 291   int aPosTo = Search(theTo, myEntities);
 292   if (aPosFrom < 0 || aPosFrom >= (int)myEntities.size() ||
 293       aPosTo < 0 || aPosTo >= (int)myEntities.size())
 294     return;
 295
 296   Slvs_Entity aEntFrom = myEntities[aPosFrom];
 297   Slvs_Entity aEntTo = myEntities[aPosTo];
 298   int i = 0;
 299   while (aEntFrom.point[i] != SLVS_E_UNKNOWN) {
 300     copyEntity(aEntFrom.point[i], aEntTo.point[i]);
 301     i++;
 302   }
 303   if (aEntFrom.param[0] != SLVS_E_UNKNOWN) {
 304     aPosFrom = Search(aEntFrom.param[0], myParameters);
 305     aPosTo = Search(aEntTo.param[0], myParameters);
 306     i = 0;
 307     while (aEntFrom.param[i] != SLVS_E_UNKNOWN) {
 308       myParameters[aPosTo++].val = myParameters[aPosFrom++].val;
 309       i++;
 310     }
 311   }
 312 }
 313
 314
 315 bool SketchSolver_Storage::isPointFixed(
 316     const Slvs_hEntity& thePointID, Slvs_hConstraint& theFixed, bool theAccurate) const
 317 {
 318   // Search the set of coincident points
 319   std::set<Slvs_hEntity> aCoincident;
 320   aCoincident.insert(thePointID);
 321   std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 322   for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 323     if (aCPIter->find(thePointID) != aCPIter->end()) {
 324       aCoincident = *aCPIter;
 325       break;
 326     }
 327
 328   // Search the Rigid constraint
 329   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 330   for (; aConstrIter != myConstraints.end(); aConstrIter++)
 331     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 332         aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
 333       theFixed = aConstrIter->h;
 334       return true;
 335     }
 336
 337   if (theAccurate) {
 338     // Try to find the fixed entity which uses such point or its coincidence
 339     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 340     for (; anEntIter != myEntities.end(); anEntIter++) {
 341       for (int i = 0; i < 4; i++) {
 342         Slvs_hEntity aPt = anEntIter->point[i];
 343         if (aPt != SLVS_E_UNKNOWN &&
 344             (aPt == thePointID || aCoincident.find(aPt) != aCoincident.end())) {
 345           if (isEntityFixed(anEntIter->h, true))
 346             return true;
 347         }
 348       }
 349     }
 350   }
 351   return SLVS_E_UNKNOWN;
 352 }
 353
 354 bool SketchSolver_Storage::isEntityFixed(const Slvs_hEntity& theEntityID, bool theAccurate) const
 355 {
 356   int aPos = Search(theEntityID, myEntities);
 357   if (aPos < 0 || aPos >= (int)myEntities.size())
 358     return false;
 359
 360   // Firstly, find how many points are under Rigid constraint
 361   int aNbFixed = 0;
 362   for (int i = 0; i < 4; i++) {
 363     Slvs_hEntity aPoint = myEntities[aPos].point[i];
 364     if (aPoint == SLVS_E_UNKNOWN)
 365       continue;
 366
 367     std::set<Slvs_hEntity> aCoincident;
 368     aCoincident.insert(aPoint);
 369     std::vector< std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 370     for (; aCPIter != myCoincidentPoints.end(); aCPIter++)
 371       if (aCPIter->find(aPoint) != aCPIter->end()) {
 372         aCoincident = *aCPIter;
 373         break;
 374       }
 375
 376     // Search the Rigid constraint
 377     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 378     for (; aConstrIter != myConstraints.end(); aConstrIter++)
 379       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 380           aCoincident.find(aConstrIter->ptA) != aCoincident.end())
 381         aNbFixed++;
 382   }
 383
 384   std::list<Slvs_Constraint> aList;
 385   std::list<Slvs_Constraint>::iterator anIt;
 386   Slvs_hConstraint aTempID; // used in isPointFixed() method
 387
 388   if (myEntities[aPos].type == SLVS_E_LINE_SEGMENT) {
 389     if (aNbFixed == 2)
 390       return true;
 391     else if (aNbFixed == 0 || !theAccurate)
 392       return false;
 393     // Additional check (the line may be fixed if it is used by different constraints):
 394     // 1. The line is used in Equal constraint, another entity is fixed and there is a fixed point on line
 395     aList = getConstraintsByType(SLVS_C_PT_ON_LINE);
 396     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 397       if (anIt->entityA == theEntityID && isPointFixed(anIt->ptA, aTempID))
 398         break;
 399     if (anIt != aList.end()) {
 400       aList = getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
 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     }
 409     // 2. The line is used in Parallel/Perpendicular/Vertical/Horizontal and Length constraints
 410     aList = getConstraintsByType(SLVS_C_PARALLEL);
 411     aList.splice(aList.end(), getConstraintsByType(SLVS_C_PERPENDICULAR));
 412     aList.splice(aList.end(), getConstraintsByType(SLVS_C_VERTICAL));
 413     aList.splice(aList.end(), getConstraintsByType(SLVS_C_HORIZONTAL));
 414     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 415       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 416         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 417         if (isEntityFixed(anOther, false))
 418           break;
 419       }
 420     if (anIt != aList.end()) {
 421       aList = getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 422       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 423         if ((anIt->ptA == myEntities[aPos].point[0] && anIt->ptB == myEntities[aPos].point[1]) ||
 424             (anIt->ptA == myEntities[aPos].point[1] && anIt->ptB == myEntities[aPos].point[0]))
 425           return true;
 426     }
 427     // 3. Another verifiers ...
 428   } else if (myEntities[aPos].type == SLVS_E_CIRCLE) {
 429     if (aNbFixed == 0)
 430       return false;
 431     // Search for Diameter constraint
 432     aList = getConstraintsByType(SLVS_C_DIAMETER);
 433     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 434       if (anIt->entityA == theEntityID)
 435         return true;
 436     if (!theAccurate)
 437       return false;
 438     // Additional check (the circle may be fixed if it is used by different constraints):
 439     // 1. The circle is used in Equal constraint and another entity is fixed
 440     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 441     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 442       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 443         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 444         if (isEntityFixed(anOther, false))
 445           return true;
 446       }
 447     // 2. Another verifiers ...
 448   } else if (myEntities[aPos].type == SLVS_E_ARC_OF_CIRCLE) {
 449     if (aNbFixed > 2)
 450       return true;
 451     else if (aNbFixed <= 1)
 452       return false;
 453     // Search for Diameter constraint
 454     aList = getConstraintsByType(SLVS_C_DIAMETER);
 455     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 456       if (anIt->entityA == theEntityID)
 457         return true;
 458     if (!theAccurate)
 459       return false;
 460     // Additional check (the arc may be fixed if it is used by different constraints):
 461     // 1. The arc is used in Equal constraint and another entity is fixed
 462     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 463     aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 464     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 465       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 466         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 467         if (isEntityFixed(anOther, false))
 468           return true;
 469       }
 470     // 2. Another verifiers ...
 471   }
 472   return false;
 473 }
 474
 475
 476 Slvs_hConstraint SketchSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 477 {
 478   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 479     // Constraint is already used, rewrite it
 480     return updateConstraint(theConstraint);
 481   }
 482
 483   Slvs_Constraint aConstraint = theConstraint;
 484
 485   // Find a constraint with same type uses same arguments to show user overconstraint situation
 486   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 487   for (; aCIt != myConstraints.end(); aCIt++) {
 488     if (aConstraint.type != aCIt->type)
 489       continue;
 490     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 491         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 492         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD)
 493       myDuplicatedConstraint = true;
 494   }
 495
 496   if (aConstraint.h > myConstrMaxID)
 497     myConstrMaxID = aConstraint.h;
 498   else
 499     aConstraint.h = ++myConstrMaxID;
 500   myConstraints.push_back(aConstraint);
 501   myNeedToResolve = true;
 502   if (aConstraint.type == SLVS_C_POINTS_COINCIDENT)
 503     addCoincidentPoints(aConstraint.ptA, aConstraint.ptB);
 504   return aConstraint.h;
 505 }
 506
 507 Slvs_hConstraint SketchSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 508 {
 509   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 510     // Constraint already used, rewrite it
 511     int aPos = Search(theConstraint.h, myConstraints);
 512     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 513       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 514       myConstraints[aPos] = theConstraint;
 515       if (theConstraint.type == SLVS_C_POINTS_COINCIDENT)
 516         addCoincidentPoints(theConstraint.ptA, theConstraint.ptB);
 517       return theConstraint.h;
 518     }
 519   }
 520
 521   // Constraint is not found, add new one
 522   Slvs_Constraint aConstraint = theConstraint;
 523   aConstraint.h = 0;
 524   return addConstraint(aConstraint);
 525 }
 526
 527 bool SketchSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 528 {
 529   bool aResult = true;
 530   int aPos = Search(theConstraintID, myConstraints);
 531   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 532     Slvs_Constraint aConstraint = myConstraints[aPos];
 533     myConstraints.erase(myConstraints.begin() + aPos);
 534     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 535     myNeedToResolve = true;
 536     myRemovedConstraints.insert(theConstraintID);
 537     // Remove all entities
 538     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
 539         aConstraint.entityA, aConstraint.entityB,
 540         aConstraint.entityC, aConstraint.entityD};
 541     for (int i = 0; i < 6; i++)
 542       if (anEntities[i] != SLVS_E_UNKNOWN)
 543         aResult = removeEntity(anEntities[i]) && aResult;
 544     // remove temporary fixed point, if available
 545     if (myFixed == theConstraintID)
 546       myFixed = SLVS_E_UNKNOWN;
 547     if (myDuplicatedConstraint) {
 548       // Check the duplicated constraints are still available
 549       myDuplicatedConstraint = false;
 550       std::vector<Slvs_Constraint>::const_iterator anIt1 = myConstraints.begin();
 551       std::vector<Slvs_Constraint>::const_iterator anIt2 = myConstraints.begin();
 552       for (; anIt1 != myConstraints.end() && !myDuplicatedConstraint; anIt1++)
 553         for (anIt2 = anIt1+1; anIt2 != myConstraints.end() && !myDuplicatedConstraint; anIt2++) {
 554           if (anIt1->type != anIt2->type)
 555             continue;
 556           if (anIt1->ptA == anIt2->ptA && anIt1->ptB == anIt2->ptB &&
 557               anIt1->entityA == anIt2->entityA && anIt1->entityB == anIt2->entityB &&
 558               anIt1->entityC == anIt2->entityC && anIt1->entityD == anIt2->entityD)
 559             myDuplicatedConstraint = true;
 560         }
 561     }
 562   }
 563   return aResult;
 564 }
 565
 566 const Slvs_Constraint& SketchSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
 567 {
 568   int aPos = Search(theConstraintID, myConstraints);
 569   if (aPos >= 0 && aPos < (int)myConstraints.size())
 570     return myConstraints[aPos];
 571
 572   // Constraint is not found, return empty object
 573   static Slvs_Constraint aDummy;
 574   aDummy.h = 0;
 575   return aDummy;
 576 }
 577
 578 std::list<Slvs_Constraint> SketchSolver_Storage::getConstraintsByType(int theConstraintType) const
 579 {
 580   std::list<Slvs_Constraint> aResult;
 581   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 582   for (; aCIter != myConstraints.end(); aCIter++)
 583     if (aCIter->type == theConstraintType)
 584       aResult.push_back(*aCIter);
 585   return aResult;
 586 }
 587
 588
 589 void SketchSolver_Storage::addConstraintWhereDragged(const Slvs_hConstraint& theConstraintID)
 590 {
 591   if (myFixed != SLVS_E_UNKNOWN)
 592     return; // the point is already fixed
 593   int aPos = Search(theConstraintID, myConstraints);
 594   if (aPos >= 0 && aPos < (int)myConstraints.size())
 595     myFixed = theConstraintID;
 596 }
 597
 598 void SketchSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
 599 {
 600   myTemporaryConstraints.insert(theConstraintID);
 601 }
 602
 603 void SketchSolver_Storage::removeTemporaryConstraints()
 604 {
 605   myTemporaryConstraints.clear();
 606 }
 607
 608 int SketchSolver_Storage::deleteTemporaryConstraint()
 609 {
 610   if (myTemporaryConstraints.empty())
 611     return 0;
 612   // Search the point-on-line or a non-rigid constraint
 613   std::set<Slvs_hConstraint>::iterator aCIt = myTemporaryConstraints.begin();
 614   for (; aCIt != myTemporaryConstraints.end(); aCIt++) {
 615     int aPos = Search(*aCIt, myConstraints);
 616     if (aPos >= (int)myConstraints.size() || myConstraints[aPos].type != SLVS_C_WHERE_DRAGGED)
 617       break;
 618     std::vector<Slvs_Constraint>::iterator anIt = myConstraints.begin();
 619     for (; anIt != myConstraints.end(); anIt++)
 620       if (anIt->type == SLVS_C_PT_ON_LINE && anIt->ptA == myConstraints[aPos].ptA)
 621         break;
 622     if (anIt != myConstraints.end())
 623       break;
 624   }
 625   if (aCIt == myTemporaryConstraints.end())
 626     aCIt = myTemporaryConstraints.begin();
 627   bool aNewFixed = (*aCIt == myFixed);
 628   removeConstraint(*aCIt);
 629   myTemporaryConstraints.erase(aCIt);
 630   if (aNewFixed) {
 631     for (aCIt = myTemporaryConstraints.begin(); aCIt != myTemporaryConstraints.end(); aCIt++) {
 632       int aPos = Search(*aCIt, myConstraints);
 633       if (myConstraints[aPos].type == SLVS_C_WHERE_DRAGGED) {
 634         myFixed = *aCIt;
 635         break;
 636       }
 637     }
 638   }
 639   return (int)myTemporaryConstraints.size();
 640 }
 641
 642 bool SketchSolver_Storage::isTemporary(const Slvs_hConstraint& theConstraintID) const
 643 {
 644   return myTemporaryConstraints.find(theConstraintID) != myTemporaryConstraints.end();
 645 }
 646
 647
 648 void SketchSolver_Storage::getRemoved(
 649     std::set<Slvs_hParam>& theParameters,
 650     std::set<Slvs_hEntity>& theEntities,
 651     std::set<Slvs_hConstraint>& theConstraints)
 652 {
 653   theParameters = myRemovedParameters;
 654   theEntities = myRemovedEntities;
 655   theConstraints = myRemovedConstraints;
 656
 657   myRemovedParameters.clear();
 658   myRemovedEntities.clear();
 659   myRemovedConstraints.clear();
 660 }
 661
 662 void SketchSolver_Storage::initializeSolver(SketchSolver_Solver& theSolver)
 663 {
 664   theSolver.setParameters(myParameters.data(), (int)myParameters.size());
 665   theSolver.setEntities(myEntities.data(), (int)myEntities.size());
 666
 667   // Copy constraints excluding the fixed one
 668   std::vector<Slvs_Constraint> aConstraints = myConstraints;
 669   if (myFixed != SLVS_E_UNKNOWN) {
 670     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
 671     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
 672     for (; anIt != aConstraints.end(); anIt++)
 673       if (anIt->h == myFixed) {
 674         aFixedPoint = anIt->ptA;
 675         aConstraints.erase(anIt);
 676         break;
 677       }
 678     // set dragged parameters
 679     int aPos = Search(aFixedPoint, myEntities);
 680     theSolver.setDraggedParameters(myEntities[aPos].param);
 681   }
 682   theSolver.setConstraints(aConstraints.data(), (int)aConstraints.size());
 683 }
 684
 685 void SketchSolver_Storage::addCoincidentPoints(
 686     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
 687 {
 688   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 689   std::vector< std::set<Slvs_hEntity> >::iterator aFoundIter = myCoincidentPoints.end(); // already found coincidence
 690   bool isFound = false;
 691   for (; aCIter != myCoincidentPoints.end(); aCIter++) {
 692     bool isFirstFound = aCIter->find(thePoint1) != aCIter->end();
 693     bool isSecondFound = aCIter->find(thePoint2) != aCIter->end();
 694     isFound = isFound || isFirstFound || isSecondFound;
 695     if (isFirstFound && isSecondFound)
 696       break; // already coincident
 697     else if (isFirstFound || isSecondFound) {
 698       if (aFoundIter != myCoincidentPoints.end()) {
 699         // merge two sets
 700         aFoundIter->insert(aCIter->begin(), aCIter->end());
 701         myCoincidentPoints.erase(aCIter);
 702         break;
 703       }
 704       aCIter->insert(thePoint1);
 705       aCIter->insert(thePoint2);
 706     }
 707   }
 708   // coincident points not found
 709   if (!isFound) {
 710     std::set<Slvs_hEntity> aNewSet;
 711     aNewSet.insert(thePoint1);
 712     aNewSet.insert(thePoint2);
 713     myCoincidentPoints.push_back(aNewSet);
 714   }
 715 }
 716
 717 void SketchSolver_Storage::removeCoincidentPoint(const Slvs_hEntity& thePoint)
 718 {
 719   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 720   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 721     if (aCIter->find(thePoint) != aCIter->end()) {
 722       aCIter->erase(thePoint);
 723       if (aCIter->size() <= 1)
 724         myCoincidentPoints.erase(aCIter);
 725       break;
 726     }
 727 }
 728
 729 bool SketchSolver_Storage::isCoincident(
 730     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
 731 {
 732   std::vector< std::set<Slvs_hEntity> >::const_iterator aCIter = myCoincidentPoints.begin();
 733   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 734     if (aCIter->find(thePoint1) != aCIter->end() && aCIter->find(thePoint2) != aCIter->end())
 735       return true;
 736   // precise checking of coincidence
 737   int aEnt1Pos = Search(thePoint1, myEntities);
 738   int aEnt2Pos = Search(thePoint2, myEntities);
 739   if (aEnt1Pos >= 0 && aEnt1Pos < (int)myEntities.size() &&
 740       aEnt2Pos >= 0 && aEnt2Pos < (int)myEntities.size()) {
 741     double aDist[2];
 742     int aParamPos;
 743     for (int i = 0; i < 2; i++) {
 744       aParamPos = Search(myEntities[aEnt1Pos].param[i], myParameters);
 745       aDist[i] = myParameters[aParamPos].val;
 746       aParamPos = Search(myEntities[aEnt2Pos].param[i], myParameters);
 747       aDist[i] -= myParameters[aParamPos].val;
 748     }
 749     if (aDist[0] * aDist[0] + aDist[1] * aDist[1] < tolerance * tolerance)
 750       return true;
 751   }
 752   return false;
 753 }
 754
 755
 756
 757
 758 // ========================================================
 759 // =========      Auxiliary functions       ===============
 760 // ========================================================
 761
 762 template<typename T>
 763 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 764 {
 765   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 766   int aVecSize = theEntities.size();
 767   if (theEntities.empty())
 768     return 1;
 769   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 770     aResIndex--;
 771   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
 772     aResIndex++;
 773   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
 774     aResIndex = aVecSize;
 775   return aResIndex;
 776 }
 777
 778 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
 779 {
 780   return fabs(theParam1.val - theParam2.val) > tolerance;
 781 }
 782
 783 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
 784 {
 785   int i = 0;
 786   for (; theEntity1.param[i] != 0 && i < 4; i++)
 787     if (theEntity1.param[i] != theEntity2.param[i])
 788       return true;
 789   i = 0;
 790   for (; theEntity1.point[i] != 0 && i < 4; i++)
 791     if (theEntity1.point[i] != theEntity2.point[i])
 792       return true;
 793   return false;
 794 }
 795
 796 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
 797 {
 798   return theConstraint1.ptA != theConstraint2.ptA ||
 799          theConstraint1.ptB != theConstraint2.ptB ||
 800          theConstraint1.entityA != theConstraint2.entityA ||
 801          theConstraint1.entityB != theConstraint2.entityB ||
 802          theConstraint1.entityC != theConstraint2.entityC ||
 803          theConstraint1.entityD != theConstraint2.entityD ||
 804          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
 805 }