basic infrastructure

This commit is contained in:
Stefan Tröger 2013-04-25 11:58:21 +02:00 committed by Stefan Tröger
parent 638cfcc2cf
commit 474fbbcb3e
42 changed files with 488 additions and 3460 deletions

View File

@ -27,6 +27,7 @@
#define BASE_EXCEPTION_H
#include <exception>
#include <stdexcept>
#include <string>
#include <signal.h>
#include "FileInfo.h"

View File

@ -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}
)

View File

@ -1,7 +1,8 @@
/***************************************************************************
* Copyright (c) 2012 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* 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 <Standard_Failure.hxx>
#include <TopTools_IndexedMapOfShape.hxx>
#include <TopExp.hxx>
#include <TopoDS.hxx>
#include <GeomAbs_SurfaceType.hxx>
#include <gp_Pln.hxx>
#include <GeomAbs_CurveType.hxx>
#include <BRep_Tool.hxx>
#include <TopoDS_Vertex.hxx>
#include <gp_Pnt.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <TopoDS_Edge.hxx>
#include <gp_Cylinder.hxx>
#include <BRepAdaptor_Surface.hxx>
#include <TopoDS_Face.hxx>
#include <Base/Placement.h>
#include <Base/Console.h>
#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<Assembly::ItemPart*>(First.getValue());
if(!part1->m_part) {
part1->m_part = solver->createPart(part1->Placement.getValue(), part1->Uid.getValueStr());
part1->m_part->connectSignal<dcm::recalculated>(boost::bind(&ItemPart::setCalculatedPlacement, part1, _1));
}
Assembly::ItemPart* part2 = static_cast<Assembly::ItemPart*>(Second.getValue());
if(!part2->m_part) {
part2->m_part = solver->createPart(part2->Placement.getValue(), part2->Uid.getValueStr());
part2->m_part->connectSignal<dcm::recalculated>(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;
};
}
}

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2012 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -27,6 +28,10 @@
#include <App/PropertyLinks.h>
#include <App/DocumentObject.h>
#include <TopoDS_Shape.hxx>
#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<Constraint3D> m_constraint;
boost::shared_ptr<Geometry3D> 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> solver);
};
} //namespace Assembly

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2012 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2012 Juergen Riegel <FreeCAD@juergen-riegel.net>
* 2013 Stefan Tröger <stefantroeger@gmx.net>
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -26,9 +27,12 @@
#endif
#include <Base/Placement.h>
#include <Base/Console.h>
#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);
}
}

View File

@ -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> solver);
};
} //namespace Assembly

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2010 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -26,8 +27,12 @@
#endif
#include <Base/Placement.h>
#include <Base/Console.h>
#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<App::DocumentObject*>::iterator iter;
std::vector<App::DocumentObject*> vec = getInList();
for(iter it = vec.begin(); it!=vec.end(); it++) {
if( (*it)->getTypeId() == ItemAssembly::getClassTypeId() ) {
assembly = static_cast<ItemAssembly*>(*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;
}
}
}

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2010 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -26,7 +27,10 @@
#include <App/PropertyLinks.h>
#include <App/DocumentObject.h>
#include <App/FeaturePython.h>
#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<Solver> m_solver;
public:
ConstraintGroup();
void addConstraint(Constraint* c);
PyObject *getPyObject(void);
App::PropertyLinkList Constraints;

View File

@ -13,5 +13,12 @@
<Author Licence="LGPL" Name="Juergen Riegel" EMail="FreeCAD@juergen-riegel.net" />
<UserDocu>Base class of all objects in Assembly</UserDocu>
</Documentation>
<Methode Name="addConstraint">
<Documentation>
<UserDocu>Adds a constraint object to the constraint group</UserDocu>
</Documentation>
</Methode>
</PythonExport>
</GenerateModel>

View File

@ -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("<ConstraintGroup object>");
}
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<Assembly::ConstraintPy*>(pcObj)->getConstraintPtr();
this->getConstraintGroupPtr()->addConstraint(c);
}
Py_Return;
}
PyObject *ConstraintGroupPy::getCustomAttributes(const char* /*attr*/) const
{

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2010 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -27,10 +28,11 @@
#include <Base/Placement.h>
#include <Base/Uuid.h>
#include <Base/Console.h>
#include "Item.h"
#include "ItemPy.h"
#include <Standard_Failure.hxx>
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;
}

