1 #include <SketchSolver_ConstraintMultiRotation.h>
2 #include <SketchSolver_Group.h>
3 #include <SketchSolver_Error.h>
5 #include <SketchPlugin_Arc.h>
6 #include <SketchPlugin_MultiRotation.h>
8 #include <ModelAPI_AttributeDouble.h>
9 #include <ModelAPI_AttributeInteger.h>
10 #include <ModelAPI_AttributeRefAttr.h>
11 #include <ModelAPI_AttributeRefList.h>
12 #include <ModelAPI_ResultConstruction.h>
14 #include <GeomAPI_Dir2d.h>
15 #include <GeomAPI_XY.h>
19 static double squareDistance(
20 StoragePtr theStorage, const Slvs_hEntity& thePoint1, const Slvs_hEntity& thePoint2)
22 Slvs_Entity aPoint1 = theStorage->getEntity(thePoint1);
23 Slvs_Entity aPoint2 = theStorage->getEntity(thePoint2);
24 double x1 = theStorage->getParameter(aPoint1.param[0]).val;
25 double y1 = theStorage->getParameter(aPoint1.param[1]).val;
26 double x2 = theStorage->getParameter(aPoint2.param[0]).val;
27 double y2 = theStorage->getParameter(aPoint2.param[1]).val;
28 return (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2);
31 void SketchSolver_ConstraintMultiRotation::getAttributes(
32 Slvs_hEntity& theCenter, double& theAngle,
33 std::vector<std::vector<Slvs_hEntity> >& thePoints,
34 std::vector<std::vector<Slvs_hEntity> >& theCircular)
36 DataPtr aData = myBaseConstraint->data();
37 theAngle = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
38 aData->attribute(SketchPlugin_MultiRotation::ANGLE_ID()))->value();
40 AttributePtr aCenterAttr = aData->attribute(SketchPlugin_MultiRotation::CENTER_ID());
41 if (!aCenterAttr || !aCenterAttr->isInitialized()) {
42 myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
45 int aType = SLVS_E_UNKNOWN; // type of created entity
46 Slvs_hEntity anEntityID = myGroup->getAttributeId(aCenterAttr);
47 if (anEntityID == SLVS_E_UNKNOWN)
48 anEntityID = changeEntity(aCenterAttr, aType);
49 theCenter = anEntityID;
51 // Lists of objects and number of copies
52 AttributeRefListPtr anInitialRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
53 aData->attribute(SketchPlugin_Constraint::ENTITY_A()));
54 myNumberOfObjects = anInitialRefList->size();
55 myNumberOfCopies = (size_t) aData->integer(SketchPlugin_MultiRotation::NUMBER_OF_COPIES_ID())->value();
56 AttributeRefListPtr aRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
57 myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_B()));
59 myErrorMsg = SketchSolver_Error::INCORRECT_ATTRIBUTE();
63 // Obtain all points of initial features and store them into separate lists
64 // containing their translated copies.
65 // Also all circles and arc collected too, because they will be constrained by equal radii.
67 ResultConstructionPtr aRC;
68 std::vector<Slvs_hEntity> aPoints[2]; // lists of points of features
69 std::vector<Slvs_hEntity> aCircs; // list of circular objects
70 std::list<ObjectPtr> anObjectList = aRefList->list();
71 std::list<ObjectPtr>::iterator anObjectIter = anObjectList.begin();
72 while (anObjectIter != anObjectList.end()) {
77 for (size_t i = 0; i <= myNumberOfCopies && anObjectIter != anObjectList.end(); i++, anObjectIter++) {
78 aFeature = ModelAPI_Feature::feature(*anObjectIter);
81 anEntityID = changeEntity(aFeature, aType);
82 Slvs_Entity anEntity = myStorage->getEntity(anEntityID);
84 case SLVS_E_POINT_IN_2D:
85 case SLVS_E_POINT_IN_3D:
86 aPoints[0].push_back(anEntityID);
88 case SLVS_E_LINE_SEGMENT:
89 aPoints[0].push_back(anEntity.point[0]); // start point of line
90 aPoints[1].push_back(anEntity.point[1]); // end point of line
93 aPoints[0].push_back(anEntity.point[0]); // center of circle
94 aCircs.push_back(anEntityID);
96 case SLVS_E_ARC_OF_CIRCLE:
97 aPoints[0].push_back(anEntity.point[1]); // start point of arc
98 aPoints[1].push_back(anEntity.point[2]); // end point of arc
99 aCircs.push_back(anEntityID);
102 myErrorMsg = SketchSolver_Error::INCORRECT_ATTRIBUTE();
107 if (!aPoints[0].empty())
108 thePoints.push_back(aPoints[0]);
109 if (!aPoints[1].empty())
110 thePoints.push_back(aPoints[1]);
112 theCircular.push_back(aCircs);
116 void SketchSolver_ConstraintMultiRotation::process()
119 if (!myBaseConstraint || !myStorage || myGroup == 0) {
120 /// TODO: Put error message here
123 if (!mySlvsConstraints.empty()) // some data is changed, update constraint
124 update(myBaseConstraint);
126 Slvs_hEntity aCenter;
127 std::vector<std::vector<Slvs_hEntity> > aPointsAndCopies;
128 std::vector<std::vector<Slvs_hEntity> > aCircsAndCopies;
129 getAttributes(aCenter, myAngle, aPointsAndCopies, aCircsAndCopies);
130 if (!myErrorMsg.empty())
135 // Create lines between neighbor rotated points and make angle between them equal to anAngle.
136 // Also these lines should have equal lengths.
137 Slvs_Constraint aConstraint;
138 myRotationCenter = aCenter;
139 Slvs_Entity aPrevLine;
140 std::vector<std::vector<Slvs_hEntity> >::iterator aCopyIter = aPointsAndCopies.begin();
141 for (; aCopyIter != aPointsAndCopies.end(); aCopyIter++) {
142 size_t aSize = aCopyIter->size();
143 if (aSize <= 1) continue;
145 aPrevLine = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, myGroup->getId(),
146 myGroup->getWorkplaneId(), aCenter, (*aCopyIter)[0]);
147 aPrevLine.h = myStorage->addEntity(aPrevLine);
148 std::vector<Slvs_hEntity> anEqualLines(1, aPrevLine.h);
149 for (size_t i = 1; i < aSize; i++) {
150 Slvs_Entity aLine = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, myGroup->getId(),
151 myGroup->getWorkplaneId(), aCenter, (*aCopyIter)[i]);
152 aLine.h = myStorage->addEntity(aLine);
153 // Equal length constraint
154 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
155 SLVS_C_EQUAL_LENGTH_LINES, myGroup->getWorkplaneId(), 0.0,
156 SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aPrevLine.h, aLine.h);
157 aConstraint.h = myStorage->addConstraint(aConstraint);
158 mySlvsConstraints.push_back(aConstraint.h);
160 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
161 SLVS_C_ANGLE, myGroup->getWorkplaneId(), fabs(myAngle), SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
162 aPrevLine.h, aLine.h);
163 if (myAngle < 0.0) // clockwise rotation
164 aConstraint.other = true;
165 aConstraint.h = myStorage->addConstraint(aConstraint);
166 mySlvsConstraints.push_back(aConstraint.h);
169 anEqualLines.push_back(aPrevLine.h);
172 myAuxLines.push_back(anEqualLines);
174 // Equal radii constraints
175 for (aCopyIter = aCircsAndCopies.begin(); aCopyIter != aCircsAndCopies.end(); aCopyIter++) {
176 size_t aSize = aCopyIter->size();
177 for (size_t i = 0; i < aSize - 1; i++) {
178 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
179 SLVS_C_EQUAL_RADIUS, myGroup->getWorkplaneId(), 0.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
180 (*aCopyIter)[i], (*aCopyIter)[i+1]);
181 aConstraint.h = myStorage->addConstraint(aConstraint);
182 mySlvsConstraints.push_back(aConstraint.h);
186 // Set the rotation center unchanged during constraint recalculation
187 if (!myStorage->isPointFixed(myRotationCenter, aConstraint.h, true)) {
188 aConstraint = Slvs_MakeConstraint(
189 SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_WHERE_DRAGGED, myGroup->getWorkplaneId(), 0.0,
190 myRotationCenter, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
191 aConstraint.h = myStorage->addConstraint(aConstraint);
192 mySlvsConstraints.push_back(aConstraint.h);
198 void SketchSolver_ConstraintMultiRotation::update(ConstraintPtr theConstraint)
201 if (!theConstraint || theConstraint == myBaseConstraint) {
202 AttributeRefListPtr anInitialRefList = std::dynamic_pointer_cast<ModelAPI_AttributeRefList>(
203 myBaseConstraint->attribute(SketchPlugin_Constraint::ENTITY_A()));
204 AttributeIntegerPtr aNbCopies = myBaseConstraint->integer(SketchPlugin_MultiRotation::NUMBER_OF_COPIES_ID());
205 if (anInitialRefList->size() != myNumberOfObjects || aNbCopies->value() != myNumberOfCopies) {
206 remove(myBaseConstraint);
212 // update angle value
213 myAngle = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
214 myBaseConstraint->attribute(SketchPlugin_MultiRotation::ANGLE_ID()))->value();
216 SketchSolver_Constraint::update();
219 bool SketchSolver_ConstraintMultiRotation::remove(ConstraintPtr theConstraint)
222 if (theConstraint && theConstraint != myBaseConstraint)
224 bool isFullyRemoved = true;
225 std::vector<Slvs_hEntity>::iterator aCIter = mySlvsConstraints.begin();
226 for (; aCIter != mySlvsConstraints.end(); aCIter++)
227 isFullyRemoved = myStorage->removeConstraint(*aCIter) && isFullyRemoved;
228 mySlvsConstraints.clear();
230 std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt = myFeatureMap.begin();
231 for (; aFeatIt != myFeatureMap.end(); aFeatIt++)
232 myStorage->removeEntity(aFeatIt->second);
234 if (isFullyRemoved) {
235 myFeatureMap.clear();
236 myAttributeMap.clear();
239 cleanRemovedEntities();
243 void SketchSolver_ConstraintMultiRotation::adjustConstraint()
245 if (abs(myAngle) < tolerance) {
246 myStorage->setNeedToResolve(false);
250 // Check the lengths of auxiliary lines are zero.
251 // If they become zero, remove corresponding Angle constraints.
252 // It they become non-zero (but were zero recently), add Angle constraint.
253 std::vector<Slvs_hConstraint>::iterator aConstr = mySlvsConstraints.begin();
254 std::map<Slvs_hEntity, Slvs_hEntity> anEqualLines;
255 bool isFirstRemoved = false;
256 for (; aConstr != mySlvsConstraints.end();
257 isFirstRemoved ? aConstr = mySlvsConstraints.begin() : ++aConstr) {
258 isFirstRemoved = false;
259 Slvs_Constraint aConstraint = myStorage->getConstraint(*aConstr);
260 if (aConstraint.type == SLVS_C_ANGLE || aConstraint.type == SLVS_C_EQUAL_LENGTH_LINES) {
261 Slvs_Entity aLine = myStorage->getEntity(aConstraint.entityA);
262 // Line length became zero => remove constraint
263 if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance) {
264 myStorage->removeConstraint(aConstraint.h);
265 isFirstRemoved = aConstr == mySlvsConstraints.begin();
266 std::vector<Slvs_hConstraint>::iterator aTmpIter = aConstr--;
267 mySlvsConstraints.erase(aTmpIter);
269 // Store the lines into the map
270 anEqualLines[aConstraint.entityB] = aConstraint.entityA;
273 // Create Angle and Equal constraints for non-degenerated lines
274 AuxLinesList::iterator anIt = myAuxLines.begin();
275 for (; anIt != myAuxLines.end(); ++anIt) {
276 if (anEqualLines.find(anIt->back()) != anEqualLines.end())
279 std::vector<Slvs_hEntity>::iterator anEqLinesIt = anIt->begin();
280 Slvs_hEntity aPrevLine = (*anEqLinesIt);
281 // Check the length of the line
282 Slvs_Entity aLine = myStorage->getEntity(aPrevLine);
283 if (squareDistance(myStorage, aLine.point[0], aLine.point[1]) < tolerance * tolerance)
286 for (++anEqLinesIt; anEqLinesIt != anIt->end(); ++anEqLinesIt) {
287 Slvs_hEntity aLine = (*anEqLinesIt);
288 // Equal length constraint
289 Slvs_Constraint aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
290 SLVS_C_EQUAL_LENGTH_LINES, myGroup->getWorkplaneId(), 0.0,
291 SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, aPrevLine, aLine);
292 aConstraint.h = myStorage->addConstraint(aConstraint);
293 mySlvsConstraints.push_back(aConstraint.h);
295 aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(),
296 SLVS_C_ANGLE, myGroup->getWorkplaneId(), fabs(myAngle), SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
298 if (myAngle < 0.0) // clockwise rotation
299 aConstraint.other = true;
300 aConstraint.h = myStorage->addConstraint(aConstraint);
301 mySlvsConstraints.push_back(aConstraint.h);
307 // Obtain coordinates of rotation center
308 Slvs_Entity aRotCenter = myStorage->getEntity(myRotationCenter);
310 for (int i = 0; i < 2; i++)
311 aCenterXY[i] = myStorage->getParameter(aRotCenter.param[i]).val;
313 double cosA = cos(myAngle * PI / 180.0);
314 double sinA = sin(myAngle * PI / 180.0);
316 // Update positions of all points to satisfy angles
317 std::list<Slvs_Constraint> aConstrAngle = myStorage->getConstraintsByType(SLVS_C_ANGLE);
318 std::list<Slvs_Constraint>::iterator anAngIt = aConstrAngle.begin();
319 std::vector<Slvs_hConstraint>::iterator aCIt;
320 Slvs_hConstraint aFixed; // temporary variable
321 double aVec[2]; // coordinates of vector defining a line
322 Slvs_Param aTarget[2]; // parameter to be changed
323 for (; anAngIt != aConstrAngle.end(); anAngIt++) {
324 for (aCIt = mySlvsConstraints.begin(); aCIt != mySlvsConstraints.end(); aCIt++)
325 if (anAngIt->h == *aCIt)
327 if (aCIt == mySlvsConstraints.end())
329 Slvs_Entity aLineA = myStorage->getEntity(anAngIt->entityA);
330 Slvs_Entity aLineB = myStorage->getEntity(anAngIt->entityB);
331 if (myStorage->isPointFixed(aLineB.point[1], aFixed))
333 Slvs_Entity aPointA = myStorage->getEntity(aLineA.point[1]);
334 Slvs_Entity aPointB = myStorage->getEntity(aLineB.point[1]);
335 for (int i = 0; i < 2; i++) {
336 aVec[i] = myStorage->getParameter(aPointA.param[i]).val - aCenterXY[i];
337 aTarget[i] = myStorage->getParameter(aPointB.param[i]);
339 aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
340 aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
341 myStorage->updateParameter(aTarget[0]);
342 myStorage->updateParameter(aTarget[1]);
344 anAngIt->valA = myAngle;
345 myStorage->updateConstraint(*anAngIt);
348 // update positions of centers of arcs for correct radius calculation
349 std::list<Slvs_Constraint> aRadii = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
350 std::map<FeaturePtr, Slvs_hEntity>::iterator aFeatIt;
351 for (anAngIt = aRadii.begin(); anAngIt != aRadii.end(); anAngIt++) {
352 int aNbFound = 0; // number of arcs used in translation
353 for (aFeatIt = myFeatureMap.begin(); aFeatIt != myFeatureMap.end(); aFeatIt++)
354 if (aFeatIt->second == anAngIt->entityA || aFeatIt->second == anAngIt->entityB) {
355 if (aFeatIt->first->getKind() == SketchPlugin_Arc::ID())
362 // two arcs were found, update their centers
363 Slvs_Entity anArcA = myStorage->getEntity(anAngIt->entityA);
364 Slvs_Entity anArcB = myStorage->getEntity(anAngIt->entityB);
365 if (myStorage->isPointFixed(anArcB.point[0], aFixed))
367 Slvs_Entity aCenterA = myStorage->getEntity(anArcA.point[0]);
368 Slvs_Entity aCenterB = myStorage->getEntity(anArcB.point[0]);
369 for (int i = 0; i < 2; i++) {
370 aVec[i] = myStorage->getParameter(aCenterA.param[i]).val - aCenterXY[i];
371 aTarget[i] = myStorage->getParameter(aCenterB.param[i]);
373 aTarget[0].val = aCenterXY[0] + aVec[0] * cosA - aVec[1] * sinA;
374 aTarget[1].val = aCenterXY[1] + aVec[0] * sinA + aVec[1] * cosA;
375 myStorage->updateParameter(aTarget[0]);
376 myStorage->updateParameter(aTarget[1]);