From: azv <azv@opencascade.com>
Date: Fri, 10 Apr 2015 07:30:00 +0000 (+0300)
Subject: Update Mirror and Rigid constraints to work properly with entities under Equal constraint
X-Git-Tag: V_1.1.0~51^2~5
X-Git-Url: http://git.salome-platform.org/gitweb/?a=commitdiff_plain;h=041e61bc73a3917195bb1f32d9375baefdfdad77;p=modules%2Fshaper.git

Update Mirror and Rigid constraints to work properly with entities under Equal constraint
---

diff --git a/src/SketchSolver/SketchSolver_Constraint.cpp b/src/SketchSolver/SketchSolver_Constraint.cpp
index 36bde98f8..946ee0ede 100644
--- a/src/SketchSolver/SketchSolver_Constraint.cpp
+++ b/src/SketchSolver/SketchSolver_Constraint.cpp
@@ -33,10 +33,16 @@ SketchSolver_Constraint::~SketchSolver_Constraint()
     myStorage->removeEntity(anIt2->second);
   myAttributeMap.clear();
 
-  std::map<FeaturePtr, Slvs_hEntity>::const_iterator anIt3 =  myFeatureMap.begin();
-  for (; anIt3 != myFeatureMap.end(); anIt3++)
-    myStorage->removeEntity(anIt3->second);
-  myFeatureMap.clear();
+  std::map<FeaturePtr, Slvs_hEntity>::iterator anIt3 =  myFeatureMap.begin();
+  while (!myFeatureMap.empty()) {
+    std::shared_ptr<SketchPlugin_Feature> aFeature =
+        std::dynamic_pointer_cast<SketchPlugin_Feature>(anIt3->first);
+    Slvs_hEntity anEnt = anIt3->second;
+    std::map<FeaturePtr, Slvs_hEntity>::iterator aRemIt = anIt3++;
+    myFeatureMap.erase(aRemIt);
+    if (!myGroup->isInteract(aFeature))
+      myStorage->removeEntity(anEnt);
+  }
 
   std::vector<Slvs_hConstraint>::const_iterator anIt4 = mySlvsConstraints.begin();
   for (; anIt4 != mySlvsConstraints.end(); anIt4++)
diff --git a/src/SketchSolver/SketchSolver_ConstraintMirror.cpp b/src/SketchSolver/SketchSolver_ConstraintMirror.cpp
index fde4b811e..8ed85ff49 100644
--- a/src/SketchSolver/SketchSolver_ConstraintMirror.cpp
+++ b/src/SketchSolver/SketchSolver_ConstraintMirror.cpp
@@ -207,6 +207,7 @@ void SketchSolver_ConstraintMirror::update(ConstraintPtr theConstraint)
     if (aMirroredRefList->size() != myNumberOfObjects) {
       remove(myBaseConstraint);
       process();
+      return;
     }
   }
   SketchSolver_Constraint::update();
diff --git a/src/SketchSolver/SketchSolver_ConstraintRigid.cpp b/src/SketchSolver/SketchSolver_ConstraintRigid.cpp
index 956f4b727..7f12c7d32 100644
--- a/src/SketchSolver/SketchSolver_ConstraintRigid.cpp
+++ b/src/SketchSolver/SketchSolver_ConstraintRigid.cpp
@@ -5,6 +5,7 @@
 #include <SketchPlugin_Arc.h>
 #include <SketchPlugin_Circle.h>
 #include <SketchPlugin_ConstraintRigid.h>
+#include <SketchPlugin_Line.h>
 
 #include <GeomAPI_Pnt2d.h>
 #include <GeomAPI_XY.h>
@@ -31,63 +32,20 @@ void SketchSolver_ConstraintRigid::process()
   double aValue;
   std::vector<Slvs_hEntity> anEntities;
   getAttributes(aValue, anEntities);
-  if (!myErrorMsg.empty())
+  if (!myErrorMsg.empty() || myFeatureMap.empty())
     return;
 
-  Slvs_Constraint aConstraint;
-  std::vector<Slvs_hConstraint>::iterator aConstrIter = mySlvsConstraints.begin();
-  bool isEmpty = aConstrIter == mySlvsConstraints.end();
-
-  // Check the fixed entity is an arc
-  if (isEmpty) {
-    if (!myFeatureMap.empty() && myFeatureMap.begin()->first->getKind() == SketchPlugin_Arc::ID()) {
-      Slvs_Entity anArc = myStorage->getEntity(myFeatureMap.begin()->second);
-      fixArc(anArc);
-      return;
-    }
+  if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Line::ID()) {
+    Slvs_Entity aLine = myStorage->getEntity(myFeatureMap.begin()->second);
+    fixLine(aLine);
   }
