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