src/SketchSolver/SketchSolver_Constraint.cpp

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