src/SketchSolver/SketchSolver_Constraint.cpp

   1 #include <SketchSolver_Constraint.h>
   2 #include <SketchSolver_Group.h>
   3 #include <SketchSolver_Error.h>
   4
   5 #include <SketchPlugin_Arc.h>
   6 #include <SketchPlugin_Circle.h>
   7 #include <SketchPlugin_Line.h>
   8 #include <SketchPlugin_Point.h>
   9
  10 #include <GeomDataAPI_Point.h>
  11 #include <GeomDataAPI_Point2D.h>
  12 #include <ModelAPI_AttributeDouble.h>
  13 #include <ModelAPI_ResultConstruction.h>
  14
  15 #include <math.h>
  16
  17 SketchSolver_Constraint::SketchSolver_Constraint(
  18     ConstraintPtr  theConstraint)
  19   : myBaseConstraint(theConstraint),
  20     myGroup(0)
  21 {
  22 }
  23
  24 SketchSolver_Constraint::~SketchSolver_Constraint()
  25 {
  26   std::map<AttributePtr, Slvs_hParam>::const_iterator anIt1 = myValueMap.begin();
  27   for (; anIt1 != myValueMap.end(); anIt1++)
  28     myStorage->removeParameter(anIt1->second);
  29   myValueMap.clear();
  30
  31   std::map<AttributePtr, Slvs_hEntity>::const_iterator anIt2 = myAttributeMap.begin();
  32   for (; anIt2 != myAttributeMap.end(); anIt2++)
  33     myStorage->removeEntity(anIt2->second);
  34   myAttributeMap.clear();
  35
  36   std::map<FeaturePtr, Slvs_hEntity>::const_iterator anIt3 =  myFeatureMap.begin();
  37   for (; anIt3 != myFeatureMap.end(); anIt3++)
  38     myStorage->removeEntity(anIt3->second);
  39   myFeatureMap.clear();
  40
  41   std::vector<Slvs_hConstraint>::const_iterator anIt4 = mySlvsConstraints.begin();
  42   for (; anIt4 != mySlvsConstraints.end(); anIt4++)
  43     myStorage->removeConstraint(*anIt4);
  44   mySlvsConstraints.clear();
  45 }
  46
  47 void SketchSolver_Constraint::setStorage(StoragePtr theStorage)
  48 {
  49   myStorage = theStorage;
  50   process();
  51 }
  52
  53 void SketchSolver_Constraint::setGroup(SketchSolver_Group* theGroup)
  54 {
  55   myGroup = theGroup;
  56   process();
  57 }
  58
  59
  60 void SketchSolver_Constraint::process()
  61 {
  62   cleanErrorMsg();
  63   if (!myBaseConstraint || !myStorage || myGroup == 0) {
  64     /// TODO: Put error message here
  65     return;
  66   }
  67   if (!mySlvsConstraints.empty()) // some data is changed, update constraint
  68     update(myBaseConstraint);
  69
  70   int aConstrType = getType();
  71   double aValue = 0.0;
  72   std::vector<Slvs_hEntity> anAttributes;
  73   getAttributes(aValue, anAttributes);
  74   if (!myErrorMsg.empty())
  75     return;
  76
  77   Slvs_hGroup aGroupID = myGroup->getId();
  78   Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
  79   Slvs_Constraint aConstraint;
  80   if (mySlvsConstraints.empty())
  81     aConstraint = Slvs_MakeConstraint(SLVS_C_UNKNOWN, aGroupID, aConstrType, aWorkplaneID,
  82         aValue, anAttributes[0], anAttributes[1], anAttributes[2], anAttributes[3]);
  83   else {
  84     aConstraint = myStorage->getConstraint(mySlvsConstraints[0]);
  85     aConstraint.valA = aValue;
  86     static const int aNbAttrs = 6;
  87     Slvs_hEntity* aConstrAttrs[aNbAttrs] = {
  88         &aConstraint.ptA, &aConstraint.ptB,
  89         &aConstraint.entityA, &aConstraint.entityB,
  90         &aConstraint.entityC, &aConstraint.entityD};
  91     std::vector<Slvs_hEntity>::const_iterator anIter = anAttributes.begin();
  92     for (int i = 0; i < aNbAttrs && anIter != anAttributes.end(); i++, anIter++)
  93       *(aConstrAttrs[i]) = *anIter;
  94   }
  95
  96   Slvs_hConstraint anID = myStorage->addConstraint(aConstraint);
  97   if (mySlvsConstraints.empty())
  98     mySlvsConstraints.push_back(anID);
  99   else
 100     mySlvsConstraints[0] = anID;
 101   adjustConstraint();
 102 }
 103
 104 void SketchSolver_Constraint::update(ConstraintPtr theConstraint)
 105 {
 106   cleanErrorMsg();
 107   if (theConstraint && theConstraint != myBaseConstraint) {
 108     if (theConstraint->getKind() != myBaseConstraint->getKind())
 109       return;
 110     remove(myBaseConstraint);
 111     myBaseConstraint = theConstraint;
 112     process();
 113   }
 114
 115   // Update all attributes
 116   int aType;
 117   std::map<Slvs_hEntity, Slvs_hEntity> aRelocationMap;
 118   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIter = myFeatureMap.begin();
 119   for (; aFeatIter != myFeatureMap.end(); aFeatIter++) {
 120     Slvs_hEntity aPrevID = aFeatIter->second;
 121     aFeatIter->second = changeEntity(aFeatIter->first, aType);
 122     if (aFeatIter->second != aPrevID)
 123       aRelocationMap[aPrevID] = aFeatIter->second;
 124   }
 125   std::map<AttributePtr, Slvs_hEntity>::iterator anAttrIter = myAttributeMap.begin();
 126   for (; anAttrIter != myAttributeMap.end(); anAttrIter++) {
 127     Slvs_hEntity aPrevID = anAttrIter->second;
 128     anAttrIter->second = changeEntity(anAttrIter->first, aType);
 129     if (anAttrIter->second != aPrevID)
 130       aRelocationMap[aPrevID] = anAttrIter->second;
 131   }
 132
 133   // Value if exists
 134   AttributeDoublePtr aValueAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 135     myBaseConstraint->data()->attribute(SketchPlugin_Constraint::VALUE()));
 136   double aValue = aValueAttr ? aValueAttr->value() : 0.0;
 137
 138   // Update constraint
 139   std::vector<Slvs_hConstraint>::iterator aCIter = mySlvsConstraints.begin();
 140   for (; aCIter != mySlvsConstraints.end(); aCIter++) {
 141     Slvs_Constraint aConstraint = myStorage->getConstraint(*aCIter);
 142     aConstraint.valA = aValue;
 143     Slvs_hEntity* aCoeffs[6] = {
 144         &aConstraint.ptA, &aConstraint.ptB,
 145         &aConstraint.entityA, &aConstraint.entityB,
 146         &aConstraint.entityC, &aConstraint.entityD};
 147     for (int i = 0; i < 6; i++) {
 148       if (*(aCoeffs[i]) == SLVS_E_UNKNOWN)
 149         continue;
 150       std::map<Slvs_hEntity, Slvs_hEntity>::iterator aFound = aRelocationMap.find(*(aCoeffs[i]));
 151       if (aFound != aRelocationMap.end())
 152         *(aCoeffs[i]) = aFound->second;
 153     }
 154     *aCIter = myStorage->addConstraint(aConstraint);
 155   }
 156   adjustConstraint();
 157 }
 158
 159 bool SketchSolver_Constraint::remove(ConstraintPtr theConstraint)
 160 {
 161   cleanErrorMsg();
 162   if (theConstraint && theConstraint != myBaseConstraint)
 163     return false;
 164   if (mySlvsConstraints.empty())
 165     return true;
 166   bool isFullyRemoved = myStorage->removeConstraint(mySlvsConstraints.front());
 167   if (isFullyRemoved) {
 168     myFeatureMap.clear();
 169     myAttributeMap.clear();
 170     myValueMap.clear();
 171   } else
 172     cleanRemovedEntities();
 173   mySlvsConstraints.clear();
 174   return true;
 175 }
 176
 177 void SketchSolver_Constraint::cleanRemovedEntities()
 178 {
 179   std::set<Slvs_hParam> aRemovedParams;
 180   std::set<Slvs_hEntity> aRemovedEntities;
 181   std::set<Slvs_hConstraint> aRemovedConstraints;
 182   myStorage->getRemoved(aRemovedParams, aRemovedEntities, aRemovedConstraints);
 183   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt = myFeatureMap.begin();
 184   while (aFeatIt != myFeatureMap.end()) {
 185     if (aRemovedEntities.find(aFeatIt->second) == aRemovedEntities.end()) {
 186       aFeatIt++;
 187       continue;
 188     }
 189     std::map<FeaturePtr, Slvs_hEntity>::iterator aTmpIter = aFeatIt++;
 190     myFeatureMap.erase(aTmpIter);
 191   }
 192   std::map<AttributePtr, Slvs_hEntity>::iterator anAttrIt = myAttributeMap.begin();
 193   while (anAttrIt != myAttributeMap.end()) {
 194     if (aRemovedEntities.find(anAttrIt->second) == aRemovedEntities.end()) {
 195       anAttrIt++;
 196       continue;
 197     }
 198     std::map<AttributePtr, Slvs_hEntity>::iterator aTmpIter = anAttrIt++;
 199     myAttributeMap.erase(aTmpIter);
 200   }
 201   std::map<AttributePtr, Slvs_hParam>::iterator aValIt = myValueMap.begin();
 202   while (aValIt != myValueMap.end()) {
 203     if (aRemovedParams.find(aValIt->second) == aRemovedParams.end()) {
 204       aValIt++;
 205       continue;
 206     }
 207     std::map<AttributePtr, Slvs_hParam>::iterator aTmpIter = aValIt++;
 208     myValueMap.erase(aTmpIter);
 209   }
 210 }
 211
 212 void SketchSolver_Constraint::getAttributes(
 213     double& theValue,
 214     std::vector<Slvs_hEntity>& theAttributes)
 215 {
 216   static const int anInitNbOfAttr = 4;
 217   theAttributes.assign(anInitNbOfAttr, SLVS_E_UNKNOWN);
 218
 219   DataPtr aData = myBaseConstraint->data();
 220
 221   AttributeDoublePtr aValueAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 222     aData->attribute(SketchPlugin_Constraint::VALUE()));
 223   theValue = aValueAttr ? aValueAttr->value() : 0.0;
 224
 225   int aPtInd = 0; // index of first point in the list of attributes
 226   int aEntInd = 2; // index of first antity in the list of attributes
 227   std::list<AttributePtr> aConstrAttrs = aData->attributes(ModelAPI_AttributeRefAttr::typeId());
 228   std::list<AttributePtr>::iterator anIter = aConstrAttrs.begin();
 229   for (; anIter != aConstrAttrs.end(); anIter++) {
 230     AttributeRefAttrPtr aRefAttr =
 231         std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(*anIter);
 232     if (!aRefAttr || !aRefAttr->isInitialized()) {
 233       myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 234       return;
 235     }
 236
 237     int aType = SLVS_E_UNKNOWN; // type of created entity
 238     Slvs_hEntity anEntity = myGroup->getAttributeId(aRefAttr);
 239     if (anEntity == SLVS_E_UNKNOWN)
 240       anEntity = changeEntity(aRefAttr, aType);
 241     else {
 242       Slvs_Entity anEnt = myStorage->getEntity(anEntity);
 243       aType = anEnt.type;
 244     }
 245
 246     if (aType == SLVS_E_UNKNOWN)
 247       continue;
 248     else if (aType == SLVS_E_POINT_IN_2D || aType == SLVS_E_POINT_IN_3D)
 249       theAttributes[aPtInd++] = anEntity; // the point is created
 250     else { // another entity (not a point) is created
 251       if (aEntInd < anInitNbOfAttr)
 252         theAttributes[aEntInd] = anEntity;
 253       else
 254         theAttributes.push_back(anEntity);
 255       aEntInd++;
 256     }
 257   }
 258 }
 259
 260 Slvs_hEntity SketchSolver_Constraint::changeEntity(AttributeRefAttrPtr theAttribute, int& theType)
 261 {
 262   // Convert the object of the attribute to the feature
 263   FeaturePtr aFeature;
 264   if (theAttribute->isObject() && theAttribute->object()) {
 265     ResultConstructionPtr aRC = std::dynamic_pointer_cast<ModelAPI_ResultConstruction>(
 266         theAttribute->object());
 267     if (!aRC) {
 268       myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 269       return SLVS_E_UNKNOWN;
 270     }
 271     std::shared_ptr<ModelAPI_Document> aDoc = aRC->document();
 272     aFeature = aDoc->feature(aRC);
 273
 274     return changeEntity(aFeature, theType);
 275   }
 276
 277   return changeEntity(theAttribute->attr(), theType);
 278 }
 279
 280 Slvs_hEntity SketchSolver_Constraint::changeEntity(AttributePtr theEntity, int& theType)
 281 {
 282   Slvs_hEntity aResult = SLVS_E_UNKNOWN;
 283   if (!isInitialized(theEntity))
 284     return SLVS_E_UNKNOWN;
 285
 286   // If the entity is already in the group, try to find it
 287   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator anEntIter =
 288       myAttributeMap.find(theEntity);
 289   Slvs_Entity aCurrentEntity;
 290   aCurrentEntity.h = SLVS_E_UNKNOWN;
 291   if (anEntIter != myAttributeMap.end())
 292     aCurrentEntity = myStorage->getEntity(anEntIter->second);
 293   else {
 294     aResult = myGroup->getAttributeId(theEntity);
 295     if (aResult != SLVS_E_UNKNOWN) {
 296       Slvs_Entity anEnt = myStorage->getEntity(aResult);
 297       theType = anEnt.type;
 298       return aResult;
 299     }
 300   }
 301
 302   Slvs_hGroup aGroupID = myGroup->getId();
 303   // Point in 3D
 304   std::shared_ptr<GeomDataAPI_Point> aPoint =
 305       std::dynamic_pointer_cast<GeomDataAPI_Point>(theEntity);
 306   if (aPoint) {
 307     double aXYZ[3] = {aPoint->x(), aPoint->y(), aPoint->z()};
 308     Slvs_hParam aParams[3];
 309     for (int i = 0; i < 3; i++) {
 310       Slvs_Param aPar = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 311           myStorage->getParameter(aCurrentEntity.param[i]) :
 312           Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 313       aPar.val = aXYZ[i];
 314       aParams[i] = myStorage->addParameter(aPar);
 315     }
 316
 317     if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 318       aCurrentEntity = Slvs_MakePoint3d(SLVS_E_UNKNOWN, aGroupID, aParams[0], aParams[1], aParams[2]);
 319     else { // update entity data
 320       for (int i = 0; i < 3; i++)
 321         aCurrentEntity.param[i] = aParams[i];
 322     }
 323     aResult = myStorage->addEntity(aCurrentEntity);
 324   } else {
 325     // All entities except 3D points are created on workplane. So, if there is no workplane yet, then error
 326     Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
 327     if (aWorkplaneID == SLVS_E_UNKNOWN)
 328       return SLVS_E_UNKNOWN;
 329
 330     // Point in 2D
 331     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 332         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
 333     if (aPoint2D) {
 334       double aXY[2] = {aPoint2D->x(), aPoint2D->y()};
 335       Slvs_hParam aParams[2];
 336       for (int i = 0; i < 2; i++) {
 337         Slvs_Param aPar = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 338             myStorage->getParameter(aCurrentEntity.param[i]) :
 339             Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 340         aPar.val = aXY[i];
 341         aParams[i] = myStorage->addParameter(aPar);
 342       }
 343
 344       if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 345         aCurrentEntity = Slvs_MakePoint2d(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aParams[0], aParams[1]);
 346       else { // update entity data
 347         for (int i = 0; i < 2; i++)
 348           aCurrentEntity.param[i] = aParams[i];
 349       }
 350       aResult = myStorage->addEntity(aCurrentEntity);
 351     } else {
 352       // Scalar value (used for the distance entities)
 353       AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theEntity);
 354       if (aScalar) {
 355         Slvs_Param aParam = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 356             myStorage->getParameter(aCurrentEntity.param[0]) :
 357             Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 358         aParam.val = aScalar->value();
 359         Slvs_hParam aValue = myStorage->addParameter(aParam);
 360
 361         if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 362           aCurrentEntity = Slvs_MakeDistance(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aValue);
 363         else
 364           aCurrentEntity.param[0] = aValue;
 365         aResult = myStorage->addEntity(aCurrentEntity);
 366       }
 367     }
 368   }
 369
 370   myAttributeMap[theEntity] = aResult;
 371   theType = aCurrentEntity.type;
 372   return aResult;
 373 }
 374
 375 Slvs_hEntity SketchSolver_Constraint::changeEntity(FeaturePtr theEntity, int& theType)
 376 {
 377   Slvs_hEntity aResult = SLVS_E_UNKNOWN;
 378   if (!theEntity->data() || !theEntity->data()->isValid())
 379     return SLVS_E_UNKNOWN;
 380   // If the entity is already in the group, try to find it
 381   std::map<FeaturePtr, Slvs_hEntity>::const_iterator anEntIter = myFeatureMap.find(theEntity);
 382   Slvs_Entity aCurrentEntity;
 383   aCurrentEntity.h = SLVS_E_UNKNOWN;
 384   if (anEntIter != myFeatureMap.end())
 385     aCurrentEntity = myStorage->getEntity(anEntIter->second);
 386   else {
 387     aResult = myGroup->getFeatureId(theEntity);
 388     if (aResult != SLVS_E_UNKNOWN) {
 389       Slvs_Entity anEnt = myStorage->getEntity(aResult);
 390       theType = anEnt.type;
 391       return aResult;
 392     }
 393   }
 394
 395   Slvs_hGroup aGroupID = myGroup->getId();
 396   Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
 397   DataPtr aData = theEntity->data();
 398
 399   // SketchPlugin features
 400   const std::string& aFeatureKind = theEntity->getKind();
 401   AttributePtr anAttribute;
 402   int anAttrType;
 403   // Line
 404   if (aFeatureKind == SketchPlugin_Line::ID()) {
 405     anAttribute = aData->attribute(SketchPlugin_Line::START_ID());
 406     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 407     Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
 408
 409     anAttribute = aData->attribute(SketchPlugin_Line::END_ID());
 410     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 411     Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
 412
 413     if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 414       aCurrentEntity = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aStart, aEnd);
 415     else {
 416       aCurrentEntity.point[0] = aStart;
 417       aCurrentEntity.point[1] = aEnd;
 418     }
 419     aResult = myStorage->addEntity(aCurrentEntity);
 420   }
 421   // Circle
 422   else if (aFeatureKind == SketchPlugin_Circle::ID()) {
 423     anAttribute = aData->attribute(SketchPlugin_Circle::CENTER_ID());
 424     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 425     Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
 426
 427     anAttribute = aData->attribute(SketchPlugin_Circle::RADIUS_ID());
 428     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 429     Slvs_hEntity aRadius = changeEntity(anAttribute, anAttrType);
 430
 431     if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
 432       Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
 433       aCurrentEntity = Slvs_MakeCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
 434                                         aCenter, aWorkplane.normal, aRadius);
 435     } else {
 436       aCurrentEntity.point[0] = aCenter;
 437       aCurrentEntity.distance = aRadius;
 438     }
 439     aResult = myStorage->addEntity(aCurrentEntity);
 440   }
 441   // Arc
 442   else if (aFeatureKind == SketchPlugin_Arc::ID()) {
 443     anAttribute = aData->attribute(SketchPlugin_Arc::CENTER_ID());
 444     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 445     Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
 446
 447     anAttribute = aData->attribute(SketchPlugin_Arc::START_ID());
 448     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 449     Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
 450
 451     anAttribute = aData->attribute(SketchPlugin_Arc::END_ID());
 452     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 453     Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
 454
 455     if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
 456       Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
 457       aCurrentEntity = Slvs_MakeArcOfCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
 458                                             aWorkplane.normal, aCenter, aStart, aEnd);
 459     } else {
 460       aCurrentEntity.point[0] = aCenter;
 461       aCurrentEntity.point[1] = aStart;
 462       aCurrentEntity.point[2] = aEnd;
 463     }
 464     aResult = myStorage->addEntity(aCurrentEntity);
 465   }
 466   // Point (it has low probability to be an attribute of constraint, so it is checked at the end)
 467   else if (aFeatureKind == SketchPlugin_Point::ID()) {
 468     anAttribute = aData->attribute(SketchPlugin_Point::COORD_ID());
 469     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 470     // Both the sketch point and its attribute (coordinates) link to the same SolveSpace point identifier
 471     aResult = changeEntity(anAttribute, anAttrType);
 472     aCurrentEntity.type = SLVS_E_POINT_IN_3D;
 473   }
 474
 475   if (aResult != SLVS_E_UNKNOWN) {
 476     myFeatureMap[theEntity] = aResult;
 477     theType = aCurrentEntity.type;
 478   }
 479   return aResult;
 480 }
 481
 482 std::list<ConstraintPtr> SketchSolver_Constraint::constraints() const
 483 {
 484   std::list<ConstraintPtr> aConstraints;
 485   aConstraints.push_back(myBaseConstraint);
 486   return aConstraints;
 487 }
 488
 489 void SketchSolver_Constraint::refresh()
 490 {
 491   cleanErrorMsg();
 492   std::map<AttributePtr, Slvs_hEntity>::iterator anAttrIter = myAttributeMap.begin();
 493   for (; anAttrIter != myAttributeMap.end(); anAttrIter++) {
 494     std::shared_ptr<GeomDataAPI_Point> aPoint =
 495         std::dynamic_pointer_cast<GeomDataAPI_Point>(anAttrIter->first);
 496     if (aPoint) {
 497       Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 498       double aXYZ[3];
 499       for (int i = 0; i < 3; i++) {
 500         Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
 501         aXYZ[i] = aPar.val;
 502       }
 503       if (fabs(aPoint->x() - aXYZ[0]) > tolerance ||
 504           fabs(aPoint->y() - aXYZ[1]) > tolerance ||
 505           fabs(aPoint->z() - aXYZ[2]) > tolerance)
 506         aPoint->setValue(aXYZ[0], aXYZ[1], aXYZ[2]);
 507     } else {
 508       // Point in 2D
 509       std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 510           std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrIter->first);
 511       if (aPoint2D) {
 512         Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 513         double aXY[2];
 514         for (int i = 0; i < 2; i++) {
 515           Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
 516           aXY[i] = aPar.val;
 517         }
 518         if (fabs(aPoint2D->x() - aXY[0]) > tolerance ||
 519             fabs(aPoint2D->y() - aXY[1]) > tolerance)
 520         aPoint2D->setValue(aXY[0], aXY[1]);
 521       } else {
 522         // Scalar value (used for the distance entities)
 523         AttributeDoublePtr aScalar =
 524             std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
 525         if (aScalar) {
 526           Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 527           Slvs_Param aPar = myStorage->getParameter(anEntity.param[0]);
 528           if (fabs(aScalar->value() - aPar.val) > tolerance)
 529             aScalar->setValue(aPar.val);
 530         }
 531       }
 532     }
 533   }
 534
 535   std::map<AttributePtr, Slvs_hParam>::iterator aValIter = myValueMap.begin();
 536   for (; aValIter != myValueMap.end(); aValIter++) {
 537     AttributeDoublePtr aScalar =
 538         std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
 539     if (aScalar) {
 540       Slvs_Param aPar = myStorage->getParameter(anAttrIter->second);
 541       aScalar->setValue(aPar.val);
 542     }
 543   }
 544 }
 545
 546 Slvs_hEntity SketchSolver_Constraint::getId(FeaturePtr theFeature) const
 547 {
 548   std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFIter = myFeatureMap.find(theFeature);
 549   if (aFIter == myFeatureMap.end())
 550     return SLVS_E_UNKNOWN;
 551   return aFIter->second;
 552 }
 553
 554 Slvs_hEntity SketchSolver_Constraint::getId(AttributePtr theAttribute) const
 555 {
 556   std::map<AttributePtr, Slvs_hEntity>::const_iterator anAttrIter = myAttributeMap.find(theAttribute);
 557   if (anAttrIter == myAttributeMap.end())
 558     return SLVS_E_UNKNOWN;
 559   return anAttrIter->second;
 560 }
 561
 562 bool SketchSolver_Constraint::isInitialized(AttributePtr theAttribute)
 563 {
 564   if (theAttribute->isInitialized())
 565     return true;
 566   myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 567   return false;
 568 }
 569
 570
 571 void SketchSolver_Constraint::calculateMiddlePoint(
 572     const Slvs_Entity& theEntity, double theCoeff, double& theX, double& theY) const
 573 {
 574   if (theEntity.type == SLVS_E_LINE_SEGMENT) {
 575     double aStartEndXY[2][2];
 576     Slvs_Entity aPoint;
 577     for (int i = 0; i < 2; i++) {
 578       aPoint = myStorage->getEntity(theEntity.point[i]);
 579       for (int j = 0; j < 2; j++)
 580         aStartEndXY[i][j] = myStorage->getParameter(aPoint.param[j]).val;
 581     }
 582     theX = (1.0 - theCoeff) * aStartEndXY[0][0] + theCoeff * aStartEndXY[1][0];
 583     theY = (1.0 - theCoeff) * aStartEndXY[0][1] + theCoeff * aStartEndXY[1][1];
 584   } else if (theEntity.type == SLVS_E_ARC_OF_CIRCLE) {
 585     double anArcPoint[3][2];
 586     Slvs_Entity aPoint;
 587     for (int i = 0; i < 3; i++) {
 588       aPoint = myStorage->getEntity(theEntity.point[i]);
 589       for (int j = 0; j < 2; j++)
 590         anArcPoint[i][j] = myStorage->getParameter(aPoint.param[j]).val;
 591     }
 592     // project last point of arc on the arc
 593     double x = anArcPoint[1][0] - anArcPoint[0][0];
 594     double y = anArcPoint[1][1] - anArcPoint[0][1];
 595     double aRad = sqrt(x*x + y*y);
 596     x = anArcPoint[2][0] - anArcPoint[0][0];
 597     y = anArcPoint[2][1] - anArcPoint[0][1];
 598     double aNorm = sqrt(x*x + y*y);
 599     if (aNorm >= tolerance) {
 600       anArcPoint[2][0] = x * aRad / aNorm;
 601       anArcPoint[2][1] = y * aRad / aNorm;
 602     }
 603     anArcPoint[1][0] -= anArcPoint[0][0];
 604     anArcPoint[1][1] -= anArcPoint[0][1];
 605     if (theCoeff < tolerance) {
 606       theX = anArcPoint[0][0] + anArcPoint[1][0];
 607       theY = anArcPoint[0][1] + anArcPoint[1][1];
 608       return;
 609     } else if (1 - theCoeff < tolerance) {
 610       theX = anArcPoint[0][0] + anArcPoint[2][0];
 611       theY = anArcPoint[0][1] + anArcPoint[2][1];
 612       return;
 613     }
 614
 615     double xStart = anArcPoint[1][0] / aRad, xEnd = anArcPoint[2][0] / aRad;
 616     double yStart = anArcPoint[1][1] / aRad, yEnd = anArcPoint[2][1] / aRad;
 617     if (anArcPoint[1][0] * anArcPoint[2][0] < 0.0) {
 618       if (anArcPoint[1][0] > 0.0)
 619         yEnd = 2.0 - yEnd;
 620       else
 621         yStart = -2.0 - yStart;
 622     } else {
 623       if (yStart > yEnd) {
 624         yStart = 2.0 - yStart;
 625         yEnd = -2.0 - yEnd;
 626       } else {
 627         yStart = -2.0 - yStart;
 628         yEnd = 2.0 - yEnd;
 629       }
 630     }
 631     if (anArcPoint[1][1] * anArcPoint[2][1] < 0.0) {
 632       if (anArcPoint[1][1] > 0.0)
 633         xEnd = 2.0 - xEnd;
 634       else
 635         xStart = -2.0 - xStart;
 636     } else {
 637       if (xStart > xEnd) {
 638         xStart = 2.0 - xStart;
 639         xEnd = -2.0 - xEnd;
 640       } else {
 641         xStart = -2.0 - xStart;
 642         xEnd = 2.0 - xEnd;
 643       }
 644     }
 645     x = (1.0 - theCoeff) * xStart + theCoeff * xEnd;
 646     y = (1.0 - theCoeff) * yStart + theCoeff * yEnd;
 647     if (x > 1.0) x = 2.0 - x;
 648     if (x < -1.0) x = -2.0 - x;
 649     if (y > 1.0) y = 2.0 - y;
 650     if (y < -1.0) y = -2.0 - y;
 651
 652     aNorm = sqrt(x*x + y*y);
 653     if (aNorm >= tolerance) {
 654       x *= aRad / aNorm;
 655       y *= aRad / aNorm;
 656     } else {
 657       x = -0.5 * (anArcPoint[2][1] + anArcPoint[1][1]);
 658       y = -0.5 * (anArcPoint[2][0] + anArcPoint[1][0]);
 659     }
 660     theX = anArcPoint[0][0] + x;
 661     theY = anArcPoint[0][1] + y;
 662   }
 663 }
 664