1 #include <SketchSolver_ConstraintRigid.h>
2 #include <SketchSolver_Error.h>
3 #include <SketchSolver_Group.h>
5 #include <SketchPlugin_Arc.h>
6 #include <SketchPlugin_Circle.h>
7 #include <SketchPlugin_ConstraintRigid.h>
8 #include <SketchPlugin_Line.h>
10 #include <GeomAPI_Pnt2d.h>
11 #include <GeomAPI_XY.h>
12 #include <GeomDataAPI_Point2D.h>
13 #include <ModelAPI_AttributeDouble.h>
15 SketchSolver_ConstraintRigid::SketchSolver_ConstraintRigid(FeaturePtr theFeature)
16 : SketchSolver_Constraint(),
17 myBaseFeature(theFeature)
22 void SketchSolver_ConstraintRigid::process()
25 if ((!myBaseConstraint && !myBaseFeature) || !myStorage || myGroup == 0) {
26 /// TODO: Put error message here
29 if (!mySlvsConstraints.empty()) // some data is changed, update constraint
30 update(myBaseConstraint);
33 std::vector<Slvs_hEntity> anEntities;
34 getAttributes(aValue, anEntities);
35 if (!myErrorMsg.empty() || myFeatureMap.empty())
38 if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Line::ID()) {
39 Slvs_Entity aLine = myStorage->getEntity(myFeatureMap.begin()->second);
42 else if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Arc::ID()) {
43 Slvs_Entity anArc = myStorage->getEntity(myFeatureMap.begin()->second);
46 else if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Circle::ID()) {
47 Slvs_Entity aCirc = myStorage->getEntity(myFeatureMap.begin()->second);
53 void SketchSolver_ConstraintRigid::getAttributes(
55 std::vector<Slvs_hEntity>& theAttributes)
58 int aType = SLVS_E_UNKNOWN; // type of created entity
59 Slvs_hEntity anEntityID = SLVS_E_UNKNOWN;
60 if (myBaseConstraint) {
61 // Get the attribute of constraint
62 AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
63 myBaseConstraint->attribute(SketchPlugin_ConstraintRigid::ENTITY_A()));
64 if (!aRefAttr || !aRefAttr->isInitialized()) {
65 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
68 anEntityID = myGroup->getAttributeId(aRefAttr);
69 if (anEntityID == SLVS_E_UNKNOWN)
70 anEntityID = changeEntity(aRefAttr, aType);
72 anEntityID = myGroup->getFeatureId(myBaseFeature);
73 if (anEntityID == SLVS_E_UNKNOWN)
74 anEntityID = changeEntity(myBaseFeature, aType);
76 myFeatureMap[myBaseFeature] = anEntityID;
79 if (anEntityID == SLVS_E_UNKNOWN) {
80 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
84 // Check the entity is complex
85 Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
86 if (anEntity.point[0] != SLVS_E_UNKNOWN) {
87 for (int i = 0; i < 4 && anEntity.point[i]; i++)
88 theAttributes.push_back(anEntity.point[i]);
89 } else // simple entity
90 theAttributes.push_back(anEntityID);
93 void SketchSolver_ConstraintRigid::adjustConstraint()
95 if (myFeatureMap.empty() || (
96 myFeatureMap.begin()->first->getKind() != SketchPlugin_Arc::ID() &&
97 myFeatureMap.begin()->first->getKind() != SketchPlugin_Circle::ID()))
99 FeaturePtr aFeature = myFeatureMap.begin()->first;
101 // Search radius constraints and update them
102 Slvs_Constraint aConstraint;
103 std::vector<Slvs_hConstraint>::iterator aCIter = mySlvsConstraints.begin();
104 for (; aCIter != mySlvsConstraints.end(); aCIter++) {
105 aConstraint = myStorage->getConstraint(*aCIter);
106 if (aConstraint.type != SLVS_C_DIAMETER)
108 double aRadius = 0.0;
109 if (aFeature->getKind() == SketchPlugin_Arc::ID()) {
110 std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
111 aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
112 std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
113 aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
114 aRadius = aCenter->distance(aStart);
116 aRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
117 aFeature->attribute(SketchPlugin_Circle::RADIUS_ID()))->value();
120 aConstraint.valA = aRadius * 2.0;
121 *aCIter = myStorage->updateConstraint(aConstraint);
126 bool SketchSolver_ConstraintRigid::remove(ConstraintPtr theConstraint)
129 if (theConstraint && theConstraint != myBaseConstraint)
131 bool isFullyRemoved = true;
132 std::vector<Slvs_hConstraint>::iterator aCIter = mySlvsConstraints.begin();
133 for (; aCIter != mySlvsConstraints.end(); aCIter++)
134 isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
136 if (isFullyRemoved) {
137 myFeatureMap.clear();
138 myAttributeMap.clear();
141 cleanRemovedEntities();
145 void SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
147 if (thePointID == SLVS_E_UNKNOWN)
150 Slvs_Constraint aConstraint;
151 Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
152 bool isFixed = myStorage->isPointFixed(thePointID, aConstrID, true);
153 bool isForceUpdate = (isFixed && !myBaseConstraint &&
154 myStorage->isTemporary(aConstrID));
155 if (!isForceUpdate) { // create new constraint
157 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
158 0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
159 aConstraint.h = myStorage->addConstraint(aConstraint);
160 mySlvsConstraints.push_back(aConstraint.h);
161 if (!myBaseConstraint)
162 myStorage->addConstraintWhereDragged(aConstraint.h);
163 } else { // update already existent constraint
164 if (!isFixed || aConstrID == SLVS_E_UNKNOWN || myBaseConstraint)
166 aConstraint = myStorage->getConstraint(aConstrID);
167 aConstraint.ptA = thePointID;
168 myStorage->addConstraint(aConstraint);
169 if (!myBaseConstraint)
170 myStorage->addConstraintWhereDragged(aConstraint.h);
174 void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
176 Slvs_Constraint anEqual;
177 if (isUsedInEqual(theLine, anEqual)) {
178 // Check another entity of Equal is already fixed
179 Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
180 if (myStorage->isEntityFixed(anOtherEntID, true)) {
181 // Fix start point of the line (if end point is not fixed yet) ...
182 Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
183 bool isFixed = myStorage->isPointFixed(theLine.point[1], anEndFixedID, true);
184 if (isFixed == SLVS_E_UNKNOWN)
185 fixPoint(theLine.point[0]);
186 // ... and create fixed point lying on this line
187 Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
188 // Firstly, search already fixed point on line
189 bool isPonLineFixed = false;
190 Slvs_hEntity aFixedPoint;
191 std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
192 std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
193 for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
194 if (aPLIter->entityA == theLine.h) {
195 isPonLineFixed = myStorage->isPointFixed(aPLIter->ptA, anEndFixedID);
196 aFixedPoint = aPLIter->ptA;
199 if (isPonLineFixed) { // update existent constraint
200 myStorage->copyEntity(aPointToCopy, aFixedPoint);
201 } else { // create new constraint
202 Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
203 Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
204 myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
205 aPonLine.h = myStorage->addConstraint(aPonLine);
206 mySlvsConstraints.push_back(aPonLine.h);
213 for (int i = 0; i < 2; i++)
214 fixPoint(theLine.point[i]);
217 void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
219 bool isFixRadius = true;
220 // Verify the arc is under Equal constraint
221 Slvs_Constraint anEqual;
222 if (isUsedInEqual(theCircle, anEqual)) {
223 // Check another entity of Equal is already fixed
224 Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
225 if (myStorage->isEntityFixed(anOtherEntID, true))
229 fixPoint(theCircle.point[0]);
232 // Fix radius of a circle
233 AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
234 myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
235 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
236 myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
237 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
238 aFixedR.h = myStorage->addConstraint(aFixedR);
239 mySlvsConstraints.push_back(aFixedR.h);
243 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
245 bool isFixRadius = true;
246 std::list<Slvs_hEntity> aPointsToFix;
247 aPointsToFix.push_back(theArc.point[1]);
248 aPointsToFix.push_back(theArc.point[2]);
250 // Verify the arc is under Equal constraint
251 Slvs_Constraint anEqual;
252 if (isUsedInEqual(theArc, anEqual)) {
253 // Check another entity of Equal is already fixed
254 Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
255 if (myStorage->isEntityFixed(anOtherEntID, true)) {
257 Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
258 if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
259 aPointsToFix.pop_back();
260 aPointsToFix.push_back(theArc.point[0]);
265 Slvs_hConstraint aConstrID;
266 int aNbPointsToFix = 2; // number of fixed points for the arc
267 if (myStorage->isPointFixed(theArc.point[0], aConstrID, true))
271 FeaturePtr aFeature = myFeatureMap.begin()->first;
272 std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
273 aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
274 std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
275 aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
276 double aRadius = aCenter->distance(aStart);
278 // Update end point of the arc to be on a curve
279 std::shared_ptr<GeomAPI_Pnt2d> anEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
280 aFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
281 double aDistance = anEnd->distance(aCenter);
282 std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
283 if (aDistance < tolerance)
284 aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
286 aDir = aDir->multiplied(aRadius / aDistance);
287 double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
288 Slvs_Entity aEndPoint = myStorage->getEntity(theArc.point[2]);
289 for (int i = 0; i < 2; i++) {
290 Slvs_Param aParam = myStorage->getParameter(aEndPoint.param[i]);
292 myStorage->updateParameter(aParam);
295 std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
296 for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
300 // Fix radius of the arc
301 bool isExists = false;
302 std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
303 std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
304 for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
305 if (anIt->entityA == myFeatureMap.begin()->second)
308 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
309 myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
310 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
311 aFixedR.h = myStorage->addConstraint(aFixedR);
312 mySlvsConstraints.push_back(aFixedR.h);
313 if (!myBaseConstraint)
314 myStorage->addConstraintWhereDragged(aFixedR.h);
319 bool SketchSolver_ConstraintRigid::isUsedInEqual(
320 const Slvs_Entity& theEntity, Slvs_Constraint& theEqual) const
322 // Check the entity is used in Equal constraint
323 std::list<Slvs_Constraint> anEqualConstr = myStorage->getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
324 std::list<Slvs_Constraint> anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN);
325 anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
326 anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
327 anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
329 std::list<Slvs_Constraint>::const_iterator anEqIter = anEqualConstr.begin();
330 for (; anEqIter != anEqualConstr.end(); anEqIter++)
331 if (anEqIter->entityA == theEntity.h || anEqIter->entityB == theEntity.h) {
332 theEqual = *anEqIter;