src/SketchSolver/SketchSolver_ConstraintMultiRotation.cpp

   1 #include <SketchSolver_ConstraintMultiRotation.h>
   2 #include <SketchSolver_Group.h>
   3 #include <SketchSolver_Error.h>
   4
   5 #include <SketchPlugin_Arc.h>
   6 #include <SketchPlugin_MultiRotation.h>
   7
   8 #include <ModelAPI_AttributeDouble.h>
   9 #include <ModelAPI_AttributeInteger.h>
  10 #include <ModelAPI_AttributeRefAttr.h>
  11 #include <ModelAPI_AttributeRefList.h>
  12 #include <ModelAPI_ResultConstruction.h>
  13
  14 #include <GeomAPI_Dir2d.h>
  15 #include <GeomAPI_XY.h>
  16
  17 #include <math.h>
  18
  19 static double squareDistance(
  20     StoragePtr theStorage, const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
  21 {
  22   Slvs_Entity aPoint1 = theStorage->getEntity(thePoint1);
  23   Slvs_Entity aPoint2 = theStorage->getEntity(thePoint2);
  24   double x1 = theStorage->getParameter(aPoint1.param[0]).val;
  25   double y1 = theStorage->getParameter(aPoint1.param[1]).val;
  26   double x2 = theStorage->getParameter(aPoint2.param[0]).val;
  27   double y2 = theStorage->getParameter(aPoint2.param[1]).val;
  28   return (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2);
  29 }
  30
  31 void SketchSolver_ConstraintMultiRotation::getAttributes(
  32     Slvs_hEntity& theCenter, double& theAngle,
  33     std::vector<std::vector<Slvs_hEntity> >& thePoints,
  34     std::vector<std::vector<Slvs_hEntity> >& theCircular)
  35 {
  36   DataPtr aData = myBaseConstraint->data();
  37   theAngle = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
  38       aData->attribute(SketchPlugin_MultiRotation::ANGLE_ID()))->value();
  39
  40   AttributePtr aCenterAttr = aData->attribute(SketchPlugin_MultiRotation::CENTER_ID());
  41   if (!aCenterAttr || !aCenterAttr->isInitialized()) {
  42     myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
  43     return;
  44   }
  45   int aType = SLVS_E_UNKNOWN; // type of created entity
  46   Slvs_hEntity anEntityID = myGroup->getAttributeId(aCenterAttr);
  47   if (anEntityID == SLVS_E_UNKNOWN)
  48     anEntityID = changeEntity(aCenterAttr, aType);
  49   theCenter = anEntityID;
  50
  51   // Lists of objects and number of copies
  52   AttributeRefListPtr anInitialRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
  53       aData->attribute(SketchPlugin_Constraint::ENTITY_A()));
  54   myNumberOfObjects = anInitialRefList->size();
  55   myNumberOfCopies = (size_t) aData->integer(SketchPlugin_MultiRotation::NUMBER_OF_COPIES_ID())->value();
  56   AttributeRefListPtr aRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
  57       myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
  58   if (!aRefList) {
  59     myErrorMsg = SketchSolver_Error::INCORRECT_ATTRIBUTE();
  60     return;
  61   }
  62
  63   // Obtain all points of initial features and store them into separate lists
  64   // containing their translated copies.
  65   // Also all circles and arc collected too, because they will be constrained by equal radii.
  66   FeaturePtr aFeature;
  67   ResultConstructionPtr aRC;
  68   std::vector<Slvs_hEntity> aPoints[2]; // lists of points of features
  69   std::vector<Slvs_hEntity> aCircs;     // list of circular objects
  70   std::list<ObjectPtr> anObjectList = aRefList->list();
  71   std::list<ObjectPtr>::iterator anObjectIter = anObjectList.begin();
  72   while (anObjectIter != anObjectList.end()) {
  73     aPoints[0].clear();
  74     aPoints[1].clear();
  75     aCircs.clear();
  76
  77     for (size_t i = 0; i <= myNumberOfCopies && anObjectIter != anObjectList.end(); i++, anObjectIter++) {
  78       aFeature = ModelAPI_Feature::feature(*anObjectIter);
  79       if (!aFeature)
  80         continue;
  81       anEntityID = changeEntity(aFeature, aType);
  82       Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
  83       switch (aType) {
  84       case SLVS_E_POINT_IN_2D:
  85       case SLVS_E_POINT_IN_3D:
  86         aPoints[0].push_back(anEntityID);
  87         break;
  88       case SLVS_E_LINE_SEGMENT:
  89         aPoints[0].push_back(anEntity.point[0]); // start point of line
  90         aPoints[1].push_back(anEntity.point[1]); // end point of line
  91         break;
  92       case SLVS_E_CIRCLE:
  93         aPoints[0].push_back(anEntity.point[0]); // center of circle
  94         aCircs.push_back(anEntityID);
  95         break;
  96       case SLVS_E_ARC_OF_CIRCLE:
  97         aPoints[0].push_back(anEntity.point[1]); // start point of arc
  98         aPoints[1].push_back(anEntity.point[2]); // end point of arc
  99         aCircs.push_back(anEntityID);
 100         break;
 101       default:
 102         myErrorMsg = SketchSolver_Error::INCORRECT_ATTRIBUTE();
 103         return;
 104       }
 105     }
 106
 107     if (!aPoints[0].empty())
 108       thePoints.push_back(aPoints[0]);
 109     if (!aPoints[1].empty())
 110       thePoints.push_back(aPoints[1]);
 111     if (!aCircs.empty())
 112       theCircular.push_back(aCircs);
 113   }
 114 }
 115
 116 void SketchSolver_ConstraintMultiRotation::process()
 117 {
 118   cleanErrorMsg();
 119   if (!myBaseConstraint || !myStorage || myGroup == 0) {
 120     /// TODO: Put error message here
 121     return;
 122   }
 123   if (!mySlvsConstraints.empty()) // some data is changed, update constraint
 124     update(myBaseConstraint);
 125
 126   Slvs_hEntity aCenter;
 127   std::vector<std::vector<Slvs_hEntity> > aPointsAndCopies;
 128   std::vector<std::vector<Slvs_hEntity> > aCircsAndCopies;
 129   getAttributes(aCenter, myAngle, aPointsAndCopies, aCircsAndCopies);
 130   if (!myErrorMsg.empty())
 131     return;
 132
 133   myAuxLines.clear();
 134
 135   // Create lines between neighbor rotated points and make angle between them equal to anAngle.
 136   // Also these lines should have equal lengths.
 137   Slvs_Constraint aConstraint;
 138   myRotationCenter = aCenter;
 139   Slvs_Entity aPrevLine;
 140   std::vector<std::vector<Slvs_hEntity> >::iterator aCopyIter = aPointsAndCopies.begin();
 141   for (; aCopyIter != aPointsAndCopies.end(); aCopyIter++) {
 142     size_t aSize = aCopyIter->size();
 143     if (aSize <= 1) continue;
 144
 145     aPrevLine = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, myGroup->getId(),
 146           myGroup->getWorkplaneId(), aCenter, (*aCopyIter)[0]);
 147     aPrevLine.h = myStorage->addEntity(aPrevLine);
 148     std::vector<Slvs_hEntity> anEqualLines(1, aPrevLine.h);
 149     for (size_t i = 1; i < aSize; i++) {
 150       Slvs_Entity aLine = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, myGroup->getId(),
 151           myGroup->getWorkplaneId(), aCenter, (*aCopyIter)[i]);
 152       aLine.h = myStorage->addEntity(aLine);
 153       // Equal length constraint
 154       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 155           SLVS_C_EQUAL_LENGTH_LINES, myGroup->getWorkplaneId(), 0.0,
 156           SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aPrevLine.h, aLine.h);
 157       aConstraint.h = myStorage->addConstraint(aConstraint);
 158       mySlvsConstraints.push_back(aConstraint.h);
 159       // Angle constraint
 160       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 161           SLVS_C_ANGLE, myGroup->getWorkplaneId(), fabs(myAngle), SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 162           aPrevLine.h, aLine.h);
 163       if (myAngle < 0.0) // clockwise rotation
 164         aConstraint.other = true;
 165       aConstraint.h = myStorage->addConstraint(aConstraint);
 166       mySlvsConstraints.push_back(aConstraint.h);
 167
 168       aPrevLine = aLine;
 169       anEqualLines.push_back(aPrevLine.h);
 170     }
 171
 172     myAuxLines.push_back(anEqualLines);
 173   }
 174   // Equal radii constraints
 175   for (aCopyIter = aCircsAndCopies.begin(); aCopyIter != aCircsAndCopies.end(); aCopyIter++) {
 176     size_t aSize = aCopyIter->size();
 177     for (size_t i = 0; i < aSize - 1; i++) {
 178       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 179           SLVS_C_EQUAL_RADIUS, myGroup->getWorkplaneId(), 0.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 180           (*aCopyIter)[i], (*aCopyIter)[i+1]);
 181       aConstraint.h = myStorage->addConstraint(aConstraint);
 182       mySlvsConstraints.push_back(aConstraint.h);
 183     }
 184   }
 185
 186   // Set the rotation center unchanged during constraint recalculation
 187   if (!myStorage->isPointFixed(myRotationCenter, aConstraint.h, true)) {
 188     aConstraint = Slvs_MakeConstraint(
 189         SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_WHERE_DRAGGED, myGroup->getWorkplaneId(), 0.0,
 190         myRotationCenter, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 191     aConstraint.h = myStorage->addConstraint(aConstraint);
 192     mySlvsConstraints.push_back(aConstraint.h);
 193   }
 194
 195   adjustConstraint();
 196 }
 197
 198 void SketchSolver_ConstraintMultiRotation::update(ConstraintPtr theConstraint)
 199 {
 200   cleanErrorMsg();
 201   if (!theConstraint || theConstraint == myBaseConstraint) {
 202     AttributeRefListPtr anInitialRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 203         myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 204     AttributeIntegerPtr aNbCopies = myBaseConstraint->integer(SketchPlugin_MultiRotation::NUMBER_OF_COPIES_ID());
 205     if (anInitialRefList->size() != myNumberOfObjects || aNbCopies->value() != myNumberOfCopies) {
 206       remove(myBaseConstraint);
 207       process();
 208       return;
 209     }
 210   }
 211
 212   // update angle value
 213   myAngle = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 214       myBaseConstraint->attribute(SketchPlugin_MultiRotation::ANGLE_ID()))->value();
 215
 216   SketchSolver_Constraint::update();
 217 }
 218
 219 bool SketchSolver_ConstraintMultiRotation::remove(ConstraintPtr theConstraint)
 220 {
 221   cleanErrorMsg();
 222   if (theConstraint && theConstraint != myBaseConstraint)
 223     return false;
 224   bool isFullyRemoved = true;
 225   std::vector<Slvs_hEntity>::iterator aCIter = mySlvsConstraints.begin();
 226   for (; aCIter != mySlvsConstraints.end(); aCIter++)
 227    isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
 228   mySlvsConstraints.clear();
 229
 230   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt = myFeatureMap.begin();
 231   for (; aFeatIt != myFeatureMap.end(); aFeatIt++)
 232     myStorage->removeEntity(aFeatIt->second);
 233
 234   if (isFullyRemoved) {
 235     myFeatureMap.clear();
 236     myAttributeMap.clear();
 237     myValueMap.clear();
 238   } else
 239     cleanRemovedEntities();
 240   return true;
 241 }
 242
 243 void SketchSolver_ConstraintMultiRotation::adjustConstraint()
 244 {
 245   if (abs(myAngle) < tolerance) {
 246     myStorage->setNeedToResolve(false);
 247     return;
 248   }
 249
 250   // Check the lengths of auxiliary lines are zero.
 251   // If they become zero, remove corresponding Angle constraints.
 252   // It they become non-zero (but were zero recently), add Angle constraint.
 253   std::vector<Slvs_hConstraint>::iterator aConstr = mySlvsConstraints.begin();
 254   std::map<Slvs_hEntity, Slvs_hEntity> anEqualLines;
 255   bool isFirstRemoved = false;
 256   for (; aConstr != mySlvsConstraints.end();
 257        isFirstRemoved ? aConstr = mySlvsConstraints.begin() : ++aConstr) {
 258     isFirstRemoved = false;
 259     Slvs_Constraint aConstraint = myStorage->getConstraint(*aConstr);
 260     if (aConstraint.type == SLVS_C_ANGLE || aConstraint.type == SLVS_C_EQUAL_LENGTH_LINES) {
 261       Slvs_Entity aLine = myStorage->getEntity(aConstraint.entityA);
 262       // Line length became zero => remove constraint
 263       if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance) {
 264         myStorage->removeConstraint(aConstraint.h);
 265         isFirstRemoved = aConstr == mySlvsConstraints.begin();
 266         std::vector<Slvs_hConstraint>::iterator aTmpIter = aConstr--;
 267         mySlvsConstraints.erase(aTmpIter);
 268       }
 269       // Store the lines into the map
 270       anEqualLines[aConstraint.entityB] = aConstraint.entityA;
 271     }
 272   }
 273   // Create Angle and Equal constraints for non-degenerated lines
 274   AuxLinesList::iterator anIt = myAuxLines.begin();
 275   for (; anIt != myAuxLines.end(); ++anIt) {
 276     if (anEqualLines.find(anIt->back()) != anEqualLines.end())
 277       continue;
 278
 279     std::vector<Slvs_hEntity>::iterator anEqLinesIt = anIt->begin();
 280     Slvs_hEntity aPrevLine = (*anEqLinesIt);
 281     // Check the length of the line
 282     Slvs_Entity aLine = myStorage->getEntity(aPrevLine);
 283     if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance)
 284       continue;
 285
 286     for (++anEqLinesIt; anEqLinesIt != anIt->end(); ++anEqLinesIt) {
 287       Slvs_hEntity aLine = (*anEqLinesIt);
 288       // Equal length constraint
 289       Slvs_Constraint aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 290           SLVS_C_EQUAL_LENGTH_LINES, myGroup->getWorkplaneId(), 0.0,
 291           SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aPrevLine, aLine);
 292       aConstraint.h = myStorage->addConstraint(aConstraint);
 293       mySlvsConstraints.push_back(aConstraint.h);
 294       // Angle constraint
 295       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 296           SLVS_C_ANGLE, myGroup->getWorkplaneId(), fabs(myAngle), SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 297           aPrevLine, aLine);
 298       if (myAngle < 0.0) // clockwise rotation
 299         aConstraint.other = true;
 300       aConstraint.h = myStorage->addConstraint(aConstraint);
 301       mySlvsConstraints.push_back(aConstraint.h);
 302
 303       aPrevLine = aLine;
 304     }
 305   }
 306
 307   // Obtain coordinates of rotation center
 308   Slvs_Entity aRotCenter = myStorage->getEntity(myRotationCenter);
 309   double aCenterXY[2];
 310   for (int i = 0; i < 2; i++)
 311     aCenterXY[i] = myStorage->getParameter(aRotCenter.param[i]).val;
 312
 313   double cosA = cos(myAngle * PI / 180.0);
 314   double sinA = sin(myAngle * PI / 180.0);
 315
 316   // Update positions of all points to satisfy angles
 317   std::list<Slvs_Constraint> aConstrAngle = myStorage->getConstraintsByType(SLVS_C_ANGLE);
 318   std::list<Slvs_Constraint>::iterator anAngIt = aConstrAngle.begin();
 319   std::vector<Slvs_hConstraint>::iterator aCIt;
 320   Slvs_hConstraint aFixed; // temporary variable
 321   double aVec[2]; // coordinates of vector defining a line
 322   Slvs_Param aTarget[2]; // parameter to be changed
 323   for (; anAngIt != aConstrAngle.end(); anAngIt++) {
 324     for (aCIt = mySlvsConstraints.begin(); aCIt != mySlvsConstraints.end(); aCIt++)
 325       if (anAngIt->h == *aCIt)
 326         break;
 327     if (aCIt == mySlvsConstraints.end())
 328       continue;
 329     Slvs_Entity aLineA = myStorage->getEntity(anAngIt->entityA);
 330     Slvs_Entity aLineB = myStorage->getEntity(anAngIt->entityB);
 331     if (myStorage->isPointFixed(aLineB.point[1], aFixed))
 332       continue;
 333     Slvs_Entity aPointA = myStorage->getEntity(aLineA.point[1]);
 334     Slvs_Entity aPointB = myStorage->getEntity(aLineB.point[1]);
 335     for (int i = 0; i < 2; i++) {
 336       aVec[i] = myStorage->getParameter(aPointA.param[i]).val - aCenterXY[i];
 337       aTarget[i] = myStorage->getParameter(aPointB.param[i]);
 338     }
 339     aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
 340     aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
 341     myStorage->updateParameter(aTarget[0]);
 342     myStorage->updateParameter(aTarget[1]);
 343
 344     anAngIt->valA = myAngle;
 345     myStorage->updateConstraint(*anAngIt);
 346   }
 347
 348   // update positions of centers of arcs for correct radius calculation
 349   std::list<Slvs_Constraint> aRadii = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 350   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt;
 351   for (anAngIt = aRadii.begin(); anAngIt != aRadii.end(); anAngIt++) {
 352     int aNbFound = 0; // number of arcs used in translation
 353     for (aFeatIt = myFeatureMap.begin(); aFeatIt != myFeatureMap.end(); aFeatIt++)
 354       if (aFeatIt->second == anAngIt->entityA || aFeatIt->second == anAngIt->entityB) {
 355         if (aFeatIt->first->getKind() == SketchPlugin_Arc::ID())
 356           aNbFound++;
 357         else
 358           break;
 359       }
 360     if (aNbFound != 2)
 361       continue;
 362     // two arcs were found, update their centers
 363     Slvs_Entity anArcA = myStorage->getEntity(anAngIt->entityA);
 364     Slvs_Entity anArcB = myStorage->getEntity(anAngIt->entityB);
 365     if (myStorage->isPointFixed(anArcB.point[0], aFixed))
 366       continue;
 367     Slvs_Entity aCenterA = myStorage->getEntity(anArcA.point[0]);
 368     Slvs_Entity aCenterB = myStorage->getEntity(anArcB.point[0]);
 369     for (int i = 0; i < 2; i++) {
 370       aVec[i] = myStorage->getParameter(aCenterA.param[i]).val - aCenterXY[i];
 371       aTarget[i] = myStorage->getParameter(aCenterB.param[i]);
 372     }
 373     aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
 374     aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
 375     myStorage->updateParameter(aTarget[0]);
 376     myStorage->updateParameter(aTarget[1]);
 377   }
 378 }