]> SALOME platform Git repositories - modules/shaper.git/blob - src/SketchSolver/SketchSolver_ConstraintRigid.cpp
Salome HOME
c5043d945674fdbd04cdee7d0f6b35bee654ddd0
[modules/shaper.git] / 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     mySlvsConstraints.clear();
167   } else
168     cleanRemovedEntities();
169   return true;
170 }
171
172 void SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
173 {
174   if (thePointID == SLVS_E_UNKNOWN)
175     return;
176
177   Slvs_Constraint aConstraint;
178   Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
179   bool isFixed = myStorage->isPointFixed(thePointID, aConstrID, true);
180   bool isForceUpdate = (isFixed && !myBaseConstraint &&
181                         myStorage->isTemporary(aConstrID));
182   if (!isForceUpdate) { // create new constraint
183     if (isFixed) return;
184     aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
185         0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
186     aConstraint.h = myStorage->addConstraint(aConstraint);
187     mySlvsConstraints.push_back(aConstraint.h);
188     if (!myBaseConstraint)
189       myStorage->addConstraintWhereDragged(aConstraint.h);
190   } else { // update already existent constraint
191     if (!isFixed || aConstrID == SLVS_E_UNKNOWN || myBaseConstraint)
192       return;
193     aConstraint = myStorage->getConstraint(aConstrID);
194     aConstraint.ptA = thePointID;
195     myStorage->addConstraint(aConstraint);
196     if (!myBaseConstraint)
197       myStorage->addConstraintWhereDragged(aConstraint.h);
198   }
199 }
200
201 void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
202 {
203   Slvs_Constraint anEqual;
204   if (isAxisParallel(theLine)) {
205     // Fix one point and a line length
206     Slvs_hConstraint aFixed;
207     if (!myStorage->isPointFixed(theLine.point[0], aFixed, true) &&
208         !myStorage->isPointFixed(theLine.point[1], aFixed, true))
209       fixPoint(theLine.point[0]);
210     if (!isUsedInEqual(theLine, anEqual)) {
211       // Check the distance is not set yet
212       std::list<Slvs_Constraint> aDistConstr = myStorage->getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
213       std::list<Slvs_Constraint>::const_iterator aDIt = aDistConstr.begin();
214       for (; aDIt != aDistConstr.end(); aDIt++)
215         if ((aDIt->ptA == theLine.point[0] && aDIt->ptB == theLine.point[1]) ||
216             (aDIt->ptA == theLine.point[1] && aDIt->ptB == theLine.point[0]))
217           return;
218       // Calculate distance between points on the line
219       double aCoords[4];
220       for (int i = 0; i < 2; i++) {
221         Slvs_Entity aPnt = myStorage->getEntity(theLine.point[i]);
222         for (int j = 0; j < 2; j++) {
223           Slvs_Param aParam = myStorage->getParameter(aPnt.param[j]);
224           aCoords[2*i+j] = aParam.val;
225         }
226       }
227       double aLength = sqrt((aCoords[2] - aCoords[0]) * (aCoords[2] - aCoords[0]) + 
228                             (aCoords[3] - aCoords[1]) * (aCoords[3] - aCoords[1]));
229       // fix line length
230       Slvs_Constraint aDistance = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
231           SLVS_C_PT_PT_DISTANCE, myGroup->getWorkplaneId(), aLength,
232           theLine.point[0], theLine.point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
233       aDistance.h = myStorage->addConstraint(aDistance);
234       mySlvsConstraints.push_back(aDistance.h);
235     }
236     return;
237   }
238   else if (isUsedInEqual(theLine, anEqual)) {
239     // Check another entity of Equal is already fixed
240     Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
241     if (myStorage->isEntityFixed(anOtherEntID, true)) {
242       // Fix start point of the line (if end point is not fixed yet) ...
243       Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
244       bool isFixed = myStorage->isPointFixed(theLine.point[1], anEndFixedID, true);
245       if (isFixed == SLVS_E_UNKNOWN)
246         fixPoint(theLine.point[0]);
247       // ...  and create fixed point lying on this line
248       Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
249       // Firstly, search already fixed point on line
250       bool isPonLineFixed = false;
251       Slvs_hEntity aFixedPoint;
252       std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
253       std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
254       for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
255         if (aPLIter->entityA == theLine.h) {
256           isPonLineFixed = myStorage->isPointFixed(aPLIter->ptA, anEndFixedID);
257           aFixedPoint = aPLIter->ptA;
258         }
259
260       if (isPonLineFixed) { // update existent constraint
261         myStorage->copyEntity(aPointToCopy, aFixedPoint);
262       } else { // create new constraint
263         Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
264         Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
265             myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
266         aPonLine.h = myStorage->addConstraint(aPonLine);
267         mySlvsConstraints.push_back(aPonLine.h);
268         fixPoint(aCopied);
269       }
270       return;
271     }
272   }
273
274   for (int i = 0; i < 2; i++)
275     fixPoint(theLine.point[i]);
276 }
277
278 void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
279 {
280   bool isFixRadius = true;
281   // Verify the arc is under Equal constraint
282   Slvs_Constraint anEqual;
283   if (isUsedInEqual(theCircle, anEqual)) {
284     // Check another entity of Equal is already fixed
285     Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
286     if (myStorage->isEntityFixed(anOtherEntID, true))
287       isFixRadius = false;
288   }
289
290   fixPoint(theCircle.point[0]);
291
292   if (isFixRadius) {
293     // Search the radius is already fixed
294     std::list<Slvs_Constraint> aDiamConstr = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
295     std::list<Slvs_Constraint>::const_iterator aDiamIter = aDiamConstr.begin();
296     for (; aDiamIter != aDiamConstr.end(); aDiamIter++)
297       if (aDiamIter->entityA == theCircle.h)
298         return;
299
300     // Fix radius of a circle
301     AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
302         myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
303     Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
304         myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
305         myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
306     aFixedR.h = myStorage->addConstraint(aFixedR);
307     mySlvsConstraints.push_back(aFixedR.h);
308   }
309 }
310
311 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
312 {
313   bool isFixRadius = true;
314   std::list<Slvs_hEntity> aPointsToFix;
315   aPointsToFix.push_back(theArc.point[1]);
316   aPointsToFix.push_back(theArc.point[2]);
317
318   // Verify the arc is under Equal constraint
319   Slvs_Constraint anEqual;
320   if (isUsedInEqual(theArc, anEqual)) {
321     // Check another entity of Equal is already fixed
322     Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
323     if (myStorage->isEntityFixed(anOtherEntID, true)) {
324       isFixRadius = false;
325       Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
326       if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
327         aPointsToFix.pop_back();
328         aPointsToFix.push_back(theArc.point[0]);
329       }
330     }
331   }
332
333   Slvs_hConstraint aConstrID;
334   int aNbPointsToFix = 2; // number of fixed points for the arc
335   if (myStorage->isPointFixed(theArc.point[0], aConstrID, true))
336     aNbPointsToFix--;
337
338   // Radius of the arc
339   FeaturePtr aFeature = myFeatureMap.begin()->first;
340   std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
341     aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
342   std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
343     aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
344   double aRadius = aCenter->distance(aStart);
345
346   // Update end point of the arc to be on a curve
347   std::shared_ptr<GeomAPI_Pnt2d> anEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
348     aFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
349   double aDistance = anEnd->distance(aCenter);
350   std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
351   if (aDistance < tolerance)
352     aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
353   else
354     aDir = aDir->multiplied(aRadius / aDistance);
355   double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
356   Slvs_Entity aEndPoint = myStorage->getEntity(theArc.point[2]);
357   for (int i = 0; i < 2; i++) {
358     Slvs_Param aParam = myStorage->getParameter(aEndPoint.param[i]);
359     aParam.val = xy[i];
360     myStorage->updateParameter(aParam);
361   }
362
363   std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
364   for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
365     fixPoint(*aPtIt);
366
367   if (isFixRadius) {
368     // Fix radius of the arc
369     bool isExists = false;
370     std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
371     std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
372     for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
373       if (anIt->entityA == theArc.h)
374         isExists = true;
375     if (!isExists) {
376       Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
377           myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
378           myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
379       aFixedR.h = myStorage->addConstraint(aFixedR);
380       mySlvsConstraints.push_back(aFixedR.h);
381       if (!myBaseConstraint)
382         myStorage->addConstraintWhereDragged(aFixedR.h);
383     }
384   }
385 }
386
387 bool SketchSolver_ConstraintRigid::isUsedInEqual(
388     const Slvs_Entity& theEntity, Slvs_Constraint& theEqual) const
389 {
390   // Check the entity is used in Equal constraint
391   std::list<Slvs_Constraint> anEqualConstr = myStorage->getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
392   std::list<Slvs_Constraint> anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN);
393   anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
394   anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
395   anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
396
397   std::list<Slvs_Constraint>::const_iterator anEqIter = anEqualConstr.begin();
398   for (; anEqIter != anEqualConstr.end(); anEqIter++)
399     if (anEqIter->entityA == theEntity.h || anEqIter->entityB == theEntity.h) {
400       theEqual = *anEqIter;
401       return true;
402     }
403   return false;
404 }
405
406 bool SketchSolver_ConstraintRigid::isAxisParallel(const Slvs_Entity& theEntity) const
407 {
408   std::list<Slvs_Constraint> aConstr = myStorage->getConstraintsByType(SLVS_C_HORIZONTAL);
409   std::list<Slvs_Constraint> aVert = myStorage->getConstraintsByType(SLVS_C_VERTICAL);
410   aConstr.insert(aConstr.end(), aVert.begin(), aVert.end());
411
412   std::list<Slvs_Constraint>::const_iterator anIter = aConstr.begin();
413   for (; anIter != aConstr.end(); anIter++)
414     if (anIter->entityA == theEntity.h)
415       return true;
416   return false;
417 }