Revert "Make the copies fixed in multi-rotation, multi-translation (issue #1471)"

[modules/shaper.git] / src / SketchSolver / SolveSpaceSolver / SolveSpaceSolver_Solver.cpp

diff --git a/src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp b/src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp
index de1cc3edd1880ed05ed5ad3e17eac5aadf309b4b..5b34b1174cc6047e6b64170cfbf4a8bac9f45d1d 100644 (file)
--- a/src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp
+++ b/src/SketchSolver/SolveSpaceSolver/SolveSpaceSolver_Solver.cpp
@@ -27,6 +27,8 @@ SolveSpaceSolver_Solver::SolveSpaceSolver_Solver()
   // If the set of constraints is inconsistent,
   // the failed field will contain wrong constraints
   myEquationsSystem.calculateFaileds = 0;
+
+  myParamsCopy = 0;
 }
 
 SolveSpaceSolver_Solver::~SolveSpaceSolver_Solver()
@@ -37,6 +39,9 @@ SolveSpaceSolver_Solver::~SolveSpaceSolver_Solver()
   if (myEquationsSystem.failed)
     delete[] myEquationsSystem.failed;
   myEquationsSystem.failed = 0;
+  if (myParamsCopy)
+    delete [] myParamsCopy;
+  myParamsCopy = 0;
 }
 
 void SolveSpaceSolver_Solver::setParameters(Slvs_Param* theParameters, int theSize)
@@ -82,13 +87,17 @@ void SolveSpaceSolver_Solver::setConstraints(Slvs_Constraint* theConstraints, in
 
 SketchSolver_SolveStatus SolveSpaceSolver_Solver::solve()
 {
-  if (myEquationsSystem.constraints <= 0)
-    return STATUS_EMPTYSET;
+  //if (myEquationsSystem.constraints <= 0)
+  //  return STATUS_EMPTYSET;
 
-  myEquationsSystem.calculateFaileds = myFindFaileds ? 1 : 0;
+  myEquationsSystem.calculateFaileds = 0;
+  if (myFindFaileds) {
+    myEquationsSystem.calculateFaileds = 1;
+    myEquationsSystem.faileds = myEquationsSystem.constraints;
+  }
 
   Events_LongOp::start(this);
-  Slvs_Solve(&myEquationsSystem, myGroup);
+  Slvs_Solve(&myEquationsSystem, (Slvs_hGroup)myGroup);
   Events_LongOp::end(this);
 
   SketchSolver_SolveStatus aStatus;
@@ -104,5 +113,100 @@ SketchSolver_SolveStatus SolveSpaceSolver_Solver::solve()
   default:
     aStatus = STATUS_FAILED;
   }
+
+  if (aStatus == STATUS_OK) {
+    // additional verification of arcs to be non-degenerated
+    if (hasDegeneratedArcs()) {
+      undo();
+      aStatus = STATUS_INCONSISTENT;
+    }
+  }
+
   return aStatus;
 }
+
+void SolveSpaceSolver_Solver::prepare()
+{
+  // make a copy of parameters to be able to make undo
+  if (myParamsCopy)
+    delete [] myParamsCopy;
+  myParamsCopy = new Slvs_Param[myEquationsSystem.params];
+  memcpy(myParamsCopy, myEquationsSystem.param, myEquationsSystem.params * sizeof(Slvs_Param));
+}
+
+void SolveSpaceSolver_Solver::undo()
+{
+  if (myParamsCopy) {
+    memcpy(myEquationsSystem.param, myParamsCopy, myEquationsSystem.params * sizeof(Slvs_Param));
+    delete [] myParamsCopy;
+  }
+  myParamsCopy = 0;
+}
+
+
+bool SolveSpaceSolver_Solver::hasDegeneratedArcs() const
+{
+  const double aTol2 = tolerance * tolerance;
+  double anArcPoints[3][2];
+
+  for (int anEnt = 0; anEnt < myEquationsSystem.entities; ++anEnt) {
+    const Slvs_Entity& anEntity = myEquationsSystem.entity[anEnt];
+    if (anEntity.type != SLVS_E_ARC_OF_CIRCLE)
+      continue;
+
+    for (int aPnt = 0; aPnt < 3; ++aPnt) {
+      // search point of arc
+      const int aShift = anEntity.point[aPnt] - anEntity.h;
+      int aPntInd = anEnt + aShift;
+      int aStep = 1;
+      if (aPntInd < 0)
+        aPntInd = 0;
+      else if (aPntInd >= myEquationsSystem.entities)
+        aPntInd = myEquationsSystem.entities - 1;
+      if (myEquationsSystem.entity[aPntInd].h > anEntity.point[aPnt])
+        aStep = -1;
+      for (; aPntInd >=0 && aPntInd < myEquationsSystem.entities; aPntInd += aStep)
+        if (myEquationsSystem.entity[aPntInd].h == anEntity.point[aPnt])
+          break;
+
+      // search coordinates of the point
+      int aParamInd = myEquationsSystem.entity[aPntInd].param[0];
+      if (aParamInd >= myEquationsSystem.params) {
+        aParamInd = myEquationsSystem.params - 1;
+        aStep = -1;
+      }
+      else if ((int)myEquationsSystem.param[aParamInd].h > aParamInd)
+        aStep = -1;
+      else aStep = 1;
+
+      for (; aParamInd >=0 && aParamInd < myEquationsSystem.params; aParamInd += aStep)
+        if (myEquationsSystem.param[aParamInd].h == myEquationsSystem.entity[aPntInd].param[0])
+          break;
+      anArcPoints[aPnt][0] = myEquationsSystem.param[aParamInd].val;
+      anArcPoints[aPnt][1] = myEquationsSystem.param[aParamInd+1].val;
+    }
+
+    // check radius of arc
+    anArcPoints[1][0] -= anArcPoints[0][0];
+    anArcPoints[1][1] -= anArcPoints[0][1];
+    anArcPoints[2][0] -= anArcPoints[0][0];
+    anArcPoints[2][1] -= anArcPoints[0][1];
+    if (anArcPoints[1][0] * anArcPoints[1][0] + anArcPoints[1][1] * anArcPoints[1][1] < aTol2 ||
+        anArcPoints[2][0] * anArcPoints[2][0] + anArcPoints[2][1] * anArcPoints[2][1] < aTol2)
+      return true;
+  }
+  return false;
+}
+
+bool SolveSpaceSolver_Solver::isConflicting(const ConstraintID& theConstraint) const
+{
+  for (int i = 0; i < myEquationsSystem.faileds; ++i)
+    if (myEquationsSystem.failed[i] == (Slvs_hConstraint)theConstraint)
+      return true;
+  return false;
+}
+
+int SolveSpaceSolver_Solver::dof() const
+{
+  return myEquationsSystem.dof;
+}