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