]> SALOME platform Git repositories - modules/shaper.git/blobdiff - src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_Tools.cpp
Salome HOME
Task 2.12. New entities: ellipses and arcs of ellipses (issue #3003)
[modules/shaper.git] / src / SketchSolver / PlaneGCSSolver / PlaneGCSSolver_Tools.cpp
index 63813ba7555663d5512928142132a8b33008dead..35cbb4d722daca0db3431ee468d7bd03919e922f 100644 (file)
@@ -1,8 +1,21 @@
-// Copyright (C) 2014-20xx CEA/DEN, EDF R&D
-
-// File:    SketchSolver_Builder.cpp
-// Created: 25 Mar 2015
-// Author:  Artem ZHIDKOV
+// Copyright (C) 2014-2019  CEA/DEN, EDF R&D
+//
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+//
+// See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
+//
 
 #include <PlaneGCSSolver_Tools.h>
 #include <PlaneGCSSolver_EdgeWrapper.h>
@@ -13,6 +26,7 @@
 #include <SketchSolver_Constraint.h>
 #include <SketchSolver_ConstraintAngle.h>
 #include <SketchSolver_ConstraintCoincidence.h>
+#include <SketchPlugin_ConstraintCoincidenceInternal.h>
 #include <SketchSolver_ConstraintCollinear.h>
 #include <SketchSolver_ConstraintDistance.h>
 #include <SketchSolver_ConstraintEqual.h>
@@ -28,6 +42,8 @@
 #include <SketchPlugin_ConstraintCoincidence.h>
 #include <SketchPlugin_ConstraintCollinear.h>
 #include <SketchPlugin_ConstraintDistance.h>
+#include <SketchPlugin_ConstraintDistanceHorizontal.h>
+#include <SketchPlugin_ConstraintDistanceVertical.h>
 #include <SketchPlugin_ConstraintEqual.h>
 #include <SketchPlugin_ConstraintLength.h>
 #include <SketchPlugin_ConstraintMiddle.h>
 #include <SketchPlugin_MultiRotation.h>
 #include <SketchPlugin_MultiTranslation.h>
 
+#include <GeomAPI_Circ2d.h>
+#include <GeomAPI_Dir2d.h>
+#include <GeomAPI_Ellipse2d.h>
+#include <GeomAPI_Lin2d.h>
+#include <GeomAPI_Pnt2d.h>
+
 #include <cmath>
 
 
@@ -62,6 +84,11 @@ static ConstraintWrapperPtr
   createConstraintDistancePointLine(std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theValue,
                                     std::shared_ptr<PlaneGCSSolver_PointWrapper>  thePoint,
                                     std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity);
+static ConstraintWrapperPtr
+  createConstraintHVDistance(const SketchSolver_ConstraintType& theType,
+                             std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theValue,
+                             std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint1,
+                             std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint2);
 static ConstraintWrapperPtr
   createConstraintRadius(std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theValue,
                          std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity);
@@ -86,7 +113,18 @@ static ConstraintWrapperPtr
                         std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theIntermed);
 static ConstraintWrapperPtr
   createConstraintMiddlePoint(std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint,
-                              std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity);
+                              std::shared_ptr<PlaneGCSSolver_EdgeWrapper>  theEntity,
+                              std::shared_ptr<PlaneGCSSolver_PointWrapper> theAuxParameters);
+
+static GCS::SET_pD scalarParameters(const ScalarWrapperPtr& theScalar);
+static GCS::SET_pD pointParameters(const PointWrapperPtr& thePoint);
+static GCS::SET_pD lineParameters(const EdgeWrapperPtr& theLine);
+static GCS::SET_pD circleParameters(const EdgeWrapperPtr& theCircle);
+static GCS::SET_pD arcParameters(const EdgeWrapperPtr& theArc);
+static GCS::SET_pD ellipseParameters(const EdgeWrapperPtr& theEllipse);
+static GCS::SET_pD ellipticArcParameters(const EdgeWrapperPtr& theEllipticArc);
+
+static double distance(const GCS::Point& thePnt1, const GCS::Point& thePnt2);
 
 
 