-
-  std::vector<Slvs_hEntity>::const_iterator anEntIter = anEntities.begin();
-  for (; anEntIter != anEntities.end(); anEntIter++) {
-    if (*anEntIter == SLVS_E_UNKNOWN)
-      continue;
-    Slvs_hConstraint aConstrID = myStorage->isPointFixed(*anEntIter);
-    bool isForceUpdate = (aConstrID != SLVS_E_UNKNOWN && !myBaseConstraint &&
-                          myStorage->isTemporary(aConstrID));
-    if (isEmpty && !isForceUpdate) { // create new constraint
-      if (aConstrID != SLVS_E_UNKNOWN)
-        continue; // the coincident point is already fixed
-      aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
-          aValue, *anEntIter, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
-      aConstraint.h = myStorage->addConstraint(aConstraint);
-      mySlvsConstraints.push_back(aConstraint.h);
-      if (!myBaseConstraint)
-        myStorage->addConstraintWhereDragged(aConstraint.h);
-    } else { // update already existent constraint
-      if (aConstrID == SLVS_E_UNKNOWN || myBaseConstraint)
-        aConstrID = *aConstrIter;
-      aConstraint = myStorage->getConstraint(aConstrID);
-      aConstraint.ptA = *anEntIter;
-      myStorage->addConstraint(aConstraint);
-      if (!myBaseConstraint)
-        myStorage->addConstraintWhereDragged(aConstraint.h);
-      if (!isEmpty) {
-        aConstrIter++;
-        isEmpty = aConstrIter == mySlvsConstraints.end();
-      }
-    }
+  else if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Arc::ID()) {
+    Slvs_Entity anArc = myStorage->getEntity(myFeatureMap.begin()->second);
+    fixArc(anArc);
   }
-
-  if (!myFeatureMap.empty() && myFeatureMap.begin()->first->getKind() == SketchPlugin_Circle::ID()) {
-    // Fix radius of a circle
-    AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
-        myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
-    aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER, myGroup->getWorkplaneId(),
-      aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
-    aConstraint.h = myStorage->addConstraint(aConstraint);
-    mySlvsConstraints.push_back(aConstraint.h);
-    if (!myBaseConstraint)
-      myStorage->addConstraintWhereDragged(aConstraint.h);
+  else if (myFeatureMap.begin()->first->getKind() == SketchPlugin_Circle::ID()) {
+    Slvs_Entity aCirc = myStorage->getEntity(myFeatureMap.begin()->second);
+    fixArc(aCirc);
   }
 }
 
@@ -184,14 +142,131 @@ bool SketchSolver_ConstraintRigid::remove(ConstraintPtr theConstraint)
   return true;
 }
 
