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