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> >& theEntities)
  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   static const size_t MAX_POINTS = 3;
  69   std::vector<Slvs_hEntity> aPoints[MAX_POINTS]; // lists of points of features
  70   std::vector<Slvs_hEntity> anEntities;
  71   std::list<ObjectPtr> anObjectList = aRefList->list();
  72   std::list<ObjectPtr>::iterator anObjectIter = anObjectList.begin();
  73   while (anObjectIter != anObjectList.end()) {
  74     for (size_t i = 0; i < MAX_POINTS; ++i)
  75       aPoints[i].clear();
  76     anEntities.clear();
  77
  78     for (size_t i = 0; i <= myNumberOfCopies && anObjectIter != anObjectList.end(); i++, anObjectIter++) {
  79       aFeature = ModelAPI_Feature::feature(*anObjectIter);
  80       if (!aFeature)
  81         continue;
  82       anEntityID = changeEntity(aFeature, aType);
  83       anEntities.push_back(anEntityID);
  84       Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
  85       switch (aType) {
  86       case SLVS_E_POINT_IN_2D:
  87       case SLVS_E_POINT_IN_3D:
  88         aPoints[0].push_back(anEntityID);
  89         break;
  90       case SLVS_E_LINE_SEGMENT:
  91         aPoints[0].push_back(anEntity.point[0]); // start point of line
  92         aPoints[1].push_back(anEntity.point[1]); // end point of line
  93         break;
  94       case SLVS_E_CIRCLE:
  95         aPoints[0].push_back(anEntity.point[0]); // center of circle
  96         break;
  97       case SLVS_E_ARC_OF_CIRCLE:
  98         aPoints[0].push_back(anEntity.point[0]); // center of arc
  99         aPoints[1].push_back(anEntity.point[1]); // start point of arc
 100         aPoints[2].push_back(anEntity.point[2]); // end point of arc
 101         break;
 102       default:
 103         myErrorMsg = SketchSolver_Error::INCORRECT_ATTRIBUTE();
 104         return;
 105       }
 106     }
 107
 108     for (size_t i = 0; i < MAX_POINTS; ++i)
 109       if (!aPoints[i].empty())
 110         thePoints.push_back(aPoints[i]);
 111     if (!anEntities.empty())
 112       theEntities.push_back(anEntities);
 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   std::vector<std::vector<Slvs_hEntity> > anEntitiesAndCopies;
 127   getAttributes(myRotationCenter, myAngle, myPointsAndCopies, anEntitiesAndCopies);
 128   if (!myErrorMsg.empty())
 129     return;
 130
 131   // Set the rotation center unchanged during constraint recalculation
 132   Slvs_Constraint aConstraint;
 133   if (!myStorage->isPointFixed(myRotationCenter, aConstraint.h, true)) {
 134     aConstraint = Slvs_MakeConstraint(
 135         SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_WHERE_DRAGGED, myGroup->getWorkplaneId(), 0.0,
 136         myRotationCenter, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 137     aConstraint.h = myStorage->addConstraint(aConstraint);
 138     mySlvsConstraints.push_back(aConstraint.h);
 139   }
 140
 141   // Keep all objects unchanged (only initial object may be changed by user)
 142   myCircsAndCopies.clear();
 143   std::vector<std::vector<Slvs_hEntity> >::const_iterator anEntIt = anEntitiesAndCopies.begin();
 144   std::vector<Slvs_hEntity>::const_iterator aCpIt;
 145   for (; anEntIt != anEntitiesAndCopies.end(); ++anEntIt) {
 146     std::vector<Slvs_hEntity> aCircs;
 147     aCpIt = anEntIt->begin();
 148     // Obtain initial points
 149     Slvs_Entity anInitial = myStorage->getEntity(*aCpIt);
 150     if (anInitial.type == SLVS_E_POINT_IN_2D || anInitial.type == SLVS_E_POINT_IN_3D)
 151       myInitialPoints.insert(anInitial.h);
 152     else {
 153       for (int i = 0; i < 4 && anInitial.point[i] != SLVS_E_UNKNOWN; i++)
 154         myInitialPoints.insert(anInitial.point[i]);
 155     }
 156
 157     // Fix the copies
 158     for (++aCpIt; aCpIt != anEntIt->end(); ++aCpIt) {
 159       const Slvs_Entity& anEntity = myStorage->getEntity(*aCpIt);
 160       std::vector<Slvs_hConstraint> aNewConstr;
 161       if (anEntity.type == SLVS_E_CIRCLE) {
 162         aCircs.push_back(anEntity.distance);
 163         // for circles we fix only center
 164         aNewConstr = myStorage->fixEntity(anEntity.point[0]);
 165       } else
 166         aNewConstr = myStorage->fixEntity(*aCpIt);
 167       if (anEntity.type == SLVS_E_ARC_OF_CIRCLE)
 168         aCircs.push_back(anEntity.h);
 169       mySlvsConstraints.insert(mySlvsConstraints.end(), aNewConstr.begin(), aNewConstr.end());
 170     }
 171
 172     if (!aCircs.empty()) {
 173       if (anInitial.type == SLVS_E_CIRCLE)
 174         aCircs.insert(aCircs.begin(), anInitial.distance);
 175       else
 176         aCircs.insert(aCircs.begin(), anInitial.h);
 177       myCircsAndCopies.push_back(aCircs);
 178     }
 179   }
 180
 181   adjustConstraint();
 182 }
 183
 184 void SketchSolver_ConstraintMultiRotation::update(ConstraintPtr theConstraint)
 185 {
 186   cleanErrorMsg();
 187   if (!theConstraint || theConstraint == myBaseConstraint) {
 188     AttributeRefListPtr anInitialRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
 189         myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
 190     AttributeIntegerPtr aNbCopies = myBaseConstraint->integer(SketchPlugin_MultiRotation::NUMBER_OF_COPIES_ID());
 191     if (anInitialRefList->size() != myNumberOfObjects || aNbCopies->value() != myNumberOfCopies) {
 192       remove(myBaseConstraint);
 193       process();
 194       return;
 195     }
 196   }
 197
 198   // update angle value
 199   myAngle = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 200       myBaseConstraint->attribute(SketchPlugin_MultiRotation::ANGLE_ID()))->value();
 201
 202   SketchSolver_Constraint::update();
 203 }
 204
 205 bool SketchSolver_ConstraintMultiRotation::remove(ConstraintPtr theConstraint)
 206 {
 207   cleanErrorMsg();
 208   if (theConstraint && theConstraint != myBaseConstraint)
 209     return false;
 210   bool isFullyRemoved = true;
 211   std::vector<Slvs_hEntity>::iterator aCIter = mySlvsConstraints.begin();
 212   for (; aCIter != mySlvsConstraints.end(); aCIter++)
 213    isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
 214   mySlvsConstraints.clear();
 215
 216   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt = myFeatureMap.begin();
 217   for (; aFeatIt != myFeatureMap.end(); aFeatIt++)
 218     myStorage->removeEntity(aFeatIt->second);
 219   myStorage->removeUnusedEntities();
 220
 221   std::map<FeaturePtr, Slvs_hEntity> aFeatureMapCopy = myFeatureMap;
 222
 223   if (isFullyRemoved) {
 224     myFeatureMap.clear();
 225     myAttributeMap.clear();
 226     myValueMap.clear();
 227   } else
 228     cleanRemovedEntities();
 229
 230   // Restore initial features
 231   std::map<FeaturePtr, Slvs_hEntity>::iterator aFIter = aFeatureMapCopy.begin();
 232   for (; aFIter != aFeatureMapCopy.end(); ++aFIter)
 233   {
 234     if (myFeatureMap.find(aFIter->first) != myFeatureMap.end())
 235       continue; // the feature was not removed
 236     Slvs_hEntity anEntity = myGroup->getFeatureId(aFIter->first);
 237     if (anEntity != SLVS_E_UNKNOWN)
 238       myFeatureMap[aFIter->first] = anEntity;
 239   }
 240
 241   // Clear list of rotated points
 242   myPointsAndCopies.clear();
 243   myInitialPoints.clear();
 244
 245   return true;
 246 }
 247
 248 void SketchSolver_ConstraintMultiRotation::addFeature(FeaturePtr theFeature)
 249 {
 250   SketchSolver_Constraint::addFeature(theFeature);
 251
 252   std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt = myFeatureMap.find(theFeature);
 253   if (aFeatIt == myFeatureMap.end())
 254     return;
 255
 256   // store list of points of the feature
 257   const Slvs_Entity& theEntity = myStorage->getEntity(aFeatIt->second);
 258   for (int i = 0; i < 4; i++)
 259     if (theEntity.point[i] != SLVS_E_UNKNOWN)
 260       myPointsJustUpdated.insert(theEntity.point[i]);
 261 }
 262
 263 void SketchSolver_ConstraintMultiRotation::adjustConstraint()
 264 {
 265   if (abs(myAngle) < tolerance) {
 266     myStorage->setNeedToResolve(false);
 267     return;
 268   }
 269
 270   std::list<Slvs_Constraint> aCoincident = myStorage->getConstraintsByType(SLVS_C_POINTS_COINCIDENT);
 271   std::list<Slvs_Constraint>::const_iterator aCoIt;
 272
 273   // Check overconstrained on rotation center (if it is coincident with other fixed point)
 274   Slvs_hConstraint aFixedCenter;
 275   if (myStorage->isPointFixed(myRotationCenter, aFixedCenter, false)) {
 276     Slvs_hConstraint aFixed;
 277     for (aCoIt = aCoincident.begin(); aCoIt != aCoincident.end(); ++aCoIt)
 278       if ((aCoIt->ptA == myRotationCenter && myStorage->isPointFixed(aCoIt->ptB, aFixed, true)) ||
 279           (aCoIt->ptB == myRotationCenter && myStorage->isPointFixed(aCoIt->ptA, aFixed, true))) {
 280         // Un-fix the center
 281         myStorage->removeConstraint(aFixedCenter);
 282         std::vector<Slvs_hConstraint>::iterator aSCIt = mySlvsConstraints.begin();
 283         for (; aSCIt != mySlvsConstraints.end(); ++aSCIt)
 284           if (*aSCIt == aFixedCenter) {
 285             mySlvsConstraints.erase(aSCIt);
 286             break;
 287           }
 288       }
 289   }
 290
 291   // Obtain coordinates of rotation center
 292   Slvs_Entity aRotCenter = myStorage->getEntity(myRotationCenter);
 293   double aCenterXY[2];
 294   for (int i = 0; i < 2; i++)
 295     aCenterXY[i] = myStorage->getParameter(aRotCenter.param[i]).val;
 296
 297   double cosA = cos(myAngle * PI / 180.0);
 298   double sinA = sin(myAngle * PI / 180.0);
 299
 300   double aVec[2]; // coordinates of vector defining a direction from rotation center to a point
 301
 302   // Update positions of all points to satisfy angles
 303   std::vector< std::vector<Slvs_hEntity> >::const_iterator aPointsIter = myPointsAndCopies.begin();
 304   std::vector<Slvs_hEntity>::const_iterator aCopyIter;
 305   for (; aPointsIter != myPointsAndCopies.end(); ++aPointsIter) {
 306     aCopyIter = aPointsIter->begin();
 307     const Slvs_Entity& anInitial = myStorage->getEntity(*aCopyIter);
 308     for (int i = 0; i < 2; i++)
 309       aVec[i] = myStorage->getParameter(anInitial.param[i]).val - aCenterXY[i];
 310
 311     // if the point is coincident with another one which is temporary fixed (moved by user),
 312     // we will update its position correspondingly
 313     Slvs_hConstraint aFixed;
 314     for (aCoIt = aCoincident.begin(); aCoIt != aCoincident.end(); ++aCoIt) {
 315       if ((aCoIt->ptA == anInitial.h && myInitialPoints.find(aCoIt->ptB) != myInitialPoints.end()) ||
 316           (aCoIt->ptB == anInitial.h && myInitialPoints.find(aCoIt->ptA) != myInitialPoints.end())) {
 317         Slvs_hEntity anOtherId = aCoIt->ptA == anInitial.h ? aCoIt->ptB : aCoIt->ptA;
 318         if (!myStorage->isTemporary(aFixed) &&
 319             myPointsJustUpdated.find(anOtherId) == myPointsJustUpdated.end())
 320           continue; // nothing to change
 321
 322         const Slvs_Entity& anOtherPnt = myStorage->getEntity(anOtherId);
 323         for (int i = 0; i < 2; i++) {
 324           Slvs_Param anInitParam = myStorage->getParameter(anInitial.param[i]);
 325           const Slvs_Param& anOtherParam = myStorage->getParameter(anOtherPnt.param[i]);
 326           anInitParam.val = anOtherParam.val;
 327           myStorage->updateParameter(anInitParam);
 328           aVec[i] = anOtherParam.val - aCenterXY[i];
 329         }
 330       }
 331     }
 332
 333     // update copied points
 334     aCopyIter = aPointsIter->begin();
 335     for (++aCopyIter; aCopyIter != aPointsIter->end(); ++aCopyIter) {
 336       // rotate direction
 337       double aTemp = aVec[0] * cosA - aVec[1] * sinA;
 338       aVec[1] = aVec[0] * sinA + aVec[1] * cosA;
 339       aVec[0] = aTemp;
 340
 341       const Slvs_Entity& aTarget = myStorage->getEntity(*aCopyIter);
 342       for (int i = 0; i < 2; i++) {
 343         Slvs_Param aParam = myStorage->getParameter(aTarget.param[i]);
 344         aParam.val = aCenterXY[i] + aVec[i];
 345         myStorage->updateParameter(aParam);
 346       }
 347     }
 348   }
 349
 350   std::list<Slvs_Constraint> aDiamConstr;
 351   for (aPointsIter = myCircsAndCopies.begin(); aPointsIter != myCircsAndCopies.end(); ++aPointsIter) {
 352     aCopyIter = aPointsIter->begin();
 353     const Slvs_Entity& anInitial = myStorage->getEntity(*aCopyIter);
 354     if (anInitial.type == SLVS_E_DISTANCE) {
 355       const Slvs_Param& anInitRad = myStorage->getParameter(anInitial.param[0]);
 356       for (++aCopyIter; aCopyIter != aPointsIter->end(); ++aCopyIter) {
 357         const Slvs_Entity& aCopy = myStorage->getEntity(*aCopyIter);
 358         Slvs_Param aCopyRad = myStorage->getParameter(aCopy.param[0]);
 359         aCopyRad.val = anInitRad.val;
 360         myStorage->updateParameter(aCopyRad);
 361       }
 362     } else if (anInitial.type == SLVS_E_ARC_OF_CIRCLE) {
 363       const Slvs_Entity& aCenterEnt = myStorage->getEntity(anInitial.point[0]);
 364       const Slvs_Entity& aStartEnt = myStorage->getEntity(anInitial.point[1]);
 365
 366       if (aDiamConstr.empty())
 367         aDiamConstr = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
 368       // Calculate diameter of initial arc
 369       double aDiam = 0.0;
 370       for (int i = 0; i < 2; i++) {
 371         double d = myStorage->getParameter(aStartEnt.param[i]).val -
 372                    myStorage->getParameter(aCenterEnt.param[i]).val;
 373         aDiam += d * d;
 374       }
 375       aDiam = sqrt(aDiam) * 2.0;
 376       // Update the Diameter constraints of copied arcs
 377       for (++aCopyIter; aCopyIter != aPointsIter->end(); ++aCopyIter) {
 378         std::list<Slvs_Constraint>::iterator aDCIt = aDiamConstr.begin();
 379         for (; aDCIt != aDiamConstr.end(); ++aDCIt)
 380           if (aDCIt->entityA == *aCopyIter) {
 381             aDCIt->valA = aDiam;
 382             myStorage->updateConstraint(*aDCIt);
 383             aDiamConstr.erase(aDCIt);
 384             break;
 385           }
 386       }
 387     }
 388   }
 389
 390   myPointsJustUpdated.clear();
 391 }