+void SketchSolver_ConstraintRigid::fixPoint(const Slvs_hEntity& thePointID)
+{
+  if (thePointID == SLVS_E_UNKNOWN)
+    return;
+
+  Slvs_Constraint aConstraint;
+  Slvs_hConstraint aConstrID = myStorage->isPointFixed(thePointID);
+  bool isForceUpdate = (aConstrID != SLVS_E_UNKNOWN && !myBaseConstraint &&
+                        myStorage->isTemporary(aConstrID));
+  if (!isForceUpdate) { // create new constraint
+    if (aConstrID != SLVS_E_UNKNOWN)
+      return; // the coincident point is already fixed
+    aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
+        0.0, thePointID, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
+    aConstraint.h = myStorage->addConstraint(aConstraint);
+    mySlvsConstraints.push_back(aConstraint.h);
+    if (!myBaseConstraint)
+      myStorage->addConstraintWhereDragged(aConstraint.h);
+  } else { // update already existent constraint
+    if (aConstrID == SLVS_E_UNKNOWN || myBaseConstraint)
+      return;
+    aConstraint = myStorage->getConstraint(aConstrID);
+    aConstraint.ptA = thePointID;
+    myStorage->addConstraint(aConstraint);
+    if (!myBaseConstraint)
+      myStorage->addConstraintWhereDragged(aConstraint.h);
+  }
+}
+
+void SketchSolver_ConstraintRigid::fixLine(const Slvs_Entity& theLine)
+{
+  Slvs_Constraint anEqual;
+  if (isUsedInEqual(theLine, anEqual)) {
+    // Check another entity of Equal is already fixed
+    Slvs_hEntity anOtherEntID = anEqual.entityA == theLine.h ? anEqual.entityB : anEqual.entityA;
+    Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
+    if (isFixed(anOtherEntity)) {
+      // Fix start point of the line (if end point is not fixed yet) ...
+      Slvs_hConstraint anEndFixedID = myStorage->isPointFixed(theLine.point[1]);
+      if (anEndFixedID == SLVS_E_UNKNOWN)
+        fixPoint(theLine.point[0]);
+      // ...  and create fixed point lying on this line
+      Slvs_hEntity aPointToCopy = anEndFixedID == SLVS_E_UNKNOWN ? theLine.point[1] : theLine.point[0];
+      // Firstly, search already fixed point on line
+      bool isPonLineFixed = false;
+      Slvs_hEntity aFixedPoint;
+      std::list<Slvs_Constraint> aPonLineList = myStorage->getConstraintsByType(SLVS_C_PT_ON_LINE);
+      std::list<Slvs_Constraint>::const_iterator aPLIter = aPonLineList.begin();
+      for (; aPLIter != aPonLineList.end() && !isPonLineFixed; aPLIter++)
+        if (aPLIter->entityA == theLine.h) {
+          isPonLineFixed = (myStorage->isPointFixed(aPLIter->ptA) != SLVS_E_UNKNOWN);
+          aFixedPoint = aPLIter->ptA;
+        }
+
+      if (isPonLineFixed) { // update existent constraint
+        myStorage->copyEntity(aPointToCopy, aFixedPoint);
+      } else { // create new constraint
+        Slvs_hEntity aCopied = myStorage->copyEntity(aPointToCopy);
+        Slvs_Constraint aPonLine = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_PT_ON_LINE,
+            myGroup->getWorkplaneId(), 0.0, aCopied, SLVS_E_UNKNOWN, theLine.h, SLVS_E_UNKNOWN);
+        aPonLine.h = myStorage->addConstraint(aPonLine);
+        mySlvsConstraints.push_back(aPonLine.h);
+        fixPoint(aCopied);
+      }
+      return;
+    }
+  }
+
+  for (int i = 0; i < 2; i++)
+    fixPoint(theLine.point[i]);
+}
+
+void SketchSolver_ConstraintRigid::fixCircle(const Slvs_Entity& theCircle)
+{
+  bool isFixRadius = true;
+  // Verify the arc is under Equal constraint
+  Slvs_Constraint anEqual;
+  if (isUsedInEqual(theCircle, anEqual)) {
+    // Check another entity of Equal is already fixed
+    Slvs_hEntity anOtherEntID = anEqual.entityA == theCircle.h ? anEqual.entityB : anEqual.entityA;
+    Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
+    if (isFixed(anOtherEntity))
+      isFixRadius = false;
+  }
+
+  fixPoint(theCircle.point[0]);
+
+  if (isFixRadius) {
+    // Fix radius of a circle
+    AttributeDoublePtr aRadiusAttr = std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(
+        myFeatureMap.begin()->first->attribute(SketchPlugin_Circle::RADIUS_ID()));
+    Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
+        myGroup->getWorkplaneId(), aRadiusAttr->value() * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
+        myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
+    aFixedR.h = myStorage->addConstraint(aFixedR);
+    mySlvsConstraints.push_back(aFixedR.h);
+  }
+}
 
 void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
 {
-  Slvs_Constraint aConstraint;
+  bool isFixRadius = true;
+  std::list<Slvs_hEntity> aPointsToFix;
+  aPointsToFix.push_back(theArc.point[1]);
+  aPointsToFix.push_back(theArc.point[2]);
+
+  // Verify the arc is under Equal constraint
+  Slvs_Constraint anEqual;
+  if (isUsedInEqual(theArc, anEqual)) {
+    // Check another entity of Equal is already fixed
+    Slvs_hEntity anOtherEntID = anEqual.entityA == theArc.h ? anEqual.entityB : anEqual.entityA;
+    Slvs_Entity anOtherEntity = myStorage->getEntity(anOtherEntID);
+    if (isFixed(anOtherEntity)) {
+      isFixRadius = false;
+      if (anOtherEntity.type == SLVS_E_LINE_SEGMENT) {
+        aPointsToFix.pop_back();
+        aPointsToFix.push_back(theArc.point[0]);
+      }
+    }
+  }
+
   Slvs_hConstraint aConstrID = myStorage->isPointFixed(theArc.point[0]);
-  int aPointsToFix = 2; // number of fixed points for the arc
+  int aNbPointsToFix = 2; // number of fixed points for the arc
   if (aConstrID != SLVS_E_UNKNOWN)
-    aPointsToFix--;
+    aNbPointsToFix--;
 
   // Radius of the arc
   FeaturePtr aFeature = myFeatureMap.begin()->first;
@@ -218,32 +293,69 @@ void SketchSolver_ConstraintRigid::fixArc(const Slvs_Entity& theArc)
     myStorage->updateParameter(aParam);
   }
 
