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