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,
1303     std::shared_ptr<ModelAPI_Attribute> theNorm)
1304 {
1305   std::shared_ptr<GeomDataAPI_Dir> aNorm = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theNorm);
1306   std::shared_ptr<GeomDataAPI_Dir> aDirX = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirX);
1307   if (!aDirX || (fabs(aDirX->x()) + fabs(aDirX->y()) + fabs(aDirX->z()) < tolerance))
1308     return SLVS_E_UNKNOWN;
1309   // calculate Y direction
1310   std::shared_ptr<GeomAPI_Dir> aDirY(new GeomAPI_Dir(aNorm->dir()->cross(aDirX->dir())));
1311
1312   // quaternion parameters of normal vector
1313   double qw, qx, qy, qz;
1314   Slvs_MakeQuaternion(aDirX->x(), aDirX->y(), aDirX->z(), aDirY->x(), aDirY->y(), aDirY->z(), &qw,
1315                       &qx, &qy, &qz);
1316   double aNormCoord[4] = { qw, qx, qy, qz };
1317
1318   // Try to find existent normal
1319   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aEntIter =
1320       myEntityAttrMap.find(theNorm);
1321   std::vector<Slvs_Param>::const_iterator aParamIter;  // looks to the first parameter of already existent entity or to the end of vector otherwise
1322   if (aEntIter == myEntityAttrMap.end())  // no such entity => should be created
1323     aParamIter = myParams.end();
1324   else {  // the entity already exists, update it
1325     int aEntPos = Search(aEntIter->second, myEntities);
1326     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
1327     aParamIter = myParams.begin() + aParamPos;
1328   }
1329
1330   // Change parameters of the normal
1331   Slvs_hParam aNormParams[4];
1332   for (int i = 0; i < 4; i++)
1333     aNormParams[i] = changeParameter(aNormCoord[i], aParamIter);
1334
1335   if (aEntIter != myEntityAttrMap.end())  // the entity already exists
1336     return aEntIter->second;
1337
1338   // Create a normal
1339   Slvs_Entity aNormal = Slvs_MakeNormal3d(++myEntityMaxID, myID, aNormParams[0], aNormParams[1],
1340                                           aNormParams[2], aNormParams[3]);
1341   myEntities.push_back(aNormal);
1342   myEntOfConstr.push_back(false);
1343   myEntityAttrMap[theNorm] = aNormal.h;
1344   return aNormal.h;
1345 }
1346
1347 // ============================================================================
1348 //  Function: addWorkplane
1349 //  Class:    SketchSolver_ConstraintGroup
1350 //  Purpose:  create workplane for the group
1351 // ============================================================================
1352 bool SketchSolver_ConstraintGroup::addWorkplane(std::shared_ptr<ModelAPI_CompositeFeature> theSketch)
1353 {
1354   if (myWorkplane.h || theSketch->getKind().compare(SketchPlugin_Sketch::ID()) != 0)
1355     return false;  // the workplane already exists or the function parameter is not Sketch
1356
1357   mySketch = theSketch;
1358   updateWorkplane();
1359   return true;
1360 }
1361
1362 // ============================================================================
1363 //  Function: updateWorkplane
1364 //  Class:    SketchSolver_ConstraintGroup
1365 //  Purpose:  update parameters of workplane
1366 // ============================================================================
1367 bool SketchSolver_ConstraintGroup::updateWorkplane()
1368 {
1369   if (!mySketch->data())
1370     return false; // case sketch is deleted
1371   // Get parameters of workplane
1372   std::shared_ptr<ModelAPI_Attribute> aDirX = mySketch->data()->attribute(
1373       SketchPlugin_Sketch::DIRX_ID());
1374   std::shared_ptr<ModelAPI_Attribute> aNorm = mySketch->data()->attribute(
1375       SketchPlugin_Sketch::NORM_ID());
1376   std::shared_ptr<ModelAPI_Attribute> anOrigin = mySketch->data()->attribute(
1377       SketchPlugin_Sketch::ORIGIN_ID());
1378   // Transform them into SolveSpace format
1379   Slvs_hEntity aNormalWP = changeNormal(aDirX, aNorm);
1380   if (!aNormalWP)
1381     return false;
1382   Slvs_hEntity anOriginWP = changeEntity(anOrigin);
1383   if (!anOriginWP)
1384     return false;
1385
1386   if (!myWorkplane.h) {
1387     // Create workplane
1388     myWorkplane = Slvs_MakeWorkplane(++myEntityMaxID, myID, anOriginWP, aNormalWP);
1389     // Workplane should be added to the list of entities
1390     myEntities.push_back(myWorkplane);
1391     myEntOfConstr.push_back(false);
1392   }
1393   return true;
1394 }
1395
1396 // ============================================================================
1397 //  Function: changeParameter
1398 //  Class:    SketchSolver_ConstraintGroup
1399 //  Purpose:  create/update value of parameter
1400 // ============================================================================
1401 Slvs_hParam SketchSolver_ConstraintGroup::changeParameter(
1402     const double& theParam, std::vector<Slvs_Param>::const_iterator& thePrmIter)
1403 {
1404   if (thePrmIter != myParams.end()) {  // Parameter should be updated
1405     int aParamPos = thePrmIter - myParams.begin();
1406     if (fabs(thePrmIter->val - theParam) > tolerance) {
1407       myNeedToSolve = true;  // parameter is changed, need to resolve constraints
1408       myParams[aParamPos].val = theParam;
1409     }
1410     thePrmIter++;
1411     return myParams[aParamPos].h;
1412   }
1413
1414   // Newly created parameter
1415   Slvs_Param aParam = Slvs_MakeParam(++myParamMaxID, myID, theParam);
1416   myParams.push_back(aParam);
1417   myNeedToSolve = true;
1418   // The list of parameters is changed, move iterator to the end of the list to avoid problems
1419   thePrmIter = myParams.end();
1420   return aParam.h;
1421 }
1422
1423 // ============================================================================
1424 //  Function: resolveConstraints
1425 //  Class:    SketchSolver_ConstraintGroup
1426 //  Purpose:  solve the set of constraints for the current group
1427 // ============================================================================
1428 bool SketchSolver_ConstraintGroup::resolveConstraints()
1429 {
1430   if (!myNeedToSolve)
1431     return false;
1432
1433   myConstrSolver.setGroupID(myID);
1434   myConstrSolver.setParameters(myParams);
1435   myConstrSolver.setEntities(myEntities);
1436   myConstrSolver.setConstraints(myConstraints);
1437   myConstrSolver.setDraggedParameters(myTempPointWhereDragged);
1438
1439   int aResult = myConstrSolver.solve();
1440   if (aResult == SLVS_RESULT_OKAY) {  // solution succeeded, store results into correspondent attributes
1441                                       // Obtain result into the same list of parameters
1442     if (!myConstrSolver.getResult(myParams))
1443       return true;
1444
1445     // We should go through the attributes map, because only attributes have valued parameters
1446     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntIter =
1447         myEntityAttrMap.begin();
1448     for (; anEntIter != myEntityAttrMap.end(); anEntIter++) {
1449       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1450         anEntIter->first->owner()->data()->blockSendAttributeUpdated(true);
1451       if (updateAttribute(anEntIter->first, anEntIter->second))
1452         updateRelatedConstraints(anEntIter->first);
1453     }
1454     updateFilletConstraints();
1455     // unblock all features then
1456     for (anEntIter = myEntityAttrMap.begin(); anEntIter != myEntityAttrMap.end(); anEntIter++) {
1457       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1458         anEntIter->first->owner()->data()->blockSendAttributeUpdated(false);
1459     }
1460   } else if (!myConstraints.empty())
1461     Events_Error::send(SketchSolver_Error::CONSTRAINTS(), this);
1462
1463   removeTemporaryConstraints();
1464   myNeedToSolve = false;
1465   return true;
1466 }
1467
1468 // ============================================================================
1469 //  Function: mergeGroups
1470 //  Class:    SketchSolver_ConstraintGroup
1471 //  Purpose:  append specified group to the current group
1472 // ============================================================================
1473 void SketchSolver_ConstraintGroup::mergeGroups(const SketchSolver_ConstraintGroup& theGroup)
1474 {
1475   // If specified group is empty, no need to merge
1476   if (theGroup.myConstraintMap.empty())
1477     return;
1478
1479   // Map between old and new indexes of SolveSpace constraints
1480   std::map<Slvs_hConstraint, Slvs_hConstraint> aConstrMap;
1481
1482   // Add all constraints from theGroup to the current group
1483   ConstraintMap::const_iterator aConstrIter = theGroup.myConstraintMap.begin();
1484   for (; aConstrIter != theGroup.myConstraintMap.end(); aConstrIter++)
1485     if (changeConstraint(aConstrIter->first))
1486       aConstrMap[aConstrIter->second.back()] = myConstrMaxID;  // the constraint was added => store its ID
1487
1488   // Add temporary constraints from theGroup
1489   std::list<Slvs_hConstraint>::const_iterator aTempConstrIter = theGroup.myTempConstraints.begin();
1490   for (; aTempConstrIter != theGroup.myTempConstraints.end(); aTempConstrIter++) {
1491     std::map<Slvs_hConstraint, Slvs_hConstraint>::iterator aFind = aConstrMap.find(
1492         *aTempConstrIter);
1493     if (aFind != aConstrMap.end())
1494       myTempConstraints.push_back(aFind->second);
1495   }
1496
1497   if (myTempPointWhereDragged.empty())
1498     myTempPointWhereDragged = theGroup.myTempPointWhereDragged;
1499   else if (!theGroup.myTempPointWhereDragged.empty()) {  // Need to create additional transient constraint
1500     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aFeatureIter =
1501         theGroup.myEntityAttrMap.begin();
1502     for (; aFeatureIter != theGroup.myEntityAttrMap.end(); aFeatureIter++)
1503       if (aFeatureIter->second == myTempPointWDrgdID) {
1504         addTemporaryConstraintWhereDragged(aFeatureIter->first);
1505         break;
1506       }
1507   }
1508
1509   myNeedToSolve = myNeedToSolve || theGroup.myNeedToSolve;
1510 }
1511
1512 // ============================================================================
1513 //  Function: splitGroup
1514 //  Class:    SketchSolver_ConstraintGroup
1515 //  Purpose:  divide the group into several subgroups
1516 // ============================================================================
1517 void SketchSolver_ConstraintGroup::splitGroup(std::vector<SketchSolver_ConstraintGroup*>& theCuts)
1518 {
1519   // Divide constraints and entities into several groups
1520   std::vector<std::set<Slvs_hEntity> > aGroupsEntities;
1521   std::vector<std::set<Slvs_hConstraint> > aGroupsConstr;
1522   int aMaxNbEntities = 0;  // index of the group with maximal nuber of elements (this group will be left in the current)
1523   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1524   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1525     Slvs_hEntity aConstrEnt[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA,
1526         aConstrIter->entityB };
1527     std::vector<int> anIndexes;
1528     // Go through the groupped entities and find even one of entities of current constraint
1529     std::vector<std::set<Slvs_hEntity> >::iterator aGrEntIter;
1530     for (aGrEntIter = aGroupsEntities.begin(); aGrEntIter != aGroupsEntities.end(); aGrEntIter++) {
1531       bool isFound = false;
1532       for (int i = 0; i < 4 && !isFound; i++)
1533         if (aConstrEnt[i] != 0) {
1534           isFound = (aGrEntIter->find(aConstrEnt[i]) != aGrEntIter->end());
1535           // Also we need to check sub-entities
1536           int aEntPos = Search(aConstrEnt[i], myEntities);
1537           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1538             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1539             for (int j = 0; *aSub != 0 && j < 4 && !isFound; aSub++, j++)
1540               isFound = (aGrEntIter->find(*aSub) != aGrEntIter->end());
1541           }
1542         }
1543       if (isFound)
1544         anIndexes.push_back(aGrEntIter - aGroupsEntities.begin());
1545     }
1546     // Add new group if no one is found
1547     if (anIndexes.empty()) {
1548       std::set<Slvs_hEntity> aNewGrEnt;
1549       for (int i = 0; i < 4; i++)
1550         if (aConstrEnt[i] != 0) {
1551           aNewGrEnt.insert(aConstrEnt[i]);
1552           int aEntPos = Search(aConstrEnt[i], myEntities);
1553           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1554             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1555             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1556               aNewGrEnt.insert(*aSub);
1557           }
1558         }
1559       std::set<Slvs_hConstraint> aNewGrConstr;
1560       aNewGrConstr.insert(aConstrIter->h);
1561
1562       aGroupsEntities.push_back(aNewGrEnt);
1563       aGroupsConstr.push_back(aNewGrConstr);
1564       if (aNewGrEnt.size() > aGroupsEntities[aMaxNbEntities].size())
1565         aMaxNbEntities = aGroupsEntities.size() - 1;
1566     } else {  // Add entities indexes into the found group
1567       aGrEntIter = aGroupsEntities.begin() + anIndexes.front();
1568       for (int i = 0; i < 4; i++)
1569         if (aConstrEnt[i] != 0) {
1570           aGrEntIter->insert(aConstrEnt[i]);
1571           int aEntPos = Search(aConstrEnt[i], myEntities);
1572           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1573             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1574             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1575               aGrEntIter->insert(*aSub);
1576           }
1577         }
1578       aGroupsConstr[anIndexes.front()].insert(aConstrIter->h);
1579       if (aGrEntIter->size() > aGroupsEntities[aMaxNbEntities].size())
1580         aMaxNbEntities = aGrEntIter - aGroupsEntities.begin();
1581       if (anIndexes.size() > 1) {  // There are found several connected groups, merge them
1582         std::vector<std::set<Slvs_hEntity> >::iterator aFirstGroup = aGroupsEntities.begin()
1583             + anIndexes.front();
1584         std::vector<std::set<Slvs_hConstraint> >::iterator aFirstConstr = aGroupsConstr.begin()
1585             + anIndexes.front();
1586         std::vector<int>::iterator anInd = anIndexes.begin();
1587         for (++anInd; anInd != anIndexes.end(); anInd++) {
1588           aFirstGroup->insert(aGroupsEntities[*anInd].begin(), aGroupsEntities[*anInd].end());
1589           aFirstConstr->insert(aGroupsConstr[*anInd].begin(), aGroupsConstr[*anInd].end());
1590         }
1591         if (aFirstGroup->size() > aGroupsEntities[aMaxNbEntities].size())
1592           aMaxNbEntities = anIndexes.front();
1593         // Remove merged groups
1594         for (anInd = anIndexes.end() - 1; anInd != anIndexes.begin(); anInd--) {
1595           aGroupsEntities.erase(aGroupsEntities.begin() + (*anInd));
1596           aGroupsConstr.erase(aGroupsConstr.begin() + (*anInd));
1597         }
1598       }
1599     }
1600   }
1601
1602   if (aGroupsEntities.size() <= 1)
1603     return;
1604
1605   // Remove the group with maximum elements as it will be left in the current group
1606   aGroupsEntities.erase(aGroupsEntities.begin() + aMaxNbEntities);
1607   aGroupsConstr.erase(aGroupsConstr.begin() + aMaxNbEntities);
1608
1609   // Add new groups of constraints and divide current group
1610   std::vector<SketchSolver_ConstraintGroup*> aNewGroups;
1611   for (int i = aGroupsEntities.size(); i > 0; i--) {
1612     SketchSolver_ConstraintGroup* aG = new SketchSolver_ConstraintGroup(mySketch);
1613     aNewGroups.push_back(aG);
1614   }
1615   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.begin();
1616   int aConstrMapPos = 0;  // position of iterator in the map (used to restore iterator after removing constraint)
1617   while (aConstrMapIter != myConstraintMap.end()) {
1618     std::vector<std::set<Slvs_hConstraint> >::const_iterator aGIter = aGroupsConstr.begin();
1619     std::vector<SketchSolver_ConstraintGroup*>::iterator aGroup = aNewGroups.begin();
1620     for (; aGIter != aGroupsConstr.end(); aGIter++, aGroup++)
1621       if (aGIter->find(aConstrMapIter->second.front()) != aGIter->end()) {
1622         (*aGroup)->changeConstraint(aConstrMapIter->first);
1623         removeConstraint(aConstrMapIter->first);
1624         // restore iterator
1625         aConstrMapIter = myConstraintMap.begin();
1626         for (int i = 0; i < aConstrMapPos; i++)
1627           aConstrMapIter++;
1628         break;
1629       }
1630     if (aGIter == aGroupsConstr.end()) {
1631       aConstrMapIter++;
1632       aConstrMapPos++;
1633     }
1634   }
1635
1636   theCuts.insert(theCuts.end(), aNewGroups.begin(), aNewGroups.end());
1637 }
1638
1639 // ============================================================================
1640 //  Function: updateGroup
1641 //  Class:    SketchSolver_ConstraintGroup
1642 //  Purpose:  search removed entities and constraints
1643 // ============================================================================
1644 bool SketchSolver_ConstraintGroup::updateGroup()
1645 {
1646   ConstraintMap::reverse_iterator aConstrIter = myConstraintMap.rbegin();
1647   bool isAllValid = true;
1648   bool isCCRemoved = false;  // indicates that at least one of coincidence constraints was removed
1649   int aConstrIndex = 0;
1650   while (/*isAllValid && */aConstrIter != myConstraintMap.rend()) {
1651     if (!aConstrIter->first->data() || !aConstrIter->first->data()->isValid()) {
1652       if (aConstrIter->first->getKind().compare(SketchPlugin_ConstraintCoincidence::ID()) == 0)
1653         isCCRemoved = true;
1654       removeConstraint(aConstrIter->first);
1655       isAllValid = false;
1656       // Get back the correct position of iterator after the "remove" operation
1657       aConstrIter = myConstraintMap.rbegin();
1658       for (int i = 0; i < aConstrIndex && aConstrIter != myConstraintMap.rend(); i++)
1659         aConstrIter++;
1660     } else {
1661       aConstrIter++;
1662       aConstrIndex++;
1663     }
1664   }
1665
1666   // Check if some entities are invalid too
1667   std::set<Slvs_hEntity> anEntToRemove;
1668   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1669       anAttrIter = myEntityAttrMap.begin();
1670   while (anAttrIter != myEntityAttrMap.end()) {
1671     if (!anAttrIter->first->owner() || !anAttrIter->first->owner()->data() ||
1672         !anAttrIter->first->owner()->data()->isValid()) {
1673       anEntToRemove.insert(anAttrIter->second);
1674       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1675           aRemovedIter = anAttrIter;
1676       anAttrIter++;
1677       myEntityAttrMap.erase(aRemovedIter);
1678     } else
1679       anAttrIter++;
1680   }
1681   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIter = myEntityFeatMap.begin();
1682   while (aFeatIter != myEntityFeatMap.end()) {
1683     if (!aFeatIter->first || !aFeatIter->first->data() ||
1684         !aFeatIter->first->data()->isValid()) {
1685       anEntToRemove.insert(aFeatIter->second);
1686       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = aFeatIter;
1687       aFeatIter++;
1688       myEntityFeatMap.erase(aRemovedIter);
1689     } else
1690       aFeatIter++;
1691   }
1692   removeEntitiesById(anEntToRemove);
1693
1694   // Probably, need to update coincidence constraints
1695   if (isCCRemoved && !myExtraCoincidence.empty()) {
1696     // Make a copy, because the new list of unused constrtaints will be generated
1697     std::set<std::shared_ptr<SketchPlugin_Constraint> > anExtraCopy = myExtraCoincidence;
1698     myExtraCoincidence.clear();
1699
1700     std::set<std::shared_ptr<SketchPlugin_Constraint> >::iterator aCIter = anExtraCopy.begin();
1701     for (; aCIter != anExtraCopy.end(); aCIter++)
1702       if ((*aCIter)->data() && (*aCIter)->data()->isValid())
1703         changeConstraint(*aCIter);
1704   }
1705
1706   return !isAllValid;
1707 }
1708
1709 // ============================================================================
1710 //  Function: updateAttribute
1711 //  Class:    SketchSolver_ConstraintGroup
1712 //  Purpose:  update features of sketch after resolving constraints
1713 // ============================================================================
1714 bool SketchSolver_ConstraintGroup::updateAttribute(
1715     std::shared_ptr<ModelAPI_Attribute> theAttribute, const Slvs_hEntity& theEntityID)
1716 {
1717   // Search the position of the first parameter of the entity
1718   int anEntPos = Search(theEntityID, myEntities);
1719   int aFirstParamPos = Search(myEntities[anEntPos].param[0], myParams);
1720
1721   // Look over supported types of entities
1722
1723   // Point in 3D
1724   std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1725       theAttribute);
1726   if (aPoint) {
1727     if (fabs(aPoint->x() - myParams[aFirstParamPos].val) > tolerance
1728         || fabs(aPoint->y() - myParams[aFirstParamPos + 1].val) > tolerance
1729         || fabs(aPoint->z() - myParams[aFirstParamPos + 2].val) > tolerance) {
1730       aPoint->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val,
1731                        myParams[aFirstParamPos + 2].val);
1732       return true;
1733     }
1734     return false;
1735   }
1736
1737   // Point in 2D
1738   std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1739       std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theAttribute);
1740   if (aPoint2D) {
1741     if (fabs(aPoint2D->x() - myParams[aFirstParamPos].val) > tolerance
1742         || fabs(aPoint2D->y() - myParams[aFirstParamPos + 1].val) > tolerance) {
1743       aPoint2D->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val);
1744       return true;
1745     }
1746     return false;
1747   }
1748
1749   // Scalar value
1750   AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theAttribute);
1751   if (aScalar) {
1752     if (fabs(aScalar->value() - myParams[aFirstParamPos].val) > tolerance) {
1753       aScalar->setValue(myParams[aFirstParamPos].val);
1754       return true;
1755     }
1756     return false;
1757   }
1758
1759   /// \todo Support other types of entities
1760   return false;
1761 }
1762
1763 // ============================================================================
1764 //  Function: updateEntityIfPossible
1765 //  Class:    SketchSolver_ConstraintGroup
1766 //  Purpose:  search the entity in this group and update it
1767 // ============================================================================
1768 void SketchSolver_ConstraintGroup::updateEntityIfPossible(
1769     std::shared_ptr<ModelAPI_Attribute> theEntity)
1770 {
1771   if (myEntityAttrMap.find(theEntity) != myEntityAttrMap.end()) {
1772     // If the attribute is a point and it is changed (the group needs to rebuild),
1773     // probably user has dragged this point into this position,
1774     // so it is necessary to add constraint which will guarantee the point will not change
1775
1776     // Store myNeedToSolve flag to verify the entity is really changed
1777     bool aNeedToSolveCopy = myNeedToSolve;
1778     myNeedToSolve = false;
1779
1780     changeEntity(theEntity);
1781
1782     if (myNeedToSolve)  // the entity is changed
1783     {
1784       // Verify the entity is a point and add temporary constraint of permanency
1785       std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1786           theEntity);
1787       std::shared_ptr<GeomDataAPI_Point2D> aPoint2D = std::dynamic_pointer_cast<
1788           GeomDataAPI_Point2D>(theEntity);
1789       if (aPoint || aPoint2D)
1790         addTemporaryConstraintWhereDragged(theEntity);
1791     }
1792
1793     // Restore flag of changes
1794     myNeedToSolve = myNeedToSolve || aNeedToSolveCopy;
1795
1796     if (myNeedToSolve)
1797       updateRelatedConstraints(theEntity);
1798   }
1799 }
1800
1801 // ============================================================================
1802 //  Function: addTemporaryConstraintWhereDragged
1803 //  Class:    SketchSolver_ConstraintGroup
1804 //  Purpose:  add transient constraint SLVS_C_WHERE_DRAGGED for the entity,
1805 //            which was moved by user
1806 // ============================================================================
1807 void SketchSolver_ConstraintGroup::addTemporaryConstraintWhereDragged(
1808     std::shared_ptr<ModelAPI_Attribute> theEntity,
1809     bool theAllowToFit)
1810 {
1811   // Find identifier of the entity
1812   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator anEntIter =
1813       myEntityAttrMap.find(theEntity);
1814   if (anEntIter == myEntityAttrMap.end())
1815     return;
1816
1817   // Get identifiers of all dragged points
1818   std::set<Slvs_hEntity> aDraggedPntID;
1819   aDraggedPntID.insert(myTempPointWDrgdID);
1820   std::list<Slvs_hConstraint>::const_iterator aTmpCoIter = myTempConstraints.begin();
1821   for (; aTmpCoIter != myTempConstraints.end(); aTmpCoIter++) {
1822     unsigned int aConstrPos = Search(*aTmpCoIter, myConstraints);
1823     if (aConstrPos < myConstraints.size())
1824       aDraggedPntID.insert(myConstraints[aConstrPos].ptA);
1825   }
1826   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1827   for (; aConstrIter != myConstraints.end(); aConstrIter++)
1828     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED)
1829       aDraggedPntID.insert(aConstrIter->ptA);
1830   // Find whether there is a point coincident with theEntity, which already has SLVS_C_WHERE_DRAGGED
1831   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
1832   for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++) {
1833     if (aCoPtIter->find(anEntIter->second) == aCoPtIter->end())
1834       continue;  // the entity was not found in current set
1835
1836     // Find one of already created SLVS_C_WHERE_DRAGGED constraints in current set of coincident points
1837     std::set<Slvs_hEntity>::const_iterator aDrgIter = aDraggedPntID.begin();
1838     for (; aDrgIter != aDraggedPntID.end(); aDrgIter++)
1839       if (aCoPtIter->find(*aDrgIter) != aCoPtIter->end())
1840         return;  // the SLVS_C_WHERE_DRAGGED constraint already exists
1841   }
1842   if (aDraggedPntID.find(anEntIter->second) != aDraggedPntID.end())
1843     return;
1844
1845   // If this is a first dragged point, its parameters should be placed
1846   // into Slvs_System::dragged field to avoid system inconsistense
1847   if (myTempPointWhereDragged.empty() && theAllowToFit) {
1848     int anEntPos = Search(anEntIter->second, myEntities);
1849     Slvs_hParam* aDraggedParam = myEntities[anEntPos].param;
1850     for (int i = 0; i < 4; i++, aDraggedParam++)
1851       if (*aDraggedParam != 0)
1852         myTempPointWhereDragged.push_back(*aDraggedParam);
1853     myTempPointWDrgdID = myEntities[anEntPos].h;
1854     return;
1855   }
1856
1857   // Create additional SLVS_C_WHERE_DRAGGED constraint if myTempPointWhereDragged field is not empty
1858   Slvs_Constraint aWDConstr = Slvs_MakeConstraint(++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED,
1859                                                   myWorkplane.h, 0.0, anEntIter->second, 0, 0, 0);
1860   myConstraints.push_back(aWDConstr);
1861   myTempConstraints.push_back(aWDConstr.h);
1862 }
1863
1864 // ============================================================================
1865 //  Function: removeTemporaryConstraints
1866 //  Class:    SketchSolver_ConstraintGroup
1867 //  Purpose:  remove all transient SLVS_C_WHERE_DRAGGED constraints after
1868 //            resolving the set of constraints
1869 // ============================================================================
1870 void SketchSolver_ConstraintGroup::removeTemporaryConstraints(
1871     const std::set<Slvs_hConstraint>& theRemoved)
1872 {
1873   std::list<Slvs_hConstraint>::reverse_iterator aTmpConstrIter;
1874   for (aTmpConstrIter = myTempConstraints.rbegin(); aTmpConstrIter != myTempConstraints.rend();
1875       aTmpConstrIter++) {
1876     if (!theRemoved.empty() && theRemoved.find(*aTmpConstrIter) == theRemoved.end())
1877       continue;
1878     unsigned int aConstrPos = Search(*aTmpConstrIter, myConstraints);
1879     if (aConstrPos >= myConstraints.size())
1880       continue;
1881     myConstraints.erase(myConstraints.begin() + aConstrPos);
1882
1883     // If the removing constraint has higher index, decrease the indexer
1884     if (*aTmpConstrIter == myConstrMaxID)
1885       myConstrMaxID--;
1886   }
1887   myTempConstraints.clear();
1888
1889   // Clear basic dragged point
1890   myTempPointWhereDragged.clear();
1891   myTempPointWDrgdID = SLVS_E_UNKNOWN;
1892 }
1893
1894 // ============================================================================
1895 //  Function: removeConstraint
1896 //  Class:    SketchSolver_ConstraintGroup
1897 //  Purpose:  remove constraint and all unused entities
1898 // ============================================================================
1899 void SketchSolver_ConstraintGroup::removeConstraint(
1900     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
1901 {
1902   ConstraintMap::iterator anIterToRemove = myConstraintMap.find(theConstraint);
1903   if (anIterToRemove == myConstraintMap.end())
1904     return;
1905
1906   std::vector<Slvs_hConstraint> aCnstrToRemove = anIterToRemove->second;
1907   // Remove constraint from the map
1908   myConstraintMap.erase(anIterToRemove);
1909
1910   std::set<Slvs_hEntity> anEntToRemove;
1911
1912   // Find unused entities
1913   std::vector<Slvs_hConstraint>::iterator aCnstrToRemoveIter = aCnstrToRemove.begin();
1914   for (; aCnstrToRemoveIter != aCnstrToRemove.end(); aCnstrToRemoveIter++) {
1915     int aConstrPos = Search(*aCnstrToRemoveIter, myConstraints);
1916     Slvs_hEntity aCnstEnt[] = { myConstraints[aConstrPos].ptA, myConstraints[aConstrPos].ptB,
1917         myConstraints[aConstrPos].entityA, myConstraints[aConstrPos].entityB };
1918     for (int i = 0; i < 4; i++)
1919       if (aCnstEnt[i] != 0)
1920         anEntToRemove.insert(aCnstEnt[i]);
1921     myConstraints.erase(myConstraints.begin() + aConstrPos);
1922     if (*aCnstrToRemoveIter == myConstrMaxID)
1923       myConstrMaxID--;
1924   }
1925
1926   // Find all entities which are based on these unused
1927   std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
1928   for ( ; anEntIter != myEntities.end(); anEntIter++)
1929     if (anEntIter->type == SLVS_E_LINE_SEGMENT || anEntIter->type == SLVS_E_CIRCLE ||
1930         anEntIter->type == SLVS_E_ARC_OF_CIRCLE) {
1931       for (int i = 0; i < 4; i++)
1932         if (anEntToRemove.find(anEntIter->point[i]) != anEntToRemove.end()) {
1933           anEntToRemove.insert(anEntIter->h);
1934           for (int j = 0; j < 4; j++)
1935             if (anEntIter->point[j] != 0)
1936               anEntToRemove.insert(anEntIter->point[j]);
1937           break;
1938         }
1939     }
1940
1941   // Find entities used by remaining constraints and remove them from the list to delete
1942   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1943   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1944     Slvs_hEntity aEnts[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA, aConstrIter
1945         ->entityB };
1946     for (int i = 0; i < 4; i++)
1947       if (aEnts[i] != 0 && anEntToRemove.find(aEnts[i]) != anEntToRemove.end()) {
1948         anEntToRemove.erase(aEnts[i]);
1949         // remove from the list all points of current entity
1950         int aEntPos = Search(aEnts[i], myEntities);
1951         for (int j = 0; j < 4; j++)
1952           if (myEntities[aEntPos].point[j] != 0)
1953             anEntToRemove.erase(myEntities[aEntPos].point[j]);
1954       }
1955   }
1956
1957   if (anEntToRemove.empty())
1958     return;
1959
1960   // Remove unused entities
1961   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntAttrIter =
1962       myEntityAttrMap.begin();
1963   while (anEntAttrIter != myEntityAttrMap.end()) {
1964     if (anEntToRemove.find(anEntAttrIter->second) != anEntToRemove.end()) {
1965       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator aRemovedIter =
1966           anEntAttrIter;
1967       anEntAttrIter++;
1968       myEntityAttrMap.erase(aRemovedIter);
1969     } else
1970       anEntAttrIter++;
1971   }
1972   std::map<FeaturePtr, Slvs_hEntity>::iterator anEntFeatIter = myEntityFeatMap.begin();
1973   while (anEntFeatIter != myEntityFeatMap.end()) {
1974     if (anEntToRemove.find(anEntFeatIter->second) != anEntToRemove.end()) {
1975       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = anEntFeatIter;
1976       anEntFeatIter++;
1977       myEntityFeatMap.erase(aRemovedIter);
1978     } else
1979       anEntFeatIter++;
1980   }
1981
1982   removeEntitiesById(anEntToRemove);
1983
1984   if (myCoincidentPoints.size() == 1 && myCoincidentPoints.front().empty())
1985     myCoincidentPoints.clear();
1986 }
1987
1988 // ============================================================================
1989 //  Function: removeEntitiesById
1990 //  Class:    SketchSolver_ConstraintGroup
1991 //  Purpose:  Removes specified entities and their parameters
1992 // ============================================================================
1993 void SketchSolver_ConstraintGroup::removeEntitiesById(const std::set<Slvs_hEntity>& theEntities)
1994 {
1995   std::set<Slvs_hEntity>::const_reverse_iterator aRemIter = theEntities.rbegin();
1996   for (; aRemIter != theEntities.rend(); aRemIter++) {
1997     unsigned int anEntPos = Search(*aRemIter, myEntities);
1998     if (anEntPos >= myEntities.size())
1999       continue;
2000     if (myEntities[anEntPos].param[0] != 0) {
2001       unsigned int aParamPos = Search(myEntities[anEntPos].param[0], myParams);
2002       if (aParamPos >= myParams.size())
2003         continue;
2004       int aNbParams = 0;
2005       while (myEntities[anEntPos].param[aNbParams] != 0)
2006         aNbParams++;
2007       if (myEntities[anEntPos].param[aNbParams - 1] == myParamMaxID)
2008         myParamMaxID -= aNbParams;
2009       myParams.erase(myParams.begin() + aParamPos, myParams.begin() + aParamPos + aNbParams);
2010       if (*aRemIter == myEntityMaxID)
2011         myEntityMaxID--;
2012     }
2013     myEntities.erase(myEntities.begin() + anEntPos);
2014     myEntOfConstr.erase(myEntOfConstr.begin() + anEntPos);
2015
2016     // Remove entity's ID from the lists of conincident points
2017     std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
2018     for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++)
2019       aCoPtIter->erase(*aRemIter);
2020   }
2021 }
2022
2023 // ============================================================================
2024 //  Function: addCoincidentPoints
2025 //  Class:    SketchSolver_ConstraintGroup
2026 //  Purpose:  add coincident point the appropriate list of such points
2027 // ============================================================================
2028 bool SketchSolver_ConstraintGroup::addCoincidentPoints(const Slvs_hEntity& thePoint1,
2029                                                        const Slvs_hEntity& thePoint2)
2030 {
2031   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
2032   std::vector<std::set<Slvs_hEntity> >::iterator aFirstFound = myCoincidentPoints.end();
2033   while (aCoPtIter != myCoincidentPoints.end()) {
2034     bool isFound[2] = {  // indicate which point ID was already in coincidence constraint
2035         aCoPtIter->find(thePoint1) != aCoPtIter->end(), aCoPtIter->find(thePoint2)
2036             != aCoPtIter->end(), };
2037     if (isFound[0] && isFound[1])  // points are already connected by coincidence constraints => no need additional one
2038       return false;
2039     if ((isFound[0] && !isFound[1]) || (!isFound[0] && isFound[1])) {
2040       if (aFirstFound != myCoincidentPoints.end()) {  // there are two groups of coincident points connected by created constraint => merge them
2041         int aFirstFoundShift = aFirstFound - myCoincidentPoints.begin();
2042         int aCurrentShift = aCoPtIter - myCoincidentPoints.begin();
2043         aFirstFound->insert(aCoPtIter->begin(), aCoPtIter->end());
2044         myCoincidentPoints.erase(aCoPtIter);
2045         aFirstFound = myCoincidentPoints.begin() + aFirstFoundShift;
2046         aCoPtIter = myCoincidentPoints.begin() + aCurrentShift;
2047         continue;
2048       } else {
2049         aCoPtIter->insert(isFound[0] ? thePoint2 : thePoint1);
2050         aFirstFound = aCoPtIter;
2051       }
2052     }
2053     aCoPtIter++;
2054   }
2055   // No points were found, need to create new set
2056   if (aFirstFound == myCoincidentPoints.end()) {
2057     std::set<Slvs_hEntity> aNewSet;
2058     aNewSet.insert(thePoint1);
2059     aNewSet.insert(thePoint2);
2060     myCoincidentPoints.push_back(aNewSet);
2061   }
2062
2063   return true;
2064 }
2065
2066 // ============================================================================
2067 //  Function: updateRelatedConstraints
2068 //  Class:    SketchSolver_ConstraintGroup
2069 //  Purpose:  emit the signal to update constraints
2070 // ============================================================================
2071 void SketchSolver_ConstraintGroup::updateRelatedConstraints(
2072     std::shared_ptr<ModelAPI_Attribute> theEntity) const
2073 {
2074   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2075   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2076     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2077         ->attributes(std::string());
2078
2079     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2080     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2081       bool isUpd = (*anAttrIter == theEntity);
2082       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2083           ModelAPI_AttributeRefAttr>(*anAttrIter);
2084       if (aRefAttr && !aRefAttr->isObject() && aRefAttr->attr() == theEntity)
2085         isUpd = true;
2086
2087       if (isUpd) {
2088         static Events_ID anEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
2089         ModelAPI_EventCreator::get()->sendUpdated(aConstrIter->first, anEvent);
2090         break;
2091       }
2092     }
2093   }
2094 }
2095
2096 void SketchSolver_ConstraintGroup::updateRelatedConstraintsFeature(
2097     std::shared_ptr<ModelAPI_Feature> theFeature) const
2098 {
2099   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2100   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2101     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2102         ->attributes(std::string());
2103
2104     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2105     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2106       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2107           ModelAPI_AttributeRefAttr>(*anAttrIter);
2108       if (aRefAttr && aRefAttr->isObject() && aRefAttr->object() == theFeature) {
2109         static Events_ID anEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
2110         ModelAPI_EventCreator::get()->sendUpdated(aConstrIter->first, anEvent);
2111         break;
2112       }
2113     }
2114   }
2115 }
2116
2117 // ============================================================================
2118 //  Function: updateFilletConstraints
2119 //  Class:    SketchSolver_ConstraintGroup
2120 //  Purpose:  change fillet arc to be less than 180 degree
2121 // ============================================================================
2122 void SketchSolver_ConstraintGroup::updateFilletConstraints()
2123 {
2124   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2125   for (; aConstrIter != myConstraintMap.end(); aConstrIter++)
2126     if (aConstrIter->first->getKind() == SketchPlugin_ConstraintFillet::ID()) {
2127       AttributeRefListPtr aFilletRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
2128           aConstrIter->first->data()->attribute(SketchPlugin_ConstraintFillet::ENTITY_C()));
2129       if (!aFilletRefList)
2130         return;
2131       ObjectPtr anArcObj = aFilletRefList->object(2);
2132       std::shared_ptr<GeomDataAPI_Point2D> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2133           anArcObj->data()->attribute(SketchPlugin_Arc::CENTER_ID()));
2134       std::shared_ptr<GeomDataAPI_Point2D> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2135           anArcObj->data()->attribute(SketchPlugin_Arc::START_ID()));
2136       std::shared_ptr<GeomDataAPI_Point2D> aEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2137           anArcObj->data()->attribute(SketchPlugin_Arc::END_ID()));
2138       double aCosA = aStart->x() - aCenter->x();
2139       double aSinA = aStart->y() - aCenter->y();
2140       double aCosB = aEnd->x() - aCenter->x();
2141       double aSinB = aEnd->y() - aCenter->y();
2142       if (aCosA * aSinB - aSinA * aCosB <= 0.0) {
2143         anArcObj->data()->blockSendAttributeUpdated(true);
2144         double x = aStart->x();
2145         double y = aStart->y();
2146         aStart->setValue(aEnd->x(), aEnd->y());
2147         aEnd->setValue(x, y);
2148         // Update constraint data
2149         changeFilletConstraint(aConstrIter->first);
2150         anArcObj->data()->blockSendAttributeUpdated(false);
2151       }
2152     }
2153 }
2154
2155 // ============================================================================
2156 //  Function: makeMirrorEntity
2157 //  Class:    SketchSolver_ConstraintGroup
2158 //  Purpose:  change entities parameters to make them symmetric relating to the mirror line
2159 // ============================================================================
2160 void SketchSolver_ConstraintGroup::makeMirrorEntity(const Slvs_hEntity& theBase,
2161                                                     const Slvs_hEntity& theMirror,
2162                                                     const Slvs_hEntity& theMirrorLine)
2163 {
2164   Slvs_Entity aBase = myEntities[Search(theBase, myEntities)];
2165   Slvs_Entity aMirror = myEntities[Search(theMirror, myEntities)];
2166   int i = 0;
2167   while (aBase.point[i] != 0 && aMirror.point[i] != 0) {
2168     makeMirrorEntity(aBase.point[i], aMirror.point[i], theMirrorLine);
2169     i++;
2170   }
2171   if (aBase.param[0] != 0 && aMirror.param[0] != 0) { // this is a point, copy it
2172     Slvs_Entity aMirrorLine = myEntities[Search(theMirrorLine, myEntities)];
2173     std::shared_ptr<GeomAPI_XY> aLinePoints[2];
2174     for (i = 0; i < 2; i++) {
2175       Slvs_Entity aPoint = myEntities[Search(aMirrorLine.point[i], myEntities)];
2176       int aParamPos = Search(aPoint.param[0], myParams);
2177       aLinePoints[i] = std::shared_ptr<GeomAPI_XY>(
2178           new GeomAPI_XY(myParams[aParamPos].val, myParams[1+aParamPos].val));
2179     }
2180     int aBaseParamPos = Search(aBase.param[0], myParams);
2181     int aMirrorParamPos = Search(aMirror.param[0], myParams);
2182     std::shared_ptr<GeomAPI_XY> aPoint = std::shared_ptr<GeomAPI_XY>(
2183         new GeomAPI_XY(myParams[aBaseParamPos].val, myParams[1+aBaseParamPos].val));
2184
2185     // direction of a mirror line
2186     std::shared_ptr<GeomAPI_Dir2d> aDir = std::shared_ptr<GeomAPI_Dir2d>(
2187         new GeomAPI_Dir2d(aLinePoints[1]->added(aLinePoints[0]->multiplied(-1.0))));
2188     // orthogonal direction
2189     aDir = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(aDir->y(), -aDir->x()));
2190
2191     std::shared_ptr<GeomAPI_XY> aVec = std::shared_ptr<GeomAPI_XY>(
2192         new GeomAPI_XY(aPoint->x() - aLinePoints[0]->x(), aPoint->y() - aLinePoints[0]->y()));
2193     double aDist = aVec->dot(aDir->xy());
2194     std::shared_ptr<GeomAPI_XY> aMirrorPoint = aPoint->added(aDir->xy()->multiplied(-2.0 * aDist));
2195
2196     myParams[aMirrorParamPos].val = aMirrorPoint->x();
2197     myParams[1+aMirrorParamPos].val = aMirrorPoint->y();
2198   }
2199 }
2200
2201 // ============================================================================
2202 //  Function: calculateMiddlePoint
2203 //  Class:    SketchSolver_ConstraintGroup
2204 //  Purpose:  calculates middle point on line or arc
2205 // ============================================================================
2206 void SketchSolver_ConstraintGroup::calculateMiddlePoint(
2207     const Slvs_hEntity& theEntity,
2208     double& theX,
2209     double& theY) const
2210 {
2211   int anInd = Search(theEntity, myEntities);
2212   if (myEntities[anInd].type == SLVS_E_LINE_SEGMENT) {
2213     int aLineParams[2];
2214     for (int i = 0; i < 2; i++) {
2215       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2216       aLineParams[i] = Search(myEntities[aPtPos].param[0], myParams);
2217     }
2218     theX = 0.5 * (myParams[aLineParams[0]].val + myParams[aLineParams[1]].val);
2219     theY = 0.5 * (myParams[1 + aLineParams[0]].val + myParams[1 + aLineParams[1]].val);
2220   } else if (myEntities[anInd].type == SLVS_E_ARC_OF_CIRCLE) {
2221     double anArcPoint[3][2];
2222     for (int i = 0; i < 3; i++) {
2223       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2224       int anArcParam = Search(myEntities[aPtPos].param[0], myParams);
2225       anArcPoint[i][0] = myParams[anArcParam].val;
2226       anArcPoint[i][1] = myParams[1 + anArcParam].val;
2227     }
2228     // project last point of arc on the arc
2229     double x = anArcPoint[1][0] - anArcPoint[0][0];
2230     double y = anArcPoint[1][1] - anArcPoint[0][1];
2231     double aRad = sqrt(x*x + y*y);
2232     x = anArcPoint[2][0] - anArcPoint[0][0];
2233     y = anArcPoint[2][1] - anArcPoint[0][1];
2234     double aNorm = sqrt(x*x + y*y);
2235     if (aNorm >= tolerance) {
2236       anArcPoint[2][0] = anArcPoint[0][0] + x * aRad / aNorm;
2237       anArcPoint[2][1] = anArcPoint[0][1] + y * aRad / aNorm;
2238     }
2239
2240     x = anArcPoint[1][0] + anArcPoint[2][0] - 2.0 * anArcPoint[0][0];
2241     y = anArcPoint[1][1] + anArcPoint[2][1] - 2.0 * anArcPoint[0][1];
2242     aNorm = sqrt(x*x + y*y);
2243     if (aNorm >= tolerance) {
2244       x *= aRad / aNorm;
2245       y *= aRad / aNorm;
2246     } else { // obtain orthogonal direction
2247       x = 0.5 * (anArcPoint[2][1] - anArcPoint[1][1]);
2248       y = -0.5 * (anArcPoint[2][0] - anArcPoint[1][0]);
2249     }
2250     theX = anArcPoint[0][0] + x;
2251     theY = anArcPoint[0][1] + y;
2252   }
2253 }
2254
2255
2256 // ========================================================
2257 // =========      Auxiliary functions       ===============
2258 // ========================================================
2259
2260 template<typename T>
2261 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
2262 {
2263   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
2264   int aVecSize = theEntities.size();
2265   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
2266     aResIndex--;
2267   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
2268     aResIndex++;
2269   if (aResIndex == -1)
2270     aResIndex = aVecSize;
2271   return aResIndex;
2272 }