]> SALOME platform Git repositories - modules/shaper.git/blob - src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp
Salome HOME
Merge remote-tracking branch 'remotes/origin/BR_PlaneGCS' into CodeCleanup
[modules/shaper.git] / src / SketchSolver / SolveSpaceSolver / SolveSpaceSolver_Solver.cpp
1 // Copyright (C) 2014-20xx CEA/DEN, EDF R&D
2
3 // File:    SolveSpaceSolver_Solver.cpp
4 // Created: 07 May 2014
5 // Author:  Artem ZHIDKOV
6
7 #include "SolveSpaceSolver_Solver.h"
8 #include <Events_LongOp.h>
9
10 SolveSpaceSolver_Solver::SolveSpaceSolver_Solver()
11 {
12   // Nullify all elements of the set of equations
13   myEquationsSystem.param = 0;
14   myEquationsSystem.params = 0;
15   myEquationsSystem.entity = 0;
16   myEquationsSystem.entities = 0;
17   myEquationsSystem.constraint = 0;
18   myEquationsSystem.constraints = 0;
19   myEquationsSystem.failed = 0;
20   myEquationsSystem.faileds = 0;
21
22   myEquationsSystem.dragged[0] = 0;
23   myEquationsSystem.dragged[1] = 0;
24   myEquationsSystem.dragged[2] = 0;
25   myEquationsSystem.dragged[3] = 0;
26
27   // If the set of constraints is inconsistent,
28   // the failed field will contain wrong constraints
29   myEquationsSystem.calculateFaileds = 0;
30
31   myParamsCopy = 0;
32 }
33
34 SolveSpaceSolver_Solver::~SolveSpaceSolver_Solver()
35 {
36   if (myEquationsSystem.constraint)
37     delete[] myEquationsSystem.constraint;
38   myEquationsSystem.constraint = 0;
39   if (myEquationsSystem.failed)
40     delete[] myEquationsSystem.failed;
41   myEquationsSystem.failed = 0;
42   if (myParamsCopy)
43     delete [] myParamsCopy;
44   myParamsCopy = 0;
45 }
46
47 void SolveSpaceSolver_Solver::setParameters(Slvs_Param* theParameters, int theSize)
48 {
49   myEquationsSystem.param = theParameters;
50   myEquationsSystem.params = theSize;
51 }
52
53
54 void SolveSpaceSolver_Solver::setDraggedParameters(const Slvs_hParam* theDragged)
55 {
56   for (unsigned int i = 0; i < 4; i++)
57     myEquationsSystem.dragged[i] = theDragged[i];
58 }
59
60 void SolveSpaceSolver_Solver::setEntities(Slvs_Entity* theEntities, int theSize)
61 {
62   myEquationsSystem.entity = theEntities;
63   myEquationsSystem.entities = theSize;
64 }
65
66 void SolveSpaceSolver_Solver::setConstraints(Slvs_Constraint* theConstraints, int theSize)
67 {
68   if (!myEquationsSystem.constraint) {
69     myEquationsSystem.constraint = new Slvs_Constraint[theSize];
70     myEquationsSystem.constraints = theSize;
71     myEquationsSystem.failed = new Slvs_hConstraint[theSize];
72   }
73   else if (myEquationsSystem.constraints != theSize) {
74     if (theSize > myEquationsSystem.constraints) {
75       delete[] myEquationsSystem.constraint;
76       myEquationsSystem.constraint = new Slvs_Constraint[theSize];
77       if (myEquationsSystem.failed)
78         delete[] myEquationsSystem.failed;
79       myEquationsSystem.failed = new Slvs_hConstraint[theSize];
80     }
81     myEquationsSystem.constraints = theSize;
82   }
83   memcpy(myEquationsSystem.constraint, theConstraints, theSize * sizeof(Slvs_Constraint));
84   memset(myEquationsSystem.failed, SLVS_C_UNKNOWN, theSize * sizeof(Slvs_hConstraint));
85 }
86
87
88 SketchSolver_SolveStatus SolveSpaceSolver_Solver::solve()
89 {
90   //if (myEquationsSystem.constraints <= 0)
91   //  return STATUS_EMPTYSET;
92
93   myEquationsSystem.calculateFaileds = myFindFaileds ? 1 : 0;
94
95   Events_LongOp::start(this);
96   Slvs_Solve(&myEquationsSystem, myGroup);
97   Events_LongOp::end(this);
98
99   SketchSolver_SolveStatus aStatus;
100   switch (myEquationsSystem.result) {
101   case SLVS_RESULT_OKAY:
102     aStatus = STATUS_OK;
103     break;
104   case SLVS_RESULT_INCONSISTENT:
105   case SLVS_RESULT_DIDNT_CONVERGE:
106   case SLVS_RESULT_TOO_MANY_UNKNOWNS:
107     aStatus = STATUS_INCONSISTENT;
108     break;
109   default:
110     aStatus = STATUS_FAILED;
111   }
112
113   if (aStatus == STATUS_OK) {
114     // additional verification of arcs to be non-degenerated
115     if (hasDegeneratedArcs()) {
116       undo();
117       aStatus = STATUS_INCONSISTENT;
118     }
119   }
120
121   return aStatus;
122 }
123
124 void SolveSpaceSolver_Solver::prepare()
125 {
126   // make a copy of parameters to be able to make undo
127   if (myParamsCopy)
128     delete [] myParamsCopy;
129   myParamsCopy = new Slvs_Param[myEquationsSystem.params];
130   memcpy(myParamsCopy, myEquationsSystem.param, myEquationsSystem.params * sizeof(Slvs_Param));
131 }
132
133 void SolveSpaceSolver_Solver::undo()
134 {
135   if (myParamsCopy) {
136     memcpy(myEquationsSystem.param, myParamsCopy, myEquationsSystem.params * sizeof(Slvs_Param));
137     delete [] myParamsCopy;
138   }
139   myParamsCopy = 0;
140 }
141
142
143 bool SolveSpaceSolver_Solver::hasDegeneratedArcs() const
144 {
145   const double aTol2 = tolerance * tolerance;
146   double anArcPoints[3][2];
147
148   for (int anEnt = 0; anEnt < myEquationsSystem.entities; ++anEnt) {
149     const Slvs_Entity& anEntity = myEquationsSystem.entity[anEnt];
150     if (anEntity.type != SLVS_E_ARC_OF_CIRCLE)
151       continue;
152
153     for (int aPnt = 0; aPnt < 3; ++aPnt) {
154       // search point of arc
155       const int aShift = anEntity.point[aPnt] - anEntity.h;
156       int aPntInd = anEnt + aShift;
157       int aStep = 1;
158       if (myEquationsSystem.entity[aPntInd].h > anEntity.point[aPnt])
159         aStep = -1;
160       for (; aPntInd >=0 && aPntInd < myEquationsSystem.entities; aPntInd += aStep)
161         if (myEquationsSystem.entity[aPntInd].h == anEntity.point[aPnt])
162           break;
163
164       // search coordinates of the point
165       int aParamInd = myEquationsSystem.entity[aPntInd].param[0];
166       if (aParamInd >= myEquationsSystem.params) {
167         aParamInd = myEquationsSystem.params - 1;
168         aStep = -1;
169       }
170       else if ((int)myEquationsSystem.param[aParamInd].h > aParamInd)
171         aStep = -1;
172       else aStep = 1;
173
174       for (; aParamInd >=0 && aParamInd < myEquationsSystem.params; aParamInd += aStep)
175         if (myEquationsSystem.param[aParamInd].h == myEquationsSystem.entity[aPntInd].param[0])
176           break;
177       anArcPoints[aPnt][0] = myEquationsSystem.param[aParamInd].val;
178       anArcPoints[aPnt][1] = myEquationsSystem.param[aParamInd+1].val;
179     }
180
181     // check radius of arc
182     anArcPoints[1][0] -= anArcPoints[0][0];
183     anArcPoints[1][1] -= anArcPoints[0][1];
184     anArcPoints[2][0] -= anArcPoints[0][0];
185     anArcPoints[2][1] -= anArcPoints[0][1];
186     if (anArcPoints[1][0] * anArcPoints[1][0] + anArcPoints[1][1] * anArcPoints[1][1] < aTol2 ||
187         anArcPoints[2][0] * anArcPoints[2][0] + anArcPoints[2][1] * anArcPoints[2][1] < aTol2)
188       return true;
189   }
190   return false;
191 }