View File

@ -1,5 +1,6 @@
/***************************************************************************
* Copyright (c) 2010 Juergen Riegel <FreeCAD@juergen-riegel.net> *
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
@ -28,7 +29,6 @@
#include <App/GeoFeature.h>
#include <TopoDS_Shape.hxx>
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

View File

@ -28,9 +28,11 @@
#endif
#include <Base/Placement.h>
#include <Base/Console.h>
#include <Base/Exception.h>
#include "ItemAssembly.h"
#include <ItemAssemblyPy.h>
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<Solver>(m_solver->createSubsystem());
assembly->m_solver->setTransformation(assembly->Placement.getValue());
}
ItemPart* ItemAssembly::getContainingPart(App::DocumentObject* obj) {
typedef std::vector<App::DocumentObject*>::const_iterator iter;
const std::vector<App::DocumentObject*>& vector = Items.getValues();
for(iter it=vector.begin(); it != vector.end(); it++) {
if( (*it)->getTypeId() == Assembly::ItemPart::getClassTypeId() ) {
if(static_cast<Assembly::ItemPart*>(*it)->holdsObject(obj))
return static_cast<Assembly::ItemPart*>(*it);
}
else if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) {
Assembly::ItemPart* part = static_cast<Assembly::ItemAssembly*>(*it)->getContainingPart(obj);
if(part)
return part;
}
};
return NULL;
}
}

View File

@ -25,15 +25,17 @@
#define ItemAssembly_ItemAssembly_H
#include <App/PropertyStandard.h>
#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<Solver> m_solver;
};
} //namespace Assembly

View File

@ -13,5 +13,15 @@
<Author Licence="LGPL" Name="Juergen Riegel" EMail="FreeCAD@juergen-riegel.net" />
<UserDocu>Base class of all objects in Assembly</UserDocu>
</Documentation>
<Methode Name="addPart">
<Documentation>
<UserDocu>Adds a Assembly::ItemPart object to the assembly</UserDocu>
</Documentation>
</Methode>
<Methode Name="addComponent">
<Documentation>
<UserDocu>Adds a Assembly::ItemAssembly object to the assembly</UserDocu>
</Documentation>
</Methode>
</PythonExport>
</GenerateModel>

View File

@ -6,6 +6,7 @@
// inclusion of the generated files (generated out of ItemAssemblyPy.xml)
#include "ItemAssemblyPy.h"
#include "ItemAssemblyPy.cpp"
#include <ItemPartPy.h>
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<Assembly::ItemPartPy*>(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<Assembly::ItemAssemblyPy*>(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

View File

@ -26,9 +26,20 @@
#endif
#include <Base/Placement.h>
#include <Base/Console.h>
#include "ItemPart.h"
#include <Mod/Part/App/PartFeature.h>
#include <Mod/Part/App/BodyBase.h>
#include <ItemPartPy.h>
#include <TopoDS.hxx>
#include <GeomAbs_SurfaceType.hxx>
#include <BRepAdaptor_Surface.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <TopoDS_Edge.hxx>
#include <TopoDS_Vertex.hxx>
#include <BRep_Tool.hxx>
#include <GeomAbs_CurveType.hxx>
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<Part::BodyBase*>(Model.getValue());
const std::vector<App::DocumentObject*>& 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<Base::Placement>(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<Geometry3D> 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<Part::Feature*>(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;
}
}

View File

@ -26,6 +26,7 @@
#include <App/PropertyLinks.h>
#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<Part3D> m_part;
virtual boost::shared_ptr<Geometry3D> getGeometry3D(const char* Type );
void setCalculatedPlacement( boost::shared_ptr<Part3D> part );
};
} //namespace Assembly

View File

@ -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})

View File

@ -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 <cmath>
#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

View File

@ -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 <Eigen/Dense>
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

View File

@ -1,28 +0,0 @@
<GCS_AS>
<!-- This example constraint two faces to each other -->
<points>
<point id="1" x="-58" y="28" z="0"></point>
<point id="2" x="-58" y="28" z="50"></point>
</points>
<normals>
<normal id="1" x="0" y="1" z="0"></normal>
<normal id="2" x="0" y="0" z="1"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="100" z="0"></displacement>
<displacement id="2" x="0" y="200" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="2" quaternion="2"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="2" type="2"></constraint>
<constraint id="2" kind="Distance_AS" solid1="2" solid2="2" distance="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,61 +0,0 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other so that no rotational degree of freedom is left.
Body one has three constraints to body two and body two three constraints to body 3.
Note that this system is overconstrained, but this is the case that would appear when added 3 mate
constraints to 2 bodys as needed to fix all translational dofs.-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="12" y="1" z="3"></displacement>
<displacement id="2" x="4" y="1" z="-2"></displacement>
<displacement id="3" x="-2" y="7" z="5"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="4" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="6" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,55 +0,0 @@
<GCS_AS>
<!-- This example constraints 2 bodys to each other by one mate constraint (1 rot and 1 trans constraint)
Note that all values left the same as in the first examples but many are not used-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="12" y="1" z="3"></displacement>
<displacement id="2" x="4" y="1" z="-2"></displacement>
<displacement id="3" x="-2" y="7" z="5"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,64 +0,0 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="2" b="-3" c="4" d="1"></quaternion>
<quaternion id="2" a="0" b="2" c="0" d="1"></quaternion>
<quaternion id="3" a="1" b="2" c="3" d="2"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,61 +0,0 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="0" z="0"></normal>
<normal id="2" x="0" y="1" z="0"></normal>
<normal id="3" x="0" y="0" z="1"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="4" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="10" z="0"></displacement>
<displacement id="2" x="0" y="20" z="0"></displacement>
<displacement id="3" x="0" y="30" z="0"></displacement>
<displacement id="4" x="0" y="40" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="1" normal="1" displacement="2" quaternion="2"></solid>
<solid id="5" point="2" normal="2" displacement="2" quaternion="2"></solid>
<solid id="6" point="3" normal="3" displacement="2" quaternion="2"></solid>
<solid id="7" point="1" normal="1" displacement="3" quaternion="3"></solid>
<solid id="8" point="2" normal="2" displacement="3" quaternion="3"></solid>
<solid id="9" point="3" normal="3" displacement="3" quaternion="3"></solid>
<solid id="10" point="1" normal="1" displacement="4" quaternion="4"></solid>
<solid id="11" point="2" normal="2" displacement="4" quaternion="4"></solid>
<solid id="12" point="3" normal="3" displacement="4" quaternion="4"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="5" solid2="1" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="5" solid2="1" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="6" solid2="2" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="6" solid2="2" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="4" solid2="3" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="4" solid2="3" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="8" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="8" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="9" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="9" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="7" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="7" solid2="6" distance="0"></constraint>
<constraint id="13" kind="Parallel_AS" solid1="11" solid2="7" type="0"></constraint>
<constraint id="14" kind="Distance_AS" solid1="11" solid2="7" distance="0"></constraint>
<constraint id="15" kind="Parallel_AS" solid1="12" solid2="8" type="0"></constraint>
<constraint id="16" kind="Distance_AS" solid1="12" solid2="8" distance="0"></constraint>
<constraint id="17" kind="Parallel_AS" solid1="10" solid2="9" type="0"></constraint>
<constraint id="18" kind="Distance_AS" solid1="10" solid2="9" distance="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,64 +0,0 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="5" z="2"></normal>
<normal id="2" x="3" y="1" z="-4"></normal>
<normal id="3" x="-22" y="10" z="-14"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="4" y="-2" z="3"></normal>
<normal id="5" x="-5" y="-1" z="6"></normal>
<normal id="6" x="-9" y="-39" z="-14"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="-3" y="3" z="0"></normal>
<normal id="8" x="3" y="3" z="-6"></normal>
<normal id="9" x="-18" y="-18" z="-18"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

View File

@ -1,64 +0,0 @@
<GCS_AS>
<!-- This example constraints 3 bodys to each other by 6 mate constraint (6 rot and 6 trans constraint)-->
<points>
<point id="1" x="1" y="0" z="0"></point>
<point id="2" x="2" y="1" z="1"></point>
<point id="3" x="3" y="2" z="2"></point>
<point id="4" x="7" y="6" z="7"></point>
<point id="5" x="8" y="7" z="8"></point>
<point id="6" x="9" y="8" z="9"></point>
<point id="7" x="3" y="-1" z="7"></point>
<point id="8" x="4" y="-2" z="8"></point>
<point id="9" x="5" y="-3" z="9"></point>
</points>
<normals>
<!-- norm 1 is perpendicular to norm 2 and 3 perpendicular to both-->
<normal id="1" x="1" y="0" z="0"></normal>
<normal id="2" x="0" y="1" z="0"></normal>
<normal id="3" x="0" y="0" z="1"></normal>
<!-- norm 4 is perpendicular to norm 5 and 6 perpendicular to both -->
<normal id="4" x="0" y="1" z="0"></normal>
<normal id="5" x="0" y="0" z="1"></normal>
<normal id="6" x="1" y="0" z="0"></normal>
<!-- norm 7 is perpendicular to norm 8 and 9 perpendicular to both -->
<normal id="7" x="0" y="0" z="1"></normal>
<normal id="8" x="1" y="0" z="0"></normal>
<normal id="9" x="0" y="1" z="0"></normal>
</normals>
<quaternions>
<quaternion id="1" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="2" a="0" b="0" c="0" d="1"></quaternion>
<quaternion id="3" a="0" b="0" c="0" d="1"></quaternion>
</quaternions>
<displacements>
<displacement id="1" x="0" y="0" z="0"></displacement>
<displacement id="2" x="0" y="0" z="0"></displacement>
<displacement id="3" x="0" y="0" z="0"></displacement>
</displacements>
<solids>
<solid id="1" point="1" normal="1" displacement="1" quaternion="1"></solid>
<solid id="2" point="2" normal="2" displacement="1" quaternion="1"></solid>
<solid id="3" point="3" normal="3" displacement="1" quaternion="1"></solid>
<solid id="4" point="4" normal="4" displacement="2" quaternion="2"></solid>
<solid id="5" point="5" normal="5" displacement="2" quaternion="2"></solid>
<solid id="6" point="6" normal="6" displacement="2" quaternion="2"></solid>
<solid id="7" point="7" normal="7" displacement="3" quaternion="3"></solid>
<solid id="8" point="8" normal="8" displacement="3" quaternion="3"></solid>
<solid id="9" point="9" normal="9" displacement="3" quaternion="3"></solid>
</solids>
<constraints>
<constraint id="1" kind="Parallel_AS" solid1="1" solid2="4" type="0"></constraint>
<constraint id="2" kind="Distance_AS" solid1="1" solid2="4" distance="0"></constraint>
<constraint id="3" kind="Parallel_AS" solid1="2" solid2="5" type="0"></constraint>
<constraint id="4" kind="Distance_AS" solid1="2" solid2="5" distance="0"></constraint>
<constraint id="5" kind="Parallel_AS" solid1="3" solid2="6" type="0"></constraint>
<constraint id="6" kind="Distance_AS" solid1="3" solid2="6" distance="0"></constraint>
<constraint id="7" kind="Parallel_AS" solid1="7" solid2="4" type="0"></constraint>
<constraint id="8" kind="Distance_AS" solid1="7" solid2="4" distance="0"></constraint>
<constraint id="9" kind="Parallel_AS" solid1="8" solid2="5" type="0"></constraint>
<constraint id="10" kind="Distance_AS" solid1="8" solid2="5" distance="0"></constraint>
<constraint id="11" kind="Parallel_AS" solid1="9" solid2="6" type="0"></constraint>
<constraint id="12" kind="Distance_AS" solid1="9" solid2="6" distance="0"></constraint>
</constraints>
</GCS_AS>

File diff suppressed because it is too large Load Diff

View File

@ -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<Constraint *> clist;
std::map<Constraint *,VEC_pD > c2p; // constraint to parameter adjacency list
std::map<double *,std::vector<Constraint *> > 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<Constraint *> clist_ );
~System();
void clear();
void clearByTag ( int tagId );
int addConstraint ( Constraint *constr );
void removeConstraint ( Constraint *constr );
void initSolution ( VEC_pD &params );
int solve ( bool isFine=true, Algorithm alg=DogLeg );
int solve ( VEC_pD &params, 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<SubSystem *> &subsysvec );
void applySolution();
bool isInit() const {
return init;
}
int diagnose ( VEC_pD &params, 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<Constraint *> &constrvec );
void free ( std::vector<SubSystem *> &subsysvec );
} //namespace GCS
#endif // FREEGCS_GCS_H

View File

@ -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

View File

@ -1,359 +0,0 @@
#include <sys/stat.h>
#include <iostream>
#include "InputParser.h"
#include <xercesc/util/XMLDouble.hpp>
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<double *> &variables,
std::vector<double *> &parameters,
std::vector<GCS::Point> &points,
std::vector<GCS::Normal> &norms,
std::vector<GCS::Displacement> &disps,
std::vector<GCS::Quaternion> &quats,
std::vector<GCS::Solid> &solids,
std::vector<GCS::Constraint *> &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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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<xercesc::DOMElement*>(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();
}

View File

@ -1,60 +0,0 @@
#include <xercesc/dom/DOM.hpp>
#include <xercesc/parsers/XercesDOMParser.hpp>
#include "GCS.h"
class InputParser
{
public:
InputParser();
~InputParser();
bool readInputFileAS(std::string &fileName,
std::vector<double *> &variables,
std::vector<double *> &parameters,
std::vector<GCS::Point> &points,
std::vector<GCS::Normal> &normals,
std::vector<GCS::Displacement> &displacements,
std::vector<GCS::Quaternion> &quaternions,
std::vector<GCS::Solid> &solids,
std::vector<GCS::Constraint *> &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;
};

View File

@ -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 <iostream>
#include <iterator>
#include "SubSystem.h"
namespace GCS
{
// SubSystem
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params)
: clist(clist_)
{
MAP_pD_pD dummymap;
initialize(params, dummymap);
}
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params,
MAP_pD_pD &reductionmap)
: clist(clist_)
{
initialize(params, reductionmap);
}
SubSystem::~SubSystem()
{
}
void SubSystem::initialize(VEC_pD &params, 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<Constraint *>::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<Constraint *>::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<Constraint *>::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<Constraint *>::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 &params, 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 &params, 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<Constraint *>::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<Constraint *>::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<Constraint *>::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<Constraint *> constrs=p2c[param->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
jacobi.set(*constr,param->second,(*constr)->grad(param->second));
}
}
*/
void SubSystem::calcJacobi(VEC_pD &params, 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 &params, 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<Constraint *> constrs=p2c[pmapfind->second];
for (std::vector<Constraint *>::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 &params, 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<Constraint *>::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<Constraint *>::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

View File

@ -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 <Eigen/Dense>
#include "Constraints.h"
namespace GCS
{
class SubSystem
{
private:
int psize, csize;
std::vector<Constraint *> 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<Constraint *,VEC_pD > c2p; // constraint to parameter adjacency list
std::map<double *,std::vector<Constraint *> > p2c; // parameter to constraint adjacency list
void initialize(VEC_pD &params, MAP_pD_pD &reductionmap); // called by the constructors
public:
SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params);
SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params,
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 &params, Eigen::VectorXd &xOut);
void getParams(Eigen::VectorXd &xOut);
void setParams(VEC_pD &params, 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 &params, Eigen::MatrixXd &jacobi);
void calcJacobi(Eigen::MatrixXd &jacobi);
void calcGrad(VEC_pD &params, Eigen::VectorXd &grad);
void calcGrad(Eigen::VectorXd &grad);
double maxStep(VEC_pD &params, 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

View File

@ -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 <vector>
#include <map>
#include <set>
namespace GCS
{
typedef std::vector<double *> VEC_pD;
typedef std::vector<double> VEC_D;
typedef std::vector<int> VEC_I;
typedef std::map<double *, double *> MAP_pD_pD;
typedef std::map<double *, double> MAP_pD_D;
typedef std::map<double *, int> MAP_pD_I;
typedef std::set<double *> SET_pD;
typedef std::set<int> SET_I;
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
} //namespace GCS
#endif // FREEGCS_UTIL_H

View File

@ -1,122 +0,0 @@
#include <iostream>
#include <vector>
#include "Geo.h"
#include "Constraints.h"
#include "InputParser.h"
#include "OutputWriter.h"
#include <fstream>
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<double *> variables;
vector<double *> parameters;
vector<GCS::Point> points;
vector<GCS::Normal> norms;
vector<GCS::Displacement> disps;
vector<GCS::Quaternion> quats;
vector<GCS::Solid> solids;
vector<GCS::Constraint *> 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<solids.size(); i++) {
file << "plane = Part.makePlane(2,2,"<<Vec(Place(solids[i].p, solids[i].q, solids[i].d))<<",";
file << Vec(Place(solids[i].n, solids[i].q, emd)) << ")" << endl;
file << "Part.show(plane)" << endl;
file << "Gui.ActiveDocument.ActiveObject.ShapeColor = ("<< 0.9-0.05*(double)i <<",0.0,0.0)" << endl;
}
cout<<endl<<"solving...";
GCS::System GCSsys(constraints);
GCSsys.solve(variables);
GCSsys.applySolution();
for (int i=0; i<solids.size(); i++) {
file << "plane = Part.makePlane(2,2,"<<Vec(Place(solids[i].p, solids[i].q, solids[i].d))<<",";
file << Vec(Place(solids[i].n, solids[i].q, emd)) << ")" << endl;
file << "Part.show(plane)" << endl;
file << "Gui.ActiveDocument.ActiveObject.ShapeColor = (0.0,0.0,"<< 0.9-0.05*(double)i <<")" << endl;
}
// Clean up
GCS::free(variables);
GCS::free(parameters);
GCS::free(constraints);
return 0;
}

View File

@ -1,65 +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 <iostream>
#include <Eigen/QR>
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<MatrixXd> 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<Upper>()
.transpose()
.solve<OnTheRight>(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;
}

View File

@ -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 <Eigen/Dense>
int qp_eq(Eigen::MatrixXd &H, Eigen::VectorXd &g, Eigen::MatrixXd &A, Eigen::VectorXd &c,
Eigen::VectorXd &x, Eigen::MatrixXd &Y, Eigen::MatrixXd &Z);

View File

@ -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

View File

@ -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());
}
}

View File

@ -30,6 +30,7 @@
#include <Gui/Command.h>
#include <Gui/MainWindow.h>
#include <Gui/FileDialog.h>
#include <Gui/Selection.h>
#include <Mod/Assembly/App/ItemAssembly.h>
#include <Mod/Assembly/App/ConstraintGroup.h>
@ -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<Gui::SelectionObject> 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());
}