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