diff --git a/src/Base/Exception.h b/src/Base/Exception.h index f17fa71cb..a4995b082 100644 --- a/src/Base/Exception.h +++ b/src/Base/Exception.h @@ -27,6 +27,7 @@ #define BASE_EXCEPTION_H #include +#include #include #include #include "FileInfo.h" diff --git a/src/Mod/Assembly/App/CMakeLists.txt b/src/Mod/Assembly/App/CMakeLists.txt index ca11c4ec1..2c44c9c2f 100644 --- a/src/Mod/Assembly/App/CMakeLists.txt +++ b/src/Mod/Assembly/App/CMakeLists.txt @@ -4,9 +4,14 @@ else(MSVC) add_definitions(-DHAVE_LIMITS_H -DHAVE_CONFIG_H) endif(MSVC) +if (CMAKE_BUILD_TYPE STREQUAL "Debug") + add_definitions( -DUSE_LOGGING ) +endif (CMAKE_BUILD_TYPE STREQUAL "Debug") + include_directories( ${CMAKE_SOURCE_DIR}/src ${CMAKE_BINARY_DIR}/src + ${CMAKE_SOURCE_DIR}/src/Mod/Assembly/App ${CMAKE_CURRENT_BINARY_DIR} ${Boost_INCLUDE_DIRS} ${OCC_INCLUDE_DIR} @@ -23,27 +28,20 @@ set(Assembly_LIBS ${OCC_LIBRARIES} Part FreeCADApp + boost_log + rt + ${Boost_SYSTEM_LIBRARY} + ${Boost_FILESYSTEM_LIBRARY} + ${Boost_THREAD_LIBRARY} ) generate_from_xml(ItemPy) generate_from_xml(ItemAssemblyPy) generate_from_xml(ItemPartPy) +generate_from_xml(ConstraintPy) +generate_from_xml(ConstraintAxisPy) generate_from_xml(ConstraintGroupPy) -SET(FreeGCS3D_SRCS - gcs3d/GCS.cpp - gcs3d/GCS.h - gcs3d/Util.h - gcs3d/Geo.h - gcs3d/Constraints.cpp - gcs3d/Constraints.h - gcs3d/SubSystem.cpp - gcs3d/SubSystem.h - gcs3d/qp_eq.cpp - gcs3d/qp_eq.h -) -SOURCE_GROUP("FreeGCS3D" FILES ${FreeGCS3D_SRCS}) - SET(Features_SRCS Item.cpp Item.h @@ -83,6 +81,10 @@ SET(Python_SRCS ItemAssemblyPyImp.cpp ItemPartPy.xml ItemPartPyImp.cpp + ConstraintPy.xml + ConstraintPyImp.cpp + ConstraintAxisPy.xml + ConstraintAxisPyImp.cpp ConstraintGroupPy.xml ConstraintGroupPyImp.cpp ) @@ -92,7 +94,6 @@ SET(Assembly_SRCS ${Features_SRCS} ${Python_SRCS} ${Module_SRCS} - ${FreeGCS3D_SRCS} ) diff --git a/src/Mod/Assembly/App/Constraint.cpp b/src/Mod/Assembly/App/Constraint.cpp index 01aa71c5a..62f1265cb 100644 --- a/src/Mod/Assembly/App/Constraint.cpp +++ b/src/Mod/Assembly/App/Constraint.cpp @@ -1,7 +1,8 @@ /*************************************************************************** * Copyright (c) 2012 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * - * This file is part of the FreeCAD CAx development system. * + * This file is part of the FreeCAD CAx development m_solvertem. * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Library General Public * @@ -25,9 +26,28 @@ #ifndef _PreComp_ #endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include #include "Constraint.h" +#include "Item.h" +#include "ItemPart.h" using namespace Assembly; @@ -57,4 +77,37 @@ App::DocumentObjectExecReturn *Constraint::execute(void) return App::DocumentObject::StdReturn; } +void Constraint::init(boost::shared_ptr< Solver > solver) { + + //check if we have Assembly::ItemPart's + if( First.getValue()->getTypeId() != ItemPart::getClassTypeId() || + Second.getValue()->getTypeId() != ItemPart::getClassTypeId() ) { + Base::Console().Message("Links are not ItemPart's, the constraint is invalid\n"); + return; + }; + + //see if the parts are already initialized for the solver + Assembly::ItemPart* part1 = static_cast(First.getValue()); + if(!part1->m_part) { + part1->m_part = solver->createPart(part1->Placement.getValue(), part1->Uid.getValueStr()); + part1->m_part->connectSignal(boost::bind(&ItemPart::setCalculatedPlacement, part1, _1)); + } + + Assembly::ItemPart* part2 = static_cast(Second.getValue()); + if(!part2->m_part) { + part2->m_part = solver->createPart(part2->Placement.getValue(), part2->Uid.getValueStr()); + part2->m_part->connectSignal(boost::bind(&ItemPart::setCalculatedPlacement, part2, _1)); + } + + //let's get the geometrys + m_first_geom = part1->getGeometry3D(First.getSubValues()[0].c_str()); + m_second_geom = part2->getGeometry3D(Second.getSubValues()[0].c_str()); + + if(!m_first_geom || !m_second_geom) { + Base::Console().Message("Unable to initialize geometry\n"); + return; + }; +} + + } \ No newline at end of file diff --git a/src/Mod/Assembly/App/Constraint.h b/src/Mod/Assembly/App/Constraint.h index fd3562ce5..00562fb51 100644 --- a/src/Mod/Assembly/App/Constraint.h +++ b/src/Mod/Assembly/App/Constraint.h @@ -1,5 +1,6 @@ /*************************************************************************** * Copyright (c) 2012 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * * This file is part of the FreeCAD CAx development system. * * * @@ -27,6 +28,10 @@ #include #include +#include + +#include "Solver.h" + namespace Assembly { @@ -34,6 +39,10 @@ namespace Assembly class AssemblyExport Constraint : public App::DocumentObject { PROPERTY_HEADER(Assembly::Constraint); + +protected: + boost::shared_ptr m_constraint; + boost::shared_ptr m_first_geom, m_second_geom; public: Constraint(); @@ -47,10 +56,13 @@ public: App::DocumentObjectExecReturn *execute(void); short mustExecute() const; /// returns the type name of the view provider - //const char* getViewProviderName(void) const { - // return "AssemblyGui::ViewProviderConstraint"; - //} - //@} + const char* getViewProviderName(void) const { + return "Gui::ViewProviderDocumentObject"; + } + + /** @brief initialize the constraint in the assembly solver + */ + virtual void init(boost::shared_ptr solver); }; } //namespace Assembly diff --git a/src/Mod/Assembly/App/ConstraintAxis.cpp b/src/Mod/Assembly/App/ConstraintAxis.cpp index b2ed9ab40..1361e88c0 100644 --- a/src/Mod/Assembly/App/ConstraintAxis.cpp +++ b/src/Mod/Assembly/App/ConstraintAxis.cpp @@ -1,5 +1,6 @@ /*************************************************************************** - * Copyright (c) 2012 Juergen Riegel * + * Copyright (c) 2012 Juergen Riegel + * 2013 Stefan Tröger * * * This file is part of the FreeCAD CAx development system. * * * @@ -26,9 +27,12 @@ #endif #include +#include #include "ConstraintAxis.h" +#include "ConstraintAxisPy.h" +#include "ItemPart.h" using namespace Assembly; @@ -42,6 +46,15 @@ ConstraintAxis::ConstraintAxis() } +PyObject *ConstraintAxis::getPyObject(void) +{ + if (PythonObject.is(Py::_None())){ + // ref counter is set to 1 + PythonObject = Py::Object(new ConstraintAxisPy(this),true); + } + return Py::new_reference_to(PythonObject); +} + short ConstraintAxis::mustExecute() const { //if (Sketch.isTouched() || @@ -52,8 +65,20 @@ short ConstraintAxis::mustExecute() const App::DocumentObjectExecReturn *ConstraintAxis::execute(void) { - + Base::Console().Message("Recalculate axis constraint\n"); return App::DocumentObject::StdReturn; } +void ConstraintAxis::init(boost::shared_ptr< Solver > solver) +{ + Base::Console().Message("Init constraint axis\n"); + + //init the parts and geometries + Constraint::init(solver); + + //init the constraint + m_constraint = solver->createConstraint3D(getNameInDocument(), m_first_geom, m_second_geom, dcm::distance = 0); +} + + } \ No newline at end of file diff --git a/src/Mod/Assembly/App/ConstraintAxis.h b/src/Mod/Assembly/App/ConstraintAxis.h index c776ce892..f0e8bc108 100644 --- a/src/Mod/Assembly/App/ConstraintAxis.h +++ b/src/Mod/Assembly/App/ConstraintAxis.h @@ -38,6 +38,7 @@ class AssemblyExport ConstraintAxis : public Assembly::Constraint public: ConstraintAxis(); + PyObject *getPyObject(void); /** @name methods override feature */ //@{ @@ -49,6 +50,8 @@ public: // return "PartDesignGui::ViewProviderConstraintAxis"; //} //@} + + virtual void init(boost::shared_ptr solver); }; } //namespace Assembly diff --git a/src/Mod/Assembly/App/ConstraintGroup.cpp b/src/Mod/Assembly/App/ConstraintGroup.cpp index b27746f7e..71fe93f0a 100644 --- a/src/Mod/Assembly/App/ConstraintGroup.cpp +++ b/src/Mod/Assembly/App/ConstraintGroup.cpp @@ -1,5 +1,6 @@ /*************************************************************************** * Copyright (c) 2010 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * * This file is part of the FreeCAD CAx development system. * * * @@ -26,8 +27,12 @@ #endif #include +#include +#include "ConstraintGroupPy.h" #include "ConstraintGroup.h" +#include "ItemPart.h" +#include "ItemAssembly.h" using namespace Assembly; @@ -42,6 +47,55 @@ ConstraintGroup::ConstraintGroup() ADD_PROPERTY(Constraints,(0)); } +PyObject *ConstraintGroup::getPyObject(void) +{ + if (PythonObject.is(Py::_None())){ + // ref counter is set to 1 + PythonObject = Py::Object(new ConstraintGroupPy(this),true); + } + return Py::new_reference_to(PythonObject); +} + +void ConstraintGroup::addConstraint(Constraint* c) +{ + //add the constraint to our list + const std::vector< App::DocumentObject * > &vals = this->Constraints.getValues(); + std::vector< App::DocumentObject * > newVals(vals); + newVals.push_back(c); + this->Constraints.setValues(newVals); + + //let's retrieve the solver if not already done + ItemAssembly* assembly = NULL; + if(!m_solver) { + + typedef std::vector::iterator iter; + std::vector vec = getInList(); + + for(iter it = vec.begin(); it!=vec.end(); it++) { + + if( (*it)->getTypeId() == ItemAssembly::getClassTypeId() ) { + assembly = static_cast(*it); + m_solver = assembly->m_solver; + break; + }; + } + }; + + //check if we have been successfull + if(!m_solver) { + Base::Console().Message("ConstraintGroup: Unable to retrieve assembly solver\n"); + return; + }; + + //init the constraint + c->init(m_solver); + + //solve the system and propagate the change upstream + m_solver->solve(); + assembly->touch(); +} + + short ConstraintGroup::mustExecute() const { //if (Sketch.isTouched() || @@ -53,7 +107,8 @@ short ConstraintGroup::mustExecute() const App::DocumentObjectExecReturn *ConstraintGroup::execute(void) { + Base::Console().Message("Recalculate constraint group\n"); return App::DocumentObject::StdReturn; } -} \ No newline at end of file +} diff --git a/src/Mod/Assembly/App/ConstraintGroup.h b/src/Mod/Assembly/App/ConstraintGroup.h index 2d4157666..167cd3451 100644 --- a/src/Mod/Assembly/App/ConstraintGroup.h +++ b/src/Mod/Assembly/App/ConstraintGroup.h @@ -1,5 +1,6 @@ /*************************************************************************** * Copyright (c) 2010 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * * This file is part of the FreeCAD CAx development system. * * * @@ -26,7 +27,10 @@ #include #include +#include +#include "Constraint.h" +#include "Solver.h" namespace Assembly { @@ -35,8 +39,14 @@ class AssemblyExport ConstraintGroup : public App::DocumentObject { PROPERTY_HEADER(Assembly::ConstraintGroup); +protected: + boost::shared_ptr m_solver; + public: ConstraintGroup(); + void addConstraint(Constraint* c); + + PyObject *getPyObject(void); App::PropertyLinkList Constraints; diff --git a/src/Mod/Assembly/App/ConstraintGroupPy.xml b/src/Mod/Assembly/App/ConstraintGroupPy.xml index 6ff8732e5..9691e9668 100644 --- a/src/Mod/Assembly/App/ConstraintGroupPy.xml +++ b/src/Mod/Assembly/App/ConstraintGroupPy.xml @@ -13,5 +13,12 @@ Base class of all objects in Assembly + + + Adds a constraint object to the constraint group + + + + diff --git a/src/Mod/Assembly/App/ConstraintGroupPyImp.cpp b/src/Mod/Assembly/App/ConstraintGroupPyImp.cpp index 0fbcfaff1..19bd9fd63 100644 --- a/src/Mod/Assembly/App/ConstraintGroupPyImp.cpp +++ b/src/Mod/Assembly/App/ConstraintGroupPyImp.cpp @@ -2,6 +2,7 @@ #include "PreCompiled.h" #include "Mod/Assembly/App/ConstraintGroup.h" +#include "Mod/Assembly/App/ConstraintPy.h" // inclusion of the generated files (generated out of ConstraintGroupPy.xml) #include "ConstraintGroupPy.h" @@ -15,11 +16,19 @@ std::string ConstraintGroupPy::representation(void) const return std::string(""); } +PyObject* ConstraintGroupPy::addConstraint(PyObject * args) +{ + PyObject *pcObj; + if (!PyArg_ParseTuple(args, "O", &pcObj)) + return 0; - - - - + if (PyObject_TypeCheck(pcObj, &(Assembly::ConstraintPy::Type))) { + Base::Console().Message("Add constraint\n"); + Assembly::Constraint *c = static_cast(pcObj)->getConstraintPtr(); + this->getConstraintGroupPtr()->addConstraint(c); + } + Py_Return; +} PyObject *ConstraintGroupPy::getCustomAttributes(const char* /*attr*/) const { diff --git a/src/Mod/Assembly/App/Item.cpp b/src/Mod/Assembly/App/Item.cpp index 9cf152f9c..897f357ff 100644 --- a/src/Mod/Assembly/App/Item.cpp +++ b/src/Mod/Assembly/App/Item.cpp @@ -1,5 +1,6 @@ /*************************************************************************** * Copyright (c) 2010 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * * This file is part of the FreeCAD CAx development system. * * * @@ -27,10 +28,11 @@ #include #include +#include #include "Item.h" #include "ItemPy.h" - +#include using namespace Assembly; @@ -73,6 +75,7 @@ short Item::mustExecute() const App::DocumentObjectExecReturn *Item::execute(void) { + Base::Console().Message("Recalculate Assembly::Item\n"); return App::DocumentObject::StdReturn; } diff --git a/src/Mod/Assembly/App/Item.h b/src/Mod/Assembly/App/Item.h index a4393a885..9424fdd33 100644 --- a/src/Mod/Assembly/App/Item.h +++ b/src/Mod/Assembly/App/Item.h @@ -1,5 +1,6 @@ /*************************************************************************** * Copyright (c) 2010 Juergen Riegel * + * Copyright (c) 2013 Stefan Tröger * * * * This file is part of the FreeCAD CAx development system. * * * @@ -28,7 +29,6 @@ #include #include - namespace Assembly { @@ -83,7 +83,7 @@ public: /// Visibility App::PropertyBool Visibility; //@} - + /** @name methods override feature */ //@{ /// recalculate the feature @@ -98,6 +98,7 @@ public: virtual TopoDS_Shape getShape(void)const =0 ; PyObject *getPyObject(void); + }; } //namespace Assembly diff --git a/src/Mod/Assembly/App/ItemAssembly.cpp b/src/Mod/Assembly/App/ItemAssembly.cpp index 6ae738e6b..0e86369d0 100644 --- a/src/Mod/Assembly/App/ItemAssembly.cpp +++ b/src/Mod/Assembly/App/ItemAssembly.cpp @@ -28,9 +28,11 @@ #endif #include +#include #include #include "ItemAssembly.h" +#include using namespace Assembly; @@ -40,7 +42,7 @@ namespace Assembly { PROPERTY_SOURCE(Assembly::ItemAssembly, Assembly::Item) -ItemAssembly::ItemAssembly() +ItemAssembly::ItemAssembly() : m_solver(new Solver) { ADD_PROPERTY(Items,(0)); ADD_PROPERTY(Annotations,(0)); @@ -48,15 +50,13 @@ ItemAssembly::ItemAssembly() short ItemAssembly::mustExecute() const { - //if (Sketch.isTouched() || - // Length.isTouched()) - // return 1; return 0; } App::DocumentObjectExecReturn *ItemAssembly::execute(void) { - + Base::Console().Message("Execute ItemAssembly\n"); + this->touch(); return App::DocumentObject::StdReturn; } @@ -92,4 +92,67 @@ TopoDS_Shape ItemAssembly::getShape(void) const } +PyObject *ItemAssembly::getPyObject(void) +{ + if (PythonObject.is(Py::_None())){ + // ref counter is set to 1 + PythonObject = Py::Object(new ItemAssemblyPy(this),true); + } + return Py::new_reference_to(PythonObject); +} + +void ItemAssembly::addPart(ItemPart* part) { + + if(part->m_part) { + //TODO: destroy old part + } + + //add the part to our list + const std::vector< App::DocumentObject * > &vals = this->Items.getValues(); + std::vector< App::DocumentObject * > newVals(vals); + newVals.push_back(part); + this->Items.setValues(newVals); + + part->m_part = m_solver->createPart(part->Placement.getValue(), part->Uid.getValueStr()); +} + +void ItemAssembly::addComponent(ItemAssembly* assembly) { + + if(assembly->m_solver) { + //TODO: destroy old solver system + } + + //add the component to our list + const std::vector< App::DocumentObject * > &vals = this->Items.getValues(); + std::vector< App::DocumentObject * > newVals(vals); + newVals.push_back(assembly); + this->Items.setValues(newVals); + + assembly->m_solver = boost::shared_ptr(m_solver->createSubsystem()); + assembly->m_solver->setTransformation(assembly->Placement.getValue()); +} + +ItemPart* ItemAssembly::getContainingPart(App::DocumentObject* obj) { + + typedef std::vector::const_iterator iter; + + const std::vector& vector = Items.getValues(); + for(iter it=vector.begin(); it != vector.end(); it++) { + + if( (*it)->getTypeId() == Assembly::ItemPart::getClassTypeId() ) { + if(static_cast(*it)->holdsObject(obj)) + return static_cast(*it); + } + else if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) { + + Assembly::ItemPart* part = static_cast(*it)->getContainingPart(obj); + if(part) + return part; + } + }; + + return NULL; +} + + } \ No newline at end of file diff --git a/src/Mod/Assembly/App/ItemAssembly.h b/src/Mod/Assembly/App/ItemAssembly.h index 156f1f6d8..58036bbff 100644 --- a/src/Mod/Assembly/App/ItemAssembly.h +++ b/src/Mod/Assembly/App/ItemAssembly.h @@ -25,15 +25,17 @@ #define ItemAssembly_ItemAssembly_H #include -#include "Item.h" +#include "Item.h" +#include "Solver.h" +#include "ItemPart.h" namespace Assembly { class AssemblyExport ItemAssembly : public Assembly::Item { - PROPERTY_HEADER(Assembly::ItemAssembly); + PROPERTY_HEADER(Assembly::ItemAssembly); public: ItemAssembly(); @@ -50,9 +52,17 @@ public: const char* getViewProviderName(void) const { return "AssemblyGui::ViewProviderItemAssembly"; } + PyObject *getPyObject(void); //@} virtual TopoDS_Shape getShape(void) const; + + void addPart(Assembly::ItemPart* part); + void addComponent(Assembly::ItemAssembly* assembly); + + Assembly::ItemPart* getContainingPart(App::DocumentObject* obj); + + boost::shared_ptr m_solver; }; } //namespace Assembly diff --git a/src/Mod/Assembly/App/ItemAssemblyPy.xml b/src/Mod/Assembly/App/ItemAssemblyPy.xml index e2f560946..7e9ba5727 100644 --- a/src/Mod/Assembly/App/ItemAssemblyPy.xml +++ b/src/Mod/Assembly/App/ItemAssemblyPy.xml @@ -13,5 +13,15 @@ Base class of all objects in Assembly + + + Adds a Assembly::ItemPart object to the assembly + + + + + Adds a Assembly::ItemAssembly object to the assembly + + diff --git a/src/Mod/Assembly/App/ItemAssemblyPyImp.cpp b/src/Mod/Assembly/App/ItemAssemblyPyImp.cpp index 7542d312f..5d7ff419c 100644 --- a/src/Mod/Assembly/App/ItemAssemblyPyImp.cpp +++ b/src/Mod/Assembly/App/ItemAssemblyPyImp.cpp @@ -6,6 +6,7 @@ // inclusion of the generated files (generated out of ItemAssemblyPy.xml) #include "ItemAssemblyPy.h" #include "ItemAssemblyPy.cpp" +#include using namespace Assembly; @@ -16,9 +17,39 @@ std::string ItemAssemblyPy::representation(void) const } +PyObject* ItemAssemblyPy::addPart(PyObject * args) +{ + PyObject *pcObj; + if (!PyArg_ParseTuple(args, "O", &pcObj)) { + Base::Console().Message("Test 1 fails\n"); + return 0; + } + if (PyObject_TypeCheck(pcObj, &(Assembly::ItemPartPy::Type))) { + Assembly::ItemPart *c = static_cast(pcObj)->getItemPartPtr(); + getItemAssemblyPtr()->addPart(c); + Base::Console().Message("Add Part\n"); + } + else Base::Console().Message("Test 2 fails\n"); + Py_Return; +} +PyObject* ItemAssemblyPy::addComponent(PyObject * args) +{ + PyObject *pcObj; + if (!PyArg_ParseTuple(args, "O", &pcObj)) { + Base::Console().Message("Test 1 fails\n"); + return 0; + } + if (PyObject_TypeCheck(pcObj, &(Assembly::ItemAssemblyPy::Type))) { + Assembly::ItemAssembly *c = static_cast(pcObj)->getItemAssemblyPtr(); + getItemAssemblyPtr()->addComponent(c); + Base::Console().Message("Add Component\n"); + } + else Base::Console().Message("Test 2 fails\n"); + Py_Return; +} PyObject *ItemAssemblyPy::getCustomAttributes(const char* /*attr*/) const diff --git a/src/Mod/Assembly/App/ItemPart.cpp b/src/Mod/Assembly/App/ItemPart.cpp index f2911f676..61a5b9fc0 100644 --- a/src/Mod/Assembly/App/ItemPart.cpp +++ b/src/Mod/Assembly/App/ItemPart.cpp @@ -26,9 +26,20 @@ #endif #include +#include #include "ItemPart.h" #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace Assembly; @@ -54,6 +65,8 @@ short ItemPart::mustExecute() const App::DocumentObjectExecReturn *ItemPart::execute(void) { + Base::Console().Message("Recalculate ItemPart\n"); + this->touch(); return App::DocumentObject::StdReturn; } @@ -66,7 +79,109 @@ TopoDS_Shape ItemPart::getShape(void) const } return TopoDS_Shape(); +} +PyObject *ItemPart::getPyObject(void) +{ + if (PythonObject.is(Py::_None())){ + // ref counter is set to 1 + PythonObject = Py::Object(new ItemPartPy(this),true); + } + return Py::new_reference_to(PythonObject); +} + +bool ItemPart::holdsObject(App::DocumentObject* obj) const { + + //get the body object and the relevant model list + Part::BodyBase* base = static_cast(Model.getValue()); + const std::vector& vector = base->Model.getValues(); + + //check if it holds the relevant document object + return std::find(vector.begin(), vector.end(), obj)!=vector.end(); +} + +void ItemPart::setCalculatedPlacement(boost::shared_ptr< Part3D > part) { + + //part is the same as m_part, so it doasn't matter which one we use + Base::Console().Message("Set new calculated part placement\n"); + + Base::Placement p = dcm::get(part); + Placement.setValue(p); +} + + +boost::shared_ptr< Geometry3D > ItemPart::getGeometry3D(const char* Type) +{ + + //check if the item is initialized + if(!m_part) + return boost::shared_ptr< Geometry3D >(); + + boost::shared_ptr geometry; + if(m_part->hasGeometry3D(Type)) { + return m_part->getGeometry3D(Type); + //Base::Console().Message("Already has geometry, nothing added\n"); + } else { + Part::TopoShape ts; + App::DocumentObject* obj = Model.getValue(); + + if (obj->getTypeId().isDerivedFrom(Part::Feature::getClassTypeId())) { + ts = static_cast(obj)->Shape.getShape(); + } + else return boost::shared_ptr< Geometry3D >(); + + TopoDS_Shape s = ts.getSubShape(Type); + if(s.ShapeType() == TopAbs_FACE) { + TopoDS_Face face = TopoDS::Face(s); + BRepAdaptor_Surface surface(face); + //Base::Console().Message("Fase selected\n"); + switch(surface.GetType()) { + case GeomAbs_Plane: { + //Base::Console().Message("plane selected\n"); + gp_Pln plane = surface.Plane(); + if(face.Orientation()==TopAbs_REVERSED) { + gp_Dir dir = plane.Axis().Direction(); + plane = gp_Pln(plane.Location(), dir.Reversed()); + } + geometry = m_part->addGeometry3D(plane, Type, dcm::Global); + break; + } + case GeomAbs_Cylinder: { + //Base::Console().Message("cylinder selected\n"); + gp_Cylinder cyl = surface.Cylinder(); + geometry = m_part->addGeometry3D(cyl, Type, dcm::Global); + break; + } + default: + Base::Console().Message("Unsuported Surface Geometrie Type at selection 1\n"); + return boost::shared_ptr< Geometry3D >(); + } + + } else if(s.ShapeType() == TopAbs_EDGE) { + TopoDS_Edge edge = TopoDS::Edge(s); + BRepAdaptor_Curve curve(edge); + switch(curve.GetType()) { + case GeomAbs_Line: { + gp_Lin line = curve.Line(); + geometry = m_part->addGeometry3D(line, Type, dcm::Global); + break; + } + default: + //Base::Console().Message("Unsuported Curve Geometrie Type at selection 1\n"); + return boost::shared_ptr< Geometry3D >(); + } + + } else if(s.ShapeType() == TopAbs_VERTEX) { + TopoDS_Vertex v1 = TopoDS::Vertex(s); + gp_Pnt point = BRep_Tool::Pnt(v1); + geometry = m_part->addGeometry3D(point, Type, dcm::Global); + + } else { + Base::Console().Message("Unsuported Topologie Type at selection 1\n"); + return boost::shared_ptr< Geometry3D >(); + } + }; + return geometry; } } \ No newline at end of file diff --git a/src/Mod/Assembly/App/ItemPart.h b/src/Mod/Assembly/App/ItemPart.h index d1d77cc29..c19e82740 100644 --- a/src/Mod/Assembly/App/ItemPart.h +++ b/src/Mod/Assembly/App/ItemPart.h @@ -26,6 +26,7 @@ #include #include "Item.h" +#include "Solver.h" namespace Assembly @@ -34,7 +35,7 @@ namespace Assembly class AssemblyExport ItemPart : public Assembly::Item { PROPERTY_HEADER(Assembly::ItemPart); - + public: ItemPart(); @@ -50,9 +51,16 @@ public: const char* getViewProviderName(void) const { return "AssemblyGui::ViewProviderItemPart"; } + PyObject *getPyObject(void); //@} virtual TopoDS_Shape getShape(void) const; + + bool holdsObject(App::DocumentObject* obj) const; + + boost::shared_ptr m_part; + virtual boost::shared_ptr getGeometry3D(const char* Type ); + void setCalculatedPlacement( boost::shared_ptr part ); }; } //namespace Assembly diff --git a/src/Mod/Assembly/App/gcs3d/CMakeLists.txt b/src/Mod/Assembly/App/gcs3d/CMakeLists.txt deleted file mode 100644 index 518b8553e..000000000 --- a/src/Mod/Assembly/App/gcs3d/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -project(solver) - -cmake_minimum_required(VERSION 2.6.0 FATAL_ERROR) -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") - -find_package(XercesC REQUIRED) -find_package(Eigen3) - -include_directories(${EIGEN3_INCLUDE_DIR}) - -#add_subdirectory(NewSolver) - -set(solver_SRC main.cpp - Constraints.cpp - GCS.cpp - InputParser.cpp - qp_eq.cpp - SubSystem.cpp) - -add_executable(solver ${solver_SRC}) - -target_link_libraries(solver ${XERCESC_LIBRARIES}) - - diff --git a/src/Mod/Assembly/App/gcs3d/Constraints.cpp b/src/Mod/Assembly/App/gcs3d/Constraints.cpp deleted file mode 100644 index 8dbe67935..000000000 --- a/src/Mod/Assembly/App/gcs3d/Constraints.cpp +++ /dev/null @@ -1,416 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#include -#include "Constraints.h" - -using namespace Eigen; - -namespace GCS -{ - -/////////////////////////////////////// -// Constraints -/////////////////////////////////////// - -Constraint::Constraint() -: origpvec(0), pvec(0), scale(1.), tag(0) -{ -} - -void Constraint::redirectParams(MAP_pD_pD redirectionmap) -{ - int i=0; - for (VEC_pD::iterator param=origpvec.begin(); - param != origpvec.end(); ++param, i++) { - MAP_pD_pD::const_iterator it = redirectionmap.find(*param); - if (it != redirectionmap.end()) - pvec[i] = it->second; - } -} - -void Constraint::revertParams() -{ - pvec = origpvec; -} - -ConstraintType Constraint::getTypeId() -{ - return None; -} - -void Constraint::rescale(double coef) -{ - scale = coef * 1.; -} - -double Constraint::error() -{ - return 0.; -} - -double Constraint::grad(double *param) -{ - return 0.; -} - -double Constraint::maxStep(MAP_pD_D &dir, double lim) -{ - return lim; -} - - -//quattorot and derivatives - -Matrix3d rotation(double a, double b, double c, double d) { - - double norm = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double x=a/norm, y=b/norm, z=c/norm, w=d/norm; - - Matrix3d rot; - rot(0,0) = 1-2*(pow(y,2)+pow(z,2)); - rot(0,1) = -2.0*w*z + 2.0*x*y; - rot(0,2) = 2.0*w*y + 2.0*x*z; - rot(1,0) = 2.0*w*z + 2.0*x*y; - rot(1,1) = 1-2*(pow(x,2)+pow(z,2)); - rot(1,2) = -2.0*w*x + 2.0*y*z; - rot(2,0) = -2.0*w*y + 2.0*x*z; - rot(2,1) = 2.0*w*x + 2.0*y*z; - rot(2,2) = 1-2*(pow(x,2)+pow(y,2)); - - return rot; -} - -Matrix3d rotation_da(double a, double b, double c, double d) { - - double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); - double x=a/no, y=b/no, z=c/no, w=d/no; - - double dxa = 1.0/no - pow(a,2)/pow(no,3); - double dya = (-1.0*b*a)/div; - double dza = (-1.0*c*a)/div; - double dwa = (-1.0*d*a)/div; - - Matrix3d rot; - rot << -4.0*(y*dya+z*dza), -2.0*(w*dza+dwa*z)+2.0*(x*dya+dxa*y), 2.0*(dwa*y+w*dya)+2.0*(dxa*z+x*dza), - 2.0*(w*dza+dwa*z)+2.0*(x*dya+dxa*y), -4.0*(x*dxa+z*dza), -2.0*(dwa*x+w*dxa)+2.0*(dya*z+y*dza), - -2.0*(dwa*y+w*dya)+2.0*(dxa*z+x*dza), 2.0*(dwa*x+w*dxa)+2.0*(dya*z+y*dza), -4.0*(x*dxa+y*dya); - - return rot; -} - -Matrix3d rotation_db(double a, double b, double c, double d) { - - double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); - double x=a/no, y=b/no, z=c/no, w=d/no; - - double dxb = (-1.0*a*b)/div; - double dyb = 1.0/no - pow(b,2)/pow(no,3); - double dzb = (-1.0*c*b)/div; - double dwb = (-1.0*d*b)/div; - - Matrix3d rot; - rot << -4.0*(y*dyb+z*dzb), -2.0*(w*dzb+dwb*z)+2.0*(x*dyb+dxb*y), 2.0*(dwb*y+w*dyb)+2.0*(dxb*z+x*dzb), - 2.0*(w*dzb+dwb*z)+2.0*(x*dyb+dxb*y), -4.0*(x*dxb+z*dzb), -2.0*(dwb*x+w*dxb)+2.0*(dyb*z+y*dzb), - -2.0*(dwb*y+w*dyb)+2.0*(dxb*z+x*dzb), 2.0*(dwb*x+w*dxb)+2.0*(dyb*z+y*dzb), -4.0*(x*dxb+y*dyb); - - return rot; -} - -Matrix3d rotation_dc(double a, double b, double c, double d) { - - double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); - double x=a/no, y=b/no, z=c/no, w=d/no; - - double dxc = (-1.0*a*c)/div; - double dyc = (-1.0*b*c)/div; - double dzc = 1.0/no - pow(c,2)/pow(no,3); - double dwc = (-1.0*d*c)/div; - - Matrix3d rot; - rot << -4.0*(y*dyc+z*dzc), -2.0*(w*dzc+dwc*z)+2.0*(x*dyc+dxc*y), 2.0*(dwc*y+w*dyc)+2.0*(dxc*z+x*dzc), - 2.0*(w*dzc+dwc*z)+2.0*(x*dyc+dxc*y), -4.0*(x*dxc+z*dzc), -2.0*(dwc*x+w*dxc)+2.0*(dyc*z+y*dzc), - -2.0*(dwc*y+w*dyc)+2.0*(dxc*z+x*dzc), 2.0*(dwc*x+w*dxc)+2.0*(dyc*z+y*dzc), -4.0*(x*dxc+y*dyc); - - return rot; -} - -Matrix3d rotation_dd(double a, double b, double c, double d) { - - double no = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double div = pow(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2), 3.0/2.0); - double x=a/no, y=b/no, z=c/no, w=d/no; - - double dxd = (-1.0*a*d)/div; - double dyd = (-1.0*b*d)/div; - double dzd = (-1.0*c*d)/div; - double dwd = 1.0/no - pow(d,2)/pow(no,3); - - Matrix3d rot; - rot << -4.0*(y*dyd+z*dzd), -2.0*(w*dzd+dwd*z)+2.0*(x*dyd+dxd*y), 2.0*(dwd*y+w*dyd)+2.0*(dxd*z+x*dzd), - 2.0*(w*dzd+dwd*z)+2.0*(x*dyd+dxd*y), -4.0*(x*dxd+z*dzd), -2.0*(dwd*x+w*dxd)+2.0*(dyd*z+y*dzd), - -2.0*(dwd*y+w*dyd)+2.0*(dxd*z+x*dzd), 2.0*(dwd*x+w*dxd)+2.0*(dyd*z+y*dzd), -4.0*(x*dxd+y*dyd); - - return rot; -} - - -//Plane parallel (need to be treated special as angle=0° or angle=180° dos not work with angle constraint -ConstraintParralelFaceAS::ConstraintParralelFaceAS( GCS::Solid& s0, GCS::Solid& s1, ParallelType *t) -{ - type = t; - - //get the face placements in the objects coordinate system and calculate the normals - Vector3d a,b; - a << s0.n.x, s0.n.y, s0.n.z; - b << s1.n.x, s1.n.y, s1.n.z; - - //get the normal vector - n0 = (a).normalized(); - n1 = (b).normalized(); - - - pvec.push_back(s0.q.a); - pvec.push_back(s0.q.b); - pvec.push_back(s0.q.c); - pvec.push_back(s0.q.d); - pvec.push_back(s1.q.a); - pvec.push_back(s1.q.b); - pvec.push_back(s1.q.c); - pvec.push_back(s1.q.d); - origpvec = pvec; - rescale(); - -} - -ConstraintType ConstraintParralelFaceAS::getTypeId() -{ - return ASParallel; -} - -void ConstraintParralelFaceAS::rescale(double coef) -{ - scale = coef; -} - -double ConstraintParralelFaceAS::error() -{ - - Matrix3d rot0, rot1; - - rot0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); - rot1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); - - Vector3d n0_g = (rot0*n0); - Vector3d n1_g = (rot1*n1); - - double error = 0; - if (*type == GCS::NormalSame) - error = pow((n0_g-n1_g).norm(),2); - else - error = pow((n0_g+n1_g).norm(),2); - - return error; -} - -double ConstraintParralelFaceAS::grad(double* param) -{ - - Vector3d dn; - Matrix3d r0, r1; - r0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); - r1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); - - if (param == q0a()) { - Matrix3d rot = rotation_da(*q0a(), *q0b(), *q0c(), *q0d()); - dn = rot*n0; - } - else if (param == q0b()) { - Matrix3d rot = rotation_db(*q0a(), *q0b(), *q0c(), *q0d()); - dn = rot*n0; - } - else if (param == q0c()) { - Matrix3d rot = rotation_dc(*q0a(), *q0b(), *q0c(), *q0d()); - dn = rot*n0; - } - else if (param == q0d()) { - Matrix3d rot = rotation_dd(*q0a(), *q0b(), *q0c(), *q0d()); - dn = rot*n0; - } - else if (param == q1a()) { - Matrix3d rot = rotation_da(*q1a(), *q1b(), *q1c(), *q1d()); - dn = rot*n1*-1; - } - else if (param == q1b()) { - Matrix3d rot = rotation_db(*q1a(), *q1b(), *q1c(), *q1d()); - dn = rot*n1*-1; - } - else if (param == q1c()) { - Matrix3d rot = rotation_dc(*q1a(), *q1b(), *q1c(), *q1d()); - dn = rot*n1*-1; - } - else if (param == q1d()) { - Matrix3d rot = rotation_dd(*q1a(), *q1b(), *q1c(), *q1d()); - dn = rot*n1*-1; - } - else return 0; - - - Vector3d n0n = r0*n0; - Vector3d n1n = r1*n1; - - double div = 0; - if (*type == NormalSame) - div = (n0n-n1n).dot(dn)*2; - else - div = (n0n+n1n).dot(dn)*2; - - return div; -} - - -//dDistance constraint - -ConstraintFaceDistanceAS::ConstraintFaceDistanceAS(GCS::Solid& s0, GCS::Solid& s1, double *d) -{ - - n0 << s0.n.x, s0.n.y, s0.n.z; - n0.normalize(); - p0 << s0.p.x, s0.p.y, s0.p.z; - p1 << s1.p.x, s1.p.y, s1.p.z; - - //and positions - pvec.push_back(s0.d.x); - pvec.push_back(s0.d.y); - pvec.push_back(s0.d.z); - pvec.push_back(s1.d.x); - pvec.push_back(s1.d.y); - pvec.push_back(s1.d.z); - - //quaternions - pvec.push_back(s0.q.a); - pvec.push_back(s0.q.b); - pvec.push_back(s0.q.c); - pvec.push_back(s0.q.d); - pvec.push_back(s1.q.a); - pvec.push_back(s1.q.b); - pvec.push_back(s1.q.c); - pvec.push_back(s1.q.d); - - //distance - dist = d; - - origpvec = pvec; - rescale(); - -} - -ConstraintType ConstraintFaceDistanceAS::getTypeId() -{ - return ASDistance; -} - -void ConstraintFaceDistanceAS::rescale(double coef) -{ - scale = coef; -} - -double ConstraintFaceDistanceAS::error() -{ - - Matrix3d rot0, rot1; - Vector3d v0, v1; - - rot0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); - rot1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); - v0 << *p0x(), *p0y(), *p0z(); - v1 << *p1x(), *p1y(), *p1z(); - - double error = std::pow(((rot0*n0).dot(rot1*p1+v1) - (rot0*n0).dot(rot0*p0+v0))/(rot0*n0).norm() - *dist,2); - - return error; -} - -double ConstraintFaceDistanceAS::grad(double* param) -{ - - Matrix3d r0, r1; - Vector3d v0, v1; - - v0 << *p0x(), *p0y(), *p0z(); - v1 << *p1x(), *p1y(), *p1z(); - r0 = rotation(*q0a(), *q0b(), *q0c(), *q0d()); - r1 = rotation(*q1a(), *q1b(), *q1c(), *q1d()); - - Matrix3d dr0, dr1; - double div = 0; - double error=((r0*n0).dot(r1*p1+v1) - (r0*n0).dot(r0*p0+v0))/(r0*n0).norm() - *dist; - if (param == q0a() || param == q0b() || param == q0c() || param == q0d()) { - - if (param == q0a()) dr0 = rotation_da(*q0a(), *q0b(), *q0c(), *q0d()); - else if (param == q0b()) dr0 = rotation_db(*q0a(), *q0b(), *q0c(), *q0d()); - else if (param == q0c()) dr0 = rotation_dc(*q0a(), *q0b(), *q0c(), *q0d()); - else if (param == q0d()) dr0 = rotation_dd(*q0a(), *q0b(), *q0c(), *q0d()); - - VectorXd r0n = r0*n0; - div = ( (dr0*n0).dot(r1*p1+v1)*r0n.norm() - r0n.dot(r1*p1+v1)*r0n.dot(dr0*n0)/r0n.norm() ); - div -= ( ((dr0*n0).dot(r0*p0+v0)+r0n.dot(dr0*p0))*r0n.norm() ); - div -= ( r0n.dot(r0*p0+v0)*r0n.dot(dr0*n0)/r0n.norm() ); - div /= pow(r0n.norm(),2); - } - else if (param == q1a() || param == q1b() || param == q1c() || param == q1d()) { - - if (param == q1a()) dr1 = rotation_da(*q1a(), *q1b(), *q1c(), *q1d()); - else if (param == q1b()) dr1 = rotation_db(*q1a(), *q1b(), *q1c(), *q1d()); - else if (param == q1c()) dr1 = rotation_dc(*q1a(), *q1b(), *q1c(), *q1d()); - else if (param == q1d()) dr1 = rotation_dd(*q1a(), *q1b(), *q1c(), *q1d()); - - div = (r0*n0).dot(dr1*p1)/(r0*n0).norm(); - } - else if (param == p0x() || param == p0y() || param == p0z()) { - - Vector3d dp_g; - if (param == p0x()) dp_g << 1,0,0; - else if (param == p0y()) dp_g << 0,1,0; - else if (param == p0z()) dp_g << 0,0,1; - - div = -1*(r0*n0).dot(dp_g)/(r0*n0).norm(); - } - else if (param == p1x() || param == p1y() || param == p1z()) { - - Vector3d dp_g; - if (param == p1x()) dp_g << 1,0,0; - else if (param == p1y()) dp_g << 0,1,0; - else if (param == p1z()) dp_g << 0,0,1; - - div = (r0*n0).dot(dp_g)/(r0*n0).norm(); - } - else return 0; - - div *= 2*error; - return div; -} - -} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/Constraints.h b/src/Mod/Assembly/App/gcs3d/Constraints.h deleted file mode 100644 index f8b2b38c9..000000000 --- a/src/Mod/Assembly/App/gcs3d/Constraints.h +++ /dev/null @@ -1,133 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#ifndef FREEGCS_CONSTRAINTS_H -#define FREEGCS_CONSTRAINTS_H - -#include "Geo.h" -#include "Util.h" -#include - -namespace GCS -{ - - /////////////////////////////////////// - // Constraints - /////////////////////////////////////// - - enum ConstraintType { - None = 0, - ASParallel, - ASDistance - }; - - enum ParallelType { - - NormalWhatever = 0, - NormalSame, - NormalOpposite - }; - - class Constraint - { - protected: - VEC_pD origpvec; // is used only as a reference for redirecting and reverting pvec - VEC_pD pvec; - double scale; - int tag; - public: - Constraint(); - - inline VEC_pD params() { return pvec; } - - void redirectParams(MAP_pD_pD redirectionmap); - void revertParams(); - void setTag(int tagId) { tag = tagId; } - int getTag() { return tag; } - - virtual ConstraintType getTypeId(); - virtual void rescale(double coef=1.); - virtual double error(); - virtual double grad(double *); - // virtual void grad(MAP_pD_D &deriv); --> TODO: vectorized grad version - virtual double maxStep(MAP_pD_D &dir, double lim=1.); - }; - - // AS_plane_parallel - class ConstraintParralelFaceAS : public Constraint - { - private: - inline double* q0a() { return pvec[0]; } - inline double* q0b() { return pvec[1]; } - inline double* q0c() { return pvec[2]; } - inline double* q0d() { return pvec[3]; } - inline double* q1a() { return pvec[4]; } - inline double* q1b() { return pvec[5]; } - inline double* q1c() { return pvec[6]; } - inline double* q1d() { return pvec[7]; } - - Eigen::Vector3d n0, n1; - ParallelType *type; - - public: - ConstraintParralelFaceAS(GCS::Solid& s0, GCS::Solid& s1, ParallelType *t); - - virtual ConstraintType getTypeId(); - virtual void rescale(double coef=1.); - virtual double error(); - virtual double grad(double *); - }; - - class ConstraintFaceDistanceAS : public Constraint - { - private: - inline double* p0x() { return pvec[0]; } - inline double* p0y() { return pvec[1]; } - inline double* p0z() { return pvec[2]; } - inline double* p1x() { return pvec[3]; } - inline double* p1y() { return pvec[4]; } - inline double* p1z() { return pvec[5]; } - inline double* q0a() { return pvec[6]; } - inline double* q0b() { return pvec[7]; } - inline double* q0c() { return pvec[8]; } - inline double* q0d() { return pvec[9]; } - inline double* q1a() { return pvec[10]; } - inline double* q1b() { return pvec[11]; } - inline double* q1c() { return pvec[12]; } - inline double* q1d() { return pvec[13]; } - - Eigen::Vector3d p0, p1; - Eigen::Vector3d n0; - - double *dist; - public: - ConstraintFaceDistanceAS(GCS::Solid& s0, GCS::Solid& s1, double *d); - virtual ConstraintType getTypeId(); - virtual void rescale(double coef=1.); - virtual double error(); - virtual double grad(double *); - }; - - -} //namespace GCS - -#endif // FREEGCS_CONSTRAINTS_H diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx deleted file mode 100644 index 8216a86e5..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex1.gcx +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx deleted file mode 100644 index 68e3d8c5d..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex2.gcx +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx deleted file mode 100644 index 207e00067..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex3.gcx +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx deleted file mode 100644 index 5164d326e..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex4.gcx +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx deleted file mode 100644 index d505e772c..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex5.gcx +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx deleted file mode 100644 index c4ccdfa2c..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex6.gcx +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx b/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx deleted file mode 100644 index ab4cd6873..000000000 --- a/src/Mod/Assembly/App/gcs3d/Example/AS_ex7.gcx +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/Mod/Assembly/App/gcs3d/GCS.cpp b/src/Mod/Assembly/App/gcs3d/GCS.cpp deleted file mode 100644 index 8f8592883..000000000 --- a/src/Mod/Assembly/App/gcs3d/GCS.cpp +++ /dev/null @@ -1,1140 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ -#include -#include -#include - -#include "GCS.h" -#include "qp_eq.h" -#include - -#include -#include -#include - -namespace GCS -{ - - -/////////////////////////////////////// -// Solver -/////////////////////////////////////// - -// System -System::System() - : clist(0), - c2p(), p2c(), - subsys0(0), - subsys1(0), - subsys2(0), - reference(), - init(false) -{ -} - -System::System(std::vector clist_) - : c2p(), p2c(), - subsys0(0), - subsys1(0), - subsys2(0), - reference(), - init(false) -{ - // create own (shallow) copy of constraints - for (std::vector::iterator constr=clist_.begin(); - constr != clist_.end(); ++constr) { - Constraint *newconstr; - switch ((*constr)->getTypeId()) { - - case GCS::ASParallel: { - ConstraintParralelFaceAS *oldconstr = static_cast(*constr); - newconstr = new ConstraintParralelFaceAS(*oldconstr); - break; - } - case GCS::ASDistance: { - ConstraintFaceDistanceAS *oldconstr = static_cast(*constr); - newconstr = new ConstraintFaceDistanceAS(*oldconstr); - break; - } - case None: - break; - } - if (newconstr) - addConstraint(newconstr); - } -} - -System::~System() -{ - clear(); -} - -void System::clear() -{ - clearReference(); - clearSubSystems(); - free(clist); - c2p.clear(); - p2c.clear(); -} - -void System::clearByTag(int tagId) -{ - std::vector constrvec; - for (std::vector::const_iterator - constr=clist.begin(); constr != clist.end(); ++constr) { - if ((*constr)->getTag() == tagId) - constrvec.push_back(*constr); - } - for (std::vector::const_iterator - constr=constrvec.begin(); constr != constrvec.end(); ++constr) { - removeConstraint(*constr); - } -} - -int System::addConstraint(Constraint *constr) -{ - clearReference(); - - clist.push_back(constr); - VEC_pD constr_params = constr->params(); - for (VEC_pD::const_iterator param=constr_params.begin(); - param != constr_params.end(); ++param) { -// jacobi.set(constr, *param, 0.); - c2p[constr].push_back(*param); - p2c[*param].push_back(constr); - } - return clist.size()-1; -} - -void System::removeConstraint(Constraint *constr) -{ - clearReference(); - clearSubSystems(); - - std::vector::iterator it; - it = std::find(clist.begin(), clist.end(), constr); - clist.erase(it); - - VEC_pD constr_params = c2p[constr]; - for (VEC_pD::const_iterator param=constr_params.begin(); - param != constr_params.end(); ++param) { - std::vector &constraints = p2c[*param]; - it = std::find(constraints.begin(), constraints.end(), constr); - constraints.erase(it); - } - c2p.erase(constr); - - std::vector constrvec; - constrvec.push_back(constr); - free(constrvec); -} - - - -void System::initSolution(VEC_pD ¶ms) -{ - // - Stores the current parameters in the vector "reference" - // - Identifies the equality constraints tagged with ids >= 0 - // and prepares a corresponding system reduction - // - Organizes the rest of constraints into two subsystems for - // tag ids >=0 and < 0 respectively and applies the - // system reduction specified in the previous step - - clearReference(); - for (VEC_pD::const_iterator param=params.begin(); - param != params.end(); ++param) - reference[*param] = **param; - - // identification of equality constraints and parameter reduction - std::set eliminated; // constraints that will be eliminated through reduction - reductionmap.clear(); - { - VEC_pD reduced_params=params; - MAP_pD_I params_index; - for (int i=0; i < int(params.size()); ++i) - params_index[params[i]] = i; - - /* for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - if ((*constr)->getTag() >= 0 && (*constr)->getTypeId() == Equal) { - MAP_pD_I::const_iterator it1,it2; - it1 = params_index.find((*constr)->params()[0]); - it2 = params_index.find((*constr)->params()[1]); - if (it1 != params_index.end() && it2 != params_index.end()) { - eliminated.insert(*constr); - double *p_kept = reduced_params[it1->second]; - double *p_replaced = reduced_params[it2->second]; - for (int i=0; i < int(params.size()); ++i) - if (reduced_params[i] == p_replaced) - reduced_params[i] = p_kept; - } - } - }*/ - for (int i=0; i < int(params.size()); ++i) - if (params[i] != reduced_params[i]) - reductionmap[params[i]] = reduced_params[i]; - } - - int i=0; - std::vector clist0, clist1, clist2; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { - if (eliminated.count(*constr) == 0) { - if ((*constr)->getTag() >= 0) - clist0.push_back(*constr); - else if ((*constr)->getTag() == -1) // move constraints - clist1.push_back(*constr); - else // distance from reference constraints - clist2.push_back(*constr); - } - } - - clearSubSystems(); - if (clist0.size() > 0) - subsys0 = new SubSystem(clist0, params, reductionmap); - if (clist1.size() > 0) - subsys1 = new SubSystem(clist1, params, reductionmap); - if (clist2.size() > 0) - subsys2 = new SubSystem(clist2, params, reductionmap); - init = true; -} - -void System::clearReference() -{ - init = false; - reference.clear(); -} - -void System::resetToReference() -{ - for (MAP_pD_D::const_iterator it=reference.begin(); - it != reference.end(); ++it) - *(it->first) = it->second; -} - -int System::solve(VEC_pD ¶ms, bool isFine, Algorithm alg) -{ - initSolution(params); - return solve(isFine, alg); -} - -int System::solve(bool isFine, Algorithm alg) -{ - if (subsys0) { - resetToReference(); - if (subsys2) { - int ret = solve(subsys0, subsys2, isFine); - if (subsys1) // give subsys1 higher priority than subsys2 - // in this case subsys2 acts like a preconditioner - return solve(subsys0, subsys1, isFine); - else - return ret; - } - else if (subsys1) - return solve(subsys0, subsys1, isFine); - else - return solve(subsys0, isFine, alg); - } - else if (subsys1) { - resetToReference(); - if (subsys2) - return solve(subsys1, subsys2, isFine); - else - return solve(subsys1, isFine, alg); - } - else - // return success in order to permit coincidence constraints to be applied - return Success; -} - -int System::solve(SubSystem *subsys, bool isFine, Algorithm alg) -{ - - std::cout << std::endl << "Initial Error: " << subsys->error() << std::endl; - clock_t begin = clock(); - int ret; - if (alg == BFGS) - ret= solve_BFGS(subsys, isFine); - else if (alg == LevenbergMarquardt) - ret= solve_LM(subsys); - else if (alg == DogLeg) - ret= solve_DL(subsys); - else if (alg == HOPS) - ret= solve_EX(subsys); - - clock_t end = clock(); - std::cout << "Time elapsed: " << double( ((end-begin)*1000)/CLOCKS_PER_SEC ) << " ms"<< std::endl; - - return ret; -} - -int System::solve_EX(SubSystem* subsys) { - - - return 0; - -} - - -int System::solve_BFGS(SubSystem *subsys, bool isFine) -{ - int xsize = subsys->pSize(); - if (xsize == 0) - return Success; - - subsys->redirectParams(); - - Eigen::MatrixXd D = Eigen::MatrixXd::Identity(xsize, xsize); - Eigen::VectorXd x(xsize); - Eigen::VectorXd xdir(xsize); - Eigen::VectorXd grad(xsize); - Eigen::VectorXd h(xsize); - Eigen::VectorXd y(xsize); - Eigen::VectorXd Dy(xsize); - - // Initial unknowns vector and initial gradient vector - subsys->getParams(x); - subsys->calcGrad(grad); - - // Initial search direction oposed to gradient (steepest-descent) - xdir = grad; - lineSearch(subsys, xdir); - double err = subsys->error(); - - h = x; - subsys->getParams(x); - h = x - h; // = x xold - - double convergence = isFine ? XconvergenceFine : XconvergenceRough; - int maxIterNumber = MaxIterations * xsize; - double diverging_lim = 1e6*err + 1e12; - - for (int iter=1; iter < maxIterNumber; iter++) { - - if (h.norm() <= convergence || err <= smallF) - break; - if (err > diverging_lim || err != err) // check for diverging and NaN - break; - - y = grad; - subsys->calcGrad(grad); - y = grad - y; // = grad gradold - - double hty = h.dot(y); - //make sure that hty is never 0 - if (hty == 0) - hty = .0000000001; - - Dy = D * y; - - double ytDy = y.dot(Dy); - - //Now calculate the BFGS update on D - D += (1.+ytDy/hty)/hty * h * h.transpose(); - D -= 1./hty * (h * Dy.transpose() + Dy * h.transpose()); - - xdir = D * grad; - lineSearch(subsys, xdir); - err = subsys->error(); - - h = x; - subsys->getParams(x); - h = x - h; // = x xold - } - - subsys->revertParams(); - - std::cout << "Error after BFGS solving: " << err << std::endl; - - if (err <= smallF) - return Success; - if (h.norm() <= convergence) - return Converged; - return Failed; -} - -int System::solve_LM(SubSystem* subsys) -{ - int xsize = subsys->pSize(); - int csize = subsys->cSize(); - - if (xsize == 0) - return Success; - - Eigen::VectorXd e(csize), e_new(csize); // vector of all function errors (every constraint is one function) - Eigen::MatrixXd J(csize, xsize); // Jacobi of the subsystem - Eigen::MatrixXd A(xsize, xsize); - Eigen::VectorXd x(xsize), h(xsize), x_new(xsize), g(xsize), diag_A(xsize); - - subsys->redirectParams(); - - subsys->getParams(x); - subsys->calcResidual(e); - e*=-1; - - int maxIterNumber = MaxIterations * xsize; - double diverging_lim = 1e6*e.squaredNorm() + 1e12; - - double eps=1e-10, eps1=1e-80; - double tau=1e-3; - double nu=2, mu=0; - int iter=0, stop=0; - for (iter=0; iter < maxIterNumber && !stop; ++iter) { - - // check error - double err=e.squaredNorm(); - if (err <= eps) { // error is small, Success - stop = 1; - break; - } - else if (err > diverging_lim || err != err) { // check for diverging and NaN - stop = 6; - break; - } - - // J^T J, J^T e - subsys->calcJacobi(J);; - - A = J.transpose()*J; - g = J.transpose()*e; - - // Compute ||J^T e||_inf - double g_inf = g.lpNorm(); - diag_A = A.diagonal(); // save diagonal entries so that augmentation can be later canceled - - // check for convergence - if (g_inf <= eps1) { - stop = 2; - break; - } - - // compute initial damping factor - if (iter == 0) - mu = tau * A.diagonal().lpNorm(); - - // determine increment using adaptive damping - while (1) { - // augment normal equations A = A+uI - for (int i=0; i < xsize; ++i) - A(i,i) += mu; - - //solve augmented functions A*h=-g - h = A.fullPivLu().solve(g); - double rel_error = (A*h - g).norm() / g.norm(); - - // check if solving works - if (rel_error < 1e-5) { -/* - double scale = subsys->maxStep() / h.lpNorm(); - if ( scale < 1.) - h *= scale; -*/ - // compute par's new estimate and ||d_par||^2 - x_new = x + h; - double h_norm = h.squaredNorm(); - - if (h_norm <= eps1*eps1*x.norm()) { // relative change in p is small, stop - stop = 3; - break; - } - else if (h_norm >= (x.norm()+eps1)/(DBL_EPSILON*DBL_EPSILON)) { // almost singular - stop = 4; - break; - } - - subsys->setParams(x_new); - subsys->calcResidual(e_new); - e_new *= -1; - - double dF = e.squaredNorm() - e_new.squaredNorm(); - double dL = h.dot(mu*h+g); - - if (dF>0. && dL>0.) { // reduction in error, increment is accepted - double tmp=2*dF/dL-1.; - mu *= std::max(1./3., 1.-tmp*tmp*tmp); - nu=2; - - // update par's estimate - x = x_new; - e = e_new; - break; - } - } - - // if this point is reached, either the linear system could not be solved or - // the error did not reduce; in any case, the increment must be rejected - - mu*=nu; - nu*=2.0; - for (int i=0; i < xsize; ++i) // restore diagonal J^T J entries - A(i,i) = diag_A(i); - } - } - - if (iter >= maxIterNumber) - stop = 5; - - std::cout << "Error after LM solving: " << subsys->error() << std::endl; - - subsys->revertParams(); - - return (stop == 1) ? Success : Failed; -} - - -int System::solve_DL(SubSystem* subsys) -{ - double tolg=1e-80, tolx=1e-80, tolf=1e-10; - - int xsize = subsys->pSize(); - int csize = subsys->cSize(); - - Eigen::VectorXd x(xsize), x_new(xsize); - Eigen::VectorXd fx(csize), fx_new(csize); - Eigen::MatrixXd Jx(csize, xsize), Jx_new(csize, xsize); - Eigen::VectorXd g(xsize), h_sd(xsize), h_gn(xsize), h_dl(xsize); - - subsys->redirectParams(); - - double err; - subsys->getParams(x); - subsys->calcResidual(fx, err); - subsys->calcJacobi(Jx); - - g = Jx.transpose()*(-fx); - - // get the infinity norm fx_inf and g_inf - double g_inf = g.lpNorm(); - double fx_inf = fx.lpNorm(); - - int maxIterNumber = MaxIterations * xsize; - double diverging_lim = 1e6*err + 1e12; - - double delta=0.1; - double alpha=0.; - double nu=2.; - int iter=0, stop=0, reduce=0; - while (!stop) { - - // check if finished - if (fx_inf <= tolf) // Success - stop = 1; - else if (g_inf <= tolg) - stop = 2; - else if (delta <= tolx*(tolx + x.norm())) - stop = 3; - else if (iter >= maxIterNumber) - stop = 4; - else if (err > diverging_lim || err != err) { // check for diverging and NaN - stop = 6; - } - else { - // get the steepest descent direction - alpha = g.squaredNorm()/(Jx*g).squaredNorm(); - h_sd = alpha*g; - - // get the gauss-newton step - h_gn = Jx.fullPivLu().solve(-fx); - double rel_error = (Jx*h_gn + fx).norm() / fx.norm(); - if (rel_error > 1e15) - break; - - // compute the dogleg step - if (h_gn.norm() < delta) { - h_dl = h_gn; - if (h_dl.norm() <= tolx*(tolx + x.norm())) { - stop = 5; - break; - } - } - else if (alpha*g.norm() >= delta) { - h_dl = (delta/(alpha*g.norm()))*h_sd; - } - else { - //compute beta - double beta = 0; - Eigen::VectorXd b = h_gn - h_sd; - double bb = (b.transpose()*b).norm(); - double gb = (h_sd.transpose()*b).norm(); - double c = (delta + h_sd.norm())*(delta - h_sd.norm()); - - if (gb > 0) - beta = c / (gb + sqrt(gb * gb + c * bb)); - else - beta = (sqrt(gb * gb + c * bb) - gb)/bb; - - // and update h_dl and dL with beta - h_dl = h_sd + beta*b; - } - } - - // see if we are already finished - if (stop) - break; -/* - double scale = subsys->maxStep() / h_dl.lpNorm(); - if ( scale < 1.) - h_dl *= scale; -*/ - // get the new values - double err_new; - x_new = x + h_dl; - subsys->setParams(x_new); - subsys->calcResidual(fx_new, err_new); - subsys->calcJacobi(Jx_new); - - // calculate the linear model and the update ratio - double dL = err - 0.5*(fx + Jx*h_dl).squaredNorm(); - double dF = err - err_new; - double rho = dL/dF; - - if (dF > 0 && dL > 0) { - x = x_new; - Jx = Jx_new; - fx = fx_new; - err = err_new; - - g = Jx.transpose()*(-fx); - - // get infinity norms - g_inf = g.lpNorm(); - fx_inf = fx.lpNorm(); - } - else - rho = -1; - - // update delta - if (fabs(rho-1.) < 0.2 && h_dl.norm() > delta/3. && reduce <= 0) { - delta = 3*delta; - nu = 2; - reduce = 0; - } - else if (rho < 0.25) { - delta = delta/nu; - nu = 2*nu; - reduce = 2; - } - else - reduce--; - - // count this iteration and start again - iter++; - } - - std::cout << "Error after DogLeg solving: " << subsys->error() << std::endl; - - - subsys->revertParams(); - - return (stop == 1) ? Success : Failed; -} - -// The following solver variant solves a system compound of two subsystems -// treating the first of them as of higher priority than the second -int System::solve(SubSystem *subsysA, SubSystem *subsysB, bool isFine) -{ - int xsizeA = subsysA->pSize(); - int xsizeB = subsysB->pSize(); - int csizeA = subsysA->cSize(); - - VEC_pD plist(xsizeA+xsizeB); - { - VEC_pD plistA, plistB; - subsysA->getParamList(plistA); - subsysB->getParamList(plistB); - - std::sort(plistA.begin(),plistA.end()); - std::sort(plistB.begin(),plistB.end()); - - VEC_pD::const_iterator it; - it = std::set_union(plistA.begin(),plistA.end(), - plistB.begin(),plistB.end(),plist.begin()); - plist.resize(it-plist.begin()); - } - int xsize = plist.size(); - - Eigen::MatrixXd B = Eigen::MatrixXd::Identity(xsize, xsize); - Eigen::MatrixXd JA(csizeA, xsize); - Eigen::MatrixXd Y,Z; - - Eigen::VectorXd resA(csizeA); - Eigen::VectorXd lambda(csizeA), lambda0(csizeA), lambdadir(csizeA); - Eigen::VectorXd x(xsize), x0(xsize), xdir(xsize), xdir1(xsize); - Eigen::VectorXd grad(xsize); - Eigen::VectorXd h(xsize); - Eigen::VectorXd y(xsize); - Eigen::VectorXd Bh(xsize); - - // We assume that there are no common constraints in subsysA and subsysB - subsysA->redirectParams(); - subsysB->redirectParams(); - - subsysB->getParams(plist,x); - subsysA->getParams(plist,x); - subsysB->setParams(plist,x); // just to ensure that A and B are synchronized - - subsysB->calcGrad(plist,grad); - subsysA->calcJacobi(plist,JA); - subsysA->calcResidual(resA); - - double convergence = isFine ? XconvergenceFine : XconvergenceRough; - int maxIterNumber = MaxIterations * xsize; - double diverging_lim = 1e6*subsysA->error() + 1e12; - - double mu = 0; - lambda.setZero(); - for (int iter=1; iter < maxIterNumber; iter++) { - int status = qp_eq(B, grad, JA, resA, xdir, Y, Z); - if (status) - break; - - x0 = x; - lambda0 = lambda; - lambda = Y.transpose() * (B * xdir + grad); - lambdadir = lambda - lambda0; - - // line search - { - double eta=0.25; - double tau=0.5; - double rho=0.5; - double alpha=1; - alpha = std::min(alpha, subsysA->maxStep(plist,xdir)); - - // Eq. 18.32 - // double mu = lambda.lpNorm() + 0.01; - // Eq. 18.33 - // double mu = grad.dot(xdir) / ( (1.-rho) * resA.lpNorm<1>()); - // Eq. 18.36 - mu = std::max(mu, - (grad.dot(xdir) + std::max(0., 0.5*xdir.dot(B*xdir))) / - ( (1. - rho) * resA.lpNorm<1>() ) ); - - // Eq. 18.27 - double f0 = subsysB->error() + mu * resA.lpNorm<1>(); - - // Eq. 18.29 - double deriv = grad.dot(xdir) - mu * resA.lpNorm<1>(); - - x = x0 + alpha * xdir; - subsysA->setParams(plist,x); - subsysB->setParams(plist,x); - subsysA->calcResidual(resA); - double f = subsysB->error() + mu * resA.lpNorm<1>(); - - // line search, Eq. 18.28 - bool first = true; - while (f > f0 + eta * alpha * deriv) { - if (first) { // try a second order step - // xdir1 = JA.jacobiSvd(Eigen::ComputeThinU | - // Eigen::ComputeThinV).solve(-resA); - xdir1 = -Y*resA; - x += xdir1; // = x0 + alpha * xdir + xdir1 - subsysA->setParams(plist,x); - subsysB->setParams(plist,x); - subsysA->calcResidual(resA); - f = subsysB->error() + mu * resA.lpNorm<1>(); - if (f < f0 + eta * alpha * deriv) - break; - } - alpha = tau * alpha; - x = x0 + alpha * xdir; - subsysA->setParams(plist,x); - subsysB->setParams(plist,x); - subsysA->calcResidual(resA); - f = subsysB->error() + mu * resA.lpNorm<1>(); - } - lambda = lambda0 + alpha * lambdadir; - - } - h = x - x0; - - y = grad - JA.transpose() * lambda; - { - subsysB->calcGrad(plist,grad); - subsysA->calcJacobi(plist,JA); - subsysA->calcResidual(resA); - } - y = grad - JA.transpose() * lambda - y; // Eq. 18.13 - - if (iter > 1) { - double yTh = y.dot(h); - if (yTh != 0) { - Bh = B * h; - //Now calculate the BFGS update on B - B += 1./yTh * y * y.transpose(); - B -= 1./h.dot(Bh) * (Bh * Bh.transpose()); - } - } - - double err = subsysA->error(); - if (h.norm() <= convergence && err <= smallF) - break; - if (err > diverging_lim || err != err) // check for diverging and NaN - break; - } - - int ret; - if (subsysA->error() <= smallF) - ret = Success; - else if (h.norm() <= convergence) - ret = Converged; - else - ret = Failed; - - subsysA->revertParams(); - subsysB->revertParams(); - return ret; - -} - -void System::getSubSystems(std::vector &subsysvec) -{ - subsysvec.clear(); - if (subsys0) - subsysvec.push_back(subsys0); - if (subsys1) - subsysvec.push_back(subsys1); - if (subsys2) - subsysvec.push_back(subsys2); -} - -void System::applySolution() -{ - if (subsys2) - subsys2->applySolution(); - if (subsys1) - subsys1->applySolution(); - if (subsys0) - subsys0->applySolution(); - - for (MAP_pD_pD::const_iterator it=reductionmap.begin(); - it != reductionmap.end(); ++it) - *(it->first) = *(it->second); -} - -int System::diagnose(VEC_pD ¶ms, VEC_I &conflicting) -{ - // Analyses the constrainess grad of the system and provides feedback - // The vector "conflicting" will hold a group of conflicting constraints - conflicting.clear(); - std::vector conflictingIndex; - VEC_I tags; - Eigen::MatrixXd J(clist.size(), params.size()); - int count=0; - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); - if ((*constr)->getTag() >= 0) { - count++; - tags.push_back((*constr)->getTag()); - for (int j=0; j < int(params.size()); j++) - J(count-1,j) = (*constr)->grad(params[j]); - } - } - - if (J.rows() > 0) { - Eigen::FullPivHouseholderQR qrJT(J.topRows(count).transpose()); - Eigen::MatrixXd Q = qrJT.matrixQ (); - int params_num = qrJT.rows(); - int constr_num = qrJT.cols(); - int rank = qrJT.rank(); - - Eigen::MatrixXd R; - if (constr_num >= params_num) - R = qrJT.matrixQR().triangularView(); - else - R = qrJT.matrixQR().topRows(constr_num) - .triangularView(); - - if (constr_num > rank) { // conflicting constraints - for (int i=1; i < rank; i++) { - // eliminate non zeros above pivot - assert(R(i,i) != 0); - for (int row=0; row < i; row++) { - if (R(row,i) != 0) { - double coef=R(row,i)/R(i,i); - R.block(row,i+1,1,constr_num-i-1) -= coef * R.block(i,i+1,1,constr_num-i-1); - R(row,i) = 0; - } - } - } - conflictingIndex.resize(constr_num-rank); - for (int j=rank; j < constr_num; j++) { - for (int row=0; row < rank; row++) { - if (R(row,j) != 0) { - int orig_col = qrJT.colsPermutation().indices()[row]; - conflictingIndex[j-rank].push_back(orig_col); - } - } - int orig_col = qrJT.colsPermutation().indices()[j]; - conflictingIndex[j-rank].push_back(orig_col); - } - - SET_I tags_set; - for (int i=0; i < conflictingIndex.size(); i++) { - for (int j=0; j < conflictingIndex[i].size(); j++) { - tags_set.insert(tags[conflictingIndex[i][j]]); - } - } - tags_set.erase(0); // exclude constraints tagged with zero - conflicting.resize(tags_set.size()); - std::copy(tags_set.begin(), tags_set.end(), conflicting.begin()); - - if (params_num == rank) // over-constrained - return params_num - constr_num; - } - - return params_num - rank; - } - return params.size(); -} - -void System::clearSubSystems() -{ - init = false; - std::vector subsystems; - getSubSystems(subsystems); - free(subsystems); - subsys0 = NULL; - subsys1 = NULL; - subsys2 = NULL; -} - - -//calculates the wolfe condition test functions p and g -Eigen::Vector2d PG(SubSystem* sys, Eigen::VectorXd xdir, double sigma) { - - int xsize = sys->pSize(); - Eigen::Vector2d PG; - Eigen::VectorXd x(xsize), x_new(xsize); - Eigen::VectorXd gr(xsize), grn(xsize); - double fx, fxn; - - sys->getParams(x); - fx = sys->error(); - sys->calcGrad(gr); - x_new = x-sigma*xdir; - sys->setParams(x_new); - fxn = sys->error(); - sys->calcGrad(grn); - sys->setParams(x); - - if (sigma == 0) - PG(1) = 1; - else - PG(1) = (fx-fxn) / (sigma * gr.transpose()*xdir).norm(); - - PG(0) = (grn.transpose()*xdir).norm() / (gr.transpose()*xdir).norm(); - - return PG; -} - - -double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir) -{ - double alpha, beta; - int xsize = subsys->pSize(); - - - - //get inital alpha and beta interval - //********************************** - double c3 = 0.1, c4 = 5; - double delta = 0.01, kappa = 0.9, mu = 0.8, sigma, gamma; - double fx, fxmd; - bool run = true; - - Eigen::VectorXd x(xsize), x_new(xsize); - Eigen::VectorXd grad(xsize); - - - subsys->getParams(x); - subsys->calcGrad(grad); - fx = subsys->error(); - x_new = x-xdir; - subsys->setParams(x_new); - fxmd= subsys->error(); - subsys->setParams(x); - - //calculate an guess for sigma - double comp1 = (grad.transpose()*xdir).norm() / std::pow(xdir.norm(),2); - double comp2 = (grad.transpose()*xdir).norm() /(2*(fxmd-fx+(grad.transpose()*xdir).norm())); - sigma = std::min(c4*comp1 , std::max(c3*comp1, comp2)); - - //calculate test functions with guess - Eigen::Vector2d pg = PG(subsys, xdir, sigma); - - //see if we are alread fisished - if ( pg(1)>=delta && pg(0) <= kappa) - run = false; - else if ( pg(1)>=delta && pg(0)>kappa) { - alpha = sigma; - //beta is sigma/mu^j with j minimal so that g(beta)=delta and p(alpha)>=kappa (j element natural numbers) - for (int i=1; i<=1000; i++) { - alpha = sigma*(std::pow(mu, i)); - pg = PG(subsys, xdir, alpha); - if (pg(0)>=kappa && pg(1) >= delta) break; - } - } - - //start sigma search in interval alpha beta - //***************************************** - while (run) { - - gamma = (alpha + beta)/2.; - pg = PG(subsys, xdir, gamma); - - if (pg(0) > kappa && pg(1) >= delta) alpha = gamma; - else if (pg(0) <= kappa && pg(1) >= delta) { - sigma = gamma; - run = false; - } - else beta = gamma; - - if (alpha == beta) break; - } - - x_new = x - sigma* xdir; - subsys->setParams(x_new); - - std::cout << "sigma: "<maxStep(xdir); - - Eigen::VectorXd x0, x; - - //Save initial values - subsys->getParams(x0); - - //Start at the initial position alpha1 = 0 - alpha1 = 0.; - f1 = subsys->error(); - - //Take a step of alpha2 = 1 - alpha2 = 1.; - x = x0 + alpha2 * xdir; - subsys->setParams(x); - f2 = subsys->error(); - - //Take a step of alpha3 = 2*alpha2 - alpha3 = alpha2*2; - x = x0 + alpha3 * xdir; - subsys->setParams(x); - f3 = subsys->error(); - - //Now reduce or lengthen alpha2 and alpha3 until the minimum is - //Bracketed by the triplet f1>f2 f1 || f2 > f3) { - if (f2 > f1) { - //If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1 - //Effectively both are shortened by a factor of two. - alpha3 = alpha2; - f3 = f2; - alpha2 = alpha2 / 2; - x = x0 + alpha2 * xdir; - subsys->setParams(x); - f2 = subsys->error(); - } - else if (f2 > f3) { - if (alpha3 >= alphaMax) - break; - //If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1 - //Effectively both are lengthened by a factor of two. - alpha2 = alpha3; - f2 = f3; - alpha3 = alpha3 * 2; - x = x0 + alpha3 * xdir; - subsys->setParams(x); - f3 = subsys->error(); - } - } - //Get the alpha for the minimum f of the quadratic approximation - alphaStar = alpha2 + ((alpha2-alpha1)*(f1-f3))/(3*(f1-2*f2+f3)); - - //Guarantee that the new alphaStar is within the bracket - if (alphaStar >= alpha3 || alphaStar <= alpha1) - alphaStar = alpha2; - - if (alphaStar > alphaMax) - alphaStar = alphaMax; - - if (alphaStar != alphaStar) - alphaStar = 0.; - - //Take a final step to alphaStar - x = x0 + alphaStar * xdir; - subsys->setParams(x); - - return alphaStar; -}*/ - - -void free(VEC_pD &doublevec) -{ - for (VEC_pD::iterator it = doublevec.begin(); - it != doublevec.end(); ++it) - if (*it) delete *it; - doublevec.clear(); -} - -void free(std::vector &constrvec) -{ - for (std::vector::iterator constr=constrvec.begin(); - constr != constrvec.end(); ++constr) { - if (*constr) { - switch ((*constr)->getTypeId()) { - - case None: - default: - delete *constr; - } - } - } - constrvec.clear(); -} - -void free(std::vector &subsysvec) -{ - for (std::vector::iterator it=subsysvec.begin(); - it != subsysvec.end(); ++it) - if (*it) delete *it; -} - -} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/GCS.h b/src/Mod/Assembly/App/gcs3d/GCS.h deleted file mode 100644 index c4a2ac671..000000000 --- a/src/Mod/Assembly/App/gcs3d/GCS.h +++ /dev/null @@ -1,122 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#ifndef FREEGCS_GCS_H -#define FREEGCS_GCS_H - -#include "SubSystem.h" - - -namespace GCS { - - - -/////////////////////////////////////// -// Solver -/////////////////////////////////////// - -enum SolveStatus { - Success = 0, // Found a solution zeroing the error function - Converged = 1, // Found a solution minimizing the error function - Failed = 2 // Failed to find any solution -}; - -enum Algorithm { - BFGS = 0, - LevenbergMarquardt = 1, - DogLeg = 2, - HOPS, -}; - -class System { - // This is the main class. It holds all constraints and information - // about partitioning into subsystems and solution strategies -private: - std::vector clist; - - std::map c2p; // constraint to parameter adjacency list - std::map > p2c; // parameter to constraint adjacency list - - SubSystem *subsys0; // has the highest priority, always used as the primary subsystem - SubSystem *subsys1; // normally used as secondary subsystem, it is considered primary only if subsys0 is missing - SubSystem *subsys2; // has the lowest priority, always used as secondary system - void clearSubSystems(); - - MAP_pD_D reference; - void clearReference(); - void resetToReference(); - - MAP_pD_pD reductionmap; // for simplification of equality constraints - - bool init; - - int solve_BFGS ( SubSystem *subsys, bool isFine ); - int solve_LM ( SubSystem *subsys ); - int solve_DL ( SubSystem *subsys ); - int solve_EX ( SubSystem *subsys ); -public: - System(); - System ( std::vector clist_ ); - ~System(); - - void clear(); - void clearByTag ( int tagId ); - - int addConstraint ( Constraint *constr ); - void removeConstraint ( Constraint *constr ); - - void initSolution ( VEC_pD ¶ms ); - - int solve ( bool isFine=true, Algorithm alg=DogLeg ); - int solve ( VEC_pD ¶ms, bool isFine=true, Algorithm alg=DogLeg ); - int solve ( SubSystem *subsys, bool isFine=true, Algorithm alg=DogLeg ); - int solve ( SubSystem *subsysA, SubSystem *subsysB, bool isFine=true ); - - void getSubSystems ( std::vector &subsysvec ); - void applySolution(); - - bool isInit() const { - return init; - } - - int diagnose ( VEC_pD ¶ms, VEC_I &conflicting ); -}; - -/////////////////////////////////////// -// BFGS Solver parameters -/////////////////////////////////////// -#define XconvergenceRough 1e-8 -#define XconvergenceFine 1e-10 -#define smallF 1e-20 -#define MaxIterations 100 //Note that the total number of iterations allowed is MaxIterations *xLength - -/////////////////////////////////////// -// Helper elements -/////////////////////////////////////// - -void free ( VEC_pD &doublevec ); -void free ( std::vector &constrvec ); -void free ( std::vector &subsysvec ); - -} //namespace GCS - -#endif // FREEGCS_GCS_H diff --git a/src/Mod/Assembly/App/gcs3d/Geo.h b/src/Mod/Assembly/App/gcs3d/Geo.h deleted file mode 100644 index c7629b51f..000000000 --- a/src/Mod/Assembly/App/gcs3d/Geo.h +++ /dev/null @@ -1,67 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#ifndef FREEGCS_GEO_H -#define FREEGCS_GEO_H - -namespace GCS -{ - - class Displacement { - - public: - Displacement(){x = 0; y = 0; z = 0;} - double *x, *y, *z; - }; - - class Point { - - public: - Point() { x=y=z=0;} - double x,y,z; - }; - - class Normal : public Point { - - public: - Normal() {}; - }; - - class Quaternion { - - public: - Quaternion() {a = b = c = d = 0;}; - double *a, *b, *c, *d; - }; - - class Solid { - - public: - Solid() {}; - Quaternion q; - Displacement d; - Normal n; - Point p; - }; -} //namespace GCS - -#endif // FREEGCS_GEO_H diff --git a/src/Mod/Assembly/App/gcs3d/InputParser.cpp b/src/Mod/Assembly/App/gcs3d/InputParser.cpp deleted file mode 100644 index 43f47f59e..000000000 --- a/src/Mod/Assembly/App/gcs3d/InputParser.cpp +++ /dev/null @@ -1,359 +0,0 @@ -#include -#include - -#include "InputParser.h" - -#include - - - -InputParser::InputParser() -{ - try { - xercesc::XMLPlatformUtils::Initialize(); // Initialize Xerces infrastructure - } - catch (xercesc::XMLException& e) { - char* message = xercesc::XMLString::transcode(e.getMessage()); - std::cout << "XML toolkit initialization error: " << message << std::endl; - xercesc::XMLString::release(&message); - } - - // Tags and attributes used in XML file. - // Can't call transcode till after Xerces Initialize() - TAG_points = xercesc::XMLString::transcode("points"); - TAG_point = xercesc::XMLString::transcode("point"); - TAG_constraints= xercesc::XMLString::transcode("constraints"); - TAG_constraint = xercesc::XMLString::transcode("constraint"); - ATTR_id = xercesc::XMLString::transcode("id"); - ATTR_x = xercesc::XMLString::transcode("x"); - ATTR_y = xercesc::XMLString::transcode("y"); - ATTR_z = xercesc::XMLString::transcode("z"); - ATTR_point = xercesc::XMLString::transcode("point"); - ATTR_kind = xercesc::XMLString::transcode("kind"); - ATTR_distance = xercesc::XMLString::transcode("distance"); - - TAG_GCS_AS = xercesc::XMLString::transcode("GCS_AS"); - TAG_quaternion = xercesc::XMLString::transcode("quaternion"); - TAG_quaternions = xercesc::XMLString::transcode("quaternions"); - TAG_normal = xercesc::XMLString::transcode("normal"); - TAG_normals = xercesc::XMLString::transcode("normals"); - TAG_displacement = xercesc::XMLString::transcode("displacement"); - TAG_displacements = xercesc::XMLString::transcode("displacements"); - TAG_solid = xercesc::XMLString::transcode("solid"); - TAG_solids = xercesc::XMLString::transcode("solids"); - KIND_Parallel_AS = xercesc::XMLString::transcode("Parallel_AS"); - KIND_Distance_AS = xercesc::XMLString::transcode("Distance_AS"); - ATTR_a = xercesc::XMLString::transcode("a"); - ATTR_b= xercesc::XMLString::transcode("b"); - ATTR_c = xercesc::XMLString::transcode("c"); - ATTR_d = xercesc::XMLString::transcode("d"); - ATTR_type = xercesc::XMLString::transcode("type"); - ATTR_quaternion = xercesc::XMLString::transcode("quaternion"); - ATTR_normal = xercesc::XMLString::transcode("normal"); - ATTR_displacement = xercesc::XMLString::transcode("displacement"); - ATTR_solid1 = xercesc::XMLString::transcode("solid1"); - ATTR_solid2 = xercesc::XMLString::transcode("solid2"); - - - m_InputFileParser = new xercesc::XercesDOMParser; -} - -InputParser::~InputParser() -{ - // Free memory - delete m_InputFileParser; -} - -bool InputParser::readInputFileAS(std::string &fileName, - std::vector &variables, - std::vector ¶meters, - std::vector &points, - std::vector &norms, - std::vector &disps, - std::vector &quats, - std::vector &solids, - std::vector &constraints) -{ - int pointsOffset = points.size(); - int normsOffset = norms.size(); - int dispsOffset = disps.size(); - int quatsOffset = quats.size(); - int solidsOffset = solids.size(); - - // Test to see if the file is ok. - struct stat fileStatus; - if (stat(fileName.c_str(), &fileStatus) != 0) { - std::cout << "Error reading the file " << fileName << std::endl; - return false; - } - - // Configure DOM parser. - m_InputFileParser->setValidationScheme(xercesc::XercesDOMParser::Val_Never); - m_InputFileParser->setDoNamespaces(false); - m_InputFileParser->setDoSchema(false); - m_InputFileParser->setLoadExternalDTD(false); - - try { - m_InputFileParser->parse(fileName.c_str()); - - // no need to free this pointer - owned by the parent parser object - xercesc::DOMDocument* xmlDoc = m_InputFileParser->getDocument(); - - // Get the top-level element: Name is "GCS_3D". No attributes for "GCS_3D" - xercesc::DOMElement* elementRoot = xmlDoc->getDocumentElement(); - if (!elementRoot) { - std::cout << "empty XML document" << std::endl; - return false; - } - else if (!xercesc::XMLString::equals(elementRoot->getTagName(), TAG_GCS_AS)) { - std::cout << "wrong root element in the XML file" << std::endl; - return false; - } - - // Parse XML file for tags of interest: "ApplicationSettings" - // Look one level nested within "root". (child of root) - xercesc::DOMNodeList* children = elementRoot->getChildNodes(); - const XMLSize_t nodeCount = children->getLength(); - - // Points - xercesc::DOMNodeList* pointsNodes = elementRoot->getElementsByTagName(TAG_points); - const XMLSize_t pointsCount = pointsNodes->getLength(); - - for (XMLSize_t i = 0; i < pointsCount; ++i) { - xercesc::DOMNode* pointsNode = pointsNodes->item(i); - xercesc::DOMElement* pointsElement = dynamic_cast(pointsNode); - - xercesc::DOMNodeList* pointNodes = pointsElement->getElementsByTagName(TAG_point); - const XMLSize_t pointCount = pointNodes->getLength(); - - for (XMLSize_t j = 0; j < pointCount; ++j) { - xercesc::DOMNode* pointNode = pointNodes->item(j); - xercesc::DOMElement* pointElement = dynamic_cast(pointNode); - - int id = getIntAttr(pointElement, ATTR_id); - double x = getDoubleAttr(pointElement, ATTR_x); - double y = getDoubleAttr(pointElement, ATTR_y); - double z = getDoubleAttr(pointElement, ATTR_z); - - if (id + pointsOffset > points.size()) - points.resize(id + pointsOffset); - - GCS::Point p; - p.x = x; - p.y = y; - p.z = z; - points[id + pointsOffset -1] = p; - } - } - - // normals - xercesc::DOMNodeList* normsNodes = elementRoot->getElementsByTagName(TAG_normals); - const XMLSize_t normsCount = normsNodes->getLength(); - - for (XMLSize_t i = 0; i < normsCount; ++i) { - xercesc::DOMNode* normsNode = normsNodes->item(i); - xercesc::DOMElement* normsElement = dynamic_cast(normsNode); - - xercesc::DOMNodeList* lineNodes = normsElement->getElementsByTagName(TAG_normal); - const XMLSize_t lineCount = lineNodes->getLength(); - - for (XMLSize_t j = 0; j < lineCount; ++j) { - xercesc::DOMNode* normNode = lineNodes->item(j); - xercesc::DOMElement* normElement = dynamic_cast(normNode); - - int id = getIntAttr(normElement, ATTR_id); - double x = getDoubleAttr(normElement, ATTR_x); - double y = getDoubleAttr(normElement, ATTR_y); - double z = getDoubleAttr(normElement, ATTR_z); - - if (id + normsOffset > norms.size()) - norms.resize(id + normsOffset); - - GCS::Normal p; - p.x = x; - p.y = y; - p.z = z; - norms[id + normsOffset -1] = p; - } - } - - // Displacements - xercesc::DOMNodeList* dispsNodes = elementRoot->getElementsByTagName(TAG_displacements); - const XMLSize_t dispsCount = dispsNodes->getLength(); - - for (XMLSize_t i = 0; i < dispsCount; ++i) { - xercesc::DOMNode* dispsNode = dispsNodes->item(i); - xercesc::DOMElement* dispsElement = dynamic_cast(dispsNode); - - xercesc::DOMNodeList* lineNodes = dispsElement->getElementsByTagName(TAG_displacement); - const XMLSize_t lineCount = lineNodes->getLength(); - - for (XMLSize_t j = 0; j < lineCount; ++j) { - xercesc::DOMNode* dispNode = lineNodes->item(j); - xercesc::DOMElement* dispElement = dynamic_cast(dispNode); - - int id = getIntAttr(dispElement, ATTR_id); - double x = getDoubleAttr(dispElement, ATTR_x); - double y = getDoubleAttr(dispElement, ATTR_y); - double z = getDoubleAttr(dispElement, ATTR_z); - - if (id + dispsOffset > disps.size()) - disps.resize(id + dispsOffset); - - // Memory allocation!! - int varStartIndex = variables.size(); - variables.push_back(new double(x)); - variables.push_back(new double(y)); - variables.push_back(new double(z)); - - GCS::Displacement p; - p.x = variables[varStartIndex+0]; - p.y = variables[varStartIndex+1]; - p.z = variables[varStartIndex+2]; - disps[id + dispsOffset -1] = p; - } - } - - // Quaternions - xercesc::DOMNodeList* quatsNodes = elementRoot->getElementsByTagName(TAG_quaternions); - const XMLSize_t quatCount = quatsNodes->getLength(); - - for (XMLSize_t i = 0; i < quatCount; ++i) { - xercesc::DOMNode* quatsNode = quatsNodes->item(i); - xercesc::DOMElement* quatsElement = dynamic_cast(quatsNode); - - xercesc::DOMNodeList* quatNodes = quatsElement->getElementsByTagName(TAG_quaternion); - const XMLSize_t quatCount = quatNodes->getLength(); - - for (XMLSize_t j = 0; j < quatCount; ++j) { - xercesc::DOMNode* quatNode = quatNodes->item(j); - xercesc::DOMElement* quatElement = dynamic_cast(quatNode); - - int id = getIntAttr(quatElement, ATTR_id); - double a = getDoubleAttr(quatElement, ATTR_a); - double b = getDoubleAttr(quatElement, ATTR_b); - double c = getDoubleAttr(quatElement, ATTR_c); - double d = getDoubleAttr(quatElement, ATTR_d); - - if (id + quatsOffset > quats.size()) - quats.resize(id + quatsOffset); - - GCS::Quaternion q; - // Memory allocation!! - int varStartIndex = variables.size(); - variables.push_back(new double(a)); - variables.push_back(new double(b)); - variables.push_back(new double(c)); - variables.push_back(new double(d)); - - q.a = variables[varStartIndex+0]; - q.b = variables[varStartIndex+1]; - q.c = variables[varStartIndex+2]; - q.d = variables[varStartIndex+3]; - quats[id + quatsOffset -1] = q; - } - } - - // Solids - xercesc::DOMNodeList* solidsNodes = elementRoot->getElementsByTagName(TAG_solids); - const XMLSize_t solidCount = solidsNodes->getLength(); - - for (XMLSize_t i = 0; i < solidCount; ++i) { - xercesc::DOMNode* solidsNode = solidsNodes->item(i); - xercesc::DOMElement* solidsElement = dynamic_cast(solidsNode); - - xercesc::DOMNodeList* solidNodes = solidsElement->getElementsByTagName(TAG_solid); - const XMLSize_t solidCount = solidNodes->getLength(); - - for (XMLSize_t j = 0; j < solidCount; ++j) { - xercesc::DOMNode* solidNode = solidNodes->item(j); - xercesc::DOMElement* solidElement = dynamic_cast(solidNode); - - int id = getIntAttr(solidElement, ATTR_id); - int p = getIntAttr(solidElement, ATTR_point); - int n = getIntAttr(solidElement, ATTR_normal); - int d = getIntAttr(solidElement, ATTR_displacement); - int q = getIntAttr(solidElement, ATTR_quaternion); - - GCS::Solid s; - s.p = points[p-1]; - s.n = norms[n-1]; - s.d = disps[d-1]; - s.q = quats[q-1]; - - solids.push_back(s); - } - } - - - - // Constraints - xercesc::DOMNodeList* constraintsNodes = elementRoot->getElementsByTagName(TAG_constraints); - const XMLSize_t constraintsCount = constraintsNodes->getLength(); - - for (XMLSize_t i = 0; i < constraintsCount; ++i) { - xercesc::DOMNode* constraintsNode = constraintsNodes->item(i); - xercesc::DOMElement* constraintsElement = dynamic_cast(constraintsNode); - - xercesc::DOMNodeList* constraintNodes = constraintsElement->getElementsByTagName(TAG_constraint); - const XMLSize_t constraintCount = constraintNodes->getLength(); - - for (XMLSize_t j = 0; j < constraintCount; ++j) { - xercesc::DOMNode* constraintNode = constraintNodes->item(j); - xercesc::DOMElement* constraintElement = dynamic_cast(constraintNode); - - int id = getIntAttr(constraintElement, ATTR_id); - - const XMLCh* xmlch_kind = constraintElement->getAttribute(ATTR_kind); - - //PlaneParallel Assembly - if (xercesc::XMLString::equals(xmlch_kind, KIND_Parallel_AS)) { - int s1 = getIntAttr(constraintElement, ATTR_solid1); - int s2 = getIntAttr(constraintElement, ATTR_solid2); - int t = getIntAttr(constraintElement, ATTR_type); - - int paramStartIndex = parameters.size(); - parameters.push_back(new double(t)); - - GCS::Constraint *constr - = new GCS::ConstraintParralelFaceAS( solids[s1-1], solids[s2-1], - (GCS::ParallelType*) parameters[paramStartIndex] ); - constraints.push_back(constr); - } - //Distance Assembly - if (xercesc::XMLString::equals(xmlch_kind, KIND_Distance_AS)) { - int s1 = getIntAttr(constraintElement, ATTR_solid1); - int s2 = getIntAttr(constraintElement, ATTR_solid2); - double t = getDoubleAttr(constraintElement, ATTR_distance); - - int paramStartIndex = parameters.size(); - parameters.push_back(new double(t)); - - GCS::Constraint *constr - = new GCS::ConstraintFaceDistanceAS( solids[s1-1], solids[s2-1], parameters[paramStartIndex] ); - constraints.push_back(constr); - } - } - } - return true; - } - catch (xercesc::XMLException& e) { - char* message = xercesc::XMLString::transcode(e.getMessage()); - std::cout << "Error parsing file: " << message << std::endl; - xercesc::XMLString::release(&message); - } -} - -int InputParser::getIntAttr(xercesc::DOMElement* element, XMLCh* attr) -{ - const XMLCh* xmlch_val = element->getAttribute(attr); - return xercesc::XMLString::parseInt(xmlch_val); -} - -double InputParser::getDoubleAttr(xercesc::DOMElement* element, XMLCh* attr) -{ - const XMLCh* xmlch_val = element->getAttribute(attr); - xercesc::XMLDouble xmldouble_val(xmlch_val); - return xmldouble_val.getValue(); -} - diff --git a/src/Mod/Assembly/App/gcs3d/InputParser.h b/src/Mod/Assembly/App/gcs3d/InputParser.h deleted file mode 100644 index e6f71b1d5..000000000 --- a/src/Mod/Assembly/App/gcs3d/InputParser.h +++ /dev/null @@ -1,60 +0,0 @@ - -#include -#include - -#include "GCS.h" - -class InputParser -{ -public: - InputParser(); - ~InputParser(); - - bool readInputFileAS(std::string &fileName, - std::vector &variables, - std::vector ¶meters, - std::vector &points, - std::vector &normals, - std::vector &displacements, - std::vector &quaternions, - std::vector &solids, - std::vector &constraints); - - int getIntAttr(xercesc::DOMElement* element, XMLCh* attr); - double getDoubleAttr(xercesc::DOMElement* element, XMLCh* attr); -private: - xercesc::XercesDOMParser *m_InputFileParser; - - XMLCh* TAG_points; - XMLCh* TAG_constraints; - XMLCh* TAG_point; - XMLCh* TAG_constraint; - XMLCh* ATTR_id; - XMLCh* ATTR_x; - XMLCh* ATTR_y; - XMLCh* ATTR_z; - XMLCh* ATTR_point; - XMLCh* ATTR_kind; - XMLCh* ATTR_distance; - XMLCh* TAG_GCS_AS; - XMLCh* TAG_quaternions; - XMLCh* TAG_quaternion; - XMLCh* TAG_normals; - XMLCh* TAG_normal; - XMLCh* TAG_displacements; - XMLCh* TAG_displacement; - XMLCh* TAG_solids; - XMLCh* TAG_solid; - XMLCh* KIND_Parallel_AS; - XMLCh* KIND_Distance_AS; - XMLCh* ATTR_a; - XMLCh* ATTR_b; - XMLCh* ATTR_c; - XMLCh* ATTR_d; - XMLCh* ATTR_quaternion; - XMLCh* ATTR_type; - XMLCh* ATTR_normal; - XMLCh* ATTR_displacement; - XMLCh* ATTR_solid1; - XMLCh* ATTR_solid2; -}; diff --git a/src/Mod/Assembly/App/gcs3d/SubSystem.cpp b/src/Mod/Assembly/App/gcs3d/SubSystem.cpp deleted file mode 100644 index e6bb83212..000000000 --- a/src/Mod/Assembly/App/gcs3d/SubSystem.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#include -#include -#include "SubSystem.h" - -namespace GCS -{ - -// SubSystem -SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms) -: clist(clist_) -{ - MAP_pD_pD dummymap; - initialize(params, dummymap); -} - -SubSystem::SubSystem(std::vector &clist_, VEC_pD ¶ms, - MAP_pD_pD &reductionmap) -: clist(clist_) -{ - initialize(params, reductionmap); -} - -SubSystem::~SubSystem() -{ -} - -void SubSystem::initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap) -{ - csize = clist.size(); - - // tmpplist will contain the subset of parameters from params that are - // relevant for the constraints listed in clist - VEC_pD tmpplist; - { - SET_pD s1(params.begin(), params.end()); - SET_pD s2; - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); // ensure that the constraint points to the original parameters - VEC_pD constr_params = (*constr)->params(); - s2.insert(constr_params.begin(), constr_params.end()); - } - std::set_intersection(s1.begin(), s1.end(), s2.begin(), s2.end(), - std::back_inserter(tmpplist) ); - } - - plist.clear(); - MAP_pD_I rindex; - if (reductionmap.size() > 0) { - int i=0; - MAP_pD_I pindex; - for (VEC_pD::const_iterator itt=tmpplist.begin(); - itt != tmpplist.end(); itt++) { - MAP_pD_pD::const_iterator itr = reductionmap.find(*itt); - if (itr != reductionmap.end()) { - MAP_pD_I::const_iterator itp = pindex.find(itr->second); - if (itp == pindex.end()) { // the reduction target is not in plist yet, so add it now - plist.push_back(itr->second); - rindex[itr->first] = i; - pindex[itr->second] = i; - i++; - } - else // the reduction target is already in plist, just inform rindex - rindex[itr->first] = itp->second; - } - else if (pindex.find(*itt) == pindex.end()) { // not in plist yet, so add it now - plist.push_back(*itt); - pindex[*itt] = i; - i++; - } - } - } - else - plist = tmpplist; - - psize = plist.size(); - pvals.resize(psize); - pmap.clear(); - for (int j=0; j < psize; j++) { - pmap[plist[j]] = &pvals[j]; - pvals[j] = *plist[j]; - } - for (MAP_pD_I::const_iterator itr=rindex.begin(); itr != rindex.end(); ++itr) - pmap[itr->first] = &pvals[itr->second]; - - c2p.clear(); - p2c.clear(); - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); // ensure that the constraint points to the original parameters - VEC_pD constr_params_orig = (*constr)->params(); - SET_pD constr_params; - for (VEC_pD::const_iterator p=constr_params_orig.begin(); - p != constr_params_orig.end(); ++p) { - MAP_pD_pD::const_iterator pmapfind = pmap.find(*p); - if (pmapfind != pmap.end()) - constr_params.insert(pmapfind->second); - } - for (SET_pD::const_iterator p=constr_params.begin(); - p != constr_params.end(); ++p) { -// jacobi.set(*constr, *p, 0.); - c2p[*constr].push_back(*p); - p2c[*p].push_back(*constr); - } -// (*constr)->redirectParams(pmap); // redirect parameters to pvec - } -} - -void SubSystem::redirectParams() -{ - // copying values to pvals - for (MAP_pD_pD::const_iterator p=pmap.begin(); - p != pmap.end(); ++p) - *(p->second) = *(p->first); - - // redirect constraints to point to pvals - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - (*constr)->revertParams(); // this line will normally not be necessary - (*constr)->redirectParams(pmap); - } -} - -void SubSystem::revertParams() -{ - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) - (*constr)->revertParams(); -} - -void SubSystem::getParamMap(MAP_pD_pD &pmapOut) -{ - pmapOut = pmap; -} - -void SubSystem::getParamList(VEC_pD &plistOut) -{ - plistOut = plist; -} - -void SubSystem::getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut) -{ - if (xOut.size() != int(params.size())) - xOut.setZero(params.size()); - - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); - if (pmapfind != pmap.end()) - xOut[j] = *(pmapfind->second); - } -} - -void SubSystem::getParams(Eigen::VectorXd &xOut) -{ - if (xOut.size() != psize) - xOut.setZero(psize); - - for (int i=0; i < psize; i++) - xOut[i] = pvals[i]; -} - -void SubSystem::setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn) -{ - assert(xIn.size() == int(params.size())); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); - if (pmapfind != pmap.end()) - *(pmapfind->second) = xIn[j]; - } -} - -void SubSystem::setParams(Eigen::VectorXd &xIn) -{ - assert(xIn.size() == psize); - for (int i=0; i < psize; i++) - pvals[i] = xIn[i]; -} - -double SubSystem::error() -{ - double err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr) { - double tmp = (*constr)->error(); - err += tmp*tmp; - } - err *= 0.5; - return err; -} - -void SubSystem::calcResidual(Eigen::VectorXd &r) -{ - assert(r.size() == csize); - - int i=0; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { - r[i] = (*constr)->error(); - } -} - -void SubSystem::calcResidual(Eigen::VectorXd &r, double &err) -{ - assert(r.size() == csize); - - int i=0; - err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { - r[i] = (*constr)->error(); - err += r[i]*r[i]; - } - err *= 0.5; -} - -/* -void SubSystem::calcJacobi() -{ - assert(grad.size() != xsize); - - for (MAP_pD_pD::const_iterator param=pmap.begin(); - param != pmap.end(); ++param) { - // assert(p2c.find(param->second) != p2c.end()); - std::vector constrs=p2c[param->second]; - for (std::vector::const_iterator constr = constrs.begin(); - constr != constrs.end(); ++constr) - jacobi.set(*constr,param->second,(*constr)->grad(param->second)); - } -} -*/ - -void SubSystem::calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi) -{ - jacobi.setZero(csize, params.size()); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); - if (pmapfind != pmap.end()) - for (int i=0; i < csize; i++) - jacobi(i,j) = clist[i]->grad(pmapfind->second); - } -} - -void SubSystem::calcJacobi(Eigen::MatrixXd &jacobi) -{ - calcJacobi(plist, jacobi); -} - -void SubSystem::calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad) -{ - assert(grad.size() == int(params.size())); - - grad.setZero(); - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator - pmapfind = pmap.find(params[j]); - if (pmapfind != pmap.end()) { - // assert(p2c.find(pmapfind->second) != p2c.end()); - std::vector constrs=p2c[pmapfind->second]; - for (std::vector::const_iterator constr = constrs.begin(); - constr != constrs.end(); ++constr) - grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second); - } - } -} - -void SubSystem::calcGrad(Eigen::VectorXd &grad) -{ - calcGrad(plist, grad); -} - -double SubSystem::maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir) -{ - assert(xdir.size() == int(params.size())); - - MAP_pD_D dir; - for (int j=0; j < int(params.size()); j++) { - MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); - if (pmapfind != pmap.end()) - dir[pmapfind->second] = xdir[j]; - } - - double alpha=1e10; - for (std::vector::iterator constr=clist.begin(); - constr != clist.end(); ++constr) - alpha = (*constr)->maxStep(dir, alpha); - - return alpha; -} - -double SubSystem::maxStep(Eigen::VectorXd &xdir) -{ - return maxStep(plist, xdir); -} - -void SubSystem::applySolution() -{ - for (MAP_pD_pD::const_iterator it=pmap.begin(); - it != pmap.end(); ++it) - *(it->first) = *(it->second); -} - -void SubSystem::analyse(Eigen::MatrixXd &J, Eigen::MatrixXd &ker, Eigen::MatrixXd &img) -{ -} - -void SubSystem::report() -{ -} - -void SubSystem::printResidual() -{ - Eigen::VectorXd r(csize); - int i=0; - double err = 0.; - for (std::vector::const_iterator constr=clist.begin(); - constr != clist.end(); ++constr, i++) { - r[i] = (*constr)->error(); - err += r[i]*r[i]; - } - err *= 0.5; - std::cout << "Residual r = " << r.transpose() << std::endl; - std::cout << "Residual err = " << err << std::endl; -} - - -} //namespace GCS diff --git a/src/Mod/Assembly/App/gcs3d/SubSystem.h b/src/Mod/Assembly/App/gcs3d/SubSystem.h deleted file mode 100644 index 1e313e35e..000000000 --- a/src/Mod/Assembly/App/gcs3d/SubSystem.h +++ /dev/null @@ -1,89 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#ifndef FREEGCS_SUBSYSTEM_H -#define FREEGCS_SUBSYSTEM_H - -#undef min -#undef max - -#include -#include "Constraints.h" - -namespace GCS -{ - - class SubSystem - { - private: - int psize, csize; - std::vector clist; - VEC_pD plist; // pointers to the original parameters - MAP_pD_pD pmap; // redirection map from the original parameters to pvals - VEC_D pvals; // current variables vector (psize) -// JacobianMatrix jacobi; // jacobi matrix of the residuals - std::map c2p; // constraint to parameter adjacency list - std::map > p2c; // parameter to constraint adjacency list - void initialize(VEC_pD ¶ms, MAP_pD_pD &reductionmap); // called by the constructors - public: - SubSystem(std::vector &clist_, VEC_pD ¶ms); - SubSystem(std::vector &clist_, VEC_pD ¶ms, - MAP_pD_pD &reductionmap); - ~SubSystem(); - - int pSize() { return psize; }; - int cSize() { return csize; }; - - void redirectParams(); - void revertParams(); - - void getParamMap(MAP_pD_pD &pmapOut); - void getParamList(VEC_pD &plistOut); - - void getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut); - void getParams(Eigen::VectorXd &xOut); - void setParams(VEC_pD ¶ms, Eigen::VectorXd &xIn); - void setParams(Eigen::VectorXd &xIn); - - double error(); - void calcResidual(Eigen::VectorXd &r); - void calcResidual(Eigen::VectorXd &r, double &err); - void calcJacobi(VEC_pD ¶ms, Eigen::MatrixXd &jacobi); - void calcJacobi(Eigen::MatrixXd &jacobi); - void calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad); - void calcGrad(Eigen::VectorXd &grad); - - double maxStep(VEC_pD ¶ms, Eigen::VectorXd &xdir); - double maxStep(Eigen::VectorXd &xdir); - - void applySolution(); - void analyse(Eigen::MatrixXd &J, Eigen::MatrixXd &ker, Eigen::MatrixXd &img); - void report(); - -void printResidual(); - }; - - double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir); - -} //namespace GCS - -#endif // FREEGCS_SUBSYSTEM_H diff --git a/src/Mod/Assembly/App/gcs3d/Util.h b/src/Mod/Assembly/App/gcs3d/Util.h deleted file mode 100644 index 18eacf9ad..000000000 --- a/src/Mod/Assembly/App/gcs3d/Util.h +++ /dev/null @@ -1,47 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ - -#ifndef FREEGCS_UTIL_H -#define FREEGCS_UTIL_H - -#include -#include -#include - -namespace GCS -{ - typedef std::vector VEC_pD; - typedef std::vector VEC_D; - typedef std::vector VEC_I; - typedef std::map MAP_pD_pD; - typedef std::map MAP_pD_D; - typedef std::map MAP_pD_I; - typedef std::set SET_pD; - typedef std::set SET_I; - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -} //namespace GCS - -#endif // FREEGCS_UTIL_H diff --git a/src/Mod/Assembly/App/gcs3d/main.cpp b/src/Mod/Assembly/App/gcs3d/main.cpp deleted file mode 100644 index a68ac1f22..000000000 --- a/src/Mod/Assembly/App/gcs3d/main.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include -#include - -#include "Geo.h" -#include "Constraints.h" -#include "InputParser.h" -#include "OutputWriter.h" - -#include - -using namespace std; -using namespace Eigen; - -Vector3d Place(GCS::Point p, GCS::Quaternion q, GCS::Displacement di) { - - double a=*q.a, b=*q.b, c=*q.c, d=*q.d; - double norm = sqrt(pow(a,2)+pow(b,2)+pow(c,2)+pow(d,2)); - double x=a/norm, y=b/norm, z=c/norm, w=d/norm; - - Matrix3d rot; - rot(0,0) = 1-2*(pow(y,2)+pow(z,2)); - rot(0,1) = -2.0*w*z + 2.0*x*y; - rot(0,2) = 2.0*w*y + 2.0*x*z; - rot(1,0) = 2.0*w*z + 2.0*x*y; - rot(1,1) = 1-2*(pow(x,2)+pow(z,2)); - rot(1,2) = -2.0*w*x + 2.0*y*z; - rot(2,0) = -2.0*w*y + 2.0*x*z; - rot(2,1) = 2.0*w*x + 2.0*y*z; - rot(2,2) = 1-2*(pow(x,2)+pow(y,2)); - - Vector3d v(p.x, p.y, p.z); - Vector3d n = rot*v; - - if (di.x != NULL) { - n(0) += *di.x; - n(1) += *di.y; - n(2) += *di.z; - } - - return n; -} - -std::string Vec(Vector3d vec) { - - std::stringstream s; - s<<"Base.Vector(" << vec(0) << ", " << vec(1) << ", " << vec(2) <<")"; - return s.str(); -} - - -int main(int argc, char **argv) { - - if (argc<3) return 1; - - cout << endl << "loading file: " << argv[1]; - cout << endl << "generating output: " << argv[2] << endl << endl; - - vector variables; - vector parameters; - vector points; - vector norms; - vector disps; - vector quats; - vector solids; - vector constraints; - - string inputfile(argv[1]); - string outputfile(argv[2]); - - bool d2,d3; - - InputParser parser; - d3 = parser.readInputFileAS(inputfile, variables, parameters, points, norms, disps, quats, solids, constraints); - - cout << "Variables: " << variables.size() << endl << "parameters: " << parameters.size() << endl; - cout << "Points: " << points.size() << endl << "Normals: " << norms.size() << endl; - cout << "Displacements: " << disps.size() << endl; - cout << "Quaternions: " << quats.size() << endl; - cout << "Solids: " << solids.size() << endl; - cout << "Constraints: " << constraints.size() << endl; - - //init output writing - remove(outputfile.c_str()); - ofstream file; - file.open (outputfile.c_str()); - if (!file.is_open()) return 0; - //import all needed modules - file << "import Part" << endl << "from FreeCAD import Base" << endl << endl; - //open new document - file << "App.newDocument(\"solver\")" << endl << endl; - GCS::Displacement emd; - for (int i=0; i -#include - -using namespace Eigen; - -// minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 ) -// it returns the solution in x, the row-space of A in Y, and the null space of A in Z -int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c, - VectorXd &x, MatrixXd &Y, MatrixXd &Z) -{ - FullPivHouseholderQR qrAT(A.transpose()); - MatrixXd Q = qrAT.matrixQ (); - - size_t params_num = qrAT.rows(); - size_t constr_num = qrAT.cols(); - size_t rank = qrAT.rank(); - - if (rank != constr_num || constr_num > params_num) - return -1; - - // A^T = Q*R*P^T = Q1*R1*P^T - // Q = [Q1,Q2], R=[R1;0] - // Y = Q1 * inv(R^T) * P^T - // Z = Q2 - Y = qrAT.matrixQR().topRows(constr_num) - .triangularView() - .transpose() - .solve(Q.leftCols(rank)) - * qrAT.colsPermutation().transpose(); - if (params_num == rank) - x = - Y * c; - else { - Z = Q.rightCols(params_num-rank); - - MatrixXd ZTHZ = Z.transpose() * H * Z; - VectorXd rhs = Z.transpose() * (H * Y * c - g); - - VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs); - - x = - Y * c + Z * y; - } - - return 0; -} diff --git a/src/Mod/Assembly/App/gcs3d/qp_eq.h b/src/Mod/Assembly/App/gcs3d/qp_eq.h deleted file mode 100644 index f05ca6d52..000000000 --- a/src/Mod/Assembly/App/gcs3d/qp_eq.h +++ /dev/null @@ -1,25 +0,0 @@ -/*************************************************************************** - * Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 * - * * - * This file is part of the FreeCAD CAx development system. * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Library General Public * - * License as published by the Free Software Foundation; either * - * version 2 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 Library General Public License for more details. * - * * - * You should have received a copy of the GNU Library General Public * - * License along with this library; see the file COPYING.LIB. If not, * - * write to the Free Software Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307, USA * - * * - ***************************************************************************/ -#include - -int qp_eq(Eigen::MatrixXd &H, Eigen::VectorXd &g, Eigen::MatrixXd &A, Eigen::VectorXd &c, - Eigen::VectorXd &x, Eigen::MatrixXd &Y, Eigen::MatrixXd &Z); diff --git a/src/Mod/Assembly/Gui/CMakeLists.txt b/src/Mod/Assembly/Gui/CMakeLists.txt index b28a58e02..133be2982 100644 --- a/src/Mod/Assembly/Gui/CMakeLists.txt +++ b/src/Mod/Assembly/Gui/CMakeLists.txt @@ -6,6 +6,7 @@ endif(MSVC) include_directories( ${CMAKE_SOURCE_DIR}/src + ${CMAKE_SOURCE_DIR}/src/Mod/Assembly/App ${CMAKE_CURRENT_BINARY_DIR} ${Boost_INCLUDE_DIRS} ${COIN_INCLUDE_DIR} @@ -15,7 +16,7 @@ include_directories( ${PYTHON_INCLUDE_DIRS} ${COIN3D_INCLUDE_DIRS} ${XercesC_INCLUDE_DIRS} - #${ODE_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ) set(AssemblyGui_LIBS diff --git a/src/Mod/Assembly/Gui/Command.cpp b/src/Mod/Assembly/Gui/Command.cpp index 4af10a38c..f655e857d 100644 --- a/src/Mod/Assembly/Gui/Command.cpp +++ b/src/Mod/Assembly/Gui/Command.cpp @@ -77,7 +77,7 @@ void CmdAssemblyAddNewPart::activated(int iMsg) doCommand(Doc,"App.activeDocument().addObject('Assembly::ItemPart','%s')",PartName.c_str()); if(dest){ std::string fatherName = dest->getNameInDocument(); - doCommand(Doc,"App.activeDocument().%s.Items = App.activeDocument().%s.Items + [App.activeDocument().%s] ",fatherName.c_str(),fatherName.c_str(),PartName.c_str()); + doCommand(Doc,"App.activeDocument().%s.addPart(App.activeDocument().%s) ",fatherName.c_str(),PartName.c_str()); } Command::addModule(App,"PartDesign"); Command::addModule(Gui,"PartDesignGui"); @@ -139,7 +139,7 @@ void CmdAssemblyAddNewComponent::activated(int iMsg) doCommand(Doc,"App.activeDocument().addObject('Assembly::ItemAssembly','%s')",CompName.c_str()); if(dest){ std::string fatherName = dest->getNameInDocument(); - doCommand(Doc,"App.activeDocument().%s.Items = App.activeDocument().%s.Items + [App.activeDocument().%s] ",fatherName.c_str(),fatherName.c_str(),CompName.c_str()); + doCommand(Doc,"App.activeDocument().%s.addComponent(App.activeDocument().%s) ",fatherName.c_str(), CompName.c_str()); } } diff --git a/src/Mod/Assembly/Gui/CommandConstraints.cpp b/src/Mod/Assembly/Gui/CommandConstraints.cpp index 6ef91bffc..349950c56 100644 --- a/src/Mod/Assembly/Gui/CommandConstraints.cpp +++ b/src/Mod/Assembly/Gui/CommandConstraints.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,17 @@ bool getConstraintPrerequisits(Assembly::ItemAssembly **Asm,Assembly::Constraint return false; } + +std::string asSubLinkString(Assembly::ItemPart* part, std::string element) { + std::string buf; + buf += "(App.ActiveDocument."; + buf += part->getNameInDocument(); + buf += ",['"; + buf += element; + buf += "'])"; + return buf; +} + //=========================================================================== DEF_STD_CMD(CmdAssemblyConstraintAxle); @@ -108,11 +120,26 @@ void CmdAssemblyConstraintAxle::activated(int iMsg) // retrive the standard objects needed if(getConstraintPrerequisits(&Asm,&ConstGrp)) return; + + std::vector objs = Gui::Selection().getSelectionEx(); + if(objs.size() != 2) { + Base::Console().Message("you must select two geometries on two diffrent parts\n"); + return; + }; + + Assembly::ItemPart* part1 = Asm->getContainingPart(objs[0].getObject()); + Assembly::ItemPart* part2 = Asm->getContainingPart(objs[1].getObject()); + if(!part1 || !part2) { + Base::Console().Message("The selected objects need to belong to the active assembly\n"); + return; + }; openCommand("Insert Constraint Axle"); std::string ConstrName = getUniqueObjectName("Axle"); - doCommand(Doc,"App.activeDocument().addObject('Assembly::ItemPart','%s')",ConstrName.c_str()); - doCommand(Doc,"App.activeDocument().%s.Constraints = App.activeDocument().%s.Constraints + [App.activeDocument().ActiveObject]",ConstGrp->getNameInDocument(),ConstGrp->getNameInDocument()); + doCommand(Doc,"App.activeDocument().addObject('Assembly::ConstraintAxis','%s')",ConstrName.c_str()); + doCommand(Doc,"App.activeDocument().ActiveObject.First = %s", asSubLinkString(part1, objs[0].getSubNames()[0]).c_str()); + doCommand(Doc,"App.activeDocument().ActiveObject.Second = %s", asSubLinkString(part2, objs[1].getSubNames()[0]).c_str()); + doCommand(Doc,"App.activeDocument().%s.addConstraint(App.activeDocument().ActiveObject)",ConstGrp->getNameInDocument()); }