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::fixFeature()
46 if (!myFeatureMap.empty())
47 anEntID = myFeatureMap.begin()->second;
49 anEntID = myAttributeMap.begin()->second;
50 if (myStorage->isEntityFixed(anEntID, true)) {
51 myErrorMsg = SketchSolver_Error::ALREADY_FIXED();
56 if (!myFeatureMap.empty())
57 aKind = myFeatureMap.begin()->first->getKind();
59 aKind = myAttributeMap.begin()->first->attributeType();
61 if (aKind == SketchPlugin_Line::ID()) {
62 Slvs_Entity aLine = myStorage->getEntity(anEntID);
65 else if (aKind == SketchPlugin_Arc::ID()) {
66 Slvs_Entity anArc = myStorage->getEntity(anEntID);
69 else if (aKind == SketchPlugin_Circle::ID()) {
70 Slvs_Entity aCirc = myStorage->getEntity(anEntID);
73 else if (aKind == SketchPlugin_Point::ID() || aKind == GeomDataAPI_Point2D::typeId()) {
79 void SketchSolver_ConstraintRigid::getAttributes(
81 std::vector<Slvs_hEntity>& theAttributes)
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();
94 anEntityID = myGroup->getAttributeId(aRefAttr);
95 if (anEntityID == SLVS_E_UNKNOWN)
96 anEntityID = changeEntity(aRefAttr, aType);
98 anEntityID = myGroup->getFeatureId(myBaseFeature);
99 if (anEntityID == SLVS_E_UNKNOWN)
100 anEntityID = changeEntity(myBaseFeature, aType);
102 myFeatureMap[myBaseFeature] = anEntityID;
105 if (anEntityID == SLVS_E_UNKNOWN) {
106 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
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);
119 void SketchSolver_ConstraintRigid::adjustConstraint()
121 if (myFeatureMap.empty() || (
122 myFeatureMap.begin()->first->getKind() != SketchPlugin_Arc::ID() &&
123 myFeatureMap.begin()->first->getKind() != SketchPlugin_Circle::ID()))
125 FeaturePtr aFeature = myFeatureMap.begin()->first;
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)
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);
142 aRadius = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
143 aFeature->attribute(SketchPlugin_Circle::RADIUS_ID()))->value();
146 aConstraint.valA = aRadius * 2.0;
147 *aCIter = myStorage->updateConstraint(aConstraint);
152 bool SketchSolver_ConstraintRigid::remove(ConstraintPtr theConstraint)
155 if (theConstraint && theConstraint != myBaseConstraint)
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;
162 if (isFullyRemoved) {
163 myFeatureMap.clear();
164 myAttributeMap.clear();
167 cleanRemovedEntities();
171 void SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
173 if (thePointID == SLVS_E_UNKNOWN)
176 Slvs_Constraint aConstraint;
177 Slvs_hConstraint aConstrID = SLVS_E_UNKNOWN;
178 bool isFixed = myStorage->isPointFixed(thePointID, aConstrID, true);
179 bool isForceUpdate = (isFixed && !myBaseConstraint &&
180 myStorage->isTemporary(aConstrID));
181 if (!isForceUpdate) { // create new constraint
183 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
184 0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
185 aConstraint.h = myStorage->addConstraint(aConstraint);
186 mySlvsConstraints.push_back(aConstraint.h);
187 if (!myBaseConstraint)
188 myStorage->addConstraintWhereDragged(aConstraint.h);
189 } else { // update already existent constraint
190 if (!isFixed || aConstrID == SLVS_E_UNKNOWN || myBaseConstraint)
192 aConstraint = myStorage->getConstraint(aConstrID);
193 aConstraint.ptA = thePointID;
194 myStorage->addConstraint(aConstraint);
195 if (!myBaseConstraint)
196 myStorage->addConstraintWhereDragged(aConstraint.h);
200 void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
202 Slvs_Constraint anEqual;
203 if (isAxisParallel(theLine)) {
204 // Fix one point and a line length
205 Slvs_hConstraint aFixed;
206 if (!myStorage->isPointFixed(theLine.point[0], aFixed, true) &&
207 !myStorage->isPointFixed(theLine.point[1], aFixed, true))
208 fixPoint(theLine.point[0]);
209 if (!isUsedInEqual(theLine, anEqual)) {
210 // Check the distance is not set yet
211 std::list<Slvs_Constraint> aDistConstr = myStorage->getConstraintsByType(SLVS_C_PT_PT_DISTANCE);
212 std::list<Slvs_Constraint>::const_iterator aDIt = aDistConstr.begin();
213 for (; aDIt != aDistConstr.end(); aDIt++)
214 if ((aDIt->ptA == theLine.point[0] && aDIt->ptB == theLine.point[1]) ||
215 (aDIt->ptA == theLine.point[1] && aDIt->ptB == theLine.point[0]))
217 // Calculate distance between points on the line
219 for (int i = 0; i < 2; i++) {
220 Slvs_Entity aPnt = myStorage->getEntity(theLine.point[i]);
221 for (int j = 0; j < 2; j++) {
222 Slvs_Param aParam = myStorage->getParameter(aPnt.param[j]);
223 aCoords[2*i+j] = aParam.val;
226 double aLength = sqrt((aCoords[2] - aCoords[0]) * (aCoords[2] - aCoords[0]) +
227 (aCoords[3] - aCoords[1]) * (aCoords[3] - aCoords[1]));
229 Slvs_Constraint aDistance = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
230 SLVS_C_PT_PT_DISTANCE, myGroup->getWorkplaneId(), aLength,
231 theLine.point[0], theLine.point[1], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
232 aDistance.h = myStorage->addConstraint(aDistance);
233 mySlvsConstraints.push_back(aDistance.h);
237 else if (isUsedInEqual(theLine, anEqual)) {
238 // Check another entity of Equal is already fixed
239 Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
240 if (myStorage->isEntityFixed(anOtherEntID, true)) {
241 // Fix start point of the line (if end point is not fixed yet) ...
242 Slvs_hConstraint anEndFixedID = SLVS_E_UNKNOWN;
243 bool isFixed = myStorage->isPointFixed(theLine.point[1], anEndFixedID, true);
244 if (isFixed == SLVS_E_UNKNOWN)
245 fixPoint(theLine.point[0]);
246 // ... and create fixed point lying on this line
247 Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
248 // Firstly, search already fixed point on line
249 bool isPonLineFixed = false;
250 Slvs_hEntity aFixedPoint;
251 std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
252 std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
253 for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
254 if (aPLIter->entityA == theLine.h) {
255 isPonLineFixed = myStorage->isPointFixed(aPLIter->ptA, anEndFixedID);
256 aFixedPoint = aPLIter->ptA;
259 if (isPonLineFixed) { // update existent constraint
260 myStorage->copyEntity(aPointToCopy, aFixedPoint);
261 } else { // create new constraint
262 Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
263 Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
264 myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
265 aPonLine.h = myStorage->addConstraint(aPonLine);
266 mySlvsConstraints.push_back(aPonLine.h);
273 for (int i = 0; i < 2; i++)
274 fixPoint(theLine.point[i]);
277 void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
279 bool isFixRadius = true;
280 // Verify the arc is under Equal constraint
281 Slvs_Constraint anEqual;
282 if (isUsedInEqual(theCircle, anEqual)) {
283 // Check another entity of Equal is already fixed
284 Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
285 if (myStorage->isEntityFixed(anOtherEntID, true))
289 fixPoint(theCircle.point[0]);
292 // Search the radius is already fixed
293 std::list<Slvs_Constraint> aDiamConstr = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
294 std::list<Slvs_Constraint>::const_iterator aDiamIter = aDiamConstr.begin();
295 for (; aDiamIter != aDiamConstr.end(); aDiamIter++)
296 if (aDiamIter->entityA == theCircle.h)
299 // Fix radius of a circle
300 AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
301 myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
302 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
303 myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
304 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
305 aFixedR.h = myStorage->addConstraint(aFixedR);
306 mySlvsConstraints.push_back(aFixedR.h);
310 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
312 bool isFixRadius = true;
313 std::list<Slvs_hEntity> aPointsToFix;
314 aPointsToFix.push_back(theArc.point[1]);
315 aPointsToFix.push_back(theArc.point[2]);
317 // Verify the arc is under Equal constraint
318 Slvs_Constraint anEqual;
319 if (isUsedInEqual(theArc, anEqual)) {
320 // Check another entity of Equal is already fixed
321 Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
322 if (myStorage->isEntityFixed(anOtherEntID, true)) {
324 Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
325 if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
326 aPointsToFix.pop_back();
327 aPointsToFix.push_back(theArc.point[0]);
332 Slvs_hConstraint aConstrID;
333 int aNbPointsToFix = 2; // number of fixed points for the arc
334 if (myStorage->isPointFixed(theArc.point[0], aConstrID, true))
338 FeaturePtr aFeature = myFeatureMap.begin()->first;
339 std::shared_ptr<GeomAPI_Pnt2d> aCenter = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
340 aFeature->attribute(SketchPlugin_Arc::CENTER_ID()))->pnt();
341 std::shared_ptr<GeomAPI_Pnt2d> aStart = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
342 aFeature->attribute(SketchPlugin_Arc::START_ID()))->pnt();
343 double aRadius = aCenter->distance(aStart);
345 // Update end point of the arc to be on a curve
346 std::shared_ptr<GeomAPI_Pnt2d> anEnd = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
347 aFeature->attribute(SketchPlugin_Arc::END_ID()))->pnt();
348 double aDistance = anEnd->distance(aCenter);
349 std::shared_ptr<GeomAPI_XY> aDir = anEnd->xy()->decreased(aCenter->xy());
350 if (aDistance < tolerance)
351 aDir = aStart->xy()->decreased(aCenter->xy())->multiplied(-1.0);
353 aDir = aDir->multiplied(aRadius / aDistance);
354 double xy[2] = {aCenter->x() + aDir->x(), aCenter->y() + aDir->y()};
355 Slvs_Entity aEndPoint = myStorage->getEntity(theArc.point[2]);
356 for (int i = 0; i < 2; i++) {
357 Slvs_Param aParam = myStorage->getParameter(aEndPoint.param[i]);
359 myStorage->updateParameter(aParam);
362 std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
363 for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
367 // Fix radius of the arc
368 bool isExists = false;
369 std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
370 std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
371 for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
372 if (anIt->entityA == theArc.h)
375 Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
376 myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
377 myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
378 aFixedR.h = myStorage->addConstraint(aFixedR);
379 mySlvsConstraints.push_back(aFixedR.h);
380 if (!myBaseConstraint)
381 myStorage->addConstraintWhereDragged(aFixedR.h);
386 bool SketchSolver_ConstraintRigid::isUsedInEqual(
387 const Slvs_Entity& theEntity, Slvs_Constraint& theEqual) const
389 // Check the entity is used in Equal constraint
390 std::list<Slvs_Constraint> anEqualConstr = myStorage->getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
391 std::list<Slvs_Constraint> anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN);
392 anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
393 anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
394 anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
396 std::list<Slvs_Constraint>::const_iterator anEqIter = anEqualConstr.begin();
397 for (; anEqIter != anEqualConstr.end(); anEqIter++)
398 if (anEqIter->entityA == theEntity.h || anEqIter->entityB == theEntity.h) {
399 theEqual = *anEqIter;
405 bool SketchSolver_ConstraintRigid::isAxisParallel(const Slvs_Entity& theEntity) const
407 std::list<Slvs_Constraint> aConstr = myStorage->getConstraintsByType(SLVS_C_HORIZONTAL);
408 std::list<Slvs_Constraint> aVert = myStorage->getConstraintsByType(SLVS_C_VERTICAL);
409 aConstr.insert(aConstr.end(), aVert.begin(), aVert.end());
411 std::list<Slvs_Constraint>::const_iterator anIter = aConstr.begin();
412 for (; anIter != aConstr.end(); anIter++)
413 if (anIter->entityA == theEntity.h)