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     myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 338     return SLVS_E_UNKNOWN;
 339   }
 340
 341   // If the entity is already in the group, try to find it
 342   std::map<std::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator anEntIter =
 343       myAttributeMap.find(theEntity);
 344   Slvs_Entity aCurrentEntity;
 345   aCurrentEntity.h = SLVS_E_UNKNOWN;
 346   if (anEntIter != myAttributeMap.end())
 347     aCurrentEntity = myStorage->getEntity(anEntIter->second);
 348   else {
 349     aResult = myGroup->getAttributeId(theEntity);
 350     if (aResult != SLVS_E_UNKNOWN) {
 351       Slvs_Entity anEnt = myStorage->getEntity(aResult);
 352       theType = anEnt.type;
 353       myAttributeMap[theEntity] = aResult;
 354       return aResult;
 355     }
 356   }
 357
 358   Slvs_hGroup aGroupID = myGroup->getId();
 359   // Point in 3D
 360   std::shared_ptr<GeomDataAPI_Point> aPoint =
 361       std::dynamic_pointer_cast<GeomDataAPI_Point>(theEntity);
 362   if (aPoint) {
 363     double aXYZ[3] = {aPoint->x(), aPoint->y(), aPoint->z()};
 364     Slvs_hParam aParams[3];
 365     for (int i = 0; i < 3; i++) {
 366       Slvs_Param aPar = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 367           myStorage->getParameter(aCurrentEntity.param[i]) :
 368           Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 369       aPar.val = aXYZ[i];
 370       aParams[i] = myStorage->addParameter(aPar);
 371     }
 372
 373     if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 374       aCurrentEntity = Slvs_MakePoint3d(SLVS_E_UNKNOWN, aGroupID, aParams[0], aParams[1], aParams[2]);
 375     else { // update entity data
 376       for (int i = 0; i < 3; i++)
 377         aCurrentEntity.param[i] = aParams[i];
 378     }
 379     aResult = myStorage->addEntity(aCurrentEntity);
 380   } else {
 381     // All entities except 3D points are created on workplane. So, if there is no workplane yet, then error
 382     Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
 383     if (aWorkplaneID == SLVS_E_UNKNOWN)
 384       return SLVS_E_UNKNOWN;
 385
 386     // Point in 2D
 387     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 388         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
 389     if (aPoint2D) {
 390       double aXY[2] = {aPoint2D->x(), aPoint2D->y()};
 391       Slvs_hParam aParams[2];
 392       for (int i = 0; i < 2; i++) {
 393         Slvs_Param aPar = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 394             myStorage->getParameter(aCurrentEntity.param[i]) :
 395             Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 396         aPar.val = aXY[i];
 397         aParams[i] = myStorage->addParameter(aPar);
 398       }
 399
 400       if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 401         aCurrentEntity = Slvs_MakePoint2d(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aParams[0], aParams[1]);
 402       else { // update entity data
 403         for (int i = 0; i < 2; i++)
 404           aCurrentEntity.param[i] = aParams[i];
 405       }
 406       aResult = myStorage->addEntity(aCurrentEntity);
 407     } else {
 408       // Scalar value (used for the distance entities)
 409       AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theEntity);
 410       if (aScalar) {
 411         Slvs_Param aParam = aCurrentEntity.h != SLVS_E_UNKNOWN ?
 412             myStorage->getParameter(aCurrentEntity.param[0]) :
 413             Slvs_MakeParam(SLVS_E_UNKNOWN, aGroupID, 0.0);
 414         aParam.val = aScalar->value();
 415         Slvs_hParam aValue = myStorage->addParameter(aParam);
 416
 417         if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 418           aCurrentEntity = Slvs_MakeDistance(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aValue);
 419         else
 420           aCurrentEntity.param[0] = aValue;
 421         aResult = myStorage->addEntity(aCurrentEntity);
 422       }
 423     }
 424   }
 425
 426   myAttributeMap[theEntity] = aResult;
 427   theType = aCurrentEntity.type;
 428   return aResult;
 429 }
 430
 431 Slvs_hEntity SketchSolver_Constraint::changeEntity(FeaturePtr theEntity, int& theType)
 432 {
 433   Slvs_hEntity aResult = SLVS_E_UNKNOWN;
 434   if (!theEntity || !theEntity->data() || !theEntity->data()->isValid())
 435     return SLVS_E_UNKNOWN;
 436   // If the entity is already in the group, try to find it
 437   std::map<FeaturePtr, Slvs_hEntity>::const_iterator anEntIter = myFeatureMap.find(theEntity);
 438   Slvs_Entity aCurrentEntity;
 439   aCurrentEntity.h = SLVS_E_UNKNOWN;
 440   if (anEntIter != myFeatureMap.end())
 441     aCurrentEntity = myStorage->getEntity(anEntIter->second);
 442   else {
 443     aResult = myGroup->getFeatureId(theEntity);
 444     if (aResult != SLVS_E_UNKNOWN) {
 445       Slvs_Entity anEnt = myStorage->getEntity(aResult);
 446       theType = anEnt.type;
 447       myFeatureMap[theEntity] = aResult;
 448       return aResult;
 449     }
 450   }
 451
 452   Slvs_hGroup aGroupID = myGroup->getId();
 453   Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
 454   DataPtr aData = theEntity->data();
 455
 456   // SketchPlugin features
 457   const std::string& aFeatureKind = theEntity->getKind();
 458   AttributePtr anAttribute;
 459   int anAttrType;
 460   // Line
 461   if (aFeatureKind == SketchPlugin_Line::ID()) {
 462     anAttribute = aData->attribute(SketchPlugin_Line::START_ID());
 463     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 464     Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
 465
 466     anAttribute = aData->attribute(SketchPlugin_Line::END_ID());
 467     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 468     Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
 469
 470     if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
 471       aCurrentEntity = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aStart, aEnd);
 472     else {
 473       aCurrentEntity.point[0] = aStart;
 474       aCurrentEntity.point[1] = aEnd;
 475     }
 476     aResult = myStorage->addEntity(aCurrentEntity);
 477   }
 478   // Circle
 479   else if (aFeatureKind == SketchPlugin_Circle::ID()) {
 480     anAttribute = aData->attribute(SketchPlugin_Circle::CENTER_ID());
 481     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 482     Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
 483
 484     anAttribute = aData->attribute(SketchPlugin_Circle::RADIUS_ID());
 485     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 486     Slvs_hEntity aRadius = changeEntity(anAttribute, anAttrType);
 487
 488     if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
 489       Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
 490       aCurrentEntity = Slvs_MakeCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
 491                                         aCenter, aWorkplane.normal, aRadius);
 492     } else {
 493       aCurrentEntity.point[0] = aCenter;
 494       aCurrentEntity.distance = aRadius;
 495     }
 496     aResult = myStorage->addEntity(aCurrentEntity);
 497   }
 498   // Arc
 499   else if (aFeatureKind == SketchPlugin_Arc::ID()) {
 500     anAttribute = aData->attribute(SketchPlugin_Arc::CENTER_ID());
 501     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 502     Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
 503
 504     anAttribute = aData->attribute(SketchPlugin_Arc::START_ID());
 505     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 506     Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
 507
 508     anAttribute = aData->attribute(SketchPlugin_Arc::END_ID());
 509     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 510     Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
 511
 512     if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
 513       Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
 514       aCurrentEntity = Slvs_MakeArcOfCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
 515                                             aWorkplane.normal, aCenter, aStart, aEnd);
 516     } else {
 517       aCurrentEntity.point[0] = aCenter;
 518       aCurrentEntity.point[1] = aStart;
 519       aCurrentEntity.point[2] = aEnd;
 520     }
 521     aResult = myStorage->addEntity(aCurrentEntity);
 522   }
 523   // Point (it has low probability to be an attribute of constraint, so it is checked at the end)
 524   else if (aFeatureKind == SketchPlugin_Point::ID()) {
 525     anAttribute = aData->attribute(SketchPlugin_Point::COORD_ID());
 526     if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
 527     // Both the sketch point and its attribute (coordinates) link to the same SolveSpace point identifier
 528     aResult = changeEntity(anAttribute, anAttrType);
 529     aCurrentEntity.type = SLVS_E_POINT_IN_3D;
 530   }
 531
 532   if (aResult != SLVS_E_UNKNOWN) {
 533     myFeatureMap[theEntity] = aResult;
 534     theType = aCurrentEntity.type;
 535   }
 536   return aResult;
 537 }
 538
 539 std::list<ConstraintPtr> SketchSolver_Constraint::constraints() const
 540 {
 541   std::list<ConstraintPtr> aConstraints;
 542   aConstraints.push_back(myBaseConstraint);
 543   return aConstraints;
 544 }
 545
 546 void SketchSolver_Constraint::refresh()
 547 {
 548   cleanErrorMsg();
 549   std::map<AttributePtr, Slvs_hEntity>::iterator anAttrIter = myAttributeMap.begin();
 550   for (; anAttrIter != myAttributeMap.end(); anAttrIter++) {
 551     std::shared_ptr<GeomDataAPI_Point> aPoint =
 552         std::dynamic_pointer_cast<GeomDataAPI_Point>(anAttrIter->first);
 553     if (aPoint) {
 554       Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 555       double aXYZ[3];
 556       for (int i = 0; i < 3; i++) {
 557         Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
 558         aXYZ[i] = aPar.val;
 559       }
 560       if (fabs(aPoint->x() - aXYZ[0]) > tolerance ||
 561           fabs(aPoint->y() - aXYZ[1]) > tolerance ||
 562           fabs(aPoint->z() - aXYZ[2]) > tolerance)
 563         aPoint->setValue(aXYZ[0], aXYZ[1], aXYZ[2]);
 564     } else {
 565       // Point in 2D
 566       std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 567           std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrIter->first);
 568       if (aPoint2D) {
 569         Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 570         double aXY[2];
 571         for (int i = 0; i < 2; i++) {
 572           Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
 573           aXY[i] = aPar.val;
 574         }
 575         if (fabs(aPoint2D->x() - aXY[0]) > tolerance ||
 576             fabs(aPoint2D->y() - aXY[1]) > tolerance)
 577           aPoint2D->setValue(aXY[0], aXY[1]);
 578       } else {
 579         // Scalar value (used for the distance entities)
 580         AttributeDoublePtr aScalar =
 581             std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
 582         if (aScalar) {
 583           Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
 584           Slvs_Param aPar = myStorage->getParameter(anEntity.param[0]);
 585           if (fabs(aScalar->value() - aPar.val) > tolerance)
 586             aScalar->setValue(aPar.val);
 587         }
 588       }
 589     }
 590   }
 591
 592   std::map<AttributePtr, Slvs_hParam>::iterator aValIter = myValueMap.begin();
 593   for (; aValIter != myValueMap.end(); aValIter++) {
 594     AttributeDoublePtr aScalar =
 595         std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
 596     if (aScalar) {
 597       Slvs_Param aPar = myStorage->getParameter(anAttrIter->second);
 598       aScalar->setValue(aPar.val);
 599     }
 600   }
 601 }
 602
 603 Slvs_hEntity SketchSolver_Constraint::getId(FeaturePtr theFeature) const
 604 {
 605   std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFIter = myFeatureMap.find(theFeature);
 606   if (aFIter == myFeatureMap.end())
 607     return SLVS_E_UNKNOWN;
 608   // check the Feature is really in the storage
 609   Slvs_Entity anEntity = myStorage->getEntity(aFIter->second);
 610   if (anEntity.h == SLVS_E_UNKNOWN) {
 611     // rebuild feature
 612     int aType;
 613     anEntity.h = const_cast<SketchSolver_Constraint*>(this)->changeEntity(aFIter->first, aType);
 614     const_cast<SketchSolver_Constraint*>(this)->myFeatureMap[theFeature] = anEntity.h;
 615   }
 616   return anEntity.h;
 617 }
 618
 619 Slvs_hEntity SketchSolver_Constraint::getId(AttributePtr theAttribute) const
 620 {
 621   std::map<AttributePtr, Slvs_hEntity>::const_iterator anAttrIter = myAttributeMap.find(theAttribute);
 622   if (anAttrIter == myAttributeMap.end())
 623     return SLVS_E_UNKNOWN;
 624   return anAttrIter->second;
 625 }
 626
 627 bool SketchSolver_Constraint::isInitialized(AttributePtr theAttribute)
 628 {
 629   if (theAttribute->isInitialized())
 630     return true;
 631   myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 632   return false;
 633 }
 634
 635
 636 void SketchSolver_Constraint::calculateMiddlePoint(
 637     const Slvs_Entity& theEntity, double theCoeff, double& theX, double& theY) const
 638 {
 639   if (theEntity.type == SLVS_E_LINE_SEGMENT) {
 640     double aStartEndXY[2][2];
 641     Slvs_Entity aPoint;
 642     for (int i = 0; i < 2; i++) {
 643       aPoint = myStorage->getEntity(theEntity.point[i]);
 644       for (int j = 0; j < 2; j++)
 645         aStartEndXY[i][j] = myStorage->getParameter(aPoint.param[j]).val;
 646     }
 647     theX = (1.0 - theCoeff) * aStartEndXY[0][0] + theCoeff * aStartEndXY[1][0];
 648     theY = (1.0 - theCoeff) * aStartEndXY[0][1] + theCoeff * aStartEndXY[1][1];
 649   } else if (theEntity.type == SLVS_E_ARC_OF_CIRCLE) {
 650     double anArcPoint[3][2];
 651     Slvs_Entity aPoint;
 652     for (int i = 0; i < 3; i++) {
 653       aPoint = myStorage->getEntity(theEntity.point[i]);
 654       for (int j = 0; j < 2; j++)
 655         anArcPoint[i][j] = myStorage->getParameter(aPoint.param[j]).val;
 656     }
 657     // project last point of arc on the arc
 658     double x = anArcPoint[1][0] - anArcPoint[0][0];
 659     double y = anArcPoint[1][1] - anArcPoint[0][1];
 660     double aRad = sqrt(x*x + y*y);
 661     x = anArcPoint[2][0] - anArcPoint[0][0];
 662     y = anArcPoint[2][1] - anArcPoint[0][1];
 663     double aNorm = sqrt(x*x + y*y);
 664     if (aNorm >= tolerance) {
 665       anArcPoint[2][0] = x * aRad / aNorm;
 666       anArcPoint[2][1] = y * aRad / aNorm;
 667     }
 668     anArcPoint[1][0] -= anArcPoint[0][0];
 669     anArcPoint[1][1] -= anArcPoint[0][1];
 670     if (theCoeff < tolerance) {
 671       theX = anArcPoint[0][0] + anArcPoint[1][0];
 672       theY = anArcPoint[0][1] + anArcPoint[1][1];
 673       return;
 674     } else if (1 - theCoeff < tolerance) {
 675       theX = anArcPoint[0][0] + anArcPoint[2][0];
 676       theY = anArcPoint[0][1] + anArcPoint[2][1];
 677       return;
 678     }
 679
 680     double xStart = anArcPoint[1][0] / aRad, xEnd = anArcPoint[2][0] / aRad;
 681     double yStart = anArcPoint[1][1] / aRad, yEnd = anArcPoint[2][1] / aRad;
 682     double aTanStart = abs(xStart) < tolerance ? yStart : yStart / xStart;
 683     double aTanEnd   = abs(xEnd) < tolerance   ? yEnd   : yEnd / xEnd;
 684     double aCotStart = abs(yStart) < tolerance ? xStart : xStart / yStart;
 685     double aCotEnd   = abs(yEnd) < tolerance   ? xEnd   : xEnd / yEnd;
 686     if (anArcPoint[1][0] * anArcPoint[2][0] < 0.0) {
 687       if (anArcPoint[1][0] > 0.0)
 688         yEnd = 2.0 - yEnd;
 689       else
 690         yStart = -2.0 - yStart;
 691     } else {
 692       if (aTanStart > aTanEnd) {
 693         if (yStart > yEnd) {
 694           yStart = 2.0 - yStart;
 695           yEnd = -2.0 - yEnd;
 696         } else {
 697           yStart = -2.0 - yStart;
 698           yEnd = 2.0 - yEnd;
 699         }
 700       }
 701     }
 702     if (anArcPoint[1][1] * anArcPoint[2][1] < 0.0) {
 703       if (anArcPoint[1][1] > 0.0)
 704         xEnd = 2.0 - xEnd;
 705       else
 706         xStart = -2.0 - xStart;
 707     } else {
 708       if (aCotStart < aCotEnd) {
 709         if (xStart > xEnd) {
 710           xStart = 2.0 - xStart;
 711           xEnd = -2.0 - xEnd;
 712         } else {
 713           xStart = -2.0 - xStart;
 714           xEnd = 2.0 - xEnd;
 715         }
 716       }
 717     }
 718     x = (1.0 - theCoeff) * xStart + theCoeff * xEnd;
 719     y = (1.0 - theCoeff) * yStart + theCoeff * yEnd;
 720     if (x > 1.0) x = 2.0 - x;
 721     if (x < -1.0) x = -2.0 - x;
 722     if (y > 1.0) y = 2.0 - y;
 723     if (y < -1.0) y = -2.0 - y;
 724
 725     aNorm = sqrt(x*x + y*y);
 726     if (aNorm >= tolerance) {
 727       x *= aRad / aNorm;
 728       y *= aRad / aNorm;
 729     } else {
 730       x = -0.5 * (anArcPoint[2][1] + anArcPoint[1][1]);
 731       y = -0.5 * (anArcPoint[2][0] + anArcPoint[1][0]);
 732     }
 733     theX = anArcPoint[0][0] + x;
 734     theY = anArcPoint[0][1] + y;
 735   }
 736 }
 737
 738 void SketchSolver_Constraint::makeTemporary() const
 739 {
 740   std::vector<Slvs_hConstraint>::const_iterator anIt = mySlvsConstraints.begin();
 741   for (; anIt != mySlvsConstraints.end(); anIt++)
 742     myStorage->addTemporaryConstraint(*anIt);
 743 }
 744