src/SketchSolver/SketchSolver_ConstraintRigid.cpp

   1 #include <SketchSolver_ConstraintRigid.h>
   2 #include <SketchSolver_Error.h>
   3 #include <SketchSolver_Group.h>
   4
   5 #include <SketchPlugin_Arc.h>
   6 #include <SketchPlugin_Circle.h>
   7 #include <SketchPlugin_ConstraintRigid.h>
   8 #include <SketchPlugin_Line.h>
   9 #include <SketchPlugin_Point.h>
  10
  11 #include <GeomAPI_Pnt2d.h>
  12 #include <GeomAPI_XY.h>
  13 #include <GeomDataAPI_Point2D.h>
  14 #include <ModelAPI_AttributeDouble.h>
  15
  16 #include <math.h>
  17
  18 SketchSolver_ConstraintRigid::SketchSolver_ConstraintRigid(FeaturePtr theFeature)
  19   : SketchSolver_Constraint(),
  20     myBaseFeature(theFeature)
  21 {
  22   process();
  23 }
  24
  25 void SketchSolver_ConstraintRigid::process()
  26 {
  27   cleanErrorMsg();
  28   if ((!myBaseConstraint && !myBaseFeature) || !myStorage || myGroup == 0) {
  29     /// TODO: Put error message here
  30     return;
  31   }
  32   if (!mySlvsConstraints.empty()) // some data is changed, update constraint
  33     update(myBaseConstraint);
  34
  35   double aValue;
  36   std::vector<Slvs_hEntity> anEntities;
  37   getAttributes(aValue, anEntities);
  38   if (!myErrorMsg.empty() || (myFeatureMap.empty() && myAttributeMap.empty()))
  39     return;
  40   fixFeature();
  41 }
  42
  43 void SketchSolver_ConstraintRigid::update(ConstraintPtr theConstraint)
  44 {
  45   cleanErrorMsg();
  46   if (theConstraint && theConstraint == myBaseConstraint &&
  47       theConstraint->getKind() == myBaseConstraint->getKind() &&
  48       checkAttributesChanged(theConstraint)) {
  49     // remove previous constraint and set the given one
  50     remove(myBaseConstraint);
  51     myBaseConstraint = theConstraint;
  52     process();
  53   }
  54 }
  55
  56 static void fixEntity(StoragePtr theStorage, const Slvs_hEntity& theEntID)
  57 {
  58   Slvs_Entity anEntity = theStorage->getEntity(theEntID);
  59   anEntity.group = SLVS_G_OUTOFGROUP;
  60   theStorage->updateEntity(anEntity);
  61   // move out of group all sub-entities
  62   for (int i = 0; i < 4; ++i)
  63     if (anEntity.point[i] != SLVS_E_UNKNOWN)
  64       fixEntity(theStorage, anEntity.point[i]);
  65   // move out of group the radius of circle
  66   if (anEntity.distance != SLVS_E_UNKNOWN)
  67     fixEntity(theStorage, anEntity.distance);
  68   // move out of group parameters
  69   for (int i = 0; i < 4; ++i)
  70     if (anEntity.param[i] != SLVS_E_UNKNOWN) {
  71       Slvs_Param aParam = theStorage->getParameter(anEntity.param[i]);
  72       aParam.group = SLVS_G_OUTOFGROUP;
  73       theStorage->updateParameter(aParam);
  74     }
  75 }
  76
  77 void SketchSolver_ConstraintRigid::fixFeature()
  78 {
  79   Slvs_hEntity anEntID = fixedEntity();
  80   if (anEntID != SLVS_E_UNKNOWN)
  81     fixEntity(myStorage, anEntID);
  82 }
  83
  84 Slvs_hEntity SketchSolver_ConstraintRigid::fixedEntity() const
  85 {
  86   Slvs_hEntity anEntID = SLVS_E_UNKNOWN;
  87   if (myBaseConstraint) {
  88     AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
  89         myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
  90     if (aRefAttr->isObject()) {
  91       FeaturePtr aFeature = ModelAPI_Feature::feature(aRefAttr->object());
  92       std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFound = myFeatureMap.find(aFeature);
  93       if (aFound != myFeatureMap.end())
  94         anEntID = aFound->second;
  95     } else {
  96       std::map<AttributePtr, Slvs_hEntity>::const_iterator aFound = myAttributeMap.find(aRefAttr->attr());
  97       if (aFound != myAttributeMap.end())
  98         anEntID = aFound->second;
  99     }
 100   }
 101   else if (myBaseFeature) {
 102     std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFound = myFeatureMap.find(myBaseFeature);
 103     if (aFound != myFeatureMap.end())
 104       anEntID = aFound->second;
 105   }
 106   return anEntID;
 107 }
 108
 109 void SketchSolver_ConstraintRigid::getAttributes(
 110     double& theValue,
 111     std::vector<Slvs_hEntity>& theAttributes)
 112 {
 113   theValue = 0.0;
 114   int aType = SLVS_E_UNKNOWN; // type of created entity
 115   Slvs_hEntity anEntityID = SLVS_E_UNKNOWN;
 116   if (myBaseConstraint) {
 117     // Get the attribute of constraint
 118     AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
 119         myBaseConstraint->attribute(SketchPlugin_ConstraintRigid::ENTITY_A()));
 120     if (!aRefAttr || !aRefAttr->isInitialized()) {
 121       myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 122       return;
 123     }
 124     anEntityID = myGroup->getAttributeId(aRefAttr);
 125     if (anEntityID == SLVS_E_UNKNOWN)
 126       anEntityID = changeEntity(aRefAttr, aType);
 127   } else {
 128     anEntityID = myGroup->getFeatureId(myBaseFeature);
 129     if (anEntityID == SLVS_E_UNKNOWN)
 130       anEntityID = changeEntity(myBaseFeature, aType);
 131     else
 132       myFeatureMap[myBaseFeature] = anEntityID;
 133   }
 134
 135   if (anEntityID == SLVS_E_UNKNOWN) {
 136     myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
 137     return;
 138   }
 139
 140   // Check the entity is complex
 141   Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
 142   if (anEntity.point[0] != SLVS_E_UNKNOWN) {
 143     for (int i = 0; i < 4 && anEntity.point[i]; i++)
 144       theAttributes.push_back(anEntity.point[i]);
 145   } else // simple entity
 146     theAttributes.push_back(anEntityID);
 147 }
 148
 149
 150 bool SketchSolver_ConstraintRigid::remove(ConstraintPtr theConstraint)
 151 {
 152   cleanErrorMsg();
 153   if (theConstraint && theConstraint != myBaseConstraint)
 154     return false;
 155   bool isFullyRemoved = true;
 156
 157   std::vector<Slvs_hConstraint>::iterator aCIter = mySlvsConstraints.begin();
 158   for (; aCIter != mySlvsConstraints.end(); ++aCIter)
 159     isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
 160
 161   std::map<FeaturePtr, Slvs_hEntity>::iterator aFIter = myFeatureMap.begin();
 162   for (; aFIter != myFeatureMap.end(); ++aFIter)
 163     isFullyRemoved = myStorage->removeEntity(aFIter->second) && isFullyRemoved;
 164
 165   std::map<AttributePtr, Slvs_hEntity>::iterator anAtIter = myAttributeMap.begin();
 166   for (; anAtIter != myAttributeMap.end(); ++anAtIter)
 167     isFullyRemoved = myStorage->removeEntity(anAtIter->second) && isFullyRemoved;
 168
 169   if (isFullyRemoved) {
 170     myFeatureMap.clear();
 171     myAttributeMap.clear();
 172     myValueMap.clear();
 173     mySlvsConstraints.clear();
 174   } else
 175     cleanRemovedEntities();
 176   return true;
 177 }
 178
 179 Slvs_hConstraint SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
 180 {
 181   if (thePointID == SLVS_E_UNKNOWN)
 182     return SLVS_C_UNKNOWN;
 183
 184   Slvs_Constraint aConstraint;
 185   Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
 186   bool isFixed = myStorage->isPointFixed(thePointID, aConstrID, true);
 187   bool isForceUpdate = (isFixed && !myBaseConstraint &&
 188                         myStorage->isTemporary(aConstrID));
 189   if (!isForceUpdate) { // create new constraint
 190     if (isFixed) return aConstrID;
 191     aConstraint = Slvs_MakeConstraint(SLVS_C_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
 192         0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 193     aConstraint.h = myStorage->addConstraint(aConstraint);
 194     mySlvsConstraints.push_back(aConstraint.h);
 195     if (!myBaseConstraint)
 196       myStorage->addConstraintWhereDragged(aConstraint.h);
 197   } else { // update already existent constraint
 198     if (!isFixed || aConstrID == SLVS_C_UNKNOWN || myBaseConstraint)
 199       return SLVS_C_UNKNOWN;
 200     aConstraint = myStorage->getConstraint(aConstrID);
 201     aConstraint.ptA = thePointID;
 202     myStorage->addConstraint(aConstraint);
 203     if (!myBaseConstraint)
 204       myStorage->addConstraintWhereDragged(aConstraint.h);
 205   }
 206   return aConstraint.h;
 207 }
 208
 209 void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
 210 {
 211   Slvs_Constraint anEqual;
 212   if (myStorage->isAxisParallel(theLine.h)) {
 213     // Fix one point and a line length
 214     Slvs_hConstraint aFixed;
 215     if (!myStorage->isPointFixed(theLine.point[0], aFixed, true) &&
 216         !myStorage->isPointFixed(theLine.point[1], aFixed, true))
 217       fixPoint(theLine.point[0]);
 218     if (!myStorage->isUsedInEqual(theLine.h, anEqual)) {
 219       // Check the distance is not set yet
 220       std::list<Slvs_Constraint> aDistConstr = myStorage->getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
 221       std::list<Slvs_Constraint>::const_iterator aDIt = aDistConstr.begin();
 222       for (; aDIt != aDistConstr.end(); aDIt++)
 223         if ((aDIt->ptA == theLine.point[0] && aDIt->ptB == theLine.point[1]) ||
 224             (aDIt->ptA == theLine.point[1] && aDIt->ptB == theLine.point[0]))
 225           return;
 226       // Calculate distance between points on the line
 227       double aCoords[4];
 228       for (int i = 0; i < 2; i++) {
 229         Slvs_Entity aPnt = myStorage->getEntity(theLine.point[i]);
 230         for (int j = 0; j < 2; j++) {
 231           Slvs_Param aParam = myStorage->getParameter(aPnt.param[j]);
 232           aCoords[2*i+j] = aParam.val;
 233         }
 234       }
 235       double aLength = sqrt((aCoords[2] - aCoords[0]) * (aCoords[2] - aCoords[0]) +
 236                             (aCoords[3] - aCoords[1]) * (aCoords[3] - aCoords[1]));
 237       // fix line length
 238       Slvs_Constraint aDistance = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
 239           SLVS_C_PT_PT_DISTANCE, myGroup->getWorkplaneId(), aLength,
 240           theLine.point[0], theLine.point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
 241       aDistance.h = myStorage->addConstraint(aDistance);
 242       mySlvsConstraints.push_back(aDistance.h);
 243     }
 244     return;
 245   }
 246   else if (myStorage->isUsedInEqual(theLine.h, anEqual)) {
 247     // Check another entity of Equal is already fixed
 248     Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
 249     if (myStorage->isEntityFixed(anOtherEntID, true)) {
 250       // Fix start point of the line (if end point is not fixed yet) ...
 251       Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
 252       bool isFixed = myStorage->isPointFixed(theLine.point[1], anEndFixedID, true);
 253       if (isFixed == SLVS_E_UNKNOWN)
 254         fixPoint(theLine.point[0]);
 255       // ...  and create fixed point lying on this line
 256       Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
 257       // Firstly, search already fixed point on line
 258       bool isPonLineFixed = false;
 259       Slvs_hEntity aFixedPoint;
 260       std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
 261       std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
 262       for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
 263         if (aPLIter->entityA == theLine.h) {
 264           isPonLineFixed = myStorage->isPointFixed(aPLIter->ptA, anEndFixedID);
 265           aFixedPoint = aPLIter->ptA;
 266         }
 267
 268       if (isPonLineFixed) { // update existent constraint
 269         myStorage->copyEntity(aPointToCopy, aFixedPoint);
 270       } else { // create new constraint
 271         Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
 272         Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
 273             myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
 274         aPonLine.h = myStorage->addConstraint(aPonLine);
 275         mySlvsConstraints.push_back(aPonLine.h);
 276         fixPoint(aCopied);
 277       }
 278       return;
 279     }
 280   }
 281
 282   for (int i = 0; i < 2; i++)
 283     fixPoint(theLine.point[i]);
 284 }
 285
 286 void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
 287 {
 288   bool isFixRadius = true;
 289   // Verify the arc is under Equal constraint
 290   Slvs_Constraint anEqual;
 291   if (myStorage->isUsedInEqual(theCircle.h, anEqual)) {
 292     // Check another entity of Equal is already fixed
 293     Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
 294     if (myStorage->isEntityFixed(anOtherEntID, true))
 295       isFixRadius = false;
 296   }
 297
 298   fixPoint(theCircle.point[0]);
 299
 300   if (isFixRadius) {
 301     // Search the radius is already fixed
 302     std::list<Slvs_Constraint> aDiamConstr = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
 303     std::list<Slvs_Constraint>::const_iterator aDiamIter = aDiamConstr.begin();
 304     for (; aDiamIter != aDiamConstr.end(); aDiamIter++)
 305       if (aDiamIter->entityA == theCircle.h)
 306         return;
 307
 308     // Fix radius of a circle
 309     AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
 310         myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
 311     Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
 312         myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 313         myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
 314     aFixedR.h = myStorage->addConstraint(aFixedR);
 315     mySlvsConstraints.push_back(aFixedR.h);
 316   }
 317 }
 318
 319 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
 320 {
 321   bool isFixRadius = true;
 322   std::list<Slvs_hEntity> aPointsToFix;
 323   aPointsToFix.push_back(theArc.point[1]);
 324   aPointsToFix.push_back(theArc.point[2]);
 325
 326   // Verify the arc is under Equal constraint
 327   Slvs_Constraint anEqual;
 328   if (myStorage->isUsedInEqual(theArc.h, anEqual)) {
 329     // Check another entity of Equal is already fixed
 330     Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
 331     if (myStorage->isEntityFixed(anOtherEntID, true)) {
 332       isFixRadius = false;
 333       Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
 334       if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
 335         aPointsToFix.pop_back();
 336         aPointsToFix.push_back(theArc.point[0]);
 337       }
 338     }
 339   }
 340
 341   Slvs_hConstraint aConstrID;
 342   int aNbPointsToFix = 2; // number of fixed points for the arc
 343   if (myStorage->isPointFixed(theArc.point[0], aConstrID, true))
 344     aNbPointsToFix--;
 345
 346   // Radius of the arc
 347   FeaturePtr aFeature = myFeatureMap.begin()->first;
 348   std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 349     aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
 350   std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 351     aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
 352   double aRadius = aCenter->distance(aStart);
 353
 354   // Update end point of the arc to be on a curve
 355   std::shared_ptr<GeomAPI_Pnt2d> anEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
 356     aFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
 357   double aDistance = anEnd->distance(aCenter);
 358   std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
 359   if (aDistance < tolerance)
 360     aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
 361   else
 362     aDir = aDir->multiplied(aRadius / aDistance);
 363   double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
 364   Slvs_Entity aEndPoint = myStorage->getEntity(theArc.point[2]);
 365   for (int i = 0; i < 2; i++) {
 366     Slvs_Param aParam = myStorage->getParameter(aEndPoint.param[i]);
 367     aParam.val = xy[i];
 368     myStorage->updateParameter(aParam);
 369   }
 370
 371   std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
 372   for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
 373     fixPoint(*aPtIt);
 374
 375   if (isFixRadius) {
 376     // Fix radius of the arc
 377     bool isExists = false;
 378     std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
 379     std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
 380     for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
 381       if (anIt->entityA == theArc.h)
 382         isExists = true;
 383     if (!isExists) {
 384       Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
 385           myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
 386           myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
 387       aFixedR.h = myStorage->addConstraint(aFixedR);
 388       mySlvsConstraints.push_back(aFixedR.h);
 389       if (!myBaseConstraint)
 390         myStorage->addConstraintWhereDragged(aFixedR.h);
 391     }
 392   }
 393 }