-  for (int i = 1; aPointsToFix > 0; i++, aPointsToFix--) {
-    aConstrID = myStorage->isPointFixed(theArc.point[i]);
-    if (aConstrID != SLVS_E_UNKNOWN)
-      continue; // the coincident point is already fixed
-    aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), getType(), myGroup->getWorkplaneId(),
-        0.0, theArc.point[i], SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN);
-    aConstraint.h = myStorage->addConstraint(aConstraint);
-    mySlvsConstraints.push_back(aConstraint.h);
-    if (!myBaseConstraint)
-      myStorage->addConstraintWhereDragged(aConstraint.h);
+  std::list<Slvs_hEntity>::iterator aPtIt = aPointsToFix.begin();
+  for (; aNbPointsToFix > 0; aPtIt++, aNbPointsToFix--)
+    fixPoint(*aPtIt);
+
+  if (isFixRadius) {
+    // Fix radius of the arc
+    bool isExists = false;
+    std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
+    std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
+    for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
+      if (anIt->entityA == myFeatureMap.begin()->second)
+        isExists = true;
+    if (!isExists) {
+      Slvs_Constraint aFixedR = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER,
+          myGroup->getWorkplaneId(), aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN,
+          myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
+      aFixedR.h = myStorage->addConstraint(aFixedR);
+      mySlvsConstraints.push_back(aFixedR.h);
+      if (!myBaseConstraint)
+        myStorage->addConstraintWhereDragged(aFixedR.h);
+    }
   }
+}
 
-  // Fix radius of the arc
-  bool isExists = false;
-  std::list<Slvs_Constraint> aDiamConstraints = myStorage->getConstraintsByType(SLVS_C_DIAMETER);
-  std::list<Slvs_Constraint>::iterator anIt = aDiamConstraints.begin();
-  for (; anIt != aDiamConstraints.end() && !isExists; anIt++)
-    if (anIt->entityA == myFeatureMap.begin()->second)
-      isExists = true;
-  if (!isExists) {
-    aConstraint = Slvs_MakeConstraint(SLVS_E_UNKNOWN, myGroup->getId(), SLVS_C_DIAMETER, myGroup->getWorkplaneId(),
-      aRadius * 2.0, SLVS_E_UNKNOWN, SLVS_E_UNKNOWN, myFeatureMap.begin()->second, SLVS_E_UNKNOWN);
-    aConstraint.h = myStorage->addConstraint(aConstraint);
-    mySlvsConstraints.push_back(aConstraint.h);
-    if (!myBaseConstraint)
-      myStorage->addConstraintWhereDragged(aConstraint.h);
+bool SketchSolver_ConstraintRigid::isUsedInEqual(
+    const Slvs_Entity& theEntity, Slvs_Constraint& theEqual) const
+{
+  // Check the entity is used in Equal constraint
+  std::list<Slvs_Constraint> anEqualConstr = myStorage->getConstraintsByType(SLVS_C_EQUAL_LENGTH_LINES);
+  std::list<Slvs_Constraint> anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_LINE_ARC_LEN);
+  anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
+  anAddList = myStorage->getConstraintsByType(SLVS_C_EQUAL_RADIUS);
+  anEqualConstr.insert(anEqualConstr.end(), anAddList.begin(), anAddList.end());
+
+  std::list<Slvs_Constraint>::const_iterator anEqIter = anEqualConstr.begin();
+  for (; anEqIter != anEqualConstr.end(); anEqIter++)
+    if (anEqIter->entityA == theEntity.h || anEqIter->entityB == theEntity.h) {
+      theEqual = *anEqIter;
+      return true;
+    }
+  return false;
+}
+
+bool SketchSolver_ConstraintRigid::isFixed(const Slvs_Entity& theEntity) const
+{
+  if (theEntity.type == SLVS_E_POINT_IN_2D)
+    return myStorage->isPointFixed(theEntity.h) != SLVS_E_UNKNOWN;
+
+  // Check all the points of entity are fixed
+  int aNbFixed = 0;
+  for (int i = 0; i < 4; i++) {
+    if (theEntity.point[i] != SLVS_E_UNKNOWN &&
+        myStorage->isPointFixed(theEntity.point[i]) != SLVS_E_UNKNOWN)
+      aNbFixed++;
+  }
+
+  switch (theEntity.type) {
+  case SLVS_E_LINE_SEGMENT:
+  case SLVS_E_ARC_OF_CIRCLE:
+    if (aNbFixed == 2) return true;
+  case SLVS_E_CIRCLE:
+    if (aNbFixed == 1) return true;
   }
+  return false;
 }
 
