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/Vertical/Horizontal and Length constraints
 351     aList = getConstraintsByType(SLVS_C_PARALLEL);
 352     aList.splice(aList.end(), getConstraintsByType(SLVS_C_PERPENDICULAR));
 353     aList.splice(aList.end(), getConstraintsByType(SLVS_C_VERTICAL));
 354     aList.splice(aList.end(), getConstraintsByType(SLVS_C_HORIZONTAL));
 355     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 356       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 357         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 358         if (isEntityFixed(anOther, false))
 359           break;
 360       }
 361     if (anIt != aList.end()) {
 362       aList = getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 363       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 364         if ((anIt->ptA == myEntities[aPos].point[0] && anIt->ptB == myEntities[aPos].point[1]) ||
 365             (anIt->ptA == myEntities[aPos].point[1] && anIt->ptB == myEntities[aPos].point[0]))
 366           return true;
 367     }
 368     // 3. Another verifiers ...
 369   } else if (myEntities[aPos].type == SLVS_E_CIRCLE) {
 370     if (aNbFixed == 0)
 371       return false;
 372     // Search for Diameter constraint
 373     aList = getConstraintsByType(SLVS_C_DIAMETER);
 374     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 375       if (anIt->entityA == theEntityID)
 376         return true;
 377     if (!theAccurate)
 378       return false;
 379     // Additional check (the circle may be fixed if it is used by different constraints):
 380     // 1. The circle is used in Equal constraint and another entity is fixed
 381     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 382     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 383       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 384         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 385         if (isEntityFixed(anOther, false))
 386           return true;
 387       }
 388     // 2. Another verifiers ...
 389   } else if (myEntities[aPos].type == SLVS_E_ARC_OF_CIRCLE) {
 390     if (aNbFixed > 2)
 391       return true;
 392     else if (aNbFixed <= 1)
 393       return false;
 394     // Search for Diameter constraint
 395     aList = getConstraintsByType(SLVS_C_DIAMETER);
 396     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 397       if (anIt->entityA == theEntityID)
 398         return true;
 399     if (!theAccurate)
 400       return false;
 401     // Additional check (the arc may be fixed if it is used by different constraints):
 402     // 1. The arc is used in Equal constraint and another entity is fixed
 403     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 404     aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 405     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 406       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 407         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 408         if (isEntityFixed(anOther, false))
 409           return true;
 410       }
 411     // 2. Another verifiers ...
 412   }
 413   return false;
 414 }
 415
 416
 417 Slvs_hConstraint SketchSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 418 {
 419   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 420     // Constraint is already used, rewrite it
 421     return updateConstraint(theConstraint);
 422   }
 423
 424   Slvs_Constraint aConstraint = theConstraint;
 425
 426   // Find a constraint with same type uses same arguments to show user overconstraint situation
 427   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 428   for (; aCIt != myConstraints.end(); aCIt++) {
 429     if (aConstraint.type != aCIt->type)
 430       continue;
 431     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 432         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 433         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD)
 434       myDuplicatedConstraint = true;
 435   }
 436
 437   if (aConstraint.h > myConstrMaxID)
 438     myConstrMaxID = aConstraint.h;
 439   else
 440     aConstraint.h = ++myConstrMaxID;
 441   myConstraints.push_back(aConstraint);
 442   myNeedToResolve = true;
 443   if (aConstraint.type == SLVS_C_POINTS_COINCIDENT)
 444     addCoincidentPoints(aConstraint.ptA, aConstraint.ptB);
 445   return aConstraint.h;
 446 }
 447
 448 Slvs_hConstraint SketchSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 449 {
 450   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 451     // Constraint already used, rewrite it
 452     int aPos = Search(theConstraint.h, myConstraints);
 453     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 454       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 455       myConstraints[aPos] = theConstraint;
 456       if (theConstraint.type == SLVS_C_POINTS_COINCIDENT)
 457         addCoincidentPoints(theConstraint.ptA, theConstraint.ptB);
 458       return theConstraint.h;
 459     }
 460   }
 461
 462   // Constraint is not found, add new one
 463   Slvs_Constraint aConstraint = theConstraint;
 464   aConstraint.h = 0;
 465   return addConstraint(aConstraint);
 466 }
 467
 468 bool SketchSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 469 {
 470   bool aResult = true;
 471   int aPos = Search(theConstraintID, myConstraints);
 472   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 473     Slvs_Constraint aConstraint = myConstraints[aPos];
 474     myConstraints.erase(myConstraints.begin() + aPos);
 475     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 476     myNeedToResolve = true;
 477     myRemovedConstraints.insert(theConstraintID);
 478     // Remove all entities
 479     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
 480         aConstraint.entityA, aConstraint.entityB,
 481         aConstraint.entityC, aConstraint.entityD};
 482     for (int i = 0; i < 6; i++)
 483       if (anEntities[i] != SLVS_E_UNKNOWN)
 484         aResult = removeEntity(anEntities[i]) && aResult;
 485     // remove temporary fixed point, if available
 486     if (myFixed == theConstraintID)
 487       myFixed = SLVS_E_UNKNOWN;
 488     if (myDuplicatedConstraint) {
 489       // Check the duplicated constraints are still available
 490       myDuplicatedConstraint = false;
 491       std::vector<Slvs_Constraint>::const_iterator anIt1 = myConstraints.begin();
 492       std::vector<Slvs_Constraint>::const_iterator anIt2 = myConstraints.begin();
 493       for (; anIt1 != myConstraints.end() && !myDuplicatedConstraint; anIt1++)
 494         for (anIt2 = anIt1+1; anIt2 != myConstraints.end() && !myDuplicatedConstraint; anIt2++) {
 495           if (anIt1->type != anIt2->type)
 496             continue;
 497           if (anIt1->ptA == anIt2->ptA && anIt1->ptB == anIt2->ptB &&
 498               anIt1->entityA == anIt2->entityA && anIt1->entityB == anIt2->entityB &&
 499               anIt1->entityC == anIt2->entityC && anIt1->entityD == anIt2->entityD)
 500             myDuplicatedConstraint = true;
 501         }
 502     }
 503   }
 504   return aResult;
 505 }
 506
 507 const Slvs_Constraint& SketchSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
 508 {
 509   int aPos = Search(theConstraintID, myConstraints);
 510   if (aPos >= 0 && aPos < (int)myConstraints.size())
 511     return myConstraints[aPos];
 512
 513   // Constraint is not found, return empty object
 514   static Slvs_Constraint aDummy;
 515   aDummy.h = 0;
 516   return aDummy;
 517 }
 518
 519 std::list<Slvs_Constraint> SketchSolver_Storage::getConstraintsByType(int theConstraintType) const
 520 {
 521   std::list<Slvs_Constraint> aResult;
 522   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 523   for (; aCIter != myConstraints.end(); aCIter++)
 524     if (aCIter->type == theConstraintType)
 525       aResult.push_back(*aCIter);
 526   return aResult;
 527 }
 528
 529
 530 void SketchSolver_Storage::addConstraintWhereDragged(const Slvs_hConstraint& theConstraintID)
 531 {
 532   if (myFixed != SLVS_E_UNKNOWN)
 533     return; // the point is already fixed
 534   int aPos = Search(theConstraintID, myConstraints);
 535   if (aPos >= 0 && aPos < (int)myConstraints.size())
 536     myFixed = theConstraintID;
 537 }
 538
 539 void SketchSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
 540 {
 541   myTemporaryConstraints.insert(theConstraintID);
 542 }
 543
 544 void SketchSolver_Storage::removeTemporaryConstraints()
 545 {
 546   myTemporaryConstraints.clear();
 547 }
 548
 549 int SketchSolver_Storage::deleteTemporaryConstraint()
 550 {
 551   if (myTemporaryConstraints.empty())
 552     return 0;
 553   // Search the point-on-line or a non-rigid constraint
 554   std::set<Slvs_hConstraint>::iterator aCIt = myTemporaryConstraints.begin();
 555   for (; aCIt != myTemporaryConstraints.end(); aCIt++) {
 556     int aPos = Search(*aCIt, myConstraints);
 557     if (aPos >= (int)myConstraints.size() || myConstraints[aPos].type != SLVS_C_WHERE_DRAGGED)
 558       break;
 559     std::vector<Slvs_Constraint>::iterator anIt = myConstraints.begin();
 560     for (; anIt != myConstraints.end(); anIt++)
 561       if (anIt->type == SLVS_C_PT_ON_LINE && anIt->ptA == myConstraints[aPos].ptA)
 562         break;
 563     if (anIt != myConstraints.end())
 564       break;
 565   }
 566   if (aCIt == myTemporaryConstraints.end())
 567     aCIt = myTemporaryConstraints.begin();
 568   bool aNewFixed = (*aCIt == myFixed);
 569   removeConstraint(*aCIt);
 570   myTemporaryConstraints.erase(aCIt);
 571   if (aNewFixed) {
 572     for (aCIt = myTemporaryConstraints.begin(); aCIt != myTemporaryConstraints.end(); aCIt++) {
 573       int aPos = Search(*aCIt, myConstraints);
 574       if (myConstraints[aPos].type == SLVS_C_WHERE_DRAGGED) {
 575         myFixed = *aCIt;
 576         break;
 577       }
 578     }
 579   }
 580   return (int)myTemporaryConstraints.size();
 581 }
 582
 583 bool SketchSolver_Storage::isTemporary(const Slvs_hConstraint& theConstraintID) const
 584 {
 585   return myTemporaryConstraints.find(theConstraintID) != myTemporaryConstraints.end();
 586 }
 587
 588
 589 void SketchSolver_Storage::getRemoved(
 590     std::set<Slvs_hParam>& theParameters,
 591     std::set<Slvs_hEntity>& theEntities,
 592     std::set<Slvs_hConstraint>& theConstraints)
 593 {
 594   theParameters = myRemovedParameters;
 595   theEntities = myRemovedEntities;
 596   theConstraints = myRemovedConstraints;
 597
 598   myRemovedParameters.clear();
 599   myRemovedEntities.clear();
 600   myRemovedConstraints.clear();
 601 }
 602
 603 void SketchSolver_Storage::initializeSolver(SketchSolver_Solver& theSolver)
 604 {
 605   theSolver.setParameters(myParameters.data(), (int)myParameters.size());
 606   theSolver.setEntities(myEntities.data(), (int)myEntities.size());
 607
 608   // Copy constraints excluding the fixed one
 609   std::vector<Slvs_Constraint> aConstraints = myConstraints;
 610   if (myFixed != SLVS_E_UNKNOWN) {
 611     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
 612     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
 613     for (; anIt != aConstraints.end(); anIt++)
 614       if (anIt->h == myFixed) {
 615         aFixedPoint = anIt->ptA;
 616         aConstraints.erase(anIt);
 617         break;
 618       }
 619     // set dragged parameters
 620     int aPos = Search(aFixedPoint, myEntities);
 621     theSolver.setDraggedParameters(myEntities[aPos].param);
 622   }
 623   theSolver.setConstraints(aConstraints.data(), (int)aConstraints.size());
 624 }
 625
 626 void SketchSolver_Storage::addCoincidentPoints(
 627     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
 628 {
 629   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 630   std::vector< std::set<Slvs_hEntity> >::iterator aFoundIter = myCoincidentPoints.end(); // already found coincidence
 631   bool isFound = false;
 632   for (; aCIter != myCoincidentPoints.end(); aCIter++) {
 633     bool isFirstFound = aCIter->find(thePoint1) != aCIter->end();
 634     bool isSecondFound = aCIter->find(thePoint2) != aCIter->end();
 635     isFound = isFound || isFirstFound || isSecondFound;
 636     if (isFirstFound && isSecondFound)
 637       break; // already coincident
 638     else if (isFirstFound || isSecondFound) {
 639       if (aFoundIter != myCoincidentPoints.end()) {
 640         // merge two sets
 641         aFoundIter->insert(aCIter->begin(), aCIter->end());
 642         myCoincidentPoints.erase(aCIter);
 643         break;
 644       }
 645       aCIter->insert(thePoint1);
 646       aCIter->insert(thePoint2);
 647     }
 648   }
 649   // coincident points not found
 650   if (!isFound) {
 651     std::set<Slvs_hEntity> aNewSet;
 652     aNewSet.insert(thePoint1);
 653     aNewSet.insert(thePoint2);
 654     myCoincidentPoints.push_back(aNewSet);
 655   }
 656 }
 657
 658 void SketchSolver_Storage::removeCoincidentPoint(const Slvs_hEntity& thePoint)
 659 {
 660   std::vector< std::set<Slvs_hEntity> >::iterator aCIter = myCoincidentPoints.begin();
 661   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 662     if (aCIter->find(thePoint) != aCIter->end()) {
 663       aCIter->erase(thePoint);
 664       if (aCIter->size() <= 1)
 665         myCoincidentPoints.erase(aCIter);
 666       break;
 667     }
 668 }
 669
 670 bool SketchSolver_Storage::isCoincident(
 671     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
 672 {
 673   std::vector< std::set<Slvs_hEntity> >::const_iterator aCIter = myCoincidentPoints.begin();
 674   for (; aCIter != myCoincidentPoints.end(); aCIter++)
 675     if (aCIter->find(thePoint1) != aCIter->end() && aCIter->find(thePoint2) != aCIter->end())
 676       return true;
 677   // precise checking of coincidence
 678   int aEnt1Pos = Search(thePoint1, myEntities);
 679   int aEnt2Pos = Search(thePoint2, myEntities);
 680   if (aEnt1Pos >= 0 && aEnt1Pos < (int)myEntities.size() &&
 681       aEnt2Pos >= 0 && aEnt2Pos < (int)myEntities.size()) {
 682     double aDist[2];
 683     int aParamPos;
 684     for (int i = 0; i < 2; i++) {
 685       aParamPos = Search(myEntities[aEnt1Pos].param[i], myParameters);
 686       aDist[i] = myParameters[aParamPos].val;
 687       aParamPos = Search(myEntities[aEnt2Pos].param[i], myParameters);
 688       aDist[i] -= myParameters[aParamPos].val;
 689     }
 690     if (aDist[0] * aDist[0] + aDist[1] * aDist[1] < tolerance * tolerance)
 691       return true;
 692   }
 693   return false;
 694 }
 695
 696
 697
 698
 699 // ========================================================
 700 // =========      Auxiliary functions       ===============
 701 // ========================================================
 702
 703 template<typename T>
 704 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 705 {
 706   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 707   int aVecSize = theEntities.size();
 708   if (theEntities.empty())
 709     return 1;
 710   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 711     aResIndex--;
 712   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
 713     aResIndex++;
 714   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
 715     aResIndex = aVecSize;
 716   return aResIndex;
 717 }
 718
 719 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
 720 {
 721   return fabs(theParam1.val - theParam2.val) > tolerance;
 722 }
 723
 724 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
 725 {
 726   int i = 0;
 727   for (; theEntity1.param[i] != 0 && i < 4; i++)
 728     if (theEntity1.param[i] != theEntity2.param[i])
 729       return true;
 730   i = 0;
 731   for (; theEntity1.point[i] != 0 && i < 4; i++)
 732     if (theEntity1.point[i] != theEntity2.point[i])
 733       return true;
 734   return false;
 735 }
 736
 737 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
 738 {
 739   return theConstraint1.ptA != theConstraint2.ptA ||
 740          theConstraint1.ptB != theConstraint2.ptB ||
 741          theConstraint1.entityA != theConstraint2.entityA ||
 742          theConstraint1.entityB != theConstraint2.entityB ||
 743          theConstraint1.entityC != theConstraint2.entityC ||
 744          theConstraint1.entityD != theConstraint2.entityD ||
 745          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
 746 }