Salome HOME
Inconsistency constraint maintenance (issue #1558)
[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   bool isFullyRemoved = SketchSolver_Storage::remove(theEntity);
246   if (isFullyRemoved) {
247     if (theEntity->type() == ENTITY_ARC) {
248       // remove arc additional constraints
249       std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::iterator
250           aFound = myArcConstraintMap.find(theEntity);
251       if (aFound != myArcConstraintMap.end()) {
252         myRemovedConstraints.insert(myRemovedConstraints.end(),
253             aFound->second.begin(), aFound->second.end());
254         myArcConstraintMap.erase(aFound);
255       }
256     }
257     if (theEntity->id() == myEntityLastID)
258       --myEntityLastID;
259   }
260   return isFullyRemoved;
261 }
262
263 bool PlaneGCSSolver_Storage::remove(ParameterWrapperPtr theParameter)
264 {
265   std::shared_ptr<PlaneGCSSolver_ParameterWrapper> aParam =
266       std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(theParameter);
267   if (aParam->isProcessed()) {
268     double* aValPtr = aParam->parameter();
269     GCS::VEC_pD::iterator anIt =  myParameters.begin();
270     for (; anIt != myParameters.end(); ++anIt)
271       if (*anIt == aValPtr)
272         break;
273     if (anIt != myParameters.end()) {
274       myParameters.erase(anIt);
275       setNeedToResolve(true);
276       aParam->setProcessed(false);
277     }
278     else {
279       for (anIt = myConst.begin(); anIt != myConst.end(); ++anIt)
280         if (*anIt == aValPtr)
281           break;
282       if (anIt != myConst.end()) {
283         myConst.erase(anIt);
284         setNeedToResolve(true);
285         aParam->setProcessed(false);
286       }
287     }
288   }
289   return true;
290 }
291
292
293 void PlaneGCSSolver_Storage::addCoincidentPoints(
294     EntityWrapperPtr theMaster, EntityWrapperPtr theSlave)
295 {
296   if (theMaster->type() != ENTITY_POINT || theSlave->type() != ENTITY_POINT)
297     return;
298
299   std::shared_ptr<PlaneGCSSolver_PointWrapper> aMaster =
300       std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(theMaster);
301   if (!aMaster)
302     aMaster = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(
303       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theMaster)->subEntities().front());
304   std::shared_ptr<PlaneGCSSolver_PointWrapper> aSlave =
305       std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(theSlave);
306   if (!aSlave)
307     aSlave = std::dynamic_pointer_cast<PlaneGCSSolver_PointWrapper>(
308       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theSlave)->subEntities().front());
309
310   // Search available coincidence
311   CoincidentPointsMap::iterator aMasterFound = myCoincidentPoints.find(aMaster);
312   CoincidentPointsMap::iterator aSlaveFound = myCoincidentPoints.find(aSlave);
313   if (aMasterFound == myCoincidentPoints.end() &&  aSlaveFound == myCoincidentPoints.end()) {
314     // try to find master and slave points in the lists of slaves of already existent coincidences
315     CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
316     for (; anIt != myCoincidentPoints.end(); ++anIt) {
317       if (anIt->second.find(aMaster) != anIt->second.end())
318         aMasterFound = anIt;
319       else if (anIt->second.find(aSlave) != anIt->second.end())
320         aSlaveFound = anIt;
321
322       if (aMasterFound != myCoincidentPoints.end() &&  aSlaveFound != myCoincidentPoints.end())
323         break;
324     }
325   }
326
327   if (aMasterFound == myCoincidentPoints.end()) {
328     // create new group
329     myCoincidentPoints[aMaster] = std::set<EntityWrapperPtr>();
330     aMasterFound = myCoincidentPoints.find(aMaster);
331   } else if (aMasterFound == aSlaveFound)
332     return; // already coincident
333
334   if (aSlaveFound != myCoincidentPoints.end()) {
335     // A slave has been found, we need to attach all points coincident with it to the new master
336     std::set<EntityWrapperPtr> aNewSlaves = aSlaveFound->second;
337     aNewSlaves.insert(aSlaveFound->first);
338     myCoincidentPoints.erase(aSlaveFound);
339
340     std::set<EntityWrapperPtr>::const_iterator aSlIt = aNewSlaves.begin();
341     for (; aSlIt != aNewSlaves.end(); ++aSlIt)
342       addCoincidentPoints(aMaster, *aSlIt);
343   } else {
344     //std::list<ParameterWrapperPtr> aSlaveParams = aSlave->parameters();
345     //aSlave->setParameters(aMaster->parameters());
346
347     //// Remove slave's parameters
348     //std::list<ParameterWrapperPtr>::iterator aParIt = aSlaveParams.begin();
349     //for (; aParIt != aSlaveParams.end(); ++aParIt)
350     //  remove(*aParIt);
351
352     aMasterFound->second.insert(aSlave);
353   }
354 }
355
356
357 void PlaneGCSSolver_Storage::changeGroup(EntityWrapperPtr theEntity, const GroupID& theGroup)
358 {
359   theEntity->setGroup(theGroup);
360   if (theGroup == myGroupID)
361     makeVariable(theEntity);
362   else {
363     if (theEntity->type() == ENTITY_POINT)
364       update(theEntity);
365     makeConstant(theEntity);
366   }
367 }
368
369 void PlaneGCSSolver_Storage::changeGroup(ParameterWrapperPtr theParam, const GroupID& theGroup)
370 {
371   // TODO
372 }
373
374 void PlaneGCSSolver_Storage::verifyFixed()
375 {
376   // TODO
377 }
378
379 void PlaneGCSSolver_Storage::processArc(const EntityWrapperPtr& theArc)
380 {
381   // no need to constraint a fixed arc
382   if (theArc->group() == GID_OUTOFGROUP)
383     return;
384
385   // Calculate additional parameters necessary for PlaneGCS
386   const std::list<EntityWrapperPtr>& aSubs = theArc->subEntities();
387   std::list<EntityWrapperPtr>::const_iterator aSubIt = aSubs.begin();
388   while ((*aSubIt)->type() == ENTITY_POINT) // search scalar entities
389     ++aSubIt;
390   double* aStartAngle = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt++)->scalar();
391   double* aEndAngle   = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt++)->scalar();
392   double* aRadius     = std::dynamic_pointer_cast<PlaneGCSSolver_ScalarWrapper>(*aSubIt)->scalar();
393
394   std::shared_ptr<SketchPlugin_Feature> anArcFeature =
395       std::dynamic_pointer_cast<SketchPlugin_Feature>(theArc->baseFeature());
396   std::shared_ptr<GeomDataAPI_Point2D> aCenterAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
397       anArcFeature->attribute(SketchPlugin_Arc::CENTER_ID()));
398   std::shared_ptr<GeomDataAPI_Point2D> aStartAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
399       anArcFeature->attribute(SketchPlugin_Arc::START_ID()));
400   std::shared_ptr<GeomDataAPI_Point2D> aEndAttr = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
401       anArcFeature->attribute(SketchPlugin_Arc::END_ID()));
402   if (!aCenterAttr || !aStartAttr || !aEndAttr)
403     return;
404   std::shared_ptr<GeomAPI_Pnt2d> aCenterPnt = aCenterAttr->pnt();
405   std::shared_ptr<GeomAPI_Pnt2d> aStartPnt  = aStartAttr->pnt();
406   std::shared_ptr<GeomAPI_Pnt2d> aEndPnt    = aEndAttr->pnt();
407
408   *aRadius = aCenterPnt->distance(aStartPnt);
409   if (!anArcFeature->lastResult())
410     return;
411   std::shared_ptr<GeomAPI_Edge> anArcEdge =
412       std::dynamic_pointer_cast<GeomAPI_Edge>(anArcFeature->lastResult()->shape());
413   if (!anArcEdge)
414     return;
415   anArcEdge->getRange(*aStartAngle, *aEndAngle);
416
417   // do not constraint copied arc
418   if (anArcFeature->isCopy())
419     return;
420   // No need to add constraints if they are already exist
421   std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::const_iterator
422       aFound = myArcConstraintMap.find(theArc);
423   if (aFound != myArcConstraintMap.end())
424     return;
425
426   // Prepare additional constraints to produce the arc
427   std::vector<GCSConstraintPtr> anArcConstraints;
428   std::shared_ptr<PlaneGCSSolver_EntityWrapper> anArcEnt =
429       std::dynamic_pointer_cast<PlaneGCSSolver_EntityWrapper>(theArc);
430   std::shared_ptr<GCS::Arc> anArc = std::dynamic_pointer_cast<GCS::Arc>(anArcEnt->entity());
431   // Distances from center till start and end points are equal to radius
432   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
433       anArc->center, anArc->start, anArc->rad)));
434   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
435       anArc->center, anArc->end, anArc->rad)));
436   // Angles of start and end points should be equal to given angles
437   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PAngle(
438       anArc->center, anArc->start, anArc->startAngle)));
439   anArcConstraints.push_back(GCSConstraintPtr(new GCS::ConstraintP2PAngle(
440       anArc->center, anArc->end, anArc->endAngle)));
441
442   myArcConstraintMap[theArc] = anArcConstraints;
443 }
444
445
446 void PlaneGCSSolver_Storage::makeConstant(const EntityWrapperPtr& theEntity)
447 {
448   toggleEntity(theEntity, myParameters, myConst);
449   if (theEntity->type() == ENTITY_POINT)
450     updateCoincident(theEntity);
451 }
452
453 void PlaneGCSSolver_Storage::makeVariable(const EntityWrapperPtr& theEntity)
454 {
455   toggleEntity(theEntity, myConst, myParameters);
456 }
457
458 static void getParametersToMove(const EntityWrapperPtr& theEntity, std::set<double*>& theParamList)
459 {
460   const std::list<ParameterWrapperPtr> aParams = theEntity->parameters();
461   std::list<ParameterWrapperPtr>::const_iterator aPIt = aParams.begin();
462   for (; aPIt != aParams.end(); ++aPIt)
463     theParamList.insert(
464         std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(*aPIt)->parameter());
465
466   const std::list<EntityWrapperPtr> aSubs = theEntity->subEntities();
467   std::list<EntityWrapperPtr>::const_iterator aSIt = aSubs.begin();
468
469   if (theEntity->type() == ENTITY_ARC) {
470     // workaround for the arc processing, because the arc is fixed by a set of constraints,
471     // which will conflict with all parameters fixed:
472     // 1. take center
473     getParametersToMove(*aSIt++, theParamList);
474     // 2. take start point
475     getParametersToMove(*aSIt++, theParamList);
476     // 3. skip end point, radius and start angle, but take end angle parameter
477     getParametersToMove(*(++aSIt), theParamList);
478   } else {
479     for (; aSIt != aSubs.end(); ++aSIt)
480       getParametersToMove(*aSIt, theParamList);
481   }
482 }
483
484 void PlaneGCSSolver_Storage::toggleEntity(
485     const EntityWrapperPtr& theEntity, GCS::VEC_pD& theFrom, GCS::VEC_pD& theTo)
486 {
487   std::set<double*> aParamsToMove;
488   getParametersToMove(theEntity, aParamsToMove);
489
490   GCS::VEC_pD::iterator anIt = theFrom.begin();
491   while (anIt != theFrom.end()) {
492     if (aParamsToMove.find(*anIt) == aParamsToMove.end()) {
493       ++anIt;
494       continue;
495     }
496
497     theTo.push_back(*anIt);
498     int aShift = int(anIt - theFrom.begin());
499     theFrom.erase(anIt);
500     anIt = theFrom.begin() + aShift;
501   }
502 }
503
504 void PlaneGCSSolver_Storage::updateCoincident(const EntityWrapperPtr& thePoint)
505 {
506   CoincidentPointsMap::iterator anIt = myCoincidentPoints.begin();
507   for (; anIt != myCoincidentPoints.end(); ++anIt) {
508     if (anIt->first == thePoint || anIt->second.find(thePoint) != anIt->second.end()) {
509       std::set<EntityWrapperPtr> aCoincident = anIt->second;
510       aCoincident.insert(anIt->first);
511
512       const std::list<ParameterWrapperPtr>& aBaseParams = thePoint->parameters();
513       std::list<ParameterWrapperPtr> aParams;
514       std::list<ParameterWrapperPtr>::const_iterator aBaseIt, anUpdIt;
515
516       std::set<EntityWrapperPtr>::const_iterator aCoincIt = aCoincident.begin();
517       for (; aCoincIt != aCoincident.end(); ++aCoincIt)
518         if (*aCoincIt != thePoint && (*aCoincIt)->group() != GID_OUTOFGROUP) {
519           aParams = (*aCoincIt)->parameters();
520           aBaseIt = aBaseParams.begin();
521           for (anUpdIt = aParams.begin(); anUpdIt != aParams.end(); ++anUpdIt, ++aBaseIt)
522             (*anUpdIt)->setValue((*aBaseIt)->value());
523         }
524
525       break;
526     }
527   }
528 }
529
530
531 bool PlaneGCSSolver_Storage::isRedundant(
532     GCSConstraintPtr theCheckedConstraint,
533     ConstraintWrapperPtr theParentConstraint,
534     std::list<std::set<double*> >& theCoincidentPoints) const
535 {
536   if (theParentConstraint->type() == CONSTRAINT_SYMMETRIC) {
537     if (theCheckedConstraint->getTypeId() == GCS::Perpendicular) {
538       BuilderPtr aBuilder = PlaneGCSSolver_Builder::getInstance();
539       // check the initial point is placed on the mirror line
540       std::list<EntityWrapperPtr> aSubs = theParentConstraint->entities();
541       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(aSubs.front());
542       std::shared_ptr<GeomAPI_Lin2d> aLine = aBuilder->line(aSubs.back());
543       return aLine->distance(aPoint) < tolerance;
544     }
545   }
546   else if (theParentConstraint->type() == CONSTRAINT_PT_PT_COINCIDENT) {
547     // Verify that the coincidence between points is already added
548     GCS::VEC_pD aParams = theCheckedConstraint->params();
549
550     std::list<std::set<double*> >::iterator aCoincIt, aFound1, aFound2;
551     aFound1 = aFound2 = theCoincidentPoints.end();
552     for (aCoincIt = theCoincidentPoints.begin(); aCoincIt != theCoincidentPoints.end(); ++aCoincIt) {
553       if (aFound1 == theCoincidentPoints.end() && aCoincIt->find(aParams[0]) != aCoincIt->end())
554         aFound1 = aCoincIt;
555       if (aFound2 == theCoincidentPoints.end() && aCoincIt->find(aParams[1]) != aCoincIt->end())
556         aFound2 = aCoincIt;
557       if (aFound1 != theCoincidentPoints.end() && aFound2 != theCoincidentPoints.end())
558         break;
559     }
560     if (aCoincIt != theCoincidentPoints.end()) { // both point are found
561       if (aFound1 == aFound2)
562         return true;
563       // merge two groups of coincidence
564       aFound1->insert(aFound2->begin(), aFound2->end());
565       theCoincidentPoints.erase(aFound2);
566     } else {
567       if (aFound1 != theCoincidentPoints.end())
568         aFound1->insert(aParams[1]);
569       else if (aFound2 != theCoincidentPoints.end())
570         aFound2->insert(aParams[0]);
571       else {
572         std::set<double*> aNewCoincidence;
573         aNewCoincidence.insert(aParams[0]);
574         aNewCoincidence.insert(aParams[1]);
575         theCoincidentPoints.push_back(aNewCoincidence);
576       }
577     }
578   }
579
580   return false;
581 }
582
583 void PlaneGCSSolver_Storage::initializeSolver(SolverPtr theSolver)
584 {
585   std::shared_ptr<PlaneGCSSolver_Solver> aSolver =
586       std::dynamic_pointer_cast<PlaneGCSSolver_Solver>(theSolver);
587   if (!aSolver)
588     return;
589   aSolver->clear();
590
591   if (myExistArc)
592     processArcs();
593
594   // initialize constraints
595   std::map<ConstraintPtr, std::list<ConstraintWrapperPtr> >::const_iterator
596       aCIt = myConstraintMap.begin();
597   GCS::SET_I aTangentIDs;
598   std::list<std::set<double*> > aCoincidentPoints;
599   for (; aCIt != myConstraintMap.end(); ++aCIt) {
600     std::list<ConstraintWrapperPtr>::const_iterator aCWIt = aCIt->second.begin();
601     for (; aCWIt != aCIt->second.end(); ++ aCWIt) {
602       std::shared_ptr<PlaneGCSSolver_ConstraintWrapper> aGCS =
603           std::dynamic_pointer_cast<PlaneGCSSolver_ConstraintWrapper>(*aCWIt);
604       std::list<GCSConstraintPtr>::const_iterator anIt = aGCS->constraints().begin();
605       for (; anIt != aGCS->constraints().end(); ++anIt)
606         if (!isRedundant(*anIt, aGCS, aCoincidentPoints))
607           aSolver->addConstraint(*anIt);
608     }
609     // store IDs of tangent constraints to avoid incorrect report of redundant constraints
610     if (aCIt->first && aCIt->first->getKind() == SketchPlugin_ConstraintTangent::ID())
611       for (aCWIt = aCIt->second.begin(); aCWIt != aCIt->second.end(); ++ aCWIt)
612         aTangentIDs.insert((int)(*aCWIt)->id());
613   }
614   // additional constraints for arcs
615   std::map<EntityWrapperPtr, std::vector<GCSConstraintPtr> >::const_iterator
616       anArcIt = myArcConstraintMap.begin();
617   for (; anArcIt != myArcConstraintMap.end(); ++anArcIt) {
618     std::vector<GCSConstraintPtr>::const_iterator anIt = anArcIt->second.begin();
619     for (; anIt != anArcIt->second.end(); ++anIt)
620       aSolver->addConstraint(*anIt);
621   }
622   // removed waste constraints
623   std::list<GCSConstraintPtr>::const_iterator aRemIt = myRemovedConstraints.begin();
624   for (; aRemIt != myRemovedConstraints.end(); ++aRemIt)
625     aSolver->removeConstraint(*aRemIt);
626   myRemovedConstraints.clear();
627   // set list of tangent constraints
628   aSolver->setTangent(aTangentIDs);
629   // initialize unknowns
630   aSolver->setParameters(myParameters);
631 }
632
633 void PlaneGCSSolver_Storage::refresh(bool theFixedOnly) const
634 {
635   //blockEvents(true);
636
637   std::map<AttributePtr, EntityWrapperPtr>::const_iterator anIt = myAttributeMap.begin();
638   std::list<ParameterWrapperPtr> aParams;
639   std::list<ParameterWrapperPtr>::const_iterator aParIt;
640   for (; anIt != myAttributeMap.end(); ++anIt) {
641     // the external feature always should keep the up to date values, so, 
642     // refresh from the solver is never needed
643     bool isExternal = false;
644     if (anIt->first.get()) {
645       std::shared_ptr<SketchPlugin_Feature> aSketchFeature = 
646         std::dynamic_pointer_cast<SketchPlugin_Feature>(anIt->first->owner());
647       if (aSketchFeature.get() && aSketchFeature->isExternal())
648         isExternal = true;
649     }
650
651     // update parameter wrappers and obtain values of attributes
652     aParams = anIt->second->parameters();
653     double aCoords[3];
654     bool isUpd[3] = {false};
655     int i = 0;
656     for (aParIt = aParams.begin(); i < 3 && aParIt != aParams.end(); ++aParIt, ++i) {
657       if (!theFixedOnly || isExternal ||
658           (*aParIt)->group() == GID_OUTOFGROUP || (*aParIt)->isParametric()) {
659         aCoords[i] = (*aParIt)->value();
660         isUpd[i] = true;
661       }
662     }
663     if (!isUpd[0] && !isUpd[1] && !isUpd[2])
664       continue; // nothing is updated
665
666     std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
667         std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anIt->first);
668     if (aPoint2D) {
669       if ((isUpd[0] && fabs(aPoint2D->x() - aCoords[0]) > tolerance) ||
670           (isUpd[1] && fabs(aPoint2D->y() - aCoords[1]) > tolerance) || isExternal) {
671         if (!isUpd[0] || isExternal) aCoords[0] = aPoint2D->x();
672         if (!isUpd[1] || isExternal) aCoords[1] = aPoint2D->y();
673         aPoint2D->setValue(aCoords[0], aCoords[1]);
674         // Find points coincident with this one (probably not in GID_OUTOFGROUP)
675         CoincidentPointsMap::const_iterator aCoincIt = myCoincidentPoints.begin();
676         for (; aCoincIt != myCoincidentPoints.end(); ++aCoincIt)
677           if (aCoincIt->first == anIt->second ||
678               aCoincIt->second.find(anIt->second) != aCoincIt->second.end())
679             break;
680         if (aCoincIt != myCoincidentPoints.end()) {
681           aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>(
682               aCoincIt->first->baseAttribute());
683           if (aPoint2D)
684             aPoint2D->setValue(aCoords[0], aCoords[1]);
685           std::set<EntityWrapperPtr>::const_iterator aSlaveIt = aCoincIt->second.begin();
686           for (; aSlaveIt != aCoincIt->second.end(); ++aSlaveIt) {
687             aPoint2D = std::dynamic_pointer_cast<GeomDataAPI_Point2D>((*aSlaveIt)->baseAttribute());
688             if (aPoint2D)
689               aPoint2D->setValue(aCoords[0], aCoords[1]);
690           }
691         }
692       }
693       continue;
694     }
695     AttributeDoublePtr aScalar = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anIt->first);
696     if (aScalar && !isExternal) {
697       if (isUpd[0] && fabs(aScalar->value() - aCoords[0]) > tolerance)
698         aScalar->setValue(aCoords[0]);
699       continue;
700     }
701   }
702
703   //blockEvents(false);
704 }
705
706 EntityWrapperPtr PlaneGCSSolver_Storage::calculateMiddlePoint(
707     EntityWrapperPtr theBase, double theCoeff)
708 {
709   std::shared_ptr<PlaneGCSSolver_Builder> aBuilder =
710       std::dynamic_pointer_cast<PlaneGCSSolver_Builder>(PlaneGCSSolver_Builder::getInstance());
711
712   std::shared_ptr<GeomAPI_XY> aMidPoint;
713   if (theBase->type() == ENTITY_LINE) {
714     std::shared_ptr<GeomAPI_Pnt2d> aPoints[2];
715     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
716     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
717     for (int i = 0; i < 2; ++i, ++anIt)
718       aPoints[i] = aBuilder->point(*anIt);
719     aMidPoint = aPoints[0]->xy()->multiplied(1.0 - theCoeff)->added(
720         aPoints[1]->xy()->multiplied(theCoeff));
721   }
722   else if (theBase->type() == ENTITY_ARC) {
723     double theX, theY;
724     double anArcPoint[3][2];
725     const std::list<EntityWrapperPtr>& aSubs = theBase->subEntities();
726     std::list<EntityWrapperPtr>::const_iterator anIt = aSubs.begin();
727     for (int i = 0; i < 3; ++i, ++anIt) {
728       std::shared_ptr<GeomAPI_Pnt2d> aPoint = aBuilder->point(*anIt);
729       anArcPoint[i][0] = aPoint->x();
730       anArcPoint[i][1] = aPoint->y();
731     }
732     // project last point of arc on the arc
733     double x = anArcPoint[1][0] - anArcPoint[0][0];
734     double y = anArcPoint[1][1] - anArcPoint[0][1];
735     double aRad = sqrt(x*x + y*y);
736     x = anArcPoint[2][0] - anArcPoint[0][0];
737     y = anArcPoint[2][1] - anArcPoint[0][1];
738     double aNorm = sqrt(x*x + y*y);
739     if (aNorm >= tolerance) {
740       anArcPoint[2][0] = x * aRad / aNorm;
741       anArcPoint[2][1] = y * aRad / aNorm;
742     }
743     anArcPoint[1][0] -= anArcPoint[0][0];
744     anArcPoint[1][1] -= anArcPoint[0][1];
745     if (theCoeff < tolerance) {
746       theX = anArcPoint[0][0] + anArcPoint[1][0];
747       theY = anArcPoint[0][1] + anArcPoint[1][1];
748     } else if (1 - theCoeff < tolerance) {
749       theX = anArcPoint[0][0] + anArcPoint[2][0];
750       theY = anArcPoint[0][1] + anArcPoint[2][1];
751     } else {
752       std::shared_ptr<GeomAPI_Dir2d> aStartDir(new GeomAPI_Dir2d(anArcPoint[1][0], anArcPoint[1][1]));
753       std::shared_ptr<GeomAPI_Dir2d> aEndDir(new GeomAPI_Dir2d(anArcPoint[2][0], anArcPoint[2][1]));
754       double anAngle = aStartDir->angle(aEndDir);
755       if (anAngle < 0)
756         anAngle += 2.0 * PI;
757       anAngle *= theCoeff;
758       double aCos = cos(anAngle);
759       double aSin = sin(anAngle);
760       theX = anArcPoint[0][0] + anArcPoint[1][0] * aCos - anArcPoint[1][1] * aSin;
761       theY = anArcPoint[0][1] + anArcPoint[1][0] * aSin + anArcPoint[1][1] * aCos;
762     }
763     aMidPoint = std::shared_ptr<GeomAPI_XY>(new GeomAPI_XY(theX, theY));
764   }
765
766   if (!aMidPoint)
767     return EntityWrapperPtr();
768
769   std::list<ParameterWrapperPtr> aParameters;
770   aParameters.push_back(aBuilder->createParameter(myGroupID, aMidPoint->x()));
771   aParameters.push_back(aBuilder->createParameter(myGroupID, aMidPoint->y()));
772   // Create entity (parameters are not filled)
773   GCSPointPtr aPnt(new GCS::Point);
774   aPnt->x = std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(aParameters.front())->parameter();
775   aPnt->y = std::dynamic_pointer_cast<PlaneGCSSolver_ParameterWrapper>(aParameters.back())->parameter();
776
777   EntityWrapperPtr aResult(new PlaneGCSSolver_PointWrapper(AttributePtr(), aPnt));
778   aResult->setGroup(myGroupID);
779   aResult->setParameters(aParameters);
780
781   update(aResult);
782   return aResult;
783 }