src/SketchSolver/SketchSolver_ConstraintManager.cpp

   1 // File:    SketchSolver_ConstraintManager.cpp
   2 // Created: 08 May 2014
   3 // Author:  Artem ZHIDKOV
   4
   5 #include "SketchSolver_ConstraintManager.h"
   6
   7 #include <Events_Loop.h>
   8 #include <GeomDataAPI_Dir.h>
   9 #include <GeomDataAPI_Point.h>
  10 #include <GeomDataAPI_Point2D.h>
  11 #include <ModelAPI_AttributeDouble.h>
  12 #include <ModelAPI_AttributeRefList.h>
  13 #include <ModelAPI_Data.h>
  14 #include <Model_Events.h>
  15 #include <SketchPlugin_Constraint.h>
  16 #include <SketchPlugin_ConstraintCoincidence.h>
  17 #include <SketchPlugin_Line.h>
  18 #include <SketchPlugin_Point.h>
  19 #include <SketchPlugin_Sketch.h>
  20
  21 #include <math.h>
  22 #include <assert.h>
  23
  24 /// Tolerance for value of parameters
  25 const double tolerance = 1.e-10;
  26
  27 // Initialization of constraint manager self pointer
  28 SketchSolver_ConstraintManager* SketchSolver_ConstraintManager::_self = 0;
  29
  30 /// Global constraint manager object
  31 SketchSolver_ConstraintManager* myManager = SketchSolver_ConstraintManager::Instance();
  32
  33 /// This value is used to give unique index to the groups
  34 static Slvs_hGroup myGroupIndexer = 0;
  35
  36 /** \brief Search the entity/parameter with specified ID in the list of elements
  37  *  \param[in] theEntityID unique ID of the element
  38  *  \param[in] theEntities list of elements
  39  *  \return position of the found element or -1 if the element is not found
  40  */
  41 template <typename T>
  42 static int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities);
  43
  44
  45
  46 // ========================================================
  47 // ========= SketchSolver_ConstraintManager ===============
  48 // ========================================================
  49 SketchSolver_ConstraintManager* SketchSolver_ConstraintManager::Instance()
  50 {
  51   if (!_self)
  52     _self = new SketchSolver_ConstraintManager();
  53   return _self;
  54 }
  55
  56 SketchSolver_ConstraintManager::SketchSolver_ConstraintManager()
  57 {
  58   myGroups.clear();
  59
  60   // Register in event loop
  61   Events_Loop::loop()->registerListener(this, Events_Loop::eventByName(EVENT_FEATURE_CREATED));
  62   Events_Loop::loop()->registerListener(this, Events_Loop::eventByName(EVENT_FEATURE_UPDATED));
  63   Events_Loop::loop()->registerListener(this, Events_Loop::eventByName(EVENT_FEATURE_DELETED));
  64   Events_Loop::loop()->registerListener(this, Events_Loop::eventByName(EVENT_FEATURES_MOVED));
  65 }
  66
  67 SketchSolver_ConstraintManager::~SketchSolver_ConstraintManager()
  68 {
  69   myGroups.clear();
  70 }
  71
  72 void SketchSolver_ConstraintManager::processEvent(const Events_Message* theMessage)
  73 {
  74   if (theMessage->eventID() == Events_Loop::loop()->eventByName(EVENT_FEATURE_CREATED) ||
  75       theMessage->eventID() == Events_Loop::loop()->eventByName(EVENT_FEATURE_UPDATED))
  76   {
  77     const Model_FeatureUpdatedMessage* aUpdateMsg = dynamic_cast<const Model_FeatureUpdatedMessage*>(theMessage);
  78
  79     // Only sketches and constraints can be added by Create event
  80     const std::string& aFeatureKind = aUpdateMsg->feature()->getKind();
  81     if (aFeatureKind.compare("Sketch") == 0)
  82     {
  83       boost::shared_ptr<SketchPlugin_Feature> aSketch =
  84         boost::dynamic_pointer_cast<SketchPlugin_Feature>(aUpdateMsg->feature());
  85       changeWorkplane(aSketch);
  86       return ;
  87     }
  88     boost::shared_ptr<SketchPlugin_Constraint> aConstraint =
  89       boost::dynamic_pointer_cast<SketchPlugin_Constraint>(aUpdateMsg->feature());
  90     if (aConstraint)
  91       changeConstraint(aConstraint);
  92     else
  93     {
  94       // Sketch plugin features can be only updated
  95       boost::shared_ptr<SketchPlugin_Feature> aFeature =
  96         boost::dynamic_pointer_cast<SketchPlugin_Feature>(aUpdateMsg->feature());
  97       if (aFeature)
  98         updateEntity(aFeature);
  99     }
 100
 101     // Solve the set of constraints
 102     resolveConstraints();
 103   }
 104   else if (theMessage->eventID() == Events_Loop::loop()->eventByName(EVENT_FEATURE_DELETED))
 105   {
 106     const Model_FeatureDeletedMessage* aDeleteMsg = dynamic_cast<const Model_FeatureDeletedMessage*>(theMessage);
 107
 108     if (aDeleteMsg->group().compare("Sketch") == 0)
 109     {
 110       std::vector<SketchSolver_ConstraintGroup*>::iterator aGroupIter = myGroups.begin();
 111       while (aGroupIter != myGroups.end())
 112       {
 113         if ((*aGroupIter)->updateGroup())
 114         { // the group should be removed
 115           delete *aGroupIter;
 116           int aShift = aGroupIter - myGroups.begin();
 117           myGroups.erase(aGroupIter);
 118           aGroupIter = myGroups.begin() + aShift;
 119         }
 120         else aGroupIter++;
 121       }
 122     }
 123   }
 124 }
 125
 126 bool SketchSolver_ConstraintManager::changeWorkplane(boost::shared_ptr<SketchPlugin_Feature> theSketch)
 127 {
 128   bool aResult = true; // changed when a workplane wrongly updated
 129   bool isUpdated = false;
 130   // Try to update specified workplane in all groups
 131   std::vector<SketchSolver_ConstraintGroup*>::iterator aGroupIter;
 132   for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 133     if ((*aGroupIter)->isBaseWorkplane(theSketch))
 134     {
 135       isUpdated = true;
 136       if (!(*aGroupIter)->updateWorkplane())
 137         aResult = false;
 138     }
 139   // If the workplane is not updated, so this is a new workplane
 140   if (!isUpdated)
 141   {
 142     SketchSolver_ConstraintGroup* aNewGroup = new SketchSolver_ConstraintGroup(theSketch);
 143     // Verify that the group is created successfully
 144     if (!aNewGroup->isBaseWorkplane(theSketch))
 145     {
 146       delete aNewGroup;
 147       return false;
 148     }
 149     myGroups.push_back(aNewGroup);
 150   }
 151   return aResult;
 152 }
 153
 154 bool SketchSolver_ConstraintManager::changeConstraint(
 155               boost::shared_ptr<SketchPlugin_Constraint> theConstraint)
 156 {
 157   // Search the groups which this constraint touchs
 158   std::vector<Slvs_hGroup> aGroups;
 159   findGroups(theConstraint, aGroups);
 160
 161   // Process the groups list
 162   if (aGroups.size() == 0)
 163   { // There are no groups applicable for this constraint => create new one
 164     boost::shared_ptr<SketchPlugin_Feature> aWP = findWorkplaneForConstraint(theConstraint);
 165     if (!aWP) return false;
 166     SketchSolver_ConstraintGroup* aGroup = new SketchSolver_ConstraintGroup(aWP);
 167     if (!aGroup->changeConstraint(theConstraint))
 168     {
 169       delete aGroup;
 170       return false;
 171     }
 172     myGroups.push_back(aGroup);
 173     return true;
 174   }
 175   else if (aGroups.size() == 1)
 176   { // Only one group => add constraint into it
 177     Slvs_hGroup aGroupId = *(aGroups.begin());
 178     std::vector<SketchSolver_ConstraintGroup*>::iterator aGroupIter;
 179     for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 180       if ((*aGroupIter)->getId() == aGroupId)
 181         return (*aGroupIter)->changeConstraint(theConstraint);
 182   }
 183   else if (aGroups.size() > 1)
 184   { // Several groups applicable for this constraint => need to merge them
 185     /// \todo Implement merging of groups
 186   }
 187
 188   // Something goes wrong
 189   return false;
 190 }
 191
 192 void SketchSolver_ConstraintManager::updateEntity(boost::shared_ptr<SketchPlugin_Feature> theFeature)
 193 {
 194   // Create list of attributes depending on type of the feature
 195   std::vector<std::string> anAttrList;
 196   const std::string& aFeatureKind = theFeature->getKind();
 197   // Point
 198   if (aFeatureKind.compare("SketchPoint") == 0)
 199     anAttrList.push_back(POINT_ATTR_COORD);
 200   // Line
 201   else if (aFeatureKind.compare("SketchLine") == 0)
 202   {
 203     anAttrList.push_back(LINE_ATTR_START);
 204     anAttrList.push_back(LINE_ATTR_END);
 205   }
 206   /// \todo Other types of features should be implemented
 207
 208   // Check changing of feature's attributes (go through the groups and search usage of the attributes)
 209   std::vector<std::string>::const_iterator anAttrIter;
 210   for (anAttrIter = anAttrList.begin(); anAttrIter != anAttrList.end(); anAttrIter++)
 211   {
 212     std::vector<SketchSolver_ConstraintGroup*>::iterator aGroupIter;
 213     for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 214     {
 215       boost::shared_ptr<ModelAPI_Attribute> anAttribute =
 216         boost::dynamic_pointer_cast<ModelAPI_Attribute>(theFeature->data()->attribute(*anAttrIter));
 217       (*aGroupIter)->updateEntityIfPossible(anAttribute);
 218     }
 219   }
 220 }
 221
 222
 223 void SketchSolver_ConstraintManager::findGroups(
 224               boost::shared_ptr<SketchPlugin_Constraint> theConstraint,
 225               std::vector<Slvs_hGroup>&                  theGroupIDs) const
 226 {
 227   boost::shared_ptr<SketchPlugin_Feature> aWP = findWorkplaneForConstraint(theConstraint);
 228
 229   std::vector<SketchSolver_ConstraintGroup*>::const_iterator aGroupIter;
 230   for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 231     if (aWP == (*aGroupIter)->getWorkplane() && (*aGroupIter)->isInteract(theConstraint))
 232       theGroupIDs.push_back((*aGroupIter)->getId());
 233 }
 234
 235 boost::shared_ptr<SketchPlugin_Feature> SketchSolver_ConstraintManager::findWorkplaneForConstraint(
 236               boost::shared_ptr<SketchPlugin_Constraint> theConstraint) const
 237 {
 238   std::vector<SketchSolver_ConstraintGroup*>::const_iterator aGroupIter;
 239   for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 240   {
 241     boost::shared_ptr<SketchPlugin_Feature> aWP = (*aGroupIter)->getWorkplane();
 242     boost::shared_ptr<ModelAPI_AttributeRefList> aWPFeatures =
 243       boost::dynamic_pointer_cast<ModelAPI_AttributeRefList>(aWP->data()->attribute(SKETCH_ATTR_FEATURES));
 244     std::list< boost::shared_ptr<ModelAPI_Feature> > aFeaturesList = aWPFeatures->list();
 245     std::list< boost::shared_ptr<ModelAPI_Feature> >::const_iterator anIter;
 246     for (anIter = aFeaturesList.begin(); anIter != aFeaturesList.end(); anIter++)
 247       if (*anIter == theConstraint)
 248         return aWP; // workplane is found
 249   }
 250
 251   return boost::shared_ptr<SketchPlugin_Feature>();
 252 }
 253
 254 void SketchSolver_ConstraintManager::resolveConstraints()
 255 {
 256   std::vector<SketchSolver_ConstraintGroup*>::iterator aGroupIter;
 257   for (aGroupIter = myGroups.begin(); aGroupIter != myGroups.end(); aGroupIter++)
 258     (*aGroupIter)->resolveConstraints();
 259 }
 260
 261
 262
 263 // ========================================================
 264 // =========  SketchSolver_ConstraintGroup  ===============
 265 // ========================================================
 266
 267 SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::
 268   SketchSolver_ConstraintGroup(boost::shared_ptr<SketchPlugin_Feature> theWorkplane)
 269   : myID(++myGroupIndexer),
 270     myParamMaxID(0),
 271     myEntityMaxID(0),
 272     myConstrMaxID(0),
 273     myConstraintMap(),
 274     myNeedToSolve(false),
 275     myConstrSolver()
 276 {
 277   myParams.clear();
 278   myEntities.clear();
 279   myConstraints.clear();
 280
 281   // Initialize workplane
 282   myWorkplane.h = SLVS_E_UNKNOWN;
 283   assert(addWorkplane(theWorkplane));
 284 }
 285
 286 SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::~SketchSolver_ConstraintGroup()
 287 {
 288   myParams.clear();
 289   myEntities.clear();
 290   myConstraints.clear();
 291   myConstraintMap.clear();
 292
 293   // If the group with maximal identifier is deleted, decrease the indexer
 294   if (myID == myGroupIndexer)
 295     myGroupIndexer--;
 296 }
 297
 298 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::isBaseWorkplane(
 299                 boost::shared_ptr<SketchPlugin_Feature> theWorkplane) const
 300 {
 301   return theWorkplane == mySketch;
 302 }
 303
 304 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::isInteract(
 305                 boost::shared_ptr<SketchPlugin_Constraint> theConstraint) const
 306 {
 307   // Check the group is empty
 308   if (myWorkplane.h != SLVS_E_UNKNOWN && myConstraints.empty())
 309     return true;
 310
 311   /// \todo Should be implemented
 312   return false;
 313 }
 314
 315 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::changeConstraint(
 316                 boost::shared_ptr<SketchPlugin_Constraint> theConstraint)
 317 {
 318   // There is no workplane yet, something wrong
 319   if (myWorkplane.h == SLVS_E_UNKNOWN)
 320     return false;
 321
 322   // Search this constraint in the current group to update it
 323   std::map<boost::shared_ptr<SketchPlugin_Constraint>, Slvs_hConstraint>::const_iterator
 324     aConstrMapIter = myConstraintMap.find(theConstraint);
 325   std::vector<Slvs_Constraint>::iterator aConstrIter;
 326   if (aConstrMapIter != myConstraintMap.end())
 327   {
 328     int aConstrPos = Search(aConstrMapIter->second, myConstraints);
 329     aConstrIter = myConstraints.begin() + aConstrPos;
 330   }
 331
 332   // Get constraint type and verify the constraint parameters are correct
 333   int aConstrType = getConstraintType(theConstraint);
 334   if (aConstrType == SLVS_C_UNKNOWN ||
 335      (aConstrMapIter != myConstraintMap.end() && aConstrIter->type != aConstrType))
 336     return false;
 337
 338   // Create constraint parameters
 339   double aDistance = 0.0; // scalar value of the constraint
 340   boost::shared_ptr<ModelAPI_AttributeDouble> aDistAttr =
 341     boost::dynamic_pointer_cast<ModelAPI_AttributeDouble>(theConstraint->data()->attribute(CONSTRAINT_ATTR_VALUE));
 342   if (aDistAttr)
 343   {
 344     aDistance = aDistAttr->value();
 345     if (aConstrMapIter != myConstraintMap.end() && fabs(aConstrIter->valA - aDistance) > tolerance)
 346     {
 347       myNeedToSolve = true;
 348       aConstrIter->valA = aDistance;
 349     }
 350   }
 351
 352   Slvs_hEntity aConstrEnt[CONSTRAINT_ATTR_SIZE]; // parameters of the constraint
 353   for (unsigned int indAttr = 0; indAttr < CONSTRAINT_ATTR_SIZE; indAttr++)
 354   {
 355     aConstrEnt[indAttr] = SLVS_E_UNKNOWN;
 356     boost::shared_ptr<ModelAPI_AttributeRefAttr> aConstrAttr =
 357       boost::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 358         theConstraint->data()->attribute(CONSTRAINT_ATTRIBUTES[indAttr])
 359       );
 360     if (!aConstrAttr) continue;
 361     aConstrEnt[indAttr] = changeEntity(aConstrAttr->attr());
 362   }
 363
 364   if (aConstrMapIter == myConstraintMap.end())
 365   {
 366     // Create SolveSpace constraint structure
 367     Slvs_Constraint aConstraint =
 368       Slvs_MakeConstraint(++myConstrMaxID, myID, aConstrType, myWorkplane.h,
 369                           aDistance, aConstrEnt[0], aConstrEnt[1], aConstrEnt[2], aConstrEnt[3]);
 370     myConstraints.push_back(aConstraint);
 371     myConstraintMap[theConstraint] = aConstraint.h;
 372   }
 373   return true;
 374 }
 375
 376 Slvs_hEntity SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::changeEntity(
 377                 boost::shared_ptr<ModelAPI_Attribute> theEntity)
 378 {
 379   // If the entity is already in the group, try to find it
 380   std::map<boost::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator
 381     aEntIter = myEntityMap.find(theEntity);
 382   std::vector<Slvs_Param>::const_iterator aParamIter; // looks at first parameter of already existent entity or at the end of vector otherwise
 383   if (aEntIter == myEntityMap.end()) // no such entity => should be created
 384     aParamIter = myParams.end();
 385   else
 386   { // the entity already exists
 387     int aEntPos = Search(aEntIter->second, myEntities);
 388     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
 389     aParamIter = myParams.begin() + aParamPos;
 390   }
 391
 392   // Look over supported types of entities
 393
 394   // Point in 3D
 395   boost::shared_ptr<GeomDataAPI_Point> aPoint =
 396     boost::dynamic_pointer_cast<GeomDataAPI_Point>(theEntity);
 397   if (aPoint)
 398   {
 399     Slvs_hParam aX = changeParameter(aPoint->x(), aParamIter);
 400     Slvs_hParam aY = changeParameter(aPoint->y(), aParamIter);
 401     Slvs_hParam aZ = changeParameter(aPoint->z(), aParamIter);
 402
 403     if (aEntIter != myEntityMap.end()) // the entity already exists
 404       return aEntIter->second;
 405
 406     // New entity
 407     Slvs_Entity aPtEntity = Slvs_MakePoint3d(++myEntityMaxID, myID, aX, aY, aZ);
 408     myEntities.push_back(aPtEntity);
 409     myEntityMap[theEntity] = aPtEntity.h;
 410     return aPtEntity.h;
 411   }
 412
 413   // Point in 2D
 414   boost::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 415     boost::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
 416   if (aPoint2D)
 417   {
 418     // The 2D points are created on workplane. So, if there is no workplane yet, then error
 419     if (myWorkplane.h == SLVS_E_UNKNOWN)
 420       return SLVS_E_UNKNOWN;
 421     Slvs_hParam aU = changeParameter(aPoint2D->x(), aParamIter);
 422     Slvs_hParam aV = changeParameter(aPoint2D->y(), aParamIter);
 423
 424     if (aEntIter != myEntityMap.end()) // the entity already exists
 425       return aEntIter->second;
 426
 427     // New entity
 428     Slvs_Entity aPt2DEntity = Slvs_MakePoint2d(++myEntityMaxID, myID, myWorkplane.h, aU, aV);
 429     myEntities.push_back(aPt2DEntity);
 430     myEntityMap[theEntity] = aPt2DEntity.h;
 431     return aPt2DEntity.h;
 432   }
 433
 434   /// \todo Other types of entities
 435
 436   // Unsupported or wrong entity type
 437   return SLVS_E_UNKNOWN;
 438 }
 439
 440 Slvs_hEntity SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::changeNormal(
 441                 boost::shared_ptr<ModelAPI_Attribute> theDirX,
 442                 boost::shared_ptr<ModelAPI_Attribute> theDirY,
 443                 boost::shared_ptr<ModelAPI_Attribute> theNorm)
 444 {
 445   boost::shared_ptr<GeomDataAPI_Dir> aDirX =
 446     boost::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirX);
 447   boost::shared_ptr<GeomDataAPI_Dir> aDirY =
 448     boost::dynamic_pointer_cast<GeomDataAPI_Dir>(theDirY);
 449   if (!aDirX || !aDirY ||
 450      (fabs(aDirX->x()) + fabs(aDirX->y()) + fabs(aDirX->z()) < tolerance) ||
 451      (fabs(aDirY->x()) + fabs(aDirY->y()) + fabs(aDirY->z()) < tolerance))
 452     return SLVS_E_UNKNOWN;
 453
 454   // quaternion parameters of normal vector
 455   double qw, qx, qy, qz;
 456   Slvs_MakeQuaternion(aDirX->x(), aDirX->y(), aDirX->z(),
 457                       aDirY->x(), aDirY->y(), aDirY->z(),
 458                       &qw, &qx, &qy, &qz);
 459   double aNormCoord[4] = {qw, qx, qy, qz};
 460
 461   // Try to find existent normal
 462   std::map<boost::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator
 463     aEntIter = myEntityMap.find(theNorm);
 464   std::vector<Slvs_Param>::const_iterator aParamIter; // looks at first parameter of already existent entity or at the end of vector otherwise
 465   if (aEntIter == myEntityMap.end()) // no such entity => should be created
 466     aParamIter = myParams.end();
 467   else
 468   { // the entity already exists, update it
 469     int aEntPos = Search(aEntIter->second, myEntities);
 470     int aParamPos = Search(myEntities[aEntPos].param[0], myParams);
 471     aParamIter = myParams.begin() + aParamPos;
 472   }
 473
 474   // Change parameters of the normal
 475   Slvs_hParam aNormParams[4];
 476   for (int i = 0; i < 4; i++)
 477     aNormParams[i] = changeParameter(aNormCoord[i], aParamIter);
 478
 479   if (aEntIter != myEntityMap.end()) // the entity already exists
 480     return aEntIter->second;
 481
 482   // Create a normal
 483   Slvs_Entity aNormal = Slvs_MakeNormal3d(++myEntityMaxID, myID,
 484                 aNormParams[0], aNormParams[1], aNormParams[2], aNormParams[3]);
 485   myEntities.push_back(aNormal);
 486   myEntityMap[theNorm] = aNormal.h;
 487   return aNormal.h;
 488 }
 489
 490
 491 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::addWorkplane(
 492                 boost::shared_ptr<SketchPlugin_Feature> theSketch)
 493 {
 494   if (myWorkplane.h || theSketch->getKind().compare("Sketch") != 0)
 495     return false; // the workplane already exists or the function parameter is not Sketch
 496
 497   mySketch = theSketch;
 498   updateWorkplane();
 499   return true;
 500 }
 501
 502 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::updateWorkplane()
 503 {
 504   // Get parameters of workplane
 505   boost::shared_ptr<ModelAPI_Attribute> aDirX    = mySketch->data()->attribute(SKETCH_ATTR_DIRX);
 506   boost::shared_ptr<ModelAPI_Attribute> aDirY    = mySketch->data()->attribute(SKETCH_ATTR_DIRY);
 507   boost::shared_ptr<ModelAPI_Attribute> aNorm    = mySketch->data()->attribute(SKETCH_ATTR_NORM);
 508   boost::shared_ptr<ModelAPI_Attribute> anOrigin = mySketch->data()->attribute(SKETCH_ATTR_ORIGIN);
 509   // Transform them into SolveSpace format
 510   Slvs_hEntity aNormalWP = changeNormal(aDirX, aDirY, aNorm);
 511   if (!aNormalWP) return false;
 512   Slvs_hEntity anOriginWP = changeEntity(anOrigin);
 513   if (!anOriginWP) return false;
 514
 515   if (!myWorkplane.h)
 516   {
 517     // Create workplane
 518     myWorkplane = Slvs_MakeWorkplane(++myEntityMaxID, myID, anOriginWP, aNormalWP);
 519     // Workplane should be added to the list of entities
 520     myEntities.push_back(myWorkplane);
 521   }
 522   return true;
 523 }
 524
 525
 526 Slvs_hParam SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::changeParameter(
 527                 const double&                            theParam,
 528                 std::vector<Slvs_Param>::const_iterator& thePrmIter)
 529 {
 530   if (thePrmIter != myParams.end())
 531   { // Parameter should be updated
 532     int aParamPos = thePrmIter - myParams.begin();
 533     if (fabs(thePrmIter->val - theParam) > tolerance)
 534     {
 535       myNeedToSolve = true; // parameter is changed, need to resolve constraints
 536       myParams[aParamPos].val = theParam;
 537     }
 538     thePrmIter++;
 539     return myParams[aParamPos].h;
 540   }
 541
 542   // Newly created parameter
 543   Slvs_Param aParam = Slvs_MakeParam(++myParamMaxID, myID, theParam);
 544   myParams.push_back(aParam);
 545   myNeedToSolve = true;
 546   // The list of parameters is changed, move iterator to the end of the list to avoid problems
 547   thePrmIter = myParams.end();
 548   return aParam.h;
 549 }
 550
 551 int SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::getConstraintType(
 552                 const boost::shared_ptr<SketchPlugin_Constraint>& theConstraint) const
 553 {
 554   const std::string& aConstraintKind = theConstraint->getKind();
 555   // Constraint for coincidence of two points
 556   if (aConstraintKind.compare("SketchConstraintCoincidence") == 0)
 557   {
 558     // Verify the constraint has only two attributes and they are points
 559     int aPt2d = 0; // bit-mapped field, each bit indicates whether the attribute is 2D point
 560     int aPt3d = 0; // bit-mapped field, the same information for 3D points
 561     for (unsigned int indAttr = 0; indAttr < CONSTRAINT_ATTR_SIZE; indAttr++)
 562     {
 563       boost::shared_ptr<ModelAPI_AttributeRefAttr> anAttr =
 564         boost::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 565           theConstraint->data()->attribute(CONSTRAINT_ATTRIBUTES[indAttr])
 566         );
 567       if (!anAttr) continue;
 568       // Verify the attribute is a 2D point
 569       boost::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 570         boost::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttr->attr());
 571       if (aPoint2D)
 572       {
 573         aPt2d |= (1 << indAttr);
 574         continue;
 575       }
 576       // Verify the attribute is a 3D point
 577       boost::shared_ptr<GeomDataAPI_Point> aPoint3D =
 578         boost::dynamic_pointer_cast<GeomDataAPI_Point>(anAttr->attr());
 579       if (aPoint3D)
 580       {
 581         aPt3d |= (1 << indAttr);
 582         continue;
 583       }
 584       // Attribute neither 2D nor 3D point is not supported by this type of constraint
 585       return SLVS_C_UNKNOWN;
 586     }
 587     // The constrained points should be in first and second positions,
 588     // so the expected value of aPt2d or aPt3d is 3
 589     if ((aPt2d == 3 && aPt3d == 0) || (aPt2d == 0 && aPt3d == 3))
 590       return SLVS_C_POINTS_COINCIDENT;
 591     // Constraint parameters are wrong
 592     return SLVS_C_UNKNOWN;
 593   }
 594
 595   /// \todo Implement other kind of constrtaints
 596
 597   return SLVS_C_UNKNOWN;
 598 }
 599
 600 void SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::resolveConstraints()
 601 {
 602   if (!myNeedToSolve)
 603     return;
 604
 605   myConstrSolver.setGroupID(myID);
 606   myConstrSolver.setParameters(myParams);
 607   myConstrSolver.setEntities(myEntities);
 608   myConstrSolver.setConstraints(myConstraints);
 609
 610   if (myConstrSolver.solve() == SLVS_RESULT_OKAY)
 611   { // solution succeeded, store results into correspondent attributes
 612     // Obtain result into the same list of parameters
 613     if (!myConstrSolver.getResult(myParams))
 614       return;
 615
 616     std::map<boost::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
 617       anEntIter = myEntityMap.begin();
 618     for ( ; anEntIter != myEntityMap.end(); anEntIter++)
 619       updateAttribute(anEntIter->first, anEntIter->second);
 620   }
 621   /// \todo Implement error handling
 622
 623   removeTemporaryConstraints();
 624   myNeedToSolve = false;
 625 }
 626
 627 bool SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::updateGroup()
 628 {
 629   // Check for valid sketch
 630   if (!mySketch->data()->isValid())
 631     return true;
 632
 633   // Fast check for constraint validity. If all constraints are valid, no need to update the group
 634   std::map<boost::shared_ptr<SketchPlugin_Constraint>, Slvs_hConstraint>::reverse_iterator
 635     aConstrIter = myConstraintMap.rbegin();
 636   bool isAllValid = true;
 637   for ( ; isAllValid && aConstrIter != myConstraintMap.rend(); aConstrIter++)
 638     if (!aConstrIter->first->data()->isValid())
 639       isAllValid = false;
 640   if (isAllValid)
 641     return false;
 642
 643   // Remove invalid constraints.
 644   // There only constraint will be deleted (parameters and entities) will be removed below
 645   std::list< boost::shared_ptr<SketchPlugin_Constraint> > aConstrToDelete;
 646   std::map<Slvs_hEntity, bool> anEntToDelete; // entities will be removed if no valid constraints use them
 647   for (aConstrIter = myConstraintMap.rbegin(); aConstrIter != myConstraintMap.rend(); aConstrIter++)
 648   {
 649     bool isValid = aConstrIter->first->data()->isValid();
 650
 651     int aConstrPos = Search(aConstrIter->second, myConstraints);
 652     if (aConstrPos < (int)myConstraints.size())
 653     {
 654       Slvs_hEntity aConstrEnt[] = {
 655         myConstraints[aConstrPos].ptA,     myConstraints[aConstrPos].ptB,
 656         myConstraints[aConstrPos].entityA, myConstraints[aConstrPos].entityB};
 657       for (int i = 0; i < 4; i++)
 658         if (aConstrEnt[i] != SLVS_E_UNKNOWN)
 659         {
 660           if (anEntToDelete.find(aConstrEnt[i]) == anEntToDelete.end())
 661             anEntToDelete[aConstrEnt[i]] = !isValid;
 662           else if (isValid) // constraint is valid => no need to remove its entities
 663             anEntToDelete[aConstrEnt[i]] = false;
 664         }
 665       if (!isValid)
 666       {
 667         myConstraints.erase(myConstraints.begin() + aConstrPos);
 668         if (aConstrIter->second == myConstrMaxID) // When the constraint with highest ID is removed, decrease indexer
 669           myConstrMaxID--;
 670         aConstrToDelete.push_front(aConstrIter->first);
 671       }
 672     }
 673   }
 674   std::list< boost::shared_ptr<SketchPlugin_Constraint> >::iterator aDelIter;
 675   for (aDelIter = aConstrToDelete.begin(); aDelIter != aConstrToDelete.end(); aDelIter++)
 676     myConstraintMap.erase(*aDelIter);
 677
 678   // Remove invalid and unused entities
 679   std::map<Slvs_hEntity, bool>::reverse_iterator aEDelIter;
 680   for (aEDelIter = anEntToDelete.rbegin(); aEDelIter != anEntToDelete.rend(); aEDelIter++)
 681     if (aEDelIter->second)
 682     {
 683       int anEntPos = Search(aEDelIter->first, myEntities);
 684       std::vector<Slvs_Entity>::iterator aEntIter = myEntities.begin() + anEntPos;
 685       // Number of parameters for the entity
 686       int aNbParams = 0;
 687       while (aEntIter->param[aNbParams]) aNbParams++;
 688       if (aNbParams == 0) continue;
 689       // Decrease parameter indexer if there are deleted parameter with higher IDs
 690       if (aEntIter->param[aNbParams-1] == myParamMaxID)
 691         myParamMaxID -= aNbParams;
 692       // Remove parameters of the entity
 693       int aParamPos = Search(aEntIter->param[0], myParams);
 694       myParams.erase(myParams.begin() + aParamPos,
 695                      myParams.begin() + aParamPos + aNbParams);
 696
 697       // Remove entity
 698       if (aEDelIter->first == myEntityMaxID)
 699         myEntityMaxID--;
 700       myEntities.erase(myEntities.begin() + anEntPos);
 701       // Remove such entity from myEntityMap
 702       std::map<boost::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::iterator
 703         anEntMapIter = myEntityMap.begin();
 704       for ( ; anEntMapIter != myEntityMap.end(); anEntMapIter++)
 705         if (anEntMapIter->second == aEDelIter->first)
 706           break;
 707       if (anEntMapIter != myEntityMap.end())
 708         myEntityMap.erase(anEntMapIter);
 709     }
 710
 711   return false;
 712 }
 713
 714 void SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::updateAttribute(
 715                 boost::shared_ptr<ModelAPI_Attribute> theAttribute,
 716                 const Slvs_hEntity&                   theEntityID)
 717 {
 718   // Search the position of the first parameter of the entity
 719   int anEntPos = Search(theEntityID, myEntities);
 720   int aFirstParamPos = Search(myEntities[anEntPos].param[0], myParams);
 721
 722   // Look over supported types of entities
 723
 724   // Point in 3D
 725   boost::shared_ptr<GeomDataAPI_Point> aPoint =
 726     boost::dynamic_pointer_cast<GeomDataAPI_Point>(theAttribute);
 727   if (aPoint)
 728   {
 729     aPoint->setValue(myParams[aFirstParamPos].val,
 730                      myParams[aFirstParamPos+1].val,
 731                      myParams[aFirstParamPos+2].val);
 732     return ;
 733   }
 734
 735   // Point in 2D
 736   boost::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 737     boost::dynamic_pointer_cast<GeomDataAPI_Point2D>(theAttribute);
 738   if (aPoint2D)
 739   {
 740     aPoint2D->setValue(myParams[aFirstParamPos].val,
 741                        myParams[aFirstParamPos+1].val);
 742     return ;
 743   }
 744
 745   /// \todo Support other types of entities
 746 }
 747
 748 void SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::updateEntityIfPossible(
 749                 boost::shared_ptr<ModelAPI_Attribute> theEntity)
 750 {
 751   if (myEntityMap.find(theEntity) != myEntityMap.end())
 752   {
 753     // If the attribute is a point and it is changed (the group needs to rebuild),
 754     // probably user has dragged this point into this position,
 755     // so it is necessary to add constraint which will guarantee the point will not change
 756
 757     // Store myNeedToSolve flag to verify the entity is really changed
 758     bool aNeedToSolveCopy = myNeedToSolve;
 759     myNeedToSolve = false;
 760
 761     changeEntity(theEntity);
 762
 763     if (myNeedToSolve) // the entity is changed
 764     {
 765       // Verify the entity is a point and add temporary constraint of permanency
 766       boost::shared_ptr<GeomDataAPI_Point> aPoint =
 767         boost::dynamic_pointer_cast<GeomDataAPI_Point>(theEntity);
 768       boost::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
 769         boost::dynamic_pointer_cast<GeomDataAPI_Point2D>(theEntity);
 770       if (aPoint || aPoint2D)
 771         addTemporaryConstraintWhereDragged(theEntity);
 772     }
 773
 774     // Restore flag of changes
 775     myNeedToSolve = myNeedToSolve || aNeedToSolveCopy;
 776   }
 777 }
 778
 779 void SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::addTemporaryConstraintWhereDragged(
 780                 boost::shared_ptr<ModelAPI_Attribute> theEntity)
 781 {
 782   // Find identifier of the entity
 783   std::map<boost::shared_ptr<ModelAPI_Attribute>, Slvs_hEntity>::const_iterator
 784     anEntIter = myEntityMap.find(theEntity);
 785
 786   // Create WHERE_DRAGGED constraint
 787   Slvs_Constraint aWDConstr = Slvs_MakeConstraint(++myConstrMaxID, myID, SLVS_C_WHERE_DRAGGED,
 788                                                   myWorkplane.h, 0.0, anEntIter->second, 0, 0, 0);
 789   myConstraints.push_back(aWDConstr);
 790   myTempConstraints.push_back(aWDConstr.h);
 791 }
 792
 793 void SketchSolver_ConstraintManager::SketchSolver_ConstraintGroup::removeTemporaryConstraints()
 794 {
 795   std::list<Slvs_hConstraint>::reverse_iterator aTmpConstrIter;
 796   for (aTmpConstrIter = myTempConstraints.rbegin(); aTmpConstrIter != myTempConstraints.rend(); aTmpConstrIter++)
 797   {
 798     int aConstrPos = Search(*aTmpConstrIter, myConstraints);
 799     myConstraints.erase(myConstraints.begin() + aConstrPos);
 800
 801     // If the removing constraint has higher index, decrease the indexer
 802     if (*aTmpConstrIter == myConstrMaxID)
 803       myConstrMaxID--;
 804   }
 805   myTempConstraints.clear();
 806 }
 807
 808
 809
 810 // ========================================================
 811 // =========      Auxiliary functions       ===============
 812 // ========================================================
 813
 814 template <typename T>
 815 int Search(const uint32_t& theEntityID, const std::vector<T>& theEntities)
 816 {
 817   int aResIndex = theEntityID <= theEntities.size() ? theEntityID - 1 : 0;
 818   int aVecSize = theEntities.size();
 819   while (aResIndex >= 0 && theEntities[aResIndex].h > theEntityID)
 820     aResIndex--;
 821   while (aResIndex < aVecSize && theEntities[aResIndex].h < theEntityID)
 822     aResIndex++;
 823   if (aResIndex == -1)
 824     aResIndex = aVecSize;
 825   return aResIndex;
 826 }