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   myStorage->removeUnusedEntities();
 234
 235   std::map<FeaturePtr, Slvs_hEntity> aFeatureMapCopy = myFeatureMap;
 236
 237   if (isFullyRemoved) {
 238     myFeatureMap.clear();
 239     myAttributeMap.clear();
 240     myValueMap.clear();
 241   } else
 242     cleanRemovedEntities();
 243
 244   // Restore initial features
 245   std::map<FeaturePtr, Slvs_hEntity>::iterator aFIter = aFeatureMapCopy.begin();
 246   for (; aFIter != aFeatureMapCopy.end(); ++aFIter)
 247   {
 248     if (myFeatureMap.find(aFIter->first) != myFeatureMap.end())
 249       continue; // the feature was not removed
 250     Slvs_hEntity anEntity = myGroup->getFeatureId(aFIter->first);
 251     if (anEntity != SLVS_E_UNKNOWN)
 252       myFeatureMap[aFIter->first] = anEntity;
 253   }
 254
 255   return true;
 256 }
 257
 258 void SketchSolver_ConstraintMultiRotation::adjustConstraint()
 259 {
 260   if (abs(myAngle) < tolerance) {
 261     myStorage->setNeedToResolve(false);
 262     return;
 263   }
 264
 265   // Check the lengths of auxiliary lines are zero.
 266   // If they become zero, remove corresponding Angle constraints.
 267   // It they become non-zero (but were zero recently), add Angle constraint.
 268   std::vector<Slvs_hConstraint>::iterator aConstr = mySlvsConstraints.begin();
 269   std::map<Slvs_hEntity, Slvs_hEntity> anEqualLines;
 270   bool isFirstRemoved = false;
 271   for (; aConstr != mySlvsConstraints.end();
 272        isFirstRemoved ? aConstr = mySlvsConstraints.begin() : ++aConstr) {
 273     isFirstRemoved = false;
 274     Slvs_Constraint aConstraint = myStorage->getConstraint(*aConstr);
 275     if (aConstraint.type == SLVS_C_ANGLE || aConstraint.type == SLVS_C_EQUAL_LENGTH_LINES) {
 276       Slvs_Entity aLine = myStorage->getEntity(aConstraint.entityA);
 277       // Line length became zero => remove constraint
 278       if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance) {
 279         myStorage->removeConstraint(aConstraint.h);
 280         isFirstRemoved = aConstr == mySlvsConstraints.begin();
 281         std::vector<Slvs_hConstraint>::iterator aTmpIter = aConstr;
 282         if (!isFirstRemoved)
 283           --aConstr;
 284         mySlvsConstraints.erase(aTmpIter);
 285       }
 286       // Store the lines into the map
 287       anEqualLines[aConstraint.entityB] = aConstraint.entityA;
 288     }
 289   }
 290   // Create Angle and Equal constraints for non-degenerated lines
 291   AuxLinesList::iterator anIt = myAuxLines.begin();
 292   for (; anIt != myAuxLines.end(); ++anIt) {
 293     if (anEqualLines.find(anIt->back()) != anEqualLines.end())
 294       continue;
 295
 296     std::vector<Slvs_hEntity>::iterator anEqLinesIt = anIt->begin();
 297     Slvs_hEntity aPrevLine = (*anEqLinesIt);
 298     // Check the length of the line
 299     Slvs_Entity aLine = myStorage->getEntity(aPrevLine);
 300     if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance)
 301       continue;
 302
 303     for (++anEqLinesIt; anEqLinesIt != anIt->end(); ++anEqLinesIt) {
 304       Slvs_hEntity aLine = (*anEqLinesIt);
 305       // Equal length constraint
 306       Slvs_Constraint aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 307           SLVS_C_EQUAL_LENGTH_LINES, myGroup->getWorkplaneId(), 0.0,
 308           SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aPrevLine, aLine);
 309       aConstraint.h = myStorage->addConstraint(aConstraint);
 310       mySlvsConstraints.push_back(aConstraint.h);
 311       // Angle constraint
 312       aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 313           SLVS_C_ANGLE, myGroup->getWorkplaneId(), fabs(myAngle), SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 314           aPrevLine, aLine);
 315       if (myAngle < 0.0) // clockwise rotation
 316         aConstraint.other = true;
 317       aConstraint.h = myStorage->addConstraint(aConstraint);
 318       mySlvsConstraints.push_back(aConstraint.h);
 319
 320       aPrevLine = aLine;
 321     }
 322   }
 323
 324   // Obtain coordinates of rotation center
 325   Slvs_Entity aRotCenter = myStorage->getEntity(myRotationCenter);
 326   double aCenterXY[2];
 327   for (int i = 0; i < 2; i++)
 328     aCenterXY[i] = myStorage->getParameter(aRotCenter.param[i]).val;
 329
 330   double cosA = cos(myAngle * PI / 180.0);
 331   double sinA = sin(myAngle * PI / 180.0);
 332
 333   // Update positions of all points to satisfy angles
 334   std::list<Slvs_Constraint> aConstrAngle = myStorage->getConstraintsByType(SLVS_C_ANGLE);
 335   std::list<Slvs_Constraint>::iterator anAngIt = aConstrAngle.begin();
 336   std::vector<Slvs_hConstraint>::iterator aCIt;
 337   Slvs_hConstraint aFixed; // temporary variable
 338   double aVec[2]; // coordinates of vector defining a line
 339   Slvs_Param aTarget[2]; // parameter to be changed
 340   for (; anAngIt != aConstrAngle.end(); anAngIt++) {
 341     for (aCIt = mySlvsConstraints.begin(); aCIt != mySlvsConstraints.end(); aCIt++)
 342       if (anAngIt->h == *aCIt)
 343         break;
 344     if (aCIt == mySlvsConstraints.end())
 345       continue;
 346     Slvs_Entity aLineA = myStorage->getEntity(anAngIt->entityA);
 347     Slvs_Entity aLineB = myStorage->getEntity(anAngIt->entityB);
 348     if (myStorage->isPointFixed(aLineB.point[1], aFixed))
 349       continue;
 350     Slvs_Entity aPointA = myStorage->getEntity(aLineA.point[1]);
 351     Slvs_Entity aPointB = myStorage->getEntity(aLineB.point[1]);
 352     for (int i = 0; i < 2; i++) {
 353       aVec[i] = myStorage->getParameter(aPointA.param[i]).val - aCenterXY[i];
 354       aTarget[i] = myStorage->getParameter(aPointB.param[i]);
 355     }
 356     aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
 357     aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
 358     myStorage->updateParameter(aTarget[0]);
 359     myStorage->updateParameter(aTarget[1]);
 360
 361     anAngIt->valA = myAngle;
 362     myStorage->updateConstraint(*anAngIt);
 363   }
 364
 365   // update positions of centers of arcs for correct radius calculation
 366   std::list<Slvs_Constraint> aRadii = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
 367   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt;
 368   for (anAngIt = aRadii.begin(); anAngIt != aRadii.end(); anAngIt++) {
 369     int aNbFound = 0; // number of arcs used in translation
 370     for (aFeatIt = myFeatureMap.begin(); aFeatIt != myFeatureMap.end(); aFeatIt++)
 371       if (aFeatIt->second == anAngIt->entityA || aFeatIt->second == anAngIt->entityB) {
 372         if (aFeatIt->first->getKind() == SketchPlugin_Arc::ID())
 373           aNbFound++;
 374         else
 375           break;
 376       }
 377     if (aNbFound != 2)
 378       continue;
 379     // two arcs were found, update their centers
 380     Slvs_Entity anArcA = myStorage->getEntity(anAngIt->entityA);
 381     Slvs_Entity anArcB = myStorage->getEntity(anAngIt->entityB);
 382     if (myStorage->isPointFixed(anArcB.point[0], aFixed))
 383       continue;
 384     Slvs_Entity aCenterA = myStorage->getEntity(anArcA.point[0]);
 385     Slvs_Entity aCenterB = myStorage->getEntity(anArcB.point[0]);
 386     for (int i = 0; i < 2; i++) {
 387       aVec[i] = myStorage->getParameter(aCenterA.param[i]).val - aCenterXY[i];
 388       aTarget[i] = myStorage->getParameter(aCenterB.param[i]);
 389     }
 390     aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
 391     aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
 392     myStorage->updateParameter(aTarget[0]);
 393     myStorage->updateParameter(aTarget[1]);
 394   }
 395 }