src/SketchSolver/SketchSolver_ConstraintGroup.cpp

   1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
   2
   3 // File:    SketchSolver_ConstraintGroup.cpp
   4 // Created: 27 May 2014
   5 // Author:  Artem ZHIDKOV
   6
   7 #include "SketchSolver_ConstraintGroup.h"
   8
   9 #include <SketchSolver_Constraint.h>
  10
  11 #include <Events_Error.h>
  12 #include <Events_Loop.h>
  13 #include <GeomAPI_XY.h>
  14 #include <GeomAPI_Dir2d.h>
  15 #include <GeomAPI_Pnt2d.h>
  16 #include <GeomDataAPI_Dir.h>
  17 #include <GeomDataAPI_Point.h>
  18 #include <GeomDataAPI_Point2D.h>
  19 #include <ModelAPI_AttributeDouble.h>
  20 #include <ModelAPI_AttributeRefList.h>
  21 #include <ModelAPI_Document.h>
  22 #include <ModelAPI_Events.h>
  23 #include <ModelAPI_ResultConstruction.h>
  24
  25 #include <SketchPlugin_Constraint.h>
  26 #include <SketchPlugin_ConstraintFillet.h>
  27 #include <SketchPlugin_ConstraintLength.h>
  28 #include <SketchPlugin_ConstraintCoincidence.h>
  29 #include <SketchPlugin_ConstraintMirror.h>
  30 #include <SketchPlugin_ConstraintRigid.h>
  31
  32 #include <SketchPlugin_Arc.h>
  33 #include <SketchPlugin_Circle.h>
  34 #include <SketchPlugin_Line.h>
  35 #include <SketchPlugin_Point.h>
  36 #include <SketchPlugin_Sketch.h>
  37
  38 #include <math.h>
  39 #include <assert.h>
  40
  41 /// Tolerance for value of parameters
  42 const double tolerance = 1.e-10;
  43
  44 /**
  45  * Collects all sketch solver error' codes
  46  * as inline static functions
  47  */
  48  // TODO: Move this class into a separate file
  49 class SketchSolver_Error
  50 {
  51  public:
  52   /// The value parameter for the constraint
  53   inline static const std::string& CONSTRAINTS()
  54   {
  55     static const std::string MY_ERROR_VALUE("Conflicting constraints");
  56     return MY_ERROR_VALUE;
  57   }
  58   /// The entities need to have shared point, but they have not
  59   inline static const std::string& NO_COINCIDENT_POINTS()
  60   {
  61     static const std::string MY_ERROR_VALUE("Objects should have coincident point");
  62     return MY_ERROR_VALUE;
  63   }
  64 };
  65
  66 /// This value is used to give unique index to the groups
  67 static Slvs_hGroup myGroupIndexer = 0;
  68
  69 /** \brief Search the entity/parameter with specified ID in the list of elements
  70  *  \param[in] theEntityID unique ID of the element
  71  *  \param[in] theEntities list of elements
  72  *  \return position of the found element or -1 if the element is not found
  73  */
  74 template<typename T>
  75 static int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities);
  76
  77 // ========================================================
  78 // =========  SketchSolver_ConstraintGroup  ===============
  79 // ========================================================
  80
  81 SketchSolver_ConstraintGroup::SketchSolver_ConstraintGroup(
  82     std::shared_ptr<ModelAPI_CompositeFeature> theWorkplane)
  83     : myID(++myGroupIndexer),
  84       myParamMaxID(0),
  85       myEntityMaxID(0),
  86       myConstrMaxID(0),
  87       myConstraintMap(),
  88       myNeedToSolve(false),
  89       myConstrSolver()
  90 {
  91   myParams.clear();
  92   myEntities.clear();
  93   myEntOfConstr.clear();
  94   myConstraints.clear();
  95
  96   myTempConstraints.clear();
  97   myTempPointWhereDragged.clear();
  98   myTempPointWDrgdID = 0;
  99
 100   // Initialize workplane
 101   myWorkplane.h = SLVS_E_UNKNOWN;
 102 #ifndef NDEBUG
 103   assert(addWorkplane(theWorkplane));
 104 #else
 105   addWorkplane(theWorkplane);
 106 #endif
 107 }
 108
 109 SketchSolver_ConstraintGroup::~SketchSolver_ConstraintGroup()
 110 {
 111   myParams.clear();
 112   myEntities.clear();
 113   myEntOfConstr.clear();
 114   myConstraints.clear();
 115   myConstraintMap.clear();
 116   myTempConstraints.clear();
 117   myTempPointWhereDragged.clear();
 118
 119   // If the group with maximal identifier is deleted, decrease the indexer
 120   if (myID == myGroupIndexer)
 121     myGroupIndexer--;
 122 }
 123
 124 // ============================================================================
 125 //  Function: isBaseWorkplane
 126 //  Class:    SketchSolver_ConstraintGroup
 127 //  Purpose:  verify the group is based on the given workplane
 128 // ============================================================================
 129 bool SketchSolver_ConstraintGroup::isBaseWorkplane(
 130     std::shared_ptr<ModelAPI_CompositeFeature> theWorkplane) const
 131 {
 132   return theWorkplane == mySketch;
 133 }
 134
 135 // ============================================================================
 136 //  Function: isInteract
 137 //  Class:    SketchSolver_ConstraintGroup
 138 //  Purpose:  verify are there any entities in the group used by given constraint
 139 // ============================================================================
 140 bool SketchSolver_ConstraintGroup::isInteract(
 141     std::shared_ptr<SketchPlugin_Feature> theFeature) const
 142 {
 143   // Check the group is empty
 144   if (isEmpty())
 145     return true;
 146
 147   // Check if the feature is already in the group
 148   if (myEntityFeatMap.find(theFeature) != myEntityFeatMap.end())
 149     return true;
 150   std::shared_ptr<SketchPlugin_Constraint> aConstr =
 151       std::dynamic_pointer_cast<SketchPlugin_Constraint>(theFeature);
 152   if (aConstr && myConstraintMap.find(aConstr) != myConstraintMap.end())
 153     return true;
 154
 155   // Go through the attributes and verify if some of them already in the group
 156   std::list<std::shared_ptr<ModelAPI_Attribute>>
 157       anAttrList = theFeature->data()->attributes(std::string());
 158   std::list<std::shared_ptr<ModelAPI_Attribute>>::const_iterator
 159       anAttrIter = anAttrList.begin();
 160   for ( ; anAttrIter != anAttrList.end(); anAttrIter++) {
 161     AttributeRefListPtr aCAttrRefList =
 162         std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(*anAttrIter);
 163     if (aCAttrRefList) {
 164       std::list<ObjectPtr> anObjList = aCAttrRefList->list();
 165       std::list<ObjectPtr>::iterator anIt = anObjList.begin();
 166       for ( ; anIt != anObjList.end(); anIt++) {
 167         FeaturePtr aFeature = std::dynamic_pointer_cast<SketchPlugin_Feature>(*anIt);
 168         if (aFeature && myEntityFeatMap.find(aFeature) != myEntityFeatMap.end())
 169           return true;
 170       }
 171       continue;
 172     }
 173     AttributeRefAttrPtr aCAttrRef =
 174         std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(*anAttrIter);
 175     if (!aCAttrRef || !aCAttrRef->isObject()) {
 176       std::shared_ptr<ModelAPI_Attribute> anAttr =
 177           aCAttrRef ? aCAttrRef->attr() : *anAttrIter;
 178       if (myEntityAttrMap.find(anAttr) != myEntityAttrMap.end())
 179         return true;
 180     } else {
 181       ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 182           aCAttrRef->object());
 183       if (!aRC)
 184         continue;
 185       std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 186       FeaturePtr aFeature = aDoc->feature(aRC);
 187       if (myEntityFeatMap.find(aFeature) != myEntityFeatMap.end())
 188         return true;
 189       // search attributes of a feature to be parameters of constraint
 190       std::list<std::shared_ptr<ModelAPI_Attribute> > aFeatAttrList =
 191           aFeature->data()->attributes(std::string());
 192       std::list<std::shared_ptr<ModelAPI_Attribute> >::const_iterator aFAIter = aFeatAttrList
 193           .begin();
 194       for (; aFAIter != aFeatAttrList.end(); aFAIter++)
 195         if (myEntityAttrMap.find(*aFAIter) != myEntityAttrMap.end())
 196           return true;
 197     }
 198   }
 199
 200   // Entities did not found
 201   return false;
 202 }
 203
 204 // ============================================================================
 205 //  Function: checkConstraintConsistence
 206 //  Class:    SketchSolver_ConstraintGroup
 207 //  Purpose:  verifies and changes parameters of the constraint
 208 // ============================================================================
 209 void SketchSolver_ConstraintGroup::checkConstraintConsistence(Slvs_Constraint& theConstraint)
 210 {
 211   if (theConstraint.type == SLVS_C_PT_LINE_DISTANCE) {
 212     // Get constraint parameters and check the sign of constraint value
 213
 214     // point coordinates
 215     int aPtPos = Search(theConstraint.ptA, myEntities);
 216     int aPtParamPos = Search(myEntities[aPtPos].param[0], myParams);
 217     std::shared_ptr<GeomAPI_XY> aPoint(
 218         new GeomAPI_XY(myParams[aPtParamPos].val, myParams[aPtParamPos + 1].val));
 219
 220     // line coordinates
 221     int aLnPos = Search(theConstraint.entityA, myEntities);
 222     aPtPos = Search(myEntities[aLnPos].point[0], myEntities);
 223     aPtParamPos = Search(myEntities[aPtPos].param[0], myParams);
 224     std::shared_ptr<GeomAPI_XY> aStart(
 225         new GeomAPI_XY(-myParams[aPtParamPos].val, -myParams[aPtParamPos + 1].val));
 226     aPtPos = Search(myEntities[aLnPos].point[1], myEntities);
 227     aPtParamPos = Search(myEntities[aPtPos].param[0], myParams);
 228     std::shared_ptr<GeomAPI_XY> aEnd(
 229         new GeomAPI_XY(myParams[aPtParamPos].val, myParams[aPtParamPos + 1].val));
 230
 231     aEnd = aEnd->added(aStart);
 232     aPoint = aPoint->added(aStart);
 233     if (aPoint->cross(aEnd) * theConstraint.valA < 0.0)
 234       theConstraint.valA *= -1.0;
 235   }
 236 }
 237
 238 // ============================================================================
 239 //  Function: changeConstraint
 240 //  Class:    SketchSolver_ConstraintGroup
 241 //  Purpose:  create/update the constraint in the group
 242 // ============================================================================
 243 bool SketchSolver_ConstraintGroup::changeConstraint(
 244     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 245 {
 246   // There is no workplane yet, something wrong
 247   if (myWorkplane.h == SLVS_E_UNKNOWN)
 248     return false;
 249
 250   if (theConstraint) {
 251     if (theConstraint->getKind() == SketchPlugin_ConstraintRigid::ID())
 252       return changeRigidConstraint(theConstraint);
 253     if (theConstraint->getKind() == SketchPlugin_ConstraintMirror::ID())
 254       return changeMirrorConstraint(theConstraint);
 255     if (theConstraint->getKind() == SketchPlugin_ConstraintFillet::ID())
 256       return changeFilletConstraint(theConstraint);
 257   }
 258
 259   // Search this constraint in the current group to update it
 260   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 261   std::vector<Slvs_Constraint>::iterator aConstrIter;
 262   if (aConstrMapIter != myConstraintMap.end()) {
 263     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 264     aConstrIter = myConstraints.begin() + aConstrPos;
 265   }
 266
 267   // Get constraint type and verify the constraint parameters are correct
 268   SketchSolver_Constraint aConstraint(theConstraint);
 269   int aConstrType = aConstraint.getType();
 270   if (aConstrType == SLVS_C_UNKNOWN
 271       || (aConstrMapIter != myConstraintMap.end() && aConstrIter->type != aConstrType))
 272     return false;
 273   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 274
 275   // Create constraint parameters
 276   double aDistance = 0.0;  // scalar value of the constraint
 277   AttributeDoublePtr aDistAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 278       theConstraint->data()->attribute(SketchPlugin_Constraint::VALUE()));
 279   if (aDistAttr) {
 280     aDistance = aDistAttr->value();
 281     // Issue #196: checking the positivity of the distance constraint
 282     if (aDistance < tolerance &&
 283        (aConstrType == SLVS_C_PT_PT_DISTANCE || aConstrType == SLVS_C_PT_LINE_DISTANCE))
 284       return false;
 285     // SketchPlugin circle defined by its radius, but SolveSpace uses constraint for diameter
 286     if (aConstrType == SLVS_C_DIAMETER)
 287       aDistance *= 2.0;
 288     if (aConstrMapIter != myConstraintMap.end()
 289         && fabs(aConstrIter->valA - aDistance) > tolerance) {
 290       myNeedToSolve = true;
 291       aConstrIter->valA = aDistance;
 292     }
 293   }
 294
 295   size_t aNbTmpConstraints = myTempConstraints.size();
 296   Slvs_hEntity aConstrEnt[CONSTRAINT_ATTR_SIZE];  // parameters of the constraint
 297   for (unsigned int indAttr = 0; indAttr < CONSTRAINT_ATTR_SIZE; indAttr++) {
 298     aConstrEnt[indAttr] = SLVS_E_UNKNOWN;
 299     std::shared_ptr<ModelAPI_AttributeRefAttr> aConstrAttr = std::dynamic_pointer_cast<
 300         ModelAPI_AttributeRefAttr>(
 301         theConstraint->data()->attribute(aConstraintAttributes[indAttr]));
 302     if (!aConstrAttr)
 303       continue;
 304
 305     // Convert the object of the attribute to the feature
 306     FeaturePtr aFeature;
 307     if (aConstrAttr->isObject() && aConstrAttr->object()) {
 308       ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 309           aConstrAttr->object());
 310       if (!aRC)
 311         continue;
 312       std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 313       aFeature = aDoc->feature(aRC);
 314     }
 315
 316     // For the length constraint the start and end points of the line should be added to the entities list instead of line
 317     if (aConstrType == SLVS_C_PT_PT_DISTANCE
 318         && theConstraint->getKind().compare(SketchPlugin_ConstraintLength::ID()) == 0) {
 319       Slvs_hEntity aLineEnt = changeEntityFeature(aFeature);
 320       int aEntPos = Search(aLineEnt, myEntities);
 321       aConstrEnt[indAttr++] = myEntities[aEntPos].point[0];
 322       aConstrEnt[indAttr++] = myEntities[aEntPos].point[1];
 323       while (indAttr < CONSTRAINT_ATTR_SIZE)
 324         aConstrEnt[indAttr++] = 0;
 325       break;  // there should be no other entities
 326     } else if (aConstrAttr->isObject())
 327       aConstrEnt[indAttr] = changeEntityFeature(aFeature);
 328     else
 329       aConstrEnt[indAttr] = changeEntity(aConstrAttr->attr());
 330   }
 331
 332   if (aConstrMapIter == myConstraintMap.end()) { // Add new constraint
 333     // Several points may be coincident, it is not necessary to store all constraints between them.
 334     // Try to find sequence of coincident points which connects the points of new constraint
 335     if (aConstrType == SLVS_C_POINTS_COINCIDENT) {
 336       if (aConstrEnt[0] == aConstrEnt[1])  // no need to add self coincidence
 337         return false;
 338       if (!addCoincidentPoints(aConstrEnt[0], aConstrEnt[1])) {
 339         myExtraCoincidence.insert(theConstraint);  // the constraint is stored for further purposes
 340         return false;
 341       }
 342       if (aNbTmpConstraints < myTempConstraints.size()) {
 343         // There was added temporary constraint. Check that there is no coincident points which already rigid.
 344
 345         // Get list of already fixed points
 346         std::set<Slvs_hEntity> anAlreadyFixed;
 347         std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 348         for (; aCIter != myConstraints.end(); aCIter++)
 349           if (aCIter->type == SLVS_C_WHERE_DRAGGED) {
 350             std::list<Slvs_hConstraint>::const_iterator aTmpIt = myTempConstraints.begin();
 351             for (; aTmpIt != myTempConstraints.end(); aTmpIt++)
 352               if (*aTmpIt == aCIter->h)
 353                 break;
 354             if (aTmpIt == myTempConstraints.end())
 355               anAlreadyFixed.insert(aCIter->ptA);
 356           }
 357
 358         std::set<Slvs_hConstraint> aTmpConstrToDelete;
 359         std::list<Slvs_hConstraint>::reverse_iterator aTmpIter = myTempConstraints.rbegin();
 360         size_t aCurSize = myTempConstraints.size();
 361         for (; aCurSize > aNbTmpConstraints && aTmpIter != myTempConstraints.rend();
 362             aTmpIter++, aCurSize--) {
 363           int aConstrPos = Search(*aTmpIter, myConstraints);
 364           std::vector<std::set<Slvs_hEntity> >::const_iterator
 365             aCoincIter = myCoincidentPoints.begin();
 366           for (; aCoincIter != myCoincidentPoints.end(); aCoincIter++)
 367             if (aCoincIter->find(myConstraints[aConstrPos].ptA) != aCoincIter->end()) {
 368               std::set<Slvs_hEntity>::const_iterator anIt;
 369               for (anIt = aCoincIter->begin(); anIt != aCoincIter->end(); anIt++)
 370                 if (anAlreadyFixed.find(*anIt) != anAlreadyFixed.end()) {
 371                   aTmpConstrToDelete.insert(*aTmpIter);
 372                   break;
 373                 }
 374               break;
 375             }
 376         }
 377         if (!aTmpConstrToDelete.empty())
 378           removeTemporaryConstraints(aTmpConstrToDelete);
 379       }
 380     }
 381     // For the tangency constraints it is necessary to identify which points of entities are coincident
 382     int aSlvsOtherFlag = 0;
 383     int aSlvsOther2Flag = 0;
 384     if (aConstrType == SLVS_C_ARC_LINE_TANGENT || aConstrType == SLVS_C_CURVE_CURVE_TANGENT) {
 385       // Search entities used by constraint
 386       int anEnt1Pos = Search(aConstrEnt[2], myEntities);
 387       int anEnt2Pos = Search(aConstrEnt[3], myEntities);
 388       // Obtain start and end points of entities
 389       Slvs_hEntity aPointsToFind[4];
 390       aPointsToFind[0] = myEntities[anEnt1Pos].point[1];
 391       aPointsToFind[1]= myEntities[anEnt1Pos].point[2];
 392       bool hasLine = (myEntities[anEnt2Pos].type == SLVS_E_LINE_SEGMENT);
 393       aPointsToFind[2]= myEntities[anEnt2Pos].point[hasLine ? 0 : 1];
 394       aPointsToFind[3]= myEntities[anEnt2Pos].point[hasLine ? 1 : 2];
 395       // Search coincident points
 396       bool isPointFound[4];
 397       std::vector<std::set<Slvs_hEntity> >::const_iterator aCPIter = myCoincidentPoints.begin();
 398       for ( ; aCPIter != myCoincidentPoints.end(); aCPIter++) {
 399         for (int i = 0; i < 4; i++)
 400           isPointFound[i] = (aCPIter->find(aPointsToFind[i]) != aCPIter->end());
 401         if ((isPointFound[0] || isPointFound[1]) && (isPointFound[2] || isPointFound[3])) {
 402           // the arc is tangent by end point
 403           if (isPointFound[1]) aSlvsOtherFlag = 1;
 404           // the second item is an arc and it is tangent by end point too
 405           if (!hasLine && isPointFound[3]) aSlvsOther2Flag = 1;
 406           break;
 407         }
 408       }
 409       if (aCPIter == myCoincidentPoints.end()) {
 410         // There is no coincident points between tangential objects. Generate error message
 411         Events_Error::send(SketchSolver_Error::NO_COINCIDENT_POINTS(), this);
 412         return false;
 413       }
 414     }
 415
 416     // Create SolveSpace constraint structure
 417     Slvs_Constraint aSlvsConstr = Slvs_MakeConstraint(++myConstrMaxID, myID, aConstrType,
 418                                                       myWorkplane.h, aDistance, aConstrEnt[0],
 419                                                       aConstrEnt[1], aConstrEnt[2], aConstrEnt[3]);
 420     if (aSlvsOtherFlag != 0) aSlvsConstr.other = aSlvsOtherFlag;
 421     if (aSlvsOther2Flag != 0) aSlvsConstr.other2 = aSlvsOther2Flag;
 422     myConstraints.push_back(aSlvsConstr);
 423     myConstraintMap[theConstraint] = std::vector<Slvs_hEntity>(1, aSlvsConstr.h);
 424     int aConstrPos = Search(aSlvsConstr.h, myConstraints);
 425     aConstrIter = myConstraints.begin() + aConstrPos;
 426     myNeedToSolve = true;
 427   } else { // Attributes of constraint may be changed => update constraint
 428     Slvs_hEntity* aCurrentAttr[] = {&aConstrIter->ptA, &aConstrIter->ptB,
 429                                    &aConstrIter->entityA, &aConstrIter->entityB,
 430                                    &aConstrIter->entityC, &aConstrIter->entityD};
 431     for (unsigned int indAttr = 0; indAttr < CONSTRAINT_ATTR_SIZE; indAttr++) {
 432       if (*(aCurrentAttr[indAttr]) != aConstrEnt[indAttr])
 433       {
 434         *(aCurrentAttr[indAttr]) = aConstrEnt[indAttr];
 435         myNeedToSolve = true;
 436       }
 437     }
 438   }
 439
 440   // Update flags of entities to be used by constraints
 441   for (unsigned int indAttr = 0; indAttr < CONSTRAINT_ATTR_SIZE; indAttr++)
 442     if (aConstrEnt[indAttr] != 0) {
 443       int aPos = Search(aConstrEnt[indAttr], myEntities);
 444       myEntOfConstr[aPos] = true;
 445       // Sub-entities should be used implcitly
 446       Slvs_hEntity* aEntPtr = myEntities[aPos].point;
 447       while (*aEntPtr != 0) {
 448         aPos = Search(*aEntPtr, myEntities);
 449         myEntOfConstr[aPos] = true;
 450         aEntPtr++;
 451       }
 452     }
 453
 454   checkConstraintConsistence(*aConstrIter);
 455   return true;
 456 }
 457
 458 // ============================================================================
 459 //  Function: changeRigidConstraint
 460 //  Class:    SketchSolver_ConstraintGroup
 461 //  Purpose:  create/update the "Rigid" constraint in the group
 462 // ============================================================================
 463 bool SketchSolver_ConstraintGroup::changeRigidConstraint(
 464     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 465 {
 466   // Search this constraint in the current group to update it
 467   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 468   std::vector<Slvs_Constraint>::iterator aConstrIter;
 469   if (aConstrMapIter != myConstraintMap.end()) {
 470     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 471     aConstrIter = myConstraints.begin() + aConstrPos;
 472   }
 473
 474   // Get constraint type and verify the constraint parameters are correct
 475   SketchSolver_Constraint aConstraint(theConstraint);
 476   int aConstrType = aConstraint.getType();
 477   if (aConstrType == SLVS_C_UNKNOWN
 478       || (aConstrMapIter != myConstraintMap.end() && aConstrIter->type != aConstrType))
 479     return false;
 480   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 481
 482   Slvs_hEntity aConstrEnt = SLVS_E_UNKNOWN;
 483   std::shared_ptr<ModelAPI_AttributeRefAttr> aConstrAttr = std::dynamic_pointer_cast<
 484       ModelAPI_AttributeRefAttr>(
 485       theConstraint->data()->attribute(aConstraintAttributes[0]));
 486   if (!aConstrAttr)
 487     return false;
 488
 489   // Convert the object of the attribute to the feature
 490   FeaturePtr aFeature;
 491   if (aConstrAttr->isObject() && aConstrAttr->object()) {
 492     ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 493         aConstrAttr->object());
 494     if (!aRC)
 495       return false;
 496     std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 497     aFeature = aDoc->feature(aRC);
 498   }
 499
 500   aConstrEnt = aConstrAttr->isObject() ? changeEntityFeature(aFeature) : changeEntity(aConstrAttr->attr());
 501
 502   if (aConstrMapIter == myConstraintMap.end()) { // Add new constraint
 503     // Check the fixed entity is not a point.
 504     std::shared_ptr<ModelAPI_AttributeRefAttr> aConstrAttr = std::dynamic_pointer_cast<
 505         ModelAPI_AttributeRefAttr>(theConstraint->data()->attribute(aConstraintAttributes[0]));
 506     std::shared_ptr<GeomDataAPI_Point> aPoint =
 507         std::dynamic_pointer_cast<GeomDataAPI_Point>(aConstrAttr->attr());
 508     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 509         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(aConstrAttr->attr());
 510     if (aPoint || aPoint2D) {
 511       // Create SolveSpace constraint structure
 512       Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 513           ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 514           aConstrEnt, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 515       myConstraints.push_back(aConstraint);
 516       myConstraintMap[theConstraint] = std::vector<Slvs_hEntity>(1, aConstraint.h);
 517       int aConstrPos = Search(aConstraint.h, myConstraints);
 518       aConstrIter = myConstraints.begin() + aConstrPos;
 519       myNeedToSolve = true;
 520     } else {
 521       myConstraintMap[theConstraint] = std::vector<Slvs_hConstraint>();
 522
 523       // To avoid SolveSpace problems:
 524       // * if the circle is rigid, we will fix its center and radius;
 525       // * if the arc is rigid, we will fix its start and end points and radius.
 526       double aRadius = 0.0;
 527       bool isArc = false;
 528       bool isCircle = false;
 529       if (aFeature) {
 530         if (aFeature->getKind() == SketchPlugin_Arc::ID()) {
 531           std::shared_ptr<GeomDataAPI_Point2D> aCenter =
 532               std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 533               aFeature->data()->attribute(SketchPlugin_Arc::CENTER_ID()));
 534           std::shared_ptr<GeomDataAPI_Point2D> aStart =
 535               std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 536               aFeature->data()->attribute(SketchPlugin_Arc::START_ID()));
 537           aRadius = aStart->pnt()->distance(aCenter->pnt());
 538           isArc = true;
 539         } else if (aFeature->getKind() == SketchPlugin_Circle::ID()) {
 540           aRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 541               aFeature->data()->attribute(SketchPlugin_Circle::RADIUS_ID()))->value();
 542           isCircle = true;
 543         }
 544       }
 545
 546       // Get list of already fixed points
 547       std::set<Slvs_hEntity> anAlreadyFixed;
 548       std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 549       for (; aCIter != myConstraints.end(); aCIter++)
 550         if (aCIter->type == SLVS_C_WHERE_DRAGGED)
 551           anAlreadyFixed.insert(aCIter->ptA);
 552
 553       // Create constraints to fix the parameters of the entity
 554       int aEntPos = Search(aConstrEnt, myEntities);
 555       Slvs_hEntity* aPointsPtr = myEntities[aEntPos].point;
 556       if (isArc) aPointsPtr++; // avoid to fix center of arc
 557       while (*aPointsPtr != 0) {
 558         // Avoid to create additional "Rigid" constraints for coincident points
 559         bool isCoincAlreadyFixed = false;
 560         if (!anAlreadyFixed.empty()) {
 561           if (anAlreadyFixed.find(*aPointsPtr) != anAlreadyFixed.end())
 562             isCoincAlreadyFixed = true;
 563
 564           std::vector<std::set<Slvs_hEntity> >::const_iterator aCoincIter =
 565               myCoincidentPoints.begin();
 566           for (; !isCoincAlreadyFixed && aCoincIter != myCoincidentPoints.end(); aCoincIter++) {
 567             if (aCoincIter->find(*aPointsPtr) == aCoincIter->end())
 568               continue;
 569             std::set<Slvs_hEntity>::const_iterator anIter = anAlreadyFixed.begin();
 570             for (; !isCoincAlreadyFixed && anIter != anAlreadyFixed.end(); anIter++)
 571               if (aCoincIter->find(*anIter) != aCoincIter->end())
 572                 isCoincAlreadyFixed = true;
 573           }
 574         }
 575
 576         if (!isCoincAlreadyFixed) {
 577           Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 578               ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 579               *aPointsPtr, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 580           myConstraints.push_back(aConstraint);
 581           myConstraintMap[theConstraint].push_back(aConstraint.h);
 582         }
 583         aPointsPtr++;
 584       }
 585
 586       if (isArc || isCircle) { // add radius constraint
 587         Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 588             ++myConstrMaxID, myID, SLVS_C_DIAMETER, myWorkplane.h, 2.0 * aRadius,
 589             SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aConstrEnt, SLVS_E_UNKNOWN);
 590         myConstraints.push_back(aConstraint);
 591         myConstraintMap[theConstraint].push_back(aConstraint.h);
 592       }
 593
 594       // The object is already rigid, so there is no constraints added
 595       if (myConstraintMap[theConstraint].empty()) {
 596         myConstraintMap.erase(theConstraint);
 597         myNeedToSolve = false;
 598       }
 599       else
 600         myNeedToSolve = true;
 601     }
 602   }
 603   return true;
 604 }
 605
 606 // ============================================================================
 607 //  Function: changeMirrorConstraint
 608 //  Class:    SketchSolver_ConstraintGroup
 609 //  Purpose:  create/update the "Mirror" constraint in the group
 610 // ============================================================================
 611 bool SketchSolver_ConstraintGroup::changeMirrorConstraint(
 612     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 613 {
 614   DataPtr aConstrData = theConstraint->data();
 615
 616   // Search this constraint in the current group to update it
 617   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 618   std::vector<Slvs_Constraint>::iterator aConstrIter;
 619   if (aConstrMapIter != myConstraintMap.end()) {
 620     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 621     aConstrIter = myConstraints.begin() + aConstrPos;
 622   }
 623
 624   // Get constraint type and verify the constraint parameters are correct
 625   SketchSolver_Constraint aConstraint(theConstraint);
 626   int aConstrType = aConstraint.getType();
 627   if (aConstrType == SLVS_C_UNKNOWN
 628       || (aConstrMapIter != myConstraintMap.end() && aConstrIter->type != aConstrType))
 629     return false;
 630   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 631
 632   Slvs_hEntity aMirrorLineEnt = SLVS_E_UNKNOWN;
 633   AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 634       aConstrData->attribute(aConstraintAttributes[0]));
 635   if (!aConstrAttr)
 636     return false;
 637
 638   // Convert the object of the attribute to the feature
 639   FeaturePtr aMirrorLineFeat;
 640   if (aConstrAttr->isObject() && aConstrAttr->object()) {
 641     ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 642         aConstrAttr->object());
 643     if (!aRC)
 644       return false;
 645     std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 646     aMirrorLineFeat = aDoc->feature(aRC);
 647   }
 648   aMirrorLineEnt = aConstrAttr->isObject() ?
 649       changeEntityFeature(aMirrorLineFeat) : changeEntity(aConstrAttr->attr());
 650
 651   if (aConstrMapIter == myConstraintMap.end()) { // Add new constraint
 652     // Append symmetric constraint for each point of mirroring features
 653     AttributeRefListPtr aBaseRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 654         aConstrData->attribute(aConstraintAttributes[1]));
 655     AttributeRefListPtr aMirroredRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 656         aConstrData->attribute(aConstraintAttributes[2]));
 657     if (!aBaseRefList || !aMirroredRefList)
 658       return false;
 659
 660     std::list<ObjectPtr> aBaseList = aBaseRefList->list();
 661     std::list<ObjectPtr> aMirroredList = aMirroredRefList->list();
 662     if (aBaseList.size() != aMirroredList.size())
 663       return false;
 664
 665     myConstraintMap[theConstraint] = std::vector<Slvs_hConstraint>();
 666
 667     FeaturePtr aBaseFeature, aMirrorFeature;
 668     ResultConstructionPtr aRC;
 669     std::list<ObjectPtr>::iterator aBaseIter = aBaseList.begin();
 670     std::list<ObjectPtr>::iterator aMirIter = aMirroredList.begin();
 671     for ( ; aBaseIter != aBaseList.end(); aBaseIter++, aMirIter++) {
 672       aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aBaseIter);
 673       aBaseFeature = aRC ? aRC->document()->feature(aRC) :
 674           std::dynamic_pointer_cast<SketchPlugin_Feature>(*aBaseIter);
 675       aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aMirIter);
 676       aMirrorFeature = aRC ? aRC->document()->feature(aRC) :
 677           std::dynamic_pointer_cast<SketchPlugin_Feature>(*aMirIter);
 678
 679       if (!aBaseFeature || !aMirrorFeature ||
 680           aBaseFeature->getKind() != aMirrorFeature->getKind())
 681         return false;
 682       Slvs_hEntity aBaseEnt = changeEntityFeature(aBaseFeature);
 683       Slvs_hEntity aMirrorEnt = changeEntityFeature(aMirrorFeature);
 684       // Make aMirrorEnt parameters to be symmetric with aBaseEnt
 685       makeMirrorEntity(aBaseEnt, aMirrorEnt, aMirrorLineEnt);
 686
 687       if (aBaseFeature->getKind() == SketchPlugin_Point::ID()) {
 688         Slvs_Constraint aConstraint = Slvs_MakeConstraint(++myConstrMaxID, myID, aConstrType,
 689             myWorkplane.h, 0.0, aBaseEnt, aMirrorEnt, aMirrorLineEnt, SLVS_E_UNKNOWN);
 690         myConstraints.push_back(aConstraint);
 691         myConstraintMap[theConstraint].push_back(aConstraint.h);
 692       } else {
 693         int aBasePos = Search(aBaseEnt, myEntities);
 694         int aMirrorPos = Search(aMirrorEnt, myEntities);
 695         if (aBaseFeature->getKind() == SketchPlugin_Line::ID()) {
 696           for (int ind = 0; ind < 2; ind++) {
 697             Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 698                 ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 699                 myEntities[aBasePos].point[ind], myEntities[aMirrorPos].point[ind],
 700                 aMirrorLineEnt, SLVS_E_UNKNOWN);
 701             myConstraints.push_back(aConstraint);
 702             myConstraintMap[theConstraint].push_back(aConstraint.h);
 703           }
 704         } else if (aBaseFeature->getKind() == SketchPlugin_Circle::ID()) {
 705           Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 706               ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 707               myEntities[aBasePos].point[0], myEntities[aMirrorPos].point[0],
 708               aMirrorLineEnt, SLVS_E_UNKNOWN);
 709           myConstraints.push_back(aConstraint);
 710           myConstraintMap[theConstraint].push_back(aConstraint.h);
 711           // Additional constraint for equal radii
 712           Slvs_Constraint anEqRadConstr = Slvs_MakeConstraint(
 713               ++myConstrMaxID, myID, SLVS_C_EQUAL_RADIUS, myWorkplane.h, 0.0,
 714               SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aBaseEnt, aMirrorEnt);
 715           myConstraints.push_back(anEqRadConstr);
 716           myConstraintMap[theConstraint].push_back(anEqRadConstr.h);
 717         } else if (aBaseFeature->getKind() == SketchPlugin_Arc::ID()) {
 718           // Workaround to avoid problems in SolveSpace.
 719           // The symmetry of two arcs will be done using symmetry of three points on these arcs:
 720           // start point, end point, and any other point on the arc
 721           Slvs_hEntity aBaseArcPoints[3] = {
 722               myEntities[aBasePos].point[1],
 723               myEntities[aBasePos].point[2],
 724               SLVS_E_UNKNOWN};
 725           Slvs_hEntity aMirrorArcPoints[3] = { // indices of points of arc, center corresponds center, first point corresponds last point
 726               myEntities[aMirrorPos].point[2],
 727               myEntities[aMirrorPos].point[1],
 728               SLVS_E_UNKNOWN};
 729           Slvs_hEntity aBothArcs[2] = {aBaseEnt, aMirrorEnt};
 730           Slvs_hEntity aBothMiddlePoints[2];
 731           for (int i = 0; i < 2; i++) {
 732             double x, y;
 733             calculateMiddlePoint(aBothArcs[i], x, y);
 734             std::vector<Slvs_Param>::iterator aParamIter = myParams.end();
 735             Slvs_hParam u = changeParameter(x, aParamIter);
 736             Slvs_hParam v = changeParameter(y, aParamIter);
 737             Slvs_Entity aPoint = Slvs_MakePoint2d(++myEntityMaxID, myID, myWorkplane.h, u, v);
 738             myEntities.push_back(aPoint);
 739             aBothMiddlePoints[i] = aPoint.h;
 740             // additional constraint point-on-curve
 741             Slvs_Constraint aPonCircConstr = Slvs_MakeConstraint(
 742                 ++myConstrMaxID, myID, SLVS_C_PT_ON_CIRCLE, myWorkplane.h, 0.0,
 743                 aPoint.h, SLVS_E_UNKNOWN, aBothArcs[i], SLVS_E_UNKNOWN);
 744             myConstraints.push_back(aPonCircConstr);
 745             myConstraintMap[theConstraint].push_back(aPonCircConstr.h);
 746           }
 747
 748           aBaseArcPoints[2] = aBothMiddlePoints[0];
 749           aMirrorArcPoints[2] = aBothMiddlePoints[1];
 750           for (int ind = 0; ind < 3; ind++) {
 751             Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 752                 ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 753                 aBaseArcPoints[ind], aMirrorArcPoints[ind], aMirrorLineEnt, SLVS_E_UNKNOWN);
 754             myConstraints.push_back(aConstraint);
 755             myConstraintMap[theConstraint].push_back(aConstraint.h);
 756           }
 757         }
 758       }
 759     }
 760
 761     // Set the mirror line unchanged during constraint recalculation
 762     int aMirrorLinePos = Search(aMirrorLineEnt, myEntities);
 763     Slvs_Constraint aRigidStart = Slvs_MakeConstraint(
 764         ++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED, myWorkplane.h, 0,
 765         myEntities[aMirrorLinePos].point[0], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 766     myConstraints.push_back(aRigidStart);
 767     myConstraintMap[theConstraint].push_back(aRigidStart.h);
 768     Slvs_Constraint aRigidEnd = Slvs_MakeConstraint(
 769         ++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED, myWorkplane.h, 0,
 770         myEntities[aMirrorLinePos].point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 771     myConstraints.push_back(aRigidEnd);
 772     myConstraintMap[theConstraint].push_back(aRigidEnd.h);
 773
 774     // Add temporary constraints for initial objects to be unchanged
 775     for (aBaseIter = aBaseList.begin(); aBaseIter != aBaseList.end(); aBaseIter++) {
 776       aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aBaseIter);
 777       aBaseFeature = aRC ? aRC->document()->feature(aRC) :
 778           std::dynamic_pointer_cast<SketchPlugin_Feature>(*aBaseIter);
 779       if (!aBaseFeature) continue;
 780       std::list<AttributePtr> aPoints = aBaseFeature->data()->attributes(GeomDataAPI_Point2D::type());
 781       std::list<AttributePtr>::iterator anIt = aPoints.begin();
 782       for ( ; anIt != aPoints.end(); anIt++) {
 783         // Arcs are fixed by center and start points only (to avoid solving errors in SolveSpace)
 784         if (aBaseFeature->getKind() == SketchPlugin_Arc::ID() &&
 785             (*anIt)->id() == SketchPlugin_Arc::END_ID())
 786           continue;
 787         addTemporaryConstraintWhereDragged(*anIt);
 788       }
 789     }
 790   }
 791   return true;
 792 }
 793
 794 // ============================================================================
 795 //  Function: changeFilletConstraint
 796 //  Class:    SketchSolver_ConstraintGroup
 797 //  Purpose:  create/update the "Fillet" constraint in the group
 798 // ============================================================================
 799 bool SketchSolver_ConstraintGroup::changeFilletConstraint(
 800     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 801 {
 802   DataPtr aConstrData = theConstraint->data();
 803
 804   // Search this constraint in the current group to update it
 805   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 806   std::vector<Slvs_Constraint>::iterator aConstrIter;
 807   if (aConstrMapIter != myConstraintMap.end()) {
 808     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 809     aConstrIter = myConstraints.begin() + aConstrPos;
 810   }
 811
 812   // Get constraint type and verify the constraint parameters are correct
 813   SketchSolver_Constraint aConstraint(theConstraint);
 814   int aConstrType = aConstraint.getType();
 815   if (aConstrType == SLVS_C_UNKNOWN)
 816     return false;
 817   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 818
 819   // Obtain hEntity for basic objects of fillet
 820   Slvs_hEntity aBaseObject[2];
 821   FeaturePtr aBaseFeature[2];
 822   for (unsigned int indAttr = 0; indAttr < 2; indAttr++) {
 823     AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 824         aConstrData->attribute(aConstraintAttributes[indAttr]));
 825     if (!aConstrAttr)
 826       return false;
 827     if (aConstrAttr->isObject() && aConstrAttr->object()) {
 828       ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 829           aConstrAttr->object());
 830       if (!aRC)
 831         return false;
 832       std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 833       aBaseFeature[indAttr] = aDoc->feature(aRC);
 834     }
 835     aBaseObject[indAttr] = aConstrAttr->isObject() ?
 836         changeEntityFeature(aBaseFeature[indAttr]) : changeEntity(aConstrAttr->attr());
 837   }
 838   // Check the base entities have a coincident point
 839   int aBaseObjInd[2] = {
 840       Search(aBaseObject[0], myEntities),
 841       Search(aBaseObject[1], myEntities)
 842     };
 843   int aShift[2] = { // shift for calculating correct start and end points for different types of objects
 844       myEntities[aBaseObjInd[0]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0,
 845       myEntities[aBaseObjInd[1]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0,
 846     };
 847   Slvs_hEntity aFirstObjPoints[2] = { // indices of start and end point of first object
 848       myEntities[aBaseObjInd[0]].point[aShift[0]],
 849       myEntities[aBaseObjInd[0]].point[1+aShift[0]]
 850     };
 851   Slvs_hEntity aSecondObjPoints[2] = { // indices of start and end point of second object
 852       myEntities[aBaseObjInd[1]].point[aShift[1]],
 853       myEntities[aBaseObjInd[1]].point[1+aShift[1]]
 854     };
 855   bool isCoincidentFound = false;
 856   int aBaseCoincInd[2] = {0, 0}; // indices in aFirstObjPoint and aSecondObjPoint identifying coincident points
 857   std::vector<std::set<Slvs_hEntity> >::iterator aCPIter = myCoincidentPoints.begin();
 858   for ( ; aCPIter != myCoincidentPoints.end() && !isCoincidentFound; aCPIter++)
 859     for (int ind1 = 0; ind1 < 2 && !isCoincidentFound; ind1++)
 860       for (int ind2 = 0; ind2 < 2 && !isCoincidentFound; ind2++)
 861         if (aCPIter->find(aFirstObjPoints[ind1]) != aCPIter->end() &&
 862             aCPIter->find(aSecondObjPoints[ind2]) != aCPIter->end()) {
 863           aBaseCoincInd[0] = ind1;
 864           aBaseCoincInd[1] = ind2;
 865           isCoincidentFound = true;
 866         }
 867   if (!isCoincidentFound) {
 868     // There is no coincident points between objects. Generate error message
 869     Events_Error::send(SketchSolver_Error::NO_COINCIDENT_POINTS(), this);
 870     return false;
 871   }
 872
 873   // Create fillet entities
 874   // - first object is placed on the first base
 875   // - second object is on the second base
 876   // - third object is a filleting arc
 877   static const int aNbFilletEnt = 3;
 878   Slvs_hEntity aFilletEnt[aNbFilletEnt];
 879   int aFilletObjInd[aNbFilletEnt];
 880   AttributeRefListPtr aFilletRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 881       aConstrData->attribute(aConstraintAttributes[2]));
 882   if (!aFilletRefList)
 883     return false;
 884   std::list<ObjectPtr> aFilletList = aFilletRefList->list();
 885   if (aFilletList.size() < aNbFilletEnt)
 886     return false;
 887   FeaturePtr aFilletFeature;
 888   ResultConstructionPtr aRC;
 889   std::list<ObjectPtr>::iterator aFilIter = aFilletList.begin();
 890   for (int indEnt = 0; aFilIter != aFilletList.end(); aFilIter++, indEnt++) {
 891     aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aFilIter);
 892     aFilletFeature = aRC ? aRC->document()->feature(aRC) :
 893         std::dynamic_pointer_cast<SketchPlugin_Feature>(*aFilIter);
 894     if (!aFilletFeature)
 895       return false;
 896     aFilletEnt[indEnt] = changeEntityFeature(aFilletFeature);
 897     aFilletObjInd[indEnt] = Search(aFilletEnt[indEnt], myEntities);
 898   }
 899   // At first time, for correct result, move floating points of fillet on the middle points of base objects
 900   if (myConstraintMap.find(theConstraint) == myConstraintMap.end()) {
 901     double anArcPoints[6];
 902     for (int indEnt = 0; indEnt < aNbFilletEnt - 1; indEnt++) {
 903       int anIndShift = myEntities[aFilletObjInd[indEnt]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0;
 904       int aPointsPos[2] = {
 905           Search(myEntities[aFilletObjInd[indEnt]].point[anIndShift], myEntities),
 906           Search(myEntities[aFilletObjInd[indEnt]].point[1+anIndShift], myEntities)
 907         };
 908       int aParamPos[2] = {
 909           Search(myEntities[aPointsPos[0]].param[0], myParams),
 910           Search(myEntities[aPointsPos[1]].param[0], myParams)
 911         };
 912       int anIndex = aParamPos[aBaseCoincInd[indEnt]];
 913       if (anIndShift == 0) {
 914         myParams[anIndex].val =
 915             0.5 * (myParams[aParamPos[0]].val + myParams[aParamPos[1]].val);
 916         myParams[1 + anIndex].val =
 917             0.5 * (myParams[1 + aParamPos[0]].val + myParams[1 + aParamPos[1]].val);
 918       } else { // place the changed point on the arc
 919         double x = 0, y = 0;
 920         calculateMiddlePoint(aFilletEnt[indEnt], x, y);
 921         myParams[anIndex].val = x;
 922         myParams[1 + anIndex].val = y;
 923       }
 924       anArcPoints[indEnt*2+2] = myParams[anIndex].val;
 925       anArcPoints[indEnt*2+3] = myParams[1 + anIndex].val;
 926     }
 927     anArcPoints[0] = 0.5 * (anArcPoints[2] + anArcPoints[4]);
 928     anArcPoints[1] = 0.5 * (anArcPoints[3] + anArcPoints[5]);
 929     for (int indArcPt = 0; indArcPt < 3; indArcPt++) {
 930       int aPtPos = Search(myEntities[aFilletObjInd[2]].point[indArcPt], myEntities);
 931       int aParamPos = Search(myEntities[aPtPos].param[0], myParams);
 932       myParams[aParamPos].val = anArcPoints[indArcPt * 2];
 933       myParams[aParamPos + 1].val = anArcPoints[indArcPt * 2 + 1];
 934     }
 935   }
 936
 937   // Check the fillet arc which point to be connected to
 938   bool isArcInversed = false; // indicates that start and end points of arc should be connected to second and first object respectively
 939   Slvs_hEntity hEnt = myEntities[aFilletObjInd[2]].point[1];
 940   int aPos = Search(hEnt, myEntities);
 941   Slvs_hParam anArcStartPoint = myEntities[aPos].param[0];
 942   aPos = Search(anArcStartPoint, myParams);
 943   double anArcPtCoord[2] = {myParams[aPos].val, myParams[aPos+1].val};
 944   double aSqDistances[2];
 945   int aPtInd;
 946   for (int indEnt = 0; indEnt < aNbFilletEnt - 1; indEnt++) {
 947     aPtInd = aBaseCoincInd[indEnt]+aShift[indEnt];
 948     hEnt = myEntities[aFilletObjInd[indEnt]].point[aPtInd];
 949     aPos = Search(hEnt, myEntities);
 950     Slvs_hParam anObjectPoint = myEntities[aPos].param[0];
 951     aPos = Search(anObjectPoint, myParams);
 952     double aPtCoord[2] = {myParams[aPos].val, myParams[aPos+1].val};
 953     aSqDistances[indEnt] =
 954         (anArcPtCoord[0] - aPtCoord[0]) * (anArcPtCoord[0] - aPtCoord[0]) +
 955         (anArcPtCoord[1] - aPtCoord[1]) * (anArcPtCoord[1] - aPtCoord[1]);
 956   }
 957   if (aSqDistances[1] < aSqDistances[0])
 958     isArcInversed = true;
 959
 960   // Create list of constraints to generate fillet
 961   std::vector<Slvs_hConstraint> aConstrList;
 962   bool isExists = myConstraintMap.find(theConstraint) != myConstraintMap.end(); // constraint already exists
 963   std::vector<Slvs_hConstraint>::iterator aCMapIter =
 964     isExists ? myConstraintMap[theConstraint].begin() : aConstrList.begin();
 965   int aCurConstrPos = isExists ? Search(*aCMapIter, myConstraints) : 0;
 966   for (int indEnt = 0; indEnt < aNbFilletEnt - 1; indEnt++) {
 967     // one point of fillet object should be coincident with the point on base, non-coincident with another base object
 968     aPtInd = 1-aBaseCoincInd[indEnt]+aShift[indEnt]; // (1-aBaseCoincInd[indEnt]) = index of non-coincident point, aShift is used to process all types of shapes
 969     Slvs_hEntity aPtBase = myEntities[aBaseObjInd[indEnt]].point[aPtInd];
 970     Slvs_hEntity aPtFillet = myEntities[aFilletObjInd[indEnt]].point[aPtInd];
 971     if (isExists) {
 972       myConstraints[aCurConstrPos].ptA = aPtBase;
 973       myConstraints[aCurConstrPos].ptB = aPtFillet;
 974       aCMapIter++;
 975       aCurConstrPos = Search(*aCMapIter, myConstraints);
 976     } else {
 977       Slvs_Constraint aCoincConstr = Slvs_MakeConstraint(
 978           ++myConstrMaxID, myID, SLVS_C_POINTS_COINCIDENT, myWorkplane.h,
 979           0, aPtBase, aPtFillet, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 980       myConstraints.push_back(aCoincConstr);
 981       aConstrList.push_back(aCoincConstr.h);
 982     }
 983
 984     // another point of fillet object should be placed on the base object
 985     Slvs_Constraint aPonCurveConstr;
 986     int aTangentType;
 987     if (myEntities[aFilletObjInd[indEnt]].type == SLVS_E_ARC_OF_CIRCLE) {
 988       // centers of arcs should be coincident
 989       aPtBase = myEntities[aBaseObjInd[indEnt]].point[0];
 990       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[0];
 991       if (isExists) {
 992         myConstraints[aCurConstrPos].ptA = aPtBase;
 993         myConstraints[aCurConstrPos].ptB = aPtFillet;
 994         aCMapIter++;
 995         aCurConstrPos = Search(*aCMapIter, myConstraints);
 996       } else {
 997         aPonCurveConstr = Slvs_MakeConstraint(
 998             ++myConstrMaxID, myID, SLVS_C_POINTS_COINCIDENT, myWorkplane.h,
 999             0, aPtBase, aPtFillet, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
1000       }
1001       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[1+aBaseCoincInd[indEnt]]; // !!! will be used below
1002       aTangentType = SLVS_C_CURVE_CURVE_TANGENT;
1003     } else {
1004       aPtInd = aBaseCoincInd[indEnt];
1005       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[aPtInd];
1006       if (isExists) {
1007         myConstraints[aCurConstrPos].ptA = aPtFillet;
1008         aCMapIter++;
1009         aCurConstrPos = Search(*aCMapIter, myConstraints);
1010       } else {
1011         aPonCurveConstr = Slvs_MakeConstraint(
1012             ++myConstrMaxID, myID, SLVS_C_PT_ON_LINE, myWorkplane.h,
1013             0, aPtFillet, SLVS_E_UNKNOWN, aBaseObject[indEnt], SLVS_E_UNKNOWN);
1014       }
1015       aTangentType = SLVS_C_ARC_LINE_TANGENT;
1016     }
1017     if (!isExists) {
1018       myConstraints.push_back(aPonCurveConstr);
1019       aConstrList.push_back(aPonCurveConstr.h);
1020     }
1021
1022     // Bound point of fillet arc should be tangently coincident with a bound point of fillet object
1023     aPtInd = 1 + (isArcInversed ? 1-indEnt : indEnt);
1024     Slvs_hEntity aPtArc = myEntities[aFilletObjInd[2]].point[aPtInd];
1025     if (isExists) {
1026       myConstraints[aCurConstrPos].ptA = aPtArc;
1027       myConstraints[aCurConstrPos].ptB = aPtFillet;
1028       aCMapIter++;
1029       aCurConstrPos = Search(*aCMapIter, myConstraints);
1030       myConstraints[aCurConstrPos].entityA = aFilletEnt[2];
1031       myConstraints[aCurConstrPos].entityB = aFilletEnt[indEnt];
1032       myConstraints[aCurConstrPos].other = (isArcInversed ? 1-indEnt : indEnt);
1033       aCMapIter++;
1034       aCurConstrPos = Search(*aCMapIter, myConstraints);
1035     } else {
1036       Slvs_Constraint aCoincConstr = Slvs_MakeConstraint(
1037           ++myConstrMaxID, myID, SLVS_C_POINTS_COINCIDENT, myWorkplane.h,
1038           0, aPtArc, aPtFillet, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
1039       myConstraints.push_back(aCoincConstr);
1040       aConstrList.push_back(aCoincConstr.h);
1041       Slvs_Constraint aTangency = Slvs_MakeConstraint(
1042           ++myConstrMaxID, myID, aTangentType, myWorkplane.h,
1043           0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aFilletEnt[2], aFilletEnt[indEnt]);
1044       aTangency.other = (isArcInversed ? 1-indEnt : indEnt);
1045       aTangency.other2 = aTangentType == SLVS_C_CURVE_CURVE_TANGENT ? aBaseCoincInd[indEnt] : 0;
1046       myConstraints.push_back(aTangency);
1047       aConstrList.push_back(aTangency.h);
1048     }
1049   }
1050
1051   // Additional constraint for fillet diameter
1052   double aRadius = 0.0;  // scalar value of the constraint
1053   AttributeDoublePtr aDistAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
1054       aConstrData->attribute(SketchPlugin_Constraint::VALUE()));
1055   aRadius = aDistAttr->value();
1056   if (isExists) {
1057     myConstraints[aCurConstrPos].entityA = aFilletEnt[2];
1058     myConstraints[aCurConstrPos].valA = aRadius * 2.0;
1059     aCMapIter++;
1060   } else {
1061     Slvs_Constraint aDiamConstr = Slvs_MakeConstraint(
1062         ++myConstrMaxID, myID, SLVS_C_DIAMETER, myWorkplane.h, aRadius * 2.0,
1063         SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aFilletEnt[2], SLVS_E_UNKNOWN);
1064     myConstraints.push_back(aDiamConstr);
1065     aConstrList.push_back(aDiamConstr.h);
1066
1067     myConstraintMap[theConstraint] = aConstrList;
1068   }
1069
1070   // Additional temporary constraints for base objects to be fixed
1071   for (unsigned int indAttr = 0; indAttr < 2; indAttr++) {
1072     if (!aBaseFeature[indAttr]) {
1073       AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
1074           aConstrData->attribute(aConstraintAttributes[indAttr]));
1075       addTemporaryConstraintWhereDragged(aConstrAttr->attr());
1076       continue;
1077     }
1078     std::list<AttributePtr> anAttributes =
1079         aBaseFeature[indAttr]->data()->attributes(GeomDataAPI_Point2D::type());
1080     std::list<AttributePtr>::iterator anIt = anAttributes.begin();
1081     for ( ; anIt != anAttributes.end(); anIt++) {
1082       // Arc should be fixed by center and start points only (to avoid "conflicting constraints" message)
1083       if (aBaseFeature[indAttr]->getKind() == SketchPlugin_Arc::ID() &&
1084           (*anIt)->id() == SketchPlugin_Arc::END_ID())
1085         continue;
1086       addTemporaryConstraintWhereDragged(*anIt);
1087     }
1088   }
1089   return true;
1090 }
1091
1092 // ============================================================================
1093 //  Function: changeEntity
1094 //  Class:    SketchSolver_ConstraintGroup
1095 //  Purpose:  create/update the element affected by any constraint
1096 // ============================================================================
1097 Slvs_hEntity SketchSolver_ConstraintGroup::changeEntity(
1098     std::shared_ptr<ModelAPI_Attribute> theEntity)
1099 {
1100   // If the entity is already in the group, try to find it
1101   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aEntIter =
1102       myEntityAttrMap.find(theEntity);
1103   int aEntPos;
1104   std::vector<Slvs_Param>::const_iterator aParamIter;  // looks at first parameter of already existent entity or at the end of vector otherwise
1105   if (aEntIter == myEntityAttrMap.end())  // no such entity => should be created
1106     aParamIter = myParams.end();
1107   else {  // the entity already exists
1108     aEntPos = Search(aEntIter->second, myEntities);
1109     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
1110     aParamIter = myParams.begin() + aParamPos;
1111   }
1112   const bool isEntExists = (aEntIter != myEntityAttrMap.end());  // defines that the entity already exists
1113   const bool isNeedToSolve = myNeedToSolve;
1114   myNeedToSolve = false;
1115
1116   if (isEntExists) {
1117     // Verify that the entity is not used by "Rigid" constraint.
1118     // If it is used, the object should not move.
1119     std::vector<std::set<Slvs_hEntity> >::iterator aCoincIter = myCoincidentPoints.begin();
1120     for (; aCoincIter != myCoincidentPoints.end(); aCoincIter++)
1121       if (aCoincIter->find(aEntIter->second) != aCoincIter->end())
1122         break;
1123     std::set<Slvs_hEntity> aCoincident;
1124     if (aCoincIter != myCoincidentPoints.end()) {
1125       aCoincident = *aCoincIter;
1126       aCoincident.erase(aEntIter->second);
1127
1128       std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1129       for (; aConstrIter != myConstraints.end(); aConstrIter++)
1130         if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
1131             aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
1132           myNeedToSolve = true;
1133           return aEntIter->second;
1134         }
1135     }
1136   }
1137
1138   // Look over supported types of entities
1139   Slvs_Entity aNewEntity;
1140   aNewEntity.h = SLVS_E_UNKNOWN;
1141
1142   // Point in 3D
1143   std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1144       theEntity);
1145   if (aPoint) {
1146     Slvs_hParam aX = changeParameter(aPoint->x(), aParamIter);
1147     Slvs_hParam aY = changeParameter(aPoint->y(), aParamIter);
1148     Slvs_hParam aZ = changeParameter(aPoint->z(), aParamIter);
1149     if (!isEntExists) // New entity
1150       aNewEntity = Slvs_MakePoint3d(++myEntityMaxID, myID, aX, aY, aZ);
1151   } else {
1152     // All entities except 3D points are created on workplane. So, if there is no workplane yet, then error
1153     if (myWorkplane.h == SLVS_E_UNKNOWN)
1154       return SLVS_E_UNKNOWN;
1155
1156     // Point in 2D
1157     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1158         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
1159     if (aPoint2D) {
1160       Slvs_hParam aU = changeParameter(aPoint2D->x(), aParamIter);
1161       Slvs_hParam aV = changeParameter(aPoint2D->y(), aParamIter);
1162       if (!isEntExists) // New entity
1163         aNewEntity = Slvs_MakePoint2d(++myEntityMaxID, myID, myWorkplane.h, aU, aV);
1164     } else {
1165       // Scalar value (used for the distance entities)
1166       AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theEntity);
1167       if (aScalar) {
1168         Slvs_hParam aValue = changeParameter(aScalar->value(), aParamIter);
1169         if (!isEntExists) // New entity
1170           aNewEntity = Slvs_MakeDistance(++myEntityMaxID, myID, myWorkplane.h, aValue);
1171       }
1172     }
1173   }
1174   /// \todo Other types of entities
1175
1176   Slvs_hEntity aResult = SLVS_E_UNKNOWN; // Unsupported or wrong entity type
1177
1178   if (isEntExists) {
1179     myNeedToSolve = myNeedToSolve || isNeedToSolve;
1180     aResult = aEntIter->second;
1181   } else if (aNewEntity.h != SLVS_E_UNKNOWN) {
1182     myEntities.push_back(aNewEntity);
1183     myEntOfConstr.push_back(false);
1184     myEntityAttrMap[theEntity] = aNewEntity.h;
1185     aResult = aNewEntity.h;
1186   }
1187
1188   // If the attribute was changed by the user, we need to fix it before solving
1189   if (myNeedToSolve && theEntity->isImmutable())
1190     addTemporaryConstraintWhereDragged(theEntity, false);
1191
1192   return aResult;
1193 }
1194
1195 // ============================================================================
1196 //  Function: changeEntity
1197 //  Class:    SketchSolver_ConstraintGroup
1198 //  Purpose:  create/update the element defined by the feature affected by any constraint
1199 // ============================================================================
1200 Slvs_hEntity SketchSolver_ConstraintGroup::changeEntityFeature(FeaturePtr theEntity)
1201 {
1202   if (!theEntity->data()->isValid())
1203     return SLVS_E_UNKNOWN;
1204   // If the entity is already in the group, try to find it
1205   std::map<FeaturePtr, Slvs_hEntity>::const_iterator aEntIter = myEntityFeatMap.find(theEntity);
1206   // defines that the entity already exists
1207   const bool isEntExists = (myEntityFeatMap.find(theEntity) != myEntityFeatMap.end());
1208
1209   Slvs_Entity aNewEntity;
1210   aNewEntity.h = SLVS_E_UNKNOWN;
1211
1212   // SketchPlugin features
1213   std::shared_ptr<SketchPlugin_Feature> aFeature = std::dynamic_pointer_cast<
1214       SketchPlugin_Feature>(theEntity);
1215   if (aFeature) {  // Verify the feature by its kind
1216     const std::string& aFeatureKind = aFeature->getKind();
1217     AttributePtr anAttribute;
1218
1219     // Line
1220     if (aFeatureKind.compare(SketchPlugin_Line::ID()) == 0) {
1221       anAttribute = aFeature->data()->attribute(SketchPlugin_Line::START_ID());
1222       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1223       Slvs_hEntity aStart = changeEntity(anAttribute);
1224
1225       anAttribute = aFeature->data()->attribute(SketchPlugin_Line::END_ID());
1226       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1227       Slvs_hEntity aEnd = changeEntity(anAttribute);
1228
1229       if (!isEntExists) // New entity
1230         aNewEntity = Slvs_MakeLineSegment(++myEntityMaxID, myID, myWorkplane.h, aStart, aEnd);
1231     }
1232     // Circle
1233     else if (aFeatureKind.compare(SketchPlugin_Circle::ID()) == 0) {
1234       anAttribute = aFeature->data()->attribute(SketchPlugin_Circle::CENTER_ID());
1235       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1236       Slvs_hEntity aCenter = changeEntity(anAttribute);
1237
1238       anAttribute = aFeature->data()->attribute(SketchPlugin_Circle::RADIUS_ID());
1239       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1240       Slvs_hEntity aRadius = changeEntity(anAttribute);
1241
1242       if (!isEntExists) // New entity
1243         aNewEntity = Slvs_MakeCircle(++myEntityMaxID, myID, myWorkplane.h, aCenter,
1244                                      myWorkplane.normal, aRadius);
1245     }
1246     // Arc
1247     else if (aFeatureKind.compare(SketchPlugin_Arc::ID()) == 0) {
1248       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::CENTER_ID());
1249       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1250       Slvs_hEntity aCenter = changeEntity(anAttribute);
1251
1252       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::START_ID());
1253       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1254       Slvs_hEntity aStart = changeEntity(anAttribute);
1255
1256       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::END_ID());
1257       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1258       Slvs_hEntity aEnd = changeEntity(anAttribute);
1259
1260       if (!isEntExists)
1261         aNewEntity = Slvs_MakeArcOfCircle(++myEntityMaxID, myID, myWorkplane.h,
1262                                           myWorkplane.normal, aCenter, aStart, aEnd);
1263     }
1264     // Point (it has low probability to be an attribute of constraint, so it is checked at the end)
1265     else if (aFeatureKind.compare(SketchPlugin_Point::ID()) == 0) {
1266       anAttribute = aFeature->data()->attribute(SketchPlugin_Point::COORD_ID());
1267       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1268       Slvs_hEntity aPoint = changeEntity(anAttribute);
1269
1270       if (isEntExists)
1271         return aEntIter->second;
1272
1273       // Both the sketch point and its attribute (coordinates) link to the same SolveSpace point identifier
1274       myEntityFeatMap[theEntity] = aPoint;
1275       myNeedToSolve = true;
1276       return aPoint;
1277     }
1278   }
1279   /// \todo Other types of features
1280
1281   if (isEntExists)
1282     return aEntIter->second;
1283
1284   if (aNewEntity.h != SLVS_E_UNKNOWN) {
1285     myEntities.push_back(aNewEntity);
1286     myEntOfConstr.push_back(false);
1287     myEntityFeatMap[theEntity] = aNewEntity.h;
1288     myNeedToSolve = true;
1289     return aNewEntity.h;
1290   }
1291
1292   // Unsupported or wrong entity type
1293   return SLVS_E_UNKNOWN;
1294 }
1295
1296 // ============================================================================
1297 //  Function: changeNormal
1298 //  Class:    SketchSolver_ConstraintGroup
1299 //  Purpose:  create/update the normal of workplane
1300 // ============================================================================
1301 Slvs_hEntity SketchSolver_ConstraintGroup::changeNormal(
1302     std::shared_ptr<ModelAPI_Attribute> theDirX, std::shared_ptr<ModelAPI_Attribute> theDirY,
1303     std::shared_ptr<ModelAPI_Attribute> theNorm)
1304 {
1305   std::shared_ptr<GeomDataAPI_Dir> aDirX = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirX);
1306   std::shared_ptr<GeomDataAPI_Dir> aDirY = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirY);
1307   if (!aDirX || !aDirY || (fabs(aDirX->x()) + fabs(aDirX->y()) + fabs(aDirX->z()) < tolerance)
1308       || (fabs(aDirY->x()) + fabs(aDirY->y()) + fabs(aDirY->z()) < tolerance))
1309     return SLVS_E_UNKNOWN;
1310
1311   // quaternion parameters of normal vector
1312   double qw, qx, qy, qz;
1313   Slvs_MakeQuaternion(aDirX->x(), aDirX->y(), aDirX->z(), aDirY->x(), aDirY->y(), aDirY->z(), &qw,
1314                       &qx, &qy, &qz);
1315   double aNormCoord[4] = { qw, qx, qy, qz };
1316
1317   // Try to find existent normal
1318   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aEntIter =
1319       myEntityAttrMap.find(theNorm);
1320   std::vector<Slvs_Param>::const_iterator aParamIter;  // looks to the first parameter of already existent entity or to the end of vector otherwise
1321   if (aEntIter == myEntityAttrMap.end())  // no such entity => should be created
1322     aParamIter = myParams.end();
1323   else {  // the entity already exists, update it
1324     int aEntPos = Search(aEntIter->second, myEntities);
1325     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
1326     aParamIter = myParams.begin() + aParamPos;
1327   }
1328
1329   // Change parameters of the normal
1330   Slvs_hParam aNormParams[4];
1331   for (int i = 0; i < 4; i++)
1332     aNormParams[i] = changeParameter(aNormCoord[i], aParamIter);
1333
1334   if (aEntIter != myEntityAttrMap.end())  // the entity already exists
1335     return aEntIter->second;
1336
1337   // Create a normal
1338   Slvs_Entity aNormal = Slvs_MakeNormal3d(++myEntityMaxID, myID, aNormParams[0], aNormParams[1],
1339                                           aNormParams[2], aNormParams[3]);
1340   myEntities.push_back(aNormal);
1341   myEntOfConstr.push_back(false);
1342   myEntityAttrMap[theNorm] = aNormal.h;
1343   return aNormal.h;
1344 }
1345
1346 // ============================================================================
1347 //  Function: addWorkplane
1348 //  Class:    SketchSolver_ConstraintGroup
1349 //  Purpose:  create workplane for the group
1350 // ============================================================================
1351 bool SketchSolver_ConstraintGroup::addWorkplane(std::shared_ptr<ModelAPI_CompositeFeature> theSketch)
1352 {
1353   if (myWorkplane.h || theSketch->getKind().compare(SketchPlugin_Sketch::ID()) != 0)
1354     return false;  // the workplane already exists or the function parameter is not Sketch
1355
1356   mySketch = theSketch;
1357   updateWorkplane();
1358   return true;
1359 }
1360
1361 // ============================================================================
1362 //  Function: updateWorkplane
1363 //  Class:    SketchSolver_ConstraintGroup
1364 //  Purpose:  update parameters of workplane
1365 // ============================================================================
1366 bool SketchSolver_ConstraintGroup::updateWorkplane()
1367 {
1368   if (!mySketch->data())
1369     return false; // case sketch is deleted
1370   // Get parameters of workplane
1371   std::shared_ptr<ModelAPI_Attribute> aDirX = mySketch->data()->attribute(
1372       SketchPlugin_Sketch::DIRX_ID());
1373   std::shared_ptr<ModelAPI_Attribute> aDirY = mySketch->data()->attribute(
1374       SketchPlugin_Sketch::DIRY_ID());
1375   std::shared_ptr<ModelAPI_Attribute> aNorm = mySketch->data()->attribute(
1376       SketchPlugin_Sketch::NORM_ID());
1377   std::shared_ptr<ModelAPI_Attribute> anOrigin = mySketch->data()->attribute(
1378       SketchPlugin_Sketch::ORIGIN_ID());
1379   // Transform them into SolveSpace format
1380   Slvs_hEntity aNormalWP = changeNormal(aDirX, aDirY, aNorm);
1381   if (!aNormalWP)
1382     return false;
1383   Slvs_hEntity anOriginWP = changeEntity(anOrigin);
1384   if (!anOriginWP)
1385     return false;
1386
1387   if (!myWorkplane.h) {
1388     // Create workplane
1389     myWorkplane = Slvs_MakeWorkplane(++myEntityMaxID, myID, anOriginWP, aNormalWP);
1390     // Workplane should be added to the list of entities
1391     myEntities.push_back(myWorkplane);
1392     myEntOfConstr.push_back(false);
1393   }
1394   return true;
1395 }
1396
1397 // ============================================================================
1398 //  Function: changeParameter
1399 //  Class:    SketchSolver_ConstraintGroup
1400 //  Purpose:  create/update value of parameter
1401 // ============================================================================
1402 Slvs_hParam SketchSolver_ConstraintGroup::changeParameter(
1403     const double& theParam, std::vector<Slvs_Param>::const_iterator& thePrmIter)
1404 {
1405   if (thePrmIter != myParams.end()) {  // Parameter should be updated
1406     int aParamPos = thePrmIter - myParams.begin();
1407     if (fabs(thePrmIter->val - theParam) > tolerance) {
1408       myNeedToSolve = true;  // parameter is changed, need to resolve constraints
1409       myParams[aParamPos].val = theParam;
1410     }
1411     thePrmIter++;
1412     return myParams[aParamPos].h;
1413   }
1414
1415   // Newly created parameter
1416   Slvs_Param aParam = Slvs_MakeParam(++myParamMaxID, myID, theParam);
1417   myParams.push_back(aParam);
1418   myNeedToSolve = true;
1419   // The list of parameters is changed, move iterator to the end of the list to avoid problems
1420   thePrmIter = myParams.end();
1421   return aParam.h;
1422 }
1423
1424 // ============================================================================
1425 //  Function: resolveConstraints
1426 //  Class:    SketchSolver_ConstraintGroup
1427 //  Purpose:  solve the set of constraints for the current group
1428 // ============================================================================
1429 bool SketchSolver_ConstraintGroup::resolveConstraints()
1430 {
1431   if (!myNeedToSolve)
1432     return false;
1433
1434   myConstrSolver.setGroupID(myID);
1435   myConstrSolver.setParameters(myParams);
1436   myConstrSolver.setEntities(myEntities);
1437   myConstrSolver.setConstraints(myConstraints);
1438   myConstrSolver.setDraggedParameters(myTempPointWhereDragged);
1439
1440   int aResult = myConstrSolver.solve();
1441   if (aResult == SLVS_RESULT_OKAY) {  // solution succeeded, store results into correspondent attributes
1442                                       // Obtain result into the same list of parameters
1443     if (!myConstrSolver.getResult(myParams))
1444       return true;
1445
1446     // We should go through the attributes map, because only attributes have valued parameters
1447     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntIter =
1448         myEntityAttrMap.begin();
1449     for (; anEntIter != myEntityAttrMap.end(); anEntIter++) {
1450       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1451         anEntIter->first->owner()->data()->blockSendAttributeUpdated(true);
1452       if (updateAttribute(anEntIter->first, anEntIter->second))
1453         updateRelatedConstraints(anEntIter->first);
1454     }
1455     updateFilletConstraints();
1456     // unblock all features then
1457     for (anEntIter = myEntityAttrMap.begin(); anEntIter != myEntityAttrMap.end(); anEntIter++) {
1458       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1459         anEntIter->first->owner()->data()->blockSendAttributeUpdated(false);
1460     }
1461   } else if (!myConstraints.empty())
1462     Events_Error::send(SketchSolver_Error::CONSTRAINTS(), this);
1463
1464   removeTemporaryConstraints();
1465   myNeedToSolve = false;
1466   return true;
1467 }
1468
1469 // ============================================================================
1470 //  Function: mergeGroups
1471 //  Class:    SketchSolver_ConstraintGroup
1472 //  Purpose:  append specified group to the current group
1473 // ============================================================================
1474 void SketchSolver_ConstraintGroup::mergeGroups(const SketchSolver_ConstraintGroup& theGroup)
1475 {
1476   // If specified group is empty, no need to merge
1477   if (theGroup.myConstraintMap.empty())
1478     return;
1479
1480   // Map between old and new indexes of SolveSpace constraints
1481   std::map<Slvs_hConstraint, Slvs_hConstraint> aConstrMap;
1482
1483   // Add all constraints from theGroup to the current group
1484   ConstraintMap::const_iterator aConstrIter = theGroup.myConstraintMap.begin();
1485   for (; aConstrIter != theGroup.myConstraintMap.end(); aConstrIter++)
1486     if (changeConstraint(aConstrIter->first))
1487       aConstrMap[aConstrIter->second.back()] = myConstrMaxID;  // the constraint was added => store its ID
1488
1489   // Add temporary constraints from theGroup
1490   std::list<Slvs_hConstraint>::const_iterator aTempConstrIter = theGroup.myTempConstraints.begin();
1491   for (; aTempConstrIter != theGroup.myTempConstraints.end(); aTempConstrIter++) {
1492     std::map<Slvs_hConstraint, Slvs_hConstraint>::iterator aFind = aConstrMap.find(
1493         *aTempConstrIter);
1494     if (aFind != aConstrMap.end())
1495       myTempConstraints.push_back(aFind->second);
1496   }
1497
1498   if (myTempPointWhereDragged.empty())
1499     myTempPointWhereDragged = theGroup.myTempPointWhereDragged;
1500   else if (!theGroup.myTempPointWhereDragged.empty()) {  // Need to create additional transient constraint
1501     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aFeatureIter =
1502         theGroup.myEntityAttrMap.begin();
1503     for (; aFeatureIter != theGroup.myEntityAttrMap.end(); aFeatureIter++)
1504       if (aFeatureIter->second == myTempPointWDrgdID) {
1505         addTemporaryConstraintWhereDragged(aFeatureIter->first);
1506         break;
1507       }
1508   }
1509
1510   myNeedToSolve = myNeedToSolve || theGroup.myNeedToSolve;
1511 }
1512
1513 // ============================================================================
1514 //  Function: splitGroup
1515 //  Class:    SketchSolver_ConstraintGroup
1516 //  Purpose:  divide the group into several subgroups
1517 // ============================================================================
1518 void SketchSolver_ConstraintGroup::splitGroup(std::vector<SketchSolver_ConstraintGroup*>& theCuts)
1519 {
1520   // Divide constraints and entities into several groups
1521   std::vector<std::set<Slvs_hEntity> > aGroupsEntities;
1522   std::vector<std::set<Slvs_hConstraint> > aGroupsConstr;
1523   int aMaxNbEntities = 0;  // index of the group with maximal nuber of elements (this group will be left in the current)
1524   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1525   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1526     Slvs_hEntity aConstrEnt[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA,
1527         aConstrIter->entityB };
1528     std::vector<int> anIndexes;
1529     // Go through the groupped entities and find even one of entities of current constraint
1530     std::vector<std::set<Slvs_hEntity> >::iterator aGrEntIter;
1531     for (aGrEntIter = aGroupsEntities.begin(); aGrEntIter != aGroupsEntities.end(); aGrEntIter++) {
1532       bool isFound = false;
1533       for (int i = 0; i < 4 && !isFound; i++)
1534         if (aConstrEnt[i] != 0) {
1535           isFound = (aGrEntIter->find(aConstrEnt[i]) != aGrEntIter->end());
1536           // Also we need to check sub-entities
1537           int aEntPos = Search(aConstrEnt[i], myEntities);
1538           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1539             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1540             for (int j = 0; *aSub != 0 && j < 4 && !isFound; aSub++, j++)
1541               isFound = (aGrEntIter->find(*aSub) != aGrEntIter->end());
1542           }
1543         }
1544       if (isFound)
1545         anIndexes.push_back(aGrEntIter - aGroupsEntities.begin());
1546     }
1547     // Add new group if no one is found
1548     if (anIndexes.empty()) {
1549       std::set<Slvs_hEntity> aNewGrEnt;
1550       for (int i = 0; i < 4; i++)
1551         if (aConstrEnt[i] != 0) {
1552           aNewGrEnt.insert(aConstrEnt[i]);
1553           int aEntPos = Search(aConstrEnt[i], myEntities);
1554           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1555             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1556             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1557               aNewGrEnt.insert(*aSub);
1558           }
1559         }
1560       std::set<Slvs_hConstraint> aNewGrConstr;
1561       aNewGrConstr.insert(aConstrIter->h);
1562
1563       aGroupsEntities.push_back(aNewGrEnt);
1564       aGroupsConstr.push_back(aNewGrConstr);
1565       if (aNewGrEnt.size() > aGroupsEntities[aMaxNbEntities].size())
1566         aMaxNbEntities = aGroupsEntities.size() - 1;
1567     } else {  // Add entities indexes into the found group
1568       aGrEntIter = aGroupsEntities.begin() + anIndexes.front();
1569       for (int i = 0; i < 4; i++)
1570         if (aConstrEnt[i] != 0) {
1571           aGrEntIter->insert(aConstrEnt[i]);
1572           int aEntPos = Search(aConstrEnt[i], myEntities);
1573           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1574             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1575             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1576               aGrEntIter->insert(*aSub);
1577           }
1578         }
1579       aGroupsConstr[anIndexes.front()].insert(aConstrIter->h);
1580       if (aGrEntIter->size() > aGroupsEntities[aMaxNbEntities].size())
1581         aMaxNbEntities = aGrEntIter - aGroupsEntities.begin();
1582       if (anIndexes.size() > 1) {  // There are found several connected groups, merge them
1583         std::vector<std::set<Slvs_hEntity> >::iterator aFirstGroup = aGroupsEntities.begin()
1584             + anIndexes.front();
1585         std::vector<std::set<Slvs_hConstraint> >::iterator aFirstConstr = aGroupsConstr.begin()
1586             + anIndexes.front();
1587         std::vector<int>::iterator anInd = anIndexes.begin();
1588         for (++anInd; anInd != anIndexes.end(); anInd++) {
1589           aFirstGroup->insert(aGroupsEntities[*anInd].begin(), aGroupsEntities[*anInd].end());
1590           aFirstConstr->insert(aGroupsConstr[*anInd].begin(), aGroupsConstr[*anInd].end());
1591         }
1592         if (aFirstGroup->size() > aGroupsEntities[aMaxNbEntities].size())
1593           aMaxNbEntities = anIndexes.front();
1594         // Remove merged groups
1595         for (anInd = anIndexes.end() - 1; anInd != anIndexes.begin(); anInd--) {
1596           aGroupsEntities.erase(aGroupsEntities.begin() + (*anInd));
1597           aGroupsConstr.erase(aGroupsConstr.begin() + (*anInd));
1598         }
1599       }
1600     }
1601   }
1602
1603   if (aGroupsEntities.size() <= 1)
1604     return;
1605
1606   // Remove the group with maximum elements as it will be left in the current group
1607   aGroupsEntities.erase(aGroupsEntities.begin() + aMaxNbEntities);
1608   aGroupsConstr.erase(aGroupsConstr.begin() + aMaxNbEntities);
1609
1610   // Add new groups of constraints and divide current group
1611   std::vector<SketchSolver_ConstraintGroup*> aNewGroups;
1612   for (int i = aGroupsEntities.size(); i > 0; i--) {
1613     SketchSolver_ConstraintGroup* aG = new SketchSolver_ConstraintGroup(mySketch);
1614     aNewGroups.push_back(aG);
1615   }
1616   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.begin();
1617   int aConstrMapPos = 0;  // position of iterator in the map (used to restore iterator after removing constraint)
1618   while (aConstrMapIter != myConstraintMap.end()) {
1619     std::vector<std::set<Slvs_hConstraint> >::const_iterator aGIter = aGroupsConstr.begin();
1620     std::vector<SketchSolver_ConstraintGroup*>::iterator aGroup = aNewGroups.begin();
1621     for (; aGIter != aGroupsConstr.end(); aGIter++, aGroup++)
1622       if (aGIter->find(aConstrMapIter->second.front()) != aGIter->end()) {
1623         (*aGroup)->changeConstraint(aConstrMapIter->first);
1624         removeConstraint(aConstrMapIter->first);
1625         // restore iterator
1626         aConstrMapIter = myConstraintMap.begin();
1627         for (int i = 0; i < aConstrMapPos; i++)
1628           aConstrMapIter++;
1629         break;
1630       }
1631     if (aGIter == aGroupsConstr.end()) {
1632       aConstrMapIter++;
1633       aConstrMapPos++;
1634     }
1635   }
1636
1637   theCuts.insert(theCuts.end(), aNewGroups.begin(), aNewGroups.end());
1638 }
1639
1640 // ============================================================================
1641 //  Function: updateGroup
1642 //  Class:    SketchSolver_ConstraintGroup
1643 //  Purpose:  search removed entities and constraints
1644 // ============================================================================
1645 bool SketchSolver_ConstraintGroup::updateGroup()
1646 {
1647   ConstraintMap::reverse_iterator aConstrIter = myConstraintMap.rbegin();
1648   bool isAllValid = true;
1649   bool isCCRemoved = false;  // indicates that at least one of coincidence constraints was removed
1650   int aConstrIndex = 0;
1651   while (/*isAllValid && */aConstrIter != myConstraintMap.rend()) {
1652     if (!aConstrIter->first->data() || !aConstrIter->first->data()->isValid()) {
1653       if (aConstrIter->first->getKind().compare(SketchPlugin_ConstraintCoincidence::ID()) == 0)
1654         isCCRemoved = true;
1655       removeConstraint(aConstrIter->first);
1656       isAllValid = false;
1657       // Get back the correct position of iterator after the "remove" operation
1658       aConstrIter = myConstraintMap.rbegin();
1659       for (int i = 0; i < aConstrIndex && aConstrIter != myConstraintMap.rend(); i++)
1660         aConstrIter++;
1661     } else {
1662       aConstrIter++;
1663       aConstrIndex++;
1664     }
1665   }
1666
1667   // Check if some entities are invalid too
1668   std::set<Slvs_hEntity> anEntToRemove;
1669   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1670       anAttrIter = myEntityAttrMap.begin();
1671   while (anAttrIter != myEntityAttrMap.end()) {
1672     if (!anAttrIter->first->owner() || !anAttrIter->first->owner()->data() ||
1673         !anAttrIter->first->owner()->data()->isValid()) {
1674       anEntToRemove.insert(anAttrIter->second);
1675       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1676           aRemovedIter = anAttrIter;
1677       anAttrIter++;
1678       myEntityAttrMap.erase(aRemovedIter);
1679     } else
1680       anAttrIter++;
1681   }
1682   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIter = myEntityFeatMap.begin();
1683   while (aFeatIter != myEntityFeatMap.end()) {
1684     if (!aFeatIter->first || !aFeatIter->first->data() ||
1685         !aFeatIter->first->data()->isValid()) {
1686       anEntToRemove.insert(aFeatIter->second);
1687       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = aFeatIter;
1688       aFeatIter++;
1689       myEntityFeatMap.erase(aRemovedIter);
1690     } else
1691       aFeatIter++;
1692   }
1693   removeEntitiesById(anEntToRemove);
1694
1695   // Probably, need to update coincidence constraints
1696   if (isCCRemoved && !myExtraCoincidence.empty()) {
1697     // Make a copy, because the new list of unused constrtaints will be generated
1698     std::set<std::shared_ptr<SketchPlugin_Constraint> > anExtraCopy = myExtraCoincidence;
1699     myExtraCoincidence.clear();
1700
1701     std::set<std::shared_ptr<SketchPlugin_Constraint> >::iterator aCIter = anExtraCopy.begin();
1702     for (; aCIter != anExtraCopy.end(); aCIter++)
1703       if ((*aCIter)->data() && (*aCIter)->data()->isValid())
1704         changeConstraint(*aCIter);
1705   }
1706
1707   return !isAllValid;
1708 }
1709
1710 // ============================================================================
1711 //  Function: updateAttribute
1712 //  Class:    SketchSolver_ConstraintGroup
1713 //  Purpose:  update features of sketch after resolving constraints
1714 // ============================================================================
1715 bool SketchSolver_ConstraintGroup::updateAttribute(
1716     std::shared_ptr<ModelAPI_Attribute> theAttribute, const Slvs_hEntity& theEntityID)
1717 {
1718   // Search the position of the first parameter of the entity
1719   int anEntPos = Search(theEntityID, myEntities);
1720   int aFirstParamPos = Search(myEntities[anEntPos].param[0], myParams);
1721
1722   // Look over supported types of entities
1723
1724   // Point in 3D
1725   std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1726       theAttribute);
1727   if (aPoint) {
1728     if (fabs(aPoint->x() - myParams[aFirstParamPos].val) > tolerance
1729         || fabs(aPoint->y() - myParams[aFirstParamPos + 1].val) > tolerance
1730         || fabs(aPoint->z() - myParams[aFirstParamPos + 2].val) > tolerance) {
1731       aPoint->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val,
1732                        myParams[aFirstParamPos + 2].val);
1733       return true;
1734     }
1735     return false;
1736   }
1737
1738   // Point in 2D
1739   std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1740       std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theAttribute);
1741   if (aPoint2D) {
1742     if (fabs(aPoint2D->x() - myParams[aFirstParamPos].val) > tolerance
1743         || fabs(aPoint2D->y() - myParams[aFirstParamPos + 1].val) > tolerance) {
1744       aPoint2D->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val);
1745       return true;
1746     }
1747     return false;
1748   }
1749
1750   // Scalar value
1751   AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theAttribute);
1752   if (aScalar) {
1753     if (fabs(aScalar->value() - myParams[aFirstParamPos].val) > tolerance) {
1754       aScalar->setValue(myParams[aFirstParamPos].val);
1755       return true;
1756     }
1757     return false;
1758   }
1759
1760   /// \todo Support other types of entities
1761   return false;
1762 }
1763
1764 // ============================================================================
1765 //  Function: updateEntityIfPossible
1766 //  Class:    SketchSolver_ConstraintGroup
1767 //  Purpose:  search the entity in this group and update it
1768 // ============================================================================
1769 void SketchSolver_ConstraintGroup::updateEntityIfPossible(
1770     std::shared_ptr<ModelAPI_Attribute> theEntity)
1771 {
1772   if (myEntityAttrMap.find(theEntity) != myEntityAttrMap.end()) {
1773     // If the attribute is a point and it is changed (the group needs to rebuild),
1774     // probably user has dragged this point into this position,
1775     // so it is necessary to add constraint which will guarantee the point will not change
1776
1777     // Store myNeedToSolve flag to verify the entity is really changed
1778     bool aNeedToSolveCopy = myNeedToSolve;
1779     myNeedToSolve = false;
1780
1781     changeEntity(theEntity);
1782
1783     if (myNeedToSolve)  // the entity is changed
1784     {
1785       // Verify the entity is a point and add temporary constraint of permanency
1786       std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1787           theEntity);
1788       std::shared_ptr<GeomDataAPI_Point2D> aPoint2D = std::dynamic_pointer_cast<
1789           GeomDataAPI_Point2D>(theEntity);
1790       if (aPoint || aPoint2D)
1791         addTemporaryConstraintWhereDragged(theEntity);
1792     }
1793
1794     // Restore flag of changes
1795     myNeedToSolve = myNeedToSolve || aNeedToSolveCopy;
1796
1797     if (myNeedToSolve)
1798       updateRelatedConstraints(theEntity);
1799   }
1800 }
1801
1802 // ============================================================================
1803 //  Function: addTemporaryConstraintWhereDragged
1804 //  Class:    SketchSolver_ConstraintGroup
1805 //  Purpose:  add transient constraint SLVS_C_WHERE_DRAGGED for the entity,
1806 //            which was moved by user
1807 // ============================================================================
1808 void SketchSolver_ConstraintGroup::addTemporaryConstraintWhereDragged(
1809     std::shared_ptr<ModelAPI_Attribute> theEntity,
1810     bool theAllowToFit)
1811 {
1812   // Find identifier of the entity
1813   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator anEntIter =
1814       myEntityAttrMap.find(theEntity);
1815   if (anEntIter == myEntityAttrMap.end())
1816     return;
1817
1818   // Get identifiers of all dragged points
1819   std::set<Slvs_hEntity> aDraggedPntID;
1820   aDraggedPntID.insert(myTempPointWDrgdID);
1821   std::list<Slvs_hConstraint>::const_iterator aTmpCoIter = myTempConstraints.begin();
1822   for (; aTmpCoIter != myTempConstraints.end(); aTmpCoIter++) {
1823     unsigned int aConstrPos = Search(*aTmpCoIter, myConstraints);
1824     if (aConstrPos < myConstraints.size())
1825       aDraggedPntID.insert(myConstraints[aConstrPos].ptA);
1826   }
1827   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1828   for (; aConstrIter != myConstraints.end(); aConstrIter++)
1829     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED)
1830       aDraggedPntID.insert(aConstrIter->ptA);
1831   // Find whether there is a point coincident with theEntity, which already has SLVS_C_WHERE_DRAGGED
1832   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
1833   for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++) {
1834     if (aCoPtIter->find(anEntIter->second) == aCoPtIter->end())
1835       continue;  // the entity was not found in current set
1836
1837     // Find one of already created SLVS_C_WHERE_DRAGGED constraints in current set of coincident points
1838     std::set<Slvs_hEntity>::const_iterator aDrgIter = aDraggedPntID.begin();
1839     for (; aDrgIter != aDraggedPntID.end(); aDrgIter++)
1840       if (aCoPtIter->find(*aDrgIter) != aCoPtIter->end())
1841         return;  // the SLVS_C_WHERE_DRAGGED constraint already exists
1842   }
1843   if (aDraggedPntID.find(anEntIter->second) != aDraggedPntID.end())
1844     return;
1845
1846   // If this is a first dragged point, its parameters should be placed
1847   // into Slvs_System::dragged field to avoid system inconsistense
1848   if (myTempPointWhereDragged.empty() && theAllowToFit) {
1849     int anEntPos = Search(anEntIter->second, myEntities);
1850     Slvs_hParam* aDraggedParam = myEntities[anEntPos].param;
1851     for (int i = 0; i < 4; i++, aDraggedParam++)
1852       if (*aDraggedParam != 0)
1853         myTempPointWhereDragged.push_back(*aDraggedParam);
1854     myTempPointWDrgdID = myEntities[anEntPos].h;
1855     return;
1856   }
1857
1858   // Create additional SLVS_C_WHERE_DRAGGED constraint if myTempPointWhereDragged field is not empty
1859   Slvs_Constraint aWDConstr = Slvs_MakeConstraint(++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED,
1860                                                   myWorkplane.h, 0.0, anEntIter->second, 0, 0, 0);
1861   myConstraints.push_back(aWDConstr);
1862   myTempConstraints.push_back(aWDConstr.h);
1863 }
1864
1865 // ============================================================================
1866 //  Function: removeTemporaryConstraints
1867 //  Class:    SketchSolver_ConstraintGroup
1868 //  Purpose:  remove all transient SLVS_C_WHERE_DRAGGED constraints after
1869 //            resolving the set of constraints
1870 // ============================================================================
1871 void SketchSolver_ConstraintGroup::removeTemporaryConstraints(
1872     const std::set<Slvs_hConstraint>& theRemoved)
1873 {
1874   std::list<Slvs_hConstraint>::reverse_iterator aTmpConstrIter;
1875   for (aTmpConstrIter = myTempConstraints.rbegin(); aTmpConstrIter != myTempConstraints.rend();
1876       aTmpConstrIter++) {
1877     if (!theRemoved.empty() && theRemoved.find(*aTmpConstrIter) == theRemoved.end())
1878       continue;
1879     unsigned int aConstrPos = Search(*aTmpConstrIter, myConstraints);
1880     if (aConstrPos >= myConstraints.size())
1881       continue;
1882     myConstraints.erase(myConstraints.begin() + aConstrPos);
1883
1884     // If the removing constraint has higher index, decrease the indexer
1885     if (*aTmpConstrIter == myConstrMaxID)
1886       myConstrMaxID--;
1887   }
1888   myTempConstraints.clear();
1889
1890   // Clear basic dragged point
1891   myTempPointWhereDragged.clear();
1892   myTempPointWDrgdID = SLVS_E_UNKNOWN;
1893 }
1894
1895 // ============================================================================
1896 //  Function: removeConstraint
1897 //  Class:    SketchSolver_ConstraintGroup
1898 //  Purpose:  remove constraint and all unused entities
1899 // ============================================================================
1900 void SketchSolver_ConstraintGroup::removeConstraint(
1901     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
1902 {
1903   ConstraintMap::iterator anIterToRemove = myConstraintMap.find(theConstraint);
1904   if (anIterToRemove == myConstraintMap.end())
1905     return;
1906
1907   std::vector<Slvs_hConstraint> aCnstrToRemove = anIterToRemove->second;
1908   // Remove constraint from the map
1909   myConstraintMap.erase(anIterToRemove);
1910
1911   std::set<Slvs_hEntity> anEntToRemove;
1912
1913   // Find unused entities
1914   std::vector<Slvs_hConstraint>::iterator aCnstrToRemoveIter = aCnstrToRemove.begin();
1915   for (; aCnstrToRemoveIter != aCnstrToRemove.end(); aCnstrToRemoveIter++) {
1916     int aConstrPos = Search(*aCnstrToRemoveIter, myConstraints);
1917     Slvs_hEntity aCnstEnt[] = { myConstraints[aConstrPos].ptA, myConstraints[aConstrPos].ptB,
1918         myConstraints[aConstrPos].entityA, myConstraints[aConstrPos].entityB };
1919     for (int i = 0; i < 4; i++)
1920       if (aCnstEnt[i] != 0)
1921         anEntToRemove.insert(aCnstEnt[i]);
1922     myConstraints.erase(myConstraints.begin() + aConstrPos);
1923     if (*aCnstrToRemoveIter == myConstrMaxID)
1924       myConstrMaxID--;
1925   }
1926
1927   // Find all entities which are based on these unused
1928   std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
1929   for ( ; anEntIter != myEntities.end(); anEntIter++)
1930     if (anEntIter->type == SLVS_E_LINE_SEGMENT || anEntIter->type == SLVS_E_CIRCLE ||
1931         anEntIter->type == SLVS_E_ARC_OF_CIRCLE) {
1932       for (int i = 0; i < 4; i++)
1933         if (anEntToRemove.find(anEntIter->point[i]) != anEntToRemove.end()) {
1934           anEntToRemove.insert(anEntIter->h);
1935           for (int j = 0; j < 4; j++)
1936             if (anEntIter->point[j] != 0)
1937               anEntToRemove.insert(anEntIter->point[j]);
1938           break;
1939         }
1940     }
1941
1942   // Find entities used by remaining constraints and remove them from the list to delete
1943   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1944   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1945     Slvs_hEntity aEnts[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA, aConstrIter
1946         ->entityB };
1947     for (int i = 0; i < 4; i++)
1948       if (aEnts[i] != 0 && anEntToRemove.find(aEnts[i]) != anEntToRemove.end()) {
1949         anEntToRemove.erase(aEnts[i]);
1950         // remove from the list all points of current entity
1951         int aEntPos = Search(aEnts[i], myEntities);
1952         for (int j = 0; j < 4; j++)
1953           if (myEntities[aEntPos].point[j] != 0)
1954             anEntToRemove.erase(myEntities[aEntPos].point[j]);
1955       }
1956   }
1957
1958   if (anEntToRemove.empty())
1959     return;
1960
1961   // Remove unused entities
1962   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntAttrIter =
1963       myEntityAttrMap.begin();
1964   while (anEntAttrIter != myEntityAttrMap.end()) {
1965     if (anEntToRemove.find(anEntAttrIter->second) != anEntToRemove.end()) {
1966       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator aRemovedIter =
1967           anEntAttrIter;
1968       anEntAttrIter++;
1969       myEntityAttrMap.erase(aRemovedIter);
1970     } else
1971       anEntAttrIter++;
1972   }
1973   std::map<FeaturePtr, Slvs_hEntity>::iterator anEntFeatIter = myEntityFeatMap.begin();
1974   while (anEntFeatIter != myEntityFeatMap.end()) {
1975     if (anEntToRemove.find(anEntFeatIter->second) != anEntToRemove.end()) {
1976       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = anEntFeatIter;
1977       anEntFeatIter++;
1978       myEntityFeatMap.erase(aRemovedIter);
1979     } else
1980       anEntFeatIter++;
1981   }
1982
1983   removeEntitiesById(anEntToRemove);
1984
1985   if (myCoincidentPoints.size() == 1 && myCoincidentPoints.front().empty())
1986     myCoincidentPoints.clear();
1987 }
1988
1989 // ============================================================================
1990 //  Function: removeEntitiesById
1991 //  Class:    SketchSolver_ConstraintGroup
1992 //  Purpose:  Removes specified entities and their parameters
1993 // ============================================================================
1994 void SketchSolver_ConstraintGroup::removeEntitiesById(const std::set<Slvs_hEntity>& theEntities)
1995 {
1996   std::set<Slvs_hEntity>::const_reverse_iterator aRemIter = theEntities.rbegin();
1997   for (; aRemIter != theEntities.rend(); aRemIter++) {
1998     unsigned int anEntPos = Search(*aRemIter, myEntities);
1999     if (anEntPos >= myEntities.size())
2000       continue;
2001     if (myEntities[anEntPos].param[0] != 0) {
2002       unsigned int aParamPos = Search(myEntities[anEntPos].param[0], myParams);
2003       if (aParamPos >= myParams.size())
2004         continue;
2005       int aNbParams = 0;
2006       while (myEntities[anEntPos].param[aNbParams] != 0)
2007         aNbParams++;
2008       if (myEntities[anEntPos].param[aNbParams - 1] == myParamMaxID)
2009         myParamMaxID -= aNbParams;
2010       myParams.erase(myParams.begin() + aParamPos, myParams.begin() + aParamPos + aNbParams);
2011       if (*aRemIter == myEntityMaxID)
2012         myEntityMaxID--;
2013     }
2014     myEntities.erase(myEntities.begin() + anEntPos);
2015     myEntOfConstr.erase(myEntOfConstr.begin() + anEntPos);
2016
2017     // Remove entity's ID from the lists of conincident points
2018     std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
2019     for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++)
2020       aCoPtIter->erase(*aRemIter);
2021   }
2022 }
2023
2024 // ============================================================================
2025 //  Function: addCoincidentPoints
2026 //  Class:    SketchSolver_ConstraintGroup
2027 //  Purpose:  add coincident point the appropriate list of such points
2028 // ============================================================================
2029 bool SketchSolver_ConstraintGroup::addCoincidentPoints(const Slvs_hEntity& thePoint1,
2030                                                        const Slvs_hEntity& thePoint2)
2031 {
2032   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
2033   std::vector<std::set<Slvs_hEntity> >::iterator aFirstFound = myCoincidentPoints.end();
2034   while (aCoPtIter != myCoincidentPoints.end()) {
2035     bool isFound[2] = {  // indicate which point ID was already in coincidence constraint
2036         aCoPtIter->find(thePoint1) != aCoPtIter->end(), aCoPtIter->find(thePoint2)
2037             != aCoPtIter->end(), };
2038     if (isFound[0] && isFound[1])  // points are already connected by coincidence constraints => no need additional one
2039       return false;
2040     if ((isFound[0] && !isFound[1]) || (!isFound[0] && isFound[1])) {
2041       if (aFirstFound != myCoincidentPoints.end()) {  // there are two groups of coincident points connected by created constraint => merge them
2042         int aFirstFoundShift = aFirstFound - myCoincidentPoints.begin();
2043         int aCurrentShift = aCoPtIter - myCoincidentPoints.begin();
2044         aFirstFound->insert(aCoPtIter->begin(), aCoPtIter->end());
2045         myCoincidentPoints.erase(aCoPtIter);
2046         aFirstFound = myCoincidentPoints.begin() + aFirstFoundShift;
2047         aCoPtIter = myCoincidentPoints.begin() + aCurrentShift;
2048         continue;
2049       } else {
2050         aCoPtIter->insert(isFound[0] ? thePoint2 : thePoint1);
2051         aFirstFound = aCoPtIter;
2052       }
2053     }
2054     aCoPtIter++;
2055   }
2056   // No points were found, need to create new set
2057   if (aFirstFound == myCoincidentPoints.end()) {
2058     std::set<Slvs_hEntity> aNewSet;
2059     aNewSet.insert(thePoint1);
2060     aNewSet.insert(thePoint2);
2061     myCoincidentPoints.push_back(aNewSet);
2062   }
2063
2064   return true;
2065 }
2066
2067 // ============================================================================
2068 //  Function: updateRelatedConstraints
2069 //  Class:    SketchSolver_ConstraintGroup
2070 //  Purpose:  emit the signal to update constraints
2071 // ============================================================================
2072 void SketchSolver_ConstraintGroup::updateRelatedConstraints(
2073     std::shared_ptr<ModelAPI_Attribute> theEntity) const
2074 {
2075   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2076   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2077     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2078         ->attributes(std::string());
2079
2080     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2081     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2082       bool isUpd = (*anAttrIter == theEntity);
2083       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2084           ModelAPI_AttributeRefAttr>(*anAttrIter);
2085       if (aRefAttr && !aRefAttr->isObject() && aRefAttr->attr() == theEntity)
2086         isUpd = true;
2087
2088       if (isUpd) {
2089         static Events_ID anEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
2090         ModelAPI_EventCreator::get()->sendUpdated(aConstrIter->first, anEvent);
2091         break;
2092       }
2093     }
2094   }
2095 }
2096
2097 void SketchSolver_ConstraintGroup::updateRelatedConstraintsFeature(
2098     std::shared_ptr<ModelAPI_Feature> theFeature) const
2099 {
2100   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2101   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2102     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2103         ->attributes(std::string());
2104
2105     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2106     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2107       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2108           ModelAPI_AttributeRefAttr>(*anAttrIter);
2109       if (aRefAttr && aRefAttr->isObject() && aRefAttr->object() == theFeature) {
2110         static Events_ID anEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
2111         ModelAPI_EventCreator::get()->sendUpdated(aConstrIter->first, anEvent);
2112         break;
2113       }
2114     }
2115   }
2116 }
2117
2118 // ============================================================================
2119 //  Function: updateFilletConstraints
2120 //  Class:    SketchSolver_ConstraintGroup
2121 //  Purpose:  change fillet arc to be less than 180 degree
2122 // ============================================================================
2123 void SketchSolver_ConstraintGroup::updateFilletConstraints()
2124 {
2125   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2126   for (; aConstrIter != myConstraintMap.end(); aConstrIter++)
2127     if (aConstrIter->first->getKind() == SketchPlugin_ConstraintFillet::ID()) {
2128       AttributeRefListPtr aFilletRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
2129           aConstrIter->first->data()->attribute(SketchPlugin_ConstraintFillet::ENTITY_C()));
2130       if (!aFilletRefList)
2131         return;
2132       ObjectPtr anArcObj = aFilletRefList->object(2);
2133       std::shared_ptr<GeomDataAPI_Point2D> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2134           anArcObj->data()->attribute(SketchPlugin_Arc::CENTER_ID()));
2135       std::shared_ptr<GeomDataAPI_Point2D> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2136           anArcObj->data()->attribute(SketchPlugin_Arc::START_ID()));
2137       std::shared_ptr<GeomDataAPI_Point2D> aEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2138           anArcObj->data()->attribute(SketchPlugin_Arc::END_ID()));
2139       double aCosA = aStart->x() - aCenter->x();
2140       double aSinA = aStart->y() - aCenter->y();
2141       double aCosB = aEnd->x() - aCenter->x();
2142       double aSinB = aEnd->y() - aCenter->y();
2143       if (aCosA * aSinB - aSinA * aCosB <= 0.0) {
2144         anArcObj->data()->blockSendAttributeUpdated(true);
2145         double x = aStart->x();
2146         double y = aStart->y();
2147         aStart->setValue(aEnd->x(), aEnd->y());
2148         aEnd->setValue(x, y);
2149         // Update constraint data
2150         changeFilletConstraint(aConstrIter->first);
2151         anArcObj->data()->blockSendAttributeUpdated(false);
2152       }
2153     }
2154 }
2155
2156 // ============================================================================
2157 //  Function: makeMirrorEntity
2158 //  Class:    SketchSolver_ConstraintGroup
2159 //  Purpose:  change entities parameters to make them symmetric relating to the mirror line
2160 // ============================================================================
2161 void SketchSolver_ConstraintGroup::makeMirrorEntity(const Slvs_hEntity& theBase,
2162                                                     const Slvs_hEntity& theMirror,
2163                                                     const Slvs_hEntity& theMirrorLine)
2164 {
2165   Slvs_Entity aBase = myEntities[Search(theBase, myEntities)];
2166   Slvs_Entity aMirror = myEntities[Search(theMirror, myEntities)];
2167   int i = 0;
2168   while (aBase.point[i] != 0 && aMirror.point[i] != 0) {
2169     makeMirrorEntity(aBase.point[i], aMirror.point[i], theMirrorLine);
2170     i++;
2171   }
2172   if (aBase.param[0] != 0 && aMirror.param[0] != 0) { // this is a point, copy it
2173     Slvs_Entity aMirrorLine = myEntities[Search(theMirrorLine, myEntities)];
2174     std::shared_ptr<GeomAPI_XY> aLinePoints[2];
2175     for (i = 0; i < 2; i++) {
2176       Slvs_Entity aPoint = myEntities[Search(aMirrorLine.point[i], myEntities)];
2177       int aParamPos = Search(aPoint.param[0], myParams);
2178       aLinePoints[i] = std::shared_ptr<GeomAPI_XY>(
2179           new GeomAPI_XY(myParams[aParamPos].val, myParams[1+aParamPos].val));
2180     }
2181     int aBaseParamPos = Search(aBase.param[0], myParams);
2182     int aMirrorParamPos = Search(aMirror.param[0], myParams);
2183     std::shared_ptr<GeomAPI_XY> aPoint = std::shared_ptr<GeomAPI_XY>(
2184         new GeomAPI_XY(myParams[aBaseParamPos].val, myParams[1+aBaseParamPos].val));
2185
2186     // direction of a mirror line
2187     std::shared_ptr<GeomAPI_Dir2d> aDir = std::shared_ptr<GeomAPI_Dir2d>(
2188         new GeomAPI_Dir2d(aLinePoints[1]->added(aLinePoints[0]->multiplied(-1.0))));
2189     // orthogonal direction
2190     aDir = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(aDir->y(), -aDir->x()));
2191
2192     std::shared_ptr<GeomAPI_XY> aVec = std::shared_ptr<GeomAPI_XY>(
2193         new GeomAPI_XY(aPoint->x() - aLinePoints[0]->x(), aPoint->y() - aLinePoints[0]->y()));
2194     double aDist = aVec->dot(aDir->xy());
2195     std::shared_ptr<GeomAPI_XY> aMirrorPoint = aPoint->added(aDir->xy()->multiplied(-2.0 * aDist));
2196
2197     myParams[aMirrorParamPos].val = aMirrorPoint->x();
2198     myParams[1+aMirrorParamPos].val = aMirrorPoint->y();
2199   }
2200 }
2201
2202 // ============================================================================
2203 //  Function: calculateMiddlePoint
2204 //  Class:    SketchSolver_ConstraintGroup
2205 //  Purpose:  calculates middle point on line or arc
2206 // ============================================================================
2207 void SketchSolver_ConstraintGroup::calculateMiddlePoint(
2208     const Slvs_hEntity& theEntity,
2209     double& theX,
2210     double& theY) const
2211 {
2212   int anInd = Search(theEntity, myEntities);
2213   if (myEntities[anInd].type == SLVS_E_LINE_SEGMENT) {
2214     int aLineParams[2];
2215     for (int i = 0; i < 2; i++) {
2216       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2217       aLineParams[i] = Search(myEntities[aPtPos].param[0], myParams);
2218     }
2219     theX = 0.5 * (myParams[aLineParams[0]].val + myParams[aLineParams[1]].val);
2220     theY = 0.5 * (myParams[1 + aLineParams[0]].val + myParams[1 + aLineParams[1]].val);
2221   } else if (myEntities[anInd].type == SLVS_E_ARC_OF_CIRCLE) {
2222     double anArcPoint[3][2];
2223     for (int i = 0; i < 3; i++) {
2224       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2225       int anArcParam = Search(myEntities[aPtPos].param[0], myParams);
2226       anArcPoint[i][0] = myParams[anArcParam].val;
2227       anArcPoint[i][1] = myParams[1 + anArcParam].val;
2228     }
2229     // project last point of arc on the arc
2230     double x = anArcPoint[1][0] - anArcPoint[0][0];
2231     double y = anArcPoint[1][1] - anArcPoint[0][1];
2232     double aRad = sqrt(x*x + y*y);
2233     x = anArcPoint[2][0] - anArcPoint[0][0];
2234     y = anArcPoint[2][1] - anArcPoint[0][1];
2235     double aNorm = sqrt(x*x + y*y);
2236     if (aNorm >= tolerance) {
2237       anArcPoint[2][0] = anArcPoint[0][0] + x * aRad / aNorm;
2238       anArcPoint[2][1] = anArcPoint[0][1] + y * aRad / aNorm;
2239     }
2240
2241     x = anArcPoint[1][0] + anArcPoint[2][0] - 2.0 * anArcPoint[0][0];
2242     y = anArcPoint[1][1] + anArcPoint[2][1] - 2.0 * anArcPoint[0][1];
2243     aNorm = sqrt(x*x + y*y);
2244     if (aNorm >= tolerance) {
2245       x *= aRad / aNorm;
2246       y *= aRad / aNorm;
2247     } else { // obtain orthogonal direction
2248       x = 0.5 * (anArcPoint[2][1] - anArcPoint[1][1]);
2249       y = -0.5 * (anArcPoint[2][0] - anArcPoint[1][0]);
2250     }
2251     theX = anArcPoint[0][0] + x;
2252     theY = anArcPoint[0][1] + y;
2253   }
2254 }
2255
2256
2257 // ========================================================
2258 // =========      Auxiliary functions       ===============
2259 // ========================================================
2260
2261 template<typename T>
2262 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
2263 {
2264   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
2265   int aVecSize = theEntities.size();
2266   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
2267     aResIndex--;
2268   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
2269     aResIndex++;
2270   if (aResIndex == -1)
2271     aResIndex = aVecSize;
2272   return aResIndex;
2273 }