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