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>
9 #include <SketchPlugin_Point.h>
11 #include <GeomAPI_Pnt2d.h>
12 #include <GeomAPI_XY.h>
13 #include <GeomDataAPI_Point2D.h>
14 #include <ModelAPI_AttributeDouble.h>
18 SketchSolver_ConstraintRigid::SketchSolver_ConstraintRigid(FeaturePtr theFeature)
19 : SketchSolver_Constraint(),
20 myBaseFeature(theFeature)
25 void SketchSolver_ConstraintRigid::process()
28 if ((!myBaseConstraint && !myBaseFeature) || !myStorage || myGroup == 0) {
29 /// TODO: Put error message here
32 if (!mySlvsConstraints.empty()) // some data is changed, update constraint
33 update(myBaseConstraint);
36 std::vector<Slvs_hEntity> anEntities;
37 getAttributes(aValue, anEntities);
38 if (!myErrorMsg.empty() || (myFeatureMap.empty() && myAttributeMap.empty()))
43 void SketchSolver_ConstraintRigid::update(ConstraintPtr theConstraint)
46 if (theConstraint && theConstraint == myBaseConstraint &&
47 theConstraint->getKind() == myBaseConstraint->getKind() &&
48 checkAttributesChanged(theConstraint)) {
49 // remove previous constraint and set the given one
50 remove(myBaseConstraint);
51 myBaseConstraint = theConstraint;
56 static void fixEntity(StoragePtr theStorage, const Slvs_hEntity& theEntID)
58 Slvs_Entity anEntity = theStorage->getEntity(theEntID);
59 anEntity.group = SLVS_G_OUTOFGROUP;
60 theStorage->updateEntity(anEntity);
61 // move out of group all sub-entities
62 for (int i = 0; i < 4; ++i)
63 if (anEntity.point[i] != SLVS_E_UNKNOWN)
64 fixEntity(theStorage, anEntity.point[i]);
65 // move out of group the radius of circle
66 if (anEntity.distance != SLVS_E_UNKNOWN)
67 fixEntity(theStorage, anEntity.distance);
68 // move out of group parameters
69 for (int i = 0; i < 4; ++i)
70 if (anEntity.param[i] != SLVS_E_UNKNOWN) {
71 Slvs_Param aParam = theStorage->getParameter(anEntity.param[i]);
72 aParam.group = SLVS_G_OUTOFGROUP;
73 theStorage->updateParameter(aParam);
77 void SketchSolver_ConstraintRigid::fixFeature()
79 Slvs_hEntity anEntID = fixedEntity();
80 if (anEntID != SLVS_E_UNKNOWN)
81 fixEntity(myStorage, anEntID);
84 Slvs_hEntity SketchSolver_ConstraintRigid::fixedEntity() const
86 Slvs_hEntity anEntID = SLVS_E_UNKNOWN;
87 if (myBaseConstraint) {
88 AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
89 myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
90 if (aRefAttr->isObject()) {
91 FeaturePtr aFeature = ModelAPI_Feature::feature(aRefAttr->object());
92 std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFound = myFeatureMap.find(aFeature);
93 if (aFound != myFeatureMap.end())
94 anEntID = aFound->second;
96 std::map<AttributePtr, Slvs_hEntity>::const_iterator aFound = myAttributeMap.find(aRefAttr->attr());
97 if (aFound != myAttributeMap.end())
98 anEntID = aFound->second;
101 else if (myBaseFeature) {
102 std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFound = myFeatureMap.find(myBaseFeature);
103 if (aFound != myFeatureMap.end())
104 anEntID = aFound->second;
109 void SketchSolver_ConstraintRigid::getAttributes(
111 std::vector<Slvs_hEntity>& theAttributes)
114 int aType = SLVS_E_UNKNOWN; // type of created entity
115 Slvs_hEntity anEntityID = SLVS_E_UNKNOWN;
116 if (myBaseConstraint) {
117 // Get the attribute of constraint
118 AttributeRefAttrPtr aRefAttr = std::dynamic_pointer_cast<ModelAPI_AttributeRefAttr>(
119 myBaseConstraint->attribute(SketchPlugin_ConstraintRigid::ENTITY_A()));
120 if (!aRefAttr || !aRefAttr->isInitialized()) {
121 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
124 anEntityID = myGroup->getAttributeId(aRefAttr);
125 if (anEntityID == SLVS_E_UNKNOWN)
126 anEntityID = changeEntity(aRefAttr, aType);
128 anEntityID = myGroup->getFeatureId(myBaseFeature);
129 if (anEntityID == SLVS_E_UNKNOWN)
130 anEntityID = changeEntity(myBaseFeature, aType);
132 myFeatureMap[myBaseFeature] = anEntityID;
135 if (anEntityID == SLVS_E_UNKNOWN) {
136 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
140 // Check the entity is complex
141 Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
142 if (anEntity.point[0] != SLVS_E_UNKNOWN) {
143 for (int i = 0; i < 4 && anEntity.point[i]; i++)
144 theAttributes.push_back(anEntity.point[i]);
145 } else // simple entity
146 theAttributes.push_back(anEntityID);
150 bool SketchSolver_ConstraintRigid::remove(ConstraintPtr theConstraint)
153 if (theConstraint && theConstraint != myBaseConstraint)
155 bool isFullyRemoved = true;
157 std::vector<Slvs_hConstraint>::iterator aCIter = mySlvsConstraints.begin();
158 for (; aCIter != mySlvsConstraints.end(); ++aCIter)
159 isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
161 std::map<FeaturePtr, Slvs_hEntity>::iterator aFIter = myFeatureMap.begin();
162 for (; aFIter != myFeatureMap.end(); ++aFIter)
163 isFullyRemoved = myStorage->removeEntity(aFIter->second) && isFullyRemoved;
165 std::map<AttributePtr, Slvs_hEntity>::iterator anAtIter = myAttributeMap.begin();
166 for (; anAtIter != myAttributeMap.end(); ++anAtIter)
167 isFullyRemoved = myStorage->removeEntity(anAtIter->second) && isFullyRemoved;
169 if (isFullyRemoved) {
170 myFeatureMap.clear();
171 myAttributeMap.clear();
173 mySlvsConstraints.clear();
175 cleanRemovedEntities();
179 Slvs_hConstraint SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
181 if (thePointID == SLVS_E_UNKNOWN)
182 return SLVS_C_UNKNOWN;
184 Slvs_Constraint aConstraint;
185 Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
186 bool isFixed = myStorage->isPointFixed(thePointID, aConstrID, true);
187 bool isForceUpdate = (isFixed && !myBaseConstraint &&
188 myStorage->isTemporary(aConstrID));
189 if (!isForceUpdate) { // create new constraint
190 if (isFixed) return aConstrID;
191 aConstraint = Slvs_MakeConstraint(SLVS_C_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
192 0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
193 aConstraint.h = myStorage->addConstraint(aConstraint);
194 mySlvsConstraints.push_back(aConstraint.h);
195 if (!myBaseConstraint)
196 myStorage->addConstraintWhereDragged(aConstraint.h);
197 } else { // update already existent constraint
198 if (!isFixed || aConstrID == SLVS_C_UNKNOWN || myBaseConstraint)
199 return SLVS_C_UNKNOWN;
200 aConstraint = myStorage->getConstraint(aConstrID);
201 aConstraint.ptA = thePointID;
202 myStorage->addConstraint(aConstraint);
203 if (!myBaseConstraint)
204 myStorage->addConstraintWhereDragged(aConstraint.h);
206 return aConstraint.h;
209 void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
211 Slvs_Constraint anEqual;
212 if (myStorage->isAxisParallel(theLine.h)) {
213 // Fix one point and a line length
214 Slvs_hConstraint aFixed;
215 if (!myStorage->isPointFixed(theLine.point[0], aFixed, true) &&
216 !myStorage->isPointFixed(theLine.point[1], aFixed, true))
217 fixPoint(theLine.point[0]);
218 if (!myStorage->isUsedInEqual(theLine.h, anEqual)) {
219 // Check the distance is not set yet
220 std::list<Slvs_Constraint> aDistConstr = myStorage->getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
221 std::list<Slvs_Constraint>::const_iterator aDIt = aDistConstr.begin();
222 for (; aDIt != aDistConstr.end(); aDIt++)
223 if ((aDIt->ptA == theLine.point[0] && aDIt->ptB == theLine.point[1]) ||
224 (aDIt->ptA == theLine.point[1] && aDIt->ptB == theLine.point[0]))
226 // Calculate distance between points on the line
228 for (int i = 0; i < 2; i++) {
229 Slvs_Entity aPnt = myStorage->getEntity(theLine.point[i]);
230 for (int j = 0; j < 2; j++) {
231 Slvs_Param aParam = myStorage->getParameter(aPnt.param[j]);
232 aCoords[2*i+j] = aParam.val;
235 double aLength = sqrt((aCoords[2] - aCoords[0]) * (aCoords[2] - aCoords[0]) +
236 (aCoords[3] - aCoords[1]) * (aCoords[3] - aCoords[1]));
238 Slvs_Constraint aDistance = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
239 SLVS_C_PT_PT_DISTANCE, myGroup->getWorkplaneId(), aLength,
240 theLine.point[0], theLine.point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
241 aDistance.h = myStorage->addConstraint(aDistance);
242 mySlvsConstraints.push_back(aDistance.h);
246 else if (myStorage->isUsedInEqual(theLine.h, anEqual)) {
247 // Check another entity of Equal is already fixed
248 Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
249 if (myStorage->isEntityFixed(anOtherEntID, true)) {
250 // Fix start point of the line (if end point is not fixed yet) ...
251 Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
252 bool isFixed = myStorage->isPointFixed(theLine.point[1], anEndFixedID, true);
253 if (isFixed == SLVS_E_UNKNOWN)
254 fixPoint(theLine.point[0]);
255 // ... and create fixed point lying on this line
256 Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
257 // Firstly, search already fixed point on line
258 bool isPonLineFixed = false;
259 Slvs_hEntity aFixedPoint;
260 std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
261 std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
262 for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
263 if (aPLIter->entityA == theLine.h) {
264 isPonLineFixed = myStorage->isPointFixed(aPLIter->ptA, anEndFixedID);
265 aFixedPoint = aPLIter->ptA;
268 if (isPonLineFixed) { // update existent constraint
269 myStorage->copyEntity(aPointToCopy, aFixedPoint);
270 } else { // create new constraint
271 Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
272 Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
273 myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
274 aPonLine.h = myStorage->addConstraint(aPonLine);
275 mySlvsConstraints.push_back(aPonLine.h);
282 for (int i = 0; i < 2; i++)
283 fixPoint(theLine.point[i]);
286 void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
288 bool isFixRadius = true;
289 // Verify the arc is under Equal constraint
290 Slvs_Constraint anEqual;
291 if (myStorage->isUsedInEqual(theCircle.h, anEqual)) {
292 // Check another entity of Equal is already fixed
293 Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
294 if (myStorage->isEntityFixed(anOtherEntID, true))
298 fixPoint(theCircle.point[0]);
301 // Search the radius is already fixed
302 std::list<Slvs_Constraint> aDiamConstr = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
303 std::list<Slvs_Constraint>::const_iterator aDiamIter = aDiamConstr.begin();
304 for (; aDiamIter != aDiamConstr.end(); aDiamIter++)
305 if (aDiamIter->entityA == theCircle.h)
308 // Fix radius of a circle
309 AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
310 myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
311 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
312 myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
313 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
314 aFixedR.h = myStorage->addConstraint(aFixedR);
315 mySlvsConstraints.push_back(aFixedR.h);
319 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
321 bool isFixRadius = true;
322 std::list<Slvs_hEntity> aPointsToFix;
323 aPointsToFix.push_back(theArc.point[1]);
324 aPointsToFix.push_back(theArc.point[2]);
326 // Verify the arc is under Equal constraint
327 Slvs_Constraint anEqual;
328 if (myStorage->isUsedInEqual(theArc.h, anEqual)) {
329 // Check another entity of Equal is already fixed
330 Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
331 if (myStorage->isEntityFixed(anOtherEntID, true)) {
333 Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
334 if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
335 aPointsToFix.pop_back();
336 aPointsToFix.push_back(theArc.point[0]);
341 Slvs_hConstraint aConstrID;
342 int aNbPointsToFix = 2; // number of fixed points for the arc
343 if (myStorage->isPointFixed(theArc.point[0], aConstrID, true))
347 FeaturePtr aFeature = myFeatureMap.begin()->first;
348 std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
349 aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
350 std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
351 aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
352 double aRadius = aCenter->distance(aStart);
354 // Update end point of the arc to be on a curve
355 std::shared_ptr<GeomAPI_Pnt2d> anEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
356 aFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
357 double aDistance = anEnd->distance(aCenter);
358 std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
359 if (aDistance < tolerance)
360 aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
362 aDir = aDir->multiplied(aRadius / aDistance);
363 double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
364 Slvs_Entity aEndPoint = myStorage->getEntity(theArc.point[2]);
365 for (int i = 0; i < 2; i++) {
366 Slvs_Param aParam = myStorage->getParameter(aEndPoint.param[i]);
368 myStorage->updateParameter(aParam);
371 std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
372 for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
376 // Fix radius of the arc
377 bool isExists = false;
378 std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
379 std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
380 for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
381 if (anIt->entityA == theArc.h)
384 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
385 myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
386 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
387 aFixedR.h = myStorage->addConstraint(aFixedR);
388 mySlvsConstraints.push_back(aFixedR.h);
389 if (!myBaseConstraint)
390 myStorage->addConstraintWhereDragged(aFixedR.h);