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