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