diff --git a/src/SketchSolver/SketchSolver_ConstraintRigid.h b/src/SketchSolver/SketchSolver_ConstraintRigid.h
index dd1487782..93de58362 100644
--- a/src/SketchSolver/SketchSolver_ConstraintRigid.h
+++ b/src/SketchSolver/SketchSolver_ConstraintRigid.h
@@ -49,11 +49,27 @@ protected:
   virtual void adjustConstraint();
 
 private:
+  /// \brief Fixing line position (start and end points)
+  void fixLine(const Slvs_Entity& theLine);
+  /// \brief Fixing circle (center and radius)
+  void fixCircle(const Slvs_Entity& theCircle);
   /// \brief The arc is fixed differently to avoid SolveSpace problems (overconstraint)
   ///
   /// There will be fixed start and end points and the radius of the arc.
   void fixArc(const Slvs_Entity& theArc);
 
+  /// \brief Fix given point
+  void fixPoint(const Slvs_hEntity& thePointID);
+
+  /// \brief Verifies the entity is used in any equal constraint
+  /// \param[in]  theEntity entity to be found
+  /// \param[out] theEqual  constraint, which uses the entity
+  /// \return \c true, if the Equal constrait is found
+  bool isUsedInEqual(const Slvs_Entity& theEntity, Slvs_Constraint& theEqual) const;
+
+  /// \brief Verifies the entity is already fixed
+  bool isFixed(const Slvs_Entity& theEntity) const;
+
 protected:
   FeaturePtr myBaseFeature; ///< fixed feature (when it is set, myBaseConstraint should be NULL)
 };
diff --git a/src/SketchSolver/SketchSolver_Error.h b/src/SketchSolver/SketchSolver_Error.h
index 3623354b1..e0446b558 100644
--- a/src/SketchSolver/SketchSolver_Error.h
+++ b/src/SketchSolver/SketchSolver_Error.h
@@ -59,6 +59,12 @@ class SketchSolver_Error
     static const std::string MY_ERROR_VALUE("Mirror constraint has wrong attributes");
     return MY_ERROR_VALUE;
   }
+  /// Crash in SolveSpace
+  inline static const std::string& SOLVESPACE_CRASH()
+  {
+    static const std::string MY_ERROR_VALUE("Caution: SolveSpace crash! Constraints are wrong");
+    return MY_ERROR_VALUE;
+  }
 };
 
 #endif
