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 (aPntInd < 0)
 159         aPntInd = 0;
 160       else if (aPntInd >= myEquationsSystem.entities)
 161         aPntInd = myEquationsSystem.entities - 1;
 162       if (myEquationsSystem.entity[aPntInd].h > anEntity.point[aPnt])
 163         aStep = -1;
 164       for (; aPntInd >=0 && aPntInd < myEquationsSystem.entities; aPntInd += aStep)
 165         if (myEquationsSystem.entity[aPntInd].h == anEntity.point[aPnt])
 166           break;
 167
 168       // search coordinates of the point
 169       int aParamInd = myEquationsSystem.entity[aPntInd].param[0];
 170       if (aParamInd >= myEquationsSystem.params) {
 171         aParamInd = myEquationsSystem.params - 1;
 172         aStep = -1;
 173       }
 174       else if ((int)myEquationsSystem.param[aParamInd].h > aParamInd)
 175         aStep = -1;
 176       else aStep = 1;
 177
 178       for (; aParamInd >=0 && aParamInd < myEquationsSystem.params; aParamInd += aStep)
 179         if (myEquationsSystem.param[aParamInd].h == myEquationsSystem.entity[aPntInd].param[0])
 180           break;
 181       anArcPoints[aPnt][0] = myEquationsSystem.param[aParamInd].val;
 182       anArcPoints[aPnt][1] = myEquationsSystem.param[aParamInd+1].val;
 183     }
 184
 185     // check radius of arc
 186     anArcPoints[1][0] -= anArcPoints[0][0];
 187     anArcPoints[1][1] -= anArcPoints[0][1];
 188     anArcPoints[2][0] -= anArcPoints[0][0];
 189     anArcPoints[2][1] -= anArcPoints[0][1];
 190     if (anArcPoints[1][0] * anArcPoints[1][0] + anArcPoints[1][1] * anArcPoints[1][1] < aTol2 ||
 191         anArcPoints[2][0] * anArcPoints[2][0] + anArcPoints[2][1] * anArcPoints[2][1] < aTol2)
 192       return true;
 193   }
 194   return false;
 195 }