src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Storage.cpp

   1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
   2
   3 // File:    SolveSpaceSolver_Storage.cpp
   4 // Created: 18 Mar 2015
   5 // Author:  Artem ZHIDKOV
   6
   7 #include <SolveSpaceSolver_Storage.h>
   8 #include <SolveSpaceSolver_ConstraintWrapper.h>
   9 #include <SolveSpaceSolver_EntityWrapper.h>
  10 #include <SolveSpaceSolver_ParameterWrapper.h>
  11 #include <SolveSpaceSolver_Builder.h>
  12
  13 #include <GeomAPI_Dir2d.h>
  14 #include <GeomAPI_Pnt2d.h>
  15 #include <GeomAPI_XY.h>
  16 #include <math.h>
  17 #include <assert.h>
  18
  19 #include <GeomDataAPI_Point.h>
  20 #include <GeomDataAPI_Point2D.h>
  21 #include <ModelAPI_AttributeDouble.h>
  22 #include <SketchPlugin_Arc.h>
  23
  24 /** \brief Search the entity/parameter with specified ID in the list of elements
  25  *  \param[in] theEntityID unique ID of the element
  26  *  \param[in] theEntities list of elements
  27  *  \return position of the found element or -1 if the element is not found
  28  */
  29 template<typename T>
  30 static int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities);
  31
  32 /// \brief Compare two parameters to be different
  33 static bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2);
  34 /// \brief Compare two entities to be different
  35 static bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2);
  36 /// \brief Compare two constraints to be different
  37 static bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2);
  38
  39
  40 SolveSpaceSolver_Storage::SolveSpaceSolver_Storage(const GroupID& theGroup)
  41   : SketchSolver_Storage(theGroup),
  42     myWorkplaneID(SLVS_E_UNKNOWN),
  43     myParamMaxID(SLVS_E_UNKNOWN),
  44     myEntityMaxID(SLVS_E_UNKNOWN),
  45     myConstrMaxID(SLVS_C_UNKNOWN),
  46     myFixed(SLVS_E_UNKNOWN),
  47     myDuplicatedConstraint(false)
  48 {
  49 }
  50
  51 bool SolveSpaceSolver_Storage::update(ConstraintWrapperPtr& theConstraint)
  52 {
  53   bool isUpdated = false;
  54   std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aConstraint =
  55       std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(theConstraint);
  56   Slvs_Constraint aSlvsConstr = getConstraint((Slvs_hConstraint)aConstraint->id());
  57   if (aSlvsConstr.h == SLVS_C_UNKNOWN)
  58     aSlvsConstr = aConstraint->constraint();
  59
  60   // update value of constraint if exist
  61   if (fabs(aSlvsConstr.valA - theConstraint->value()) > tolerance) {
  62     aSlvsConstr.valA = theConstraint->value();
  63     isUpdated = true;
  64   }
  65
  66   // update constrained entities
  67   Slvs_hEntity* aPnts[2] = {&aSlvsConstr.ptA, &aSlvsConstr.ptB};
  68   Slvs_hEntity* anEnts[4] = {&aSlvsConstr.entityA, &aSlvsConstr.entityB,
  69                              &aSlvsConstr.entityC, &aSlvsConstr.entityD};
  70   int aPtInd = 0;
  71   int aEntInd = 0;
  72
  73   std::list<EntityWrapperPtr> anEntities = theConstraint->entities();
  74   std::list<EntityWrapperPtr>::iterator anIt = anEntities.begin();
  75   for (; anIt != anEntities.end(); ++anIt) {
  76     isUpdated = update(*anIt) || isUpdated;
  77     // do not update constrained entities for Multi constraints
  78     if (aSlvsConstr.type == SLVS_C_MULTI_ROTATION || aSlvsConstr.type != SLVS_C_MULTI_TRANSLATION)
  79       continue;
  80
  81     Slvs_hEntity anID = (Slvs_hEntity)(*anIt)->id();
  82     if ((*anIt)->type() == ENTITY_POINT) {
  83       if (*(aPnts[aPtInd]) != anID) {
  84         *(aPnts[aPtInd]) = anID;
  85         isUpdated = true;
  86       }
  87       ++aPtInd;
  88     } else {
  89       if (*(anEnts[aEntInd]) != anID) {
  90         *(anEnts[aEntInd]) = anID;
  91         isUpdated = true;
  92       }
  93       ++aEntInd;
  94     }
  95   }
  96
  97   // update constraint itself (do not update constraints Multi)
  98   if (aSlvsConstr.type != SLVS_C_MULTI_ROTATION && aSlvsConstr.type != SLVS_C_MULTI_TRANSLATION) {
  99     if (aSlvsConstr.wrkpl == SLVS_E_UNKNOWN && myWorkplaneID != SLVS_E_UNKNOWN)
 100       aSlvsConstr.wrkpl = myWorkplaneID;
 101     if (aSlvsConstr.group == SLVS_G_UNKNOWN)
 102       aSlvsConstr.group = (Slvs_hGroup)myGroupID;
 103     Slvs_hConstraint aConstrID = updateConstraint(aSlvsConstr);
 104     if (aSlvsConstr.h == SLVS_C_UNKNOWN) {
 105       aConstraint->changeConstraint() = getConstraint(aConstrID);
 106       isUpdated = true;
 107     }
 108   }
 109   return isUpdated;
 110 }
 111
 112 bool SolveSpaceSolver_Storage::update(EntityWrapperPtr& theEntity)
 113 {
 114   bool isUpdated = false;
 115   std::shared_ptr<SolveSpaceSolver_EntityWrapper> anEntity =
 116       std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theEntity);
 117   Slvs_Entity aSlvsEnt = getEntity((Slvs_hEntity)anEntity->id());
 118   if (aSlvsEnt.h == SLVS_E_UNKNOWN)
 119     aSlvsEnt = anEntity->entity();
 120
 121   std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
 122   std::list<ParameterWrapperPtr>::iterator aPIt;
 123   // if the entity is an attribute, need to update its coordinates
 124   if (anEntity->baseAttribute()) {
 125     BuilderPtr aBuilder = SolveSpaceSolver_Builder::getInstance();
 126     EntityWrapperPtr anUpdAttr = aBuilder->createAttribute(anEntity->baseAttribute(), GID_UNKNOWN);
 127     if (anUpdAttr) {
 128       std::list<ParameterWrapperPtr> anUpdParams = anUpdAttr->parameters();
 129       std::list<ParameterWrapperPtr>::iterator anUpdIt = anUpdParams.begin();
 130       for (aPIt = aParams.begin(); aPIt != aParams.end() && anUpdIt != anUpdParams.end();
 131           ++aPIt, ++anUpdIt) {
 132         (*aPIt)->update(*anUpdIt);
 133       }
 134     }
 135   }
 136
 137   // update parameters
 138   int anInd = 0;
 139   for (aPIt = aParams.begin(); aPIt != aParams.end(); ++aPIt, ++anInd) {
 140     assert(anInd < 4);
 141     isUpdated = update(*aPIt) || isUpdated;
 142     if (aSlvsEnt.param[anInd] != (Slvs_hEntity)(*aPIt)->id()) {
 143       isUpdated = true;
 144       aSlvsEnt.param[anInd] = (Slvs_hEntity)(*aPIt)->id();
 145     }
 146   }
 147
 148   // update sub-entities
 149   std::list<EntityWrapperPtr> aSubEntities = theEntity->subEntities();
 150   std::list<EntityWrapperPtr>::iterator aSIt = aSubEntities.begin();
 151   for (anInd = 0; aSIt != aSubEntities.end(); ++aSIt, ++anInd) {
 152     assert(anInd < 4);
 153     isUpdated = update(*aSIt) || isUpdated;
 154
 155     Slvs_hEntity anID = Slvs_hEntity((*aSIt)->id());
 156     if ((*aSIt)->type() == ENTITY_NORMAL)
 157       aSlvsEnt.normal = anID;
 158     else if ((*aSIt)->type() == ENTITY_SCALAR)
 159       aSlvsEnt.distance = anID;
 160     else if (aSlvsEnt.point[anInd] != anID) {
 161       aSlvsEnt.point[anInd] = anID;
 162       isUpdated = true;
 163     }
 164   }
 165
 166   // update entity itself
 167   if (aSlvsEnt.wrkpl == SLVS_E_UNKNOWN && myWorkplaneID != SLVS_E_UNKNOWN)
 168     aSlvsEnt.wrkpl = myWorkplaneID;
 169   if (aSlvsEnt.group == SLVS_G_UNKNOWN)
 170     aSlvsEnt.group = (Slvs_hGroup)myGroupID;
 171   Slvs_hEntity anEntID = updateEntity(aSlvsEnt);
 172   if (aSlvsEnt.h == SLVS_E_UNKNOWN) {
 173     anEntity->changeEntity() = getEntity(anEntID);
 174     isUpdated = true;
 175
 176     if (anEntity->type() == ENTITY_SKETCH)
 177       storeWorkplane(anEntity);
 178
 179     // For the correct work with arcs we will add them if their parameter is added
 180     if (theEntity->baseAttribute()) {
 181       FeaturePtr aFeature = ModelAPI_Feature::feature(theEntity->baseAttribute()->owner());
 182       if (aFeature->getKind() == SketchPlugin_Arc::ID() &&
 183           myFeatureMap.find(aFeature) == myFeatureMap.end()) {
 184         myFeatureMap[aFeature] = EntityWrapperPtr();
 185         return SketchSolver_Storage::update(aFeature, myGroupID);
 186       }
 187     }
 188   }
 189
 190   return isUpdated;
 191 }
 192
 193 bool SolveSpaceSolver_Storage::update(ParameterWrapperPtr& theParameter)
 194 {
 195   std::shared_ptr<SolveSpaceSolver_ParameterWrapper> aParameter =
 196       std::dynamic_pointer_cast<SolveSpaceSolver_ParameterWrapper>(theParameter);
 197   const Slvs_Param& aParam = getParameter((Slvs_hParam)aParameter->id());
 198   if (aParam.h != SLVS_E_UNKNOWN && fabs(aParam.val - aParameter->value()) < tolerance)
 199     return false;
 200   Slvs_Param aParamToUpd = aParameter->parameter();
 201   if (aParamToUpd.group == SLVS_G_UNKNOWN)
 202     aParamToUpd.group = aParameter->isParametric() ? (Slvs_hGroup)GID_OUTOFGROUP : (Slvs_hGroup)myGroupID;
 203   Slvs_hParam anID = updateParameter(aParamToUpd);
 204   if (aParam.h == SLVS_E_UNKNOWN) // new parameter
 205     aParameter->changeParameter() = getParameter(anID);
 206   return true;
 207 }
 208
 209 void SolveSpaceSolver_Storage::storeWorkplane(EntityWrapperPtr theSketch)
 210 {
 211   myWorkplaneID = (Slvs_hEntity)theSketch->id();
 212
 213   // Update sub-entities of the sketch
 214   std::list<EntityWrapperPtr> aSubEntities = theSketch->subEntities();
 215   std::list<EntityWrapperPtr>::iterator aSIt = aSubEntities.begin();
 216   for (; aSIt != aSubEntities.end(); ++aSIt) {
 217     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aSub =
 218         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*aSIt);
 219     aSub->changeEntity().wrkpl = myWorkplaneID;
 220   }
 221
 222   // Update all stored entities
 223   std::vector<Slvs_Entity>::iterator anIt = myEntities.begin();
 224   for (; anIt != myEntities.end(); ++anIt)
 225     anIt->wrkpl = myWorkplaneID;
 226 }
 227
 228 void SolveSpaceSolver_Storage::changeGroup(EntityWrapperPtr theEntity, const GroupID& theGroup)
 229 {
 230   std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
 231   std::list<ParameterWrapperPtr>::iterator aPIt = aParams.begin();
 232   for (; aPIt != aParams.end(); ++aPIt)
 233     changeGroup(*aPIt, theGroup);
 234
 235   std::list<EntityWrapperPtr> aSubs = theEntity->subEntities();
 236   std::list<EntityWrapperPtr>::iterator aSIt = aSubs.begin();
 237   for (; aSIt != aSubs.end(); ++aSIt)
 238     changeGroup(*aSIt, theGroup);
 239
 240   if (theEntity->group() != theGroup) {
 241     theEntity->setGroup(theGroup);
 242     int aPos = Search((Slvs_hEntity)theEntity->id(), myEntities);
 243     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 244       myEntities[aPos].group = (Slvs_hGroup)theGroup;
 245       setNeedToResolve(true);
 246     }
 247   }
 248 }
 249
 250 void SolveSpaceSolver_Storage::changeGroup(ParameterWrapperPtr theParam, const GroupID& theGroup)
 251 {
 252   GroupID aGroup = theGroup;
 253   if (theParam->isParametric())
 254     aGroup = GID_OUTOFGROUP;
 255   if (theParam->group() == aGroup)
 256     return;
 257
 258   theParam->setGroup(aGroup);
 259   int aPos = Search((Slvs_hParam)theParam->id(), myParameters);
 260   if (aPos >= 0 && aPos < (int)myParameters.size()) {
 261     myParameters[aPos].group = (Slvs_hGroup)aGroup;
 262     setNeedToResolve(true);
 263   }
 264 }
 265
 266 void SolveSpaceSolver_Storage::addCoincidentPoints(
 267     EntityWrapperPtr theMaster, EntityWrapperPtr theSlave)
 268 {
 269   if (theMaster->type() != ENTITY_POINT || theSlave->type() != ENTITY_POINT)
 270     return;
 271
 272   // Search available coincidence
 273   CoincidentPointsMap::iterator aMasterFound = myCoincidentPoints.find(theMaster);
 274   CoincidentPointsMap::iterator aSlaveFound = myCoincidentPoints.find(theSlave);
 275   if (aMasterFound == myCoincidentPoints.end() &&  aSlaveFound == myCoincidentPoints.end()) {
 276     // try to find master and slave points in the lists of slaves of already existent coincidences
 277     CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
 278     for (; anIt != myCoincidentPoints.end(); ++anIt) {
 279       if (anIt->second.find(theMaster) != anIt->second.end())
 280         aMasterFound = anIt;
 281       else if (anIt->second.find(theSlave) != anIt->second.end())
 282         aSlaveFound = anIt;
 283
 284       if (aMasterFound != myCoincidentPoints.end() &&  aSlaveFound != myCoincidentPoints.end())
 285         break;
 286     }
 287   }
 288
 289   if (aMasterFound == myCoincidentPoints.end()) {
 290     // create new group
 291     myCoincidentPoints[theMaster] = std::set<EntityWrapperPtr>();
 292     aMasterFound = myCoincidentPoints.find(theMaster);
 293   } else if (aMasterFound == aSlaveFound)
 294     return; // already coincident
 295
 296   if (aSlaveFound != myCoincidentPoints.end()) {
 297     // A slave has been found, we need to attach all points coincident with it to the new master
 298     std::set<EntityWrapperPtr> aNewSlaves = aSlaveFound->second;
 299     aNewSlaves.insert(aSlaveFound->first);
 300     myCoincidentPoints.erase(aSlaveFound);
 301
 302     std::set<EntityWrapperPtr>::const_iterator aSlIt = aNewSlaves.begin();
 303     for (; aSlIt != aNewSlaves.end(); ++aSlIt)
 304       addCoincidentPoints(theMaster, *aSlIt);
 305   } else {
 306     // Update the slave if it was used in constraints and features
 307     replaceInFeatures(theSlave, theMaster);
 308     replaceInConstraints(theSlave, theMaster);
 309
 310     // Remove slave entity (if the IDs are equal no need to remove slave entity, just update it)
 311     if (theMaster->id() != theSlave->id())
 312       removeEntity((Slvs_hEntity)theSlave->id());
 313
 314     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aPointMaster =
 315         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theMaster);
 316     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aPointSlave =
 317         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theSlave);
 318     aPointSlave->changeEntity() = aPointMaster->entity();
 319     aPointSlave->setParameters(aPointMaster->parameters());
 320
 321     aMasterFound->second.insert(theSlave);
 322   }
 323 }
 324
 325 void SolveSpaceSolver_Storage::replaceInFeatures(
 326     EntityWrapperPtr theSource, EntityWrapperPtr theDest)
 327 {
 328   std::map<FeaturePtr, EntityWrapperPtr>::const_iterator anIt = myFeatureMap.begin();
 329   for (; anIt != myFeatureMap.end(); ++anIt) {
 330     bool isUpdated = false;
 331     std::list<EntityWrapperPtr> aSubs = anIt->second->subEntities();
 332     std::list<EntityWrapperPtr>::iterator aSubIt = aSubs.begin();
 333     for (; aSubIt != aSubs.end(); ++aSubIt)
 334       if ((*aSubIt)->id() == theSource->id()) {
 335         (*aSubIt)->update(theDest);
 336         isUpdated = true;
 337       }
 338
 339     if (!isUpdated)
 340       continue;
 341
 342     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aWrapper =
 343         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(anIt->second);
 344     // update SolveSpace entity
 345     Slvs_Entity anEnt = aWrapper->entity();
 346     for (int i = 0; i < 4; ++i)
 347       if (anEnt.point[i] == (Slvs_hEntity)theSource->id())
 348         anEnt.point[i] = (Slvs_hEntity)theDest->id();
 349     anEnt.h = updateEntity(anEnt);
 350     aWrapper->changeEntity() = anEnt;
 351
 352     // update sub-entities
 353     aWrapper->setSubEntities(aSubs);
 354   }
 355 }
 356
 357 void SolveSpaceSolver_Storage::replaceInConstraints(
 358     EntityWrapperPtr theSource, EntityWrapperPtr theDest)
 359 {
 360   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
 361       anIt = myConstraintMap.begin();
 362   std::list<ConstraintWrapperPtr>::const_iterator aCIt;
 363   for (; anIt != myConstraintMap.end(); ++anIt)
 364     for (aCIt = anIt->second.begin(); aCIt != anIt->second.end(); ++aCIt) {
 365       // Do not process coincidence between points
 366       // (these constraints are stored to keep the structure of constraints).
 367       if ((*aCIt)->type() == CONSTRAINT_PT_PT_COINCIDENT)
 368         continue;
 369
 370       bool isUpdated = false;
 371       std::list<EntityWrapperPtr> aSubs = (*aCIt)->entities();
 372       std::list<EntityWrapperPtr>::iterator aSubIt = aSubs.begin();
 373       for (; aSubIt != aSubs.end(); ++aSubIt)
 374         if ((*aSubIt)->id() == theSource->id()) {
 375           (*aSubIt)->update(theDest);
 376           isUpdated = true;
 377         }
 378
 379       if (!isUpdated)
 380         continue;
 381
 382       std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aWrapper =
 383           std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(*aCIt);
 384       // change constraint entities
 385       Slvs_Constraint aConstr = aWrapper->constraint();
 386       if (aConstr.ptA == (Slvs_hEntity)theSource->id())
 387         aConstr.ptA = (Slvs_hEntity)theDest->id();
 388       if (aConstr.ptB == (Slvs_hEntity)theSource->id())
 389         aConstr.ptB = (Slvs_hEntity)theDest->id();
 390
 391       // check the constraint is duplicated
 392       std::vector<Slvs_Constraint>::const_iterator aSlvsCIt = myConstraints.begin();
 393       for (; aSlvsCIt != myConstraints.end(); ++aSlvsCIt)
 394         if (aConstr.h != aSlvsCIt->h &&
 395             aConstr.type == aSlvsCIt->type &&
 396             aConstr.ptA == aSlvsCIt->ptA && aConstr.ptB == aSlvsCIt->ptB &&
 397             aConstr.entityA == aSlvsCIt->entityA && aConstr.entityB == aSlvsCIt->entityB &&
 398             aConstr.entityC == aSlvsCIt->entityC && aConstr.entityD == aSlvsCIt->entityD) {
 399           removeConstraint(aConstr.h);
 400           aConstr = *aSlvsCIt;
 401           break;
 402         }
 403
 404       if (aSlvsCIt != myConstraints.end()) {
 405         // constraint is duplicated, search its wrapper to add the mapping
 406         std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
 407             anIt2 = myConstraintMap.begin();
 408         std::list<ConstraintWrapperPtr>::const_iterator aCIt2;
 409         for (; anIt2 != myConstraintMap.end(); ++anIt2)
 410           for (aCIt2 = anIt2->second.begin(); aCIt2 != anIt2->second.end(); ++aCIt2)
 411             if ((Slvs_hConstraint)(*aCIt2)->id() == aConstr.h) {
 412               addSameConstraints(*aCIt2, aWrapper);
 413               break;
 414             }
 415       } else
 416         aConstr.h = updateConstraint(aConstr);
 417       aWrapper->changeConstraint() = aConstr;
 418
 419       // update sub-entities
 420       aWrapper->setEntities(aSubs);
 421     }
 422 }
 423
 424 void SolveSpaceSolver_Storage::addSameConstraints(ConstraintWrapperPtr theConstraint1,
 425                                                   ConstraintWrapperPtr theConstraint2)
 426 {
 427   SameConstraintMap::iterator anIt = myEqualConstraints.begin();
 428   for (; anIt != myEqualConstraints.end(); ++anIt) {
 429     if (anIt->find(theConstraint1) != anIt->end()) {
 430       anIt->insert(theConstraint2);
 431       return;
 432     }
 433     else if (anIt->find(theConstraint2) != anIt->end()) {
 434       anIt->insert(theConstraint1);
 435       return;
 436     }
 437   }
 438   // group not found => create new one
 439   std::set<ConstraintWrapperPtr> aNewGroup;
 440   aNewGroup.insert(theConstraint1);
 441   aNewGroup.insert(theConstraint2);
 442   myEqualConstraints.push_back(aNewGroup);
 443 }
 444
 445
 446 EntityWrapperPtr SolveSpaceSolver_Storage::calculateMiddlePoint(
 447     EntityWrapperPtr theBase, double theCoeff)
 448 {
 449   BuilderPtr aBuilder = SolveSpaceSolver_Builder::getInstance();
 450
 451   std::shared_ptr<GeomAPI_XY> aMidPoint;
 452   if (theBase->type() == ENTITY_LINE) {
 453     std::shared_ptr<GeomAPI_Pnt2d> aPoints[2];
 454     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
 455     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
 456     for (int i = 0; i < 2; ++i, ++anIt)
 457       aPoints[i] = aBuilder->point(*anIt);
 458     aMidPoint = aPoints[0]->xy()->multiplied(1.0 - theCoeff)->added(
 459         aPoints[1]->xy()->multiplied(theCoeff));
 460   }
 461   else if (theBase->type() == ENTITY_ARC) {
 462     double theX, theY;
 463     double anArcPoint[3][2];
 464     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
 465     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
 466     for (int i = 0; i < 3; ++i, ++anIt) {
 467       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(*anIt);
 468       anArcPoint[i][0] = aPoint->x();
 469       anArcPoint[i][1] = aPoint->y();
 470     }
 471     // project last point of arc on the arc
 472     double x = anArcPoint[1][0] - anArcPoint[0][0];
 473     double y = anArcPoint[1][1] - anArcPoint[0][1];
 474     double aRad = sqrt(x*x + y*y);
 475     x = anArcPoint[2][0] - anArcPoint[0][0];
 476     y = anArcPoint[2][1] - anArcPoint[0][1];
 477     double aNorm = sqrt(x*x + y*y);
 478     if (aNorm >= tolerance) {
 479       anArcPoint[2][0] = x * aRad / aNorm;
 480       anArcPoint[2][1] = y * aRad / aNorm;
 481     }
 482     anArcPoint[1][0] -= anArcPoint[0][0];
 483     anArcPoint[1][1] -= anArcPoint[0][1];
 484     if (theCoeff < tolerance) {
 485       theX = anArcPoint[0][0] + anArcPoint[1][0];
 486       theY = anArcPoint[0][1] + anArcPoint[1][1];
 487     } else if (1 - theCoeff < tolerance) {
 488       theX = anArcPoint[0][0] + anArcPoint[2][0];
 489       theY = anArcPoint[0][1] + anArcPoint[2][1];
 490     } else {
 491       std::shared_ptr<GeomAPI_Dir2d> aStartDir(new GeomAPI_Dir2d(anArcPoint[1][0], anArcPoint[1][1]));
 492       std::shared_ptr<GeomAPI_Dir2d> aEndDir(new GeomAPI_Dir2d(anArcPoint[2][0], anArcPoint[2][1]));
 493       double anAngle = aStartDir->angle(aEndDir);
 494       if (anAngle < 0)
 495         anAngle += 2.0 * PI;
 496       anAngle *= theCoeff;
 497       double aCos = cos(anAngle);
 498       double aSin = sin(anAngle);
 499       theX = anArcPoint[0][0] + anArcPoint[1][0] * aCos - anArcPoint[1][1] * aSin;
 500       theY = anArcPoint[0][1] + anArcPoint[1][0] * aSin + anArcPoint[1][1] * aCos;
 501     }
 502     aMidPoint = std::shared_ptr<GeomAPI_XY>(new GeomAPI_XY(theX, theY));
 503   }
 504
 505   if (!aMidPoint)
 506     return EntityWrapperPtr();
 507
 508   std::list<ParameterWrapperPtr> aParameters;
 509   Slvs_Param aParam1 = Slvs_MakeParam(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID, aMidPoint->x());
 510   aParam1.h = addParameter(aParam1);
 511   aParameters.push_back(ParameterWrapperPtr(new SolveSpaceSolver_ParameterWrapper(aParam1)));
 512   Slvs_Param aParam2 = Slvs_MakeParam(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID, aMidPoint->y());
 513   aParam2.h = addParameter(aParam2);
 514   aParameters.push_back(ParameterWrapperPtr(new SolveSpaceSolver_ParameterWrapper(aParam2)));
 515   // Create entity (parameters are not filled)
 516   Slvs_Entity anEntity = Slvs_MakePoint2d(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID,
 517       (Slvs_hEntity)myWorkplaneID, aParam1.h, aParam2.h);
 518   anEntity.h = addEntity(anEntity);
 519
 520   EntityWrapperPtr aResult(new SolveSpaceSolver_EntityWrapper(AttributePtr(), anEntity));
 521   aResult->setParameters(aParameters);
 522   return aResult;
 523 }
 524
 525
 526
 527
 528
 529
 530 Slvs_hParam SolveSpaceSolver_Storage::addParameter(const Slvs_Param& theParam)
 531 {
 532   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
 533     // parameter is already used, rewrite it
 534     return updateParameter(theParam);
 535   }
 536
 537   Slvs_Param aParam = theParam;
 538   if (aParam.h > myParamMaxID)
 539     myParamMaxID = aParam.h;
 540   else
 541     aParam.h = ++myParamMaxID;
 542   myParameters.push_back(aParam);
 543   myNeedToResolve = true;
 544   return aParam.h;
 545 }
 546
 547 Slvs_hParam SolveSpaceSolver_Storage::updateParameter(const Slvs_Param& theParam)
 548 {
 549   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
 550     // parameter already used, rewrite it
 551     int aPos = Search(theParam.h, myParameters);
 552     if (aPos >= 0 && aPos < (int)myParameters.size()) {
 553       if (IsNotEqual(myParameters[aPos], theParam)) {
 554         myUpdatedParameters.insert(theParam.h);
 555         setNeedToResolve(true);
 556       }
 557       myParameters[aPos] = theParam;
 558       return theParam.h;
 559     }
 560   }
 561
 562   // Parameter is not found, add new one
 563   Slvs_Param aParam = theParam;
 564   aParam.h = 0;
 565   return addParameter(aParam);
 566 }
 567
 568 bool SolveSpaceSolver_Storage::removeParameter(const Slvs_hParam& theParamID)
 569 {
 570   int aPos = Search(theParamID, myParameters);
 571   if (aPos >= 0 && aPos < (int)myParameters.size()) {
 572     // Firstly, search the parameter is not used elsewhere
 573     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 574     for (; anEntIter != myEntities.end(); anEntIter++) {
 575       for (int i = 0; i < 4; i++)
 576         if (anEntIter->param[i] == theParamID)
 577           return false;
 578     }
 579     // Remove parameter
 580     myParameters.erase(myParameters.begin() + aPos);
 581     myParamMaxID = myParameters.empty() ? SLVS_E_UNKNOWN : myParameters.back().h;
 582     myNeedToResolve = true;
 583   }
 584   return true;
 585 }
 586
 587 const Slvs_Param& SolveSpaceSolver_Storage::getParameter(const Slvs_hParam& theParamID) const
 588 {
 589   int aPos = Search(theParamID, myParameters);
 590   if (aPos >= 0 && aPos < (int)myParameters.size())
 591     return myParameters[aPos];
 592
 593   // Parameter is not found, return empty object
 594   static Slvs_Param aDummy;
 595   aDummy.h = 0;
 596   return aDummy;
 597 }
 598
 599
 600 Slvs_hEntity SolveSpaceSolver_Storage::addEntity(const Slvs_Entity& theEntity)
 601 {
 602   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 603     // Entity is already used, rewrite it
 604     return updateEntity(theEntity);
 605   }
 606
 607   Slvs_Entity aEntity = theEntity;
 608   if (aEntity.h > myEntityMaxID)
 609     myEntityMaxID = aEntity.h;
 610   else
 611     aEntity.h = ++myEntityMaxID;
 612   myEntities.push_back(aEntity);
 613   myNeedToResolve = true;
 614   return aEntity.h;
 615 }
 616
 617 Slvs_hEntity SolveSpaceSolver_Storage::updateEntity(const Slvs_Entity& theEntity)
 618 {
 619   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 620     // Entity already used, rewrite it
 621     int aPos = Search(theEntity.h, myEntities);
 622     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 623       myNeedToResolve = myNeedToResolve || IsNotEqual(myEntities[aPos], theEntity);
 624       myEntities[aPos] = theEntity;
 625       return theEntity.h;
 626     }
 627   }
 628
 629   // Entity is not found, add new one
 630   Slvs_Entity aEntity = theEntity;
 631   aEntity.h = 0;
 632   return addEntity(aEntity);
 633 }
 634
 635 bool SolveSpaceSolver_Storage::removeEntity(const Slvs_hEntity& theEntityID)
 636 {
 637   bool aResult = true;
 638   int aPos = Search(theEntityID, myEntities);
 639   if (aPos >= 0 && aPos < (int)myEntities.size()) {
 640     // Firstly, check the entity and its attributes is not used elsewhere
 641     std::set<Slvs_hEntity> anEntAndSubs;
 642     anEntAndSubs.insert(theEntityID);
 643     for (int i = 0; i < 4; i++)
 644       if (myEntities[aPos].point[i] != SLVS_E_UNKNOWN)
 645         anEntAndSubs.insert(myEntities[aPos].point[i]);
 646
 647     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 648     for (; anEntIter != myEntities.end(); anEntIter++) {
 649       if (anEntAndSubs.find(anEntIter->h) != anEntAndSubs.end())
 650         continue;
 651       for (int i = 0; i < 4; i++)
 652         if (anEntAndSubs.find(anEntIter->point[i]) != anEntAndSubs.end())
 653           return false;
 654       if (anEntAndSubs.find(anEntIter->distance) != anEntAndSubs.end())
 655         return false;
 656     }
 657     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 658     for (; aConstrIter != myConstraints.end(); aConstrIter++) {
 659       Slvs_hEntity anEntIDs[6] = {aConstrIter->ptA, aConstrIter->ptB,
 660           aConstrIter->entityA, aConstrIter->entityB,
 661           aConstrIter->entityC, aConstrIter->entityD};
 662       for (int i = 0; i < 6; i++)
 663         if (anEntAndSubs.find(anEntIDs[i]) != anEntAndSubs.end())
 664           return false;
 665     }
 666     // The entity is not used, remove it and its parameters
 667     Slvs_Entity anEntity = myEntities[aPos];
 668     myEntities.erase(myEntities.begin() + aPos);
 669     myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 670     if (anEntity.distance != SLVS_E_UNKNOWN)
 671       aResult = aResult && removeParameter(anEntity.distance);
 672     for (int i = 0; i < 4; i++)
 673       if (anEntity.param[i] != SLVS_E_UNKNOWN)
 674         aResult = removeParameter(anEntity.param[i]) && aResult;
 675     for (int i = 0; i < 4; i++)
 676       if (anEntity.point[i] != SLVS_E_UNKNOWN)
 677         aResult = removeEntity(anEntity.point[i]) && aResult;
 678     myNeedToResolve = true;
 679   }
 680   return aResult;
 681 }
 682
 683 void SolveSpaceSolver_Storage::removeUnusedEntities()
 684 {
 685   std::set<Slvs_hEntity> anUnusedEntities;
 686   std::vector<Slvs_Entity>::const_iterator aEIt = myEntities.begin();
 687   for (; aEIt != myEntities.end(); ++aEIt) {
 688     if (aEIt->h == aEIt->wrkpl) {
 689       // don't remove workplane
 690       anUnusedEntities.erase(aEIt->point[0]);
 691       anUnusedEntities.erase(aEIt->normal);
 692       continue;
 693     }
 694     anUnusedEntities.insert(aEIt->h);
 695   }
 696
 697   std::vector<Slvs_Constraint>::const_iterator aCIt = myConstraints.begin();
 698   for (; aCIt != myConstraints.end(); ++aCIt) {
 699     Slvs_hEntity aSubs[6] = {
 700         aCIt->entityA, aCIt->entityB,
 701         aCIt->entityC, aCIt->entityD,
 702         aCIt->ptA,     aCIt->ptB};
 703     for (int i = 0; i < 6; i++) {
 704       if (aSubs[i] != SLVS_E_UNKNOWN) {
 705         anUnusedEntities.erase(aSubs[i]);
 706         int aPos = Search(aSubs[i], myEntities);
 707         if (aPos >= 0 && aPos < (int)myEntities.size()) {
 708           for (int j = 0; j < 4; j++)
 709             if (myEntities[aPos].point[j] != SLVS_E_UNKNOWN)
 710               anUnusedEntities.erase(myEntities[aPos].point[j]);
 711           if (myEntities[aPos].distance != SLVS_E_UNKNOWN)
 712             anUnusedEntities.erase(myEntities[aPos].distance);
 713         }
 714       }
 715     }
 716   }
 717
 718   std::set<Slvs_hEntity>::const_iterator anEntIt = anUnusedEntities.begin();
 719   while (anEntIt != anUnusedEntities.end()) {
 720     int aPos = Search(*anEntIt, myEntities);
 721     if (aPos < 0 && aPos >= (int)myEntities.size())
 722       continue;
 723     Slvs_Entity anEntity = myEntities[aPos];
 724     // Remove entity if and only if all its parameters unused
 725     bool isUsed = false;
 726     if (anEntity.distance != SLVS_E_UNKNOWN &&
 727       anUnusedEntities.find(anEntity.distance) == anUnusedEntities.end())
 728       isUsed = true;
 729     for (int i = 0; i < 4 && !isUsed; i++)
 730       if (anEntity.point[i] != SLVS_E_UNKNOWN &&
 731           anUnusedEntities.find(anEntity.point[i]) == anUnusedEntities.end())
 732         isUsed = true;
 733     if (isUsed) {
 734       anUnusedEntities.erase(anEntity.distance);
 735       for (int i = 0; i < 4; i++)
 736         if (anEntity.point[i] != SLVS_E_UNKNOWN)
 737           anUnusedEntities.erase(anEntity.point[i]);
 738       std::set<Slvs_hEntity>::iterator aRemoveIt = anEntIt++;
 739       anUnusedEntities.erase(aRemoveIt);
 740       continue;
 741     }
 742     ++anEntIt;
 743   }
 744
 745   for (anEntIt = anUnusedEntities.begin(); anEntIt != anUnusedEntities.end(); ++anEntIt) {
 746     int aPos = Search(*anEntIt, myEntities);
 747     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 748       // Remove entity and its parameters
 749       Slvs_Entity anEntity = myEntities[aPos];
 750       myEntities.erase(myEntities.begin() + aPos);
 751       myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 752       if (anEntity.distance != SLVS_E_UNKNOWN)
 753         removeParameter(anEntity.distance);
 754       for (int i = 0; i < 4; i++)
 755         if (anEntity.param[i] != SLVS_E_UNKNOWN)
 756           removeParameter(anEntity.param[i]);
 757       for (int i = 0; i < 4; i++)
 758         if (anEntity.point[i] != SLVS_E_UNKNOWN)
 759           removeEntity(anEntity.point[i]);
 760     }
 761   }
 762
 763   if (!anUnusedEntities.empty())
 764     myNeedToResolve = true;
 765 }
 766
 767 bool SolveSpaceSolver_Storage::isUsedByConstraints(const Slvs_hEntity& theEntityID) const
 768 {
 769   std::vector<Slvs_Constraint>::const_iterator aCIt = myConstraints.begin();
 770   for (; aCIt != myConstraints.end(); ++aCIt) {
 771     Slvs_hEntity aSubs[6] = {
 772         aCIt->entityA, aCIt->entityB,
 773         aCIt->entityC, aCIt->entityD,
 774         aCIt->ptA,     aCIt->ptB};
 775     for (int i = 0; i < 6; i++)
 776       if (aSubs[i] != SLVS_E_UNKNOWN && aSubs[i] == theEntityID)
 777         return true;
 778   }
 779   return false;
 780 }
 781
 782 const Slvs_Entity& SolveSpaceSolver_Storage::getEntity(const Slvs_hEntity& theEntityID) const
 783 {
 784   int aPos = Search(theEntityID, myEntities);
 785   if (aPos >= 0 && aPos < (int)myEntities.size())
 786     return myEntities[aPos];
 787
 788   // Entity is not found, return empty object
 789   static Slvs_Entity aDummy;
 790   aDummy.h = SLVS_E_UNKNOWN;
 791   return aDummy;
 792 }
 793
 794 Slvs_hEntity SolveSpaceSolver_Storage::copyEntity(const Slvs_hEntity& theCopied)
 795 {
 796   int aPos = Search(theCopied, myEntities);
 797   if (aPos < 0 || aPos >= (int)myEntities.size())
 798     return SLVS_E_UNKNOWN;
 799
 800   Slvs_Entity aCopy = myEntities[aPos];
 801   aCopy.h = SLVS_E_UNKNOWN;
 802   int i = 0;
 803   while (aCopy.point[i] != SLVS_E_UNKNOWN) {
 804     aCopy.point[i] = copyEntity(aCopy.point[i]);
 805     i++;
 806   }
 807   if (aCopy.param[0] != SLVS_E_UNKNOWN) {
 808     aPos = Search(aCopy.param[0], myParameters);
 809     i = 0;
 810     while (aCopy.param[i] != SLVS_E_UNKNOWN) {
 811       Slvs_Param aNewParam = myParameters[aPos];
 812       aNewParam.h = SLVS_E_UNKNOWN;
 813       aCopy.param[i] = addParameter(aNewParam);
 814       i++;
 815       aPos++;
 816     }
 817   }
 818   return addEntity(aCopy);
 819 }
 820
 821 void SolveSpaceSolver_Storage::copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo)
 822 {
 823   int aPosFrom = Search(theFrom, myEntities);
 824   int aPosTo = Search(theTo, myEntities);
 825   if (aPosFrom < 0 || aPosFrom >= (int)myEntities.size() ||
 826       aPosTo < 0 || aPosTo >= (int)myEntities.size())
 827     return;
 828
 829   Slvs_Entity aEntFrom = myEntities[aPosFrom];
 830   Slvs_Entity aEntTo = myEntities[aPosTo];
 831   int i = 0;
 832   while (aEntFrom.point[i] != SLVS_E_UNKNOWN) {
 833     copyEntity(aEntFrom.point[i], aEntTo.point[i]);
 834     i++;
 835   }
 836   if (aEntFrom.param[0] != SLVS_E_UNKNOWN) {
 837     aPosFrom = Search(aEntFrom.param[0], myParameters);
 838     aPosTo = Search(aEntTo.param[0], myParameters);
 839     i = 0;
 840     while (aEntFrom.param[i] != SLVS_E_UNKNOWN) {
 841       myParameters[aPosTo++].val = myParameters[aPosFrom++].val;
 842       i++;
 843     }
 844   }
 845 }
 846
 847
 848 bool SolveSpaceSolver_Storage::isPointFixed(
 849     const Slvs_hEntity& thePointID, Slvs_hConstraint& theFixed, bool theAccurate) const
 850 {
 851   // Search the set of coincident points
 852   std::set<Slvs_hEntity> aCoincident;
 853   aCoincident.insert(thePointID);
 854
 855   // Check whether one of coincident points is out-of-group
 856   std::set<Slvs_hEntity>::const_iterator aCoincIt = aCoincident.begin();
 857   for (; aCoincIt != aCoincident.end(); ++aCoincIt) {
 858     Slvs_Entity aPoint = getEntity(*aCoincIt);
 859     if (aPoint.group == SLVS_G_OUTOFGROUP)
 860       return true;
 861   }
 862
 863   // Search the Rigid constraint
 864   theFixed = SLVS_C_UNKNOWN;
 865   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 866   for (; aConstrIter != myConstraints.end(); aConstrIter++)
 867     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 868         aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
 869       theFixed = aConstrIter->h;
 870       if (aConstrIter->ptA == thePointID)
 871         return true;
 872     }
 873   if (theFixed != SLVS_C_UNKNOWN)
 874     return true;
 875
 876   if (theAccurate) {
 877     // Try to find the fixed entity which uses such point or its coincidence
 878     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 879     for (; anEntIter != myEntities.end(); anEntIter++) {
 880       for (int i = 0; i < 4; i++) {
 881         Slvs_hEntity aPt = anEntIter->point[i];
 882         if (aPt != SLVS_E_UNKNOWN &&
 883             (aPt == thePointID || aCoincident.find(aPt) != aCoincident.end())) {
 884           if (isEntityFixed(anEntIter->h, true))
 885             return true;
 886         }
 887       }
 888     }
 889   }
 890   return SLVS_E_UNKNOWN;
 891 }
 892
 893 bool SolveSpaceSolver_Storage::isEntityFixed(const Slvs_hEntity& theEntityID, bool theAccurate) const
 894 {
 895   int aPos = Search(theEntityID, myEntities);
 896   if (aPos < 0 || aPos >= (int)myEntities.size())
 897     return false;
 898
 899   // Firstly, find how many points are under Rigid constraint
 900   int aNbFixed = 0;
 901   for (int i = 0; i < 4; i++) {
 902     Slvs_hEntity aPoint = myEntities[aPos].point[i];
 903     if (aPoint == SLVS_E_UNKNOWN)
 904       continue;
 905
 906     std::set<Slvs_hEntity> aCoincident;
 907     aCoincident.insert(aPoint);
 908
 909     // Search the Rigid constraint
 910     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 911     for (; aConstrIter != myConstraints.end(); aConstrIter++)
 912       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
 913           aCoincident.find(aConstrIter->ptA) != aCoincident.end())
 914         aNbFixed++;
 915   }
 916
 917   std::list<Slvs_Constraint> aList;
 918   std::list<Slvs_Constraint>::iterator anIt;
 919   Slvs_hConstraint aTempID; // used in isPointFixed() method
 920
 921   if (myEntities[aPos].type == SLVS_E_LINE_SEGMENT) {
 922     if (aNbFixed == 2)
 923       return true;
 924     else if (aNbFixed == 0 || !theAccurate)
 925       return false;
 926     // Additional check (the line may be fixed if it is used by different constraints):
 927     // 1. The line is used in Equal constraint, another entity is fixed and there is a fixed point on line
 928     aList = getConstraintsByType(SLVS_C_PT_ON_LINE);
 929     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 930       if (anIt->entityA == theEntityID && isPointFixed(anIt->ptA, aTempID))
 931         break;
 932     if (anIt != aList.end()) {
 933       aList = getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
 934       aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 935       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 936         if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 937           Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 938           if (isEntityFixed(anOther, false))
 939             return true;
 940         }
 941     }
 942     // 2. The line is used in Parallel/Perpendicular/Vertical/Horizontal and Length constraints
 943     aList = getConstraintsByType(SLVS_C_PARALLEL);
 944     aList.splice(aList.end(), getConstraintsByType(SLVS_C_PERPENDICULAR));
 945     aList.splice(aList.end(), getConstraintsByType(SLVS_C_VERTICAL));
 946     aList.splice(aList.end(), getConstraintsByType(SLVS_C_HORIZONTAL));
 947     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 948       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 949         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 950         if (isEntityFixed(anOther, false))
 951           break;
 952       }
 953     if (anIt != aList.end()) {
 954       aList = getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 955       for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 956         if ((anIt->ptA == myEntities[aPos].point[0] && anIt->ptB == myEntities[aPos].point[1]) ||
 957             (anIt->ptA == myEntities[aPos].point[1] && anIt->ptB == myEntities[aPos].point[0]))
 958           return true;
 959     }
 960     // 3. Another verifiers ...
 961   } else if (myEntities[aPos].type == SLVS_E_CIRCLE) {
 962     if (aNbFixed == 0)
 963       return false;
 964     // Search for Diameter constraint
 965     aList = getConstraintsByType(SLVS_C_DIAMETER);
 966     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 967       if (anIt->entityA == theEntityID)
 968         return true;
 969     if (!theAccurate)
 970       return false;
 971     // Additional check (the circle may be fixed if it is used by different constraints):
 972     // 1. The circle is used in Equal constraint and another entity is fixed
 973     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 974     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 975       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 976         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
 977         if (isEntityFixed(anOther, false))
 978           return true;
 979       }
 980     // 2. Another verifiers ...
 981   } else if (myEntities[aPos].type == SLVS_E_ARC_OF_CIRCLE) {
 982     if (aNbFixed > 2)
 983       return true;
 984     else if (aNbFixed <= 1)
 985       return false;
 986     // Search for Diameter constraint
 987     aList = getConstraintsByType(SLVS_C_DIAMETER);
 988     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 989       if (anIt->entityA == theEntityID)
 990         return true;
 991     if (!theAccurate)
 992       return false;
 993     // Additional check (the arc may be fixed if it is used by different constraints):
 994     // 1. The arc is used in Equal constraint and another entity is fixed
 995     aList = getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 996     aList.splice(aList.end(), getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN));
 997     for (anIt = aList.begin(); anIt != aList.end(); anIt++)
 998       if (anIt->entityA == theEntityID || anIt->entityB == theEntityID) {
 999         Slvs_hEntity anOther = anIt->entityA == theEntityID ? anIt->entityB : anIt->entityA;
1000         if (isEntityFixed(anOther, false))
1001           return true;
1002       }
1003     // 2. Another verifiers ...
1004   }
1005   return false;
1006 }
1007
1008
1009 Slvs_hConstraint SolveSpaceSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
1010 {
1011   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
1012     // Constraint is already used, rewrite it
1013     return updateConstraint(theConstraint);
1014   }
1015
1016   Slvs_Constraint aConstraint = theConstraint;
1017
1018   // Find a constraint with same type uses same arguments to show user overconstraint situation
1019   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
1020   for (; aCIt != myConstraints.end(); aCIt++) {
1021     if (aConstraint.type != aCIt->type)
1022       continue;
1023     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
1024         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
1025         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD)
1026       myDuplicatedConstraint = true;
1027   }
1028
1029   if (aConstraint.h > myConstrMaxID)
1030     myConstrMaxID = aConstraint.h;
1031   else
1032     aConstraint.h = ++myConstrMaxID;
1033   myConstraints.push_back(aConstraint);
1034   myNeedToResolve = true;
1035   return aConstraint.h;
1036 }
1037
1038 Slvs_hConstraint SolveSpaceSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
1039 {
1040   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
1041     // Constraint already used, rewrite it
1042     int aPos = Search(theConstraint.h, myConstraints);
1043     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
1044       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
1045       myConstraints[aPos] = theConstraint;
1046       return theConstraint.h;
1047     }
1048   }
1049
1050   // Constraint is not found, add new one
1051   Slvs_Constraint aConstraint = theConstraint;
1052   aConstraint.h = 0;
1053   return addConstraint(aConstraint);
1054 }
1055
1056 bool SolveSpaceSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
1057 {
1058   bool aResult = true;
1059   int aPos = Search(theConstraintID, myConstraints);
1060   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
1061     Slvs_Constraint aConstraint = myConstraints[aPos];
1062     myConstraints.erase(myConstraints.begin() + aPos);
1063     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
1064     myNeedToResolve = true;
1065
1066     // Remove all entities
1067     Slvs_hEntity anEntities[6] = {aConstraint.ptA, aConstraint.ptB,
1068         aConstraint.entityA, aConstraint.entityB,
1069         aConstraint.entityC, aConstraint.entityD};
1070     for (int i = 0; i < 6; i++)
1071       if (anEntities[i] != SLVS_E_UNKNOWN)
1072         aResult = removeEntity(anEntities[i]) && aResult;
1073     // remove temporary fixed point, if available
1074     if (myFixed == theConstraintID)
1075       myFixed = SLVS_E_UNKNOWN;
1076     if (myDuplicatedConstraint) {
1077       // Check the duplicated constraints are still available
1078       myDuplicatedConstraint = false;
1079       std::vector<Slvs_Constraint>::const_iterator anIt1 = myConstraints.begin();
1080       std::vector<Slvs_Constraint>::const_iterator anIt2 = myConstraints.begin();
1081       for (; anIt1 != myConstraints.end() && !myDuplicatedConstraint; anIt1++)
1082         for (anIt2 = anIt1+1; anIt2 != myConstraints.end() && !myDuplicatedConstraint; anIt2++) {
1083           if (anIt1->type != anIt2->type)
1084             continue;
1085           if (anIt1->ptA == anIt2->ptA && anIt1->ptB == anIt2->ptB &&
1086               anIt1->entityA == anIt2->entityA && anIt1->entityB == anIt2->entityB &&
1087               anIt1->entityC == anIt2->entityC && anIt1->entityD == anIt2->entityD)
1088             myDuplicatedConstraint = true;
1089         }
1090     }
1091   }
1092   return aResult;
1093 }
1094
1095 const Slvs_Constraint& SolveSpaceSolver_Storage::getConstraint(const Slvs_hConstraint& theConstraintID) const
1096 {
1097   int aPos = Search(theConstraintID, myConstraints);
1098   if (aPos >= 0 && aPos < (int)myConstraints.size())
1099     return myConstraints[aPos];
1100
1101   // Constraint is not found, return empty object
1102   static Slvs_Constraint aDummy;
1103   aDummy.h = 0;
1104   return aDummy;
1105 }
1106
1107 std::list<Slvs_Constraint> SolveSpaceSolver_Storage::getConstraintsByType(int theConstraintType) const
1108 {
1109   std::list<Slvs_Constraint> aResult;
1110   std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
1111   for (; aCIter != myConstraints.end(); aCIter++)
1112     if (aCIter->type == theConstraintType)
1113       aResult.push_back(*aCIter);
1114   return aResult;
1115 }
1116
1117
1118 void SolveSpaceSolver_Storage::addConstraintWhereDragged(const Slvs_hConstraint& theConstraintID)
1119 {
1120   if (myFixed != SLVS_E_UNKNOWN)
1121     return; // the point is already fixed
1122   int aPos = Search(theConstraintID, myConstraints);
1123   if (aPos >= 0 && aPos < (int)myConstraints.size())
1124     myFixed = theConstraintID;
1125 }
1126
1127 void SolveSpaceSolver_Storage::addTemporaryConstraint(const Slvs_hConstraint& theConstraintID)
1128 {
1129   myTemporaryConstraints.insert(theConstraintID);
1130 }
1131
1132 void SolveSpaceSolver_Storage::removeAllTemporary()
1133 {
1134   myTemporaryConstraints.clear();
1135 }
1136
1137 size_t SolveSpaceSolver_Storage::removeTemporary(size_t theNbConstraints)
1138 {
1139   if (myTemporaryConstraints.empty())
1140     return 0;
1141   // Search the point-on-line or a non-rigid constraint
1142   std::set<Slvs_hConstraint>::iterator aCIt = myTemporaryConstraints.begin();
1143   for (; aCIt != myTemporaryConstraints.end(); aCIt++) {
1144     int aPos = Search(*aCIt, myConstraints);
1145     if (aPos >= (int)myConstraints.size() || myConstraints[aPos].type != SLVS_C_WHERE_DRAGGED)
1146       break;
1147     std::vector<Slvs_Constraint>::iterator anIt = myConstraints.begin();
1148     for (; anIt != myConstraints.end(); anIt++)
1149       if (anIt->type == SLVS_C_PT_ON_LINE && anIt->ptA == myConstraints[aPos].ptA)
1150         break;
1151     if (anIt != myConstraints.end())
1152       break;
1153   }
1154   if (aCIt == myTemporaryConstraints.end())
1155     aCIt = myTemporaryConstraints.begin();
1156   bool aNewFixed = false;
1157
1158   size_t aNbRemain = theNbConstraints;
1159   while (aNbRemain > 0 && aCIt != myTemporaryConstraints.end()) {
1160     aNewFixed = aNewFixed || (*aCIt == myFixed);
1161     --aNbRemain;
1162
1163     std::set<Slvs_hConstraint>::iterator aRemoveIt = aCIt++;
1164     removeConstraint(*aRemoveIt);
1165     myTemporaryConstraints.erase(aRemoveIt);
1166     if (aCIt == myTemporaryConstraints.end())
1167       aCIt = myTemporaryConstraints.begin();
1168   }
1169
1170   if (aNewFixed) {
1171     for (aCIt = myTemporaryConstraints.begin(); aCIt != myTemporaryConstraints.end(); aCIt++) {
1172       int aPos = Search(*aCIt, myConstraints);
1173       if (myConstraints[aPos].type == SLVS_C_WHERE_DRAGGED) {
1174         myFixed = *aCIt;
1175         break;
1176       }
1177     }
1178   }
1179   return myTemporaryConstraints.size();
1180 }
1181
1182 bool SolveSpaceSolver_Storage::isTemporary(const Slvs_hConstraint& theConstraintID) const
1183 {
1184   return myTemporaryConstraints.find(theConstraintID) != myTemporaryConstraints.end();
1185 }
1186
1187
1188
1189 void SolveSpaceSolver_Storage::initializeSolver(SolverPtr theSolver)
1190 {
1191   std::shared_ptr<SolveSpaceSolver_Solver> aSolver =
1192       std::dynamic_pointer_cast<SolveSpaceSolver_Solver>(theSolver);
1193   if (!aSolver)
1194     return;
1195
1196   if (myConstraints.empty()) {
1197     // Adjust all arc to place their points correctly
1198     std::vector<Slvs_Entity>::const_iterator anEntIt = myEntities.begin();
1199     for (; anEntIt != myEntities.end(); ++anEntIt)
1200       if (anEntIt->type == SLVS_E_ARC_OF_CIRCLE)
1201         adjustArc(*anEntIt);
1202   }
1203
1204   aSolver->setParameters(myParameters.data(), (int)myParameters.size());
1205   aSolver->setEntities(myEntities.data(), (int)myEntities.size());
1206
1207   // Copy constraints excluding the fixed one
1208   std::vector<Slvs_Constraint> aConstraints = myConstraints;
1209   if (myFixed != SLVS_E_UNKNOWN) {
1210     Slvs_hEntity aFixedPoint = SLVS_E_UNKNOWN;
1211     std::vector<Slvs_Constraint>::iterator anIt = aConstraints.begin();
1212     for (; anIt != aConstraints.end(); anIt++)
1213       if (anIt->h == myFixed) {
1214         aFixedPoint = anIt->ptA;
1215         aConstraints.erase(anIt);
1216         break;
1217       }
1218     // set dragged parameters
1219     int aPos = Search(aFixedPoint, myEntities);
1220     aSolver->setDraggedParameters(myEntities[aPos].param);
1221   }
1222   aSolver->setConstraints(aConstraints.data(), (int)aConstraints.size());
1223 }
1224
1225
1226 bool SolveSpaceSolver_Storage::isEqual(
1227     const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2) const
1228 {
1229   // Precise checking of coincidence: verify that points have equal coordinates
1230   int aEnt1Pos = Search(thePoint1, myEntities);
1231   int aEnt2Pos = Search(thePoint2, myEntities);
1232   if (aEnt1Pos >= 0 && aEnt1Pos < (int)myEntities.size() &&
1233       aEnt2Pos >= 0 && aEnt2Pos < (int)myEntities.size()) {
1234     double aDist[2];
1235     int aParamPos;
1236     for (int i = 0; i < 2; i++) {
1237       aParamPos = Search(myEntities[aEnt1Pos].param[i], myParameters);
1238       aDist[i] = myParameters[aParamPos].val;
1239       aParamPos = Search(myEntities[aEnt2Pos].param[i], myParameters);
1240       aDist[i] -= myParameters[aParamPos].val;
1241     }
1242     if (aDist[0] * aDist[0] + aDist[1] * aDist[1] < tolerance * tolerance)
1243       return true;
1244   }
1245   return false;
1246 }
1247
1248
1249 std::vector<Slvs_hConstraint> SolveSpaceSolver_Storage::fixEntity(const Slvs_hEntity& theEntity)
1250 {
1251   std::vector<Slvs_hConstraint> aNewConstraints;
1252
1253   int aPos = Search(theEntity, myEntities);
1254   if (aPos >= 0 && aPos < (int)myEntities.size()) {
1255     switch (myEntities[aPos].type) {
1256     case SLVS_E_POINT_IN_2D:
1257     case SLVS_E_POINT_IN_3D:
1258       fixPoint(myEntities[aPos], aNewConstraints);
1259       break;
1260     case SLVS_E_LINE_SEGMENT:
1261       fixLine(myEntities[aPos], aNewConstraints);
1262       break;
1263     case SLVS_E_CIRCLE:
1264       fixCircle(myEntities[aPos], aNewConstraints);
1265       break;
1266     case SLVS_E_ARC_OF_CIRCLE:
1267       fixArc(myEntities[aPos], aNewConstraints);
1268       break;
1269     default:
1270       break;
1271     }
1272   }
1273
1274   return aNewConstraints;
1275 }
1276
1277 void SolveSpaceSolver_Storage::fixPoint(const Slvs_Entity& thePoint,
1278     std::vector<Slvs_hConstraint>& theCreated)
1279 {
1280   Slvs_Constraint aConstraint;
1281   Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
1282   bool isFixed = isPointFixed(thePoint.h, aConstrID, true);
1283   bool isForceUpdate = (isFixed && isTemporary(aConstrID));
1284   if (!isForceUpdate) { // create new constraint
1285     if (isFixed) return;
1286     aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, thePoint.group, SLVS_C_WHERE_DRAGGED, thePoint.wrkpl,
1287         0.0, thePoint.h, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
1288     aConstraint.h = addConstraint(aConstraint);
1289     theCreated.push_back(aConstraint.h);
1290   } else { // update already existent constraint
1291     if (!isFixed || aConstrID == SLVS_E_UNKNOWN)
1292       return;
1293     int aPos = Search(aConstrID, myConstraints);
1294     if (aPos >= 0 && aPos < (int)myConstraints.size())
1295       myConstraints[aPos].ptA = thePoint.h;
1296   }
1297 }
1298
1299 void SolveSpaceSolver_Storage::fixLine(const Slvs_Entity& theLine,
1300     std::vector<Slvs_hConstraint>& theCreated)
1301 {
1302   Slvs_Entity aPoint[2] = {
1303       getEntity(theLine.point[0]),
1304       getEntity(theLine.point[1])
1305   };
1306
1307   Slvs_Constraint anEqual;
1308   if (isAxisParallel(theLine.h)) {
1309     // Fix one point and a line length
1310     Slvs_hConstraint aFixed;
1311     if (!isPointFixed(theLine.point[0], aFixed, true) &&
1312         !isPointFixed(theLine.point[1], aFixed, true))
1313       fixPoint(aPoint[0], theCreated);
1314     if (!isUsedInEqual(theLine.h, anEqual)) {
1315       // Check the distance is not set yet
1316       std::vector<Slvs_Constraint>::const_iterator aDistIt = myConstraints.begin();
1317       for (; aDistIt != myConstraints.end(); ++aDistIt)
1318         if ((aDistIt->type == SLVS_C_PT_PT_DISTANCE) &&
1319            ((aDistIt->ptA == theLine.point[0] && aDistIt->ptB == theLine.point[1]) ||
1320             (aDistIt->ptA == theLine.point[1] && aDistIt->ptB == theLine.point[0])))
1321           return;
1322       // Calculate distance between points on the line
1323       double aCoords[4];
1324       for (int i = 0; i < 2; i++)
1325         for (int j = 0; j < 2; j++) {
1326           Slvs_Param aParam = getParameter(aPoint[i].param[j]);
1327           aCoords[2*i+j] = aParam.val;
1328         }
1329
1330       double aLength = sqrt((aCoords[2] - aCoords[0]) * (aCoords[2] - aCoords[0]) +
1331                             (aCoords[3] - aCoords[1]) * (aCoords[3] - aCoords[1]));
1332       // fix line length
1333       Slvs_Constraint aDistance = Slvs_MakeConstraint(SLVS_E_UNKNOWN, theLine.group,
1334           SLVS_C_PT_PT_DISTANCE, theLine.wrkpl, aLength,
1335           theLine.point[0], theLine.point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
1336       aDistance.h = addConstraint(aDistance);
1337       theCreated.push_back(aDistance.h);
1338     }
1339     return;
1340   }
1341   else if (isUsedInEqual(theLine.h, anEqual)) {
1342     // Check another entity of Equal is already fixed
1343     Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
1344     if (isEntityFixed(anOtherEntID, true)) {
1345       // Fix start point of the line (if end point is not fixed yet) ...
1346       Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
1347       bool isFixed = isPointFixed(theLine.point[1], anEndFixedID, true);
1348       if (isFixed == SLVS_E_UNKNOWN)
1349         fixPoint(aPoint[0], theCreated);
1350       // ...  and create fixed point lying on this line
1351       Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
1352       // Firstly, search already fixed point on line
1353       bool isPonLineFixed = false;
1354       Slvs_hEntity aFixedPoint;
1355       std::vector<Slvs_Constraint>::const_iterator aPLIter = myConstraints.begin();
1356       for (; aPLIter != myConstraints.end() && !isPonLineFixed; ++aPLIter)
1357         if (aPLIter->type == SLVS_C_PT_ON_LINE && aPLIter->entityA == theLine.h) {
1358           isPonLineFixed = isPointFixed(aPLIter->ptA, anEndFixedID);
1359           aFixedPoint = aPLIter->ptA;
1360         }
1361
1362       if (isPonLineFixed) { // update existent constraint
1363         copyEntity(aPointToCopy, aFixedPoint);
1364       } else { // create new constraint
1365         Slvs_hEntity aCopied = copyEntity(aPointToCopy);
1366         Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, theLine.group, SLVS_C_PT_ON_LINE,
1367             theLine.wrkpl, 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
1368         aPonLine.h = addConstraint(aPonLine);
1369         theCreated.push_back(aPonLine.h);
1370         fixPoint(getEntity(aCopied), theCreated);
1371       }
1372       return;
1373     }
1374   }
1375
1376   // Fix both points
1377   for (int i = 0; i < 2; i++)
1378     fixPoint(aPoint[i], theCreated);
1379 }
1380
1381 void SolveSpaceSolver_Storage::fixCircle(const Slvs_Entity& theCircle,
1382     std::vector<Slvs_hConstraint>& theCreated)
1383 {
1384   bool isFixRadius = true;
1385   // Verify the arc is under Equal constraint
1386   Slvs_Constraint anEqual;
1387   if (isUsedInEqual(theCircle.h, anEqual)) {
1388     // Check another entity of Equal is already fixed
1389     Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
1390     if (isEntityFixed(anOtherEntID, true))
1391       isFixRadius = false;
1392   }
1393
1394   fixPoint(getEntity(theCircle.point[0]), theCreated);
1395
1396   if (isFixRadius) {
1397     // Search the radius is already fixed
1398     std::vector<Slvs_Constraint>::const_iterator aDiamIter = myConstraints.begin();
1399     for (; aDiamIter != myConstraints.end(); ++aDiamIter)
1400       if (aDiamIter->type == SLVS_C_DIAMETER && aDiamIter->entityA == theCircle.h)
1401         return;
1402
1403     // Fix radius of a circle
1404     const Slvs_Entity& aRadEnt = getEntity(theCircle.distance);
1405     double aRadius = getParameter(aRadEnt.param[0]).val;
1406     Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, theCircle.group, SLVS_C_DIAMETER,
1407         theCircle.wrkpl, aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, theCircle.h, SLVS_E_UNKNOWN);
1408     aFixedR.h = addConstraint(aFixedR);
1409     theCreated.push_back(aFixedR.h);
1410   }
1411 }
1412
1413 void SolveSpaceSolver_Storage::fixArc(const Slvs_Entity& theArc,
1414     std::vector<Slvs_hConstraint>& theCreated)
1415 {
1416   Slvs_Entity aPoint[3] = {
1417       getEntity(theArc.point[0]),
1418       getEntity(theArc.point[1]),
1419       getEntity(theArc.point[2])
1420   };
1421
1422   bool isFixRadius = true;
1423   std::list<Slvs_Entity> aPointsToFix;
1424   aPointsToFix.push_back(aPoint[1]);
1425   aPointsToFix.push_back(aPoint[2]);
1426
1427   // Verify the arc is under Equal constraint
1428   Slvs_Constraint anEqual;
1429   if (isUsedInEqual(theArc.h, anEqual)) {
1430     // Check another entity of Equal is already fixed
1431     Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
1432     if (isEntityFixed(anOtherEntID, true)) {
1433       isFixRadius = false;
1434       Slvs_Entity anOtherEntity = getEntity(anOtherEntID);
1435       if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
1436         aPointsToFix.pop_back();
1437         aPointsToFix.push_back(aPoint[0]);
1438       }
1439     }
1440   }
1441
1442   Slvs_hConstraint aConstrID;
1443   int aNbPointsToFix = 2; // number of fixed points for the arc
1444   if (isPointFixed(theArc.point[0], aConstrID, true))
1445     aNbPointsToFix--;
1446
1447   double anArcPoints[3][2];
1448   for (int i = 0; i < 3; i++) {
1449     const Slvs_Entity& aPointOnArc = getEntity(theArc.point[i]);
1450     for (int j = 0; j < 2; j++)
1451       anArcPoints[i][j] = getParameter(aPointOnArc.param[j]).val;
1452   }
1453
1454   // Radius of the arc
1455   std::shared_ptr<GeomAPI_Pnt2d> aCenter(new GeomAPI_Pnt2d(anArcPoints[0][0], anArcPoints[0][1]));
1456   std::shared_ptr<GeomAPI_Pnt2d> aStart(new GeomAPI_Pnt2d(anArcPoints[1][0], anArcPoints[1][1]));
1457   double aRadius = aCenter->distance(aStart);
1458
1459   // Update end point of the arc to be on a curve
1460   std::shared_ptr<GeomAPI_Pnt2d> anEnd(new GeomAPI_Pnt2d(anArcPoints[2][0], anArcPoints[2][1]));
1461   double aDistance = anEnd->distance(aCenter);
1462   std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
1463   if (aDistance < tolerance)
1464     aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
1465   else
1466     aDir = aDir->multiplied(aRadius / aDistance);
1467   double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
1468   const Slvs_Entity& aEndPoint = getEntity(theArc.point[2]);
1469   for (int i = 0; i < 2; i++) {
1470     Slvs_Param aParam = getParameter(aEndPoint.param[i]);
1471     aParam.val = xy[i];
1472     updateParameter(aParam);
1473   }
1474
1475   std::list<Slvs_Entity>::iterator aPtIt = aPointsToFix.begin();
1476   for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
1477     fixPoint(*aPtIt, theCreated);
1478
1479   if (isFixRadius) {
1480     // Fix radius of the arc
1481     bool isExists = false;
1482     std::vector<Slvs_Constraint>::iterator anIt = myConstraints.begin();
1483     for (; anIt != myConstraints.end() && !isExists; ++anIt)
1484       if (anIt->type == SLVS_C_DIAMETER && anIt->entityA == theArc.h)
1485         isExists = true;
1486     if (!isExists) {
1487       Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, theArc.group, SLVS_C_DIAMETER,
1488           theArc.wrkpl, aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, theArc.h, SLVS_E_UNKNOWN);
1489       aFixedR.h = addConstraint(aFixedR);
1490       theCreated.push_back(aFixedR.h);
1491     }
1492   }
1493 }
1494
1495
1496 bool SolveSpaceSolver_Storage::isAxisParallel(const Slvs_hEntity& theEntity) const
1497 {
1498   std::vector<Slvs_Constraint>::const_iterator anIter = myConstraints.begin();
1499   for (; anIter != myConstraints.end(); anIter++)
1500     if ((anIter->type == SLVS_C_HORIZONTAL || anIter->type == SLVS_C_VERTICAL) &&
1501         anIter->entityA == theEntity)
1502       return true;
1503   return false;
1504 }
1505
1506 bool SolveSpaceSolver_Storage::isUsedInEqual(
1507     const Slvs_hEntity& theEntity, Slvs_Constraint& theEqual) const
1508 {
1509   // Check the entity is used in Equal constraint
1510   std::vector<Slvs_Constraint>::const_iterator anEqIter = myConstraints.begin();
1511   for (; anEqIter != myConstraints.end(); anEqIter++)
1512     if ((anEqIter->type == SLVS_C_EQUAL_LENGTH_LINES ||
1513          anEqIter->type == SLVS_C_EQUAL_LINE_ARC_LEN ||
1514          anEqIter->type == SLVS_C_EQUAL_RADIUS) &&
1515        (anEqIter->entityA == theEntity || anEqIter->entityB == theEntity)) {
1516       theEqual = *anEqIter;
1517       return true;
1518     }
1519   return false;
1520 }
1521
1522 void SolveSpaceSolver_Storage::setTemporary(ConstraintPtr theConstraint)
1523 {
1524   // TODO
1525 }
1526
1527 bool SolveSpaceSolver_Storage::removeConstraint(ConstraintPtr theConstraint)
1528 {
1529   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::iterator
1530       aFound = myConstraintMap.find(theConstraint);
1531   if (aFound == myConstraintMap.end())
1532     return true; // no constraint, already deleted
1533
1534   // Remove constraint
1535   std::list<ConstraintWrapperPtr> aConstrList = aFound->second;
1536   myConstraintMap.erase(aFound);
1537   // Remove SolveSpace constraints
1538   bool isFullyRemoved = true;
1539   std::list<ConstraintWrapperPtr>::iterator anIt = aConstrList.begin();
1540   while (anIt != aConstrList.end()) {
1541     if (remove(*anIt)) {
1542       std::list<ConstraintWrapperPtr>::iterator aRemoveIt = anIt++;
1543       aConstrList.erase(aRemoveIt);
1544     } else {
1545       isFullyRemoved = false;
1546       ++anIt;
1547     }
1548   }
1549   if (!isFullyRemoved)
1550     myConstraintMap[theConstraint] = aConstrList;
1551   return isFullyRemoved;
1552 }
1553
1554 template <class ENT_TYPE>
1555 static bool isUsed(ConstraintWrapperPtr theConstraint, ENT_TYPE theEntity)
1556 {
1557   std::list<EntityWrapperPtr>::const_iterator anEntIt = theConstraint->entities().begin();
1558   for (; anEntIt != theConstraint->entities().end(); ++anEntIt)
1559     if (std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*anEntIt)->isBase(theEntity))
1560       return true;
1561   return false;
1562 }
1563
1564 static bool isUsed(EntityWrapperPtr theFeature, AttributePtr theSubEntity)
1565 {
1566   std::list<EntityWrapperPtr>::const_iterator aSubIt = theFeature->subEntities().begin();
1567   for (; aSubIt != theFeature->subEntities().end(); ++aSubIt)
1568     if (std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*aSubIt)->isBase(theSubEntity))
1569       return true;
1570   return false;
1571 }
1572
1573 bool SolveSpaceSolver_Storage::isUsed(FeaturePtr theFeature) const
1574 {
1575   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
1576       aCIt = myConstraintMap.begin();
1577   std::list<ConstraintWrapperPtr>::const_iterator aCWIt;
1578   for (; aCIt != myConstraintMap.end(); ++aCIt)
1579     for (aCWIt = aCIt->second.begin(); aCWIt != aCIt->second.end(); ++aCWIt)
1580       if (::isUsed(*aCWIt, theFeature))
1581         return true;
1582   // check attributes
1583   std::list<AttributePtr> anAttrList = theFeature->data()->attributes(GeomDataAPI_Point2D::typeId());
1584   std::list<AttributePtr>::const_iterator anIt = anAttrList.begin();
1585   for (; anIt != anAttrList.end(); ++anIt)
1586     if (isUsed(*anIt))
1587       return true;
1588   return false;
1589 }
1590
1591 bool SolveSpaceSolver_Storage::isUsed(AttributePtr theAttribute) const
1592 {
1593   AttributePtr anAttribute = theAttribute;
1594   AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(anAttribute);
1595   if (aRefAttr) {
1596     if (aRefAttr->isObject())
1597       return isUsed(ModelAPI_Feature::feature(aRefAttr->object()));
1598     else
1599       anAttribute = aRefAttr->attr();
1600   }
1601
1602   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
1603       aCIt = myConstraintMap.begin();
1604   std::list<ConstraintWrapperPtr>::const_iterator aCWIt;
1605   for (; aCIt != myConstraintMap.end(); ++aCIt)
1606     for (aCWIt = aCIt->second.begin(); aCWIt != aCIt->second.end(); ++aCWIt)
1607       if (::isUsed(*aCWIt, anAttribute))
1608         return true;
1609   return false;
1610 }
1611
1612
1613 bool SolveSpaceSolver_Storage::removeEntity(FeaturePtr theFeature)
1614 {
1615   std::map<FeaturePtr, EntityWrapperPtr>::iterator aFound = myFeatureMap.find(theFeature);
1616   if (aFound == myFeatureMap.end())
1617     return false; // feature not found, nothing to delete
1618
1619   // Check the feature is not used by constraints
1620   if (isUsed(theFeature))
1621     return false; // the feature is used, don't remove it
1622
1623   // Remove feature
1624   EntityWrapperPtr anEntity = aFound->second;
1625   myFeatureMap.erase(aFound);
1626   if (remove(anEntity))
1627     return true;
1628   // feature is not removed, revert operation
1629   myFeatureMap[theFeature] = anEntity;
1630   return false;
1631 }
1632
1633 bool SolveSpaceSolver_Storage::removeEntity(AttributePtr theAttribute)
1634 {
1635   std::map<AttributePtr, EntityWrapperPtr>::iterator aFound = myAttributeMap.find(theAttribute);
1636   if (aFound == myAttributeMap.end())
1637     return false; // attribute not found, nothing to delete
1638
1639   // Check the attribute is not used by constraints
1640   if (isUsed(theAttribute))
1641     return false; // the attribute is used, don't remove it
1642   // Check the attribute is not used by other features
1643   std::map<FeaturePtr, EntityWrapperPtr>::const_iterator aFIt = myFeatureMap.begin();
1644   for (; aFIt != myFeatureMap.end(); ++aFIt)
1645     if (::isUsed(aFIt->second, theAttribute)) // the attribute is used, don't remove it
1646       return false;
1647
1648   // Remove attribute
1649   EntityWrapperPtr anEntity = aFound->second;
1650   myAttributeMap.erase(aFound);
1651   if (remove(anEntity))
1652     return true;
1653   // attribute is not removed, revert operation
1654   myAttributeMap[theAttribute] = anEntity;
1655   return false;
1656 }
1657
1658
1659 bool SolveSpaceSolver_Storage::remove(ConstraintWrapperPtr theConstraint)
1660 {
1661   std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aConstraint =
1662       std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(theConstraint);
1663
1664   // verify whether the constraint has duplicated
1665   SameConstraintMap::iterator anEqIt = myEqualConstraints.begin();
1666   for (; anEqIt != myEqualConstraints.end(); ++anEqIt)
1667     if (anEqIt->find(aConstraint) != anEqIt->end()) {
1668       anEqIt->erase(aConstraint);
1669       break;
1670     }
1671   if (anEqIt != myEqualConstraints.end())
1672     return true;
1673
1674   bool isFullyRemoved = removeConstraint((Slvs_hConstraint)aConstraint->id());
1675
1676   std::list<EntityWrapperPtr>::const_iterator anIt = aConstraint->entities().begin();
1677   for (; anIt != aConstraint->entities().end(); ++anIt) {
1678     std::shared_ptr<SolveSpaceSolver_EntityWrapper> anEntity =
1679         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*anIt);
1680     FeaturePtr aBaseFeature = anEntity->baseFeature();
1681     if (aBaseFeature)
1682       isFullyRemoved = removeEntity(aBaseFeature) && isFullyRemoved;
1683     else
1684       isFullyRemoved = removeEntity(anEntity->baseAttribute()) && isFullyRemoved;
1685   }
1686
1687   return isFullyRemoved;
1688 }
1689
1690 bool SolveSpaceSolver_Storage::remove(EntityWrapperPtr theEntity)
1691 {
1692   std::shared_ptr<SolveSpaceSolver_EntityWrapper> anEntity =
1693         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theEntity);
1694   bool isFullyRemoved = removeEntity((Slvs_hEntity)anEntity->id());
1695
1696   std::list<EntityWrapperPtr>::const_iterator anEntIt = anEntity->subEntities().begin();
1697   for (; anEntIt != anEntity->subEntities().end(); ++anEntIt) {
1698     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aSubEntity =
1699         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*anEntIt);
1700     FeaturePtr aBaseFeature = aSubEntity->baseFeature();
1701     if (aBaseFeature)
1702       isFullyRemoved = removeEntity(aBaseFeature) && isFullyRemoved;
1703     else
1704       isFullyRemoved = removeEntity(aSubEntity->baseAttribute()) && isFullyRemoved;
1705   }
1706
1707   std::list<ParameterWrapperPtr>::const_iterator aParIt = anEntity->parameters().begin();
1708   for (; aParIt != anEntity->parameters().end(); ++aParIt)
1709     isFullyRemoved = remove(*aParIt) && isFullyRemoved;
1710   return isFullyRemoved;
1711 }
1712
1713 bool SolveSpaceSolver_Storage::remove(ParameterWrapperPtr theParameter)
1714 {
1715   return removeParameter((Slvs_hParam)theParameter->id());
1716 }
1717
1718
1719 void SolveSpaceSolver_Storage::refresh(bool theFixedOnly) const
1720 {
1721   blockEvents(true);
1722
1723   std::map<AttributePtr, EntityWrapperPtr>::const_iterator anIt = myAttributeMap.begin();
1724   std::list<ParameterWrapperPtr> aParams;
1725   std::list<ParameterWrapperPtr>::const_iterator aParIt;
1726   for (; anIt != myAttributeMap.end(); ++anIt) {
1727     // the external feature always should keep the up to date values, so,
1728     // refresh from the solver is never needed
1729     if (anIt->first.get()) {
1730       std::shared_ptr<SketchPlugin_Feature> aSketchFeature =
1731         std::dynamic_pointer_cast<SketchPlugin_Feature>(anIt->first->owner());
1732       if (aSketchFeature.get() && aSketchFeature->isExternal())
1733         continue;
1734     }
1735
1736     // update parameter wrappers and obtain values of attributes
1737     aParams = anIt->second->parameters();
1738     double aCoords[3];
1739     bool isUpd[3] = {false};
1740     int i = 0;
1741     for (aParIt = aParams.begin(); i < 3 && aParIt != aParams.end(); ++aParIt, ++i) {
1742       std::shared_ptr<SolveSpaceSolver_ParameterWrapper> aWrapper =
1743           std::dynamic_pointer_cast<SolveSpaceSolver_ParameterWrapper>(*aParIt);
1744       if (!theFixedOnly || aWrapper->group() == GID_OUTOFGROUP || aWrapper->isParametric()) {
1745         aWrapper->changeParameter().val = getParameter((Slvs_hParam)aWrapper->id()).val;
1746         aCoords[i] = aWrapper->value();
1747         isUpd[i] = true;
1748       }
1749     }
1750     if (!isUpd[0] && !isUpd[1] && !isUpd[2])
1751       continue; // nothing is updated
1752
1753     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1754         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anIt->first);
1755     if (aPoint2D) {
1756       if ((isUpd[0] && fabs(aPoint2D->x() - aCoords[0]) > tolerance) ||
1757           (isUpd[1] && fabs(aPoint2D->y() - aCoords[1]) > tolerance)) {
1758         if (!isUpd[0]) aCoords[0] = aPoint2D->x();
1759         if (!isUpd[1]) aCoords[1] = aPoint2D->y();
1760         aPoint2D->setValue(aCoords[0], aCoords[1]);
1761         // Find points coincident with this one (probably not in GID_OUTOFGROUP)
1762         std::map<AttributePtr, EntityWrapperPtr>::const_iterator aLocIt =
1763             theFixedOnly ? myAttributeMap.begin() : anIt;
1764         for (++aLocIt; aLocIt != myAttributeMap.end(); ++aLocIt)
1765           if (anIt->second->id() == aLocIt->second->id()) {
1766             aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(aLocIt->first);
1767             aPoint2D->setValue(aCoords[0], aCoords[1]);
1768           }
1769       }
1770       continue;
1771     }
1772     AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anIt->first);
1773     if (aScalar) {
1774       if (isUpd[0] && fabs(aScalar->value() - aCoords[0]) > tolerance)
1775         aScalar->setValue(aCoords[0]);
1776       continue;
1777     }
1778     std::shared_ptr<GeomDataAPI_Point> aPoint =
1779         std::dynamic_pointer_cast<GeomDataAPI_Point>(anIt->first);
1780     if (aPoint) {
1781       if ((isUpd[0] && fabs(aPoint->x() - aCoords[0]) > tolerance) ||
1782           (isUpd[1] && fabs(aPoint->y() - aCoords[1]) > tolerance) ||
1783           (isUpd[2] && fabs(aPoint->z() - aCoords[2]) > tolerance))
1784         if (!isUpd[0]) aCoords[0] = aPoint->x();
1785         if (!isUpd[1]) aCoords[1] = aPoint->y();
1786         if (!isUpd[2]) aCoords[2] = aPoint->z();
1787         aPoint->setValue(aCoords[0], aCoords[1], aCoords[2]);
1788       continue;
1789     }
1790   }
1791
1792   blockEvents(false);
1793 }
1794
1795 void SolveSpaceSolver_Storage::verifyFixed()
1796 {
1797   std::map<AttributePtr, EntityWrapperPtr>::iterator anAttrIt = myAttributeMap.begin();
1798   for (; anAttrIt != myAttributeMap.end(); ++anAttrIt) {
1799     const std::list<ParameterWrapperPtr>& aParameters = anAttrIt->second->parameters();
1800     std::list<ParameterWrapperPtr>::const_iterator aParIt = aParameters.begin();
1801     for (; aParIt != aParameters.end(); ++aParIt)
1802       if ((*aParIt)->group() == GID_OUTOFGROUP) {
1803         Slvs_Param aParam = getParameter((Slvs_hParam)(*aParIt)->id());
1804         if (aParam.group != (Slvs_hParam)GID_OUTOFGROUP) {
1805           aParam.group = (Slvs_hParam)GID_OUTOFGROUP;
1806           updateParameter(aParam);
1807         }
1808       }
1809   }
1810 }
1811
1812
1813 void SolveSpaceSolver_Storage::adjustArc(const Slvs_Entity& theArc)
1814 {
1815   double anArcPoints[3][2];
1816   double aDist[3] = {0.0};
1817   bool isFixed[3] = {false};
1818   for (int i = 0; i < 3; ++i) {
1819     Slvs_Entity aPoint = getEntity(theArc.point[i]);
1820     isFixed[i] = (aPoint.group != (Slvs_hGroup)myGroupID);
1821     anArcPoints[i][0] = getParameter(aPoint.param[0]).val;
1822     anArcPoints[i][1] = getParameter(aPoint.param[1]).val;
1823     if (i > 0) {
1824       anArcPoints[i][0] -= anArcPoints[0][0];
1825       anArcPoints[i][1] -= anArcPoints[0][1];
1826       aDist[i] = sqrt(anArcPoints[i][0] * anArcPoints[i][0] +
1827                       anArcPoints[i][1] * anArcPoints[i][1]);
1828     }
1829   }
1830
1831   if (fabs(aDist[1] - aDist[2]) < tolerance)
1832     return;
1833
1834   int anInd = 2;
1835   while (anInd > 0 && isFixed[anInd])
1836     --anInd;
1837   if (anInd < 1)
1838     return; // adjust only start or end point of the arc
1839
1840   anArcPoints[anInd][0] /= aDist[anInd];
1841   anArcPoints[anInd][1] /= aDist[anInd];
1842
1843   Slvs_Entity aPoint = getEntity(theArc.point[anInd]);
1844   for (int i = 0; i < 2; ++i) {
1845     Slvs_Param aParam = getParameter(aPoint.param[i]);
1846     aParam.val = anArcPoints[0][i] + aDist[3-anInd] * anArcPoints[anInd][i];
1847     updateParameter(aParam);
1848   }
1849 }
1850
1851
1852
1853
1854
1855
1856
1857 // ========================================================
1858 // =========      Auxiliary functions       ===============
1859 // ========================================================
1860
1861 template<typename T>
1862 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
1863 {
1864   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
1865   int aVecSize = theEntities.size();
1866   if (theEntities.empty())
1867     return 1;
1868   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
1869     aResIndex--;
1870   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
1871     aResIndex++;
1872   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
1873     aResIndex = aVecSize;
1874   return aResIndex;
1875 }
1876
1877 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
1878 {
1879   return fabs(theParam1.val - theParam2.val) > tolerance;
1880 }
1881
1882 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
1883 {
1884   int i = 0;
1885   for (; theEntity1.param[i] != 0 && i < 4; i++)
1886     if (theEntity1.param[i] != theEntity2.param[i])
1887       return true;
1888   i = 0;
1889   for (; theEntity1.point[i] != 0 && i < 4; i++)
1890     if (theEntity1.point[i] != theEntity2.point[i])
1891       return true;
1892   return false;
1893 }
1894
1895 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
1896 {
1897   return theConstraint1.ptA != theConstraint2.ptA ||
1898          theConstraint1.ptB != theConstraint2.ptB ||
1899          theConstraint1.entityA != theConstraint2.entityA ||
1900          theConstraint1.entityB != theConstraint2.entityB ||
1901          theConstraint1.entityC != theConstraint2.entityC ||
1902          theConstraint1.entityD != theConstraint2.entityD ||
1903          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
1904 }