@@ -94,11 +132,14 @@ static ConstraintWrapperPtr
 
 SolverConstraintPtr PlaneGCSSolver_Tools::createConstraint(ConstraintPtr theConstraint)
 {
-  if (theConstraint->getKind() == SketchPlugin_ConstraintCoincidence::ID()) {
+  if (theConstraint->getKind() == SketchPlugin_ConstraintCoincidence::ID() ||
+      theConstraint->getKind() == SketchPlugin_ConstraintCoincidenceInternal::ID()) {
     return SolverConstraintPtr(new SketchSolver_ConstraintCoincidence(theConstraint));
   } else if (theConstraint->getKind() == SketchPlugin_ConstraintCollinear::ID()) {
     return SolverConstraintPtr(new SketchSolver_ConstraintCollinear(theConstraint));
-  } else if (theConstraint->getKind() == SketchPlugin_ConstraintDistance::ID()) {
+  } else if (theConstraint->getKind() == SketchPlugin_ConstraintDistance::ID() ||
+             theConstraint->getKind() == SketchPlugin_ConstraintDistanceHorizontal::ID() ||
+             theConstraint->getKind() == SketchPlugin_ConstraintDistanceVertical::ID()) {
     return SolverConstraintPtr(new SketchSolver_ConstraintDistance(theConstraint));
   } else if (theConstraint->getKind() == SketchPlugin_ConstraintEqual::ID()) {
     return SolverConstraintPtr(new SketchSolver_ConstraintEqual(theConstraint));
@@ -123,11 +164,18 @@ SolverConstraintPtr PlaneGCSSolver_Tools::createConstraint(ConstraintPtr theCons
   return SolverConstraintPtr(new SketchSolver_Constraint(theConstraint));
 }
 
-std::shared_ptr<SketchSolver_ConstraintFixed> PlaneGCSSolver_Tools::createMovementConstraint(
+std::shared_ptr<SketchSolver_ConstraintMovement> PlaneGCSSolver_Tools::createMovementConstraint(
     FeaturePtr theMovedFeature)
 {
-  return std::shared_ptr<SketchSolver_ConstraintFixed>(
-      new SketchSolver_ConstraintFixed(theMovedFeature));
+  return std::shared_ptr<SketchSolver_ConstraintMovement>(
+      new SketchSolver_ConstraintMovement(theMovedFeature));
+}
+
+std::shared_ptr<SketchSolver_ConstraintMovement> PlaneGCSSolver_Tools::createMovementConstraint(
+    AttributePtr theMovedAttribute)
+{
+  return std::shared_ptr<SketchSolver_ConstraintMovement>(
+      new SketchSolver_ConstraintMovement(theMovedAttribute));
 }
 
 
@@ -151,12 +199,11 @@ ConstraintWrapperPtr PlaneGCSSolver_Tools::createConstraint(
   case CONSTRAINT_PT_PT_COINCIDENT:
     aResult = createConstraintCoincidence(aPoint1, aPoint2);
     break;
-  case CONSTRAINT_PT_ON_LINE:
-  case CONSTRAINT_PT_ON_CIRCLE:
+  case CONSTRAINT_PT_ON_CURVE:
     aResult = createConstraintPointOnEntity(theType, aPoint1, GCS_EDGE_WRAPPER(theEntity1));
     break;
   case CONSTRAINT_MIDDLE_POINT:
-    aResult = createConstraintMiddlePoint(aPoint1, GCS_EDGE_WRAPPER(theEntity1));
+    aResult = createConstraintMiddlePoint(aPoint1, GCS_EDGE_WRAPPER(theEntity1), aPoint2);
     break;
   case CONSTRAINT_PT_PT_DISTANCE:
     aResult = createConstraintDistancePointPoint(GCS_SCALAR_WRAPPER(theValue), aPoint1, aPoint2);
@@ -166,6 +213,10 @@ ConstraintWrapperPtr PlaneGCSSolver_Tools::createConstraint(
                                                 aPoint1,
                                                 GCS_EDGE_WRAPPER(theEntity1));
     break;
+  case CONSTRAINT_HORIZONTAL_DISTANCE:
+  case CONSTRAINT_VERTICAL_DISTANCE:
+    aResult = createConstraintHVDistance(theType, GCS_SCALAR_WRAPPER(theValue), aPoint1, aPoint2);
+    break;
   case CONSTRAINT_RADIUS:
     aResult = createConstraintRadius(GCS_SCALAR_WRAPPER(theValue),
                                      GCS_EDGE_WRAPPER(theEntity1));
@@ -190,6 +241,7 @@ ConstraintWrapperPtr PlaneGCSSolver_Tools::createConstraint(
                                             GCS_EDGE_WRAPPER(theEntity2));
     break;
   case CONSTRAINT_EQUAL_LINES:
+  case CONSTRAINT_EQUAL_ELLIPSES:
     anIntermediate = GCS_SCALAR_WRAPPER(theValue); // parameter is used to store length of lines
   case CONSTRAINT_EQUAL_LINE_ARC:
   case CONSTRAINT_EQUAL_RADIUS:
@@ -241,6 +293,104 @@ std::shared_ptr<GeomAPI_Lin2d> PlaneGCSSolver_Tools::line(FeaturePtr theFeature)
   return std::shared_ptr<GeomAPI_Lin2d>(new GeomAPI_Lin2d(aStart->pnt(), aEnd->pnt()));
 }
 
+std::shared_ptr<GeomAPI_Circ2d> PlaneGCSSolver_Tools::circle(EntityWrapperPtr theEntity)
+{
+  if (theEntity->type() != ENTITY_CIRCLE && theEntity->type() != ENTITY_ARC)
+    return std::shared_ptr<GeomAPI_Circ2d>();
+
+  std::shared_ptr<PlaneGCSSolver_EdgeWrapper> anEntity =
+    std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(theEntity);
+  std::shared_ptr<GCS::Circle> aCirc = std::dynamic_pointer_cast<GCS::Circle>(anEntity->entity());
+  return std::shared_ptr<GeomAPI_Circ2d>(
+    new GeomAPI_Circ2d(*(aCirc->center.x), *(aCirc->center.y), *(aCirc->rad)));
+}
+
+std::shared_ptr<GeomAPI_Ellipse2d> PlaneGCSSolver_Tools::ellipse(EntityWrapperPtr theEntity)
+{
+  if (theEntity->type() != ENTITY_ELLIPSE && theEntity->type() != ENTITY_ELLIPTIC_ARC)
+    return std::shared_ptr<GeomAPI_Ellipse2d>();
+
+  std::shared_ptr<PlaneGCSSolver_EdgeWrapper> anEntity =
+    std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(theEntity);
+  std::shared_ptr<GCS::Ellipse> anEllipse =
+      std::dynamic_pointer_cast<GCS::Ellipse>(anEntity->entity());
+
+  std::shared_ptr<GeomAPI_Pnt2d> aCenter(
+      new GeomAPI_Pnt2d(*(anEllipse->center.x), *(anEllipse->center.y)));
+  std::shared_ptr<GeomAPI_Dir2d> anAxis(new GeomAPI_Dir2d(
+      *(anEllipse->focus1.x) - *(anEllipse->center.x),
+      *(anEllipse->focus1.y) - *(anEllipse->center.y)));
+
+  return std::shared_ptr<GeomAPI_Ellipse2d>(
+      new GeomAPI_Ellipse2d(aCenter, anAxis, anEllipse->getRadMaj(), *anEllipse->radmin));
+}
+
+void PlaneGCSSolver_Tools::recalculateArcParameters(EntityWrapperPtr theArc)
+{
+  std::shared_ptr<PlaneGCSSolver_EdgeWrapper> anEdge =
+      std::dynamic_pointer_cast<PlaneGCSSolver_EdgeWrapper>(theArc);
+  if (!anEdge)
+    return;
+
+  if (anEdge->type() == ENTITY_ARC) {
+    std::shared_ptr<GCS::Arc> anArc = std::dynamic_pointer_cast<GCS::Arc>(anEdge->entity());
+
+    GCS::Point aCenter = anArc->center;
+    GCS::Point aStartPnt = anArc->start;
+    GCS::Point aEndPnt = anArc->end;
+
+    *anArc->rad = distance(aCenter, aStartPnt);
+
+    static GeomDir2dPtr OX(new GeomAPI_Dir2d(1.0, 0.0));
+
+    GeomDir2dPtr aDir(new GeomAPI_Dir2d(*aStartPnt.x - *aCenter.x, *aStartPnt.y - *aCenter.y));
+    *anArc->startAngle = OX->angle(aDir);
+
+    aDir.reset(new GeomAPI_Dir2d(*aEndPnt.x - *aCenter.x, *aEndPnt.y - *aCenter.y));
+    *anArc->endAngle = OX->angle(aDir);
+  }
+  else if (anEdge->type() == ENTITY_ELLIPTIC_ARC) {
+    std::shared_ptr<GCS::ArcOfEllipse> aEllArc =
+        std::dynamic_pointer_cast<GCS::ArcOfEllipse>(anEdge->entity());
+
+    GeomPnt2dPtr aCenter(new GeomAPI_Pnt2d(*aEllArc->center.x, *aEllArc->center.y));
+    GeomPnt2dPtr aStartPnt(new GeomAPI_Pnt2d(*aEllArc->start.x, *aEllArc->start.y));
+    GeomPnt2dPtr aEndPnt(new GeomAPI_Pnt2d(*aEllArc->end.x, *aEllArc->end.y));
+
+    GeomDir2dPtr anAxis(new GeomAPI_Dir2d(*aEllArc->focus1.x - aCenter->x(),
+                                          *aEllArc->focus1.y - aCenter->y()));
+    GeomAPI_Ellipse2d anEllipse(aCenter, anAxis, aEllArc->getRadMaj(), *aEllArc->radmin);
+    anEllipse.parameter(aStartPnt, 1.e-4, *aEllArc->startAngle);
+    anEllipse.parameter(aEndPnt, 1.e-4, *aEllArc->endAngle);
+  }
+}
+
+
+
+GCS::SET_pD PlaneGCSSolver_Tools::parameters(const EntityWrapperPtr& theEntity)
+{
+  switch (theEntity->type()) {
+  case ENTITY_SCALAR:
+  case ENTITY_ANGLE:
+    return scalarParameters(GCS_SCALAR_WRAPPER(theEntity));
+  case ENTITY_POINT:
+    return pointParameters(GCS_POINT_WRAPPER(theEntity));
+  case ENTITY_LINE:
+    return lineParameters(GCS_EDGE_WRAPPER(theEntity));
+  case ENTITY_CIRCLE:
+    return circleParameters(GCS_EDGE_WRAPPER(theEntity));
+  case ENTITY_ARC:
+    return arcParameters(GCS_EDGE_WRAPPER(theEntity));
+  case ENTITY_ELLIPSE:
+    return ellipseParameters(GCS_EDGE_WRAPPER(theEntity));
+  case ENTITY_ELLIPTIC_ARC:
+    return ellipticArcParameters(GCS_EDGE_WRAPPER(theEntity));
+  default: break;
+  }
+  return GCS::SET_pD();
+}
+
+
 
 
 
@@ -285,6 +435,14 @@ ConstraintWrapperPtr createConstraintPointOnEntity(
         new GCS::ConstraintP2PDistance(*(thePoint->point()), aCirc->center, aCirc->rad));
     break;
     }
+  case ENTITY_ELLIPSE:
+  case ENTITY_ELLIPTIC_ARC: {
+    std::shared_ptr<GCS::Ellipse> anEllipse =
+        std::dynamic_pointer_cast<GCS::Ellipse>(theEntity->entity());
+    aNewConstr = GCSConstraintPtr(
+        new GCS::ConstraintPointOnEllipse(*(thePoint->point()), *anEllipse));
+    break;
+    }
   default:
     return ConstraintWrapperPtr();
   }
@@ -294,19 +452,38 @@ ConstraintWrapperPtr createConstraintPointOnEntity(
 
 ConstraintWrapperPtr createConstraintMiddlePoint(
     std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint,
-    std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity)
+    std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity,
+    std::shared_ptr<PlaneGCSSolver_PointWrapper> theAuxParameters)
 {
+  std::list<GCSConstraintPtr> aConstrList;
+
   GCSPointPtr aPoint = thePoint->point();
   std::shared_ptr<GCS::Line> aLine = std::dynamic_pointer_cast<GCS::Line>(theEntity->entity());
-  if (!aLine)
-    return ConstraintWrapperPtr();
-
-  std::list<GCSConstraintPtr> aConstrList;
-  aConstrList.push_back(
-      GCSConstraintPtr(new GCS::ConstraintPointOnPerpBisector(*aPoint, aLine->p1, aLine->p2)));
-  aConstrList.push_back(GCSConstraintPtr(new GCS::ConstraintPointOnLine(*aPoint, *aLine)));
+  if (aLine) {
+    aConstrList.push_back(GCSConstraintPtr(new GCS::ConstraintPointOnLine(*aPoint, *aLine)));
+    aConstrList.push_back(
+        GCSConstraintPtr(new GCS::ConstraintPointOnPerpBisector(*aPoint, aLine->p1, aLine->p2)));
+  }
+  else {
+    std::shared_ptr<GCS::Arc> anArc = std::dynamic_pointer_cast<GCS::Arc>(theEntity->entity());
+    if (anArc) {
+      double* u = theAuxParameters->point()->x;
+      double* diff = theAuxParameters->point()->y;
+      *u = (*anArc->startAngle + *anArc->endAngle) * 0.5;
+      *diff = (*anArc->endAngle - *anArc->startAngle) * 0.5;
+
+      aConstrList.push_back(GCSConstraintPtr(
+          new GCS::ConstraintCurveValue(*aPoint, aPoint->x, *anArc, u)));
+      aConstrList.push_back(GCSConstraintPtr(
+          new GCS::ConstraintCurveValue(*aPoint, aPoint->y, *anArc, u)));
+      aConstrList.push_back(GCSConstraintPtr(
+          new GCS::ConstraintDifference(anArc->startAngle, u, diff)));
+      aConstrList.push_back(GCSConstraintPtr(
+          new GCS::ConstraintDifference(u, anArc->endAngle, diff)));
+    }
+  }
 
-  return ConstraintWrapperPtr(
+  return aConstrList.empty() ? ConstraintWrapperPtr() : ConstraintWrapperPtr(
       new PlaneGCSSolver_ConstraintWrapper(aConstrList, CONSTRAINT_MIDDLE_POINT));
 }
 
@@ -338,6 +515,32 @@ ConstraintWrapperPtr createConstraintDistancePointLine(
   return aResult;
 }
 
+ConstraintWrapperPtr createConstraintHVDistance(
+    const SketchSolver_ConstraintType& theType,
+    std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theValue,
+    std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint1,
+    std::shared_ptr<PlaneGCSSolver_PointWrapper> thePoint2)
+{
+  GCSPointPtr aPoint1 = thePoint1->point();
+  GCSPointPtr aPoint2 = thePoint2->point();
+
+  double *aParam1, *aParam2;
+  if (theType == CONSTRAINT_HORIZONTAL_DISTANCE) {
+    aParam1 = aPoint1->x;
+    aParam2 = aPoint2->x;
+  } else if (theType == CONSTRAINT_VERTICAL_DISTANCE) {
+    aParam1 = aPoint1->y;
+    aParam2 = aPoint2->y;
+  }
+
+  GCSConstraintPtr aNewConstr(new GCS::ConstraintDifference(aParam1, aParam2, theValue->scalar()));
+
+  std::shared_ptr<PlaneGCSSolver_ConstraintWrapper> aResult(
+      new PlaneGCSSolver_ConstraintWrapper(aNewConstr, theType));
+  aResult->setValueParameter(theValue);
+  return aResult;
+}
+
 ConstraintWrapperPtr createConstraintRadius(
     std::shared_ptr<PlaneGCSSolver_ScalarWrapper> theValue,
     std::shared_ptr<PlaneGCSSolver_EdgeWrapper> theEntity)
@@ -410,7 +613,23 @@ ConstraintWrapperPtr createConstraintPerpendicular(
 {
   std::shared_ptr<GCS::Line> aLine1 = std::dynamic_pointer_cast<GCS::Line>(theEntity1->entity());
   std::shared_ptr<GCS::Line> aLine2 = std::dynamic_pointer_cast<GCS::Line>(theEntity2->entity());
-  GCSConstraintPtr aNewConstr(new GCS::ConstraintPerpendicular(*(aLine1), *(aLine2)));
+
+  std::shared_ptr<GCS::Circle> aCirc1 =
+      std::dynamic_pointer_cast<GCS::Circle>(theEntity1->entity());
+  std::shared_ptr<GCS::Circle> aCirc2 =
+      std::dynamic_pointer_cast<GCS::Circle>(theEntity2->entity());
+
+  GCSConstraintPtr aNewConstr;
+  if (aLine1 && aLine2)
+    aNewConstr.reset(new GCS::ConstraintPerpendicular(*(aLine1), *(aLine2)));
+  else {
+    if (aLine1 && aCirc2)
+      aCirc1 = aCirc2;
+    else if (aLine2 && aCirc1)
+      aLine1 = aLine2;
+
+    aNewConstr.reset(new GCS::ConstraintPointOnLine(aCirc1->center, *aLine1));
+  }
 
   return ConstraintWrapperPtr(
       new PlaneGCSSolver_ConstraintWrapper(aNewConstr, CONSTRAINT_PERPENDICULAR));
@@ -435,16 +654,28 @@ ConstraintWrapperPtr createConstraintEqual(
     aConstrList.push_back(GCSConstraintPtr(
         new GCS::ConstraintP2PDistance(aLine2->p1, aLine2->p2, theIntermed->scalar())));
     // update value of intermediate parameter
-    double x = *aLine1->p1.x - *aLine1->p2.x;
-    double y = *aLine1->p1.y - *aLine1->p2.y;
-    double aLen = sqrt(x*x + y*y);
-    theIntermed->setValue(aLen);
-  } else {
+    theIntermed->setValue(distance(aLine1->p1, aLine1->p2));
+  }
+  else if (theType == CONSTRAINT_EQUAL_ELLIPSES) {
+    std::shared_ptr<GCS::Ellipse> anEllipse1 =
+        std::dynamic_pointer_cast<GCS::Ellipse>(theEntity1->entity());
+    std::shared_ptr<GCS::Ellipse> anEllipse2 =
+        std::dynamic_pointer_cast<GCS::Ellipse>(theEntity2->entity());
+
+    aConstrList.push_back(GCSConstraintPtr(
+        new GCS::ConstraintEqual(anEllipse1->radmin, anEllipse2->radmin)));
+    aConstrList.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
+        anEllipse1->center, anEllipse1->focus1, theIntermed->scalar())));
+    aConstrList.push_back(GCSConstraintPtr(new GCS::ConstraintP2PDistance(
+        anEllipse2->center, anEllipse2->focus1, theIntermed->scalar())));
+    // update value of intermediate parameter
+    theIntermed->setValue(distance(anEllipse1->center, anEllipse1->focus1));
+  }
+  else {
     std::shared_ptr<GCS::Circle> aCirc1 =
         std::dynamic_pointer_cast<GCS::Circle>(theEntity1->entity());
     std::shared_ptr<GCS::Circle> aCirc2 =
         std::dynamic_pointer_cast<GCS::Circle>(theEntity2->entity());
-
     aConstrList.push_back(GCSConstraintPtr(new GCS::ConstraintEqual(aCirc1->rad, aCirc2->rad)));
   }
 
@@ -454,3 +685,86 @@ ConstraintWrapperPtr createConstraintEqual(
     aResult->setValueParameter(theIntermed);
   return aResult;
 }
+
+GCS::SET_pD scalarParameters(const ScalarWrapperPtr& theScalar)
+{
+  GCS::SET_pD aParams;
+  aParams.insert(theScalar->scalar());
+  return aParams;
+}
+
+GCS::SET_pD pointParameters(const PointWrapperPtr& thePoint)
+{
+  GCS::SET_pD aParams;
+  aParams.insert(thePoint->point()->x);
+  aParams.insert(thePoint->point()->y);
+  return aParams;
+}
+
+GCS::SET_pD lineParameters(const EdgeWrapperPtr& theLine)
+{
+  GCS::SET_pD aParams;
+  std::shared_ptr<GCS::Line> aLine = std::dynamic_pointer_cast<GCS::Line>(theLine->entity());
+  aParams.insert(aLine->p1.x);
+  aParams.insert(aLine->p1.y);
+  aParams.insert(aLine->p2.x);
+  aParams.insert(aLine->p2.y);
+  return aParams;
+}
+
+GCS::SET_pD circleParameters(const EdgeWrapperPtr& theCircle)
+{
+  GCS::SET_pD aParams;
+  std::shared_ptr<GCS::Circle> aCirc = std::dynamic_pointer_cast<GCS::Circle>(theCircle->entity());
+  aParams.insert(aCirc->center.x);
+  aParams.insert(aCirc->center.y);
+  aParams.insert(aCirc->rad);
+  return aParams;
+}
+
+GCS::SET_pD arcParameters(const EdgeWrapperPtr& theArc)
+{
+  GCS::SET_pD aParams = circleParameters(theArc);
+  std::shared_ptr<GCS::Arc> anArc = std::dynamic_pointer_cast<GCS::Arc>(theArc->entity());
+  aParams.insert(anArc->start.x);
+  aParams.insert(anArc->start.y);
+  aParams.insert(anArc->end.x);
+  aParams.insert(anArc->end.y);
+  aParams.insert(anArc->startAngle);
+  aParams.insert(anArc->endAngle);
+  return aParams;
+}
+
+GCS::SET_pD ellipseParameters(const EdgeWrapperPtr& theEllipse)
+{
+  GCS::SET_pD aParams;
+  std::shared_ptr<GCS::Ellipse> anEllipse =
+      std::dynamic_pointer_cast<GCS::Ellipse>(theEllipse->entity());
+  aParams.insert(anEllipse->center.x);
+  aParams.insert(anEllipse->center.y);
+  aParams.insert(anEllipse->focus1.x);
+  aParams.insert(anEllipse->focus1.y);
+  aParams.insert(anEllipse->radmin);
+  return aParams;
+}
+
+GCS::SET_pD ellipticArcParameters(const EdgeWrapperPtr& theEllipticArc)
+{
+  GCS::SET_pD aParams = ellipseParameters(theEllipticArc);
+  std::shared_ptr<GCS::ArcOfEllipse> anArc =
+      std::dynamic_pointer_cast<GCS::ArcOfEllipse>(theEllipticArc->entity());
+  aParams.insert(anArc->start.x);
+  aParams.insert(anArc->start.y);
+  aParams.insert(anArc->end.x);
+  aParams.insert(anArc->end.y);
+  aParams.insert(anArc->startAngle);
+  aParams.insert(anArc->endAngle);
+  return aParams;
+}
+
+double distance(const GCS::Point& thePnt1, const GCS::Point& thePnt2)
+{
+  double x = *thePnt1.x - *thePnt2.x;
+  double y = *thePnt1.y - *thePnt2.y;
+  return sqrt(x*x + y*y);
+}