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