userfriendly part movement
This commit is contained in:
Stefan Tröger
Stefan Tröger
parent
34acc5f8b4
commit
02311333b9
|
|
@ -25,6 +25,7 @@
|
|||
#ifndef _PreComp_
|
||||
# include <BRep_Builder.hxx>
|
||||
# include <TopoDS_Compound.hxx>
|
||||
#include <boost/exception/get_error_info.hpp>
|
||||
#endif
|
||||
|
||||
#include <Base/Placement.h>
|
||||
|
|
@ -43,67 +44,68 @@ namespace Assembly {
|
|||
|
||||
PROPERTY_SOURCE(Assembly::ItemAssembly, Assembly::Item)
|
||||
|
||||
ItemAssembly::ItemAssembly()
|
||||
{
|
||||
ItemAssembly::ItemAssembly() {
|
||||
ADD_PROPERTY(Items,(0));
|
||||
ADD_PROPERTY(Annotations,(0));
|
||||
}
|
||||
|
||||
short ItemAssembly::mustExecute() const
|
||||
{
|
||||
short ItemAssembly::mustExecute() const {
|
||||
return 0;
|
||||
}
|
||||
|
||||
App::DocumentObjectExecReturn *ItemAssembly::execute(void)
|
||||
{
|
||||
App::DocumentObjectExecReturn* ItemAssembly::execute(void) {
|
||||
Base::Console().Message("Execute ItemAssembly\n");
|
||||
|
||||
try{
|
||||
//create a solver and init all child assemblys with subsolvers
|
||||
m_solver = boost::shared_ptr<Solver>(new Solver);
|
||||
init(boost::shared_ptr<Solver>());
|
||||
|
||||
//get the constraint group and init the constraints
|
||||
typedef std::vector<App::DocumentObject*>::const_iterator iter;
|
||||
|
||||
const std::vector<App::DocumentObject*>& vector = Annotations.getValues();
|
||||
for(iter it=vector.begin(); it != vector.end(); it++) {
|
||||
|
||||
if( (*it)->getTypeId() == Assembly::ConstraintGroup::getClassTypeId() )
|
||||
static_cast<ConstraintGroup*>(*it)->init(this);
|
||||
};
|
||||
|
||||
//solve the system
|
||||
m_solver->solve();
|
||||
|
||||
try {
|
||||
//create a solver and init all child assemblys with subsolvers
|
||||
m_solver = boost::shared_ptr<Solver>(new Solver);
|
||||
init(boost::shared_ptr<Solver>());
|
||||
|
||||
//get the constraint group and init the constraints
|
||||
typedef std::vector<App::DocumentObject*>::const_iterator iter;
|
||||
|
||||
const std::vector<App::DocumentObject*>& vector = Annotations.getValues();
|
||||
for(iter it=vector.begin(); it != vector.end(); it++) {
|
||||
|
||||
if((*it)->getTypeId() == Assembly::ConstraintGroup::getClassTypeId())
|
||||
static_cast<ConstraintGroup*>(*it)->init(this);
|
||||
};
|
||||
|
||||
//solve the system
|
||||
m_solver->solve();
|
||||
} catch(dcm::solving_error& e) {
|
||||
Base::Console().Error("Solver failed with error %i: %s",
|
||||
*boost::get_error_info<boost::errinfo_errno>(e),
|
||||
boost::get_error_info<dcm::error_message>(e)->c_str());
|
||||
} catch(dcm::creation_error& e) {
|
||||
Base::Console().Error("Creation failed with error %i: %s",
|
||||
*boost::get_error_info<boost::errinfo_errno>(e),
|
||||
boost::get_error_info<dcm::error_message>(e)->c_str());
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
Base::Console().Message("Solver exeption: %s", e.what());
|
||||
}
|
||||
|
||||
|
||||
this->touch();
|
||||
return App::DocumentObject::StdReturn;
|
||||
}
|
||||
|
||||
TopoDS_Shape ItemAssembly::getShape(void) const
|
||||
{
|
||||
TopoDS_Shape ItemAssembly::getShape(void) const {
|
||||
std::vector<TopoDS_Shape> s;
|
||||
std::vector<App::DocumentObject*> obj = Items.getValues();
|
||||
|
||||
std::vector<App::DocumentObject*>::iterator it;
|
||||
for (it = obj.begin(); it != obj.end(); ++it) {
|
||||
if ((*it)->getTypeId().isDerivedFrom(Assembly::Item::getClassTypeId())) {
|
||||
for(it = obj.begin(); it != obj.end(); ++it) {
|
||||
if((*it)->getTypeId().isDerivedFrom(Assembly::Item::getClassTypeId())) {
|
||||
TopoDS_Shape aShape = static_cast<Assembly::Item*>(*it)->getShape();
|
||||
if (!aShape.IsNull())
|
||||
if(!aShape.IsNull())
|
||||
s.push_back(aShape);
|
||||
}
|
||||
}
|
||||
|
||||
if (s.size() > 0) {
|
||||
if(s.size() > 0) {
|
||||
TopoDS_Compound aRes = TopoDS_Compound();
|
||||
BRep_Builder aBuilder = BRep_Builder();
|
||||
aBuilder.MakeCompound(aRes);
|
||||
|
||||
for (std::vector<TopoDS_Shape>::iterator it = s.begin(); it != s.end(); ++it) {
|
||||
for(std::vector<TopoDS_Shape>::iterator it = s.begin(); it != s.end(); ++it) {
|
||||
|
||||
aBuilder.Add(aRes, *it);
|
||||
}
|
||||
|
|
@ -113,51 +115,49 @@ TopoDS_Shape ItemAssembly::getShape(void) const
|
|||
}
|
||||
// set empty shape
|
||||
return TopoDS_Compound();
|
||||
|
||||
|
||||
}
|
||||
|
||||
PyObject *ItemAssembly::getPyObject(void)
|
||||
{
|
||||
if (PythonObject.is(Py::_None())){
|
||||
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);
|
||||
return Py::new_reference_to(PythonObject);
|
||||
}
|
||||
|
||||
bool ItemAssembly::isParentAssembly(ItemPart* part) {
|
||||
|
||||
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(*it == part) return true;
|
||||
|
||||
if((*it)->getTypeId() == Assembly::ItemPart::getClassTypeId())
|
||||
if(*it == part) return true;
|
||||
};
|
||||
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
ItemAssembly* ItemAssembly::getParentAssembly(ItemPart* part) {
|
||||
|
||||
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(*it == part)
|
||||
return this;
|
||||
}
|
||||
else if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) {
|
||||
|
||||
Assembly::ItemAssembly* assembly = static_cast<Assembly::ItemAssembly*>(*it)->getParentAssembly(part);
|
||||
if(assembly)
|
||||
return assembly;
|
||||
}
|
||||
|
||||
if((*it)->getTypeId() == Assembly::ItemPart::getClassTypeId()) {
|
||||
if(*it == part)
|
||||
return this;
|
||||
} else if((*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId()) {
|
||||
|
||||
Assembly::ItemAssembly* assembly = static_cast<Assembly::ItemAssembly*>(*it)->getParentAssembly(part);
|
||||
if(assembly)
|
||||
return assembly;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
return (ItemAssembly*)NULL;
|
||||
}
|
||||
|
||||
|
|
@ -166,41 +166,40 @@ ItemAssembly* ItemAssembly::getParentAssembly(ItemPart* part) {
|
|||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void ItemAssembly::init(boost::shared_ptr<Solver> parent) {
|
||||
|
||||
if(parent)
|
||||
m_solver = boost::shared_ptr<Solver>(parent->createSubsystem());
|
||||
|
||||
m_solver = boost::shared_ptr<Solver>(parent->createSubsystem());
|
||||
|
||||
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::ItemAssembly::getClassTypeId() ) {
|
||||
|
||||
static_cast<Assembly::ItemAssembly*>(*it)->init(m_solver);
|
||||
}
|
||||
|
||||
if((*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId()) {
|
||||
|
||||
static_cast<Assembly::ItemAssembly*>(*it)->init(m_solver);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -61,6 +61,15 @@ namespace dcm {
|
|||
|
||||
namespace details {
|
||||
|
||||
template<typename seq, typename state>
|
||||
struct vector_fold : mpl::fold< seq, state,
|
||||
mpl::push_back<mpl::_1,mpl::_2> > {};
|
||||
|
||||
//also add basic properties for graph algorithms like index and color
|
||||
typedef mpl::vector2<vertex_index_prop, vertex_color_prop> bgl_v_props;
|
||||
typedef mpl::vector2<edge_index_prop, edge_color_prop> bgl_e_props;
|
||||
|
||||
|
||||
typedef boost::adjacency_list_traits<boost::listS, boost::listS, boost::undirectedS> list_traits;
|
||||
typedef int universalID;
|
||||
|
||||
|
|
@ -127,18 +136,22 @@ struct GlobalEdge {
|
|||
template< typename edge_prop, typename vertex_prop, typename cluster_prop, typename objects>
|
||||
class ClusterGraph : public boost::adjacency_list< boost::listS, boost::listS,
|
||||
boost::undirectedS,
|
||||
fusion::vector< GlobalVertex, typename details::pts<vertex_prop>::type,
|
||||
fusion::vector< GlobalVertex,
|
||||
typename details::pts<typename details::vector_fold<vertex_prop, details::bgl_v_props>::type>::type,
|
||||
typename details::sps<objects>::type>,
|
||||
fusion::vector< typename details::pts<edge_prop>::type,
|
||||
fusion::vector< typename details::pts<typename details::vector_fold<edge_prop, details::bgl_e_props>::type>::type,
|
||||
std::vector< fusion::vector< typename details::sps<objects>::type, GlobalEdge > > > >,
|
||||
public boost::noncopyable,
|
||||
public boost::enable_shared_from_this<ClusterGraph<edge_prop, vertex_prop, cluster_prop, objects> > {
|
||||
|
||||
public:
|
||||
typedef typename details::vector_fold<edge_prop, details::bgl_e_props>::type edge_properties;
|
||||
typedef typename details::vector_fold<vertex_prop, details::bgl_v_props>::type vertex_properties;
|
||||
|
||||
typedef fusion::vector< typename details::sps<objects>::type, GlobalEdge > edge_bundle_single;
|
||||
typedef fusion::vector< typename details::pts<edge_prop>::type, std::vector< edge_bundle_single > > edge_bundle;
|
||||
typedef fusion::vector< typename details::pts<edge_properties>::type, std::vector< edge_bundle_single > > edge_bundle;
|
||||
typedef typename std::vector< edge_bundle_single >::iterator edge_single_iterator;
|
||||
typedef fusion::vector< GlobalVertex, typename details::pts<vertex_prop>::type,
|
||||
typedef fusion::vector< GlobalVertex, typename details::pts<vertex_properties>::type,
|
||||
typename details::sps<objects>::type > vertex_bundle;
|
||||
|
||||
|
||||
|
|
@ -154,7 +167,7 @@ public:
|
|||
typename mpl::end<cluster_prop>::type >,
|
||||
typename mpl::push_back<cluster_prop, changed_prop>::type,
|
||||
cluster_prop >::type cluster_properties;
|
||||
|
||||
|
||||
typedef typename details::pts<cluster_properties>::type cluster_bundle;
|
||||
|
||||
typedef typename boost::graph_traits<Graph>::vertex_iterator local_vertex_iterator;
|
||||
|
|
@ -165,9 +178,6 @@ public:
|
|||
|
||||
cluster_bundle m_cluster_bundle;
|
||||
|
||||
typedef edge_prop edge_properties;
|
||||
typedef vertex_prop vertex_properties;
|
||||
|
||||
private:
|
||||
struct global_extractor {
|
||||
typedef GlobalEdge& result_type;
|
||||
|
|
@ -277,7 +287,8 @@ public:
|
|||
template<typename Functor>
|
||||
void copyInto(boost::shared_ptr<ClusterGraph> into, Functor& functor) const {
|
||||
|
||||
//lists does not provide vertex index, so we have to build our own
|
||||
//lists does not provide vertex index, so we have to build our own (cant use the internal
|
||||
//vertex_index_property as we would need to reset the indices and that's not possible in const graph)
|
||||
typedef std::map<LocalVertex, int> IndexMap;
|
||||
IndexMap mapIndex;
|
||||
boost::associative_property_map<IndexMap> propmapIndex(mapIndex);
|
||||
|
|
@ -1106,11 +1117,11 @@ protected:
|
|||
|
||||
typedef typename prop::type base_type;
|
||||
typedef base_type& result_type;
|
||||
typedef typename mpl::find<vertex_prop, prop>::type vertex_iterator;
|
||||
typedef typename mpl::find<edge_prop, prop>::type edge_iterator;
|
||||
typedef typename mpl::if_<boost::is_same<vertex_iterator,typename mpl::end<vertex_prop>::type >,
|
||||
typedef typename mpl::find<vertex_properties, prop>::type vertex_iterator;
|
||||
typedef typename mpl::find<edge_properties, prop>::type edge_iterator;
|
||||
typedef typename mpl::if_<boost::is_same<vertex_iterator,typename mpl::end<vertex_properties>::type >,
|
||||
edge_iterator, vertex_iterator>::type iterator;
|
||||
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<edge_prop>::type > >));
|
||||
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<edge_properties>::type > >));
|
||||
|
||||
//used with vertex bundle type
|
||||
template<typename bundle>
|
||||
|
|
@ -1162,6 +1173,29 @@ public:
|
|||
|
||||
setChanged();
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief recreate the internal index maps for edges and vertices
|
||||
*
|
||||
* Quite many boost graph algorithms need the indices for vertices and edges which are provided by property
|
||||
* maps. As we use list, and not vector, as underlaying storage we don't get that property for free and
|
||||
* need to create it ourself. To ease that procedure the internal property vertex_index_prop and edge_index_prop
|
||||
* can be used as property maps and can be initialized by calling this function.
|
||||
*
|
||||
* @return void
|
||||
**/
|
||||
void initIndexMaps() {
|
||||
|
||||
//just iterate over all edges and vertices and give them all a unique index
|
||||
std::pair<local_vertex_iterator, local_vertex_iterator> vit = boost::vertices(*this);
|
||||
for(int c=0; vit.first != vit.second; vit.first++, c++)
|
||||
setProperty<vertex_index_prop>(*vit.first, c);
|
||||
|
||||
std::pair<local_edge_iterator, local_edge_iterator> eit = boost::edges(*this);
|
||||
for(int c=0; eit.first != eit.second; eit.first++, c++)
|
||||
setProperty<edge_index_prop>(*eit.first, c);
|
||||
};
|
||||
|
||||
|
||||
|
||||
/********************************************************
|
||||
|
|
|
|||
|
|
@ -75,17 +75,17 @@ public:
|
|||
std::vector<boost::any> getGenericConstraints();
|
||||
std::vector<const std::type_info*> getEquationTypes();
|
||||
std::vector<const std::type_info*> getConstraintTypes();
|
||||
|
||||
|
||||
template<typename ConstraintVector>
|
||||
void initialize(ConstraintVector& obj);
|
||||
|
||||
|
||||
protected:
|
||||
int equationCount();
|
||||
|
||||
template< typename creator_type>
|
||||
void resetType(creator_type& c);
|
||||
|
||||
void calculate(Scalar scale);
|
||||
void calculate(Scalar scale, bool rotation_only = false);
|
||||
|
||||
void setMaps(MES& mes);
|
||||
|
||||
|
|
@ -96,35 +96,38 @@ protected:
|
|||
};
|
||||
|
||||
void collectPseudoPoints(Vec& vec1, Vec& vec2);
|
||||
|
||||
|
||||
//Equation is the constraint with types, the EquationSet hold all needed Maps for calculation
|
||||
template<typename Equation>
|
||||
struct EquationSet {
|
||||
EquationSet() : m_diff_first(NULL,0,DS(0,0)), m_diff_second(NULL,0,DS(0,0)),
|
||||
EquationSet() : m_diff_first(NULL,0,DS(0,0)), m_diff_first_rot(NULL,0,DS(0,0)),
|
||||
m_diff_second(NULL,0,DS(0,0)), m_diff_second_rot(NULL,0,DS(0,0)),
|
||||
m_residual(NULL,0,DS(0,0)) {};
|
||||
|
||||
Equation m_eq;
|
||||
typename Kernel::VectorMap m_diff_first; //first geometry diff
|
||||
typename Kernel::VectorMap m_diff_second; //second geometry diff
|
||||
typename Kernel::VectorMap m_diff_first, m_diff_first_rot; //first geometry diff
|
||||
typename Kernel::VectorMap m_diff_second, m_diff_second_rot; //second geometry diff
|
||||
typename Kernel::VectorMap m_residual;
|
||||
|
||||
bool pure_rotation;
|
||||
|
||||
typedef Equation eq_type;
|
||||
};
|
||||
|
||||
struct placeholder {
|
||||
virtual ~placeholder() {}
|
||||
virtual placeholder* resetConstraint(geom_ptr first, geom_ptr second) const = 0;
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale) = 0;
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false) = 0;
|
||||
virtual int equationCount() = 0;
|
||||
virtual void setMaps(MES& mes, geom_ptr first, geom_ptr second) = 0;
|
||||
virtual void collectPseudoPoints(geom_ptr first, geom_ptr second, Vec& vec1, Vec& vec2) = 0;
|
||||
virtual placeholder* clone() = 0;
|
||||
|
||||
//some runtime type infos are needed, as we cant access the contents with arbitrary functors
|
||||
|
||||
//some runtime type infos are needed, as we cant access the contents with arbitrary functors
|
||||
virtual std::vector<boost::any> getGenericEquations() = 0;
|
||||
virtual std::vector<boost::any> getGenericConstraints() = 0;
|
||||
virtual std::vector<const std::type_info*> getEquationTypes() = 0;
|
||||
virtual std::vector<const std::type_info*> getConstraintTypes() = 0;
|
||||
virtual std::vector<boost::any> getGenericConstraints() = 0;
|
||||
virtual std::vector<const std::type_info*> getEquationTypes() = 0;
|
||||
virtual std::vector<const std::type_info*> getConstraintTypes() = 0;
|
||||
};
|
||||
|
||||
public:
|
||||
|
|
@ -170,8 +173,9 @@ public:
|
|||
|
||||
geom_ptr first, second;
|
||||
Scalar scale;
|
||||
bool rot_only;
|
||||
|
||||
Calculater(geom_ptr f, geom_ptr s, Scalar sc);
|
||||
Calculater(geom_ptr f, geom_ptr s, Scalar sc, bool rotation_only = false);
|
||||
|
||||
template< typename T >
|
||||
void operator()(T& val) const;
|
||||
|
|
@ -205,16 +209,16 @@ public:
|
|||
template<typename T>
|
||||
void operator()(T& val) const;
|
||||
};
|
||||
|
||||
struct GenericConstraints {
|
||||
|
||||
struct GenericConstraints {
|
||||
std::vector<boost::any>& vec;
|
||||
GenericConstraints(std::vector<boost::any>& v);
|
||||
|
||||
template<typename T>
|
||||
void operator()(T& val) const;
|
||||
};
|
||||
|
||||
struct Types {
|
||||
|
||||
struct Types {
|
||||
std::vector<const std::type_info*>& vec;
|
||||
Types(std::vector<const std::type_info*>& v);
|
||||
|
||||
|
|
@ -222,10 +226,10 @@ public:
|
|||
void operator()(T& val) const;
|
||||
};
|
||||
|
||||
|
||||
|
||||
holder(Objects& obj);
|
||||
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale);
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false);
|
||||
virtual placeholder* resetConstraint(geom_ptr first, geom_ptr second) const;
|
||||
virtual void setMaps(MES& mes, geom_ptr first, geom_ptr second);
|
||||
virtual void collectPseudoPoints(geom_ptr f, geom_ptr s, Vec& vec1, Vec& vec2);
|
||||
|
|
@ -233,14 +237,14 @@ public:
|
|||
virtual int equationCount() {
|
||||
return mpl::size<EquationVector>::value;
|
||||
};
|
||||
|
||||
|
||||
virtual std::vector<boost::any> getGenericEquations();
|
||||
virtual std::vector<boost::any> getGenericConstraints();
|
||||
virtual std::vector<const std::type_info*> getEquationTypes();
|
||||
virtual std::vector<const std::type_info*> getConstraintTypes();
|
||||
virtual std::vector<boost::any> getGenericConstraints();
|
||||
virtual std::vector<const std::type_info*> getEquationTypes();
|
||||
virtual std::vector<const std::type_info*> getConstraintTypes();
|
||||
|
||||
EquationSets m_sets;
|
||||
Objects m_objects;
|
||||
Objects m_objects;
|
||||
};
|
||||
|
||||
protected:
|
||||
|
|
@ -282,15 +286,15 @@ Constraint<Sys, Derived, Signals, MES, Geometry>::Constraint(Sys& system, geom_p
|
|||
: first(f), second(s), content(0) {
|
||||
|
||||
this->m_system = &system;
|
||||
cf = first->template connectSignal<reset> (boost::bind(&Constraint::geometryReset, this, _1));
|
||||
cs = second->template connectSignal<reset> (boost::bind(&Constraint::geometryReset, this, _1));
|
||||
//cf = first->template connectSignal<reset> (boost::bind(&Constraint::geometryReset, this, _1));
|
||||
//cs = second->template connectSignal<reset> (boost::bind(&Constraint::geometryReset, this, _1));
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::~Constraint() {
|
||||
delete content;
|
||||
first->template disconnectSignal<reset>(cf);
|
||||
second->template disconnectSignal<reset>(cs);
|
||||
//first->template disconnectSignal<reset>(cf);
|
||||
//second->template disconnectSignal<reset>(cs);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
|
@ -342,8 +346,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::resetType(creator_type& c
|
|||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::calculate(Scalar scale) {
|
||||
content->calculate(first, second, scale);
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::calculate(Scalar scale, bool rotation_only) {
|
||||
content->calculate(first, second, scale, rotation_only);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
|
@ -391,6 +395,7 @@ Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, Equat
|
|||
typedef typename mpl::distance<typename mpl::begin<EquationVector>::type, iterator>::type distance;
|
||||
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<EquationVector>::type > >));
|
||||
val.m_eq.value = fusion::at<distance>(objects).value;
|
||||
val.pure_rotation = fusion::at<distance>(objects).pure_rotation;
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
|
@ -403,7 +408,8 @@ Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, Equat
|
|||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::Calculater::Calculater(geom_ptr f, geom_ptr s, Scalar sc) : first(f), second(s), scale(sc) {
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::Calculater::Calculater(geom_ptr f, geom_ptr s, Scalar sc, bool rotation_only)
|
||||
: first(f), second(s), scale(sc), rot_only(rotation_only) {
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -412,42 +418,79 @@ template<typename ConstraintVector, typename EquationVector>
|
|||
template< typename T >
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::Calculater::operator()(T& val) const {
|
||||
|
||||
val.m_eq.setScale(scale);
|
||||
//if we only need pure rotational functions and we are not such a nice thing, everything becomes 0
|
||||
if(rot_only && !val.pure_rotation) {
|
||||
|
||||
val.m_residual(0) = val.m_eq.calculate(first->m_parameter, second->m_parameter);
|
||||
|
||||
//now see which way we should calculate the gradient (may be diffrent for both geometries)
|
||||
if(first->m_parameterCount) {
|
||||
val.m_residual(0) = 0;
|
||||
if(first->getClusterMode()) {
|
||||
//when the cluster is fixed no maps are set as no parameters exist.
|
||||
if(!first->isClusterFixed()) {
|
||||
|
||||
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
|
||||
for(int i=0; i<6; i++) {
|
||||
typename Kernel::VectorMap block(&first->m_diffparam(0,i),first->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_first(i) = val.m_eq.calculateGradientFirst(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
val.m_diff_first_rot.setZero();
|
||||
val.m_diff_first.setZero();
|
||||
}
|
||||
} else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientFirstComplete(first->m_parameter, second->m_parameter, val.m_diff_first);
|
||||
}
|
||||
}
|
||||
if(second->m_parameterCount) {
|
||||
} else
|
||||
val.m_diff_first.setZero();
|
||||
|
||||
if(second->getClusterMode()) {
|
||||
if(!second->isClusterFixed()) {
|
||||
|
||||
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
|
||||
for(int i=0; i<6; i++) {
|
||||
typename Kernel::VectorMap block(&second->m_diffparam(0,i),second->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_second(i) = val.m_eq.calculateGradientSecond(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
val.m_diff_second_rot.setZero();
|
||||
val.m_diff_second.setZero();
|
||||
}
|
||||
} else
|
||||
val.m_diff_second.setZero();
|
||||
|
||||
}
|
||||
//we need to calculate, so lets go for it!
|
||||
else {
|
||||
|
||||
val.m_eq.setScale(scale);
|
||||
|
||||
val.m_residual(0) = val.m_eq.calculate(first->m_parameter, second->m_parameter);
|
||||
|
||||
//now see which way we should calculate the gradient (may be diffrent for both geometries)
|
||||
if(first->m_parameterCount) {
|
||||
if(first->getClusterMode()) {
|
||||
//when the cluster is fixed no maps are set as no parameters exist.
|
||||
if(!first->isClusterFixed()) {
|
||||
|
||||
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
|
||||
for(int i=0; i<3; i++) {
|
||||
typename Kernel::VectorMap block(&first->m_diffparam(0,i),first->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_first_rot(i) = val.m_eq.calculateGradientFirst(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
//and now with the translations
|
||||
for(int i=0; i<3; i++) {
|
||||
typename Kernel::VectorMap block(&first->m_diffparam(0,i+3),first->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_first(i) = val.m_eq.calculateGradientFirst(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientFirstComplete(first->m_parameter, second->m_parameter, val.m_diff_first);
|
||||
}
|
||||
}
|
||||
if(second->m_parameterCount) {
|
||||
if(second->getClusterMode()) {
|
||||
if(!second->isClusterFixed()) {
|
||||
|
||||
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
|
||||
for(int i=0; i<3; i++) {
|
||||
typename Kernel::VectorMap block(&second->m_diffparam(0,i),second->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_second_rot(i) = val.m_eq.calculateGradientSecond(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
//and the translation seperated
|
||||
for(int i=0; i<3; i++) {
|
||||
typename Kernel::VectorMap block(&second->m_diffparam(0,i+3),second->m_parameterCount,1, DS(1,1));
|
||||
val.m_diff_second(i) = val.m_eq.calculateGradientSecond(first->m_parameter,
|
||||
second->m_parameter, block);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientSecondComplete(first->m_parameter, second->m_parameter, val.m_diff_second);
|
||||
}
|
||||
} else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientSecondComplete(first->m_parameter, second->m_parameter, val.m_diff_second);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -469,14 +512,16 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
int equation = mes.setResidualMap(val.m_residual);
|
||||
if(first->getClusterMode()) {
|
||||
if(!first->isClusterFixed()) {
|
||||
mes.setJacobiMap(equation, first->m_offset, 6, val.m_diff_first);
|
||||
mes.setJacobiMap(equation, first->m_offset_rot, 3, val.m_diff_first_rot);
|
||||
mes.setJacobiMap(equation, first->m_offset, 3, val.m_diff_first);
|
||||
}
|
||||
} else mes.setJacobiMap(equation, first->m_offset, first->m_parameterCount, val.m_diff_first);
|
||||
|
||||
|
||||
if(second->getClusterMode()) {
|
||||
if(!second->isClusterFixed()) {
|
||||
mes.setJacobiMap(equation, second->m_offset, 6, val.m_diff_second);
|
||||
mes.setJacobiMap(equation, second->m_offset_rot, 3, val.m_diff_second_rot);
|
||||
mes.setJacobiMap(equation, second->m_offset, 3, val.m_diff_second);
|
||||
}
|
||||
} else mes.setJacobiMap(equation, second->m_offset, second->m_parameterCount, val.m_diff_second);
|
||||
};
|
||||
|
|
@ -544,7 +589,7 @@ template<typename ConstraintVector, typename EquationVector>
|
|||
template< typename T >
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::Types::operator()(T& val) const {
|
||||
vec.push_back(&typeid(T));
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
|
|
@ -555,8 +600,9 @@ Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, Equat
|
|||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::calculate(geom_ptr first, geom_ptr second, Scalar scale) {
|
||||
fusion::for_each(m_sets, Calculater(first, second, scale));
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::calculate(geom_ptr first, geom_ptr second,
|
||||
Scalar scale, bool rotation_only) {
|
||||
fusion::for_each(m_sets, Calculater(first, second, scale, rotation_only));
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
|
@ -589,19 +635,19 @@ Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, Equat
|
|||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
std::vector<boost::any>
|
||||
std::vector<boost::any>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::getGenericEquations() {
|
||||
std::vector<boost::any> vec;
|
||||
fusion::for_each( m_sets, GenericEquations(vec) );
|
||||
fusion::for_each(m_sets, GenericEquations(vec));
|
||||
return vec;
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
std::vector<boost::any>
|
||||
std::vector<boost::any>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::getGenericConstraints() {
|
||||
std::vector<boost::any> vec;
|
||||
fusion::for_each( m_objects, GenericConstraints(vec) );
|
||||
fusion::for_each(m_objects, GenericConstraints(vec));
|
||||
return vec;
|
||||
};
|
||||
|
||||
|
|
@ -610,7 +656,7 @@ template<typename ConstraintVector, typename EquationVector>
|
|||
std::vector<const std::type_info*>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::getEquationTypes() {
|
||||
std::vector<const std::type_info*> vec;
|
||||
mpl::for_each< EquationVector >( Types(vec) );
|
||||
mpl::for_each< EquationVector >(Types(vec));
|
||||
return vec;
|
||||
};
|
||||
|
||||
|
|
@ -619,7 +665,7 @@ template<typename ConstraintVector, typename EquationVector>
|
|||
std::vector<const std::type_info*>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::getConstraintTypes() {
|
||||
std::vector<const std::type_info*> vec;
|
||||
mpl::for_each< ConstraintVector >( Types(vec) );
|
||||
mpl::for_each< ConstraintVector >(Types(vec));
|
||||
return vec;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@
|
|||
#include <boost/fusion/include/back.hpp>
|
||||
#include <boost/fusion/include/iterator_range.hpp>
|
||||
|
||||
#include <boost/exception/exception.hpp>
|
||||
|
||||
namespace fusion = boost::fusion;
|
||||
namespace mpl = boost::mpl;
|
||||
|
||||
|
|
@ -42,6 +44,11 @@ namespace mpl = boost::mpl;
|
|||
|
||||
namespace dcm {
|
||||
|
||||
//a few exceptions to handle unsupported combinations
|
||||
struct constraint_error : virtual boost::exception { };
|
||||
typedef boost::error_info<struct first_geom, std::string> error_type_first_geometry;
|
||||
typedef boost::error_info<struct second_geom, std::string> error_type_second_geometry;
|
||||
|
||||
struct no_option {};
|
||||
|
||||
template<typename Kernel>
|
||||
|
|
@ -143,13 +150,14 @@ struct pushed_seq {
|
|||
typedef constraint_sequence<vec> type;
|
||||
};
|
||||
|
||||
template<typename Derived, typename Option>
|
||||
template<typename Derived, typename Option, bool rotation_only = false>
|
||||
struct Equation : public EQ {
|
||||
|
||||
typedef Option option_type;
|
||||
option_type value;
|
||||
bool pure_rotation;
|
||||
|
||||
Equation(option_type val = option_type()) : value(val) {};
|
||||
Equation(option_type val = option_type()) : value(val), pure_rotation(rotation_only) {};
|
||||
|
||||
Derived& operator()(const option_type val) {
|
||||
value = val;
|
||||
|
|
@ -204,6 +212,11 @@ struct Distance : public Equation<Distance, double> {
|
|||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct type {
|
||||
|
||||
type() {
|
||||
throw constraint_error() << boost::errinfo_errno(100) << error_message("unsupported geometry in distance constraint")
|
||||
<< error_type_first_geometry(typeid(Tag1).name()) << error_type_second_geometry(typeid(Tag2).name());
|
||||
};
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
|
@ -240,7 +253,7 @@ struct Distance : public Equation<Distance, double> {
|
|||
//the possible directions
|
||||
enum Direction { parallel, equal, opposite, perpendicular };
|
||||
|
||||
struct Orientation : public Equation<Orientation, Direction> {
|
||||
struct Orientation : public Equation<Orientation, Direction, true> {
|
||||
|
||||
using Equation::operator=;
|
||||
Orientation() : Equation(parallel) {};
|
||||
|
|
@ -248,6 +261,11 @@ struct Orientation : public Equation<Orientation, Direction> {
|
|||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct type : public PseudoScale<Kernel> {
|
||||
|
||||
type() {
|
||||
throw constraint_error() << boost::errinfo_errno(101) << error_message("unsupported geometry in orientation constraint")
|
||||
<< error_type_first_geometry(typeid(Tag1).name()) << error_type_second_geometry(typeid(Tag2).name());
|
||||
};
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
|
||||
|
|
@ -275,7 +293,7 @@ struct Orientation : public Equation<Orientation, Direction> {
|
|||
};
|
||||
};
|
||||
|
||||
struct Angle : public Equation<Angle, double> {
|
||||
struct Angle : public Equation<Angle, double, true> {
|
||||
|
||||
using Equation::operator=;
|
||||
Angle() : Equation(0) {};
|
||||
|
|
@ -283,6 +301,11 @@ struct Angle : public Equation<Angle, double> {
|
|||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct type : public PseudoScale<Kernel> {
|
||||
|
||||
type() {
|
||||
throw constraint_error() << boost::errinfo_errno(102) << error_message("unsupported geometry in angle constraint")
|
||||
<< error_type_first_geometry(typeid(Tag1).name()) << error_type_second_geometry(typeid(Tag2).name());
|
||||
};
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
|
||||
|
|
@ -291,15 +314,15 @@ struct Angle : public Equation<Angle, double> {
|
|||
//template definition
|
||||
Scalar calculate(Vector& param1, Vector& param2) {
|
||||
assert(false);
|
||||
return 0;
|
||||
return 0;
|
||||
};
|
||||
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
|
||||
assert(false);
|
||||
return 0;
|
||||
return 0;
|
||||
};
|
||||
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
|
||||
assert(false);
|
||||
return 0;
|
||||
return 0;
|
||||
};
|
||||
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
|
||||
assert(false);
|
||||
|
|
|
|||
|
|
@ -220,7 +220,7 @@ public:
|
|||
Variant m_geometry; //Variant holding the real geometry type
|
||||
int m_BaseParameterCount; //count of the parameters the variant geometry type needs
|
||||
int m_parameterCount; //count of the used parameters (when in cluster:6, else m_BaseParameterCount)
|
||||
int m_offset; //the starting point of our parameters in the math system parameter vector
|
||||
int m_offset, m_offset_rot; //the starting point of our parameters in the math system parameter vector
|
||||
int m_rotations; //count of rotations to be done when original vector gets rotated
|
||||
int m_translations; //count of translations to be done when original vector gets rotated
|
||||
bool m_isInCluster, m_clusterFixed, m_init;
|
||||
|
|
|
|||
|
|
@ -27,6 +27,10 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <boost/math/special_functions/fpclassify.hpp>
|
||||
#include <boost/exception/exception.hpp>
|
||||
#include <boost/exception/errinfo_errno.hpp>
|
||||
#include <boost/graph/graph_concepts.hpp>
|
||||
|
||||
#include <time.h>
|
||||
|
||||
#include "transformation.hpp"
|
||||
|
|
@ -41,6 +45,17 @@ struct nothing {
|
|||
void operator()() {};
|
||||
};
|
||||
|
||||
//the parameter types
|
||||
enum ParameterType {
|
||||
general,
|
||||
rotation,
|
||||
complete
|
||||
};
|
||||
|
||||
//all solving related errors
|
||||
typedef boost::error_info<struct user_message,std::string> error_message;
|
||||
struct solving_error : virtual boost::exception { };
|
||||
|
||||
template<typename Kernel>
|
||||
struct Dogleg {
|
||||
|
||||
|
|
@ -51,7 +66,7 @@ struct Dogleg {
|
|||
typedef typename Kernel::number_type number_type;
|
||||
number_type tolg, tolx, tolf;
|
||||
|
||||
Dogleg() : tolg(1e-40), tolx(1e-20), tolf(1e-5) {
|
||||
Dogleg() : tolg(1e-40), tolx(1e-20), tolf(1e-6) {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
log.add_attribute("Tag", attrs::constant< std::string >("Dogleg"));
|
||||
|
|
@ -59,9 +74,9 @@ struct Dogleg {
|
|||
};
|
||||
|
||||
template <typename Derived, typename Derived2, typename Derived3, typename Derived4>
|
||||
int calculateStep(const Eigen::MatrixBase<Derived>& g, const Eigen::MatrixBase<Derived3>& jacobi,
|
||||
const Eigen::MatrixBase<Derived4>& residual, Eigen::MatrixBase<Derived2>& h_dl,
|
||||
const double delta) {
|
||||
void calculateStep(const Eigen::MatrixBase<Derived>& g, const Eigen::MatrixBase<Derived3>& jacobi,
|
||||
const Eigen::MatrixBase<Derived4>& residual, Eigen::MatrixBase<Derived2>& h_dl,
|
||||
const double delta) {
|
||||
|
||||
// get the steepest descent stepsize and direction
|
||||
const double alpha(g.squaredNorm()/(jacobi*g).squaredNorm());
|
||||
|
|
@ -122,8 +137,6 @@ struct Dogleg {
|
|||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
return 0;
|
||||
};
|
||||
|
||||
int solve(typename Kernel::MappedEquationSystem& sys) {
|
||||
|
|
@ -137,15 +150,16 @@ struct Dogleg {
|
|||
clock_t start = clock();
|
||||
clock_t inc_rec = clock();
|
||||
|
||||
if(!sys.isValid()) return 5;
|
||||
if(!sys.isValid())
|
||||
throw solving_error() << boost::errinfo_errno(5) << error_message("invalid equation system");
|
||||
|
||||
|
||||
bool translate = true;
|
||||
|
||||
typename Kernel::Vector h_dl, F_old(sys.m_eqns), g(sys.m_eqns);
|
||||
typename Kernel::Matrix J_old(sys.m_eqns, sys.m_params);
|
||||
typename Kernel::Vector h_dl, F_old(sys.equationCount()), g(sys.equationCount());
|
||||
typename Kernel::Matrix J_old(sys.equationCount(), sys.parameterCount());
|
||||
|
||||
sys.recalculate();
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "initial jacobi: "<<std::endl<<sys.Jacobi<<std::endl
|
||||
<< "residual: "<<sys.Residual.transpose()<<std::endl
|
||||
|
|
@ -177,15 +191,15 @@ struct Dogleg {
|
|||
if(fx_inf <= tolf*sys.Scaling) // Success
|
||||
stop = 1;
|
||||
else if(g_inf <= tolg)
|
||||
stop = 2;
|
||||
throw solving_error() << boost::errinfo_errno(2) << error_message("g infinity norm smaller below limit");
|
||||
else if(delta <= tolx)
|
||||
stop = 3;
|
||||
throw solving_error() << boost::errinfo_errno(3) << error_message("step size below limit");
|
||||
else if(iter >= maxIterNumber)
|
||||
stop = 4;
|
||||
throw solving_error() << boost::errinfo_errno(4) << error_message("maximal iterations reached");
|
||||
else if(!boost::math::isfinite(err))
|
||||
stop = 5;
|
||||
throw solving_error() << boost::errinfo_errno(5) << error_message("error is inf or nan");
|
||||
else if(err > diverging_lim)
|
||||
stop = 6;
|
||||
throw solving_error() << boost::errinfo_errno(6) << error_message("error diverged");
|
||||
|
||||
|
||||
// see if we are already finished
|
||||
|
|
@ -243,7 +257,7 @@ struct Dogleg {
|
|||
#endif
|
||||
rescale();
|
||||
sys.recalculate();
|
||||
}
|
||||
}
|
||||
//it can also happen that the differentials get too small, however, we cant check for that
|
||||
else if(iter>1 && (counter>50)) {
|
||||
rescale();
|
||||
|
|
@ -339,48 +353,81 @@ struct Kernel {
|
|||
typedef E::Matrix<Scalar, Dim, 1> type;
|
||||
};
|
||||
|
||||
|
||||
struct MappedEquationSystem {
|
||||
|
||||
Matrix Jacobi;
|
||||
Vector Parameter;
|
||||
Vector Residual;
|
||||
number_type Scaling;
|
||||
protected:
|
||||
Matrix m_jacobi;
|
||||
Vector m_parameter;
|
||||
|
||||
bool rot_only; //calculate only rotations?
|
||||
int m_params, m_eqns; //total amount
|
||||
int m_param_offset, m_eqn_offset; //current positions while creation
|
||||
int m_param_rot_offset, m_param_trans_offset, m_eqn_offset; //current positions while creation
|
||||
|
||||
public:
|
||||
MatrixMap Jacobi;
|
||||
VectorMap Parameter;
|
||||
Vector Residual;
|
||||
|
||||
number_type Scaling;
|
||||
|
||||
int parameterCount() {
|
||||
return m_params;
|
||||
};
|
||||
int equationCount() {
|
||||
return m_eqns;
|
||||
};
|
||||
|
||||
bool rotationOnly() {
|
||||
return rot_only;
|
||||
};
|
||||
|
||||
MappedEquationSystem(int params, int equations)
|
||||
: Jacobi(equations, params),
|
||||
Parameter(params), Residual(equations),
|
||||
m_params(params), m_eqns(equations), Scaling(1.) {
|
||||
: rot_only(false), m_jacobi(equations, params),
|
||||
m_parameter(params), Residual(equations),
|
||||
m_params(params), m_eqns(equations), Scaling(1.),
|
||||
Jacobi(&m_jacobi(0,0),equations,params,DynStride(equations,1)),
|
||||
Parameter(&m_parameter(0),params,DynStride(1,1)) {
|
||||
|
||||
m_param_offset = 0;
|
||||
m_param_rot_offset = 0;
|
||||
m_param_trans_offset = params;
|
||||
m_eqn_offset = 0;
|
||||
|
||||
Jacobi.setZero(); //important as some places are never written
|
||||
m_jacobi.setZero(); //important as some places are never written
|
||||
};
|
||||
|
||||
int setParameterMap(int number, VectorMap& map) {
|
||||
int setParameterMap(int number, VectorMap& map, ParameterType t = general) {
|
||||
|
||||
new(&map) VectorMap(&Parameter(m_param_offset), number, DynStride(1,1));
|
||||
m_param_offset += number;
|
||||
return m_param_offset-number;
|
||||
if(t == rotation) {
|
||||
new(&map) VectorMap(&m_parameter(m_param_rot_offset), number, DynStride(1,1));
|
||||
m_param_rot_offset += number;
|
||||
return m_param_rot_offset-number;
|
||||
} else {
|
||||
m_param_trans_offset -= number;
|
||||
new(&map) VectorMap(&m_parameter(m_param_trans_offset), number, DynStride(1,1));
|
||||
return m_param_trans_offset;
|
||||
}
|
||||
};
|
||||
int setParameterMap(Vector3Map& map) {
|
||||
int setParameterMap(Vector3Map& map, ParameterType t = general) {
|
||||
|
||||
new(&map) Vector3Map(&Parameter(m_param_offset));
|
||||
m_param_offset += 3;
|
||||
return m_param_offset-3;
|
||||
if(t == rotation) {
|
||||
new(&map) Vector3Map(&m_parameter(m_param_rot_offset));
|
||||
m_param_rot_offset += 3;
|
||||
return m_param_rot_offset-3;
|
||||
} else {
|
||||
m_param_trans_offset -= 3;
|
||||
new(&map) Vector3Map(&m_parameter(m_param_trans_offset));
|
||||
return m_param_trans_offset;
|
||||
}
|
||||
};
|
||||
int setResidualMap(VectorMap& map) {
|
||||
new(&map) VectorMap(&Residual(m_eqn_offset), 1, DynStride(1,1));
|
||||
return m_eqn_offset++;
|
||||
};
|
||||
void setJacobiMap(int eqn, int offset, int number, CVectorMap& map) {
|
||||
new(&map) CVectorMap(&Jacobi(eqn, offset), number, DynStride(0,m_eqns));
|
||||
new(&map) CVectorMap(&m_jacobi(eqn, offset), number, DynStride(0,m_eqns));
|
||||
};
|
||||
void setJacobiMap(int eqn, int offset, int number, VectorMap& map) {
|
||||
new(&map) VectorMap(&Jacobi(eqn, offset), number, DynStride(0,m_eqns));
|
||||
new(&map) VectorMap(&m_jacobi(eqn, offset), number, DynStride(0,m_eqns));
|
||||
};
|
||||
|
||||
bool isValid() {
|
||||
|
|
@ -388,6 +435,36 @@ struct Kernel {
|
|||
return true;
|
||||
};
|
||||
|
||||
void setAccess(ParameterType t) {
|
||||
|
||||
if(t==complete) {
|
||||
new(&Jacobi) VectorMap(&m_jacobi(0,0),m_eqns,m_params,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(0),m_params,DynStride(1,1));
|
||||
} else if(t==rotation) {
|
||||
int num = m_param_trans_offset;
|
||||
new(&Jacobi) VectorMap(&m_jacobi(0,0),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(0),num,DynStride(1,1));
|
||||
} else if(t==general) {
|
||||
int num = m_params - m_param_trans_offset;
|
||||
new(&Jacobi) VectorMap(&m_jacobi(0,m_param_trans_offset),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(m_param_trans_offset),num,DynStride(1,1));
|
||||
}
|
||||
};
|
||||
|
||||
void setGeneralEquationAccess(bool general) {
|
||||
rot_only = !general;
|
||||
};
|
||||
|
||||
bool hasParameterType(ParameterType t) {
|
||||
|
||||
if(t==rotation)
|
||||
return (m_param_rot_offset>0);
|
||||
else if(t==general)
|
||||
return (m_param_trans_offset<m_params);
|
||||
else
|
||||
return (m_params>0);
|
||||
};
|
||||
|
||||
virtual void recalculate() = 0;
|
||||
|
||||
};
|
||||
|
|
@ -423,3 +500,6 @@ struct Kernel {
|
|||
#endif //GCM_KERNEL_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -105,7 +105,7 @@ typedef boost::any Connection;
|
|||
template<typename Sys, typename Derived, typename Sig>
|
||||
struct Object : public boost::enable_shared_from_this<Derived> {
|
||||
|
||||
Object() {};
|
||||
Object() {};
|
||||
Object(Sys& system);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include <boost/graph/graph_traits.hpp>
|
||||
#include <boost/graph/buffer_concepts.hpp>
|
||||
#include <boost/graph/properties.hpp>
|
||||
#include <boost/fusion/sequence.hpp>
|
||||
#include <boost/fusion/container/vector.hpp>
|
||||
|
||||
|
|
@ -45,12 +46,14 @@ template<typename Graph>
|
|||
struct vertex_selector {
|
||||
typedef typename boost::graph_traits<Graph>::vertex_descriptor key_type;
|
||||
typedef typename Graph::vertex_properties sequence_type;
|
||||
typedef mpl::int_<1> property_distance;
|
||||
};
|
||||
|
||||
template<typename Graph>
|
||||
struct edge_selector {
|
||||
typedef typename boost::graph_traits<Graph>::edge_descriptor key_type;
|
||||
typedef typename Graph::edge_properties sequence_type;
|
||||
typedef mpl::int_<0> property_distance;
|
||||
};
|
||||
|
||||
template< typename Kind, typename Graph>
|
||||
|
|
@ -97,44 +100,14 @@ public:
|
|||
|
||||
typedef Property property;
|
||||
typedef typename dcm::details::property_selector<typename Property::kind, Graph>::sequence_type sequence;
|
||||
typedef typename dcm::details::property_selector<typename Property::kind, Graph>::property_distance distance;
|
||||
|
||||
property_map(Graph& g)
|
||||
property_map(boost::shared_ptr<Graph> g)
|
||||
: m_graph(g) { }
|
||||
|
||||
Graph& m_graph;
|
||||
boost::shared_ptr<Graph> m_graph;
|
||||
};
|
||||
|
||||
template<typename P, typename G>
|
||||
typename property_map<P,G>::value_type get(const property_map<P,G>& map,
|
||||
typename property_map<P,G>::key_type key) {
|
||||
|
||||
typedef property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
return fusion::at<distance>(fusion::at_c<0>(map.m_graph[key]));
|
||||
};
|
||||
|
||||
template <typename P, typename G>
|
||||
void put(const property_map<P,G>& map,
|
||||
typename property_map<P,G>::key_type key,
|
||||
const typename property_map<P,G>::value_type& value) {
|
||||
|
||||
typedef property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
fusion::at<distance>(fusion::at_c<0>(map.m_graph[key])) = value;
|
||||
};
|
||||
|
||||
|
||||
template <typename P, typename G>
|
||||
typename property_map<P,G>::reference at(const property_map<P,G>& map,
|
||||
typename property_map<P,G>::key_type key) {
|
||||
typedef property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
return fusion::at<distance>(fusion::at_c<0>(map.m_graph[key]));
|
||||
}
|
||||
|
||||
|
||||
//now create some standart properties
|
||||
//***********************************
|
||||
|
|
@ -143,18 +116,40 @@ struct empty_prop {
|
|||
typedef int kind;
|
||||
typedef int type;
|
||||
};
|
||||
|
||||
//type of a graph cluster
|
||||
struct type_prop {
|
||||
//states the type of a cluster
|
||||
typedef cluster_property kind;
|
||||
typedef int type;
|
||||
};
|
||||
|
||||
//cluster in graph changed?
|
||||
struct changed_prop {
|
||||
typedef cluster_property kind;
|
||||
typedef bool type;
|
||||
};
|
||||
//vertex index for bgl algorithms
|
||||
struct vertex_index_prop {
|
||||
typedef vertex_property kind;
|
||||
typedef int type;
|
||||
};
|
||||
//edge index for bgl algorithms
|
||||
struct edge_index_prop {
|
||||
typedef edge_property kind;
|
||||
typedef int type;
|
||||
};
|
||||
//vertex color for bgl algorithms
|
||||
struct vertex_color_prop {
|
||||
typedef vertex_property kind;
|
||||
typedef boost::default_color_type type;
|
||||
};
|
||||
//edge color for bgl algorithms
|
||||
struct edge_color_prop {
|
||||
typedef edge_property kind;
|
||||
typedef boost::default_color_type type;
|
||||
};
|
||||
|
||||
|
||||
//object id's
|
||||
template<typename T>
|
||||
struct id_prop {
|
||||
typedef object_property kind;
|
||||
|
|
@ -163,5 +158,44 @@ struct id_prop {
|
|||
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void pretty(T t) {
|
||||
std::cout<<__PRETTY_FUNCTION__<<std::endl;
|
||||
};
|
||||
|
||||
namespace boost {
|
||||
//access the propertymap needs to be boost visable
|
||||
template<typename P, typename G>
|
||||
typename dcm::property_map<P,G>::value_type get(const dcm::property_map<P,G>& map,
|
||||
typename dcm::property_map<P,G>::key_type key) {
|
||||
|
||||
typedef dcm::property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
|
||||
return fusion::at<distance>(fusion::at<typename dcm::property_map<P,G>::distance>(map.m_graph->operator[](key)));
|
||||
};
|
||||
|
||||
template <typename P, typename G>
|
||||
void put(const dcm::property_map<P,G>& map,
|
||||
typename dcm::property_map<P,G>::key_type key,
|
||||
const typename dcm::property_map<P,G>::value_type& value) {
|
||||
|
||||
typedef dcm::property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
fusion::at<distance>(fusion::at<typename dcm::property_map<P,G>::distance>(map.m_graph->operator[](key))) = value;
|
||||
};
|
||||
|
||||
|
||||
template <typename P, typename G>
|
||||
typename dcm::property_map<P,G>::reference at(const dcm::property_map<P,G>& map,
|
||||
typename dcm::property_map<P,G>::key_type key) {
|
||||
typedef dcm::property_map<P,G> map_t;
|
||||
typedef typename mpl::find<typename map_t::sequence, typename map_t::property>::type iterator;
|
||||
typedef typename mpl::distance<typename mpl::begin<typename map_t::sequence>::type, iterator>::type distance;
|
||||
return fusion::at<distance>(fusion::at<typename dcm::property_map<P,G>::distance>(map.m_graph->operator[](key)));
|
||||
}
|
||||
}
|
||||
|
||||
#endif //GCM_PROPERTY_H
|
||||
|
|
|
|||
|
|
@ -55,10 +55,6 @@ namespace details {
|
|||
|
||||
enum { subcluster = 10};
|
||||
|
||||
template<typename seq, typename state>
|
||||
struct vector_fold : mpl::fold< seq, state,
|
||||
mpl::push_back<mpl::_1,mpl::_2> > {};
|
||||
|
||||
template<typename seq, typename state>
|
||||
struct edge_fold : mpl::fold< seq, state,
|
||||
mpl::if_< is_edge_property<mpl::_2>,
|
||||
|
|
@ -81,7 +77,7 @@ struct obj_fold : mpl::fold< seq, state,
|
|||
mpl::push_back<mpl::_1,mpl::_2>, mpl::_1 > > {};
|
||||
|
||||
template<typename objects, typename properties>
|
||||
struct property_map {
|
||||
struct property_map_fold {
|
||||
typedef typename mpl::fold<
|
||||
objects, mpl::map<>, mpl::insert< mpl::_1, mpl::pair<
|
||||
mpl::_2, details::obj_fold<properties, mpl::vector<>, mpl::_2 > > > >::type type;
|
||||
|
|
@ -164,7 +160,7 @@ public:
|
|||
typedef typename details::cluster_fold< properties,
|
||||
mpl::vector<changed_prop, type_prop> >::type cluster_properties;
|
||||
|
||||
typedef typename details::property_map<objects, properties>::type object_properties;
|
||||
typedef typename details::property_map_fold<objects, properties>::type object_properties;
|
||||
|
||||
protected:
|
||||
//object storage
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#pragma warning( disable : 4503 )
|
||||
#endif
|
||||
|
||||
#include "module3d/defines.hpp"
|
||||
#include "module3d/geometry.hpp"
|
||||
#include "module3d/distance.hpp"
|
||||
#include "module3d/parallel.hpp"
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include <Eigen/StdVector>
|
||||
#include <boost/noncopyable.hpp>
|
||||
#include <opendcm/core/logging.hpp>
|
||||
#include <opendcm/core/kernel.hpp>
|
||||
#include "defines.hpp"
|
||||
|
||||
#define MAXFAKTOR 1.2 //the maximal distance allowd by a point normed to the cluster size
|
||||
|
|
@ -64,7 +65,7 @@ public:
|
|||
typename Kernel::Vector3Map m_normQ;
|
||||
typename Kernel::Quaternion m_resetQuaternion;
|
||||
|
||||
int m_offset;
|
||||
int m_offset, m_offset_rot;
|
||||
bool init, fix;
|
||||
std::vector<Geom> m_geometry;
|
||||
|
||||
|
|
@ -84,8 +85,8 @@ public:
|
|||
public:
|
||||
ClusterMath();
|
||||
|
||||
void setParameterOffset(int offset);
|
||||
int getParameterOffset();
|
||||
void setParameterOffset(int offset, ParameterType t);
|
||||
int getParameterOffset(ParameterType t);
|
||||
|
||||
typename Kernel::Vector3Map& getNormQuaternionMap();
|
||||
typename Kernel::Vector3Map& getTranslationMap();
|
||||
|
|
@ -137,7 +138,7 @@ private:
|
|||
const typename Kernel::Vector3& p2, const typename Kernel::Vector3& p3);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -160,13 +161,19 @@ ClusterMath<Sys>::ClusterMath() : m_normQ(NULL), m_translation(NULL), init(false
|
|||
};
|
||||
|
||||
template<typename Sys>
|
||||
void ClusterMath<Sys>::setParameterOffset(int offset) {
|
||||
m_offset = offset;
|
||||
void ClusterMath<Sys>::setParameterOffset(int offset, dcm::ParameterType t) {
|
||||
if(t == general)
|
||||
m_offset = offset;
|
||||
else
|
||||
m_offset_rot = offset;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
int ClusterMath<Sys>::getParameterOffset() {
|
||||
return m_offset;
|
||||
int ClusterMath<Sys>::getParameterOffset(ParameterType t) {
|
||||
if(t == general)
|
||||
return m_offset;
|
||||
else
|
||||
return m_offset_rot;
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
|
|
@ -243,7 +250,7 @@ void ClusterMath<Sys>::finishCalculation() {
|
|||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log) << "Finish calculation";
|
||||
#endif
|
||||
|
||||
|
||||
mapsToTransform(m_transform);
|
||||
init=false;
|
||||
|
||||
|
|
@ -419,7 +426,8 @@ void ClusterMath<Sys>::map_downstream::operator()(Geom g) {
|
|||
//allow iteration over all maped geometries
|
||||
m_clusterMath.addGeometry(g);
|
||||
//set the offsets so that geometry knows where it is in the parameter map
|
||||
g->m_offset = m_clusterMath.getParameterOffset();
|
||||
g->m_offset = m_clusterMath.getParameterOffset(general);
|
||||
g->m_offset_rot = m_clusterMath.getParameterOffset(rotation);
|
||||
//position and offset of the parameters must be set to the clusters values
|
||||
g->setClusterMode(true, m_isFixed);
|
||||
//calculate the appropriate local values
|
||||
|
|
@ -716,7 +724,7 @@ typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcOnePoint(const typename
|
|||
|
||||
template<typename Sys>
|
||||
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcTwoPoints(const typename ClusterMath<Sys>::Kernel::Vector3& p1,
|
||||
const typename ClusterMath<Sys>::Kernel::Vector3& p2) {
|
||||
const typename ClusterMath<Sys>::Kernel::Vector3& p2) {
|
||||
|
||||
//two points have their minimal scale at the mid position. Scaling perpendicular to this
|
||||
//line allows arbitrary scale values. Best is to have the scale dir move towards the origin
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ namespace dcm {
|
|||
namespace details {
|
||||
|
||||
//we need a custom orientation type to allow coincidents with points
|
||||
struct ci_orientation : public Equation<ci_orientation, Direction> {
|
||||
struct ci_orientation : public Equation<ci_orientation, Direction, true> {
|
||||
|
||||
using Equation::operator=;
|
||||
ci_orientation() : Equation(parallel) {};
|
||||
|
|
@ -37,6 +37,11 @@ struct ci_orientation : public Equation<ci_orientation, Direction> {
|
|||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct type : public PseudoScale<Kernel> {
|
||||
|
||||
type() {
|
||||
throw constraint_error() << boost::errinfo_errno(103) << error_message("unsupported geometry in coincidence/alignment orientation constraint")
|
||||
<< error_type_first_geometry(typeid(Tag1).name()) << error_type_second_geometry(typeid(Tag2).name());
|
||||
};
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
|
@ -126,6 +131,11 @@ struct ci_distance : public Equation<ci_distance, double> {
|
|||
template< typename Kernel, typename Tag1, typename Tag2 >
|
||||
struct type : public PseudoScale<Kernel> {
|
||||
|
||||
type() {
|
||||
throw constraint_error() << boost::errinfo_errno(104) << error_message("unsupported geometry in coincidence/alignment distance constraint")
|
||||
<< error_type_first_geometry(typeid(Tag1).name()) << error_type_second_geometry(typeid(Tag2).name());
|
||||
};
|
||||
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename Kernel::VectorMap Vector;
|
||||
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
#ifndef GCM_DEFINES_3D_H
|
||||
#define GCM_DEFINES_3D_H
|
||||
|
||||
#include <boost/exception/exception.hpp>
|
||||
|
||||
namespace dcm {
|
||||
namespace details {
|
||||
|
||||
|
|
@ -28,6 +30,11 @@ enum { cluster3D = 100};
|
|||
struct m3d {}; //base of module3d::type to allow other modules check for it
|
||||
|
||||
}
|
||||
|
||||
//exception codes are needed by the user
|
||||
//typedef boost::error_info<struct tag_solver_failure_type,int> solver_failure_type;
|
||||
struct creation_error : virtual boost::exception {};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -212,6 +212,8 @@ struct Module3D {
|
|||
|
||||
void removeGeometry3D(Identifier id);
|
||||
void removeConstraint3D(Identifier id);
|
||||
using inheriter_base::removeGeometry3D;
|
||||
using inheriter_base::removeConstraint3D;
|
||||
|
||||
bool hasGeometry3D(Identifier id);
|
||||
Geom getGeometry3D(Identifier id);
|
||||
|
|
|
|||
|
|
@ -20,12 +20,16 @@
|
|||
#ifndef GCM_SOLVER_3D_H
|
||||
#define GCM_SOLVER_3D_H
|
||||
|
||||
#include <boost/graph/adjacency_list.hpp>
|
||||
|
||||
#include "defines.hpp"
|
||||
#include "clustermath.hpp"
|
||||
#include "opendcm/core/sheduler.hpp"
|
||||
#include "opendcm/core/traits.hpp"
|
||||
#include <opendcm/core/kernel.hpp>
|
||||
#include <opendcm/core/property.hpp>
|
||||
|
||||
#include <boost/graph/adjacency_list.hpp>
|
||||
#include <boost/graph/depth_first_search.hpp>
|
||||
#include <boost/exception/errinfo_errno.hpp>
|
||||
|
||||
namespace dcm {
|
||||
namespace details {
|
||||
|
|
@ -88,6 +92,23 @@ struct SystemSolver : public Job<Sys> {
|
|||
Eigen::aligned_allocator<typename Kernel::Vector3> >& vec);
|
||||
};
|
||||
|
||||
struct DummyScaler {
|
||||
void operator()() {};
|
||||
};
|
||||
|
||||
struct cycle_dedector:public boost::default_dfs_visitor {
|
||||
|
||||
bool& m_dedected;
|
||||
cycle_dedector(bool& ed) : m_dedected(ed) {
|
||||
m_dedected = false;
|
||||
};
|
||||
|
||||
template <class Edge, class Graph>
|
||||
void back_edge(Edge u, const Graph& g) {
|
||||
m_dedected = true;
|
||||
}
|
||||
};
|
||||
|
||||
SystemSolver();
|
||||
virtual void execute(Sys& sys);
|
||||
void solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sys);
|
||||
|
|
@ -128,7 +149,7 @@ void MES<Sys>::recalculate() {
|
|||
std::pair< oiter, oiter > oit = m_cluster->template getObjects<Constraint3D>(*eit.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
if(*oit.first)
|
||||
(*oit.first)->calculate(Base::Scaling);
|
||||
(*oit.first)->calculate(Base::Scaling, Base::rot_only);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -185,10 +206,10 @@ typename SystemSolver<Sys>::Scalar SystemSolver<Sys>::Rescaler::scaleClusters()
|
|||
|
||||
template<typename Sys>
|
||||
void SystemSolver<Sys>::Rescaler::collectPseudoPoints(
|
||||
boost::shared_ptr<typename SystemSolver<Sys>::Cluster> parent,
|
||||
LocalVertex cluster,
|
||||
std::vector<typename SystemSolver<Sys>::Kernel::Vector3,
|
||||
Eigen::aligned_allocator<typename SystemSolver<Sys>::Kernel::Vector3> >& vec) {
|
||||
boost::shared_ptr<typename SystemSolver<Sys>::Cluster> parent,
|
||||
LocalVertex cluster,
|
||||
std::vector<typename SystemSolver<Sys>::Kernel::Vector3,
|
||||
Eigen::aligned_allocator<typename SystemSolver<Sys>::Kernel::Vector3> >& vec) {
|
||||
|
||||
std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > vec2;
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> c_iter;
|
||||
|
|
@ -233,138 +254,202 @@ void SystemSolver<Sys>::execute(Sys& sys) {
|
|||
template<typename Sys>
|
||||
void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sys) {
|
||||
|
||||
//set out and solve all relevant subclusters
|
||||
typedef typename Cluster::cluster_iterator citer;
|
||||
std::pair<citer, citer> cit = cluster->clusters();
|
||||
for(; cit.first != cit.second; cit.first++) {
|
||||
try {
|
||||
//set out and solve all relevant subclusters
|
||||
typedef typename Cluster::cluster_iterator citer;
|
||||
std::pair<citer, citer> cit = cluster->clusters();
|
||||
for(; cit.first != cit.second; cit.first++) {
|
||||
|
||||
boost::shared_ptr<Cluster> c = (*cit.first).second;
|
||||
if(c->template getClusterProperty<changed_prop>() &&
|
||||
c->template getClusterProperty<type_prop>() == details::cluster3D)
|
||||
solveCluster(c, sys);
|
||||
}
|
||||
boost::shared_ptr<Cluster> c = (*cit.first).second;
|
||||
if(c->template getClusterProperty<changed_prop>() &&
|
||||
c->template getClusterProperty<type_prop>() == details::cluster3D)
|
||||
solveCluster(c, sys);
|
||||
}
|
||||
|
||||
int params=0, constraints=0;
|
||||
typename Kernel::number_type scale = 1;
|
||||
int params=0, constraints=0;
|
||||
typename Kernel::number_type scale = 1;
|
||||
|
||||
//get the ammount of parameters and constraint equations we need
|
||||
typedef typename boost::graph_traits<Cluster>::vertex_iterator iter;
|
||||
std::pair<iter, iter> it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
//get the ammount of parameters and constraint equations we need
|
||||
typedef typename boost::graph_traits<Cluster>::vertex_iterator iter;
|
||||
std::pair<iter, iter> it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
//when cluster and not fixed it has trans and rot parameter
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first)) {
|
||||
params += 6;
|
||||
}
|
||||
} else {
|
||||
params += cluster->template getObject<Geometry3D>(*it.first)->m_parameterCount;
|
||||
//when cluster and not fixed it has trans and rot parameter
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first)) {
|
||||
params += 6;
|
||||
}
|
||||
} else {
|
||||
params += cluster->template getObject<Geometry3D>(*it.first)->m_parameterCount;
|
||||
};
|
||||
}
|
||||
|
||||
//count the equations in the constraints
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> ocit;
|
||||
typedef typename boost::graph_traits<Cluster>::edge_iterator e_iter;
|
||||
std::pair<e_iter, e_iter> e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
std::pair< ocit, ocit > it = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; it.first != it.second; it.first++)
|
||||
constraints += (*it.first)->equationCount();
|
||||
};
|
||||
}
|
||||
|
||||
//count the equations in the constraints
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> ocit;
|
||||
typedef typename boost::graph_traits<Cluster>::edge_iterator e_iter;
|
||||
std::pair<e_iter, e_iter> e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
std::pair< ocit, ocit > it = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; it.first != it.second; it.first++)
|
||||
constraints += (*it.first)->equationCount();
|
||||
};
|
||||
|
||||
if(params <= 0 || constraints <= 0) {
|
||||
//TODO:throw
|
||||
if(params <= 0 || constraints <= 0) {
|
||||
//TODO:throw
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Error in system counting: params = " << params << " and constraints = "<<constraints;
|
||||
BOOST_LOG(log)<< "Error in system counting: params = " << params << " and constraints = "<<constraints;
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//initialise the system with now known size
|
||||
Mes mes(cluster, params, constraints);
|
||||
//initialise the system with now known size
|
||||
Mes mes(cluster, params, constraints);
|
||||
|
||||
//iterate all geometrys again and set the needed maps
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
//iterate all geometrys again and set the needed maps
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
details::ClusterMath<Sys>& cm = c->template getClusterProperty<math_prop>();
|
||||
//only get maps and propagate downstream if not fixed
|
||||
if(!c->template getClusterProperty<fix_prop>()) {
|
||||
//set norm Quaternion as map to the parameter vector
|
||||
int offset = mes.setParameterMap(cm.getNormQuaternionMap());
|
||||
//set translation as map to the parameter vector
|
||||
mes.setParameterMap(cm.getTranslationMap());
|
||||
//write initail values to the parameter maps
|
||||
//remember the parameter offset as all downstream geometry must use this offset
|
||||
cm.setParameterOffset(offset);
|
||||
//wirte initial values
|
||||
cm.initMaps();
|
||||
} else cm.initFixMaps();
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
details::ClusterMath<Sys>& cm = c->template getClusterProperty<math_prop>();
|
||||
//only get maps and propagate downstream if not fixed
|
||||
if(!c->template getClusterProperty<fix_prop>()) {
|
||||
//set norm Quaternion as map to the parameter vector
|
||||
int offset_rot = mes.setParameterMap(cm.getNormQuaternionMap(), rotation);
|
||||
//set translation as map to the parameter vector
|
||||
int offset = mes.setParameterMap(cm.getTranslationMap(), general);
|
||||
//write initail values to the parameter maps
|
||||
//remember the parameter offset as all downstream geometry must use this offset
|
||||
cm.setParameterOffset(offset_rot, rotation);
|
||||
cm.setParameterOffset(offset, general);
|
||||
//wirte initial values
|
||||
cm.initMaps();
|
||||
} else cm.initFixMaps();
|
||||
|
||||
//map all geometrie within that cluster to it's rotation matrix
|
||||
//for collecting all geometries which need updates
|
||||
cm.clearGeometry();
|
||||
cm.mapClusterDownstreamGeometry(c);
|
||||
//map all geometrie within that cluster to it's rotation matrix
|
||||
//for collecting all geometries which need updates
|
||||
cm.clearGeometry();
|
||||
cm.mapClusterDownstreamGeometry(c);
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
int offset = mes.setParameterMap(g->m_parameterCount, g->getParameterMap());
|
||||
g->m_offset = offset;
|
||||
//init the parametermap with initial values
|
||||
g->initMap();
|
||||
}
|
||||
}
|
||||
|
||||
//and now the constraints to set the residual and gradient maps
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> oiter;
|
||||
e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
|
||||
//as always: every local edge can hold multiple global ones, so iterate over all constraints
|
||||
//hold by the individual edge
|
||||
std::pair< oiter, oiter > oit = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
|
||||
//set the maps
|
||||
Cons c = *oit.first;
|
||||
if(c) c->setMaps(mes);
|
||||
//TODO: else throw (as every global edge was counted as one equation)
|
||||
}
|
||||
}
|
||||
|
||||
//if we don't have rotations we need no expensive scaling code
|
||||
if(!mes.hasParameterType(rotation)) {
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "No rotation parameters in system, solve without scaling";
|
||||
#endif
|
||||
DummyScaler re;
|
||||
Kernel::solve(mes, re);
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
int offset = mes.setParameterMap(g->m_parameterCount, g->getParameterMap());
|
||||
g->m_offset = offset;
|
||||
//init the parametermap with initial values
|
||||
g->initMap();
|
||||
}
|
||||
}
|
||||
|
||||
//and now the constraints to set the residual and gradient maps
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> oiter;
|
||||
e_it = boost::edges(*cluster);
|
||||
for(; e_it.first != e_it.second; e_it.first++) {
|
||||
// we have rotations, so let's check our options. first search for cycles, as systems with them
|
||||
// always need the full solver power
|
||||
bool has_cycle;
|
||||
cycle_dedector cd(has_cycle);
|
||||
//create te needed property map, fill it and run the test
|
||||
property_map<vertex_index_prop, Cluster> vi_map(cluster);
|
||||
cluster->initIndexMaps();
|
||||
boost::depth_first_search(*cluster.get(), boost::visitor(cd).vertex_index_map(vi_map));
|
||||
|
||||
//as always: every local edge can hold multiple global ones, so iterate over all constraints
|
||||
//hold by the individual edge
|
||||
std::pair< oiter, oiter > oit = cluster->template getObjects<Constraint3D>(*e_it.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
|
||||
//set the maps
|
||||
Cons c = *oit.first;
|
||||
if(c) c->setMaps(mes);
|
||||
//TODO: else throw (as every global edge was counted as one equation)
|
||||
}
|
||||
}
|
||||
|
||||
int stop = 0;
|
||||
Rescaler re(cluster, mes);
|
||||
re();
|
||||
stop = Kernel::solve(mes, re);
|
||||
bool done = false;
|
||||
if(!has_cycle) {
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Numbers of rescale: "<<re.rescales;
|
||||
BOOST_LOG(log)<< "non-cyclic system dedected"
|
||||
#endif
|
||||
//cool, lets do uncylic. first all rotational constraints with rotational parameters
|
||||
mes.setAccess(rotation);
|
||||
mes.setGeneralEquationAccess(false);
|
||||
//solve can be done without catching exceptions, because this only fails if the system in
|
||||
//unsolvable
|
||||
DummyScaler re;
|
||||
Kernel::solve(mes, re);
|
||||
|
||||
//now go to all relevant geometries and clusters and write the values back
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
//now let's see if we have to go on with the translations
|
||||
mes.setAccess(general);
|
||||
mes.setGeneralEquationAccess(true);
|
||||
mes.recalculate();
|
||||
if(mes.Residual.norm()<1e-6)
|
||||
done = true;
|
||||
else {
|
||||
//let's try translation only
|
||||
try {
|
||||
DummyScaler re;
|
||||
Kernel::solve(mes, re);
|
||||
done=true;
|
||||
} catch(boost::exception& e) {
|
||||
//not successful, so we need brute force
|
||||
done = false;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first))
|
||||
c->template getClusterProperty<math_prop>().finishCalculation();
|
||||
else
|
||||
c->template getClusterProperty<math_prop>().finishFixCalculation();
|
||||
|
||||
std::vector<Geom>& vec = c->template getClusterProperty<math_prop>().getGeometry();
|
||||
for(typename std::vector<Geom>::iterator vit = vec.begin(); vit != vec.end(); vit++)
|
||||
(*vit)->finishCalculation();
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(mes.Scaling);
|
||||
g->finishCalculation();
|
||||
//not done already? try it the hard way!
|
||||
if(!done) {
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Full scale solver used"
|
||||
#endif
|
||||
Rescaler re(cluster, mes);
|
||||
re();
|
||||
Kernel::solve(mes, re);
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Numbers of rescale: "<<re.rescales;
|
||||
#endif
|
||||
};
|
||||
}
|
||||
|
||||
//solving is done, now go to all relevant geometries and clusters and write the values back
|
||||
it = boost::vertices(*cluster);
|
||||
for(; it.first != it.second; it.first++) {
|
||||
|
||||
if(cluster->isCluster(*it.first)) {
|
||||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first))
|
||||
c->template getClusterProperty<math_prop>().finishCalculation();
|
||||
else
|
||||
c->template getClusterProperty<math_prop>().finishFixCalculation();
|
||||
|
||||
std::vector<Geom>& vec = c->template getClusterProperty<math_prop>().getGeometry();
|
||||
for(typename std::vector<Geom>::iterator vit = vec.begin(); vit != vec.end(); vit++)
|
||||
(*vit)->finishCalculation();
|
||||
|
||||
} else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(mes.Scaling);
|
||||
g->finishCalculation();
|
||||
}
|
||||
}
|
||||
//we have solved this cluster
|
||||
cluster->template setClusterProperty<changed_prop>(false);
|
||||
|
||||
} catch(boost::exception& x) {
|
||||
throw;
|
||||
}
|
||||
//we have solved this cluster
|
||||
cluster->template setClusterProperty<changed_prop>(false);
|
||||
};
|
||||
|
||||
}//details
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user