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