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 = 0;
  94   if (myFindFaileds) {
  95     myEquationsSystem.calculateFaileds = 1;
  96     myEquationsSystem.faileds = myEquationsSystem.constraints;
  97   }
  98
  99   Events_LongOp::start(this);
 100   Slvs_Solve(&myEquationsSystem, (Slvs_hGroup)myGroup);
 101   Events_LongOp::end(this);
 102
 103   SketchSolver_SolveStatus aStatus;
 104   switch (myEquationsSystem.result) {
 105   case SLVS_RESULT_OKAY:
 106     aStatus = STATUS_OK;
 107     break;
 108   case SLVS_RESULT_INCONSISTENT:
 109   case SLVS_RESULT_DIDNT_CONVERGE:
 110   case SLVS_RESULT_TOO_MANY_UNKNOWNS:
 111     aStatus = STATUS_INCONSISTENT;
 112     break;
 113   default:
 114     aStatus = STATUS_FAILED;
 115   }
 116
 117   if (aStatus == STATUS_OK) {
 118     // additional verification of arcs to be non-degenerated
 119     if (hasDegeneratedArcs()) {
 120       undo();
 121       aStatus = STATUS_INCONSISTENT;
 122     }
 123   }
 124
 125   return aStatus;
 126 }
 127
 128 void SolveSpaceSolver_Solver::prepare()
 129 {
 130   // make a copy of parameters to be able to make undo
 131   if (myParamsCopy)
 132     delete [] myParamsCopy;
 133   myParamsCopy = new Slvs_Param[myEquationsSystem.params];
 134   memcpy(myParamsCopy, myEquationsSystem.param, myEquationsSystem.params * sizeof(Slvs_Param));
 135 }
 136
 137 void SolveSpaceSolver_Solver::undo()
 138 {
 139   if (myParamsCopy) {
 140     memcpy(myEquationsSystem.param, myParamsCopy, myEquationsSystem.params * sizeof(Slvs_Param));
 141     delete [] myParamsCopy;
 142   }
 143   myParamsCopy = 0;
 144 }
 145
 146
 147 bool SolveSpaceSolver_Solver::hasDegeneratedArcs() const
 148 {
 149   const double aTol2 = tolerance * tolerance;
 150   double anArcPoints[3][2];
 151
 152   for (int anEnt = 0; anEnt < myEquationsSystem.entities; ++anEnt) {
 153     const Slvs_Entity& anEntity = myEquationsSystem.entity[anEnt];
 154     if (anEntity.type != SLVS_E_ARC_OF_CIRCLE)
 155       continue;
 156
 157     for (int aPnt = 0; aPnt < 3; ++aPnt) {
 158       // search point of arc
 159       const int aShift = anEntity.point[aPnt] - anEntity.h;
 160       int aPntInd = anEnt + aShift;
 161       int aStep = 1;
 162       if (aPntInd < 0)
 163         aPntInd = 0;
 164       else if (aPntInd >= myEquationsSystem.entities)
 165         aPntInd = myEquationsSystem.entities - 1;
 166       if (myEquationsSystem.entity[aPntInd].h > anEntity.point[aPnt])
 167         aStep = -1;
 168       for (; aPntInd >=0 && aPntInd < myEquationsSystem.entities; aPntInd += aStep)
 169         if (myEquationsSystem.entity[aPntInd].h == anEntity.point[aPnt])
 170           break;
 171
 172       // search coordinates of the point
 173       int aParamInd = myEquationsSystem.entity[aPntInd].param[0];
 174       if (aParamInd >= myEquationsSystem.params) {
 175         aParamInd = myEquationsSystem.params - 1;
 176         aStep = -1;
 177       }
 178       else if ((int)myEquationsSystem.param[aParamInd].h > aParamInd)
 179         aStep = -1;
 180       else aStep = 1;
 181
 182       for (; aParamInd >=0 && aParamInd < myEquationsSystem.params; aParamInd += aStep)
 183         if (myEquationsSystem.param[aParamInd].h == myEquationsSystem.entity[aPntInd].param[0])
 184           break;
 185       anArcPoints[aPnt][0] = myEquationsSystem.param[aParamInd].val;
 186       anArcPoints[aPnt][1] = myEquationsSystem.param[aParamInd+1].val;
 187     }
 188
 189     // check radius of arc
 190     anArcPoints[1][0] -= anArcPoints[0][0];
 191     anArcPoints[1][1] -= anArcPoints[0][1];
 192     anArcPoints[2][0] -= anArcPoints[0][0];
 193     anArcPoints[2][1] -= anArcPoints[0][1];
 194     if (anArcPoints[1][0] * anArcPoints[1][0] + anArcPoints[1][1] * anArcPoints[1][1] < aTol2 ||
 195         anArcPoints[2][0] * anArcPoints[2][0] + anArcPoints[2][1] * anArcPoints[2][1] < aTol2)
 196       return true;
 197   }
 198   return false;
 199 }
 200
 201 bool SolveSpaceSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 202 {
 203   for (int i = 0; i < myEquationsSystem.faileds; ++i)
 204     if (myEquationsSystem.failed[i] == (Slvs_hConstraint)theConstraint)
 205       return true;
 206   return false;
 207 }
 208
 209 int SolveSpaceSolver_Solver::dof() const
 210 {
 211   return myEquationsSystem.dof;
 212 }