]> SALOME platform Git repositories - modules/shaper.git/blob - src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Storage.cpp
Salome HOME
PlaneGCS: fix incorrect range of fillet arc on the XZ plane
[modules/shaper.git] / src / SketchSolver / PlaneGCSSolver / PlaneGCSSolver_Storage.cpp
1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
2
3 // File:    PlaneGCSSolver_Storage.cpp
4 // Created: 14 Dec 2015
5 // Author:  Artem ZHIDKOV
6
7 #include <PlaneGCSSolver_Storage.h>
8 #include <PlaneGCSSolver_Builder.h>
9 #include <PlaneGCSSolver_Solver.h>
10 #include <PlaneGCSSolver_ConstraintWrapper.h>
11 #include <PlaneGCSSolver_EntityWrapper.h>
12 #include <PlaneGCSSolver_PointWrapper.h>
13 #include <PlaneGCSSolver_ScalarWrapper.h>
14 #include <PlaneGCSSolver_ParameterWrapper.h>
15
16 #include <GeomAPI_Edge.h>
17 #include <GeomAPI_Dir2d.h>
18 #include <GeomAPI_Pnt2d.h>
19 #include <GeomAPI_XY.h>
20 #include <GeomDataAPI_Point2D.h>
21 #include <SketchPlugin_Arc.h>
22 #include <SketchPlugin_ConstraintTangent.h>
23
24 #include <cmath>
25
26
27 PlaneGCSSolver_Storage::PlaneGCSSolver_Storage(const GroupID& theGroup)
28   : SketchSolver_Storage(theGroup),
29     myEntityLastID(EID_SKETCH),
30     myConstraintLastID(CID_UNKNOWN)
31 {
32 }
33
34 void PlaneGCSSolver_Storage::addConstraint(
35     ConstraintPtr                   theConstraint,
36     std::list<ConstraintWrapperPtr> theSolverConstraints)
37 {
38   SketchSolver_Storage::addConstraint(theConstraint, theSolverConstraints);
39
40   // update point-point coincidence
41   if (!theSolverConstraints.empty() &&
42       theSolverConstraints.front()->type() == CONSTRAINT_PT_PT_COINCIDENT) {
43     std::list<ConstraintWrapperPtr>::iterator aCIt = theSolverConstraints.begin();
44     for (; aCIt != theSolverConstraints.end(); ++aCIt)
45       update(*aCIt);
46   }
47 }
48
49
50 bool PlaneGCSSolver_Storage::update(ConstraintWrapperPtr theConstraint)
51 {
52   bool isUpdated = false;
53   std::shared_ptr<PlaneGCSSolver_ConstraintWrapper> aConstraint =
54       std::dynamic_pointer_cast<PlaneGCSSolver_ConstraintWrapper>(theConstraint);
55
56   // point-Line distance should be positive
57   if (aConstraint->type() == CONSTRAINT_PT_LINE_DISTANCE && aConstraint->value() < 0.0)
58     aConstraint->setValue(-aConstraint->value());
59
60   // make value of constraint unchangeable
61   ParameterWrapperPtr aValue = aConstraint->valueParameter();
62   if (aValue)
63     isUpdated = update(aValue) || isUpdated;
64
65   // update constrained entities
66   std::list<EntityWrapperPtr> anEntities = theConstraint->entities();
67   std::list<EntityWrapperPtr>::iterator anIt = anEntities.begin();
68   for (; anIt != anEntities.end(); ++anIt)
69     isUpdated = update(*anIt) || isUpdated;
70
71   if (aConstraint->id() == CID_UNKNOWN) {
72     const std::list<EntityWrapperPtr>& aSubs = aConstraint->entities();
73     // check middle-point constraint conflicts with point-on-line
74     if (aConstraint->type() == CONSTRAINT_MIDDLE_POINT) {
75       std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
76           anIt = myConstraintMap.begin();
77       for (; anIt != myConstraintMap.end(); ++anIt) {
78         EntityWrapperPtr aPoint, aLine;
79         ConstraintWrapperPtr aCurrentConstr = anIt->second.front();
80         if (aCurrentConstr->type() != CONSTRAINT_PT_ON_LINE)
81           continue;
82         const std::list<EntityWrapperPtr>& aCurSubs = aCurrentConstr->entities();
83         std::list<EntityWrapperPtr>::const_iterator aSIt1, aSIt2;
84         for (aSIt1 = aSubs.begin(); aSIt1 != aSubs.end(); ++aSIt1) {
85           if ((*aSIt1)->type() == ENTITY_POINT)
86             aPoint = *aSIt1;
87           else if((*aSIt1)->type() == ENTITY_LINE)
88             aLine = *aSIt1;
89           else
90             continue;
91           for (aSIt2 = aCurSubs.begin(); aSIt2 != aCurSubs.end(); ++aSIt2)
92             if ((*aSIt1)->id() == (*aSIt2)->id())
93               break;
94           if (aSIt2 == aCurSubs.end())
95             break;
96         }
97         // point-on-line found, change it to bisector
98         if (aSIt1 == aSubs.end()) {
99           std::list<GCSConstraintPtr> aConstrList = aConstraint->constraints();
100           aConstrList.pop_front();
101           aConstraint->setConstraints(aConstrList);
102           break;
103         }
104       }
105     }
106
107     // Change ID of constraints
108     aConstraint->setId(++myConstraintLastID);
109   }
110
111   return isUpdated;
112 }
113
114 /// \brief Update coordinates of the point or scalar using its base attribute
115 static bool updateValues(EntityWrapperPtr& theEntity)
116 {
117   bool isUpdated = false;
118   AttributePtr anAttr = theEntity->baseAttribute();
119   const std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
120
121   double aCoord[2];
122
123   std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
124       std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttr);
125   if (aPoint2D) {
126     aCoord[0] = aPoint2D->x();
127     aCoord[1] = aPoint2D->y();
128   } else {
129     AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttr);
130     if (aScalar)
131       aCoord[0] = aScalar->value();
132   }
133
134   std::list<ParameterWrapperPtr>::const_iterator anIt = aParams.begin();
135   for (int i = 0; anIt != aParams.end(); ++anIt, ++i)
136     if (fabs((*anIt)->value() - aCoord[i]) > tolerance) {
137       (*anIt)->setValue(aCoord[i]);
138       isUpdated = true;
139     }
140   return isUpdated;
141 }
142
143 bool PlaneGCSSolver_Storage::update(EntityWrapperPtr theEntity)
144 {
145   if (theEntity->type() == ENTITY_SKETCH)
146     return true; // sketch is not necessary for PlaneGCS, so it is always says true
147
148   bool isUpdated = false;
149
150   if (theEntity->baseAttribute()) {
151     isUpdated = updateValues(theEntity);
152     if (isUpdated) {
153       setNeedToResolve(true);
154       if (theEntity->type() == ENTITY_POINT && theEntity->group() != myGroupID)
155         updateCoincident(theEntity);
156     }
157   }
158
159   // update parameters
160   std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
161   std::list<ParameterWrapperPtr>::iterator aPIt = aParams.begin();
162   for (; aPIt != aParams.end(); ++aPIt)
163     isUpdated = update(*aPIt) || isUpdated;
164
165   // update sub-entities
166   std::list<EntityWrapperPtr> aSubEntities = theEntity->subEntities();
167   std::list<EntityWrapperPtr>::iterator aSIt = aSubEntities.begin();
168   for (; aSIt != aSubEntities.end(); ++aSIt)
169     isUpdated = update(*aSIt) || isUpdated;
170
171   // additional constraints for the arc processing
172   if (theEntity->type() == ENTITY_ARC)
173     processArc(theEntity);
174
175   // Change entity's ID, if necessary
176   if (theEntity->id() == EID_UNKNOWN) {
177     if (theEntity->type() == ENTITY_POINT) {
178       std::shared_ptr<PlaneGCSSolver_PointWrapper> aPoint =
179           std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(theEntity);
180       if (!aPoint) {
181         aPoint = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(
182             theEntity->subEntities().front());
183       }
184       aPoint->setId(++myEntityLastID);
185     } else if (theEntity->type() == ENTITY_SCALAR) {
186       std::shared_ptr<PlaneGCSSolver_ScalarWrapper> aScalar =
187           std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(theEntity);
188       aScalar->setId(++myEntityLastID);
189     } else {
190       std::shared_ptr<PlaneGCSSolver_EntityWrapper> aGCSEnt =
191           std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theEntity);
192       aGCSEnt->setId(++myEntityLastID);
193     }
194   }
195   return isUpdated;
196 }
197
198 bool PlaneGCSSolver_Storage::update(ParameterWrapperPtr theParameter)
199 {
200   std::shared_ptr<PlaneGCSSolver_ParameterWrapper> aParam =
201       std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(theParameter);
202   if (aParam->isProcessed())
203     return false;
204   if (theParameter->group() != myGroupID || theParameter->isParametric())
205     myConst.push_back(aParam->parameter());
206   else
207     myParameters.push_back(aParam->parameter());
208   aParam->setProcessed(true);
209   return true;
210 }
211
212
213 bool PlaneGCSSolver_Storage::remove(ConstraintWrapperPtr theConstraint)
214 {
215   std::shared_ptr<PlaneGCSSolver_ConstraintWrapper> aConstraint =
216     std::dynamic_pointer_cast<PlaneGCSSolver_ConstraintWrapper>(theConstraint);
217
218   bool isFullyRemoved = true;
219   // remove point-point coincidence
220   if (aConstraint->type() == CONSTRAINT_PT_PT_COINCIDENT)
221     isFullyRemoved = removeCoincidence(theConstraint) && isFullyRemoved;
222   // remove sub-entities
223   const std::list<EntityWrapperPtr>& aSubs = aConstraint->entities();
224   std::list<EntityWrapperPtr>::const_iterator aSIt = aSubs.begin();
225   for (; aSIt != aSubs.end(); ++ aSIt)
226     isFullyRemoved = remove(*aSIt) && isFullyRemoved;
227
228   if (aConstraint->valueParameter())
229     isFullyRemoved = remove(aConstraint->valueParameter()) && isFullyRemoved;
230   if (!isFullyRemoved && aConstraint->baseConstraint() &&
231      (!aConstraint->baseConstraint()->data() || !aConstraint->baseConstraint()->data()->isValid()))
232     isFullyRemoved = true;
233   setNeedToResolve(true);
234   myRemovedConstraints.insert(myRemovedConstraints.end(),
235       aConstraint->constraints().begin(), aConstraint->constraints().end());
236
237   if (isFullyRemoved && theConstraint->id() == myConstraintLastID)
238     --myConstraintLastID;
239
240   return isFullyRemoved;
241 }
242
243 bool PlaneGCSSolver_Storage::remove(EntityWrapperPtr theEntity)
244 {
245   // do not remove entity, if it is used by constraints or other entities
246   if ((theEntity->baseFeature() && isUsed(theEntity->baseFeature())) ||
247       (theEntity->baseAttribute() && isUsed(theEntity->baseAttribute())))
248     return false;
249
250   bool isFullyRemoved = SketchSolver_Storage::remove(theEntity);
251   if (isFullyRemoved) {
252     if (theEntity->type() == ENTITY_ARC) {
253       // remove arc additional constraints
254       std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::iterator
255           aFound = myArcConstraintMap.find(theEntity);
256       if (aFound != myArcConstraintMap.end()) {
257         myRemovedConstraints.insert(myRemovedConstraints.end(),
258             aFound->second.begin(), aFound->second.end());
259         myArcConstraintMap.erase(aFound);
260       }
261     }
262     if (theEntity->id() == myEntityLastID)
263       --myEntityLastID;
264   }
265   return isFullyRemoved;
266 }
267
268 bool PlaneGCSSolver_Storage::remove(ParameterWrapperPtr theParameter)
269 {
270   std::shared_ptr<PlaneGCSSolver_ParameterWrapper> aParam =
271       std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(theParameter);
272   if (aParam->isProcessed()) {
273     double* aValPtr = aParam->parameter();
274     GCS::VEC_pD::iterator anIt =  myParameters.begin();
275     for (; anIt != myParameters.end(); ++anIt)
276       if (*anIt == aValPtr)
277         break;
278     if (anIt != myParameters.end()) {
279       myParameters.erase(anIt);
280       setNeedToResolve(true);
281       aParam->setProcessed(false);
282     }
283     else {
284       for (anIt = myConst.begin(); anIt != myConst.end(); ++anIt)
285         if (*anIt == aValPtr)
286           break;
287       if (anIt != myConst.end()) {
288         myConst.erase(anIt);
289         setNeedToResolve(true);
290         aParam->setProcessed(false);
291       }
292     }
293   }
294   return true;
295 }
296
297
298 void PlaneGCSSolver_Storage::addCoincidentPoints(
299     EntityWrapperPtr theMaster, EntityWrapperPtr theSlave)
300 {
301   if (theMaster->type() != ENTITY_POINT || theSlave->type() != ENTITY_POINT)
302     return;
303
304   std::shared_ptr<PlaneGCSSolver_PointWrapper> aMaster =
305       std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(theMaster);
306   if (!aMaster)
307     aMaster = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(
308       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theMaster)->subEntities().front());
309   std::shared_ptr<PlaneGCSSolver_PointWrapper> aSlave =
310       std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(theSlave);
311   if (!aSlave)
312     aSlave = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(
313       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theSlave)->subEntities().front());
314
315   // Search available coincidence
316   CoincidentPointsMap::iterator aMasterFound = myCoincidentPoints.find(aMaster);
317   CoincidentPointsMap::iterator aSlaveFound = myCoincidentPoints.find(aSlave);
318   if (aMasterFound == myCoincidentPoints.end() &&  aSlaveFound == myCoincidentPoints.end()) {
319     // try to find master and slave points in the lists of slaves of already existent coincidences
320     CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
321     for (; anIt != myCoincidentPoints.end(); ++anIt) {
322       if (anIt->second.find(aMaster) != anIt->second.end())
323         aMasterFound = anIt;
324       else if (anIt->second.find(aSlave) != anIt->second.end())
325         aSlaveFound = anIt;
326
327       if (aMasterFound != myCoincidentPoints.end() &&  aSlaveFound != myCoincidentPoints.end())
328         break;
329     }
330   }
331
332   if (aMasterFound == myCoincidentPoints.end()) {
333     // create new group
334     myCoincidentPoints[aMaster] = std::set<EntityWrapperPtr>();
335     aMasterFound = myCoincidentPoints.find(aMaster);
336   } else if (aMasterFound == aSlaveFound)
337     return; // already coincident
338
339   if (aSlaveFound != myCoincidentPoints.end()) {
340     // A slave has been found, we need to attach all points coincident with it to the new master
341     std::set<EntityWrapperPtr> aNewSlaves = aSlaveFound->second;
342     aNewSlaves.insert(aSlaveFound->first);
343     myCoincidentPoints.erase(aSlaveFound);
344
345     std::set<EntityWrapperPtr>::const_iterator aSlIt = aNewSlaves.begin();
346     for (; aSlIt != aNewSlaves.end(); ++aSlIt)
347       addCoincidentPoints(aMaster, *aSlIt);
348   } else {
349     //std::list<ParameterWrapperPtr> aSlaveParams = aSlave->parameters();
350     //aSlave->setParameters(aMaster->parameters());
351
352     //// Remove slave's parameters
353     //std::list<ParameterWrapperPtr>::iterator aParIt = aSlaveParams.begin();
354     //for (; aParIt != aSlaveParams.end(); ++aParIt)
355     //  remove(*aParIt);
356
357     aMasterFound->second.insert(aSlave);
358   }
359 }
360
361
362 void PlaneGCSSolver_Storage::changeGroup(EntityWrapperPtr theEntity, const GroupID& theGroup)
363 {
364   theEntity->setGroup(theGroup);
365   if (theGroup == myGroupID)
366     makeVariable(theEntity);
367   else {
368     if (theEntity->type() == ENTITY_POINT)
369       update(theEntity);
370     makeConstant(theEntity);
371   }
372 }
373
374 void PlaneGCSSolver_Storage::changeGroup(ParameterWrapperPtr theParam, const GroupID& theGroup)
375 {
376   // TODO
377 }
378
379 void PlaneGCSSolver_Storage::verifyFixed()
380 {
381   // TODO
382 }
383
384 void PlaneGCSSolver_Storage::processArc(const EntityWrapperPtr& theArc)
385 {
386   // no need to constraint a fixed arc
387   if (theArc->group() == GID_OUTOFGROUP)
388     return;
389
390   // Calculate additional parameters necessary for PlaneGCS
391   const std::list<EntityWrapperPtr>& aSubs = theArc->subEntities();
392   std::list<EntityWrapperPtr>::const_iterator aSubIt = aSubs.begin();
393   while ((*aSubIt)->type() == ENTITY_POINT) // search scalar entities
394     ++aSubIt;
395   double* aStartAngle = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt++)->scalar();
396   double* aEndAngle   = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt++)->scalar();
397   double* aRadius     = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt)->scalar();
398
399   std::shared_ptr<SketchPlugin_Feature> anArcFeature =
400       std::dynamic_pointer_cast<SketchPlugin_Feature>(theArc->baseFeature());
401   std::shared_ptr<GeomDataAPI_Point2D> aCenterAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
402       anArcFeature->attribute(SketchPlugin_Arc::CENTER_ID()));
403   std::shared_ptr<GeomDataAPI_Point2D> aStartAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
404       anArcFeature->attribute(SketchPlugin_Arc::START_ID()));
405   std::shared_ptr<GeomDataAPI_Point2D> aEndAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
406       anArcFeature->attribute(SketchPlugin_Arc::END_ID()));
407   if (!aCenterAttr || !aStartAttr || !aEndAttr)
408     return;
409   std::shared_ptr<GeomAPI_Pnt2d> aCenterPnt = aCenterAttr->pnt();
410   std::shared_ptr<GeomAPI_Pnt2d> aStartPnt  = aStartAttr->pnt();
411   std::shared_ptr<GeomAPI_Pnt2d> aEndPnt    = aEndAttr->pnt();
412
413   *aRadius = aCenterPnt->distance(aStartPnt);
414   if (!anArcFeature->lastResult())
415     return;
416   std::shared_ptr<GeomAPI_Edge> anArcEdge =
417       std::dynamic_pointer_cast<GeomAPI_Edge>(anArcFeature->lastResult()->shape());
418   if (!anArcEdge)
419     return;
420   anArcEdge->getRange(*aStartAngle, *aEndAngle);
421   // verify the range is correct and not shifted to an angle
422   std::shared_ptr<GeomAPI_Dir2d> aDir(new GeomAPI_Dir2d(cos(*aStartAngle), sin(*aStartAngle)));
423   std::shared_ptr<GeomAPI_Pnt2d> aCalcStartPnt(
424       new GeomAPI_Pnt2d(aCenterPnt->xy()->added(aDir->xy()->multiplied(*aRadius))));
425   if (aCalcStartPnt->distance(aStartPnt) > tolerance) {
426     std::shared_ptr<GeomAPI_Dir2d> aDirToStart(
427         new GeomAPI_Dir2d(aStartPnt->xy()->decreased(aCenterPnt->xy())));
428     double anAngle = aDir->angle(aDirToStart);
429     *aStartAngle += anAngle;
430     *aEndAngle += anAngle;
431   }
432
433   // do not constraint copied arc
434   if (anArcFeature->isCopy())
435     return;
436   // No need to add constraints if they are already exist
437   std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::const_iterator
438       aFound = myArcConstraintMap.find(theArc);
439   if (aFound != myArcConstraintMap.end())
440     return;
441
442   // Prepare additional constraints to produce the arc
443   std::vector<GCSConstraintPtr> anArcConstraints;
444   std::shared_ptr<PlaneGCSSolver_EntityWrapper> anArcEnt =
445       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theArc);
446   std::shared_ptr<GCS::Arc> anArc = std::dynamic_pointer_cast<GCS::Arc>(anArcEnt->entity());
447   // Distances from center till start and end points are equal to radius
448   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
449       anArc->center, anArc->start, anArc->rad)));
450   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
451       anArc->center, anArc->end, anArc->rad)));
452   // Angles of start and end points should be equal to given angles
453   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PAngle(
454       anArc->center, anArc->start, anArc->startAngle)));
455   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PAngle(
456       anArc->center, anArc->end, anArc->endAngle)));
457
458   myArcConstraintMap[theArc] = anArcConstraints;
459 }
460
461
462 void PlaneGCSSolver_Storage::makeConstant(const EntityWrapperPtr& theEntity)
463 {
464   toggleEntity(theEntity, myParameters, myConst);
465   if (theEntity->type() == ENTITY_POINT)
466     updateCoincident(theEntity);
467 }
468
469 void PlaneGCSSolver_Storage::makeVariable(const EntityWrapperPtr& theEntity)
470 {
471   toggleEntity(theEntity, myConst, myParameters);
472 }
473
474 static void getParametersToMove(const EntityWrapperPtr& theEntity, std::set<double*>& theParamList)
475 {
476   const std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
477   std::list<ParameterWrapperPtr>::const_iterator aPIt = aParams.begin();
478   for (; aPIt != aParams.end(); ++aPIt)
479     theParamList.insert(
480         std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(*aPIt)->parameter());
481
482   const std::list<EntityWrapperPtr> aSubs = theEntity->subEntities();
483   std::list<EntityWrapperPtr>::const_iterator aSIt = aSubs.begin();
484
485   if (theEntity->type() == ENTITY_ARC) {
486     // workaround for the arc processing, because the arc is fixed by a set of constraints,
487     // which will conflict with all parameters fixed:
488     // 1. take center
489     getParametersToMove(*aSIt++, theParamList);
490     // 2. take start point
491     getParametersToMove(*aSIt++, theParamList);
492     // 3. skip end point, radius and start angle, but take end angle parameter
493     getParametersToMove(*(++aSIt), theParamList);
494   } else {
495     for (; aSIt != aSubs.end(); ++aSIt)
496       getParametersToMove(*aSIt, theParamList);
497   }
498 }
499
500 void PlaneGCSSolver_Storage::toggleEntity(
501     const EntityWrapperPtr& theEntity, GCS::VEC_pD& theFrom, GCS::VEC_pD& theTo)
502 {
503   std::set<double*> aParamsToMove;
504   getParametersToMove(theEntity, aParamsToMove);
505
506   GCS::VEC_pD::iterator anIt = theFrom.begin();
507   while (anIt != theFrom.end()) {
508     if (aParamsToMove.find(*anIt) == aParamsToMove.end()) {
509       ++anIt;
510       continue;
511     }
512
513     theTo.push_back(*anIt);
514     int aShift = int(anIt - theFrom.begin());
515     theFrom.erase(anIt);
516     anIt = theFrom.begin() + aShift;
517   }
518 }
519
520 void PlaneGCSSolver_Storage::updateCoincident(const EntityWrapperPtr& thePoint)
521 {
522   CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
523   for (; anIt != myCoincidentPoints.end(); ++anIt) {
524     if (anIt->first == thePoint || anIt->second.find(thePoint) != anIt->second.end()) {
525       std::set<EntityWrapperPtr> aCoincident = anIt->second;
526       aCoincident.insert(anIt->first);
527
528       const std::list<ParameterWrapperPtr>& aBaseParams = thePoint->parameters();
529       std::list<ParameterWrapperPtr> aParams;
530       std::list<ParameterWrapperPtr>::const_iterator aBaseIt, anUpdIt;
531
532       std::set<EntityWrapperPtr>::const_iterator aCoincIt = aCoincident.begin();
533       for (; aCoincIt != aCoincident.end(); ++aCoincIt)
534         if (*aCoincIt != thePoint && (*aCoincIt)->group() != GID_OUTOFGROUP) {
535           aParams = (*aCoincIt)->parameters();
536           aBaseIt = aBaseParams.begin();
537           for (anUpdIt = aParams.begin(); anUpdIt != aParams.end(); ++anUpdIt, ++aBaseIt)
538             (*anUpdIt)->setValue((*aBaseIt)->value());
539         }
540
541       break;
542     }
543   }
544 }
545
546
547 bool PlaneGCSSolver_Storage::isRedundant(
548     GCSConstraintPtr theCheckedConstraint,
549     ConstraintWrapperPtr theParentConstraint,
550     std::list<std::set<double*> >& theCoincidentPoints) const
551 {
552   if (theParentConstraint->type() == CONSTRAINT_SYMMETRIC) {
553     if (theCheckedConstraint->getTypeId() == GCS::Perpendicular) {
554       BuilderPtr aBuilder = PlaneGCSSolver_Builder::getInstance();
555       // check the initial point is placed on the mirror line
556       std::list<EntityWrapperPtr> aSubs = theParentConstraint->entities();
557       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(aSubs.front());
558       std::shared_ptr<GeomAPI_Lin2d> aLine = aBuilder->line(aSubs.back());
559       return aLine->distance(aPoint) < tolerance;
560     }
561   }
562   else if (theParentConstraint->type() == CONSTRAINT_PT_PT_COINCIDENT) {
563     // Verify that the coincidence between points is already added
564     GCS::VEC_pD aParams = theCheckedConstraint->params();
565
566     std::list<std::set<double*> >::iterator aCoincIt, aFound1, aFound2;
567     aFound1 = aFound2 = theCoincidentPoints.end();
568     for (aCoincIt = theCoincidentPoints.begin(); aCoincIt != theCoincidentPoints.end(); ++aCoincIt) {
569       if (aFound1 == theCoincidentPoints.end() && aCoincIt->find(aParams[0]) != aCoincIt->end())
570         aFound1 = aCoincIt;
571       if (aFound2 == theCoincidentPoints.end() && aCoincIt->find(aParams[1]) != aCoincIt->end())
572         aFound2 = aCoincIt;
573       if (aFound1 != theCoincidentPoints.end() && aFound2 != theCoincidentPoints.end())
574         break;
575     }
576     if (aCoincIt != theCoincidentPoints.end()) { // both point are found
577       if (aFound1 == aFound2)
578         return true;
579       // merge two groups of coincidence
580       aFound1->insert(aFound2->begin(), aFound2->end());
581       theCoincidentPoints.erase(aFound2);
582     } else {
583       if (aFound1 != theCoincidentPoints.end())
584         aFound1->insert(aParams[1]);
585       else if (aFound2 != theCoincidentPoints.end())
586         aFound2->insert(aParams[0]);
587       else {
588         std::set<double*> aNewCoincidence;
589         aNewCoincidence.insert(aParams[0]);
590         aNewCoincidence.insert(aParams[1]);
591         theCoincidentPoints.push_back(aNewCoincidence);
592       }
593     }
594   }
595
596   return false;
597 }
598
599 void PlaneGCSSolver_Storage::initializeSolver(SolverPtr theSolver)
600 {
601   std::shared_ptr<PlaneGCSSolver_Solver> aSolver =
602       std::dynamic_pointer_cast<PlaneGCSSolver_Solver>(theSolver);
603   if (!aSolver)
604     return;
605   aSolver->clear();
606
607   if (myExistArc)
608     processArcs();
609
610   // initialize constraints
611   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
612       aCIt = myConstraintMap.begin();
613   GCS::SET_I aTangentIDs;
614   std::list<std::set<double*> > aCoincidentPoints;
615   for (; aCIt != myConstraintMap.end(); ++aCIt) {
616     std::list<ConstraintWrapperPtr>::const_iterator aCWIt = aCIt->second.begin();
617     for (; aCWIt != aCIt->second.end(); ++ aCWIt) {
618       std::shared_ptr<PlaneGCSSolver_ConstraintWrapper> aGCS =
619           std::dynamic_pointer_cast<PlaneGCSSolver_ConstraintWrapper>(*aCWIt);
620       std::list<GCSConstraintPtr>::const_iterator anIt = aGCS->constraints().begin();
621       for (; anIt != aGCS->constraints().end(); ++anIt)
622         if (!isRedundant(*anIt, aGCS, aCoincidentPoints))
623           aSolver->addConstraint(*anIt);
624     }
625     // store IDs of tangent constraints to avoid incorrect report of redundant constraints
626     if (aCIt->first && aCIt->first->getKind() == SketchPlugin_ConstraintTangent::ID())
627       for (aCWIt = aCIt->second.begin(); aCWIt != aCIt->second.end(); ++ aCWIt)
628         aTangentIDs.insert((int)(*aCWIt)->id());
629   }
630   // additional constraints for arcs
631   std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::const_iterator
632       anArcIt = myArcConstraintMap.begin();
633   for (; anArcIt != myArcConstraintMap.end(); ++anArcIt) {
634     std::vector<GCSConstraintPtr>::const_iterator anIt = anArcIt->second.begin();
635     for (; anIt != anArcIt->second.end(); ++anIt)
636       aSolver->addConstraint(*anIt);
637   }
638   // removed waste constraints
639   std::list<GCSConstraintPtr>::const_iterator aRemIt = myRemovedConstraints.begin();
640   for (; aRemIt != myRemovedConstraints.end(); ++aRemIt)
641     aSolver->removeConstraint(*aRemIt);
642   myRemovedConstraints.clear();
643   // set list of tangent constraints
644   aSolver->setTangent(aTangentIDs);
645   // initialize unknowns
646   aSolver->setParameters(myParameters);
647 }
648
649 void PlaneGCSSolver_Storage::refresh(bool theFixedOnly) const
650 {
651   //blockEvents(true);
652
653   std::map<AttributePtr, EntityWrapperPtr>::const_iterator anIt = myAttributeMap.begin();
654   std::list<ParameterWrapperPtr> aParams;
655   std::list<ParameterWrapperPtr>::const_iterator aParIt;
656   for (; anIt != myAttributeMap.end(); ++anIt) {
657     // the external feature always should keep the up to date values, so, 
658     // refresh from the solver is never needed
659     bool isExternal = false;
660     if (anIt->first.get()) {
661       std::shared_ptr<SketchPlugin_Feature> aSketchFeature = 
662         std::dynamic_pointer_cast<SketchPlugin_Feature>(anIt->first->owner());
663       if (aSketchFeature.get() && aSketchFeature->isExternal())
664         isExternal = true;
665     }
666
667     // update parameter wrappers and obtain values of attributes
668     aParams = anIt->second->parameters();
669     double aCoords[3];
670     bool isUpd[3] = {false};
671     int i = 0;
672     for (aParIt = aParams.begin(); i < 3 && aParIt != aParams.end(); ++aParIt, ++i) {
673       if (!theFixedOnly || isExternal ||
674           (*aParIt)->group() == GID_OUTOFGROUP || (*aParIt)->isParametric()) {
675         aCoords[i] = (*aParIt)->value();
676         isUpd[i] = true;
677       }
678     }
679     if (!isUpd[0] && !isUpd[1] && !isUpd[2])
680       continue; // nothing is updated
681
682     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
683         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anIt->first);
684     if (aPoint2D) {
685       if ((isUpd[0] && fabs(aPoint2D->x() - aCoords[0]) > tolerance) ||
686           (isUpd[1] && fabs(aPoint2D->y() - aCoords[1]) > tolerance) || isExternal) {
687         if (!isUpd[0] || isExternal) aCoords[0] = aPoint2D->x();
688         if (!isUpd[1] || isExternal) aCoords[1] = aPoint2D->y();
689         aPoint2D->setValue(aCoords[0], aCoords[1]);
690         // Find points coincident with this one (probably not in GID_OUTOFGROUP)
691         CoincidentPointsMap::const_iterator aCoincIt = myCoincidentPoints.begin();
692         for (; aCoincIt != myCoincidentPoints.end(); ++aCoincIt)
693           if (aCoincIt->first == anIt->second ||
694               aCoincIt->second.find(anIt->second) != aCoincIt->second.end())
695             break;
696         if (aCoincIt != myCoincidentPoints.end()) {
697           aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
698               aCoincIt->first->baseAttribute());
699           if (aPoint2D)
700             aPoint2D->setValue(aCoords[0], aCoords[1]);
701           std::set<EntityWrapperPtr>::const_iterator aSlaveIt = aCoincIt->second.begin();
702           for (; aSlaveIt != aCoincIt->second.end(); ++aSlaveIt) {
703             aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>((*aSlaveIt)->baseAttribute());
704             if (aPoint2D)
705               aPoint2D->setValue(aCoords[0], aCoords[1]);
706           }
707         }
708       }
709       continue;
710     }
711     AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anIt->first);
712     if (aScalar && !isExternal) {
713       if (isUpd[0] && fabs(aScalar->value() - aCoords[0]) > tolerance)
714         aScalar->setValue(aCoords[0]);
715       continue;
716     }
717   }
718
719   //blockEvents(false);
720 }
721
722 EntityWrapperPtr PlaneGCSSolver_Storage::calculateMiddlePoint(
723     EntityWrapperPtr theBase, double theCoeff)
724 {
725   std::shared_ptr<PlaneGCSSolver_Builder> aBuilder =
726       std::dynamic_pointer_cast<PlaneGCSSolver_Builder>(PlaneGCSSolver_Builder::getInstance());
727
728   std::shared_ptr<GeomAPI_XY> aMidPoint;
729   if (theBase->type() == ENTITY_LINE) {
730     std::shared_ptr<GeomAPI_Pnt2d> aPoints[2];
731     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
732     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
733     for (int i = 0; i < 2; ++i, ++anIt)
734       aPoints[i] = aBuilder->point(*anIt);
735     aMidPoint = aPoints[0]->xy()->multiplied(1.0 - theCoeff)->added(
736         aPoints[1]->xy()->multiplied(theCoeff));
737   }
738   else if (theBase->type() == ENTITY_ARC) {
739     double theX, theY;
740     double anArcPoint[3][2];
741     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
742     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
743     for (int i = 0; i < 3; ++i, ++anIt) {
744       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(*anIt);
745       anArcPoint[i][0] = aPoint->x();
746       anArcPoint[i][1] = aPoint->y();
747     }
748     // project last point of arc on the arc
749     double x = anArcPoint[1][0] - anArcPoint[0][0];
750     double y = anArcPoint[1][1] - anArcPoint[0][1];
751     double aRad = sqrt(x*x + y*y);
752     x = anArcPoint[2][0] - anArcPoint[0][0];
753     y = anArcPoint[2][1] - anArcPoint[0][1];
754     double aNorm = sqrt(x*x + y*y);
755     if (aNorm >= tolerance) {
756       anArcPoint[2][0] = x * aRad / aNorm;
757       anArcPoint[2][1] = y * aRad / aNorm;
758     }
759     anArcPoint[1][0] -= anArcPoint[0][0];
760     anArcPoint[1][1] -= anArcPoint[0][1];
761     if (theCoeff < tolerance) {
762       theX = anArcPoint[0][0] + anArcPoint[1][0];
763       theY = anArcPoint[0][1] + anArcPoint[1][1];
764     } else if (1 - theCoeff < tolerance) {
765       theX = anArcPoint[0][0] + anArcPoint[2][0];
766       theY = anArcPoint[0][1] + anArcPoint[2][1];
767     } else {
768       std::shared_ptr<GeomAPI_Dir2d> aStartDir(new GeomAPI_Dir2d(anArcPoint[1][0], anArcPoint[1][1]));
769       std::shared_ptr<GeomAPI_Dir2d> aEndDir(new GeomAPI_Dir2d(anArcPoint[2][0], anArcPoint[2][1]));
770       double anAngle = aStartDir->angle(aEndDir);
771       if (anAngle < 0)
772         anAngle += 2.0 * PI;
773       anAngle *= theCoeff;
774       double aCos = cos(anAngle);
775       double aSin = sin(anAngle);
776       theX = anArcPoint[0][0] + anArcPoint[1][0] * aCos - anArcPoint[1][1] * aSin;
777       theY = anArcPoint[0][1] + anArcPoint[1][0] * aSin + anArcPoint[1][1] * aCos;
778     }
779     aMidPoint = std::shared_ptr<GeomAPI_XY>(new GeomAPI_XY(theX, theY));
780   }
781
782   if (!aMidPoint)
783     return EntityWrapperPtr();
784
785   std::list<ParameterWrapperPtr> aParameters;
786   aParameters.push_back(aBuilder->createParameter(myGroupID, aMidPoint->x()));
787   aParameters.push_back(aBuilder->createParameter(myGroupID, aMidPoint->y()));
788   // Create entity (parameters are not filled)
789   GCSPointPtr aPnt(new GCS::Point);
790   aPnt->x = std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(aParameters.front())->parameter();
791   aPnt->y = std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(aParameters.back())->parameter();
792
793   EntityWrapperPtr aResult(new PlaneGCSSolver_PointWrapper(AttributePtr(), aPnt));
794   aResult->setGroup(myGroupID);
795   aResult->setParameters(aParameters);
796
797   update(aResult);
798   return aResult;
799 }