src/SketchPlugin/SketchPlugin_ConstraintFillet.cpp

   1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D -->
   2
   3 // File:    SketchPlugin_ConstraintFillet.cpp
   4 // Created: 19 Mar 2015
   5 // Author:  Artem ZHIDKOV
   6
   7 #include "SketchPlugin_ConstraintFillet.h"
   8
   9 #include <GeomAPI_Circ2d.h>
  10 #include <GeomAPI_Dir2d.h>
  11 #include <GeomAPI_Lin2d.h>
  12 #include <GeomAPI_Pnt2d.h>
  13 #include <GeomAPI_XY.h>
  14 #include <GeomDataAPI_Point2D.h>
  15 #include <ModelAPI_AttributeDouble.h>
  16 #include <ModelAPI_AttributeRefAttrList.h>
  17 #include <ModelAPI_Data.h>
  18 #include <ModelAPI_Events.h>
  19 #include <ModelAPI_Session.h>
  20 #include <ModelAPI_Validator.h>
  21
  22 #include <SketchPlugin_Arc.h>
  23 #include <SketchPlugin_Line.h>
  24 #include <SketchPlugin_Point.h>
  25 #include <SketchPlugin_Sketch.h>
  26 #include <SketchPlugin_ConstraintCoincidence.h>
  27 #include <SketchPlugin_ConstraintTangent.h>
  28 #include <SketchPlugin_ConstraintRadius.h>
  29 #include <SketchPlugin_Tools.h>
  30
  31 #include <Events_Loop.h>
  32
  33 #include <math.h>
  34
  35 const double tolerance = 1.e-7;
  36 const double paramTolerance = 1.e-4;
  37
  38 /// \brief Attract specified point on theNewArc to the attribute of theFeature
  39 static void recalculateAttributes(FeaturePtr theNewArc, const std::string& theNewArcAttribute,
  40   FeaturePtr theFeature, const std::string& theFeatureAttribute);
  41
  42 /// \brief Calculates center of fillet arc and coordinates of tangency points
  43 static void calculateFilletCenter(FeaturePtr theFeatureA, FeaturePtr theFeatureB,
  44                                   double theRadius, bool theNotInversed[2],
  45                                   std::shared_ptr<GeomAPI_XY>& theCenter,
  46                                   std::shared_ptr<GeomAPI_XY>& theTangentA,
  47                                   std::shared_ptr<GeomAPI_XY>& theTangentB);
  48
  49 /// Get point on 1/3 length of edge from fillet point
  50 static void getPointOnEdge(const FeaturePtr theFeature,
  51                            const std::shared_ptr<GeomAPI_Pnt2d> theFilletPoint,
  52                            std::shared_ptr<GeomAPI_Pnt2d>& thePoint);
  53
  54 /// Get distance from point to feature
  55 static double getProjectionDistance(const FeaturePtr theFeature,
  56                              const std::shared_ptr<GeomAPI_Pnt2d> thePoint);
  57
  58 /// Get coincide edges for fillet
  59 static std::set<FeaturePtr> getCoincides(const FeaturePtr& theConstraintCoincidence);
  60
  61 SketchPlugin_ConstraintFillet::SketchPlugin_ConstraintFillet()
  62 : myListOfPointsChangedInCode(false),
  63   myRadiusChangedByUser(false),
  64   myRadiusChangedInCode(false),
  65   myRadiusInitialized(false)
  66 {
  67 }
  68
  69 void SketchPlugin_ConstraintFillet::initAttributes()
  70 {
  71   data()->addAttribute(SketchPlugin_Constraint::VALUE(), ModelAPI_AttributeDouble::typeId());
  72   data()->addAttribute(SketchPlugin_Constraint::ENTITY_A(), ModelAPI_AttributeRefAttrList::typeId());
  73 }
  74
  75 void SketchPlugin_ConstraintFillet::execute()
  76 {
  77   std::shared_ptr<ModelAPI_Data> aData = data();
  78
  79   // Check the base objects are initialized.
  80   AttributeRefAttrListPtr aPointsRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttrList>(
  81     aData->attribute(SketchPlugin_Constraint::ENTITY_A()));
  82   if(!aPointsRefList->isInitialized()) {
  83     setError("Error: List of points is not initialized.");
  84     return;
  85   }
  86
  87   // Get fillet radius.
  88   double aFilletRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
  89     aData->attribute(SketchPlugin_Constraint::VALUE()))->value();
  90
  91   // Wait all constraints being created, then send update events
  92   static Events_ID anUpdateEvent = Events_Loop::eventByName(EVENT_OBJECT_UPDATED);
  93   bool isUpdateFlushed = Events_Loop::loop()->isFlushed(anUpdateEvent);
  94   if (isUpdateFlushed)
  95     Events_Loop::loop()->setFlushed(anUpdateEvent, false);
  96
  97   for(std::set<AttributePtr>::iterator aPointsIter = myNewPoints.begin();
  98       aPointsIter != myNewPoints.end();
  99       ++aPointsIter) {
 100     AttributePtr aPointAttr = *aPointsIter;
 101     std::shared_ptr<GeomDataAPI_Point2D> aFilletPoint2d = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(aPointAttr);
 102     if(!aFilletPoint2d.get()) {
 103       setError("Error: One of the selected points is empty.");
 104       return;
 105     }
 106     std::shared_ptr<GeomAPI_Pnt2d> aFilletPnt2d = aFilletPoint2d->pnt();
 107
 108     // Obtain base lines for fillet.
 109     bool anIsNeedNewObjects = true;
 110     FilletFeatures aFilletFeatures;
 111     std::map<AttributePtr, FilletFeatures>::iterator aPrevPointsIter = myPointFeaturesMap.find(aPointAttr);
 112     if(aPrevPointsIter != myPointFeaturesMap.end()) {
 113       anIsNeedNewObjects = false;
 114       aFilletFeatures = aPrevPointsIter->second;
 115     }
 116     FeaturePtr aBaseEdgeA, aBaseEdgeB;
 117     if(!anIsNeedNewObjects) {
 118       aBaseEdgeA = aFilletFeatures.baseEdgesState.front().first;
 119       aBaseEdgeB = aFilletFeatures.baseEdgesState.back().first;
 120     } else {
 121       // Obtain constraint coincidence for the fillet point.
 122       FeaturePtr aConstraintCoincidence;
 123       const std::set<AttributePtr>& aRefsList = aFilletPoint2d->owner()->data()->refsToMe();
 124       for(std::set<AttributePtr>::const_iterator anIt = aRefsList.cbegin(); anIt != aRefsList.cend(); ++anIt) {
 125         std::shared_ptr<ModelAPI_Attribute> anAttr = (*anIt);
 126         FeaturePtr aConstrFeature = std::dynamic_pointer_cast<ModelAPI_Feature>(anAttr->owner());
 127         if(aConstrFeature->getKind() == SketchPlugin_ConstraintCoincidence::ID()) {
 128           AttributeRefAttrPtr anAttrRefA = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 129             aConstrFeature->attribute(SketchPlugin_ConstraintCoincidence::ENTITY_A()));
 130           AttributeRefAttrPtr anAttrRefB = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 131             aConstrFeature->attribute(SketchPlugin_ConstraintCoincidence::ENTITY_B()));
 132           if(anAttrRefA.get() && !anAttrRefA->isObject()) {
 133             AttributePtr anAttrA = anAttrRefA->attr();
 134             if(aFilletPoint2d == anAttrA) {
 135               aConstraintCoincidence = aConstrFeature;
 136               break;
 137             }
 138           }
 139           if(anAttrRefB.get() && !anAttrRefB->isObject()) {
 140             AttributePtr anAttrB = anAttrRefB->attr();
 141             if(aFilletPoint2d == anAttrB) {
 142               aConstraintCoincidence = aConstrFeature;
 143               break;
 144             }
 145           }
 146         }
 147       }
 148
 149       if(!aConstraintCoincidence.get()) {
 150         setError("Error: No coincident edges at one of the selected points.");
 151         return;
 152       }
 153
 154       // Get coincide edges.
 155       std::set<FeaturePtr> aCoincides = getCoincides(aConstraintCoincidence);
 156       if(aCoincides.size() != 2) {
 157         setError("Error: One of the selected points does not have two suitable edges for fillet.");
 158         return;
 159       }
 160
 161       std::set<FeaturePtr>::iterator aLinesIt = aCoincides.begin();
 162       aBaseEdgeA = *aLinesIt++;
 163       aBaseEdgeB = *aLinesIt;
 164
 165       std::pair<FeaturePtr, bool> aBasePairA = std::make_pair(aBaseEdgeA, aBaseEdgeA->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->value());
 166       std::pair<FeaturePtr, bool> aBasePairB = std::make_pair(aBaseEdgeB, aBaseEdgeB->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->value());
 167       aFilletFeatures.baseEdgesState.push_back(aBasePairA);
 168       aFilletFeatures.baseEdgesState.push_back(aBasePairB);
 169     }
 170
 171     if(!aBaseEdgeA.get() || !aBaseEdgeB.get()) {
 172       setError("Error: One of the base edges is empty.");
 173       return;
 174     }
 175
 176     // Create new edges and arc if needed.
 177     FeaturePtr aResultEdgeA, aResultEdgeB, aResultArc;
 178     if(!anIsNeedNewObjects) {
 179       // Obtain features from the list.
 180       std::list<FeaturePtr>::iterator aResultEdgesIt = aFilletFeatures.resultEdges.begin();
 181       aResultEdgeA = *aResultEdgesIt++;
 182       aResultEdgeB = *aResultEdgesIt++;
 183       aResultArc = *aResultEdgesIt;
 184     } else {
 185       // Copy edges and create arc.
 186       aResultEdgeA = SketchPlugin_Sketch::addUniqueNamedCopiedFeature(aBaseEdgeA, sketch());
 187       aResultEdgeA->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->setValue(false);
 188       aResultEdgeB = SketchPlugin_Sketch::addUniqueNamedCopiedFeature(aBaseEdgeB, sketch());
 189       aResultEdgeB->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->setValue(false);
 190       aResultArc = sketch()->addFeature(SketchPlugin_Arc::ID());
 191
 192       aFilletFeatures.resultEdges.push_back(aResultEdgeA);
 193       aFilletFeatures.resultEdges.push_back(aResultEdgeB);
 194       aFilletFeatures.resultEdges.push_back(aResultArc);
 195     }
 196
 197     // Calculate arc attributes
 198     static const int aNbFeatures = 2;
 199     FeaturePtr aBaseFeatures[aNbFeatures] = {aBaseEdgeA, aBaseEdgeB};
 200     FeaturePtr aResultFeatures[aNbFeatures] = {aResultEdgeA, aResultEdgeB};
 201     std::shared_ptr<GeomAPI_Dir2d> aTangentDir[aNbFeatures]; // tangent directions of the features in coincident point
 202     bool isStart[aNbFeatures]; // indicates which point the features share
 203     std::shared_ptr<GeomAPI_Pnt2d> aStartEndPnt[aNbFeatures * 2]; // first pair of points relate to first feature, second pair -  to second
 204     std::string aFeatAttributes[aNbFeatures * 2]; // attributes of features
 205     for (int i = 0; i < aNbFeatures; i++) {
 206       std::string aStartAttr, aEndAttr;
 207       if (aResultFeatures[i]->getKind() == SketchPlugin_Line::ID()) {
 208         aStartAttr = SketchPlugin_Line::START_ID();
 209         aEndAttr = SketchPlugin_Line::END_ID();
 210       } else if (aResultFeatures[i]->getKind() == SketchPlugin_Arc::ID()) {
 211         aStartAttr = SketchPlugin_Arc::START_ID();
 212         aEndAttr = SketchPlugin_Arc::END_ID();
 213       } else { // wrong argument
 214         setError("Error: One of the points has wrong coincide feature");
 215         return;
 216       }
 217       aFeatAttributes[2*i] = aStartAttr;
 218       aStartEndPnt[2*i] = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 219         aBaseFeatures[i]->attribute(aStartAttr))->pnt();
 220       aFeatAttributes[2*i+1] = aEndAttr;
 221       aStartEndPnt[2*i+1] = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 222         aBaseFeatures[i]->attribute(aEndAttr))->pnt();
 223     }
 224     for (int aFeatInd = 0; aFeatInd < aNbFeatures; aFeatInd++) {
 225       for (int j = 0; j < 2; j++) // loop on start-end of each feature
 226         if (aStartEndPnt[aFeatInd * aNbFeatures + j]->distance(aFilletPnt2d) < 1.e-10) {
 227           isStart[aFeatInd] = (j==0);
 228           break;
 229         }
 230     }
 231     // tangent directions of the features
 232     for (int i = 0; i < aNbFeatures; i++) {
 233       std::shared_ptr<GeomAPI_XY> aDir;
 234       if (aResultFeatures[i]->getKind() == SketchPlugin_Line::ID()) {
 235         aDir = aStartEndPnt[2*i+1]->xy()->decreased(aStartEndPnt[2*i]->xy());
 236         if (!isStart[i])
 237           aDir = aDir->multiplied(-1.0);
 238       } else if (aResultFeatures[i]->getKind() == SketchPlugin_Arc::ID()) {
 239         std::shared_ptr<GeomAPI_Pnt2d> aCenterPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 240           aResultFeatures[i]->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
 241         aDir = isStart[i] ? aStartEndPnt[2*i]->xy() : aStartEndPnt[2*i+1]->xy();
 242         aDir = aDir->decreased(aCenterPoint->xy());
 243
 244         double x = aDir->x();
 245         double y = aDir->y();
 246         aDir->setX(-y);
 247         aDir->setY(x);
 248         if (isStart[i] == std::dynamic_pointer_cast<SketchPlugin_Arc>(aBaseFeatures[i])->isReversed())
 249           aDir = aDir->multiplied(-1.0);
 250       }
 251       aTangentDir[i] = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(aDir));
 252     }
 253
 254     // By default, the start point of fillet arc is connected to FeatureA,
 255     // and the end point - to FeatureB. But when the angle between TangentDirA and
 256     // TangentDirB greater 180 degree, the sequaence of features need to be reversed.
 257     double cosBA = aTangentDir[0]->cross(aTangentDir[1]); // cos(B-A), where A and B - angles between corresponding tanget direction and the X axis
 258     bool isReversed = cosBA > 0.0;
 259
 260     // Calculate fillet arc parameters
 261     std::shared_ptr<GeomAPI_XY> aCenter, aTangentPntA, aTangentPntB;
 262     calculateFilletCenter(aBaseEdgeA, aBaseEdgeB, aFilletRadius, isStart, aCenter, aTangentPntA, aTangentPntB);
 263     if(!aCenter.get() || !aTangentPntA.get() || !aTangentPntB.get()) {
 264       setError("Can not create fillet with the specified parameters.");
 265       return;
 266     }
 267     // update features
 268     std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 269       aResultEdgeA->attribute(aFeatAttributes[isStart[0] ? 0 : 1]))->setValue(aTangentPntA->x(), aTangentPntA->y());
 270     aResultEdgeA->execute();
 271     std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 272       aResultEdgeB->attribute(aFeatAttributes[2 + (isStart[1] ? 0 : 1)]))->setValue(aTangentPntB->x(), aTangentPntB->y());
 273     aResultEdgeB->execute();
 274     // update fillet arc: make the arc correct for sure, so, it is not needed to process the "attribute updated"
 275     // by arc; moreover, it may cause cyclicity in hte mechanism of updater
 276     aResultArc->data()->blockSendAttributeUpdated(true);
 277     std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 278       aResultArc->attribute(SketchPlugin_Arc::CENTER_ID()))->setValue(aCenter->x(), aCenter->y());
 279     if(isReversed) {
 280       std::shared_ptr<GeomAPI_XY> aTmp = aTangentPntA;
 281       aTangentPntA = aTangentPntB;
 282       aTangentPntB = aTmp;
 283     }
 284     std::shared_ptr<GeomDataAPI_Point2D> aStartPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 285       aResultArc->attribute(SketchPlugin_Arc::START_ID()));
 286     std::shared_ptr<GeomDataAPI_Point2D> aEndPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 287       aResultArc->attribute(SketchPlugin_Arc::END_ID()));
 288     if(aStartPoint->isInitialized() && aEndPoint->isInitialized() &&
 289       (aStartPoint->pnt()->xy()->distance(aTangentPntA) > tolerance ||
 290        aEndPoint->pnt()->xy()->distance(aTangentPntB) > tolerance)) {
 291       std::dynamic_pointer_cast<SketchPlugin_Arc>(aResultArc)->setReversed(false);
 292     }
 293     aStartPoint->setValue(aTangentPntA->x(), aTangentPntA->y());
 294     aEndPoint->setValue(aTangentPntB->x(), aTangentPntB->y());
 295     aResultArc->data()->blockSendAttributeUpdated(false);
 296     aResultArc->execute();
 297
 298     if(anIsNeedNewObjects) {
 299       // Create list of additional constraints:
 300       // 1. Coincidence of boundary points of features (copied lines/arcs) and fillet arc
 301       // 1.1. coincidence
 302       FeaturePtr aConstraint = sketch()->addFeature(SketchPlugin_ConstraintCoincidence::ID());
 303       AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 304           aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 305       aRefAttr->setAttr(aResultArc->attribute(SketchPlugin_Arc::START_ID()));
 306       aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 307           aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 308       int aFeatInd = isReversed ? 1 : 0;
 309       int anAttrInd = (isReversed ? 2 : 0) + (isStart[isReversed ? 1 : 0] ? 0 : 1);
 310       aRefAttr->setAttr(aResultFeatures[aFeatInd]->attribute(aFeatAttributes[anAttrInd]));
 311       recalculateAttributes(aResultArc, SketchPlugin_Arc::START_ID(), aResultFeatures[aFeatInd], aFeatAttributes[anAttrInd]);
 312       aConstraint->execute();
 313       aFilletFeatures.resultConstraints.push_back(aConstraint);
 314       ModelAPI_EventCreator::get()->sendUpdated(aConstraint, anUpdateEvent);
 315       // 1.2. coincidence
 316       aConstraint = sketch()->addFeature(SketchPlugin_ConstraintCoincidence::ID());
 317       aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 318           aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 319       aRefAttr->setAttr(aResultArc->attribute(SketchPlugin_Arc::END_ID()));
 320       aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 321           aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 322       aFeatInd = isReversed ? 0 : 1;
 323       anAttrInd = (isReversed ? 0 : 2) + (isStart[isReversed ? 0 : 1] ? 0 : 1);
 324       aRefAttr->setAttr(aResultFeatures[aFeatInd]->attribute(aFeatAttributes[anAttrInd]));
 325       recalculateAttributes(aResultArc, SketchPlugin_Arc::END_ID(), aResultFeatures[aFeatInd], aFeatAttributes[anAttrInd]);
 326       aConstraint->execute();
 327       aFilletFeatures.resultConstraints.push_back(aConstraint);
 328       ModelAPI_EventCreator::get()->sendUpdated(aConstraint, anUpdateEvent);
 329       // 2. Fillet arc radius
 330       //aConstraint = sketch()->addFeature(SketchPlugin_ConstraintRadius::ID());
 331       //aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 332       //    aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 333       //aRefAttr->setObject(aNewArc->lastResult());
 334       //std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 335       //    aConstraint->attribute(SketchPlugin_Constraint::VALUE()))->setValue(aFilletRadius);
 336       //std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 337       //    aConstraint->attribute(SketchPlugin_Constraint::FLYOUT_VALUE_PNT()))->setValue(
 338       //    isStart[0] ? aStartEndPnt[0] : aStartEndPnt[1]);
 339       //aConstraint->execute();
 340       //myProducedFeatures.push_back(aConstraint);
 341       //ModelAPI_EventCreator::get()->sendUpdated(aConstraint, anUpdateEvent);
 342       // 3. Tangency of fillet arc and features
 343       for (int i = 0; i < aNbFeatures; i++) {
 344         aConstraint = sketch()->addFeature(SketchPlugin_ConstraintTangent::ID());
 345         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 346             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 347         aRefAttr->setObject(aResultArc->lastResult());
 348         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 349             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 350         bool isArc = aResultFeatures[i]->getKind() == SketchPlugin_Arc::ID();
 351         aRefAttr->setObject(isArc ? aResultFeatures[i]->lastResult() : aResultFeatures[i]->firstResult());
 352         aConstraint->execute();
 353         aFilletFeatures.resultConstraints.push_back(aConstraint);
 354         ModelAPI_EventCreator::get()->sendUpdated(aConstraint, anUpdateEvent);
 355       }
 356       // 4. Coincidence of free boundaries of base and copied features
 357       for (int i = 0; i < aNbFeatures; i++) {
 358         anAttrInd = 2*i + (isStart[i] ? 1 : 0);
 359         aConstraint = sketch()->addFeature(SketchPlugin_ConstraintCoincidence::ID());
 360         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 361             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 362         aRefAttr->setAttr(aBaseFeatures[i]->attribute(aFeatAttributes[anAttrInd]));
 363         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 364             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 365         aRefAttr->setAttr(aResultFeatures[i]->attribute(aFeatAttributes[anAttrInd]));
 366         aFilletFeatures.resultConstraints.push_back(aConstraint);
 367       }
 368       // 4.1. Additional tangency constraints when the fillet is based on arcs.
 369       //      It is used to verify the created arc will be placed on a source.
 370       for (int i = 0; i < aNbFeatures; ++i) {
 371         if (aResultFeatures[i]->getKind() != SketchPlugin_Arc::ID())
 372           continue;
 373         aConstraint = sketch()->addFeature(SketchPlugin_ConstraintTangent::ID());
 374         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 375             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 376         aRefAttr->setObject(aBaseFeatures[i]->lastResult());
 377         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 378             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 379         aRefAttr->setObject(aResultFeatures[i]->lastResult());
 380         aConstraint->execute();
 381         aFilletFeatures.resultConstraints.push_back(aConstraint);
 382         ModelAPI_EventCreator::get()->sendUpdated(aConstraint, anUpdateEvent);
 383       }
 384       // 5. Tangent points should be placed on the base features
 385       for (int i = 0; i < aNbFeatures; i++) {
 386         anAttrInd = 2*i + (isStart[i] ? 0 : 1);
 387         aConstraint = sketch()->addFeature(SketchPlugin_ConstraintCoincidence::ID());
 388         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 389             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 390         aRefAttr->setAttr(aResultFeatures[i]->attribute(aFeatAttributes[anAttrInd]));
 391         aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 392             aConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
 393         aRefAttr->setObject(aBaseFeatures[i]->lastResult());
 394         aFilletFeatures.resultConstraints.push_back(aConstraint);
 395       }
 396       // make base features auxiliary
 397       aBaseEdgeA->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->setValue(true);
 398       aBaseEdgeB->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->setValue(true);
 399
 400       // exchange the naming IDs of newly created and old line that become auxiliary
 401       sketch()->exchangeIDs(aBaseEdgeA, aResultEdgeA);
 402       sketch()->exchangeIDs(aBaseEdgeB, aResultEdgeB);
 403
 404       // store point and features in the map.
 405       myPointFeaturesMap[aPointAttr] = aFilletFeatures;
 406     } else {
 407       // Update radius value
 408       int aNbSubs = sketch()->numberOfSubs();
 409       FeaturePtr aSubFeature;
 410       for (int aSub = 0; aSub < aNbSubs; aSub++) {
 411         aSubFeature = sketch()->subFeature(aSub);
 412         if (aSubFeature->getKind() != SketchPlugin_ConstraintRadius::ID())
 413           continue;
 414         AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 415             aSubFeature->attribute(SketchPlugin_Constraint::ENTITY_A()));
 416         if (!aRefAttr || !aRefAttr->isObject())
 417           continue;
 418         FeaturePtr aFeature = ModelAPI_Feature::feature(aRefAttr->object());
 419         if (aFeature == aResultArc) {
 420           AttributeDoublePtr aRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 421             aSubFeature->attribute(SketchPlugin_Constraint::VALUE()));
 422           aRadius->setValue(aFilletRadius);
 423           break;
 424         }
 425       }
 426     }
 427   }
 428
 429   // Send events to update the sub-features by the solver.
 430   if(isUpdateFlushed) {
 431     Events_Loop::loop()->setFlushed(anUpdateEvent, true);
 432   }
 433 }
 434
 435 void SketchPlugin_ConstraintFillet::attributeChanged(const std::string& theID)
 436 {
 437   if(theID == SketchPlugin_Constraint::ENTITY_A()) {
 438     if(myListOfPointsChangedInCode) {
 439       return;
 440     }
 441
 442     // Clear results.
 443     clearResults();
 444
 445     // Clear list of new points.
 446     myNewPoints.clear();
 447
 448     // Get list of points for fillets and current radius.
 449     AttributeRefAttrListPtr aRefListOfFilletPoints = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttrList>(
 450       data()->attribute(SketchPlugin_Constraint::ENTITY_A()));
 451     AttributeDoublePtr aRadiusAttribute = real(VALUE());
 452     int aListSize = aRefListOfFilletPoints->size();
 453     if(aListSize == 0 && !myRadiusChangedByUser) {
 454       // If list is empty reset radius to zero (if it was not changed by user).
 455       myRadiusChangedInCode = true;
 456       aRadiusAttribute->setValue(0);
 457       myRadiusChangedInCode = false;
 458       return;
 459     }
 460
 461     // Iterate over points to get base lines an calculate radius for fillets.
 462     double aMinimumRadius = 0;
 463     std::list<std::pair<ObjectPtr, AttributePtr>> aSelectedPointsList = aRefListOfFilletPoints->list();
 464     std::list<std::pair<ObjectPtr, AttributePtr>>::iterator anIter = aSelectedPointsList.begin();
 465     std::set<AttributePtr> aPointsToSkeep;
 466     for(int anIndex = 0; anIndex < aListSize; anIndex++, anIter++) {
 467       AttributePtr aFilletPointAttr = (*anIter).second;
 468       std::shared_ptr<GeomDataAPI_Point2D> aFilletPoint2D =
 469         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(aFilletPointAttr);
 470       if(!aFilletPoint2D.get()) {
 471         myNewPoints.clear();
 472         setError("Error: One of the selected points is invalid.");
 473         return;
 474       }
 475
 476       // If point or coincident point is already in list remove it from attribute.
 477       if(aPointsToSkeep.find(aFilletPointAttr) != aPointsToSkeep.end()) {
 478         myListOfPointsChangedInCode = true;
 479         aRefListOfFilletPoints->remove(aFilletPointAttr);
 480         myListOfPointsChangedInCode = false;
 481         continue;
 482       }
 483
 484       // Obtain constraint coincidence for the fillet point.
 485       FeaturePtr aConstraintCoincidence;
 486       const std::set<AttributePtr>& aRefsList = aFilletPointAttr->owner()->data()->refsToMe();
 487       for(std::set<AttributePtr>::const_iterator anIt = aRefsList.cbegin(); anIt != aRefsList.cend(); ++anIt) {
 488         std::shared_ptr<ModelAPI_Attribute> anAttr = (*anIt);
 489         FeaturePtr aConstrFeature = std::dynamic_pointer_cast<ModelAPI_Feature>(anAttr->owner());
 490         if(aConstrFeature->getKind() == SketchPlugin_ConstraintCoincidence::ID()) {
 491           AttributeRefAttrPtr anAttrRefA = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 492             aConstrFeature->attribute(SketchPlugin_ConstraintCoincidence::ENTITY_A()));
 493           AttributeRefAttrPtr anAttrRefB = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 494             aConstrFeature->attribute(SketchPlugin_ConstraintCoincidence::ENTITY_B()));
 495           if(anAttrRefA.get()) {
 496             AttributePtr anAttrA = anAttrRefA->attr();
 497             if(aFilletPointAttr == anAttrA) {
 498               aConstraintCoincidence = aConstrFeature;
 499               break;
 500             }
 501           }
 502           if(anAttrRefB.get()) {
 503             AttributePtr anAttrB = anAttrRefB->attr();
 504             if(aFilletPointAttr == anAttrB) {
 505               aConstraintCoincidence = aConstrFeature;
 506               break;
 507             }
 508           }
 509         }
 510       }
 511
 512       if(!aConstraintCoincidence.get()) {
 513         myNewPoints.clear();
 514         setError("Error: No coincident edges at one of the selected points.");
 515         return;
 516       }
 517
 518       // Get coincides from constraint.
 519       std::set<FeaturePtr> aCoincides;
 520
 521
 522       SketchPlugin_Tools::findCoincidences(aConstraintCoincidence,
 523                                            SketchPlugin_ConstraintCoincidence::ENTITY_A(),
 524                                            aCoincides);
 525       SketchPlugin_Tools::findCoincidences(aConstraintCoincidence,
 526                                            SketchPlugin_ConstraintCoincidence::ENTITY_B(),
 527                                            aCoincides);
 528
 529       // Remove points from set of coincides. Also get all attributes which is equal to this point to exclude it.
 530       std::shared_ptr<GeomAPI_Pnt2d> aFilletPnt2d = aFilletPoint2D->pnt();
 531       std::set<FeaturePtr> aNewSetOfCoincides;
 532       for(std::set<FeaturePtr>::iterator anIt = aCoincides.begin(); anIt != aCoincides.end(); ++anIt) {
 533         std::string aFeatureKind = (*anIt)->getKind();
 534         if(aFeatureKind == SketchPlugin_Point::ID()) {
 535           AttributePtr anAttr = (*anIt)->attribute(SketchPlugin_Point::COORD_ID());
 536           std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 537             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttr);
 538           if(aPoint2D.get() && aFilletPnt2d->isEqual(aPoint2D->pnt())) {
 539             aPointsToSkeep.insert(anAttr);
 540           }
 541         } else if(aFeatureKind == SketchPlugin_Line::ID()) {
 542           AttributePtr anAttrStart = (*anIt)->attribute(SketchPlugin_Line::START_ID());
 543           std::shared_ptr<GeomDataAPI_Point2D> aPointStart2D =
 544             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrStart);
 545           if(aPointStart2D.get() && aFilletPnt2d->isEqual(aPointStart2D->pnt())) {
 546             aPointsToSkeep.insert(anAttrStart);
 547           }
 548           AttributePtr anAttrEnd = (*anIt)->attribute(SketchPlugin_Line::END_ID());
 549           std::shared_ptr<GeomDataAPI_Point2D> aPointEnd2D =
 550             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrEnd);
 551           if(aPointEnd2D.get() && aFilletPnt2d->isEqual(aPointEnd2D->pnt())) {
 552             aPointsToSkeep.insert(anAttrEnd);
 553           }
 554           aNewSetOfCoincides.insert(*anIt);
 555         } else if(aFeatureKind == SketchPlugin_Arc::ID() ) {
 556           AttributePtr anAttrCenter = (*anIt)->attribute(SketchPlugin_Arc::CENTER_ID());
 557           std::shared_ptr<GeomDataAPI_Point2D> aPointCenter2D =
 558             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrCenter);
 559           if(aPointCenter2D.get() && aFilletPnt2d->isEqual(aPointCenter2D->pnt())) {
 560             aPointsToSkeep.insert(anAttrCenter);
 561             continue;
 562           }
 563           AttributePtr anAttrStart = (*anIt)->attribute(SketchPlugin_Arc::START_ID());
 564           std::shared_ptr<GeomDataAPI_Point2D> aPointStart2D =
 565             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrStart);
 566           if(aPointStart2D.get() && aFilletPnt2d->isEqual(aPointStart2D->pnt())) {
 567             aPointsToSkeep.insert(anAttrStart);
 568           }
 569           AttributePtr anAttrEnd = (*anIt)->attribute(SketchPlugin_Arc::END_ID());
 570           std::shared_ptr<GeomDataAPI_Point2D> aPointEnd2D =
 571             std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrEnd);
 572           if(aPointEnd2D.get() && aFilletPnt2d->isEqual(aPointEnd2D->pnt())) {
 573             aPointsToSkeep.insert(anAttrEnd);
 574           }
 575           aNewSetOfCoincides.insert(*anIt);
 576         }
 577       }
 578       aCoincides = aNewSetOfCoincides;
 579
 580       // If we still have more than two coincides remove auxilary entities from set of coincides.
 581       if(aCoincides.size() > 2) {
 582         aNewSetOfCoincides.clear();
 583         for(std::set<FeaturePtr>::iterator anIt = aCoincides.begin(); anIt != aCoincides.end(); ++anIt) {
 584           if(!(*anIt)->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->value()) {
 585             aNewSetOfCoincides.insert(*anIt);
 586           }
 587         }
 588         aCoincides = aNewSetOfCoincides;
 589       }
 590
 591       if(aCoincides.size() != 2) {
 592         myNewPoints.clear();
 593         setError("Error: One of the selected points does not have two suitable edges for fillet.");
 594         return;
 595       }
 596
 597       // Store base point for fillet.
 598       aPointsToSkeep.insert(aFilletPointAttr);
 599       myNewPoints.insert(aFilletPointAttr);
 600
 601       // Get base lines for fillet.
 602       FeaturePtr anOldFeatureA, anOldFeatureB;
 603       std::set<FeaturePtr>::iterator aLinesIt = aCoincides.begin();
 604       anOldFeatureA = *aLinesIt++;
 605       anOldFeatureB = *aLinesIt;
 606
 607       // Getting radius value if it was not changed by user.
 608       if(!myRadiusChangedByUser) {
 609         // Getting points located at 1/3 of edge length from fillet point.
 610         std::shared_ptr<GeomAPI_Pnt2d> aFilletPnt2d = aFilletPoint2D->pnt();
 611         std::shared_ptr<GeomAPI_Pnt2d> aPntA, aPntB;
 612         getPointOnEdge(anOldFeatureA, aFilletPnt2d, aPntA);
 613         getPointOnEdge(anOldFeatureB, aFilletPnt2d, aPntB);
 614
 615         /// Getting distances.
 616         double aDistanceA = getProjectionDistance(anOldFeatureB, aPntA);
 617         double aDistanceB = getProjectionDistance(anOldFeatureA, aPntB);
 618         double aRadius = aDistanceA < aDistanceB ? aDistanceA / 2.0 : aDistanceB / 2.0;
 619         aMinimumRadius = aMinimumRadius == 0 ? aRadius : aRadius < aMinimumRadius ? aRadius : aMinimumRadius;
 620       }
 621     }
 622
 623     // Set new default radius if it was not changed by user.
 624     if(!myRadiusChangedByUser) {
 625       myRadiusChangedInCode = true;
 626       aRadiusAttribute->setValue(aMinimumRadius);
 627       myRadiusChangedInCode = false;
 628     }
 629
 630   } else if(theID == SketchPlugin_Constraint::VALUE()) {
 631     if(myRadiusInitialized && !myRadiusChangedInCode) {
 632       myRadiusChangedByUser = true;
 633     }
 634     if(!myRadiusInitialized) {
 635       myRadiusInitialized = true;
 636     }
 637   }
 638 }
 639
 640 AISObjectPtr SketchPlugin_ConstraintFillet::getAISObject(AISObjectPtr thePrevious)
 641 {
 642   if (!sketch())
 643     return thePrevious;
 644
 645   AISObjectPtr anAIS = thePrevious;
 646   /// TODO: Equal constraint presentation should be put here
 647   return anAIS;
 648 }
 649
 650 bool SketchPlugin_ConstraintFillet::isMacro() const
 651 {
 652   return true;
 653 }
 654
 655 void SketchPlugin_ConstraintFillet::clearResults()
 656 {
 657   // Clear auxiliary flag on initial objects.
 658   for(std::map<AttributePtr, FilletFeatures>::iterator aPointsIter = myPointFeaturesMap.begin();
 659       aPointsIter != myPointFeaturesMap.end();) {
 660     const FilletFeatures& aFilletFeatures = aPointsIter->second;
 661     std::list<std::pair<FeaturePtr, bool>>::const_iterator aFeatureIt;
 662     for(aFeatureIt = aFilletFeatures.baseEdgesState.cbegin();
 663         aFeatureIt != aFilletFeatures.baseEdgesState.cend();
 664         ++aFeatureIt) {
 665       aFeatureIt->first->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->setValue(aFeatureIt->second);
 666     }
 667     ++aPointsIter;
 668   }
 669
 670   // And remove all produced features.
 671   DocumentPtr aDoc = sketch()->document();
 672   for(std::map<AttributePtr, FilletFeatures>::iterator aPointsIter = myPointFeaturesMap.begin();
 673       aPointsIter != myPointFeaturesMap.end();) {
 674     // Remove all produced constraints.
 675     const FilletFeatures& aFilletFeatures = aPointsIter->second;
 676     std::list<FeaturePtr>::const_iterator aFeatureIt;
 677     for(aFeatureIt = aFilletFeatures.resultConstraints.cbegin();
 678         aFeatureIt != aFilletFeatures.resultConstraints.cend();
 679         ++aFeatureIt) {
 680       aDoc->removeFeature(*aFeatureIt);
 681     }
 682
 683     // Remove all result edges.
 684     for(aFeatureIt = aFilletFeatures.resultEdges.cbegin();
 685         aFeatureIt != aFilletFeatures.resultEdges.cend();
 686         ++aFeatureIt) {
 687       aDoc->removeFeature(*aFeatureIt);
 688     }
 689
 690     // Remove point from map.
 691     myPointFeaturesMap.erase(aPointsIter++);
 692   }
 693 };
 694
 695
 696 // =========   Auxiliary functions   =================
 697 void recalculateAttributes(FeaturePtr theNewArc,  const std::string& theNewArcAttribute,
 698                            FeaturePtr theFeature, const std::string& theFeatureAttribute)
 699 {
 700   std::shared_ptr<GeomAPI_Pnt2d> anArcPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 701       theNewArc->attribute(theNewArcAttribute))->pnt();
 702   std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 703       theFeature->attribute(theFeatureAttribute))->setValue(anArcPoint->x(), anArcPoint->y());
 704 }
 705
 706 /// \brief Find intersections of lines shifted along normal direction
 707 void possibleFilletCenterLineLine(
 708     std::shared_ptr<GeomAPI_XY> thePointA, std::shared_ptr<GeomAPI_Dir2d> theDirA,
 709     std::shared_ptr<GeomAPI_XY> thePointB, std::shared_ptr<GeomAPI_Dir2d> theDirB,
 710     double theRadius, std::list< std::shared_ptr<GeomAPI_XY> >& theCenters)
 711 {
 712   std::shared_ptr<GeomAPI_Dir2d> aDirAT(new GeomAPI_Dir2d(-theDirA->y(), theDirA->x()));
 713   std::shared_ptr<GeomAPI_Dir2d> aDirBT(new GeomAPI_Dir2d(-theDirB->y(), theDirB->x()));
 714   std::shared_ptr<GeomAPI_XY> aPntA, aPntB;
 715   double aDet = theDirA->cross(theDirB);
 716   for (double aStepA = -1.0; aStepA <= 1.0; aStepA += 2.0) {
 717     aPntA = thePointA->added(aDirAT->xy()->multiplied(aStepA * theRadius));
 718     for (double aStepB = -1.0; aStepB <= 1.0; aStepB += 2.0) {
 719       aPntB = thePointB->added(aDirBT->xy()->multiplied(aStepB * theRadius));
 720       double aVX = aDirAT->xy()->dot(aPntA);
 721       double aVY = aDirBT->xy()->dot(aPntB);
 722       std::shared_ptr<GeomAPI_XY> aPoint(new GeomAPI_XY(
 723           (theDirB->x() * aVX - theDirA->x() * aVY) / aDet,
 724           (theDirB->y() * aVX - theDirA->y() * aVY) / aDet));
 725       theCenters.push_back(aPoint);
 726     }
 727   }
 728 }
 729
 730 /// \brief Find intersections of line shifted along normal direction in both sides
 731 ///        and a circle with extended radius
 732 void possibleFilletCenterLineArc(
 733     std::shared_ptr<GeomAPI_XY> theStartLine, std::shared_ptr<GeomAPI_Dir2d> theDirLine,
 734     std::shared_ptr<GeomAPI_XY> theCenterArc, double theRadiusArc,
 735     double theRadius, std::list< std::shared_ptr<GeomAPI_XY> >& theCenters)
 736 {
 737   std::shared_ptr<GeomAPI_Dir2d> aDirT(new GeomAPI_Dir2d(-theDirLine->y(), theDirLine->x()));
 738   std::shared_ptr<GeomAPI_XY> aPnt;
 739   double aDirNorm2 = theDirLine->dot(theDirLine);
 740   double aRad = 0.0;
 741   double aDirX = theDirLine->x();
 742   double aDirX2 = theDirLine->x() * theDirLine->x();
 743   double aDirY2 = theDirLine->y() * theDirLine->y();
 744   double aDirXY = theDirLine->x() * theDirLine->y();
 745   for (double aStepA = -1.0; aStepA <= 1.0; aStepA += 2.0) {
 746     aPnt = theStartLine->added(aDirT->xy()->multiplied(aStepA * theRadius));
 747     double aCoeff = aDirT->xy()->dot(aPnt->decreased(theCenterArc));
 748     double aCoeff2 = aCoeff * aCoeff;
 749     for (double aStepB = -1.0; aStepB <= 1.0; aStepB += 2.0) {
 750       aRad = theRadiusArc + aStepB * theRadius;
 751       double aD = aRad * aRad * aDirNorm2 - aCoeff2;
 752       if (aD < 0.0)
 753         continue;
 754       double aDs = sqrt(aD);
 755       double x1 = theCenterArc->x() + (aCoeff * aDirT->x() - aDirT->y() * aDs) / aDirNorm2;
 756       double x2 = theCenterArc->x() + (aCoeff * aDirT->x() + aDirT->y() * aDs) / aDirNorm2;
 757       double y1 = (aDirX2 * aPnt->y() + aDirY2 * theCenterArc->y() -
 758           aDirXY * (aPnt->x() - theCenterArc->x()) - theDirLine->y() * aDs) / aDirNorm2;
 759       double y2 = (aDirX2 * aPnt->y() + aDirY2 * theCenterArc->y() -
 760           aDirXY * (aPnt->x() - theCenterArc->x()) + theDirLine->y() * aDs) / aDirNorm2;
 761
 762       std::shared_ptr<GeomAPI_XY> aPoint1(new GeomAPI_XY(x1, y1));
 763       theCenters.push_back(aPoint1);
 764       std::shared_ptr<GeomAPI_XY> aPoint2(new GeomAPI_XY(x2, y2));
 765       theCenters.push_back(aPoint2);
 766     }
 767   }
 768 }
 769
 770 /// \brief Find intersections of two circles with extended radii
 771 void possibleFilletCenterArcArc(
 772     std::shared_ptr<GeomAPI_XY> theCenterA, double theRadiusA,
 773     std::shared_ptr<GeomAPI_XY> theCenterB, double theRadiusB,
 774     double theRadius, std::list< std::shared_ptr<GeomAPI_XY> >& theCenters)
 775 {
 776   std::shared_ptr<GeomAPI_XY> aCenterDir = theCenterB->decreased(theCenterA);
 777   double aCenterDist2 = aCenterDir->dot(aCenterDir);
 778   double aCenterDist = sqrt(aCenterDist2);
 779
 780   double aRadA, aRadB;
 781   for (double aStepA = -1.0; aStepA <= 1.0; aStepA += 2.0) {
 782     aRadA = theRadiusA + aStepA * theRadius;
 783     for (double aStepB = -1.0; aStepB <= 1.0; aStepB += 2.0) {
 784       aRadB = theRadiusB + aStepB * theRadius;
 785       if (aRadA + aRadB < aCenterDist || fabs(aRadA - aRadB) > aCenterDist)
 786         continue; // there is no intersections
 787
 788       double aMedDist = (aRadA * aRadA - aRadB * aRadB + aCenterDist2) / (2.0 * aCenterDist);
 789       double aHeight = sqrt(aRadA * aRadA - aMedDist * aMedDist);
 790
 791       double x1 = theCenterA->x() + (aMedDist * aCenterDir->x() + aCenterDir->y() * aHeight) / aCenterDist;
 792       double y1 = theCenterA->y() + (aMedDist * aCenterDir->y() - aCenterDir->x() * aHeight) / aCenterDist;
 793
 794       double x2 = theCenterA->x() + (aMedDist * aCenterDir->x() - aCenterDir->y() * aHeight) / aCenterDist;
 795       double y2 = theCenterA->y() + (aMedDist * aCenterDir->y() + aCenterDir->x() * aHeight) / aCenterDist;
 796
 797       std::shared_ptr<GeomAPI_XY> aPoint1(new GeomAPI_XY(x1, y1));
 798       theCenters.push_back(aPoint1);
 799       std::shared_ptr<GeomAPI_XY> aPoint2(new GeomAPI_XY(x2, y2));
 800       theCenters.push_back(aPoint2);
 801     }
 802   }
 803 }
 804
 805 void calculateFilletCenter(FeaturePtr theFeatureA, FeaturePtr theFeatureB,
 806                            double theRadius, bool theNotInversed[2],
 807                            std::shared_ptr<GeomAPI_XY>& theCenter,
 808                            std::shared_ptr<GeomAPI_XY>& theTangentA,
 809                            std::shared_ptr<GeomAPI_XY>& theTangentB)
 810 {
 811   static const int aNbFeatures = 2;
 812   FeaturePtr aFeature[aNbFeatures] = {theFeatureA, theFeatureB};
 813   std::shared_ptr<GeomAPI_XY> aStart[aNbFeatures], aEnd[aNbFeatures], aCenter[aNbFeatures];
 814   std::shared_ptr<GeomDataAPI_Point2D> aStartPoint, aEndPoint;
 815
 816   for (int i = 0; i < aNbFeatures; i++) {
 817     if (aFeature[i]->getKind() == SketchPlugin_Line::ID()) {
 818       aStartPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 819           aFeature[i]->attribute(SketchPlugin_Line::START_ID()));
 820       aEndPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 821           aFeature[i]->attribute(SketchPlugin_Line::END_ID()));
 822     } else if (aFeature[i]->getKind() == SketchPlugin_Arc::ID()) {
 823       aStartPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 824           aFeature[i]->attribute(SketchPlugin_Arc::START_ID()));
 825       aEndPoint = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 826           aFeature[i]->attribute(SketchPlugin_Arc::END_ID()));
 827       aCenter[i] = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 828           aFeature[i]->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt()->xy();
 829     } else
 830       return;
 831     aStart[i] = std::shared_ptr<GeomAPI_XY>(theNotInversed[i] ?
 832         new GeomAPI_XY(aStartPoint->x(), aStartPoint->y()) :
 833         new GeomAPI_XY(aEndPoint->x(), aEndPoint->y()));
 834     aEnd[i] = std::shared_ptr<GeomAPI_XY>(theNotInversed[i] ?
 835         new GeomAPI_XY(aEndPoint->x(), aEndPoint->y()) :
 836         new GeomAPI_XY(aStartPoint->x(), aStartPoint->y()));
 837   }
 838
 839   if (theFeatureA->getKind() == SketchPlugin_Line::ID() &&
 840       theFeatureB->getKind() == SketchPlugin_Line::ID()) {
 841     std::shared_ptr<GeomAPI_Dir2d> aDir[2];
 842     std::shared_ptr<GeomAPI_Dir2d> aDirT[2];
 843     for (int i = 0; i < aNbFeatures; i++) {
 844       aDir[i] = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(aEnd[i]->decreased(aStart[i])));
 845       aDirT[i] = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d(-aDir[i]->y(), aDir[i]->x()));
 846     }
 847
 848     // get and filter possible centers
 849     std::list< std::shared_ptr<GeomAPI_XY> > aSuspectCenters;
 850     possibleFilletCenterLineLine(aStart[0], aDir[0], aStart[1], aDir[1], theRadius, aSuspectCenters);
 851     double aDot = 0.0;
 852     std::list< std::shared_ptr<GeomAPI_XY> >::iterator anIt = aSuspectCenters.begin();
 853     for (; anIt != aSuspectCenters.end(); anIt++) {
 854       aDot = aDirT[0]->xy()->dot(aStart[0]->decreased(*anIt));
 855       theTangentA = (*anIt)->added(aDirT[0]->xy()->multiplied(aDot));
 856       if (theTangentA->decreased(aStart[0])->dot(aDir[0]->xy()) < 0.0)
 857         continue; // incorrect position
 858       aDot = aDirT[1]->xy()->dot(aStart[1]->decreased(*anIt));
 859       theTangentB = (*anIt)->added(aDirT[1]->xy()->multiplied(aDot));
 860       if (theTangentB->decreased(aStart[1])->dot(aDir[1]->xy()) < 0.0)
 861         continue; // incorrect position
 862       // the center is found, stop searching
 863       theCenter = *anIt;
 864       return;
 865     }
 866   } else if ((theFeatureA->getKind() == SketchPlugin_Arc::ID() &&
 867       theFeatureB->getKind() == SketchPlugin_Line::ID()) ||
 868       (theFeatureA->getKind() == SketchPlugin_Line::ID() &&
 869       theFeatureB->getKind() == SketchPlugin_Arc::ID())) {
 870     int aLineInd = theFeatureA->getKind() == SketchPlugin_Line::ID() ? 0 : 1;
 871     double anArcRadius = aStart[1-aLineInd]->distance(aCenter[1-aLineInd]);
 872     std::shared_ptr<GeomAPI_Dir2d> aDirLine = std::shared_ptr<GeomAPI_Dir2d>(
 873         new GeomAPI_Dir2d(aEnd[aLineInd]->decreased(aStart[aLineInd])));
 874     std::shared_ptr<GeomAPI_Dir2d> aDirT = std::shared_ptr<GeomAPI_Dir2d>(
 875         new GeomAPI_Dir2d(-aDirLine->y(), aDirLine->x()));
 876
 877     std::shared_ptr<GeomAPI_Dir2d> aStartArcDir = std::shared_ptr<GeomAPI_Dir2d>(
 878         new GeomAPI_Dir2d(aStart[1-aLineInd]->decreased(aCenter[1-aLineInd])));
 879     std::shared_ptr<GeomAPI_Dir2d> aEndArcDir = std::shared_ptr<GeomAPI_Dir2d>(
 880         new GeomAPI_Dir2d(aEnd[1-aLineInd]->decreased(aCenter[1-aLineInd])));
 881     double anArcAngle = aEndArcDir->angle(aStartArcDir);
 882
 883     // get possible centers and filter them
 884     std::list< std::shared_ptr<GeomAPI_XY> > aSuspectCenters;
 885     possibleFilletCenterLineArc(aStart[aLineInd], aDirLine, aCenter[1-aLineInd], anArcRadius, theRadius, aSuspectCenters);
 886     double aDot = 0.0;
 887     // the line is forward into the arc
 888     double innerArc = aCenter[1-aLineInd]->decreased(aStart[aLineInd])->dot(aDirLine->xy());
 889     std::shared_ptr<GeomAPI_XY> aLineTgPoint, anArcTgPoint;
 890     // The possible centers are ranged by their positions.
 891     // If the point is not satisfy one of criteria, the weight is decreased with penalty.
 892     int aBestWeight = 0;
 893     std::list< std::shared_ptr<GeomAPI_XY> >::iterator anIt = aSuspectCenters.begin();
 894     for (; anIt != aSuspectCenters.end(); anIt++) {
 895       int aWeight = 2;
 896       aDot = aDirT->xy()->dot(aStart[aLineInd]->decreased(*anIt));
 897       aLineTgPoint = (*anIt)->added(aDirT->xy()->multiplied(aDot));
 898       // Check the point is placed on the correct arc (penalty if false)
 899       if (aCenter[1-aLineInd]->distance(*anIt) * innerArc > anArcRadius * innerArc)
 900         aWeight -= 1;
 901       std::shared_ptr<GeomAPI_Dir2d> aCurDir = std::shared_ptr<GeomAPI_Dir2d>(
 902           new GeomAPI_Dir2d((*anIt)->decreased(aCenter[1-aLineInd])));
 903       double aCurAngle = aCurDir->angle(aStartArcDir);
 904       if (anArcAngle < 0.0) aCurAngle *= -1.0;
 905       if (aCurAngle < 0.0 || aCurAngle > fabs(anArcAngle))
 906         continue;
 907       if (aWeight > aBestWeight)
 908         aBestWeight = aWeight;
 909       else if (aWeight < aBestWeight ||
 910                aStart[aLineInd]->distance(*anIt) >
 911                aStart[aLineInd]->distance(theCenter)) // <-- take closer point
 912         continue;
 913       // the center is found, stop searching
 914       theCenter = *anIt;
 915       anArcTgPoint = aCenter[1-aLineInd]->added(aCurDir->xy()->multiplied(anArcRadius));
 916       if (theFeatureA->getKind() == SketchPlugin_Line::ID()) {
 917         theTangentA = aLineTgPoint;
 918         theTangentB = anArcTgPoint;
 919       } else {
 920         theTangentA = anArcTgPoint;
 921         theTangentB = aLineTgPoint;
 922       }
 923       //return;
 924     }
 925   } else if (theFeatureA->getKind() == SketchPlugin_Arc::ID() &&
 926       theFeatureB->getKind() == SketchPlugin_Arc::ID()) {
 927     double anArcRadius[aNbFeatures];
 928     double anArcAngle[aNbFeatures];
 929     std::shared_ptr<GeomAPI_Dir2d> aStartArcDir[aNbFeatures];
 930     for (int i = 0; i < aNbFeatures; i++) {
 931       anArcRadius[i] = aStart[i]->distance(aCenter[i]);
 932       aStartArcDir[i] = std::shared_ptr<GeomAPI_Dir2d>(
 933           new GeomAPI_Dir2d(aStart[i]->decreased(aCenter[i])));
 934       std::shared_ptr<GeomAPI_Dir2d> aEndArcDir = std::shared_ptr<GeomAPI_Dir2d>(
 935           new GeomAPI_Dir2d(aEnd[i]->decreased(aCenter[i])));
 936       anArcAngle[i] = aEndArcDir->angle(aStartArcDir[i]);
 937     }
 938
 939     // get and filter possible centers
 940     std::list< std::shared_ptr<GeomAPI_XY> > aSuspectCenters;
 941     possibleFilletCenterArcArc(aCenter[0], anArcRadius[0], aCenter[1], anArcRadius[1], theRadius, aSuspectCenters);
 942     double aDot = 0.0;
 943     std::shared_ptr<GeomAPI_XY> aLineTgPoint, anArcTgPoint;
 944     std::list< std::shared_ptr<GeomAPI_XY> >::iterator anIt = aSuspectCenters.begin();
 945     for (; anIt != aSuspectCenters.end(); anIt++) {
 946       std::shared_ptr<GeomAPI_Dir2d> aCurDir = std::shared_ptr<GeomAPI_Dir2d>(
 947           new GeomAPI_Dir2d((*anIt)->decreased(aCenter[0])));
 948       double aCurAngle = aCurDir->angle(aStartArcDir[0]);
 949       if (anArcAngle[0] < 0.0) aCurAngle *= -1.0;
 950       if (aCurAngle < 0.0 || aCurAngle > fabs(anArcAngle[0]))
 951         continue; // incorrect position
 952       theTangentA = aCenter[0]->added(aCurDir->xy()->multiplied(anArcRadius[0]));
 953
 954       aCurDir = std::shared_ptr<GeomAPI_Dir2d>(new GeomAPI_Dir2d((*anIt)->decreased(aCenter[1])));
 955       aCurAngle = aCurDir->angle(aStartArcDir[1]);
 956       if (anArcAngle[1] < 0.0) aCurAngle *= -1.0;
 957       if (aCurAngle < 0.0 || aCurAngle > fabs(anArcAngle[1]))
 958         continue; // incorrect position
 959       theTangentB = aCenter[1]->added(aCurDir->xy()->multiplied(anArcRadius[1]));
 960
 961       // the center is found, stop searching
 962       theCenter = *anIt;
 963       return;
 964     }
 965   }
 966 }
 967
 968 void getPointOnEdge(const FeaturePtr theFeature,
 969                     const std::shared_ptr<GeomAPI_Pnt2d> theFilletPoint,
 970                     std::shared_ptr<GeomAPI_Pnt2d>& thePoint) {
 971   if(theFeature->getKind() == SketchPlugin_Line::ID()) {
 972     std::shared_ptr<GeomAPI_Pnt2d> aPntStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 973       theFeature->attribute(SketchPlugin_Line::START_ID()))->pnt();
 974     std::shared_ptr<GeomAPI_Pnt2d> aPntEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 975       theFeature->attribute(SketchPlugin_Line::END_ID()))->pnt();
 976     if(aPntStart->distance(theFilletPoint) > 1.e-7) {
 977       aPntStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 978         theFeature->attribute(SketchPlugin_Line::END_ID()))->pnt();
 979       aPntEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 980         theFeature->attribute(SketchPlugin_Line::START_ID()))->pnt();
 981     }
 982     thePoint.reset( new GeomAPI_Pnt2d(aPntStart->xy()->added( aPntEnd->xy()->decreased( aPntStart->xy() )->multiplied(1.0 / 3.0) ) ) );
 983   } else {
 984     std::shared_ptr<GeomAPI_Pnt2d> aPntTemp;
 985     std::shared_ptr<GeomAPI_Pnt2d> aPntStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 986       theFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
 987     std::shared_ptr<GeomAPI_Pnt2d> aPntEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 988       theFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
 989     if(theFeature->attribute(SketchPlugin_Arc::INVERSED_ID())) {
 990       aPntTemp = aPntStart;
 991       aPntStart = aPntEnd;
 992       aPntEnd = aPntTemp;
 993     }
 994     std::shared_ptr<GeomAPI_Pnt2d> aCenterPnt = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 995       theFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
 996     std::shared_ptr<GeomAPI_Circ2d> aCirc(new GeomAPI_Circ2d(aCenterPnt, aPntStart));
 997     double aStartParameter(0), anEndParameter(0);
 998     aCirc->parameter(aPntStart, paramTolerance, aStartParameter);
 999     aCirc->parameter(aPntEnd, paramTolerance, anEndParameter);
1000     if(aPntStart->distance(theFilletPoint) > tolerance) {
1001       double aTmpParameter = aStartParameter;
1002       aStartParameter = anEndParameter;
1003       anEndParameter = aTmpParameter;
1004     }
1005     double aPntParameter = aStartParameter + (anEndParameter - aStartParameter) / 3.0;
1006     aCirc->D0(aPntParameter, thePoint);
1007   }
1008 }
1009
1010 double getProjectionDistance(const FeaturePtr theFeature,
1011                              const std::shared_ptr<GeomAPI_Pnt2d> thePoint)
1012 {
1013   std::shared_ptr<GeomAPI_Pnt2d> aProjectPnt;
1014   if(theFeature->getKind() == SketchPlugin_Line::ID()) {
1015     std::shared_ptr<GeomAPI_Pnt2d> aPntStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
1016       theFeature->attribute(SketchPlugin_Line::START_ID()))->pnt();
1017     std::shared_ptr<GeomAPI_Pnt2d> aPntEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
1018       theFeature->attribute(SketchPlugin_Line::END_ID()))->pnt();
1019     std::shared_ptr<GeomAPI_Lin2d> aLin(new GeomAPI_Lin2d(aPntStart, aPntEnd));
1020     aProjectPnt = aLin->project(thePoint);
1021   } else {
1022     std::shared_ptr<GeomAPI_Pnt2d> aPntStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
1023       theFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
1024     std::shared_ptr<GeomAPI_Pnt2d> aPntEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
1025       theFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
1026     std::shared_ptr<GeomAPI_Pnt2d> aCenterPnt = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
1027       theFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
1028     std::shared_ptr<GeomAPI_Circ2d> aCirc(new GeomAPI_Circ2d(aCenterPnt, aPntStart));
1029     aProjectPnt = aCirc->project(thePoint);
1030   }
1031   if(aProjectPnt.get()) {
1032     return aProjectPnt->distance(thePoint);
1033   }
1034   return -1;
1035 }
1036
1037 std::set<FeaturePtr> getCoincides(const FeaturePtr& theConstraintCoincidence)
1038 {
1039   std::set<FeaturePtr> aCoincides;
1040
1041   std::shared_ptr<GeomAPI_Pnt2d> aFilletPnt = SketchPlugin_Tools::getCoincidencePoint(theConstraintCoincidence);
1042
1043   SketchPlugin_Tools::findCoincidences(theConstraintCoincidence,
1044                                        SketchPlugin_ConstraintCoincidence::ENTITY_A(),
1045                                        aCoincides);
1046   SketchPlugin_Tools::findCoincidences(theConstraintCoincidence,
1047                                        SketchPlugin_ConstraintCoincidence::ENTITY_B(),
1048                                        aCoincides);
1049
1050   // Remove points from set of coincides.
1051   std::set<FeaturePtr> aNewSetOfCoincides;
1052   for(std::set<FeaturePtr>::iterator anIt = aCoincides.begin(); anIt != aCoincides.end(); ++anIt) {
1053     if((*anIt)->getKind() == SketchPlugin_Line::ID()) {
1054       aNewSetOfCoincides.insert(*anIt);
1055     } else if((*anIt)->getKind() == SketchPlugin_Arc::ID()) {
1056       AttributePtr anAttrCenter = (*anIt)->attribute(SketchPlugin_Arc::CENTER_ID());
1057       std::shared_ptr<GeomDataAPI_Point2D> aPointCenter2D =
1058         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrCenter);
1059       if(aPointCenter2D.get() && aFilletPnt->isEqual(aPointCenter2D->pnt())) {
1060         continue;
1061       }
1062       aNewSetOfCoincides.insert(*anIt);
1063     }
1064   }
1065   aCoincides = aNewSetOfCoincides;
1066
1067   // If we still have more than two coincides remove auxilary entities from set of coincides.
1068   if(aCoincides.size() > 2) {
1069     aNewSetOfCoincides.clear();
1070     for(std::set<FeaturePtr>::iterator anIt = aCoincides.begin(); anIt != aCoincides.end(); ++anIt) {
1071       if(!(*anIt)->boolean(SketchPlugin_SketchEntity::AUXILIARY_ID())->value()) {
1072         aNewSetOfCoincides.insert(*anIt);
1073       }
1074     }
1075     aCoincides = aNewSetOfCoincides;
1076   }
1077
1078   return aCoincides;
1079 }