]> SALOME platform Git repositories - modules/shaper.git/blob - src/SketchSolver/SketchSolver_ConstraintRigid.cpp
Salome HOME
6082f2fd41cb57e3d3ee3e517612888cf725ca56
[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
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 }