src/SketchSolver/SketchSolver_ConstraintDistance.cpp

   1 // Copyright (C) 2014-2023  CEA, EDF
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 #include <SketchSolver_ConstraintDistance.h>
  21 #include <SketchSolver_Error.h>
  22 #include <SketchSolver_Manager.h>
  23
  24 #include <PlaneGCSSolver_EdgeWrapper.h>
  25 #include <PlaneGCSSolver_PointWrapper.h>
  26 #include <PlaneGCSSolver_Storage.h>
  27 #include <PlaneGCSSolver_Tools.h>
  28 #include <PlaneGCSSolver_UpdateCoincidence.h>
  29
  30 #include <SketchPlugin_ConstraintDistanceHorizontal.h>
  31 #include <SketchPlugin_ConstraintDistanceVertical.h>
  32
  33 #include <GeomAPI_Dir2d.h>
  34 #include <GeomAPI_Lin2d.h>
  35 #include <GeomAPI_Pnt2d.h>
  36 #include <GeomAPI_XY.h>
  37
  38 #include <math.h>
  39
  40
  41 static void getPointAndLine(const ConstraintPtr& theConstraint, const StoragePtr& theStorage,
  42                             EntityWrapperPtr& thePoint, EntityWrapperPtr& theLine)
  43 {
  44   for (int i = 0; i < 2; ++i) {
  45     AttributePtr anAttr = theConstraint->attribute(SketchPlugin_Constraint::ATTRIBUTE(i));
  46     EntityWrapperPtr anEntity = theStorage->entity(anAttr);
  47     if (anEntity->type() == ENTITY_POINT)
  48       thePoint = anEntity;
  49     else if (anEntity->type() == ENTITY_LINE)
  50       theLine = anEntity;
  51   }
  52 }
  53
  54 static void adjustOddPoint(const EntityWrapperPtr& theDistPoint,
  55                            const EntityWrapperPtr& theDistLine,
  56                            GCSPointPtr& theOddPoint)
  57 {
  58   if (!theOddPoint)
  59     return;
  60
  61   std::shared_ptr<GeomAPI_Lin2d> aLine = PlaneGCSSolver_Tools::line(theDistLine);
  62   std::shared_ptr<GeomAPI_Pnt2d> aPoint = PlaneGCSSolver_Tools::point(theDistPoint);
  63
  64   std::shared_ptr<GeomAPI_XY> aLineVec = aLine->direction()->xy();
  65   std::shared_ptr<GeomAPI_XY> aPtLineVec = aPoint->xy()->decreased(aLine->location()->xy());
  66
  67   double aDot = aPtLineVec->dot(aLineVec);
  68   std::shared_ptr<GeomAPI_XY> aProjectedPnt =
  69     aLine->location()->xy()->added(aLineVec->multiplied(aDot));
  70
  71   *(theOddPoint->x) = aProjectedPnt->x();
  72   *(theOddPoint->y) = aProjectedPnt->y();
  73 }
  74
  75 static FeaturePtr getFeature(AttributeRefAttrPtr theRefAttr)
  76 {
  77   ObjectPtr anObj;
  78   if (theRefAttr->isObject())
  79     anObj = theRefAttr->object();
  80   else
  81     anObj = theRefAttr->attr()->owner();
  82   return ModelAPI_Feature::feature(anObj);
  83 }
  84
  85 static void calculateDistanceDirection(const ConstraintPtr& theConstraint,
  86                                        const StoragePtr& theStorage,
  87                                        double& theDirX, double& theDirY)
  88 {
  89   std::shared_ptr<GeomDataAPI_Dir> aDistDir = std::dynamic_pointer_cast<GeomDataAPI_Dir>(
  90       theConstraint->attribute(SketchPlugin_ConstraintDistance::DIRECTION_ID()));
  91   if (aDistDir && aDistDir->isInitialized()) {
  92     theDirX = aDistDir->x();
  93     theDirY = aDistDir->y();
  94     if (fabs(theDirX) > tolerance || fabs(theDirY) > tolerance)
  95       return;
  96   }
  97
  98   AttributeRefAttrPtr aRefAttrA = theConstraint->refattr(SketchPlugin_Constraint::ENTITY_A());
  99   AttributeRefAttrPtr aRefAttrB = theConstraint->refattr(SketchPlugin_Constraint::ENTITY_B());
 100
 101   EntityWrapperPtr aEntityA = theStorage->entity(aRefAttrA);
 102   EntityWrapperPtr aEntityB = theStorage->entity(aRefAttrB);
 103
 104   GCSPointPtr aPoint;
 105   if (aEntityA->type() != ENTITY_LINE && aEntityB->type() != ENTITY_LINE) {
 106     aPoint = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(aEntityA)->point();
 107     theDirX = 1.0;
 108     theDirY = 0.0;
 109
 110     EdgeWrapperPtr anEdgeA = std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(
 111         theStorage->entity(getFeature(aRefAttrA)));
 112     EdgeWrapperPtr anEdgeB = std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(
 113         theStorage->entity(getFeature(aRefAttrB)));
 114
 115     if (anEdgeA && anEdgeB) {
 116       GCS::DeriVector2 aDirA = anEdgeA->entity()->CalculateNormal(*aPoint);
 117       GCS::DeriVector2 aDirB = anEdgeB->entity()->CalculateNormal(*aPoint);
 118       double x = -aDirA.x + aDirB.x;
 119       double y = -aDirA.y + aDirB.y;
 120       double norm = sqrt(x*x + y*y);
 121       if (norm > tolerance) {
 122         theDirX = x / norm;
 123         theDirY = y / norm;
 124       }
 125     }
 126   }
 127 }
 128
 129 static void moveEntity(const ConstraintPtr& theConstraint,
 130                        const StoragePtr& theStorage,
 131                        const double theDX, const double theDY)
 132 {
 133   static const double THE_SHIFT = 1.e-4;
 134
 135   AttributeRefAttrPtr aRefAttrA = theConstraint->refattr(SketchPlugin_Constraint::ENTITY_A());
 136   AttributeRefAttrPtr aRefAttrB = theConstraint->refattr(SketchPlugin_Constraint::ENTITY_B());
 137
 138   EntityWrapperPtr aEntityA = theStorage->entity(aRefAttrA);
 139   EntityWrapperPtr aEntityB = theStorage->entity(aRefAttrB);
 140
 141   PointWrapperPtr aPointA = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(aEntityA);
 142   PointWrapperPtr aPointB = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(aEntityB);
 143
 144   if (aPointA) {
 145     *aPointA->point()->x -= THE_SHIFT * theDX;
 146     *aPointA->point()->y -= THE_SHIFT * theDY;
 147   }
 148   else if (aPointB) {
 149     *aPointB->point()->x += THE_SHIFT * theDX;
 150     *aPointB->point()->y += THE_SHIFT * theDY;
 151   }
 152 }
 153
 154
 155
 156 void SketchSolver_ConstraintDistance::getAttributes(
 157     EntityWrapperPtr& theValue,
 158     std::vector<EntityWrapperPtr>& theAttributes)
 159 {
 160   SketchSolver_Constraint::getAttributes(theValue, theAttributes);
 161   if (!myErrorMsg.empty() || !theAttributes[0]) {
 162     theAttributes.clear();
 163     return;
 164   }
 165
 166   ScalarWrapperPtr aValue = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(theValue);
 167   bool isCoincidence = fabs(aValue->value()) < tolerance;
 168
 169   if (theAttributes[1]) {
 170     if (myBaseConstraint->getKind() == SketchPlugin_ConstraintDistanceHorizontal::ID())
 171       myType = CONSTRAINT_HORIZONTAL_DISTANCE;
 172     else if (myBaseConstraint->getKind() == SketchPlugin_ConstraintDistanceVertical::ID())
 173       myType = CONSTRAINT_VERTICAL_DISTANCE;
 174     else
 175       myType = isCoincidence ? CONSTRAINT_PT_PT_COINCIDENT : CONSTRAINT_PT_PT_DISTANCE;
 176   } else if (theAttributes[2] && theAttributes[2]->type() == ENTITY_LINE)
 177     myType = isCoincidence ? CONSTRAINT_PT_ON_CURVE : CONSTRAINT_PT_LINE_DISTANCE;
 178   else
 179     theAttributes.clear();
 180
 181   if (myType == CONSTRAINT_HORIZONTAL_DISTANCE || myType == CONSTRAINT_VERTICAL_DISTANCE)
 182     mySignValue = aValue->value() < 0.0 ? -1.0 : 1.0;
 183
 184   myPrevValue = 0.0;
 185
 186   if (isCoincidence)
 187     myStorage->subscribeUpdates(this, PlaneGCSSolver_UpdateCoincidence::GROUP());
 188   else
 189     myStorage->subscribeUpdates(this, PlaneGCSSolver_UpdateFeature::GROUP());
 190 }
 191
 192 void SketchSolver_ConstraintDistance::adjustConstraint()
 193 {
 194   if (getType() == CONSTRAINT_PT_LINE_DISTANCE) {
 195     bool isSigned = myBaseConstraint->boolean(SketchPlugin_ConstraintDistance::SIGNED())->value();
 196     if (myIsSigned == isSigned) {
 197       // adjust auxiliary point for sign-keeping
 198       if (isSigned) {
 199         EntityWrapperPtr aDistPoint, aDistLine;
 200         getPointAndLine(myBaseConstraint, myStorage, aDistPoint, aDistLine);
 201         adjustOddPoint(aDistPoint, aDistLine, myOddPoint);
 202       }
 203     }
 204     else {
 205       // Adjust point-line distance by setting/removing additional constraints
 206       if (isSigned)
 207         addConstraintsToKeepSign();
 208       else
 209         removeConstraintsKeepingSign();
 210     }
 211     myIsSigned = isSigned;
 212   }
 213 }
 214
 215 void SketchSolver_ConstraintDistance::update()
 216 {
 217   ConstraintWrapperPtr aConstraint = myStorage->constraint(myBaseConstraint);
 218   myPrevValue = aConstraint->value();
 219
 220   bool isDistanceAlognDir =
 221     myBaseConstraint->getKind() == SketchPlugin_ConstraintDistanceHorizontal::ID() ||
 222     myBaseConstraint->getKind() == SketchPlugin_ConstraintDistanceVertical::ID();
 223
 224   AttributeDoublePtr aCurValue = myBaseConstraint->real(SketchPlugin_Constraint::VALUE());
 225   bool isZeroSwitch = fabs(myPrevValue) < tolerance && fabs(aCurValue->value()) > tolerance;
 226   bool isNonZeroSwitch = fabs(myPrevValue) > tolerance && fabs(aCurValue->value()) < tolerance;
 227
 228   if (!isDistanceAlognDir && (isZeroSwitch || isNonZeroSwitch)) {
 229     // the value is changed from non-zero to zero or vice versa
 230     remove();
 231     process();
 232
 233     // move entities to avoid conflicting constraints
 234     if (isZeroSwitch) {
 235       double aDirX, aDirY;
 236       // calculate the direction basing on the distanced objects
 237       calculateDistanceDirection(myBaseConstraint, myStorage, aDirX, aDirY);
 238       moveEntity(myBaseConstraint, myStorage, aDirX, aDirY);
 239
 240       if (myOddPoint) {
 241         removeConstraintsKeepingSign();
 242         addConstraintsToKeepSign();
 243       }
 244     }
 245   }
 246   else {
 247     SketchSolver_Constraint::update();
 248     if (isDistanceAlognDir && mySignValue * aConstraint->value() < 0.0) {
 249       if (isZeroSwitch)
 250         aConstraint->setValue(-aConstraint->value());
 251       else
 252         mySignValue *= -1.0;
 253     }
 254   }
 255 }
 256
 257 bool SketchSolver_ConstraintDistance::remove()
 258 {
 259   removeConstraintsKeepingSign();
 260   return SketchSolver_Constraint::remove();
 261 }
 262
 263 void SketchSolver_ConstraintDistance::addConstraintsToKeepSign()
 264 {
 265   std::shared_ptr<PlaneGCSSolver_Storage> aStorage =
 266       std::dynamic_pointer_cast<PlaneGCSSolver_Storage>(myStorage);
 267
 268   ConstraintWrapperPtr aConstraint = aStorage->constraint(myBaseConstraint);
 269   std::list<GCSConstraintPtr> aGCSConstraints = aConstraint->constraints();
 270
 271   // calculate projection of the point on the line and find a sign of a distance
 272   EntityWrapperPtr aDistPoint, aDistLine;
 273   getPointAndLine(myBaseConstraint, myStorage, aDistPoint, aDistLine);
 274
 275   std::shared_ptr<GeomAPI_Lin2d> aLine = PlaneGCSSolver_Tools::line(aDistLine);
 276   std::shared_ptr<GeomAPI_Pnt2d> aPoint = PlaneGCSSolver_Tools::point(aDistPoint);
 277
 278   std::shared_ptr<GeomAPI_XY> aLineVec = aLine->direction()->xy();
 279   std::shared_ptr<GeomAPI_XY> aPtLineVec = aPoint->xy()->decreased(aLine->location()->xy());
 280   if (aLineVec->cross(aPtLineVec) >= 0.)
 281     mySignValue = PI/2.0;
 282   else
 283     mySignValue = - PI/2.0;
 284
 285   // create auxiliary point on the line and set auxiliary constraints
 286   myOddPoint = GCSPointPtr(new GCS::Point);
 287   myOddPoint->x = aStorage->createParameter();
 288   myOddPoint->y = aStorage->createParameter();
 289   adjustOddPoint(aDistPoint, aDistLine, myOddPoint);
 290
 291   PointWrapperPtr aPointWr = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(aDistPoint);
 292   EdgeWrapperPtr anEdgeWr = std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(aDistLine);
 293   std::shared_ptr<GCS::Line> aGCSLine = std::dynamic_pointer_cast<GCS::Line>(anEdgeWr->entity());
 294   // point-on-line
 295   GCSConstraintPtr aNewConstraint(new GCS::ConstraintPointOnLine(*myOddPoint, *aGCSLine));
 296   aGCSConstraints.push_back(aNewConstraint);
 297   // angle which keep orientation
 298   aNewConstraint = GCSConstraintPtr(new GCS::ConstraintL2LAngle(
 299       aGCSLine->p1, aGCSLine->p2, *myOddPoint, *(aPointWr->point()), &mySignValue));
 300   aGCSConstraints.push_back(aNewConstraint);
 301
 302   aConstraint->setConstraints(aGCSConstraints);
 303
 304   aStorage->removeConstraint(myBaseConstraint);
 305   aStorage->addConstraint(myBaseConstraint, aConstraint);
 306 }
 307
 308 void SketchSolver_ConstraintDistance::removeConstraintsKeepingSign()
 309 {
 310   if (!myOddPoint)
 311     return; // no sign kept => nothing to remove
 312
 313   std::shared_ptr<PlaneGCSSolver_Storage> aStorage =
 314       std::dynamic_pointer_cast<PlaneGCSSolver_Storage>(myStorage);
 315
 316   ConstraintWrapperPtr aConstraint = aStorage->constraint(myBaseConstraint);
 317   std::list<GCSConstraintPtr> aGCSConstraints = aConstraint->constraints();
 318
 319   aStorage->removeConstraint(myBaseConstraint);
 320
 321   // remove parameters related to auxiliary point
 322   GCS::SET_pD aParams;
 323   aParams.insert(myOddPoint->x);
 324   aParams.insert(myOddPoint->y);
 325   aStorage->removeParameters(aParams);
 326
 327   // remove constraints keeping sign of point-line distance,
 328   // not more than 2 additional constraints is possible
 329   if (!aGCSConstraints.empty())
 330     aGCSConstraints.pop_back();
 331   if (!aGCSConstraints.empty())
 332     aGCSConstraints.pop_back();
 333   aConstraint->setConstraints(aGCSConstraints);
 334   aStorage->addConstraint(myBaseConstraint, aConstraint);
 335
 336   myIsSigned = false;
 337 }
 338
 339 void SketchSolver_ConstraintDistance::notify(const FeaturePtr& theFeature,
 340                                              PlaneGCSSolver_Update*)
 341 {
 342   if (getType() == CONSTRAINT_PT_LINE_DISTANCE && myIsSigned &&
 343       theFeature->getKind() == SketchPlugin_Sketch::ID()) {
 344     // the sketch plane was updated, recalculate auxiliary constraints
 345     removeConstraintsKeepingSign();
 346     addConstraintsToKeepSign();
 347     myIsSigned = true; // reset it, due to changing by removeConstraintsKeepingSign()
 348   }
 349 }