diff --git a/src/SketchSolver/SketchSolver_FeatureStorage.cpp b/src/SketchSolver/SketchSolver_FeatureStorage.cpp
index d647d075b..e274244fb 100644
--- a/src/SketchSolver/SketchSolver_FeatureStorage.cpp
+++ b/src/SketchSolver/SketchSolver_FeatureStorage.cpp
@@ -248,6 +248,8 @@ bool SketchSolver_FeatureStorage::isInteract(FeaturePtr theFeature) const
 {
   if (myFeatures.find(theFeature) != myFeatures.end())
     return true;
+  if (!theFeature->data() || !theFeature->data()->isValid())
+    return false;
 
   std::list<AttributePtr> anAttributes = theFeature->data()->attributes(std::string());
   std::list<AttributePtr>::iterator anIter = anAttributes.begin();
diff --git a/src/SketchSolver/SketchSolver_Group.cpp b/src/SketchSolver/SketchSolver_Group.cpp
index 7fdc32a4c..5a6e9802a 100644
--- a/src/SketchSolver/SketchSolver_Group.cpp
+++ b/src/SketchSolver/SketchSolver_Group.cpp
@@ -384,7 +384,13 @@ bool SketchSolver_Group::resolveConstraints()
     myConstrSolver.setGroupID(myID);
     myStorage->initializeSolver(myConstrSolver);
 
-    int aResult = myConstrSolver.solve();
+    int aResult = SLVS_RESULT_OKAY;
+    try {
+      aResult = myConstrSolver.solve();
+    } catch (...) {
+      Events_Error::send(SketchSolver_Error::SOLVESPACE_CRASH(), this);
+      return false;
+    }
     if (aResult == SLVS_RESULT_OKAY) {  // solution succeeded, store results into correspondent attributes
       myFeatureStorage->blockEvents(true);
       ConstraintConstraintMap::iterator aConstrIter = myConstraints.begin();
diff --git a/src/SketchSolver/SketchSolver_Storage.cpp b/src/SketchSolver/SketchSolver_Storage.cpp
index 40d1d281f..f13ad6b6f 100644
--- a/src/SketchSolver/SketchSolver_Storage.cpp
+++ b/src/SketchSolver/SketchSolver_Storage.cpp
@@ -192,6 +192,60 @@ const Slvs_Entity& SketchSolver_Storage::getEntity(const Slvs_hEntity& theEntity
   return aDummy;
 }
 
+Slvs_hEntity SketchSolver_Storage::copyEntity(const Slvs_hEntity& theCopied)
+{
+  int aPos = Search(theCopied, myEntities);
+  if (aPos < 0 || aPos >= (int)myEntities.size())
+    return SLVS_E_UNKNOWN;
+
+  Slvs_Entity aCopy = myEntities[aPos];
+  aCopy.h = SLVS_E_UNKNOWN;
+  int i = 0;
+  while (aCopy.point[i] != SLVS_E_UNKNOWN) {
+    aCopy.point[i] = copyEntity(aCopy.point[i]);
+    i++;
+  }
+  if (aCopy.param[0] != SLVS_E_UNKNOWN) {
+    aPos = Search(aCopy.param[0], myParameters);
+    i = 0;
+    while (aCopy.param[i] != SLVS_E_UNKNOWN) {
+      Slvs_Param aNewParam = myParameters[aPos];
+      aNewParam.h = SLVS_E_UNKNOWN;
+      aCopy.param[i] = addParameter(aNewParam);
+      i++;
+      aPos++;
+    }
+  }
+  return addEntity(aCopy);
+}
+
+void SketchSolver_Storage::copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo)
+{
+  int aPosFrom = Search(theFrom, myEntities);
+  int aPosTo = Search(theTo, myEntities);
+  if (aPosFrom < 0 || aPosFrom >= (int)myEntities.size() || 
+      aPosTo < 0 || aPosTo >= (int)myEntities.size())
+    return;
+
+  Slvs_Entity aEntFrom = myEntities[aPosFrom];
+  Slvs_Entity aEntTo = myEntities[aPosTo];
+  int i = 0;
+  while (aEntFrom.point[i] != SLVS_E_UNKNOWN) {
+    copyEntity(aEntFrom.point[i], aEntTo.point[i]);
+    i++;
+  }
+  if (aEntFrom.param[0] != SLVS_E_UNKNOWN) {
+    aPosFrom = Search(aEntFrom.param[0], myParameters);
+    aPosTo = Search(aEntTo.param[0], myParameters);
+    i = 0;
+    while (aEntFrom.param[i] != SLVS_E_UNKNOWN) {
+      myParameters[aPosTo++].val = myParameters[aPosFrom++].val;
+      i++;
+    }
+  }
+}
+
+
 Slvs_hConstraint SketchSolver_Storage::isPointFixed(const Slvs_hEntity& thePointID) const
 {
   // Search the set of coincident points
diff --git a/src/SketchSolver/SketchSolver_Storage.h b/src/SketchSolver/SketchSolver_Storage.h
index a95d83c72..613e0ec29 100644
--- a/src/SketchSolver/SketchSolver_Storage.h
+++ b/src/SketchSolver/SketchSolver_Storage.h
@@ -60,6 +60,10 @@ public:
   bool removeEntity(const Slvs_hEntity& theEntityID);
   /// \brief Returns the entity by its ID
   const Slvs_Entity& getEntity(const Slvs_hEntity& theEntityID) const;
+  /// \brief Makes a full copy of the given entity
+  Slvs_hEntity copyEntity(const Slvs_hEntity& theCopied);
+  /// \brief Copy one entity to another
+  void copyEntity(const Slvs_hEntity& theFrom, const Slvs_hEntity& theTo);
 
   /// \brief Verifies the current point or another coincident one is fixed
   /// \return the ID of the Fixed constraint or SLVS_E_UNKNOWN