src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Storage.cpp

   1 // Copyright (C) 2014-2017  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or
  18 // email : webmaster.salome@opencascade.com<mailto:webmaster.salome@opencascade.com>
  19 //
  20
  21 #include <SolveSpaceSolver_Storage.h>
  22 #include <SolveSpaceSolver_ConstraintWrapper.h>
  23 #include <SolveSpaceSolver_EntityWrapper.h>
  24 #include <SolveSpaceSolver_ParameterWrapper.h>
  25 #include <SolveSpaceSolver_Builder.h>
  26
  27 #include <GeomAPI_Dir2d.h>
  28 #include <GeomAPI_Pnt2d.h>
  29 #include <GeomAPI_XY.h>
  30 #include <math.h>
  31 #include <assert.h>
  32
  33 #include <GeomDataAPI_Point.h>
  34 #include <GeomDataAPI_Point2D.h>
  35 #include <ModelAPI_AttributeDouble.h>
  36 #include <ModelAPI_AttributeRefAttr.h>
  37 #include <SketchPlugin_Arc.h>
  38 #include <SketchPlugin_ConstraintMiddle.h>
  39
  40 /** \brief Search the entity/parameter with specified ID in the list of elements
  41  *  \param[in] theEntityID unique ID of the element
  42  *  \param[in] theEntities list of elements
  43  *  \return position of the found element or -1 if the element is not found
  44  */
  45 template<typename T>
  46 static int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities);
  47
  48 /// \brief Compare two parameters to be different
  49 static bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2);
  50 /// \brief Compare two entities to be different
  51 static bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2);
  52 /// \brief Compare two constraints to be different
  53 static bool IsNotEqual(const Slvs_Constraint& theConstraint1,
  54                        const Slvs_Constraint& theConstraint2);
  55
  56
  57 SolveSpaceSolver_Storage::SolveSpaceSolver_Storage(const GroupID& theGroup)
  58   : SketchSolver_Storage(theGroup),
  59     myWorkplaneID(SLVS_E_UNKNOWN),
  60     myParamMaxID(SLVS_E_UNKNOWN),
  61     myEntityMaxID(SLVS_E_UNKNOWN),
  62     myConstrMaxID(SLVS_C_UNKNOWN),
  63     myDuplicatedConstraint(false)
  64 {
  65 }
  66
  67 bool SolveSpaceSolver_Storage::update(ConstraintWrapperPtr theConstraint)
  68 {
  69   bool isUpdated = false;
  70   std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aConstraint =
  71       std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(theConstraint);
  72   Slvs_Constraint aSlvsConstr = getConstraint((Slvs_hConstraint)aConstraint->id());
  73   if (aSlvsConstr.h == SLVS_C_UNKNOWN)
  74     aSlvsConstr = aConstraint->constraint();
  75
  76   // update value of constraint if exist
  77   double aCoeff = aSlvsConstr.type == SLVS_C_DIAMETER ? 2.0 : 1.0;
  78   if (fabs(aSlvsConstr.valA - theConstraint->value() * aCoeff) > tolerance) {
  79     aSlvsConstr.valA = theConstraint->value() * aCoeff;
  80     isUpdated = true;
  81   }
  82
  83   // update constrained entities
  84   Slvs_hEntity* aPnts[2] = {&aSlvsConstr.ptA, &aSlvsConstr.ptB};
  85   Slvs_hEntity* anEnts[4] = {&aSlvsConstr.entityA, &aSlvsConstr.entityB,
  86                              &aSlvsConstr.entityC, &aSlvsConstr.entityD};
  87   int aPtInd = 0;
  88   int aEntInd = 0;
  89
  90   std::list<EntityWrapperPtr> anEntities = theConstraint->entities();
  91   std::list<EntityWrapperPtr>::iterator anIt = anEntities.begin();
  92   for (; anIt != anEntities.end(); ++anIt) {
  93     isUpdated = update(*anIt) || isUpdated;
  94     // do not update constrained entities for Multi constraints,
  95     // and for middle point constraint translated to equal lines
  96     ConstraintPtr aBaseConstraint = theConstraint->baseConstraint();
  97     if (aSlvsConstr.type == SLVS_C_MULTI_ROTATION || aSlvsConstr.type == SLVS_C_MULTI_TRANSLATION ||
  98        (aBaseConstraint && aBaseConstraint->getKind() == SketchPlugin_ConstraintMiddle::ID() &&
  99         aSlvsConstr.type != SLVS_C_AT_MIDPOINT))
 100       continue;
 101
 102     Slvs_hEntity anID = (Slvs_hEntity)(*anIt)->id();
 103     if ((*anIt)->type() == ENTITY_POINT) {
 104       if (*(aPnts[aPtInd]) != anID) {
 105         *(aPnts[aPtInd]) = anID;
 106         isUpdated = true;
 107       }
 108       ++aPtInd;
 109     } else {
 110       if (*(anEnts[aEntInd]) != anID) {
 111         *(anEnts[aEntInd]) = anID;
 112         isUpdated = true;
 113       }
 114       ++aEntInd;
 115     }
 116   }
 117
 118   // update constraint itself (do not update constraints Multi)
 119   if (aSlvsConstr.type != SLVS_C_MULTI_ROTATION && aSlvsConstr.type != SLVS_C_MULTI_TRANSLATION) {
 120     if (aSlvsConstr.wrkpl == SLVS_E_UNKNOWN && myWorkplaneID != SLVS_E_UNKNOWN)
 121       aSlvsConstr.wrkpl = myWorkplaneID;
 122     if (aSlvsConstr.group == SLVS_G_UNKNOWN)
 123       aSlvsConstr.group = (Slvs_hGroup)myGroupID;
 124     bool hasDupConstraints = myDuplicatedConstraint;
 125     Slvs_hConstraint aConstrID = updateConstraint(aSlvsConstr);
 126     if (aSlvsConstr.h == SLVS_C_UNKNOWN) {
 127       aConstraint->changeConstraint() = getConstraint(aConstrID);
 128       isUpdated = true;
 129       // check duplicated constraints based on different attributes
 130       if (myDuplicatedConstraint && findSameConstraint(aConstraint) && !hasDupConstraints)
 131         myDuplicatedConstraint = false;
 132     }
 133   }
 134   return isUpdated;
 135 }
 136
 137 bool SolveSpaceSolver_Storage::update(EntityWrapperPtr theEntity)
 138 {
 139   bool isUpdated = false;
 140   std::shared_ptr<SolveSpaceSolver_EntityWrapper> anEntity =
 141       std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theEntity);
 142   Slvs_Entity aSlvsEnt = getEntity((Slvs_hEntity)anEntity->id());
 143   if (aSlvsEnt.h == SLVS_E_UNKNOWN)
 144     aSlvsEnt = anEntity->entity();
 145
 146   std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
 147   std::list<ParameterWrapperPtr>::iterator aPIt;
 148   // if the entity is an attribute, need to update its coordinates
 149   if (anEntity->baseAttribute()) {
 150     BuilderPtr aBuilder = SolveSpaceSolver_Builder::getInstance();
 151     EntityWrapperPtr anUpdAttr = aBuilder->createAttribute(anEntity->baseAttribute(), GID_UNKNOWN);
 152     if (anUpdAttr) {
 153       std::list<ParameterWrapperPtr> anUpdParams = anUpdAttr->parameters();
 154       std::list<ParameterWrapperPtr>::iterator anUpdIt = anUpdParams.begin();
 155       for (aPIt = aParams.begin(); aPIt != aParams.end() && anUpdIt != anUpdParams.end();
 156           ++aPIt, ++anUpdIt) {
 157         (*aPIt)->update(*anUpdIt);
 158       }
 159     }
 160   }
 161
 162   // update parameters
 163   int anInd = 0;
 164   for (aPIt = aParams.begin(); aPIt != aParams.end(); ++aPIt, ++anInd) {
 165     assert(anInd < 4);
 166     isUpdated = update(*aPIt) || isUpdated;
 167     if (aSlvsEnt.param[anInd] != (Slvs_hEntity)(*aPIt)->id()) {
 168       isUpdated = true;
 169       aSlvsEnt.param[anInd] = (Slvs_hEntity)(*aPIt)->id();
 170     }
 171   }
 172
 173   // update sub-entities
 174   std::list<EntityWrapperPtr> aSubEntities = theEntity->subEntities();
 175   std::list<EntityWrapperPtr>::iterator aSIt = aSubEntities.begin();
 176   for (anInd = 0; aSIt != aSubEntities.end(); ++aSIt, ++anInd) {
 177     assert(anInd < 4);
 178     isUpdated = update(*aSIt) || isUpdated;
 179
 180     Slvs_hEntity anID = Slvs_hEntity((*aSIt)->id());
 181     if ((*aSIt)->type() == ENTITY_NORMAL)
 182       aSlvsEnt.normal = anID;
 183     else if ((*aSIt)->type() == ENTITY_SCALAR)
 184       aSlvsEnt.distance = anID;
 185     else if (aSlvsEnt.point[anInd] != anID) {
 186       aSlvsEnt.point[anInd] = anID;
 187       if ((*aSIt)->baseAttribute())
 188         SketchSolver_Storage::addEntity((*aSIt)->baseAttribute(), *aSIt);
 189       isUpdated = true;
 190     }
 191   }
 192   if (theEntity->type() == ENTITY_POINT && aSubEntities.size() == 1) {
 193     // theEntity is based on SketchPlugin_Point => need to substitute its attribute instead
 194     bool isNew = (aSlvsEnt.h == SLVS_E_UNKNOWN);
 195     aSlvsEnt = getEntity(aSlvsEnt.point[0]);
 196     if (isNew) {
 197       anEntity->changeEntity() = aSlvsEnt;
 198       isUpdated = true;
 199     }
 200   }
 201
 202   // update entity itself
 203   if (aSlvsEnt.wrkpl == SLVS_E_UNKNOWN && myWorkplaneID != SLVS_E_UNKNOWN)
 204     aSlvsEnt.wrkpl = myWorkplaneID;
 205   if (aSlvsEnt.group == SLVS_G_UNKNOWN)
 206     aSlvsEnt.group = (Slvs_hGroup)myGroupID;
 207   Slvs_hEntity anEntID = updateEntity(aSlvsEnt);
 208   if (aSlvsEnt.h == SLVS_E_UNKNOWN || isUpdated) {
 209     anEntity->changeEntity() = getEntity(anEntID);
 210     isUpdated = true;
 211
 212     if (anEntity->type() == ENTITY_SKETCH)
 213       storeWorkplane(anEntity);
 214   }
 215
 216   return isUpdated;
 217 }
 218
 219 bool SolveSpaceSolver_Storage::update(ParameterWrapperPtr theParameter)
 220 {
 221   std::shared_ptr<SolveSpaceSolver_ParameterWrapper> aParameter =
 222       std::dynamic_pointer_cast<SolveSpaceSolver_ParameterWrapper>(theParameter);
 223   const Slvs_Param& aParam = getParameter((Slvs_hParam)aParameter->id());
 224   if (aParam.h != SLVS_E_UNKNOWN && fabs(aParam.val - aParameter->value()) < tolerance)
 225     return false;
 226   Slvs_Param aParamToUpd = aParameter->parameter();
 227   if (aParamToUpd.group == SLVS_G_UNKNOWN)
 228     aParamToUpd.group = aParameter->isParametric() ? (Slvs_hGroup)GID_OUTOFGROUP :
 229                                                      (Slvs_hGroup)myGroupID;
 230   Slvs_hParam anID = updateParameter(aParamToUpd);
 231   if (aParam.h == SLVS_E_UNKNOWN) // new parameter
 232     aParameter->changeParameter() = getParameter(anID);
 233   return true;
 234 }
 235
 236 void SolveSpaceSolver_Storage::storeWorkplane(EntityWrapperPtr theSketch)
 237 {
 238   mySketchID = theSketch->id();
 239   myWorkplaneID = (Slvs_hEntity)mySketchID;
 240
 241   // Update sub-entities of the sketch
 242   std::list<EntityWrapperPtr> aSubEntities = theSketch->subEntities();
 243   std::list<EntityWrapperPtr>::iterator aSIt = aSubEntities.begin();
 244   for (; aSIt != aSubEntities.end(); ++aSIt) {
 245     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aSub =
 246         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(*aSIt);
 247     aSub->changeEntity().wrkpl = myWorkplaneID;
 248   }
 249
 250   // Update all stored entities
 251   std::vector<Slvs_Entity>::iterator anIt = myEntities.begin();
 252   for (; anIt != myEntities.end(); ++anIt)
 253     anIt->wrkpl = myWorkplaneID;
 254 }
 255
 256 void SolveSpaceSolver_Storage::changeGroup(EntityWrapperPtr theEntity, const GroupID& theGroup)
 257 {
 258   std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
 259   std::list<ParameterWrapperPtr>::iterator aPIt = aParams.begin();
 260   for (; aPIt != aParams.end(); ++aPIt)
 261     changeGroup(*aPIt, theGroup);
 262
 263   std::list<EntityWrapperPtr> aSubs = theEntity->subEntities();
 264   std::list<EntityWrapperPtr>::iterator aSIt = aSubs.begin();
 265   for (; aSIt != aSubs.end(); ++aSIt)
 266     changeGroup(*aSIt, theGroup);
 267
 268   theEntity->setGroup(theGroup);
 269   int aPos = Search((Slvs_hEntity)theEntity->id(), myEntities);
 270   if (aPos >= 0 && aPos < (int)myEntities.size()) {
 271     if (myEntities[aPos].group != (Slvs_hGroup)theGroup)
 272       setNeedToResolve(true);
 273     myEntities[aPos].group = (Slvs_hGroup)theGroup;
 274   }
 275 }
 276
 277 void SolveSpaceSolver_Storage::changeGroup(ParameterWrapperPtr theParam, const GroupID& theGroup)
 278 {
 279   GroupID aGroup = theGroup;
 280   if (theParam->isParametric())
 281     aGroup = GID_OUTOFGROUP;
 282   if (theParam->group() == aGroup)
 283     return;
 284
 285   theParam->setGroup(aGroup);
 286   int aPos = Search((Slvs_hParam)theParam->id(), myParameters);
 287   if (aPos >= 0 && aPos < (int)myParameters.size()) {
 288     myParameters[aPos].group = (Slvs_hGroup)aGroup;
 289     setNeedToResolve(true);
 290   }
 291 }
 292
 293 void SolveSpaceSolver_Storage::addCoincidentPoints(
 294     EntityWrapperPtr theMaster, EntityWrapperPtr theSlave)
 295 {
 296   if (theMaster->type() != ENTITY_POINT || theSlave->type() != ENTITY_POINT)
 297     return;
 298   if (!theMaster->subEntities().empty() || !theSlave->subEntities().empty()) {
 299     EntityWrapperPtr aSubMaster = theMaster->subEntities().empty() ?
 300         theMaster : theMaster->subEntities().front();
 301     EntityWrapperPtr aSubSlave = theSlave->subEntities().empty() ?
 302         theSlave : theSlave->subEntities().front();
 303     return addCoincidentPoints(aSubMaster, aSubSlave);
 304   }
 305
 306   // Search available coincidence
 307   CoincidentPointsMap::iterator aMasterFound = myCoincidentPoints.find(theMaster);
 308   CoincidentPointsMap::iterator aSlaveFound = myCoincidentPoints.find(theSlave);
 309   if (aMasterFound == myCoincidentPoints.end() || aSlaveFound == myCoincidentPoints.end()) {
 310     // try to find master and slave points in the lists of slaves of already existent coincidences
 311     CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
 312     for (; anIt != myCoincidentPoints.end(); ++anIt) {
 313       if (anIt->second.find(theMaster) != anIt->second.end())
 314         aMasterFound = anIt;
 315       else if (anIt->second.find(theSlave) != anIt->second.end())
 316         aSlaveFound = anIt;
 317
 318       if (aMasterFound != myCoincidentPoints.end() &&  aSlaveFound != myCoincidentPoints.end())
 319         break;
 320     }
 321   }
 322
 323   if (aMasterFound == myCoincidentPoints.end()) {
 324     // create new group
 325     myCoincidentPoints[theMaster] = std::set<EntityWrapperPtr>();
 326     aMasterFound = myCoincidentPoints.find(theMaster);
 327   } else if (aMasterFound == aSlaveFound)
 328     return; // already coincident
 329
 330   if (aSlaveFound != myCoincidentPoints.end()) {
 331     // A slave has been found, we need to attach all points coincident with it to the new master
 332     std::set<EntityWrapperPtr> aNewSlaves = aSlaveFound->second;
 333     aNewSlaves.insert(aSlaveFound->first);
 334     myCoincidentPoints.erase(aSlaveFound);
 335
 336     std::set<EntityWrapperPtr>::const_iterator aSlIt = aNewSlaves.begin();
 337     for (; aSlIt != aNewSlaves.end(); ++aSlIt)
 338       addCoincidentPoints(theMaster, *aSlIt);
 339   } else {
 340     // Update the slave if it was used in constraints and features
 341     replaceInFeatures(theSlave, theMaster);
 342     replaceInConstraints(theSlave, theMaster);
 343
 344     // Remove slave entity (if the IDs are equal no need to remove slave entity, just update it)
 345     if (theMaster->id() != theSlave->id())
 346       removeEntity((Slvs_hEntity)theSlave->id());
 347
 348     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aPointMaster =
 349         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theMaster);
 350     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aPointSlave =
 351         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theSlave);
 352     aPointSlave->changeEntity() = aPointMaster->entity();
 353     aPointSlave->setParameters(aPointMaster->parameters());
 354
 355     aMasterFound->second.insert(theSlave);
 356   }
 357 }
 358
 359 void SolveSpaceSolver_Storage::replaceInFeatures(
 360     EntityWrapperPtr theSource, EntityWrapperPtr theDest)
 361 {
 362   std::map<FeaturePtr, EntityWrapperPtr>::const_iterator anIt = myFeatureMap.begin();
 363   for (; anIt != myFeatureMap.end(); ++anIt) {
 364     if (!anIt->second)
 365       continue;
 366     bool isUpdated = false;
 367     std::list<EntityWrapperPtr> aSubs = anIt->second->subEntities();
 368     std::list<EntityWrapperPtr>::iterator aSubIt = aSubs.begin();
 369     for (; aSubIt != aSubs.end(); ++aSubIt)
 370       if ((*aSubIt)->id() == theSource->id()) {
 371         (*aSubIt)->update(theDest);
 372         isUpdated = true;
 373       }
 374
 375     if (!isUpdated)
 376       continue;
 377
 378     std::shared_ptr<SolveSpaceSolver_EntityWrapper> aWrapper =
 379         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(anIt->second);
 380     // update SolveSpace entity
 381     Slvs_Entity anEnt = aWrapper->entity();
 382     for (int i = 0; i < 4; ++i)
 383       if (anEnt.point[i] == (Slvs_hEntity)theSource->id())
 384         anEnt.point[i] = (Slvs_hEntity)theDest->id();
 385     anEnt.h = updateEntity(anEnt);
 386     aWrapper->changeEntity() = anEnt;
 387
 388     // update sub-entities
 389     aWrapper->setSubEntities(aSubs);
 390   }
 391 }
 392
 393 void SolveSpaceSolver_Storage::replaceInConstraints(
 394     EntityWrapperPtr theSource, EntityWrapperPtr theDest)
 395 {
 396   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
 397       anIt = myConstraintMap.begin();
 398   std::list<ConstraintWrapperPtr>::const_iterator aCIt;
 399   for (; anIt != myConstraintMap.end(); ++anIt)
 400     for (aCIt = anIt->second.begin(); aCIt != anIt->second.end(); ++aCIt) {
 401       // Do not process coincidence between points and "multi" constraints
 402       // (these constraints are stored to keep the structure of constraints).
 403       if ((*aCIt)->type() == CONSTRAINT_PT_PT_COINCIDENT ||
 404           (*aCIt)->type() == CONSTRAINT_MULTI_ROTATION ||
 405           (*aCIt)->type() == CONSTRAINT_MULTI_TRANSLATION)
 406         continue;
 407
 408       bool isUpdated = false;
 409       std::list<EntityWrapperPtr> aSubs = (*aCIt)->entities();
 410       std::list<EntityWrapperPtr>::iterator aSubIt = aSubs.begin();
 411       for (; aSubIt != aSubs.end(); ++aSubIt)
 412         if ((*aSubIt)->id() == theSource->id()) {
 413           (*aSubIt)->update(theDest);
 414           isUpdated = true;
 415         }
 416
 417       if (!isUpdated)
 418         continue;
 419
 420       std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aWrapper =
 421           std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(*aCIt);
 422       if (theSource->id() == theDest->id()) {
 423         // No need to update SolveSpace constraint if the entities are the same
 424         aWrapper->changeConstraint() = getConstraint((Slvs_hConstraint)aWrapper->id());
 425         aWrapper->setEntities(aSubs);
 426         continue;
 427       }
 428
 429       // change constraint entities
 430       Slvs_Constraint aConstr = aWrapper->constraint();
 431       if (aConstr.ptA == (Slvs_hEntity)theSource->id())
 432         aConstr.ptA = (Slvs_hEntity)theDest->id();
 433       if (aConstr.ptB == (Slvs_hEntity)theSource->id())
 434         aConstr.ptB = (Slvs_hEntity)theDest->id();
 435
 436       // check the constraint is duplicated
 437       std::vector<Slvs_Constraint>::const_iterator aSlvsCIt = myConstraints.begin();
 438       for (; aSlvsCIt != myConstraints.end(); ++aSlvsCIt)
 439         if (aConstr.h != aSlvsCIt->h &&
 440             aConstr.type == aSlvsCIt->type &&
 441             aConstr.ptA == aSlvsCIt->ptA && aConstr.ptB == aSlvsCIt->ptB &&
 442             aConstr.entityA == aSlvsCIt->entityA && aConstr.entityB == aSlvsCIt->entityB &&
 443             aConstr.entityC == aSlvsCIt->entityC && aConstr.entityD == aSlvsCIt->entityD) {
 444           Slvs_hConstraint anIDToRemove = aConstr.h;
 445           aConstr = *aSlvsCIt;
 446           int aShift = (int)(aSlvsCIt - myConstraints.begin());
 447           removeConstraint(anIDToRemove);
 448           aSlvsCIt = myConstraints.begin() + aShift - 1;
 449           for (; aSlvsCIt != myConstraints.end(); ++aSlvsCIt)
 450             if (aSlvsCIt->h == aConstr.h)
 451               break;
 452           break;
 453         }
 454
 455       if (aSlvsCIt != myConstraints.end()) {
 456         // constraint is duplicated, search its wrapper to add the mapping
 457         std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
 458             anIt2 = myConstraintMap.begin();
 459         std::list<ConstraintWrapperPtr>::const_iterator aCIt2;
 460         for (; anIt2 != myConstraintMap.end(); ++anIt2)
 461           for (aCIt2 = anIt2->second.begin(); aCIt2 != anIt2->second.end(); ++aCIt2)
 462             if ((Slvs_hConstraint)(*aCIt2)->id() == aConstr.h) {
 463               addSameConstraints(*aCIt2, aWrapper);
 464               break;
 465             }
 466       } else
 467         aConstr.h = updateConstraint(aConstr);
 468       aWrapper->changeConstraint() = aConstr;
 469
 470       // update sub-entities
 471       aWrapper->setEntities(aSubs);
 472     }
 473 }
 474
 475 void SolveSpaceSolver_Storage::addSameConstraints(ConstraintWrapperPtr theConstraint1,
 476                                                   ConstraintWrapperPtr theConstraint2)
 477 {
 478   SameConstraintMap::iterator anIt = myEqualConstraints.begin();
 479   for (; anIt != myEqualConstraints.end(); ++anIt) {
 480     if (anIt->find(theConstraint1) != anIt->end()) {
 481       anIt->insert(theConstraint2);
 482       return;
 483     }
 484     else if (anIt->find(theConstraint2) != anIt->end()) {
 485       anIt->insert(theConstraint1);
 486       return;
 487     }
 488   }
 489   // group not found => create new one
 490   std::set<ConstraintWrapperPtr> aNewGroup;
 491   aNewGroup.insert(theConstraint1);
 492   aNewGroup.insert(theConstraint2);
 493   myEqualConstraints.push_back(aNewGroup);
 494 }
 495
 496 bool SolveSpaceSolver_Storage::findSameConstraint(ConstraintWrapperPtr theConstraint)
 497 {
 498   if (theConstraint->type() == CONSTRAINT_PT_PT_COINCIDENT ||
 499       theConstraint->type() == CONSTRAINT_MULTI_ROTATION ||
 500       theConstraint->type() == CONSTRAINT_MULTI_TRANSLATION)
 501     return false;
 502
 503   const Slvs_Constraint& aCBase =
 504       std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(theConstraint)->constraint();
 505
 506   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
 507       aCIt = myConstraintMap.begin();
 508   std::list<ConstraintWrapperPtr>::const_iterator aCWIt;
 509   for (; aCIt != myConstraintMap.end(); ++aCIt)
 510     for (aCWIt = aCIt->second.begin(); aCWIt != aCIt->second.end(); ++aCWIt) {
 511       if ((*aCWIt)->type() == CONSTRAINT_PT_PT_COINCIDENT ||
 512           (*aCWIt)->type() == CONSTRAINT_MULTI_ROTATION ||
 513           (*aCWIt)->type() == CONSTRAINT_MULTI_TRANSLATION)
 514         continue;
 515       if ((*aCWIt)->type() == theConstraint->type()) {
 516         const Slvs_Constraint& aCComp = getConstraint((Slvs_hConstraint)(*aCWIt)->id());
 517
 518         if (aCBase.ptA == aCComp.ptA && aCBase.ptB == aCComp.ptB &&
 519             aCBase.entityA == aCComp.entityA && aCBase.entityB == aCComp.entityB &&
 520             aCBase.entityC == aCComp.entityC && aCBase.entityD == aCComp.entityD &&
 521             fabs(aCBase.valA -aCComp.valA) < tolerance) {
 522           addSameConstraints(*aCWIt, theConstraint);
 523           return true;
 524         }
 525       }
 526     }
 527   return false;
 528 }
 529
 530
 531 EntityWrapperPtr SolveSpaceSolver_Storage::calculateMiddlePoint(
 532     EntityWrapperPtr theBase, double theCoeff)
 533 {
 534   BuilderPtr aBuilder = SolveSpaceSolver_Builder::getInstance();
 535
 536   std::shared_ptr<GeomAPI_XY> aMidPoint;
 537   if (theBase->type() == ENTITY_LINE) {
 538     std::shared_ptr<GeomAPI_Pnt2d> aPoints[2];
 539     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
 540     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
 541     for (int i = 0; i < 2; ++i, ++anIt)
 542       aPoints[i] = aBuilder->point(*anIt);
 543     aMidPoint = aPoints[0]->xy()->multiplied(1.0 - theCoeff)->added(
 544         aPoints[1]->xy()->multiplied(theCoeff));
 545   }
 546   else if (theBase->type() == ENTITY_ARC) {
 547     double theX, theY;
 548     double anArcPoint[3][2];
 549     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
 550     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
 551     for (int i = 0; i < 3; ++i, ++anIt) {
 552       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(*anIt);
 553       anArcPoint[i][0] = aPoint->x();
 554       anArcPoint[i][1] = aPoint->y();
 555     }
 556     // project last point of arc on the arc
 557     double x = anArcPoint[1][0] - anArcPoint[0][0];
 558     double y = anArcPoint[1][1] - anArcPoint[0][1];
 559     double aRad = sqrt(x*x + y*y);
 560     x = anArcPoint[2][0] - anArcPoint[0][0];
 561     y = anArcPoint[2][1] - anArcPoint[0][1];
 562     double aNorm = sqrt(x*x + y*y);
 563     if (aNorm >= tolerance) {
 564       anArcPoint[2][0] = x * aRad / aNorm;
 565       anArcPoint[2][1] = y * aRad / aNorm;
 566     }
 567     anArcPoint[1][0] -= anArcPoint[0][0];
 568     anArcPoint[1][1] -= anArcPoint[0][1];
 569     if (theCoeff < tolerance) {
 570       theX = anArcPoint[0][0] + anArcPoint[1][0];
 571       theY = anArcPoint[0][1] + anArcPoint[1][1];
 572     } else if (1 - theCoeff < tolerance) {
 573       theX = anArcPoint[0][0] + anArcPoint[2][0];
 574       theY = anArcPoint[0][1] + anArcPoint[2][1];
 575     } else {
 576       std::shared_ptr<GeomAPI_Dir2d>
 577         aStartDir(new GeomAPI_Dir2d(anArcPoint[1][0], anArcPoint[1][1]));
 578       std::shared_ptr<GeomAPI_Dir2d>
 579         aEndDir(new GeomAPI_Dir2d(anArcPoint[2][0], anArcPoint[2][1]));
 580       double anAngle = aStartDir->angle(aEndDir);
 581       if (anAngle < 0)
 582         anAngle += 2.0 * PI;
 583       anAngle *= theCoeff;
 584       double aCos = cos(anAngle);
 585       double aSin = sin(anAngle);
 586       theX = anArcPoint[0][0] + anArcPoint[1][0] * aCos - anArcPoint[1][1] * aSin;
 587       theY = anArcPoint[0][1] + anArcPoint[1][0] * aSin + anArcPoint[1][1] * aCos;
 588     }
 589     aMidPoint = std::shared_ptr<GeomAPI_XY>(new GeomAPI_XY(theX, theY));
 590   }
 591
 592   if (!aMidPoint)
 593     return EntityWrapperPtr();
 594
 595   std::list<ParameterWrapperPtr> aParameters;
 596   Slvs_Param aParam1 = Slvs_MakeParam(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID, aMidPoint->x());
 597   aParam1.h = addParameter(aParam1);
 598   aParameters.push_back(ParameterWrapperPtr(new SolveSpaceSolver_ParameterWrapper(aParam1)));
 599   Slvs_Param aParam2 = Slvs_MakeParam(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID, aMidPoint->y());
 600   aParam2.h = addParameter(aParam2);
 601   aParameters.push_back(ParameterWrapperPtr(new SolveSpaceSolver_ParameterWrapper(aParam2)));
 602   // Create entity (parameters are not filled)
 603   Slvs_Entity anEntity = Slvs_MakePoint2d(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID,
 604       (Slvs_hEntity)myWorkplaneID, aParam1.h, aParam2.h);
 605   anEntity.h = addEntity(anEntity);
 606
 607   EntityWrapperPtr aResult(new SolveSpaceSolver_EntityWrapper(AttributePtr(), anEntity));
 608   aResult->setParameters(aParameters);
 609   return aResult;
 610 }
 611
 612
 613
 614
 615
 616
 617 Slvs_hParam SolveSpaceSolver_Storage::addParameter(const Slvs_Param& theParam)
 618 {
 619   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
 620     // parameter is already used, rewrite it
 621     return updateParameter(theParam);
 622   }
 623
 624   Slvs_Param aParam = theParam;
 625   if (aParam.h > myParamMaxID)
 626     myParamMaxID = aParam.h;
 627   else
 628     aParam.h = ++myParamMaxID;
 629   myParameters.push_back(aParam);
 630   myNeedToResolve = true;
 631   return aParam.h;
 632 }
 633
 634 Slvs_hParam SolveSpaceSolver_Storage::updateParameter(const Slvs_Param& theParam)
 635 {
 636   if (theParam.h > 0 && theParam.h <= myParamMaxID) {
 637     // parameter already used, rewrite it
 638     int aPos = Search(theParam.h, myParameters);
 639     if (aPos >= 0 && aPos < (int)myParameters.size()) {
 640       if (IsNotEqual(myParameters[aPos], theParam))
 641         setNeedToResolve(true);
 642       myParameters[aPos] = theParam;
 643       return theParam.h;
 644     }
 645   }
 646
 647   // Parameter is not found, add new one
 648   Slvs_Param aParam = theParam;
 649   aParam.h = 0;
 650   return addParameter(aParam);
 651 }
 652
 653 bool SolveSpaceSolver_Storage::removeParameter(const Slvs_hParam& theParamID)
 654 {
 655   int aPos = Search(theParamID, myParameters);
 656   if (aPos >= 0 && aPos < (int)myParameters.size()) {
 657     // Firstly, search the parameter is not used elsewhere
 658     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 659     for (; anEntIter != myEntities.end(); anEntIter++) {
 660       for (int i = 0; i < 4; i++)
 661         if (anEntIter->param[i] == theParamID)
 662           return false;
 663     }
 664     // Remove parameter
 665     myParameters.erase(myParameters.begin() + aPos);
 666     myParamMaxID = myParameters.empty() ? SLVS_E_UNKNOWN : myParameters.back().h;
 667     myNeedToResolve = true;
 668   }
 669   return true;
 670 }
 671
 672 const Slvs_Param& SolveSpaceSolver_Storage::getParameter(const Slvs_hParam& theParamID) const
 673 {
 674   int aPos = Search(theParamID, myParameters);
 675   if (aPos >= 0 && aPos < (int)myParameters.size())
 676     return myParameters[aPos];
 677
 678   // Parameter is not found, return empty object
 679   static Slvs_Param aDummy;
 680   aDummy.h = 0;
 681   return aDummy;
 682 }
 683
 684
 685 Slvs_hEntity SolveSpaceSolver_Storage::addEntity(const Slvs_Entity& theEntity)
 686 {
 687   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 688     // Entity is already used, rewrite it
 689     return updateEntity(theEntity);
 690   }
 691
 692   Slvs_Entity aEntity = theEntity;
 693   if (aEntity.h > myEntityMaxID)
 694     myEntityMaxID = aEntity.h;
 695   else
 696     aEntity.h = ++myEntityMaxID;
 697   myEntities.push_back(aEntity);
 698   myNeedToResolve = true;
 699   return aEntity.h;
 700 }
 701
 702 Slvs_hEntity SolveSpaceSolver_Storage::updateEntity(const Slvs_Entity& theEntity)
 703 {
 704   if (theEntity.h > 0 && theEntity.h <= myEntityMaxID) {
 705     // Entity already used, rewrite it
 706     int aPos = Search(theEntity.h, myEntities);
 707     if (aPos >= 0 && aPos < (int)myEntities.size()) {
 708       myNeedToResolve = myNeedToResolve || IsNotEqual(myEntities[aPos], theEntity);
 709       myEntities[aPos] = theEntity;
 710       return theEntity.h;
 711     }
 712   }
 713
 714   // Entity is not found, add new one
 715   Slvs_Entity aEntity = theEntity;
 716   aEntity.h = 0;
 717   return addEntity(aEntity);
 718 }
 719
 720 bool SolveSpaceSolver_Storage::removeEntity(const Slvs_hEntity& theEntityID)
 721 {
 722   bool aResult = true;
 723   int aPos = Search(theEntityID, myEntities);
 724   if (aPos >= 0 && aPos < (int)myEntities.size()) {
 725     // Firstly, check the entity and its attributes is not used elsewhere
 726     std::set<Slvs_hEntity> anEntAndSubs;
 727     anEntAndSubs.insert(theEntityID);
 728     for (int i = 0; i < 4; i++)
 729       if (myEntities[aPos].point[i] != SLVS_E_UNKNOWN)
 730         anEntAndSubs.insert(myEntities[aPos].point[i]);
 731
 732     std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
 733     for (; anEntIter != myEntities.end(); anEntIter++) {
 734       if (anEntAndSubs.find(anEntIter->h) != anEntAndSubs.end())
 735         continue;
 736       for (int i = 0; i < 4; i++)
 737         if (anEntAndSubs.find(anEntIter->point[i]) != anEntAndSubs.end())
 738           return false;
 739       if (anEntAndSubs.find(anEntIter->distance) != anEntAndSubs.end())
 740         return false;
 741     }
 742     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 743     for (; aConstrIter != myConstraints.end(); aConstrIter++) {
 744       Slvs_hEntity anEntIDs[6] = {aConstrIter->ptA, aConstrIter->ptB,
 745           aConstrIter->entityA, aConstrIter->entityB,
 746           aConstrIter->entityC, aConstrIter->entityD};
 747       for (int i = 0; i < 6; i++)
 748         if (anEntAndSubs.find(anEntIDs[i]) != anEntAndSubs.end())
 749           return false;
 750     }
 751     // The entity is not used, remove it and its parameters
 752     Slvs_Entity anEntity = myEntities[aPos];
 753     myEntities.erase(myEntities.begin() + aPos);
 754     myEntityMaxID = myEntities.empty() ? SLVS_E_UNKNOWN : myEntities.back().h;
 755     if (anEntity.distance != SLVS_E_UNKNOWN)
 756       aResult = aResult && removeEntity(anEntity.distance);
 757     for (int i = 0; i < 4; i++)
 758       if (anEntity.param[i] != SLVS_E_UNKNOWN)
 759         aResult = removeParameter(anEntity.param[i]) && aResult;
 760     myNeedToResolve = true;
 761   }
 762   return aResult;
 763 }
 764
 765 const Slvs_Entity& SolveSpaceSolver_Storage::getEntity(const Slvs_hEntity& theEntityID) const
 766 {
 767   int aPos = Search(theEntityID, myEntities);
 768   if (aPos >= 0 && aPos < (int)myEntities.size())
 769     return myEntities[aPos];
 770
 771   // Entity is not found, return empty object
 772   static Slvs_Entity aDummy;
 773   aDummy.h = SLVS_E_UNKNOWN;
 774   return aDummy;
 775 }
 776
 777
 778 Slvs_hConstraint SolveSpaceSolver_Storage::addConstraint(const Slvs_Constraint& theConstraint)
 779 {
 780   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 781     // Constraint is already used, rewrite it
 782     return updateConstraint(theConstraint);
 783   }
 784
 785   Slvs_Constraint aConstraint = theConstraint;
 786
 787   // Find a constraint with same type uses same arguments to show user overconstraint situation
 788   std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 789   for (; aCIt != myConstraints.end(); aCIt++) {
 790     if (aConstraint.type != aCIt->type)
 791       continue;
 792     if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 793         aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 794         aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD) {
 795       myDuplicatedConstraint = true;
 796       break;
 797     }
 798   }
 799
 800   if (aConstraint.h > myConstrMaxID)
 801     myConstrMaxID = aConstraint.h;
 802   else
 803     aConstraint.h = ++myConstrMaxID;
 804   myConstraints.push_back(aConstraint);
 805   myNeedToResolve = true;
 806   return aConstraint.h;
 807 }
 808
 809 Slvs_hConstraint SolveSpaceSolver_Storage::updateConstraint(const Slvs_Constraint& theConstraint)
 810 {
 811   if (theConstraint.h > 0 && theConstraint.h <= myConstrMaxID) {
 812     // Constraint already used, rewrite it
 813     int aPos = Search(theConstraint.h, myConstraints);
 814     if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 815       myNeedToResolve = myNeedToResolve || IsNotEqual(myConstraints[aPos], theConstraint);
 816       myConstraints[aPos] = theConstraint;
 817       return theConstraint.h;
 818     }
 819   }
 820
 821   // Constraint is not found, add new one
 822   Slvs_Constraint aConstraint = theConstraint;
 823   aConstraint.h = 0;
 824
 825   // Firstly, check middle-point constraint conflicts with point-on-line
 826   if (aConstraint.type == SLVS_C_AT_MIDPOINT) {
 827     std::vector<Slvs_Constraint>::const_iterator anIt = myConstraints.begin();
 828     for (; anIt != myConstraints.end(); ++anIt)
 829       if (anIt->type == SLVS_C_PT_ON_LINE &&
 830           anIt->ptA == aConstraint.ptA &&
 831           anIt->entityA == aConstraint.entityA)
 832         break;
 833     if (anIt != myConstraints.end()) {
 834       // change the constraint to the lengths equality to avoid conflicts
 835       Slvs_Entity aLine = getEntity(aConstraint.entityA);
 836       Slvs_Entity aNewLine1 = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID,
 837           myWorkplaneID, aLine.point[0], aConstraint.ptA);
 838       aNewLine1.h = addEntity(aNewLine1);
 839       Slvs_Entity aNewLine2 = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID,
 840           myWorkplaneID, aLine.point[1], aConstraint.ptA);
 841       aNewLine2.h = addEntity(aNewLine2);
 842       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, (Slvs_hGroup)myGroupID,
 843           SLVS_C_EQUAL_LENGTH_LINES,
 844           myWorkplaneID, 0.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aNewLine1.h, aNewLine2.h);
 845     }
 846   }
 847
 848   return addConstraint(aConstraint);
 849 }
 850
 851 bool SolveSpaceSolver_Storage::removeConstraint(const Slvs_hConstraint& theConstraintID)
 852 {
 853   int aPos = Search(theConstraintID, myConstraints);
 854   if (aPos >= 0 && aPos < (int)myConstraints.size()) {
 855     Slvs_Constraint aConstraint = myConstraints[aPos];
 856     myConstraints.erase(myConstraints.begin() + aPos);
 857     myConstrMaxID = myConstraints.empty() ? SLVS_E_UNKNOWN : myConstraints.back().h;
 858     myNeedToResolve = true;
 859
 860     if (myDuplicatedConstraint) {
 861       // Find a constraint with same type uses same arguments
 862       std::vector<Slvs_Constraint>::iterator aCIt = myConstraints.begin();
 863       for (; aCIt != myConstraints.end(); aCIt++) {
 864         if (aConstraint.type != aCIt->type)
 865           continue;
 866         if (aConstraint.ptA == aCIt->ptA && aConstraint.ptB == aCIt->ptB &&
 867             aConstraint.entityA == aCIt->entityA && aConstraint.entityB == aCIt->entityB &&
 868             aConstraint.entityC == aCIt->entityC && aConstraint.entityD == aCIt->entityD) {
 869           myDuplicatedConstraint = false;
 870           break;
 871         }
 872       }
 873     }
 874   }
 875   return true;
 876 }
 877
 878 const Slvs_Constraint& SolveSpaceSolver_Storage::
 879   getConstraint(const Slvs_hConstraint& theConstraintID) const
 880 {
 881   int aPos = Search(theConstraintID, myConstraints);
 882   if (aPos >= 0 && aPos < (int)myConstraints.size())
 883     return myConstraints[aPos];
 884
 885   // Constraint is not found, return empty object
 886   static Slvs_Constraint aDummy;
 887   aDummy.h = 0;
 888   return aDummy;
 889 }
 890
 891
 892 void SolveSpaceSolver_Storage::initializeSolver(SolverPtr theSolver)
 893 {
 894   std::shared_ptr<SolveSpaceSolver_Solver> aSolver =
 895       std::dynamic_pointer_cast<SolveSpaceSolver_Solver>(theSolver);
 896   if (!aSolver)
 897     return;
 898
 899   if (myExistArc)
 900     processArcs();
 901
 902   if (myConstraints.empty()) {
 903     // Adjust all arc to place their points correctly
 904     std::vector<Slvs_Entity>::const_iterator anEntIt = myEntities.begin();
 905     for (; anEntIt != myEntities.end(); ++anEntIt)
 906       if (anEntIt->type == SLVS_E_ARC_OF_CIRCLE)
 907         adjustArc(*anEntIt);
 908   }
 909
 910   aSolver->setParameters(myParameters.data(), (int)myParameters.size());
 911   aSolver->setEntities(myEntities.data(), (int)myEntities.size());
 912   aSolver->setConstraints(myConstraints.data(), (int)myConstraints.size());
 913 }
 914
 915
 916 bool SolveSpaceSolver_Storage::remove(ConstraintWrapperPtr theConstraint)
 917 {
 918   std::shared_ptr<SolveSpaceSolver_ConstraintWrapper> aConstraint =
 919       std::dynamic_pointer_cast<SolveSpaceSolver_ConstraintWrapper>(theConstraint);
 920
 921   // verify whether the constraint has duplicated
 922   bool hasSameID = false;
 923   SameConstraintMap::iterator anEqIt = myEqualConstraints.begin();
 924   for (; anEqIt != myEqualConstraints.end(); ++anEqIt) {
 925     std::set<ConstraintWrapperPtr>::const_iterator aFound = anEqIt->find(aConstraint);
 926     if (aFound != anEqIt->end()) {
 927       // verify there is a constraint with same ID
 928       std::set<ConstraintWrapperPtr>::const_iterator anIt = anEqIt->begin();
 929       ConstraintID anID = (*aFound)->id();
 930       for (++anIt; anIt != anEqIt->end() && !hasSameID; ++anIt)
 931         if ((*anIt)->id() == anID && aFound != anIt)
 932           hasSameID = true;
 933       // erase constraint
 934       anEqIt->erase(aConstraint);
 935       break;
 936     }
 937   }
 938   if (anEqIt != myEqualConstraints.end() && hasSameID)
 939     return true;
 940
 941   bool isFullyRemoved = removeConstraint((Slvs_hConstraint)aConstraint->id());
 942   // remove point-point coincidence
 943   if (aConstraint->type() == CONSTRAINT_PT_PT_COINCIDENT)
 944     isFullyRemoved = removeCoincidence(theConstraint) && isFullyRemoved;
 945   return SketchSolver_Storage::remove(theConstraint) && isFullyRemoved;
 946 }
 947
 948 bool SolveSpaceSolver_Storage::remove(EntityWrapperPtr theEntity)
 949 {
 950   if (!theEntity)
 951     return false;
 952
 953   // Additional check for entity to be used in point-point coincidence
 954   bool isCoincide = false;
 955   if (theEntity->type() == ENTITY_POINT) {
 956     CoincidentPointsMap::const_iterator anIt = myCoincidentPoints.begin();
 957     std::set<EntityWrapperPtr>::const_iterator aCIt;
 958     for (; anIt != myCoincidentPoints.end(); ++anIt) {
 959       if (anIt->first == theEntity)
 960         break;
 961       for (aCIt = anIt->second.begin(); aCIt != anIt->second.end(); ++aCIt)
 962         if (*aCIt == theEntity)
 963           break;
 964       if (aCIt != anIt->second.end())
 965         break;
 966     }
 967     if (anIt != myCoincidentPoints.end()) {
 968       if (anIt->first != theEntity && isUsed(anIt->first->baseAttribute()))
 969         isCoincide = true;
 970       for (aCIt = anIt->second.begin(); !isCoincide && aCIt != anIt->second.end(); ++aCIt)
 971         if (*aCIt != theEntity && isUsed((*aCIt)->baseAttribute()))
 972           isCoincide = true;
 973     }
 974   }
 975
 976   std::shared_ptr<SolveSpaceSolver_EntityWrapper> anEntity =
 977         std::dynamic_pointer_cast<SolveSpaceSolver_EntityWrapper>(theEntity);
 978   bool isFullyRemoved = isCoincide ? true : removeEntity((Slvs_hEntity)anEntity->id());
 979   return (SketchSolver_Storage::remove(theEntity) || isCoincide) && isFullyRemoved;
 980 }
 981
 982 bool SolveSpaceSolver_Storage::remove(ParameterWrapperPtr theParameter)
 983 {
 984   return removeParameter((Slvs_hParam)theParameter->id());
 985 }
 986
 987
 988 void SolveSpaceSolver_Storage::refresh(bool theFixedOnly) const
 989 {
 990   //blockEvents(true);
 991
 992   std::map<AttributePtr, EntityWrapperPtr>::const_iterator anIt = myAttributeMap.begin();
 993   std::list<ParameterWrapperPtr> aParams;
 994   std::list<ParameterWrapperPtr>::const_iterator aParIt;
 995   for (; anIt != myAttributeMap.end(); ++anIt) {
 996     if (!anIt->second)
 997       continue;
 998     // the external feature always should keep the up to date values, so,
 999     // refresh from the solver is never needed
1000     if (anIt->first.get()) {
1001       std::shared_ptr<SketchPlugin_Feature> aSketchFeature =
1002         std::dynamic_pointer_cast<SketchPlugin_Feature>(anIt->first->owner());
1003       if (aSketchFeature.get() && aSketchFeature->isExternal())
1004         continue;
1005       // not need to refresh here sketch's origin and normal vector
1006       CompositeFeaturePtr aSketch =
1007           std::dynamic_pointer_cast<ModelAPI_CompositeFeature>(anIt->first->owner());
1008       if (aSketch)
1009         continue;
1010     }
1011
1012     // update parameter wrappers and obtain values of attributes
1013     aParams = anIt->second->parameters();
1014     double aCoords[3];
1015     bool isUpd[3] = {false};
1016     int i = 0;
1017     for (aParIt = aParams.begin(); i < 3 && aParIt != aParams.end(); ++aParIt, ++i) {
1018       std::shared_ptr<SolveSpaceSolver_ParameterWrapper> aWrapper =
1019           std::dynamic_pointer_cast<SolveSpaceSolver_ParameterWrapper>(*aParIt);
1020       if (!theFixedOnly || aWrapper->group() == GID_OUTOFGROUP || aWrapper->isParametric()) {
1021         aWrapper->changeParameter().val = getParameter((Slvs_hParam)aWrapper->id()).val;
1022         aCoords[i] = aWrapper->value();
1023         isUpd[i] = true;
1024       }
1025     }
1026     if (!isUpd[0] && !isUpd[1] && !isUpd[2])
1027       continue; // nothing is updated
1028
1029     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1030         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anIt->first);
1031     if (aPoint2D) {
1032       if ((isUpd[0] && fabs(aPoint2D->x() - aCoords[0]) > tolerance) ||
1033           (isUpd[1] && fabs(aPoint2D->y() - aCoords[1]) > tolerance)) {
1034         if (!isUpd[0]) aCoords[0] = aPoint2D->x();
1035         if (!isUpd[1]) aCoords[1] = aPoint2D->y();
1036         aPoint2D->setValue(aCoords[0], aCoords[1]);
1037         // Find points coincident with this one (probably not in GID_OUTOFGROUP)
1038         std::map<AttributePtr, EntityWrapperPtr>::const_iterator aLocIt;
1039         if (theFixedOnly)
1040           aLocIt = myAttributeMap.begin();
1041         else {
1042           aLocIt = anIt;
1043           ++aLocIt;
1044         }
1045         for (; aLocIt != myAttributeMap.end(); ++aLocIt) {
1046           if (!aLocIt->second)
1047             continue;
1048           std::shared_ptr<SketchPlugin_Feature> aSketchFeature =
1049             std::dynamic_pointer_cast<SketchPlugin_Feature>(aLocIt->first->owner());
1050           if (aSketchFeature && aSketchFeature->isExternal())
1051             continue;
1052           if (anIt->second->id() == aLocIt->second->id()) {
1053             aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(aLocIt->first);
1054             aPoint2D->setValue(aCoords[0], aCoords[1]);
1055           }
1056         }
1057       }
1058       continue;
1059     }
1060     AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anIt->first);
1061     if (aScalar) {
1062       if (isUpd[0] && fabs(aScalar->value() - aCoords[0]) > tolerance)
1063         aScalar->setValue(aCoords[0]);
1064       continue;
1065     }
1066     std::shared_ptr<GeomDataAPI_Point> aPoint =
1067         std::dynamic_pointer_cast<GeomDataAPI_Point>(anIt->first);
1068     if (aPoint) {
1069       if ((isUpd[0] && fabs(aPoint->x() - aCoords[0]) > tolerance) ||
1070           (isUpd[1] && fabs(aPoint->y() - aCoords[1]) > tolerance) ||
1071           (isUpd[2] && fabs(aPoint->z() - aCoords[2]) > tolerance))
1072         if (!isUpd[0]) aCoords[0] = aPoint->x();
1073         if (!isUpd[1]) aCoords[1] = aPoint->y();
1074         if (!isUpd[2]) aCoords[2] = aPoint->z();
1075         aPoint->setValue(aCoords[0], aCoords[1], aCoords[2]);
1076       continue;
1077     }
1078   }
1079
1080   //blockEvents(false);
1081 }
1082
1083 void SolveSpaceSolver_Storage::verifyFixed()
1084 {
1085   std::map<AttributePtr, EntityWrapperPtr>::iterator anAttrIt = myAttributeMap.begin();
1086   for (; anAttrIt != myAttributeMap.end(); ++anAttrIt) {
1087     if (!anAttrIt->second)
1088       continue;
1089     if (anAttrIt->second->group() == GID_OUTOFGROUP) {
1090       Slvs_Entity anEnt = getEntity((Slvs_hEntity)anAttrIt->second->id());
1091       if (anEnt.group != (Slvs_hEntity)GID_OUTOFGROUP)
1092         anEnt.group = (Slvs_hEntity)GID_OUTOFGROUP;
1093       updateEntity(anEnt);
1094     }
1095
1096     const std::list<ParameterWrapperPtr>& aParameters = anAttrIt->second->parameters();
1097     std::list<ParameterWrapperPtr>::const_iterator aParIt = aParameters.begin();
1098     for (; aParIt != aParameters.end(); ++aParIt)
1099       if (anAttrIt->second->group() == GID_OUTOFGROUP || (*aParIt)->group() == GID_OUTOFGROUP) {
1100         Slvs_Param aParam = getParameter((Slvs_hParam)(*aParIt)->id());
1101         if (aParam.group != (Slvs_hParam)GID_OUTOFGROUP) {
1102           aParam.group = (Slvs_hParam)GID_OUTOFGROUP;
1103           updateParameter(aParam);
1104         }
1105       }
1106   }
1107 }
1108
1109
1110 void SolveSpaceSolver_Storage::adjustArc(const Slvs_Entity& theArc)
1111 {
1112   double anArcPoints[3][2];
1113   double aDist[3] = {0.0};
1114   bool isFixed[3] = {false};
1115   for (int i = 0; i < 3; ++i) {
1116     Slvs_Entity aPoint = getEntity(theArc.point[i]);
1117     isFixed[i] = (aPoint.group != (Slvs_hGroup)myGroupID);
1118     anArcPoints[i][0] = getParameter(aPoint.param[0]).val;
1119     anArcPoints[i][1] = getParameter(aPoint.param[1]).val;
1120     if (i > 0) {
1121       anArcPoints[i][0] -= anArcPoints[0][0];
1122       anArcPoints[i][1] -= anArcPoints[0][1];
1123       aDist[i] = sqrt(anArcPoints[i][0] * anArcPoints[i][0] +
1124                       anArcPoints[i][1] * anArcPoints[i][1]);
1125     }
1126   }
1127
1128   if (fabs(aDist[1] - aDist[2]) < tolerance)
1129     return;
1130
1131   int anInd = 2;
1132   while (anInd > 0 && isFixed[anInd])
1133     --anInd;
1134   if (anInd < 1)
1135     return; // adjust only start or end point of the arc
1136
1137   anArcPoints[anInd][0] /= aDist[anInd];
1138   anArcPoints[anInd][1] /= aDist[anInd];
1139
1140   Slvs_Entity aPoint = getEntity(theArc.point[anInd]);
1141   for (int i = 0; i < 2; ++i) {
1142     Slvs_Param aParam = getParameter(aPoint.param[i]);
1143     aParam.val = anArcPoints[0][i] + aDist[3-anInd] * anArcPoints[anInd][i];
1144     updateParameter(aParam);
1145   }
1146 }
1147
1148
1149
1150
1151
1152
1153
1154 // ========================================================
1155 // =========      Auxiliary functions       ===============
1156 // ========================================================
1157
1158 template<typename T>
1159 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
1160 {
1161   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
1162   int aVecSize = (int)theEntities.size();
1163   if (theEntities.empty())
1164     return 1;
1165   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
1166     aResIndex--;
1167   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
1168     aResIndex++;
1169   if (aResIndex == -1 || (aResIndex < aVecSize && theEntities[aResIndex].h != theEntityID))
1170     aResIndex = aVecSize;
1171   return aResIndex;
1172 }
1173
1174 bool IsNotEqual(const Slvs_Param& theParam1, const Slvs_Param& theParam2)
1175 {
1176   return fabs(theParam1.val - theParam2.val) > tolerance;
1177 }
1178
1179 bool IsNotEqual(const Slvs_Entity& theEntity1, const Slvs_Entity& theEntity2)
1180 {
1181   int i = 0;
1182   for (; theEntity1.param[i] != 0 && i < 4; i++)
1183     if (theEntity1.param[i] != theEntity2.param[i])
1184       return true;
1185   i = 0;
1186   for (; theEntity1.point[i] != 0 && i < 4; i++)
1187     if (theEntity1.point[i] != theEntity2.point[i])
1188       return true;
1189   return false;
1190 }
1191
1192 bool IsNotEqual(const Slvs_Constraint& theConstraint1, const Slvs_Constraint& theConstraint2)
1193 {
1194   return theConstraint1.ptA != theConstraint2.ptA ||
1195          theConstraint1.ptB != theConstraint2.ptB ||
1196          theConstraint1.entityA != theConstraint2.entityA ||
1197          theConstraint1.entityB != theConstraint2.entityB ||
1198          theConstraint1.entityC != theConstraint2.entityC ||
1199          theConstraint1.entityD != theConstraint2.entityD ||
1200          fabs(theConstraint1.valA - theConstraint2.valA) > tolerance;
1201 }