src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp

   1 // Copyright (C) 2014-2017  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or
  18 // email : webmaster.salome@opencascade.com<mailto:webmaster.salome@opencascade.com>
  19 //
  20
  21 #include "SolveSpaceSolver_Solver.h"
  22 #include <Events_LongOp.h>
  23
  24 SolveSpaceSolver_Solver::SolveSpaceSolver_Solver()
  25 {
  26   // Nullify all elements of the set of equations
  27   myEquationsSystem.param = 0;
  28   myEquationsSystem.params = 0;
  29   myEquationsSystem.entity = 0;
  30   myEquationsSystem.entities = 0;
  31   myEquationsSystem.constraint = 0;
  32   myEquationsSystem.constraints = 0;
  33   myEquationsSystem.failed = 0;
  34   myEquationsSystem.faileds = 0;
  35
  36   myEquationsSystem.dragged[0] = 0;
  37   myEquationsSystem.dragged[1] = 0;
  38   myEquationsSystem.dragged[2] = 0;
  39   myEquationsSystem.dragged[3] = 0;
  40
  41   // If the set of constraints is inconsistent,
  42   // the failed field will contain wrong constraints
  43   myEquationsSystem.calculateFaileds = 0;
  44
  45   myParamsCopy = 0;
  46 }
  47
  48 SolveSpaceSolver_Solver::~SolveSpaceSolver_Solver()
  49 {
  50   if (myEquationsSystem.constraint)
  51     delete[] myEquationsSystem.constraint;
  52   myEquationsSystem.constraint = 0;
  53   if (myEquationsSystem.failed)
  54     delete[] myEquationsSystem.failed;
  55   myEquationsSystem.failed = 0;
  56   if (myParamsCopy)
  57     delete [] myParamsCopy;
  58   myParamsCopy = 0;
  59 }
  60
  61 void SolveSpaceSolver_Solver::setParameters(Slvs_Param* theParameters, int theSize)
  62 {
  63   myEquationsSystem.param = theParameters;
  64   myEquationsSystem.params = theSize;
  65 }
  66
  67
  68 void SolveSpaceSolver_Solver::setDraggedParameters(const Slvs_hParam* theDragged)
  69 {
  70   for (unsigned int i = 0; i < 4; i++)
  71     myEquationsSystem.dragged[i] = theDragged[i];
  72 }
  73
  74 void SolveSpaceSolver_Solver::setEntities(Slvs_Entity* theEntities, int theSize)
  75 {
  76   myEquationsSystem.entity = theEntities;
  77   myEquationsSystem.entities = theSize;
  78 }
  79
  80 void SolveSpaceSolver_Solver::setConstraints(Slvs_Constraint* theConstraints, int theSize)
  81 {
  82   if (!myEquationsSystem.constraint) {
  83     myEquationsSystem.constraint = new Slvs_Constraint[theSize];
  84     myEquationsSystem.constraints = theSize;
  85     myEquationsSystem.failed = new Slvs_hConstraint[theSize];
  86   }
  87   else if (myEquationsSystem.constraints != theSize) {
  88     if (theSize > myEquationsSystem.constraints) {
  89       delete[] myEquationsSystem.constraint;
  90       myEquationsSystem.constraint = new Slvs_Constraint[theSize];
  91       if (myEquationsSystem.failed)
  92         delete[] myEquationsSystem.failed;
  93       myEquationsSystem.failed = new Slvs_hConstraint[theSize];
  94     }
  95     myEquationsSystem.constraints = theSize;
  96   }
  97   memcpy(myEquationsSystem.constraint, theConstraints, theSize * sizeof(Slvs_Constraint));
  98   memset(myEquationsSystem.failed, SLVS_C_UNKNOWN, theSize * sizeof(Slvs_hConstraint));
  99 }
 100
 101
 102 SketchSolver_SolveStatus SolveSpaceSolver_Solver::solve()
 103 {
 104   //if (myEquationsSystem.constraints <= 0)
 105   //  return STATUS_EMPTYSET;
 106
 107   myEquationsSystem.calculateFaileds = 0;
 108   if (myFindFaileds) {
 109     myEquationsSystem.calculateFaileds = 1;
 110     myEquationsSystem.faileds = myEquationsSystem.constraints;
 111   }
 112
 113   Events_LongOp::start(this);
 114   Slvs_Solve(&myEquationsSystem, (Slvs_hGroup)myGroup);
 115   Events_LongOp::end(this);
 116
 117   SketchSolver_SolveStatus aStatus;
 118   switch (myEquationsSystem.result) {
 119   case SLVS_RESULT_OKAY:
 120     aStatus = STATUS_OK;
 121     break;
 122   case SLVS_RESULT_INCONSISTENT:
 123   case SLVS_RESULT_DIDNT_CONVERGE:
 124   case SLVS_RESULT_TOO_MANY_UNKNOWNS:
 125     aStatus = STATUS_INCONSISTENT;
 126     break;
 127   default:
 128     aStatus = STATUS_FAILED;
 129   }
 130
 131   if (aStatus == STATUS_OK) {
 132     // additional verification of arcs to be non-degenerated
 133     if (hasDegeneratedArcs()) {
 134       undo();
 135       aStatus = STATUS_INCONSISTENT;
 136     }
 137   }
 138
 139   return aStatus;
 140 }
 141
 142 void SolveSpaceSolver_Solver::prepare()
 143 {
 144   // make a copy of parameters to be able to make undo
 145   if (myParamsCopy)
 146     delete [] myParamsCopy;
 147   myParamsCopy = new Slvs_Param[myEquationsSystem.params];
 148   memcpy(myParamsCopy, myEquationsSystem.param, myEquationsSystem.params * sizeof(Slvs_Param));
 149 }
 150
 151 void SolveSpaceSolver_Solver::undo()
 152 {
 153   if (myParamsCopy) {
 154     memcpy(myEquationsSystem.param, myParamsCopy, myEquationsSystem.params * sizeof(Slvs_Param));
 155     delete [] myParamsCopy;
 156   }
 157   myParamsCopy = 0;
 158 }
 159
 160
 161 bool SolveSpaceSolver_Solver::hasDegeneratedArcs() const
 162 {
 163   const double aTol2 = tolerance * tolerance;
 164   double anArcPoints[3][2];
 165
 166   for (int anEnt = 0; anEnt < myEquationsSystem.entities; ++anEnt) {
 167     const Slvs_Entity& anEntity = myEquationsSystem.entity[anEnt];
 168     if (anEntity.type != SLVS_E_ARC_OF_CIRCLE)
 169       continue;
 170
 171     for (int aPnt = 0; aPnt < 3; ++aPnt) {
 172       // search point of arc
 173       const int aShift = anEntity.point[aPnt] - anEntity.h;
 174       int aPntInd = anEnt + aShift;
 175       int aStep = 1;
 176       if (aPntInd < 0)
 177         aPntInd = 0;
 178       else if (aPntInd >= myEquationsSystem.entities)
 179         aPntInd = myEquationsSystem.entities - 1;
 180       if (myEquationsSystem.entity[aPntInd].h > anEntity.point[aPnt])
 181         aStep = -1;
 182       for (; aPntInd >=0 && aPntInd < myEquationsSystem.entities; aPntInd += aStep)
 183         if (myEquationsSystem.entity[aPntInd].h == anEntity.point[aPnt])
 184           break;
 185
 186       // search coordinates of the point
 187       int aParamInd = myEquationsSystem.entity[aPntInd].param[0];
 188       if (aParamInd >= myEquationsSystem.params) {
 189         aParamInd = myEquationsSystem.params - 1;
 190         aStep = -1;
 191       }
 192       else if ((int)myEquationsSystem.param[aParamInd].h > aParamInd)
 193         aStep = -1;
 194       else aStep = 1;
 195
 196       for (; aParamInd >=0 && aParamInd < myEquationsSystem.params; aParamInd += aStep)
 197         if (myEquationsSystem.param[aParamInd].h == myEquationsSystem.entity[aPntInd].param[0])
 198           break;
 199       anArcPoints[aPnt][0] = myEquationsSystem.param[aParamInd].val;
 200       anArcPoints[aPnt][1] = myEquationsSystem.param[aParamInd+1].val;
 201     }
 202
 203     // check radius of arc
 204     anArcPoints[1][0] -= anArcPoints[0][0];
 205     anArcPoints[1][1] -= anArcPoints[0][1];
 206     anArcPoints[2][0] -= anArcPoints[0][0];
 207     anArcPoints[2][1] -= anArcPoints[0][1];
 208     if (anArcPoints[1][0] * anArcPoints[1][0] + anArcPoints[1][1] * anArcPoints[1][1] < aTol2 ||
 209         anArcPoints[2][0] * anArcPoints[2][0] + anArcPoints[2][1] * anArcPoints[2][1] < aTol2)
 210       return true;
 211   }
 212   return false;
 213 }
 214
 215 bool SolveSpaceSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
 216 {
 217   for (int i = 0; i < myEquationsSystem.faileds; ++i)
 218     if (myEquationsSystem.failed[i] == (Slvs_hConstraint)theConstraint)
 219       return true;
 220   return false;
 221 }
 222
 223 int SolveSpaceSolver_Solver::dof() const
 224 {
 225   return myEquationsSystem.dof;
 226 }