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     std::shared_ptr<SketchPlugin_Point> aSketchPoint =
 511         std::dynamic_pointer_cast<SketchPlugin_Point>(aFeature);
 512     if (aPoint || aPoint2D || aSketchPoint) {
 513       // Create SolveSpace constraint structure
 514       Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 515           ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 516           aConstrEnt, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 517       myConstraints.push_back(aConstraint);
 518       myConstraintMap[theConstraint] = std::vector<Slvs_hEntity>(1, aConstraint.h);
 519       int aConstrPos = Search(aConstraint.h, myConstraints);
 520       aConstrIter = myConstraints.begin() + aConstrPos;
 521       myNeedToSolve = true;
 522     } else {
 523       myConstraintMap[theConstraint] = std::vector<Slvs_hConstraint>();
 524
 525       // To avoid SolveSpace problems:
 526       // * if the circle is rigid, we will fix its center and radius;
 527       // * if the arc is rigid, we will fix its start and end points and radius.
 528       double aRadius = 0.0;
 529       bool isArc = false;
 530       bool isCircle = false;
 531       if (aFeature) {
 532         if (aFeature->getKind() == SketchPlugin_Arc::ID()) {
 533           std::shared_ptr<GeomDataAPI_Point2D> aCenter =
 534               std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 535               aFeature->data()->attribute(SketchPlugin_Arc::CENTER_ID()));
 536           std::shared_ptr<GeomDataAPI_Point2D> aStart =
 537               std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 538               aFeature->data()->attribute(SketchPlugin_Arc::START_ID()));
 539           aRadius = aStart->pnt()->distance(aCenter->pnt());
 540           isArc = true;
 541         } else if (aFeature->getKind() == SketchPlugin_Circle::ID()) {
 542           aRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 543               aFeature->data()->attribute(SketchPlugin_Circle::RADIUS_ID()))->value();
 544           isCircle = true;
 545         }
 546       }
 547
 548       // Get list of already fixed points
 549       std::set<Slvs_hEntity> anAlreadyFixed;
 550       std::vector<Slvs_Constraint>::const_iterator aCIter = myConstraints.begin();
 551       for (; aCIter != myConstraints.end(); aCIter++)
 552         if (aCIter->type == SLVS_C_WHERE_DRAGGED)
 553           anAlreadyFixed.insert(aCIter->ptA);
 554
 555       // Create constraints to fix the parameters of the entity
 556       int aEntPos = Search(aConstrEnt, myEntities);
 557       Slvs_hEntity* aPointsPtr = myEntities[aEntPos].point;
 558       if (isArc) aPointsPtr++; // avoid to fix center of arc
 559       while (*aPointsPtr != 0) {
 560         // Avoid to create additional "Rigid" constraints for coincident points
 561         bool isCoincAlreadyFixed = false;
 562         if (!anAlreadyFixed.empty()) {
 563           if (anAlreadyFixed.find(*aPointsPtr) != anAlreadyFixed.end())
 564             isCoincAlreadyFixed = true;
 565
 566           std::vector<std::set<Slvs_hEntity> >::const_iterator aCoincIter =
 567               myCoincidentPoints.begin();
 568           for (; !isCoincAlreadyFixed && aCoincIter != myCoincidentPoints.end(); aCoincIter++) {
 569             if (aCoincIter->find(*aPointsPtr) == aCoincIter->end())
 570               continue;
 571             std::set<Slvs_hEntity>::const_iterator anIter = anAlreadyFixed.begin();
 572             for (; !isCoincAlreadyFixed && anIter != anAlreadyFixed.end(); anIter++)
 573               if (aCoincIter->find(*anIter) != aCoincIter->end())
 574                 isCoincAlreadyFixed = true;
 575           }
 576         }
 577
 578         if (!isCoincAlreadyFixed) {
 579           Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 580               ++myConstrMaxID, myID, aConstrType, myWorkplane.h, 0.0,
 581               *aPointsPtr, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 582           myConstraints.push_back(aConstraint);
 583           myConstraintMap[theConstraint].push_back(aConstraint.h);
 584         }
 585         aPointsPtr++;
 586       }
 587
 588       if (isArc || isCircle) { // add radius constraint
 589         Slvs_Constraint aConstraint = Slvs_MakeConstraint(
 590             ++myConstrMaxID, myID, SLVS_C_DIAMETER, myWorkplane.h, 2.0 * aRadius,
 591             SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aConstrEnt, SLVS_E_UNKNOWN);
 592         myConstraints.push_back(aConstraint);
 593         myConstraintMap[theConstraint].push_back(aConstraint.h);
 594       }
 595
 596       // The object is already rigid, so there is no constraints added
 597       if (myConstraintMap[theConstraint].empty()) {
 598         myConstraintMap.erase(theConstraint);
 599         myNeedToSolve = false;
 600       }
 601       else
 602         myNeedToSolve = true;
 603     }
 604   }
 605   return true;
 606 }
 607
 608 // ============================================================================
 609 //  Function: changeMirrorConstraint
 610 //  Class:    SketchSolver_ConstraintGroup
 611 //  Purpose:  create/update the "Mirror" constraint in the group
 612 // ============================================================================
 613 bool SketchSolver_ConstraintGroup::changeMirrorConstraint(
 614     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 615 {
 616   DataPtr aConstrData = theConstraint->data();
 617
 618   // Search this constraint in the current group to update it
 619   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 620   std::vector<Slvs_Constraint>::iterator aConstrIter;
 621   bool isExists = false;
 622   if (aConstrMapIter != myConstraintMap.end()) {
 623     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 624     aConstrIter = myConstraints.begin() + aConstrPos;
 625     isExists = true;
 626   }
 627
 628   // Get constraint type and verify the constraint parameters are correct
 629   SketchSolver_Constraint aConstraint(theConstraint);
 630   int aConstrType = aConstraint.getType();
 631   if (aConstrType == SLVS_C_UNKNOWN
 632       || (aConstrMapIter != myConstraintMap.end() && aConstrIter->type != aConstrType))
 633     return false;
 634   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 635
 636   Slvs_hEntity aMirrorLineEnt = SLVS_E_UNKNOWN;
 637   AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 638       aConstrData->attribute(aConstraintAttributes[0]));
 639   if (!aConstrAttr)
 640     return false;
 641
 642   // Convert the object of the attribute to the feature
 643   FeaturePtr aMirrorLineFeat;
 644   if (aConstrAttr->isObject() && aConstrAttr->object()) {
 645     ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 646         aConstrAttr->object());
 647     if (!aRC)
 648       return false;
 649     std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 650     aMirrorLineFeat = aDoc->feature(aRC);
 651   }
 652   aMirrorLineEnt = aConstrAttr->isObject() ?
 653       changeEntityFeature(aMirrorLineFeat) : changeEntity(aConstrAttr->attr());
 654
 655   // Append symmetric constraint for each point of mirroring features
 656   AttributeRefListPtr aBaseRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 657     aConstrData->attribute(aConstraintAttributes[1]));
 658   AttributeRefListPtr aMirroredRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 659     aConstrData->attribute(aConstraintAttributes[2]));
 660   if (!aBaseRefList || !aMirroredRefList)
 661     return false;
 662
 663   std::list<ObjectPtr> aBaseList = aBaseRefList->list();
 664   std::list<ObjectPtr> aMirroredList = aMirroredRefList->list();
 665   if (aBaseList.empty() || aBaseList.size() != aMirroredList.size())
 666     return false;
 667
 668   std::vector<Slvs_hConstraint> aNewConstraints;
 669   // Fill the list of already mirrored points
 670   std::vector<Slvs_Constraint> anOldConstraints;
 671   std::map<Slvs_hEntity, Slvs_hEntity> aMirroredPoints;
 672   if (isExists) {
 673     std::vector<Slvs_hConstraint>::const_iterator aCIter = aConstrMapIter->second.begin();
 674     for (; aCIter != aConstrMapIter->second.end(); aCIter++) {
 675       int anInd = Search(*aCIter, myConstraints);
 676       if (myConstraints[anInd].type != aConstrType)
 677         continue;
 678       aMirroredPoints[myConstraints[anInd].ptA] = myConstraints[anInd].ptB;
 679       anOldConstraints.push_back(myConstraints[anInd]);
 680     }
 681   }
 682
 683   FeaturePtr aBaseFeature, aMirrorFeature;
 684   ResultConstructionPtr aRC;
 685   std::list<ObjectPtr>::iterator aBaseIter = aBaseList.begin();
 686   std::list<ObjectPtr>::iterator aMirIter = aMirroredList.begin();
 687   for ( ; aBaseIter != aBaseList.end(); aBaseIter++, aMirIter++) {
 688     aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aBaseIter);
 689     aBaseFeature = aRC ? aRC->document()->feature(aRC) :
 690         std::dynamic_pointer_cast<ModelAPI_Feature>(*aBaseIter);
 691     aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aMirIter);
 692     aMirrorFeature = aRC ? aRC->document()->feature(aRC) :
 693         std::dynamic_pointer_cast<ModelAPI_Feature>(*aMirIter);
 694
 695     if (!aBaseFeature || !aMirrorFeature ||
 696         aBaseFeature->getKind() != aMirrorFeature->getKind())
 697       return false;
 698     Slvs_hEntity aBaseEnt = changeEntityFeature(aBaseFeature);
 699     Slvs_hEntity aMirrorEnt = changeEntityFeature(aMirrorFeature);
 700     // Make aMirrorEnt parameters to be symmetric with aBaseEnt
 701     makeMirrorEntity(aBaseEnt, aMirrorEnt, aMirrorLineEnt);
 702
 703     if (aBaseFeature->getKind() == SketchPlugin_Point::ID()) {
 704       Slvs_hConstraint anID = changeMirrorPoints(aBaseEnt, aMirrorEnt,
 705           aMirrorLineEnt, anOldConstraints, aMirroredPoints);
 706       aNewConstraints.push_back(anID);
 707     } else {
 708       int aBasePos = Search(aBaseEnt, myEntities);
 709       int aMirrorPos = Search(aMirrorEnt, myEntities);
 710       if (aBaseFeature->getKind() == SketchPlugin_Line::ID()) {
 711         for (int ind = 0; ind < 2; ind++) {
 712           Slvs_hConstraint anID = changeMirrorPoints(myEntities[aBasePos].point[ind],
 713               myEntities[aMirrorPos].point[ind], aMirrorLineEnt, anOldConstraints, aMirroredPoints);
 714           aNewConstraints.push_back(anID);
 715         }
 716       } else if (aBaseFeature->getKind() == SketchPlugin_Circle::ID()) {
 717         Slvs_hConstraint anID = changeMirrorPoints(myEntities[aBasePos].point[0],
 718             myEntities[aMirrorPos].point[0], aMirrorLineEnt, anOldConstraints, aMirroredPoints);
 719         aNewConstraints.push_back(anID);
 720         // Additional constraint for equal radii
 721         Slvs_Constraint anEqRadConstr = Slvs_MakeConstraint(
 722             ++myConstrMaxID, myID, SLVS_C_EQUAL_RADIUS, myWorkplane.h, 0.0,
 723             SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aBaseEnt, aMirrorEnt);
 724         myConstraints.push_back(anEqRadConstr);
 725         myConstraintMap[theConstraint].push_back(anEqRadConstr.h);
 726       } else if (aBaseFeature->getKind() == SketchPlugin_Arc::ID()) {
 727         // Workaround to avoid problems in SolveSpace.
 728         // The symmetry of two arcs will be done using symmetry of three points on these arcs:
 729         // start point, end point, and any other point on the arc
 730         Slvs_hEntity aBaseArcPoints[3] = {
 731             myEntities[aBasePos].point[1],
 732             myEntities[aBasePos].point[2],
 733             SLVS_E_UNKNOWN};
 734         Slvs_hEntity aMirrorArcPoints[3] = { // indices of points of arc, center corresponds center, first point corresponds last point
 735             myEntities[aMirrorPos].point[2],
 736             myEntities[aMirrorPos].point[1],
 737             SLVS_E_UNKNOWN};
 738         Slvs_hEntity aBothArcs[2] = {aBaseEnt, aMirrorEnt};
 739         Slvs_hEntity aBothMiddlePoints[2];
 740         for (int i = 0; i < 2; i++) {
 741           double x, y;
 742           calculateMiddlePoint(aBothArcs[i], x, y);
 743           std::vector<Slvs_Param>::iterator aParamIter = myParams.end();
 744           Slvs_hParam u = changeParameter(x, aParamIter);
 745           Slvs_hParam v = changeParameter(y, aParamIter);
 746           Slvs_Entity aPoint = Slvs_MakePoint2d(++myEntityMaxID, myID, myWorkplane.h, u, v);
 747           myEntities.push_back(aPoint);
 748           aBothMiddlePoints[i] = aPoint.h;
 749           // additional constraint point-on-curve
 750           Slvs_Constraint aPonCircConstr = Slvs_MakeConstraint(
 751               ++myConstrMaxID, myID, SLVS_C_PT_ON_CIRCLE, myWorkplane.h, 0.0,
 752               aPoint.h, SLVS_E_UNKNOWN, aBothArcs[i], SLVS_E_UNKNOWN);
 753           myConstraints.push_back(aPonCircConstr);
 754           myConstraintMap[theConstraint].push_back(aPonCircConstr.h);
 755         }
 756
 757         aBaseArcPoints[2] = aBothMiddlePoints[0];
 758         aMirrorArcPoints[2] = aBothMiddlePoints[1];
 759         for (int ind = 0; ind < 3; ind++) {
 760           Slvs_hConstraint anID = changeMirrorPoints(aBaseArcPoints[ind], aMirrorArcPoints[ind],
 761               aMirrorLineEnt, anOldConstraints, aMirroredPoints);
 762           aNewConstraints.push_back(anID);
 763         }
 764       }
 765     }
 766   }
 767
 768   // Remove unused constraints
 769   std::vector<Slvs_Constraint>::const_iterator anOldCIter = anOldConstraints.begin();
 770   for (; anOldCIter != anOldConstraints.end(); anOldCIter++) {
 771     int anInd = Search(anOldCIter->h, myConstraints);
 772     myConstraints.erase(myConstraints.begin() + anInd);
 773   }
 774   myConstraintMap[theConstraint] = aNewConstraints;
 775
 776   if (!isExists) {
 777     // Set the mirror line unchanged during constraint recalculation
 778     int aMirrorLinePos = Search(aMirrorLineEnt, myEntities);
 779     // firstly check the line is not fixed yet
 780     bool isFixed[2] = {false, false};
 781     std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
 782     for (; aConstrIter != myConstraints.end() && !(isFixed[0] && isFixed[1]); aConstrIter++)
 783       if (aConstrIter->type == SLVS_C_WHERE_DRAGGED) {
 784         if (aConstrIter->ptA == myEntities[aMirrorLinePos].point[0])
 785           isFixed[0] = true;
 786         else if (aConstrIter->ptA == myEntities[aMirrorLinePos].point[1])
 787           isFixed[1] = true;
 788       }
 789     // add new rigid constraints
 790     if (!isFixed[0]) {
 791       Slvs_Constraint aRigidStart = Slvs_MakeConstraint(
 792           ++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED, myWorkplane.h, 0,
 793           myEntities[aMirrorLinePos].point[0], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 794       myConstraints.push_back(aRigidStart);
 795       myConstraintMap[theConstraint].push_back(aRigidStart.h);
 796     }
 797     if (!isFixed[1]) {
 798       Slvs_Constraint aRigidEnd = Slvs_MakeConstraint(
 799           ++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED, myWorkplane.h, 0,
 800           myEntities[aMirrorLinePos].point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 801       myConstraints.push_back(aRigidEnd);
 802       myConstraintMap[theConstraint].push_back(aRigidEnd.h);
 803     }
 804
 805     // Add temporary constraints for initial objects to be unchanged
 806     for (aBaseIter = aBaseList.begin(); aBaseIter != aBaseList.end(); aBaseIter++) {
 807       aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aBaseIter);
 808       aBaseFeature = aRC ? aRC->document()->feature(aRC) :
 809           std::dynamic_pointer_cast<ModelAPI_Feature>(*aBaseIter);
 810       if (!aBaseFeature) continue;
 811       std::list<AttributePtr> aPoints = aBaseFeature->data()->attributes(GeomDataAPI_Point2D::typeId());
 812       std::list<AttributePtr>::iterator anIt = aPoints.begin();
 813       for ( ; anIt != aPoints.end(); anIt++) {
 814         // Arcs are fixed by center and start points only (to avoid solving errors in SolveSpace)
 815         if (aBaseFeature->getKind() == SketchPlugin_Arc::ID() &&
 816             (*anIt)->id() == SketchPlugin_Arc::END_ID())
 817           continue;
 818         addTemporaryConstraintWhereDragged(*anIt);
 819       }
 820     }
 821   }
 822   return true;
 823 }
 824
 825 // ============================================================================
 826 //  Function: changeFilletConstraint
 827 //  Class:    SketchSolver_ConstraintGroup
 828 //  Purpose:  create/update the "Fillet" constraint in the group
 829 // ============================================================================
 830 bool SketchSolver_ConstraintGroup::changeFilletConstraint(
 831     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
 832 {
 833   DataPtr aConstrData = theConstraint->data();
 834
 835   // Search this constraint in the current group to update it
 836   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.find(theConstraint);
 837   std::vector<Slvs_Constraint>::iterator aConstrIter;
 838   if (aConstrMapIter != myConstraintMap.end()) {
 839     int aConstrPos = Search(aConstrMapIter->second.front(), myConstraints);
 840     aConstrIter = myConstraints.begin() + aConstrPos;
 841   }
 842
 843   // Get constraint type and verify the constraint parameters are correct
 844   SketchSolver_Constraint aConstraint(theConstraint);
 845   int aConstrType = aConstraint.getType();
 846   if (aConstrType == SLVS_C_UNKNOWN)
 847     return false;
 848   const std::vector<std::string>& aConstraintAttributes = aConstraint.getAttributes();
 849
 850   // Obtain hEntity for basic objects of fillet
 851   Slvs_hEntity aBaseObject[2];
 852   FeaturePtr aBaseFeature[2];
 853   for (unsigned int indAttr = 0; indAttr < 2; indAttr++) {
 854     AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 855         aConstrData->attribute(aConstraintAttributes[indAttr]));
 856     if (!aConstrAttr)
 857       return false;
 858     if (aConstrAttr->isObject() && aConstrAttr->object()) {
 859       ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 860           aConstrAttr->object());
 861       if (!aRC)
 862         return false;
 863       std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 864       aBaseFeature[indAttr] = aDoc->feature(aRC);
 865     }
 866     aBaseObject[indAttr] = aConstrAttr->isObject() ?
 867         changeEntityFeature(aBaseFeature[indAttr]) : changeEntity(aConstrAttr->attr());
 868   }
 869   // Check the base entities have a coincident point
 870   int aBaseObjInd[2] = {
 871       Search(aBaseObject[0], myEntities),
 872       Search(aBaseObject[1], myEntities)
 873     };
 874   int aShift[2] = { // shift for calculating correct start and end points for different types of objects
 875       myEntities[aBaseObjInd[0]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0,
 876       myEntities[aBaseObjInd[1]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0,
 877     };
 878   Slvs_hEntity aFirstObjPoints[2] = { // indices of start and end point of first object
 879       myEntities[aBaseObjInd[0]].point[aShift[0]],
 880       myEntities[aBaseObjInd[0]].point[1+aShift[0]]
 881     };
 882   Slvs_hEntity aSecondObjPoints[2] = { // indices of start and end point of second object
 883       myEntities[aBaseObjInd[1]].point[aShift[1]],
 884       myEntities[aBaseObjInd[1]].point[1+aShift[1]]
 885     };
 886   bool isCoincidentFound = false;
 887   int aBaseCoincInd[2] = {0, 0}; // indices in aFirstObjPoint and aSecondObjPoint identifying coincident points
 888   std::vector<std::set<Slvs_hEntity> >::iterator aCPIter = myCoincidentPoints.begin();
 889   for ( ; aCPIter != myCoincidentPoints.end() && !isCoincidentFound; aCPIter++)
 890     for (int ind1 = 0; ind1 < 2 && !isCoincidentFound; ind1++)
 891       for (int ind2 = 0; ind2 < 2 && !isCoincidentFound; ind2++)
 892         if (aCPIter->find(aFirstObjPoints[ind1]) != aCPIter->end() &&
 893             aCPIter->find(aSecondObjPoints[ind2]) != aCPIter->end()) {
 894           aBaseCoincInd[0] = ind1;
 895           aBaseCoincInd[1] = ind2;
 896           isCoincidentFound = true;
 897         }
 898   if (!isCoincidentFound) {
 899     // There is no coincident points between objects. Generate error message
 900     Events_Error::send(SketchSolver_Error::NO_COINCIDENT_POINTS(), this);
 901     return false;
 902   }
 903
 904   // Create fillet entities
 905   // - first object is placed on the first base
 906   // - second object is on the second base
 907   // - third object is a filleting arc
 908   static const int aNbFilletEnt = 3;
 909   Slvs_hEntity aFilletEnt[aNbFilletEnt];
 910   int aFilletObjInd[aNbFilletEnt];
 911   AttributeRefListPtr aFilletRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 912       aConstrData->attribute(aConstraintAttributes[2]));
 913   if (!aFilletRefList)
 914     return false;
 915   std::list<ObjectPtr> aFilletList = aFilletRefList->list();
 916   if (aFilletList.size() < aNbFilletEnt)
 917     return false;
 918   FeaturePtr aFilletFeature;
 919   ResultConstructionPtr aRC;
 920   std::list<ObjectPtr>::iterator aFilIter = aFilletList.begin();
 921   for (int indEnt = 0; aFilIter != aFilletList.end(); aFilIter++, indEnt++) {
 922     aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(*aFilIter);
 923     aFilletFeature = aRC ? aRC->document()->feature(aRC) :
 924         std::dynamic_pointer_cast<ModelAPI_Feature>(*aFilIter);
 925     if (!aFilletFeature)
 926       return false;
 927     aFilletEnt[indEnt] = changeEntityFeature(aFilletFeature);
 928     if (aFilletEnt[indEnt] == SLVS_E_UNKNOWN)
 929       return false; // not all attributes are initialized yet
 930     aFilletObjInd[indEnt] = Search(aFilletEnt[indEnt], myEntities);
 931   }
 932   // At first time, for correct result, move floating points of fillet on the middle points of base objects
 933   if (myConstraintMap.find(theConstraint) == myConstraintMap.end()) {
 934     double anArcPoints[6];
 935     for (int indEnt = 0; indEnt < aNbFilletEnt - 1; indEnt++) {
 936       int anIndShift = myEntities[aFilletObjInd[indEnt]].type == SLVS_E_ARC_OF_CIRCLE ? 1 : 0;
 937       int aPointsPos[2] = {
 938           Search(myEntities[aFilletObjInd[indEnt]].point[anIndShift], myEntities),
 939           Search(myEntities[aFilletObjInd[indEnt]].point[1+anIndShift], myEntities)
 940         };
 941       int aParamPos[2] = {
 942           Search(myEntities[aPointsPos[0]].param[0], myParams),
 943           Search(myEntities[aPointsPos[1]].param[0], myParams)
 944         };
 945       int anIndex = aParamPos[aBaseCoincInd[indEnt]];
 946       if (anIndShift == 0) {
 947         myParams[anIndex].val =
 948             0.5 * (myParams[aParamPos[0]].val + myParams[aParamPos[1]].val);
 949         myParams[1 + anIndex].val =
 950             0.5 * (myParams[1 + aParamPos[0]].val + myParams[1 + aParamPos[1]].val);
 951       } else { // place the changed point on the arc
 952         double x = 0, y = 0;
 953         calculateMiddlePoint(aFilletEnt[indEnt], x, y);
 954         myParams[anIndex].val = x;
 955         myParams[1 + anIndex].val = y;
 956       }
 957       anArcPoints[indEnt*2+2] = myParams[anIndex].val;
 958       anArcPoints[indEnt*2+3] = myParams[1 + anIndex].val;
 959     }
 960     anArcPoints[0] = 0.5 * (anArcPoints[2] + anArcPoints[4]);
 961     anArcPoints[1] = 0.5 * (anArcPoints[3] + anArcPoints[5]);
 962     for (int indArcPt = 0; indArcPt < 3; indArcPt++) {
 963       int aPtPos = Search(myEntities[aFilletObjInd[2]].point[indArcPt], myEntities);
 964       int aParamPos = Search(myEntities[aPtPos].param[0], myParams);
 965       myParams[aParamPos].val = anArcPoints[indArcPt * 2];
 966       myParams[aParamPos + 1].val = anArcPoints[indArcPt * 2 + 1];
 967     }
 968   }
 969
 970   // Create list of constraints to generate fillet
 971   int aPtInd;
 972   std::vector<Slvs_hConstraint> aConstrList;
 973   bool isExists = myConstraintMap.find(theConstraint) != myConstraintMap.end(); // constraint already exists
 974   std::vector<Slvs_hConstraint>::iterator aCMapIter =
 975     isExists ? myConstraintMap[theConstraint].begin() : aConstrList.begin();
 976   std::vector<Slvs_hConstraint>::iterator aCMapEnd =
 977     isExists ? myConstraintMap[theConstraint].end() : aConstrList.end();
 978   int aCurConstrPos = isExists ? Search(*aCMapIter, myConstraints) : 0;
 979   for (int indEnt = 0; indEnt < aNbFilletEnt - 1; indEnt++) {
 980     // one point of fillet object should be coincident with the point on base, non-coincident with another base object
 981     aPtInd = 1-aBaseCoincInd[indEnt]+aShift[indEnt]; // (1-aBaseCoincInd[indEnt]) = index of non-coincident point, aShift is used to process all types of shapes
 982     Slvs_hEntity aPtBase = myEntities[aBaseObjInd[indEnt]].point[aPtInd];
 983     Slvs_hEntity aPtFillet = myEntities[aFilletObjInd[indEnt]].point[aPtInd];
 984     if (isExists) {
 985       myConstraints[aCurConstrPos].ptA = aPtBase;
 986       myConstraints[aCurConstrPos].ptB = aPtFillet;
 987       aCMapIter++;
 988       aCurConstrPos = Search(*aCMapIter, myConstraints);
 989     } else if (addCoincidentPoints(aPtBase, aPtFillet)) { // the points are not connected by coincidence yet
 990       Slvs_Constraint aCoincConstr = Slvs_MakeConstraint(
 991           ++myConstrMaxID, myID, SLVS_C_POINTS_COINCIDENT, myWorkplane.h,
 992           0, aPtBase, aPtFillet, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 993       myConstraints.push_back(aCoincConstr);
 994       aConstrList.push_back(aCoincConstr.h);
 995     }
 996
 997     // another point of fillet object should be placed on the base object
 998     Slvs_Constraint aPonCurveConstr;
 999     int aTangentType;
1000     if (myEntities[aFilletObjInd[indEnt]].type == SLVS_E_ARC_OF_CIRCLE) {
1001       // centers of arcs should be coincident
1002       aPtBase = myEntities[aBaseObjInd[indEnt]].point[0];
1003       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[0];
1004       if (isExists) {
1005         myConstraints[aCurConstrPos].ptA = aPtBase;
1006         myConstraints[aCurConstrPos].ptB = aPtFillet;
1007         aCMapIter++;
1008         if (aCMapIter != aCMapEnd)
1009           aCurConstrPos = Search(*aCMapIter, myConstraints);
1010       } else if (addCoincidentPoints(aPtBase, aPtFillet)) { // the points are not connected by coincidence yet
1011         aPonCurveConstr = Slvs_MakeConstraint(
1012             ++myConstrMaxID, myID, SLVS_C_POINTS_COINCIDENT, myWorkplane.h,
1013             0, aPtBase, aPtFillet, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
1014       }
1015       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[1+aBaseCoincInd[indEnt]]; // !!! will be used below
1016       aTangentType = SLVS_C_CURVE_CURVE_TANGENT;
1017     } else {
1018       aPtInd = aBaseCoincInd[indEnt];
1019       aPtFillet = myEntities[aFilletObjInd[indEnt]].point[aPtInd];
1020       if (isExists) {
1021         myConstraints[aCurConstrPos].ptA = aPtFillet;
1022         aCMapIter++;
1023         if (aCMapIter != aCMapEnd)
1024           aCurConstrPos = Search(*aCMapIter, myConstraints);
1025       } else {
1026         aPonCurveConstr = Slvs_MakeConstraint(
1027             ++myConstrMaxID, myID, SLVS_C_PT_ON_LINE, myWorkplane.h,
1028             0, aPtFillet, SLVS_E_UNKNOWN, aBaseObject[indEnt], SLVS_E_UNKNOWN);
1029       }
1030       aTangentType = SLVS_C_ARC_LINE_TANGENT;
1031     }
1032     if (!isExists) {
1033       myConstraints.push_back(aPonCurveConstr);
1034       aConstrList.push_back(aPonCurveConstr.h);
1035     }
1036   }
1037
1038   if (!isExists)
1039     myConstraintMap[theConstraint] = aConstrList;
1040
1041   // Additional temporary constraints for base objects to be fixed
1042   int aNbArcs = 0;
1043   for (unsigned int indAttr = 0; indAttr < 2; indAttr++) {
1044     if (!aBaseFeature[indAttr]) {
1045       AttributeRefAttrPtr aConstrAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
1046           aConstrData->attribute(aConstraintAttributes[indAttr]));
1047       addTemporaryConstraintWhereDragged(aConstrAttr->attr());
1048       continue;
1049     }
1050     if (aBaseFeature[indAttr]->getKind() == SketchPlugin_Line::ID()) {
1051       addTemporaryConstraintWhereDragged(
1052           aBaseFeature[indAttr]->attribute(SketchPlugin_Line::START_ID()));
1053       addTemporaryConstraintWhereDragged(
1054           aBaseFeature[indAttr]->attribute(SketchPlugin_Line::END_ID()));
1055     } else if (aBaseFeature[indAttr]->getKind() == SketchPlugin_Arc::ID()) {
1056       // Arc should be fixed by its center only (to avoid "conflicting constraints" message)
1057       // If the fillet is made on two arc, the shared point should be fixed too
1058       aNbArcs++;
1059       addTemporaryConstraintWhereDragged(
1060           aBaseFeature[indAttr]->attribute(SketchPlugin_Arc::CENTER_ID()));
1061     }
1062   }
1063   if (aNbArcs == 2) {
1064       addTemporaryConstraintWhereDragged(aBaseCoincInd[0] == 0 ?
1065           aBaseFeature[0]->attribute(SketchPlugin_Arc::START_ID()) :
1066           aBaseFeature[0]->attribute(SketchPlugin_Arc::END_ID()));
1067   }
1068   return true;
1069 }
1070
1071 // ============================================================================
1072 //  Function: changeEntity
1073 //  Class:    SketchSolver_ConstraintGroup
1074 //  Purpose:  create/update the element affected by any constraint
1075 // ============================================================================
1076 Slvs_hEntity SketchSolver_ConstraintGroup::changeEntity(
1077     std::shared_ptr<ModelAPI_Attribute> theEntity)
1078 {
1079   // If the entity is already in the group, try to find it
1080   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aEntIter =
1081       myEntityAttrMap.find(theEntity);
1082   int aEntPos;
1083   std::vector<Slvs_Param>::iterator aParamIter;  // looks at first parameter of already existent entity or at the end of vector otherwise
1084   if (aEntIter == myEntityAttrMap.end())  // no such entity => should be created
1085     aParamIter = myParams.end();
1086   else {  // the entity already exists
1087     aEntPos = Search(aEntIter->second, myEntities);
1088     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
1089     aParamIter = myParams.begin() + aParamPos;
1090   }
1091   const bool isEntExists = (aEntIter != myEntityAttrMap.end());  // defines that the entity already exists
1092   const bool isNeedToSolve = myNeedToSolve;
1093   myNeedToSolve = false;
1094
1095   if (isEntExists) {
1096     // Verify that the entity is not used by "Rigid" constraint.
1097     // If it is used, the object should not move.
1098     std::vector<std::set<Slvs_hEntity> >::iterator aCoincIter = myCoincidentPoints.begin();
1099     for (; aCoincIter != myCoincidentPoints.end(); aCoincIter++)
1100       if (aCoincIter->find(aEntIter->second) != aCoincIter->end())
1101         break;
1102     std::set<Slvs_hEntity> aCoincident;
1103     if (aCoincIter != myCoincidentPoints.end()) {
1104       aCoincident = *aCoincIter;
1105       aCoincident.erase(aEntIter->second);
1106
1107       std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1108       for (; aConstrIter != myConstraints.end(); aConstrIter++)
1109         if (aConstrIter->type == SLVS_C_WHERE_DRAGGED &&
1110             aCoincident.find(aConstrIter->ptA) != aCoincident.end()) {
1111           myNeedToSolve = true;
1112           return aEntIter->second;
1113         }
1114     }
1115   }
1116
1117   // Look over supported types of entities
1118   Slvs_Entity aNewEntity;
1119   aNewEntity.h = SLVS_E_UNKNOWN;
1120
1121   // Point in 3D
1122   std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1123       theEntity);
1124   if (aPoint) {
1125     Slvs_hParam aX = changeParameter(aPoint->x(), aParamIter);
1126     Slvs_hParam aY = changeParameter(aPoint->y(), aParamIter);
1127     Slvs_hParam aZ = changeParameter(aPoint->z(), aParamIter);
1128     if (!isEntExists) // New entity
1129       aNewEntity = Slvs_MakePoint3d(++myEntityMaxID, myID, aX, aY, aZ);
1130   } else {
1131     // All entities except 3D points are created on workplane. So, if there is no workplane yet, then error
1132     if (myWorkplane.h == SLVS_E_UNKNOWN)
1133       return SLVS_E_UNKNOWN;
1134
1135     // Point in 2D
1136     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1137         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
1138     if (aPoint2D) {
1139       Slvs_hParam aU = changeParameter(aPoint2D->x(), aParamIter);
1140       Slvs_hParam aV = changeParameter(aPoint2D->y(), aParamIter);
1141       if (!isEntExists) // New entity
1142         aNewEntity = Slvs_MakePoint2d(++myEntityMaxID, myID, myWorkplane.h, aU, aV);
1143     } else {
1144       // Scalar value (used for the distance entities)
1145       AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theEntity);
1146       if (aScalar) {
1147         Slvs_hParam aValue = changeParameter(aScalar->value(), aParamIter);
1148         if (!isEntExists) // New entity
1149           aNewEntity = Slvs_MakeDistance(++myEntityMaxID, myID, myWorkplane.h, aValue);
1150       }
1151     }
1152   }
1153   /// \todo Other types of entities
1154
1155   Slvs_hEntity aResult = SLVS_E_UNKNOWN; // Unsupported or wrong entity type
1156
1157   if (isEntExists) {
1158     myNeedToSolve = myNeedToSolve || isNeedToSolve;
1159     aResult = aEntIter->second;
1160   } else if (aNewEntity.h != SLVS_E_UNKNOWN) {
1161     myEntities.push_back(aNewEntity);
1162     myEntOfConstr.push_back(false);
1163     myEntityAttrMap[theEntity] = aNewEntity.h;
1164     aResult = aNewEntity.h;
1165   }
1166
1167   // If the attribute was changed by the user, we need to fix it before solving
1168   if (myNeedToSolve && theEntity->isImmutable())
1169     addTemporaryConstraintWhereDragged(theEntity, false);
1170
1171   return aResult;
1172 }
1173
1174 // ============================================================================
1175 //  Function: changeEntity
1176 //  Class:    SketchSolver_ConstraintGroup
1177 //  Purpose:  create/update the element defined by the feature affected by any constraint
1178 // ============================================================================
1179 Slvs_hEntity SketchSolver_ConstraintGroup::changeEntityFeature(FeaturePtr theEntity)
1180 {
1181   if (!theEntity->data()->isValid())
1182     return SLVS_E_UNKNOWN;
1183   // If the entity is already in the group, try to find it
1184   std::map<FeaturePtr, Slvs_hEntity>::const_iterator aEntIter = myEntityFeatMap.find(theEntity);
1185   // defines that the entity already exists
1186   const bool isEntExists = (myEntityFeatMap.find(theEntity) != myEntityFeatMap.end());
1187
1188   Slvs_Entity aNewEntity;
1189   aNewEntity.h = SLVS_E_UNKNOWN;
1190
1191   // SketchPlugin features
1192   std::shared_ptr<SketchPlugin_Feature> aFeature = std::dynamic_pointer_cast<
1193       SketchPlugin_Feature>(theEntity);
1194   if (aFeature) {  // Verify the feature by its kind
1195     const std::string& aFeatureKind = aFeature->getKind();
1196     AttributePtr anAttribute;
1197
1198     // Line
1199     if (aFeatureKind.compare(SketchPlugin_Line::ID()) == 0) {
1200       anAttribute = aFeature->data()->attribute(SketchPlugin_Line::START_ID());
1201       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1202       Slvs_hEntity aStart = changeEntity(anAttribute);
1203
1204       anAttribute = aFeature->data()->attribute(SketchPlugin_Line::END_ID());
1205       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1206       Slvs_hEntity aEnd = changeEntity(anAttribute);
1207
1208       if (!isEntExists) // New entity
1209         aNewEntity = Slvs_MakeLineSegment(++myEntityMaxID, myID, myWorkplane.h, aStart, aEnd);
1210     }
1211     // Circle
1212     else if (aFeatureKind.compare(SketchPlugin_Circle::ID()) == 0) {
1213       anAttribute = aFeature->data()->attribute(SketchPlugin_Circle::CENTER_ID());
1214       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1215       Slvs_hEntity aCenter = changeEntity(anAttribute);
1216
1217       anAttribute = aFeature->data()->attribute(SketchPlugin_Circle::RADIUS_ID());
1218       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1219       Slvs_hEntity aRadius = changeEntity(anAttribute);
1220
1221       if (!isEntExists) // New entity
1222         aNewEntity = Slvs_MakeCircle(++myEntityMaxID, myID, myWorkplane.h, aCenter,
1223                                      myWorkplane.normal, aRadius);
1224     }
1225     // Arc
1226     else if (aFeatureKind.compare(SketchPlugin_Arc::ID()) == 0) {
1227       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::CENTER_ID());
1228       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1229       Slvs_hEntity aCenter = changeEntity(anAttribute);
1230
1231       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::START_ID());
1232       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1233       Slvs_hEntity aStart = changeEntity(anAttribute);
1234
1235       anAttribute = aFeature->data()->attribute(SketchPlugin_Arc::END_ID());
1236       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1237       Slvs_hEntity aEnd = changeEntity(anAttribute);
1238
1239       if (!isEntExists)
1240         aNewEntity = Slvs_MakeArcOfCircle(++myEntityMaxID, myID, myWorkplane.h,
1241                                           myWorkplane.normal, aCenter, aStart, aEnd);
1242     }
1243     // Point (it has low probability to be an attribute of constraint, so it is checked at the end)
1244     else if (aFeatureKind.compare(SketchPlugin_Point::ID()) == 0) {
1245       anAttribute = aFeature->data()->attribute(SketchPlugin_Point::COORD_ID());
1246       if (!anAttribute->isInitialized()) return SLVS_E_UNKNOWN;
1247       Slvs_hEntity aPoint = changeEntity(anAttribute);
1248
1249       if (isEntExists)
1250         return aEntIter->second;
1251
1252       // Both the sketch point and its attribute (coordinates) link to the same SolveSpace point identifier
1253       myEntityFeatMap[theEntity] = aPoint;
1254       myNeedToSolve = true;
1255       return aPoint;
1256     }
1257   }
1258   /// \todo Other types of features
1259
1260   if (isEntExists)
1261     return aEntIter->second;
1262
1263   if (aNewEntity.h != SLVS_E_UNKNOWN) {
1264     myEntities.push_back(aNewEntity);
1265     myEntOfConstr.push_back(false);
1266     myEntityFeatMap[theEntity] = aNewEntity.h;
1267     myNeedToSolve = true;
1268     return aNewEntity.h;
1269   }
1270
1271   // Unsupported or wrong entity type
1272   return SLVS_E_UNKNOWN;
1273 }
1274
1275 // ============================================================================
1276 //  Function: changeNormal
1277 //  Class:    SketchSolver_ConstraintGroup
1278 //  Purpose:  create/update the normal of workplane
1279 // ============================================================================
1280 Slvs_hEntity SketchSolver_ConstraintGroup::changeNormal(
1281     std::shared_ptr<ModelAPI_Attribute> theDirX,
1282     std::shared_ptr<ModelAPI_Attribute> theNorm)
1283 {
1284   std::shared_ptr<GeomDataAPI_Dir> aNorm = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theNorm);
1285   std::shared_ptr<GeomDataAPI_Dir> aDirX = std::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirX);
1286   if (!aDirX || (fabs(aDirX->x()) + fabs(aDirX->y()) + fabs(aDirX->z()) < tolerance))
1287     return SLVS_E_UNKNOWN;
1288   // calculate Y direction
1289   std::shared_ptr<GeomAPI_Dir> aDirY(new GeomAPI_Dir(aNorm->dir()->cross(aDirX->dir())));
1290
1291   // quaternion parameters of normal vector
1292   double qw, qx, qy, qz;
1293   Slvs_MakeQuaternion(aDirX->x(), aDirX->y(), aDirX->z(), aDirY->x(), aDirY->y(), aDirY->z(), &qw,
1294                       &qx, &qy, &qz);
1295   double aNormCoord[4] = { qw, qx, qy, qz };
1296
1297   // Try to find existent normal
1298   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aEntIter =
1299       myEntityAttrMap.find(theNorm);
1300   std::vector<Slvs_Param>::iterator aParamIter;  // looks to the first parameter of already existent entity or to the end of vector otherwise
1301   if (aEntIter == myEntityAttrMap.end())  // no such entity => should be created
1302     aParamIter = myParams.end();
1303   else {  // the entity already exists, update it
1304     int aEntPos = Search(aEntIter->second, myEntities);
1305     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
1306     aParamIter = myParams.begin() + aParamPos;
1307   }
1308
1309   // Change parameters of the normal
1310   Slvs_hParam aNormParams[4];
1311   for (int i = 0; i < 4; i++)
1312     aNormParams[i] = changeParameter(aNormCoord[i], aParamIter);
1313
1314   if (aEntIter != myEntityAttrMap.end())  // the entity already exists
1315     return aEntIter->second;
1316
1317   // Create a normal
1318   Slvs_Entity aNormal = Slvs_MakeNormal3d(++myEntityMaxID, myID, aNormParams[0], aNormParams[1],
1319                                           aNormParams[2], aNormParams[3]);
1320   myEntities.push_back(aNormal);
1321   myEntOfConstr.push_back(false);
1322   myEntityAttrMap[theNorm] = aNormal.h;
1323   return aNormal.h;
1324 }
1325
1326 // ============================================================================
1327 //  Function: addWorkplane
1328 //  Class:    SketchSolver_ConstraintGroup
1329 //  Purpose:  create workplane for the group
1330 // ============================================================================
1331 bool SketchSolver_ConstraintGroup::addWorkplane(std::shared_ptr<ModelAPI_CompositeFeature> theSketch)
1332 {
1333   if (myWorkplane.h || theSketch->getKind().compare(SketchPlugin_Sketch::ID()) != 0)
1334     return false;  // the workplane already exists or the function parameter is not Sketch
1335
1336   mySketch = theSketch;
1337   updateWorkplane();
1338   return true;
1339 }
1340
1341 // ============================================================================
1342 //  Function: updateWorkplane
1343 //  Class:    SketchSolver_ConstraintGroup
1344 //  Purpose:  update parameters of workplane
1345 // ============================================================================
1346 bool SketchSolver_ConstraintGroup::updateWorkplane()
1347 {
1348   if (!mySketch->data())
1349     return false; // case sketch is deleted
1350   // Get parameters of workplane
1351   std::shared_ptr<ModelAPI_Attribute> aDirX = mySketch->data()->attribute(
1352       SketchPlugin_Sketch::DIRX_ID());
1353   std::shared_ptr<ModelAPI_Attribute> aNorm = mySketch->data()->attribute(
1354       SketchPlugin_Sketch::NORM_ID());
1355   std::shared_ptr<ModelAPI_Attribute> anOrigin = mySketch->data()->attribute(
1356       SketchPlugin_Sketch::ORIGIN_ID());
1357   // Transform them into SolveSpace format
1358   Slvs_hEntity aNormalWP = changeNormal(aDirX, aNorm);
1359   if (!aNormalWP)
1360     return false;
1361   Slvs_hEntity anOriginWP = changeEntity(anOrigin);
1362   if (!anOriginWP)
1363     return false;
1364
1365   if (!myWorkplane.h) {
1366     // Create workplane
1367     myWorkplane = Slvs_MakeWorkplane(++myEntityMaxID, myID, anOriginWP, aNormalWP);
1368     // Workplane should be added to the list of entities
1369     myEntities.push_back(myWorkplane);
1370     myEntOfConstr.push_back(false);
1371   }
1372   return true;
1373 }
1374
1375 // ============================================================================
1376 //  Function: changeParameter
1377 //  Class:    SketchSolver_ConstraintGroup
1378 //  Purpose:  create/update value of parameter
1379 // ============================================================================
1380 Slvs_hParam SketchSolver_ConstraintGroup::changeParameter(
1381     double theParam, std::vector<Slvs_Param>::iterator& thePrmIter)
1382 {
1383   if (thePrmIter != myParams.end()) {  // Parameter should be updated
1384     int aParamPos = thePrmIter - myParams.begin();
1385     if (fabs(thePrmIter->val - theParam) > tolerance) {
1386       myNeedToSolve = true;  // parameter is changed, need to resolve constraints
1387       myParams[aParamPos].val = theParam;
1388     }
1389     thePrmIter++;
1390     return myParams[aParamPos].h;
1391   }
1392
1393   // Newly created parameter
1394   Slvs_Param aParam = Slvs_MakeParam(++myParamMaxID, myID, theParam);
1395   myParams.push_back(aParam);
1396   myNeedToSolve = true;
1397   // The list of parameters is changed, move iterator to the end of the list to avoid problems
1398   thePrmIter = myParams.end();
1399   return aParam.h;
1400 }
1401
1402 // ============================================================================
1403 //  Function: resolveConstraints
1404 //  Class:    SketchSolver_ConstraintGroup
1405 //  Purpose:  solve the set of constraints for the current group
1406 // ============================================================================
1407 bool SketchSolver_ConstraintGroup::resolveConstraints()
1408 {
1409   if (!myNeedToSolve)
1410     return false;
1411
1412   myConstrSolver.setGroupID(myID);
1413   myConstrSolver.setParameters(myParams);
1414   myConstrSolver.setEntities(myEntities);
1415   myConstrSolver.setConstraints(myConstraints);
1416   myConstrSolver.setDraggedParameters(myTempPointWhereDragged);
1417
1418   int aResult = myConstrSolver.solve();
1419   if (aResult == SLVS_RESULT_OKAY) {  // solution succeeded, store results into correspondent attributes
1420                                       // Obtain result into the same list of parameters
1421     if (!myConstrSolver.getResult(myParams))
1422       return true;
1423
1424     // We should go through the attributes map, because only attributes have valued parameters
1425     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntIter =
1426         myEntityAttrMap.begin();
1427     for (; anEntIter != myEntityAttrMap.end(); anEntIter++) {
1428       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1429         anEntIter->first->owner()->data()->blockSendAttributeUpdated(true);
1430       if (updateAttribute(anEntIter->first, anEntIter->second))
1431         updateRelatedConstraints(anEntIter->first);
1432     }
1433     updateFilletConstraints();
1434     // unblock all features then
1435     for (anEntIter = myEntityAttrMap.begin(); anEntIter != myEntityAttrMap.end(); anEntIter++) {
1436       if (anEntIter->first->owner().get() && anEntIter->first->owner()->data().get())
1437         anEntIter->first->owner()->data()->blockSendAttributeUpdated(false);
1438     }
1439   } else if (!myConstraints.empty())
1440     Events_Error::send(SketchSolver_Error::CONSTRAINTS(), this);
1441
1442   removeTemporaryConstraints();
1443   myNeedToSolve = false;
1444   return true;
1445 }
1446
1447 // ============================================================================
1448 //  Function: mergeGroups
1449 //  Class:    SketchSolver_ConstraintGroup
1450 //  Purpose:  append specified group to the current group
1451 // ============================================================================
1452 void SketchSolver_ConstraintGroup::mergeGroups(const SketchSolver_ConstraintGroup& theGroup)
1453 {
1454   // If specified group is empty, no need to merge
1455   if (theGroup.myConstraintMap.empty())
1456     return;
1457
1458   // Map between old and new indexes of SolveSpace constraints
1459   std::map<Slvs_hConstraint, Slvs_hConstraint> aConstrMap;
1460
1461   // Add all constraints from theGroup to the current group
1462   ConstraintMap::const_iterator aConstrIter = theGroup.myConstraintMap.begin();
1463   for (; aConstrIter != theGroup.myConstraintMap.end(); aConstrIter++)
1464     if (changeConstraint(aConstrIter->first))
1465       aConstrMap[aConstrIter->second.back()] = myConstrMaxID;  // the constraint was added => store its ID
1466
1467   // Add temporary constraints from theGroup
1468   std::list<Slvs_hConstraint>::const_iterator aTempConstrIter = theGroup.myTempConstraints.begin();
1469   for (; aTempConstrIter != theGroup.myTempConstraints.end(); aTempConstrIter++) {
1470     std::map<Slvs_hConstraint, Slvs_hConstraint>::iterator aFind = aConstrMap.find(
1471         *aTempConstrIter);
1472     if (aFind != aConstrMap.end())
1473       myTempConstraints.push_back(aFind->second);
1474   }
1475
1476   if (myTempPointWhereDragged.empty())
1477     myTempPointWhereDragged = theGroup.myTempPointWhereDragged;
1478   else if (!theGroup.myTempPointWhereDragged.empty()) {  // Need to create additional transient constraint
1479     std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator aFeatureIter =
1480         theGroup.myEntityAttrMap.begin();
1481     for (; aFeatureIter != theGroup.myEntityAttrMap.end(); aFeatureIter++)
1482       if (aFeatureIter->second == myTempPointWDrgdID) {
1483         addTemporaryConstraintWhereDragged(aFeatureIter->first);
1484         break;
1485       }
1486   }
1487
1488   myNeedToSolve = myNeedToSolve || theGroup.myNeedToSolve;
1489 }
1490
1491 // ============================================================================
1492 //  Function: splitGroup
1493 //  Class:    SketchSolver_ConstraintGroup
1494 //  Purpose:  divide the group into several subgroups
1495 // ============================================================================
1496 void SketchSolver_ConstraintGroup::splitGroup(std::vector<SketchSolver_ConstraintGroup*>& theCuts)
1497 {
1498   // Divide constraints and entities into several groups
1499   std::vector<std::set<Slvs_hEntity> > aGroupsEntities;
1500   std::vector<std::set<Slvs_hConstraint> > aGroupsConstr;
1501   int aMaxNbEntities = 0;  // index of the group with maximal nuber of elements (this group will be left in the current)
1502   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1503   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1504     Slvs_hEntity aConstrEnt[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA,
1505         aConstrIter->entityB };
1506     std::vector<int> anIndexes;
1507     // Go through the groupped entities and find even one of entities of current constraint
1508     std::vector<std::set<Slvs_hEntity> >::iterator aGrEntIter;
1509     for (aGrEntIter = aGroupsEntities.begin(); aGrEntIter != aGroupsEntities.end(); aGrEntIter++) {
1510       bool isFound = false;
1511       for (int i = 0; i < 4 && !isFound; i++)
1512         if (aConstrEnt[i] != 0) {
1513           isFound = (aGrEntIter->find(aConstrEnt[i]) != aGrEntIter->end());
1514           // Also we need to check sub-entities
1515           int aEntPos = Search(aConstrEnt[i], myEntities);
1516           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1517             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1518             for (int j = 0; *aSub != 0 && j < 4 && !isFound; aSub++, j++)
1519               isFound = (aGrEntIter->find(*aSub) != aGrEntIter->end());
1520           }
1521         }
1522       if (isFound)
1523         anIndexes.push_back(aGrEntIter - aGroupsEntities.begin());
1524     }
1525     // Add new group if no one is found
1526     if (anIndexes.empty()) {
1527       std::set<Slvs_hEntity> aNewGrEnt;
1528       for (int i = 0; i < 4; i++)
1529         if (aConstrEnt[i] != 0) {
1530           aNewGrEnt.insert(aConstrEnt[i]);
1531           int aEntPos = Search(aConstrEnt[i], myEntities);
1532           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1533             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1534             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1535               aNewGrEnt.insert(*aSub);
1536           }
1537         }
1538       std::set<Slvs_hConstraint> aNewGrConstr;
1539       aNewGrConstr.insert(aConstrIter->h);
1540
1541       aGroupsEntities.push_back(aNewGrEnt);
1542       aGroupsConstr.push_back(aNewGrConstr);
1543       if (aNewGrEnt.size() > aGroupsEntities[aMaxNbEntities].size())
1544         aMaxNbEntities = aGroupsEntities.size() - 1;
1545     } else {  // Add entities indexes into the found group
1546       aGrEntIter = aGroupsEntities.begin() + anIndexes.front();
1547       for (int i = 0; i < 4; i++)
1548         if (aConstrEnt[i] != 0) {
1549           aGrEntIter->insert(aConstrEnt[i]);
1550           int aEntPos = Search(aConstrEnt[i], myEntities);
1551           if (aEntPos != myEntities.size()) { // MPV: to fix the crash on close
1552             Slvs_hEntity* aSub = myEntities[aEntPos].point;
1553             for (int j = 0; *aSub != 0 && j < 4; aSub++, j++)
1554               aGrEntIter->insert(*aSub);
1555           }
1556         }
1557       aGroupsConstr[anIndexes.front()].insert(aConstrIter->h);
1558       if (aGrEntIter->size() > aGroupsEntities[aMaxNbEntities].size())
1559         aMaxNbEntities = aGrEntIter - aGroupsEntities.begin();
1560       if (anIndexes.size() > 1) {  // There are found several connected groups, merge them
1561         std::vector<std::set<Slvs_hEntity> >::iterator aFirstGroup = aGroupsEntities.begin()
1562             + anIndexes.front();
1563         std::vector<std::set<Slvs_hConstraint> >::iterator aFirstConstr = aGroupsConstr.begin()
1564             + anIndexes.front();
1565         std::vector<int>::iterator anInd = anIndexes.begin();
1566         for (++anInd; anInd != anIndexes.end(); anInd++) {
1567           aFirstGroup->insert(aGroupsEntities[*anInd].begin(), aGroupsEntities[*anInd].end());
1568           aFirstConstr->insert(aGroupsConstr[*anInd].begin(), aGroupsConstr[*anInd].end());
1569         }
1570         if (aFirstGroup->size() > aGroupsEntities[aMaxNbEntities].size())
1571           aMaxNbEntities = anIndexes.front();
1572         // Remove merged groups
1573         for (anInd = anIndexes.end() - 1; anInd != anIndexes.begin(); anInd--) {
1574           aGroupsEntities.erase(aGroupsEntities.begin() + (*anInd));
1575           aGroupsConstr.erase(aGroupsConstr.begin() + (*anInd));
1576         }
1577       }
1578     }
1579   }
1580
1581   if (aGroupsEntities.size() <= 1)
1582     return;
1583
1584   // Remove the group with maximum elements as it will be left in the current group
1585   aGroupsEntities.erase(aGroupsEntities.begin() + aMaxNbEntities);
1586   aGroupsConstr.erase(aGroupsConstr.begin() + aMaxNbEntities);
1587
1588   // Add new groups of constraints and divide current group
1589   std::vector<SketchSolver_ConstraintGroup*> aNewGroups;
1590   for (int i = aGroupsEntities.size(); i > 0; i--) {
1591     SketchSolver_ConstraintGroup* aG = new SketchSolver_ConstraintGroup(mySketch);
1592     aNewGroups.push_back(aG);
1593   }
1594   ConstraintMap::const_iterator aConstrMapIter = myConstraintMap.begin();
1595   int aConstrMapPos = 0;  // position of iterator in the map (used to restore iterator after removing constraint)
1596   while (aConstrMapIter != myConstraintMap.end()) {
1597     std::vector<std::set<Slvs_hConstraint> >::const_iterator aGIter = aGroupsConstr.begin();
1598     std::vector<SketchSolver_ConstraintGroup*>::iterator aGroup = aNewGroups.begin();
1599     for (; aGIter != aGroupsConstr.end(); aGIter++, aGroup++)
1600       if (aGIter->find(aConstrMapIter->second.front()) != aGIter->end()) {
1601         (*aGroup)->changeConstraint(aConstrMapIter->first);
1602         removeConstraint(aConstrMapIter->first);
1603         // restore iterator
1604         aConstrMapIter = myConstraintMap.begin();
1605         for (int i = 0; i < aConstrMapPos; i++)
1606           aConstrMapIter++;
1607         break;
1608       }
1609     if (aGIter == aGroupsConstr.end()) {
1610       aConstrMapIter++;
1611       aConstrMapPos++;
1612     }
1613   }
1614
1615   theCuts.insert(theCuts.end(), aNewGroups.begin(), aNewGroups.end());
1616 }
1617
1618 // ============================================================================
1619 //  Function: updateGroup
1620 //  Class:    SketchSolver_ConstraintGroup
1621 //  Purpose:  search removed entities and constraints
1622 // ============================================================================
1623 bool SketchSolver_ConstraintGroup::updateGroup()
1624 {
1625   ConstraintMap::reverse_iterator aConstrIter = myConstraintMap.rbegin();
1626   bool isAllValid = true;
1627   bool isCCRemoved = false;  // indicates that at least one of coincidence constraints was removed
1628   int aConstrIndex = 0;
1629   while (/*isAllValid && */aConstrIter != myConstraintMap.rend()) {
1630     if (!aConstrIter->first->data() || !aConstrIter->first->data()->isValid()) {
1631       if (aConstrIter->first->getKind().compare(SketchPlugin_ConstraintCoincidence::ID()) == 0)
1632         isCCRemoved = true;
1633       removeConstraint(aConstrIter->first);
1634       isAllValid = false;
1635       // Get back the correct position of iterator after the "remove" operation
1636       aConstrIter = myConstraintMap.rbegin();
1637       for (int i = 0; i < aConstrIndex && aConstrIter != myConstraintMap.rend(); i++)
1638         aConstrIter++;
1639     } else {
1640       aConstrIter++;
1641       aConstrIndex++;
1642     }
1643   }
1644
1645   // Check if some entities are invalid too
1646   std::set<Slvs_hEntity> anEntToRemove;
1647   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1648       anAttrIter = myEntityAttrMap.begin();
1649   while (anAttrIter != myEntityAttrMap.end()) {
1650     if (!anAttrIter->first->owner() || !anAttrIter->first->owner()->data() ||
1651         !anAttrIter->first->owner()->data()->isValid()) {
1652       anEntToRemove.insert(anAttrIter->second);
1653       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
1654           aRemovedIter = anAttrIter;
1655       anAttrIter++;
1656       myEntityAttrMap.erase(aRemovedIter);
1657     } else
1658       anAttrIter++;
1659   }
1660   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIter = myEntityFeatMap.begin();
1661   while (aFeatIter != myEntityFeatMap.end()) {
1662     if (!aFeatIter->first || !aFeatIter->first->data() ||
1663         !aFeatIter->first->data()->isValid()) {
1664       anEntToRemove.insert(aFeatIter->second);
1665       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = aFeatIter;
1666       aFeatIter++;
1667       myEntityFeatMap.erase(aRemovedIter);
1668     } else
1669       aFeatIter++;
1670   }
1671   removeEntitiesById(anEntToRemove);
1672
1673   // Probably, need to update coincidence constraints
1674   if (isCCRemoved && !myExtraCoincidence.empty()) {
1675     // Make a copy, because the new list of unused constrtaints will be generated
1676     std::set<std::shared_ptr<SketchPlugin_Constraint> > anExtraCopy = myExtraCoincidence;
1677     myExtraCoincidence.clear();
1678
1679     std::set<std::shared_ptr<SketchPlugin_Constraint> >::iterator aCIter = anExtraCopy.begin();
1680     for (; aCIter != anExtraCopy.end(); aCIter++)
1681       if ((*aCIter)->data() && (*aCIter)->data()->isValid())
1682         changeConstraint(*aCIter);
1683   }
1684
1685   return !isAllValid;
1686 }
1687
1688 // ============================================================================
1689 //  Function: updateAttribute
1690 //  Class:    SketchSolver_ConstraintGroup
1691 //  Purpose:  update features of sketch after resolving constraints
1692 // ============================================================================
1693 bool SketchSolver_ConstraintGroup::updateAttribute(
1694     std::shared_ptr<ModelAPI_Attribute> theAttribute, const Slvs_hEntity& theEntityID)
1695 {
1696   // Search the position of the first parameter of the entity
1697   int anEntPos = Search(theEntityID, myEntities);
1698   int aFirstParamPos = Search(myEntities[anEntPos].param[0], myParams);
1699
1700   // Look over supported types of entities
1701
1702   // Point in 3D
1703   std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1704       theAttribute);
1705   if (aPoint) {
1706     if (fabs(aPoint->x() - myParams[aFirstParamPos].val) > tolerance
1707         || fabs(aPoint->y() - myParams[aFirstParamPos + 1].val) > tolerance
1708         || fabs(aPoint->z() - myParams[aFirstParamPos + 2].val) > tolerance) {
1709       aPoint->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val,
1710                        myParams[aFirstParamPos + 2].val);
1711       return true;
1712     }
1713     return false;
1714   }
1715
1716   // Point in 2D
1717   std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
1718       std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theAttribute);
1719   if (aPoint2D) {
1720     if (fabs(aPoint2D->x() - myParams[aFirstParamPos].val) > tolerance
1721         || fabs(aPoint2D->y() - myParams[aFirstParamPos + 1].val) > tolerance) {
1722       aPoint2D->setValue(myParams[aFirstParamPos].val, myParams[aFirstParamPos + 1].val);
1723       return true;
1724     }
1725     return false;
1726   }
1727
1728   // Scalar value
1729   AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theAttribute);
1730   if (aScalar) {
1731     if (fabs(aScalar->value() - myParams[aFirstParamPos].val) > tolerance) {
1732       aScalar->setValue(myParams[aFirstParamPos].val);
1733       return true;
1734     }
1735     return false;
1736   }
1737
1738   /// \todo Support other types of entities
1739   return false;
1740 }
1741
1742 // ============================================================================
1743 //  Function: updateEntityIfPossible
1744 //  Class:    SketchSolver_ConstraintGroup
1745 //  Purpose:  search the entity in this group and update it
1746 // ============================================================================
1747 void SketchSolver_ConstraintGroup::updateEntityIfPossible(
1748     std::shared_ptr<ModelAPI_Attribute> theEntity)
1749 {
1750   if (myEntityAttrMap.find(theEntity) != myEntityAttrMap.end()) {
1751     // If the attribute is a point and it is changed (the group needs to rebuild),
1752     // probably user has dragged this point into this position,
1753     // so it is necessary to add constraint which will guarantee the point will not change
1754
1755     // Store myNeedToSolve flag to verify the entity is really changed
1756     bool aNeedToSolveCopy = myNeedToSolve;
1757     myNeedToSolve = false;
1758
1759     changeEntity(theEntity);
1760
1761     if (myNeedToSolve)  // the entity is changed
1762     {
1763       // Verify the entity is a point and add temporary constraint of permanency
1764       std::shared_ptr<GeomDataAPI_Point> aPoint = std::dynamic_pointer_cast<GeomDataAPI_Point>(
1765           theEntity);
1766       std::shared_ptr<GeomDataAPI_Point2D> aPoint2D = std::dynamic_pointer_cast<
1767           GeomDataAPI_Point2D>(theEntity);
1768       if (aPoint || aPoint2D)
1769         addTemporaryConstraintWhereDragged(theEntity);
1770     }
1771
1772     // Restore flag of changes
1773     myNeedToSolve = myNeedToSolve || aNeedToSolveCopy;
1774
1775     if (myNeedToSolve)
1776       updateRelatedConstraints(theEntity);
1777   }
1778 }
1779
1780 // ============================================================================
1781 //  Function: addTemporaryConstraintWhereDragged
1782 //  Class:    SketchSolver_ConstraintGroup
1783 //  Purpose:  add transient constraint SLVS_C_WHERE_DRAGGED for the entity,
1784 //            which was moved by user
1785 // ============================================================================
1786 void SketchSolver_ConstraintGroup::addTemporaryConstraintWhereDragged(
1787     std::shared_ptr<ModelAPI_Attribute> theEntity,
1788     bool theAllowToFit)
1789 {
1790   // Find identifier of the entity
1791   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator anEntIter =
1792       myEntityAttrMap.find(theEntity);
1793   if (anEntIter == myEntityAttrMap.end())
1794     return;
1795
1796   // Get identifiers of all dragged points
1797   std::set<Slvs_hEntity> aDraggedPntID;
1798   aDraggedPntID.insert(myTempPointWDrgdID);
1799   std::list<Slvs_hConstraint>::const_iterator aTmpCoIter = myTempConstraints.begin();
1800   for (; aTmpCoIter != myTempConstraints.end(); aTmpCoIter++) {
1801     unsigned int aConstrPos = Search(*aTmpCoIter, myConstraints);
1802     if (aConstrPos < myConstraints.size())
1803       aDraggedPntID.insert(myConstraints[aConstrPos].ptA);
1804   }
1805   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1806   for (; aConstrIter != myConstraints.end(); aConstrIter++)
1807     if (aConstrIter->type == SLVS_C_WHERE_DRAGGED)
1808       aDraggedPntID.insert(aConstrIter->ptA);
1809   // Find whether there is a point coincident with theEntity, which already has SLVS_C_WHERE_DRAGGED
1810   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
1811   for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++) {
1812     if (aCoPtIter->find(anEntIter->second) == aCoPtIter->end())
1813       continue;  // the entity was not found in current set
1814
1815     // Find one of already created SLVS_C_WHERE_DRAGGED constraints in current set of coincident points
1816     std::set<Slvs_hEntity>::const_iterator aDrgIter = aDraggedPntID.begin();
1817     for (; aDrgIter != aDraggedPntID.end(); aDrgIter++)
1818       if (aCoPtIter->find(*aDrgIter) != aCoPtIter->end())
1819         return;  // the SLVS_C_WHERE_DRAGGED constraint already exists
1820   }
1821   if (aDraggedPntID.find(anEntIter->second) != aDraggedPntID.end())
1822     return;
1823
1824   // If this is a first dragged point, its parameters should be placed
1825   // into Slvs_System::dragged field to avoid system inconsistense
1826   if (myTempPointWhereDragged.empty() && theAllowToFit) {
1827     int anEntPos = Search(anEntIter->second, myEntities);
1828     Slvs_hParam* aDraggedParam = myEntities[anEntPos].param;
1829     for (int i = 0; i < 4; i++, aDraggedParam++)
1830       if (*aDraggedParam != 0)
1831         myTempPointWhereDragged.push_back(*aDraggedParam);
1832     myTempPointWDrgdID = myEntities[anEntPos].h;
1833     return;
1834   }
1835
1836   // Create additional SLVS_C_WHERE_DRAGGED constraint if myTempPointWhereDragged field is not empty
1837   Slvs_Constraint aWDConstr = Slvs_MakeConstraint(++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED,
1838                                                   myWorkplane.h, 0.0, anEntIter->second, 0, 0, 0);
1839   myConstraints.push_back(aWDConstr);
1840   myTempConstraints.push_back(aWDConstr.h);
1841 }
1842
1843 // ============================================================================
1844 //  Function: removeTemporaryConstraints
1845 //  Class:    SketchSolver_ConstraintGroup
1846 //  Purpose:  remove all transient SLVS_C_WHERE_DRAGGED constraints after
1847 //            resolving the set of constraints
1848 // ============================================================================
1849 void SketchSolver_ConstraintGroup::removeTemporaryConstraints(
1850     const std::set<Slvs_hConstraint>& theRemoved)
1851 {
1852   std::list<Slvs_hConstraint>::reverse_iterator aTmpConstrIter;
1853   for (aTmpConstrIter = myTempConstraints.rbegin(); aTmpConstrIter != myTempConstraints.rend();
1854       aTmpConstrIter++) {
1855     if (!theRemoved.empty() && theRemoved.find(*aTmpConstrIter) == theRemoved.end())
1856       continue;
1857     unsigned int aConstrPos = Search(*aTmpConstrIter, myConstraints);
1858     if (aConstrPos >= myConstraints.size())
1859       continue;
1860     myConstraints.erase(myConstraints.begin() + aConstrPos);
1861
1862     // If the removing constraint has higher index, decrease the indexer
1863     if (*aTmpConstrIter == myConstrMaxID)
1864       myConstrMaxID--;
1865   }
1866   myTempConstraints.clear();
1867
1868   // Clear basic dragged point
1869   myTempPointWhereDragged.clear();
1870   myTempPointWDrgdID = SLVS_E_UNKNOWN;
1871 }
1872
1873 // ============================================================================
1874 //  Function: removeConstraint
1875 //  Class:    SketchSolver_ConstraintGroup
1876 //  Purpose:  remove constraint and all unused entities
1877 // ============================================================================
1878 void SketchSolver_ConstraintGroup::removeConstraint(
1879     std::shared_ptr<SketchPlugin_Constraint> theConstraint)
1880 {
1881   ConstraintMap::iterator anIterToRemove = myConstraintMap.find(theConstraint);
1882   if (anIterToRemove == myConstraintMap.end())
1883     return;
1884
1885   std::vector<Slvs_hConstraint> aCnstrToRemove = anIterToRemove->second;
1886   // Remove constraint from the map
1887   myConstraintMap.erase(anIterToRemove);
1888
1889   std::set<Slvs_hEntity> anEntToRemove;
1890
1891   // Find unused entities
1892   std::vector<Slvs_hConstraint>::iterator aCnstrToRemoveIter = aCnstrToRemove.begin();
1893   for (; aCnstrToRemoveIter != aCnstrToRemove.end(); aCnstrToRemoveIter++) {
1894     int aConstrPos = Search(*aCnstrToRemoveIter, myConstraints);
1895     Slvs_hEntity aCnstEnt[] = { myConstraints[aConstrPos].ptA, myConstraints[aConstrPos].ptB,
1896         myConstraints[aConstrPos].entityA, myConstraints[aConstrPos].entityB };
1897     for (int i = 0; i < 4; i++)
1898       if (aCnstEnt[i] != 0)
1899         anEntToRemove.insert(aCnstEnt[i]);
1900     myConstraints.erase(myConstraints.begin() + aConstrPos);
1901     if (*aCnstrToRemoveIter == myConstrMaxID)
1902       myConstrMaxID--;
1903   }
1904
1905   // Find all entities which are based on these unused
1906   std::vector<Slvs_Entity>::const_iterator anEntIter = myEntities.begin();
1907   for ( ; anEntIter != myEntities.end(); anEntIter++)
1908     if (anEntIter->type == SLVS_E_LINE_SEGMENT || anEntIter->type == SLVS_E_CIRCLE ||
1909         anEntIter->type == SLVS_E_ARC_OF_CIRCLE) {
1910       for (int i = 0; i < 4; i++)
1911         if (anEntToRemove.find(anEntIter->point[i]) != anEntToRemove.end()) {
1912           anEntToRemove.insert(anEntIter->h);
1913           for (int j = 0; j < 4; j++)
1914             if (anEntIter->point[j] != 0)
1915               anEntToRemove.insert(anEntIter->point[j]);
1916           break;
1917         }
1918     }
1919
1920   // Find entities used by remaining constraints and remove them from the list to delete
1921   std::vector<Slvs_Constraint>::const_iterator aConstrIter = myConstraints.begin();
1922   for (; aConstrIter != myConstraints.end(); aConstrIter++) {
1923     Slvs_hEntity aEnts[] = { aConstrIter->ptA, aConstrIter->ptB, aConstrIter->entityA, aConstrIter
1924         ->entityB };
1925     for (int i = 0; i < 4; i++)
1926       if (aEnts[i] != 0 && anEntToRemove.find(aEnts[i]) != anEntToRemove.end()) {
1927         anEntToRemove.erase(aEnts[i]);
1928         // remove from the list all points of current entity
1929         int aEntPos = Search(aEnts[i], myEntities);
1930         for (int j = 0; j < 4; j++)
1931           if (myEntities[aEntPos].point[j] != 0)
1932             anEntToRemove.erase(myEntities[aEntPos].point[j]);
1933       }
1934   }
1935
1936   if (anEntToRemove.empty())
1937     return;
1938
1939   // Remove unused entities
1940   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator anEntAttrIter =
1941       myEntityAttrMap.begin();
1942   while (anEntAttrIter != myEntityAttrMap.end()) {
1943     if (anEntToRemove.find(anEntAttrIter->second) != anEntToRemove.end()) {
1944       std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator aRemovedIter =
1945           anEntAttrIter;
1946       anEntAttrIter++;
1947       myEntityAttrMap.erase(aRemovedIter);
1948     } else
1949       anEntAttrIter++;
1950   }
1951   std::map<FeaturePtr, Slvs_hEntity>::iterator anEntFeatIter = myEntityFeatMap.begin();
1952   while (anEntFeatIter != myEntityFeatMap.end()) {
1953     if (anEntToRemove.find(anEntFeatIter->second) != anEntToRemove.end()) {
1954       std::map<FeaturePtr, Slvs_hEntity>::iterator aRemovedIter = anEntFeatIter;
1955       anEntFeatIter++;
1956       myEntityFeatMap.erase(aRemovedIter);
1957     } else
1958       anEntFeatIter++;
1959   }
1960
1961   removeEntitiesById(anEntToRemove);
1962
1963   if (myCoincidentPoints.size() == 1 && myCoincidentPoints.front().empty())
1964     myCoincidentPoints.clear();
1965 }
1966
1967 // ============================================================================
1968 //  Function: removeEntitiesById
1969 //  Class:    SketchSolver_ConstraintGroup
1970 //  Purpose:  Removes specified entities and their parameters
1971 // ============================================================================
1972 void SketchSolver_ConstraintGroup::removeEntitiesById(const std::set<Slvs_hEntity>& theEntities)
1973 {
1974   std::set<Slvs_hEntity>::const_reverse_iterator aRemIter = theEntities.rbegin();
1975   for (; aRemIter != theEntities.rend(); aRemIter++) {
1976     unsigned int anEntPos = Search(*aRemIter, myEntities);
1977     if (anEntPos >= myEntities.size())
1978       continue;
1979     if (myEntities[anEntPos].param[0] != 0) {
1980       unsigned int aParamPos = Search(myEntities[anEntPos].param[0], myParams);
1981       if (aParamPos >= myParams.size())
1982         continue;
1983       int aNbParams = 0;
1984       while (myEntities[anEntPos].param[aNbParams] != 0)
1985         aNbParams++;
1986       if (myEntities[anEntPos].param[aNbParams - 1] == myParamMaxID)
1987         myParamMaxID -= aNbParams;
1988       myParams.erase(myParams.begin() + aParamPos, myParams.begin() + aParamPos + aNbParams);
1989       if (*aRemIter == myEntityMaxID)
1990         myEntityMaxID--;
1991     }
1992     myEntities.erase(myEntities.begin() + anEntPos);
1993     myEntOfConstr.erase(myEntOfConstr.begin() + anEntPos);
1994
1995     // Remove entity's ID from the lists of conincident points
1996     std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
1997     for (; aCoPtIter != myCoincidentPoints.end(); aCoPtIter++)
1998       aCoPtIter->erase(*aRemIter);
1999   }
2000 }
2001
2002 // ============================================================================
2003 //  Function: addCoincidentPoints
2004 //  Class:    SketchSolver_ConstraintGroup
2005 //  Purpose:  add coincident point the appropriate list of such points
2006 // ============================================================================
2007 bool SketchSolver_ConstraintGroup::addCoincidentPoints(const Slvs_hEntity& thePoint1,
2008                                                        const Slvs_hEntity& thePoint2)
2009 {
2010   std::vector<std::set<Slvs_hEntity> >::iterator aCoPtIter = myCoincidentPoints.begin();
2011   std::vector<std::set<Slvs_hEntity> >::iterator aFirstFound = myCoincidentPoints.end();
2012   while (aCoPtIter != myCoincidentPoints.end()) {
2013     bool isFound[2] = {  // indicate which point ID was already in coincidence constraint
2014         aCoPtIter->find(thePoint1) != aCoPtIter->end(), aCoPtIter->find(thePoint2)
2015             != aCoPtIter->end(), };
2016     if (isFound[0] && isFound[1])  // points are already connected by coincidence constraints => no need additional one
2017       return false;
2018     if ((isFound[0] && !isFound[1]) || (!isFound[0] && isFound[1])) {
2019       if (aFirstFound != myCoincidentPoints.end()) {  // there are two groups of coincident points connected by created constraint => merge them
2020         int aFirstFoundShift = aFirstFound - myCoincidentPoints.begin();
2021         int aCurrentShift = aCoPtIter - myCoincidentPoints.begin();
2022         aFirstFound->insert(aCoPtIter->begin(), aCoPtIter->end());
2023         myCoincidentPoints.erase(aCoPtIter);
2024         aFirstFound = myCoincidentPoints.begin() + aFirstFoundShift;
2025         aCoPtIter = myCoincidentPoints.begin() + aCurrentShift;
2026         continue;
2027       } else {
2028         aCoPtIter->insert(isFound[0] ? thePoint2 : thePoint1);
2029         aFirstFound = aCoPtIter;
2030       }
2031     }
2032     aCoPtIter++;
2033   }
2034   // No points were found, need to create new set
2035   if (aFirstFound == myCoincidentPoints.end()) {
2036     std::set<Slvs_hEntity> aNewSet;
2037     aNewSet.insert(thePoint1);
2038     aNewSet.insert(thePoint2);
2039     myCoincidentPoints.push_back(aNewSet);
2040   }
2041
2042   return true;
2043 }
2044
2045 // ============================================================================
2046 //  Function: updateRelatedConstraints
2047 //  Class:    SketchSolver_ConstraintGroup
2048 //  Purpose:  emit the signal to update constraints
2049 // ============================================================================
2050 void SketchSolver_ConstraintGroup::updateRelatedConstraints(
2051     std::shared_ptr<ModelAPI_Attribute> theEntity) const
2052 {
2053   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2054   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2055     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2056         ->attributes(std::string());
2057
2058     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2059     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2060       bool isUpd = (*anAttrIter == theEntity);
2061       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2062           ModelAPI_AttributeRefAttr>(*anAttrIter);
2063       if (aRefAttr && !aRefAttr->isObject() && aRefAttr->attr() == theEntity)
2064         isUpd = true;
2065
2066       if (isUpd) {
2067         static Events_ID anEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
2068         ModelAPI_EventCreator::get()->sendUpdated(aConstrIter->first, anEvent);
2069         break;
2070       }
2071     }
2072   }
2073 }
2074
2075 void SketchSolver_ConstraintGroup::updateRelatedConstraintsFeature(
2076     std::shared_ptr<ModelAPI_Feature> theFeature) const
2077 {
2078   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2079   for (; aConstrIter != myConstraintMap.end(); aConstrIter++) {
2080     std::list<std::shared_ptr<ModelAPI_Attribute> > anAttributes = aConstrIter->first->data()
2081         ->attributes(std::string());
2082
2083     std::list<std::shared_ptr<ModelAPI_Attribute> >::iterator anAttrIter = anAttributes.begin();
2084     for (; anAttrIter != anAttributes.end(); anAttrIter++) {
2085       std::shared_ptr<ModelAPI_AttributeRefAttr> aRefAttr = std::dynamic_pointer_cast<
2086           ModelAPI_AttributeRefAttr>(*anAttrIter);
2087       if (aRefAttr && aRefAttr->isObject() && aRefAttr->object() == theFeature) {
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 // ============================================================================
2097 //  Function: updateFilletConstraints
2098 //  Class:    SketchSolver_ConstraintGroup
2099 //  Purpose:  change fillet arc to be less than 180 degree
2100 // ============================================================================
2101 void SketchSolver_ConstraintGroup::updateFilletConstraints()
2102 {
2103   ConstraintMap::const_iterator aConstrIter = myConstraintMap.begin();
2104   for (; aConstrIter != myConstraintMap.end(); aConstrIter++)
2105     if (aConstrIter->first->getKind() == SketchPlugin_ConstraintFillet::ID()) {
2106       AttributeRefListPtr aFilletRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
2107           aConstrIter->first->data()->attribute(SketchPlugin_ConstraintFillet::ENTITY_C()));
2108       if (!aFilletRefList)
2109         return;
2110       ObjectPtr anArcObj = aFilletRefList->object(2);
2111       std::shared_ptr<GeomDataAPI_Point2D> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2112           anArcObj->data()->attribute(SketchPlugin_Arc::CENTER_ID()));
2113       std::shared_ptr<GeomDataAPI_Point2D> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2114           anArcObj->data()->attribute(SketchPlugin_Arc::START_ID()));
2115       std::shared_ptr<GeomDataAPI_Point2D> aEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
2116           anArcObj->data()->attribute(SketchPlugin_Arc::END_ID()));
2117       double aCosA = aStart->x() - aCenter->x();
2118       double aSinA = aStart->y() - aCenter->y();
2119       double aCosB = aEnd->x() - aCenter->x();
2120       double aSinB = aEnd->y() - aCenter->y();
2121       if (aCosA * aSinB - aSinA * aCosB <= 0.0) {
2122         anArcObj->data()->blockSendAttributeUpdated(true);
2123         double x = aStart->x();
2124         double y = aStart->y();
2125         aStart->setValue(aEnd->x(), aEnd->y());
2126         aEnd->setValue(x, y);
2127         // Update constraint data
2128         changeFilletConstraint(aConstrIter->first);
2129         anArcObj->data()->blockSendAttributeUpdated(false);
2130       }
2131     }
2132 }
2133
2134 // ============================================================================
2135 //  Function: makeMirrorEntity
2136 //  Class:    SketchSolver_ConstraintGroup
2137 //  Purpose:  change entities parameters to make them symmetric relating to the mirror line
2138 // ============================================================================
2139 void SketchSolver_ConstraintGroup::makeMirrorEntity(const Slvs_hEntity& theBase,
2140                                                     const Slvs_hEntity& theMirror,
2141                                                     const Slvs_hEntity& theMirrorLine)
2142 {
2143   Slvs_Entity aBase = myEntities[Search(theBase, myEntities)];
2144   Slvs_Entity aMirror = myEntities[Search(theMirror, myEntities)];
2145   int i = 0;
2146   while (aBase.point[i] != 0 && aMirror.point[i] != 0) {
2147     makeMirrorEntity(aBase.point[i], aMirror.point[i], theMirrorLine);
2148     i++;
2149   }
2150   if (aBase.param[0] != 0 && aMirror.param[0] != 0) { // this is a point, copy it
2151     Slvs_Entity aMirrorLine = myEntities[Search(theMirrorLine, myEntities)];
2152     std::shared_ptr<GeomAPI_XY> aLinePoints[2];
2153     for (i = 0; i < 2; i++) {
2154       Slvs_Entity aPoint = myEntities[Search(aMirrorLine.point[i], myEntities)];
2155       int aParamPos = Search(aPoint.param[0], myParams);
2156       aLinePoints[i] = std::shared_ptr<GeomAPI_XY>(
2157           new GeomAPI_XY(myParams[aParamPos].val, myParams[1+aParamPos].val));
2158     }
2159     int aBaseParamPos = Search(aBase.param[0], myParams);
2160     int aMirrorParamPos = Search(aMirror.param[0], myParams);
2161     std::shared_ptr<GeomAPI_XY> aPoint = std::shared_ptr<GeomAPI_XY>(
2162         new GeomAPI_XY(myParams[aBaseParamPos].val, myParams[1+aBaseParamPos].val));
2163
2164     // direction of a mirror line
2165     std::shared_ptr<GeomAPI_Dir2d> aDir = std::shared_ptr<GeomAPI_Dir2d>(
2166         new GeomAPI_Dir2d(aLinePoints[1]->added(aLinePoints[0]->multiplied(-1.0))));
2167     // orthogonal direction
2168     aDir = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(aDir->y(), -aDir->x()));
2169
2170     std::shared_ptr<GeomAPI_XY> aVec = std::shared_ptr<GeomAPI_XY>(
2171         new GeomAPI_XY(aPoint->x() - aLinePoints[0]->x(), aPoint->y() - aLinePoints[0]->y()));
2172     double aDist = aVec->dot(aDir->xy());
2173     std::shared_ptr<GeomAPI_XY> aMirrorPoint = aPoint->added(aDir->xy()->multiplied(-2.0 * aDist));
2174
2175     myParams[aMirrorParamPos].val = aMirrorPoint->x();
2176     myParams[1+aMirrorParamPos].val = aMirrorPoint->y();
2177   }
2178 }
2179
2180 // ============================================================================
2181 //  Function: changeMirrorPoints
2182 //  Class:    SketchSolver_ConstraintGroup
2183 //  Purpose:  creates/updates mirror constraint for two points
2184 // ============================================================================
2185 Slvs_hConstraint SketchSolver_ConstraintGroup::changeMirrorPoints(
2186     const Slvs_hEntity& theBasePoint,
2187     const Slvs_hEntity& theMirrorPoint,
2188     const Slvs_hEntity& theMirrorLine,
2189     std::vector<Slvs_Constraint>&  thePrevConstr,
2190     std::map<Slvs_hEntity, Slvs_hEntity>& thePrevMirror)
2191 {
2192   std::map<Slvs_hEntity, Slvs_hEntity>::iterator aMapIter = thePrevMirror.find(theBasePoint);
2193   if (aMapIter != thePrevMirror.end()) {
2194     thePrevMirror.erase(aMapIter);
2195     std::vector<Slvs_Constraint>::iterator anIter = thePrevConstr.begin();
2196     for (; anIter != thePrevConstr.end(); anIter++)
2197       if (anIter->ptA == theBasePoint) {
2198         if (anIter->ptB != theMirrorPoint) {
2199           int aConstrInd = Search(anIter->h, myConstraints);
2200           myConstraints[aConstrInd].ptB = theMirrorPoint;
2201           myConstraints[aConstrInd].entityA = theMirrorLine;
2202         }
2203         Slvs_hConstraint aResult = anIter->h;
2204         thePrevConstr.erase(anIter);
2205         return aResult;
2206       }
2207   }
2208
2209   // Newly created constraint
2210   Slvs_Constraint aConstraint = Slvs_MakeConstraint(
2211       ++myConstrMaxID, myID, SLVS_C_SYMMETRIC_LINE, myWorkplane.h, 0.0,
2212       theBasePoint, theMirrorPoint, theMirrorLine, SLVS_E_UNKNOWN);
2213   myConstraints.push_back(aConstraint);
2214   return aConstraint.h;
2215 }
2216
2217
2218 // ============================================================================
2219 //  Function: calculateMiddlePoint
2220 //  Class:    SketchSolver_ConstraintGroup
2221 //  Purpose:  calculates middle point on line or arc
2222 // ============================================================================
2223 void SketchSolver_ConstraintGroup::calculateMiddlePoint(
2224     const Slvs_hEntity& theEntity,
2225     double& theX,
2226     double& theY) const
2227 {
2228   int anInd = Search(theEntity, myEntities);
2229   if (myEntities[anInd].type == SLVS_E_LINE_SEGMENT) {
2230     int aLineParams[2];
2231     for (int i = 0; i < 2; i++) {
2232       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2233       aLineParams[i] = Search(myEntities[aPtPos].param[0], myParams);
2234     }
2235     theX = 0.5 * (myParams[aLineParams[0]].val + myParams[aLineParams[1]].val);
2236     theY = 0.5 * (myParams[1 + aLineParams[0]].val + myParams[1 + aLineParams[1]].val);
2237   } else if (myEntities[anInd].type == SLVS_E_ARC_OF_CIRCLE) {
2238     double anArcPoint[3][2];
2239     for (int i = 0; i < 3; i++) {
2240       int aPtPos = Search(myEntities[anInd].point[i], myEntities);
2241       int anArcParam = Search(myEntities[aPtPos].param[0], myParams);
2242       anArcPoint[i][0] = myParams[anArcParam].val;
2243       anArcPoint[i][1] = myParams[1 + anArcParam].val;
2244     }
2245     // project last point of arc on the arc
2246     double x = anArcPoint[1][0] - anArcPoint[0][0];
2247     double y = anArcPoint[1][1] - anArcPoint[0][1];
2248     double aRad = sqrt(x*x + y*y);
2249     x = anArcPoint[2][0] - anArcPoint[0][0];
2250     y = anArcPoint[2][1] - anArcPoint[0][1];
2251     double aNorm = sqrt(x*x + y*y);
2252     if (aNorm >= tolerance) {
2253       anArcPoint[2][0] = anArcPoint[0][0] + x * aRad / aNorm;
2254       anArcPoint[2][1] = anArcPoint[0][1] + y * aRad / aNorm;
2255     }
2256
2257     x = anArcPoint[1][0] + anArcPoint[2][0] - 2.0 * anArcPoint[0][0];
2258     y = anArcPoint[1][1] + anArcPoint[2][1] - 2.0 * anArcPoint[0][1];
2259     aNorm = sqrt(x*x + y*y);
2260     if (aNorm >= tolerance) {
2261       x *= aRad / aNorm;
2262       y *= aRad / aNorm;
2263     } else { // obtain orthogonal direction
2264       x = 0.5 * (anArcPoint[2][1] - anArcPoint[1][1]);
2265       y = -0.5 * (anArcPoint[2][0] - anArcPoint[1][0]);
2266     }
2267     theX = anArcPoint[0][0] + x;
2268     theY = anArcPoint[0][1] + y;
2269   }
2270 }
2271
2272
2273 // ========================================================
2274 // =========      Auxiliary functions       ===============
2275 // ========================================================
2276
2277 template<typename T>
2278 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
2279 {
2280   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
2281   int aVecSize = theEntities.size();
2282   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
2283     aResIndex--;
2284   while (aResIndex < aVecSize && aResIndex >= 0 && theEntities[aResIndex].h < theEntityID)
2285     aResIndex++;
2286   if (aResIndex == -1)
2287     aResIndex = aVecSize;
2288   return aResIndex;
2289 }