diff --git a/src/Mod/Assembly/App/ItemAssembly.cpp b/src/Mod/Assembly/App/ItemAssembly.cpp index ec8223890..cf932865d 100644 --- a/src/Mod/Assembly/App/ItemAssembly.cpp +++ b/src/Mod/Assembly/App/ItemAssembly.cpp @@ -25,6 +25,7 @@ #ifndef _PreComp_ # include # include +#include #endif #include @@ -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(new Solver); - init(boost::shared_ptr()); - - //get the constraint group and init the constraints - typedef std::vector::const_iterator iter; - - const std::vector& vector = Annotations.getValues(); - for(iter it=vector.begin(); it != vector.end(); it++) { - - if( (*it)->getTypeId() == Assembly::ConstraintGroup::getClassTypeId() ) - static_cast(*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(new Solver); + init(boost::shared_ptr()); + + //get the constraint group and init the constraints + typedef std::vector::const_iterator iter; + + const std::vector& vector = Annotations.getValues(); + for(iter it=vector.begin(); it != vector.end(); it++) { + + if((*it)->getTypeId() == Assembly::ConstraintGroup::getClassTypeId()) + static_cast(*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(e), + boost::get_error_info(e)->c_str()); + } catch(dcm::creation_error& e) { + Base::Console().Error("Creation failed with error %i: %s", + *boost::get_error_info(e), + boost::get_error_info(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 s; std::vector obj = Items.getValues(); std::vector::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(*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::iterator it = s.begin(); it != s.end(); ++it) { + for(std::vector::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::const_iterator iter; - + const std::vector& vector = Items.getValues(); for(iter it=vector.begin(); it != vector.end(); it++) { - - if( (*it)->getTypeId() == Assembly::ItemPart::getClassTypeId() ) - if(*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::const_iterator iter; - + const std::vector& vector = Items.getValues(); for(iter it=vector.begin(); it != vector.end(); it++) { - - if( (*it)->getTypeId() == Assembly::ItemPart::getClassTypeId() ) { - if(*it == part) - return this; - } - else if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) { - - Assembly::ItemAssembly* assembly = static_cast(*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(*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::const_iterator iter; - + const std::vector& vector = Items.getValues(); for(iter it=vector.begin(); it != vector.end(); it++) { - - if( (*it)->getTypeId() == Assembly::ItemPart::getClassTypeId() ) { - if(static_cast(*it)->holdsObject(obj)) - return static_cast(*it); - } - else if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) { - - Assembly::ItemPart* part = static_cast(*it)->getContainingPart(obj); - if(part) - return part; - } + + if((*it)->getTypeId() == Assembly::ItemPart::getClassTypeId()) { + if(static_cast(*it)->holdsObject(obj)) + return static_cast(*it); + } else if((*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId()) { + + Assembly::ItemPart* part = static_cast(*it)->getContainingPart(obj); + if(part) + return part; + } }; - + return NULL; } void ItemAssembly::init(boost::shared_ptr parent) { if(parent) - m_solver = boost::shared_ptr(parent->createSubsystem()); - + m_solver = boost::shared_ptr(parent->createSubsystem()); + typedef std::vector::const_iterator iter; - + const std::vector& vector = Items.getValues(); for(iter it=vector.begin(); it != vector.end(); it++) { - - if ( (*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId() ) { - - static_cast(*it)->init(m_solver); - } + + if((*it)->getTypeId() == Assembly::ItemAssembly::getClassTypeId()) { + + static_cast(*it)->init(m_solver); + } }; } -} \ No newline at end of file +} diff --git a/src/Mod/Assembly/App/opendcm/core/clustergraph.hpp b/src/Mod/Assembly/App/opendcm/core/clustergraph.hpp index 7e2e69837..9ac0cb3fe 100644 --- a/src/Mod/Assembly/App/opendcm/core/clustergraph.hpp +++ b/src/Mod/Assembly/App/opendcm/core/clustergraph.hpp @@ -61,6 +61,15 @@ namespace dcm { namespace details { +template +struct vector_fold : mpl::fold< seq, state, + mpl::push_back > {}; + +//also add basic properties for graph algorithms like index and color +typedef mpl::vector2 bgl_v_props; +typedef mpl::vector2 bgl_e_props; + + typedef boost::adjacency_list_traits 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::type, + fusion::vector< GlobalVertex, + typename details::pts::type>::type, typename details::sps::type>, - fusion::vector< typename details::pts::type, + fusion::vector< typename details::pts::type>::type, std::vector< fusion::vector< typename details::sps::type, GlobalEdge > > > >, public boost::noncopyable, public boost::enable_shared_from_this > { public: + typedef typename details::vector_fold::type edge_properties; + typedef typename details::vector_fold::type vertex_properties; + typedef fusion::vector< typename details::sps::type, GlobalEdge > edge_bundle_single; - typedef fusion::vector< typename details::pts::type, std::vector< edge_bundle_single > > edge_bundle; + typedef fusion::vector< typename details::pts::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::type, + typedef fusion::vector< GlobalVertex, typename details::pts::type, typename details::sps::type > vertex_bundle; @@ -154,7 +167,7 @@ public: typename mpl::end::type >, typename mpl::push_back::type, cluster_prop >::type cluster_properties; - + typedef typename details::pts::type cluster_bundle; typedef typename boost::graph_traits::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 void copyInto(boost::shared_ptr 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 IndexMap; IndexMap mapIndex; boost::associative_property_map propmapIndex(mapIndex); @@ -1106,11 +1117,11 @@ protected: typedef typename prop::type base_type; typedef base_type& result_type; - typedef typename mpl::find::type vertex_iterator; - typedef typename mpl::find::type edge_iterator; - typedef typename mpl::if_::type >, + typedef typename mpl::find::type vertex_iterator; + typedef typename mpl::find::type edge_iterator; + typedef typename mpl::if_::type >, edge_iterator, vertex_iterator>::type iterator; - BOOST_MPL_ASSERT((mpl::not_::type > >)); + BOOST_MPL_ASSERT((mpl::not_::type > >)); //used with vertex bundle type template @@ -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 vit = boost::vertices(*this); + for(int c=0; vit.first != vit.second; vit.first++, c++) + setProperty(*vit.first, c); + + std::pair eit = boost::edges(*this); + for(int c=0; eit.first != eit.second; eit.first++, c++) + setProperty(*eit.first, c); + }; + /******************************************************** diff --git a/src/Mod/Assembly/App/opendcm/core/constraint.hpp b/src/Mod/Assembly/App/opendcm/core/constraint.hpp index db1fc39d1..a54685bac 100644 --- a/src/Mod/Assembly/App/opendcm/core/constraint.hpp +++ b/src/Mod/Assembly/App/opendcm/core/constraint.hpp @@ -75,17 +75,17 @@ public: std::vector getGenericConstraints(); std::vector getEquationTypes(); std::vector getConstraintTypes(); - + template 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 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 getGenericEquations() = 0; - virtual std::vector getGenericConstraints() = 0; - virtual std::vector getEquationTypes() = 0; - virtual std::vector getConstraintTypes() = 0; + virtual std::vector getGenericConstraints() = 0; + virtual std::vector getEquationTypes() = 0; + virtual std::vector 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 void operator()(T& val) const; }; - - struct GenericConstraints { + + struct GenericConstraints { std::vector& vec; GenericConstraints(std::vector& v); template void operator()(T& val) const; }; - - struct Types { + + struct Types { std::vector& vec; Types(std::vector& 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::value; }; - + virtual std::vector getGenericEquations(); - virtual std::vector getGenericConstraints(); - virtual std::vector getEquationTypes(); - virtual std::vector getConstraintTypes(); + virtual std::vector getGenericConstraints(); + virtual std::vector getEquationTypes(); + virtual std::vector getConstraintTypes(); EquationSets m_sets; - Objects m_objects; + Objects m_objects; }; protected: @@ -282,15 +286,15 @@ Constraint::Constraint(Sys& system, geom_p : first(f), second(s), content(0) { this->m_system = &system; - cf = first->template connectSignal (boost::bind(&Constraint::geometryReset, this, _1)); - cs = second->template connectSignal (boost::bind(&Constraint::geometryReset, this, _1)); + //cf = first->template connectSignal (boost::bind(&Constraint::geometryReset, this, _1)); + //cs = second->template connectSignal (boost::bind(&Constraint::geometryReset, this, _1)); }; template Constraint::~Constraint() { delete content; - first->template disconnectSignal(cf); - second->template disconnectSignal(cs); + //first->template disconnectSignal(cf); + //second->template disconnectSignal(cs); }; template @@ -342,8 +346,8 @@ void Constraint::resetType(creator_type& c }; template -void Constraint::calculate(Scalar scale) { - content->calculate(first, second, scale); +void Constraint::calculate(Scalar scale, bool rotation_only) { + content->calculate(first, second, scale, rotation_only); }; template @@ -391,6 +395,7 @@ Constraint::holder::type, iterator>::type distance; BOOST_MPL_ASSERT((mpl::not_::type > >)); val.m_eq.value = fusion::at(objects).value; + val.pure_rotation = fusion::at(objects).pure_rotation; }; template @@ -403,7 +408,8 @@ Constraint::holder template -Constraint::holder::Calculater::Calculater(geom_ptr f, geom_ptr s, Scalar sc) : first(f), second(s), scale(sc) { +Constraint::holder::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 template< typename T > void Constraint::holder::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::holdergetClusterMode()) { 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 template< typename T > void Constraint::holder::Types::operator()(T& val) const { vec.push_back(&typeid(T)); -}; +}; template template @@ -555,8 +600,9 @@ Constraint::holder template -void Constraint::holder::calculate(geom_ptr first, geom_ptr second, Scalar scale) { - fusion::for_each(m_sets, Calculater(first, second, scale)); +void Constraint::holder::calculate(geom_ptr first, geom_ptr second, + Scalar scale, bool rotation_only) { + fusion::for_each(m_sets, Calculater(first, second, scale, rotation_only)); }; template @@ -589,19 +635,19 @@ Constraint::holder template -std::vector +std::vector Constraint::holder::getGenericEquations() { std::vector vec; - fusion::for_each( m_sets, GenericEquations(vec) ); + fusion::for_each(m_sets, GenericEquations(vec)); return vec; }; template template -std::vector +std::vector Constraint::holder::getGenericConstraints() { std::vector vec; - fusion::for_each( m_objects, GenericConstraints(vec) ); + fusion::for_each(m_objects, GenericConstraints(vec)); return vec; }; @@ -610,7 +656,7 @@ template std::vector Constraint::holder::getEquationTypes() { std::vector vec; - mpl::for_each< EquationVector >( Types(vec) ); + mpl::for_each< EquationVector >(Types(vec)); return vec; }; @@ -619,7 +665,7 @@ template std::vector Constraint::holder::getConstraintTypes() { std::vector vec; - mpl::for_each< ConstraintVector >( Types(vec) ); + mpl::for_each< ConstraintVector >(Types(vec)); return vec; }; diff --git a/src/Mod/Assembly/App/opendcm/core/equations.hpp b/src/Mod/Assembly/App/opendcm/core/equations.hpp index 8f3caae45..09c12d0b7 100644 --- a/src/Mod/Assembly/App/opendcm/core/equations.hpp +++ b/src/Mod/Assembly/App/opendcm/core/equations.hpp @@ -35,6 +35,8 @@ #include #include +#include + 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 error_type_first_geometry; +typedef boost::error_info error_type_second_geometry; + struct no_option {}; template @@ -143,13 +150,14 @@ struct pushed_seq { typedef constraint_sequence type; }; -template +template 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 { 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 > Vec; @@ -240,7 +253,7 @@ struct Distance : public Equation { //the possible directions enum Direction { parallel, equal, opposite, perpendicular }; -struct Orientation : public Equation { +struct Orientation : public Equation { using Equation::operator=; Orientation() : Equation(parallel) {}; @@ -248,6 +261,11 @@ struct Orientation : public Equation { template< typename Kernel, typename Tag1, typename Tag2 > struct type : public PseudoScale { + 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 { }; }; -struct Angle : public Equation { +struct Angle : public Equation { using Equation::operator=; Angle() : Equation(0) {}; @@ -283,6 +301,11 @@ struct Angle : public Equation { template< typename Kernel, typename Tag1, typename Tag2 > struct type : public PseudoScale { + 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 { //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); diff --git a/src/Mod/Assembly/App/opendcm/core/geometry.hpp b/src/Mod/Assembly/App/opendcm/core/geometry.hpp index 131a2d24e..a8c5d6746 100644 --- a/src/Mod/Assembly/App/opendcm/core/geometry.hpp +++ b/src/Mod/Assembly/App/opendcm/core/geometry.hpp @@ -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; diff --git a/src/Mod/Assembly/App/opendcm/core/kernel.hpp b/src/Mod/Assembly/App/opendcm/core/kernel.hpp index 949e3b654..80f37d753 100644 --- a/src/Mod/Assembly/App/opendcm/core/kernel.hpp +++ b/src/Mod/Assembly/App/opendcm/core/kernel.hpp @@ -27,6 +27,10 @@ #include #include +#include +#include +#include + #include #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 error_message; +struct solving_error : virtual boost::exception { }; + template 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 - int calculateStep(const Eigen::MatrixBase& g, const Eigen::MatrixBase& jacobi, - const Eigen::MatrixBase& residual, Eigen::MatrixBase& h_dl, - const double delta) { + void calculateStep(const Eigen::MatrixBase& g, const Eigen::MatrixBase& jacobi, + const Eigen::MatrixBase& residual, Eigen::MatrixBase& 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: "<= 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 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_offset0); + }; + virtual void recalculate() = 0; }; @@ -423,3 +500,6 @@ struct Kernel { #endif //GCM_KERNEL_H + + + diff --git a/src/Mod/Assembly/App/opendcm/core/object.hpp b/src/Mod/Assembly/App/opendcm/core/object.hpp index 09a1d1ac3..3218602d8 100644 --- a/src/Mod/Assembly/App/opendcm/core/object.hpp +++ b/src/Mod/Assembly/App/opendcm/core/object.hpp @@ -105,7 +105,7 @@ typedef boost::any Connection; template struct Object : public boost::enable_shared_from_this { - Object() {}; + Object() {}; Object(Sys& system); /** diff --git a/src/Mod/Assembly/App/opendcm/core/property.hpp b/src/Mod/Assembly/App/opendcm/core/property.hpp index dd7b68bba..b42b1f018 100644 --- a/src/Mod/Assembly/App/opendcm/core/property.hpp +++ b/src/Mod/Assembly/App/opendcm/core/property.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -45,12 +46,14 @@ template struct vertex_selector { typedef typename boost::graph_traits::vertex_descriptor key_type; typedef typename Graph::vertex_properties sequence_type; + typedef mpl::int_<1> property_distance; }; template struct edge_selector { typedef typename boost::graph_traits::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::sequence_type sequence; + typedef typename dcm::details::property_selector::property_distance distance; - property_map(Graph& g) + property_map(boost::shared_ptr g) : m_graph(g) { } - Graph& m_graph; + boost::shared_ptr m_graph; }; -template -typename property_map::value_type get(const property_map& map, - typename property_map::key_type key) { - - typedef property_map map_t; - typedef typename mpl::find::type iterator; - typedef typename mpl::distance::type, iterator>::type distance; - return fusion::at(fusion::at_c<0>(map.m_graph[key])); -}; - -template -void put(const property_map& map, - typename property_map::key_type key, - const typename property_map::value_type& value) { - - typedef property_map map_t; - typedef typename mpl::find::type iterator; - typedef typename mpl::distance::type, iterator>::type distance; - fusion::at(fusion::at_c<0>(map.m_graph[key])) = value; -}; - - -template -typename property_map::reference at(const property_map& map, - typename property_map::key_type key) { - typedef property_map map_t; - typedef typename mpl::find::type iterator; - typedef typename mpl::distance::type, iterator>::type distance; - return fusion::at(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 struct id_prop { typedef object_property kind; @@ -163,5 +158,44 @@ struct id_prop { } +template +void pretty(T t) { + std::cout<<__PRETTY_FUNCTION__< +typename dcm::property_map::value_type get(const dcm::property_map& map, + typename dcm::property_map::key_type key) { + + typedef dcm::property_map map_t; + typedef typename mpl::find::type iterator; + typedef typename mpl::distance::type, iterator>::type distance; + + return fusion::at(fusion::at::distance>(map.m_graph->operator[](key))); +}; + +template +void put(const dcm::property_map& map, + typename dcm::property_map::key_type key, + const typename dcm::property_map::value_type& value) { + + typedef dcm::property_map map_t; + typedef typename mpl::find::type iterator; + typedef typename mpl::distance::type, iterator>::type distance; + fusion::at(fusion::at::distance>(map.m_graph->operator[](key))) = value; +}; + + +template +typename dcm::property_map::reference at(const dcm::property_map& map, + typename dcm::property_map::key_type key) { + typedef dcm::property_map map_t; + typedef typename mpl::find::type iterator; + typedef typename mpl::distance::type, iterator>::type distance; + return fusion::at(fusion::at::distance>(map.m_graph->operator[](key))); +} +} #endif //GCM_PROPERTY_H diff --git a/src/Mod/Assembly/App/opendcm/core/system.hpp b/src/Mod/Assembly/App/opendcm/core/system.hpp index c5c9a5523..2b0ac11be 100644 --- a/src/Mod/Assembly/App/opendcm/core/system.hpp +++ b/src/Mod/Assembly/App/opendcm/core/system.hpp @@ -55,10 +55,6 @@ namespace details { enum { subcluster = 10}; -template -struct vector_fold : mpl::fold< seq, state, - mpl::push_back > {}; - template struct edge_fold : mpl::fold< seq, state, mpl::if_< is_edge_property, @@ -81,7 +77,7 @@ struct obj_fold : mpl::fold< seq, state, mpl::push_back, mpl::_1 > > {}; template -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, mpl::_2 > > > >::type type; @@ -164,7 +160,7 @@ public: typedef typename details::cluster_fold< properties, mpl::vector >::type cluster_properties; - typedef typename details::property_map::type object_properties; + typedef typename details::property_map_fold::type object_properties; protected: //object storage diff --git a/src/Mod/Assembly/App/opendcm/module3d.hpp b/src/Mod/Assembly/App/opendcm/module3d.hpp index a0c4392fc..92206745f 100644 --- a/src/Mod/Assembly/App/opendcm/module3d.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d.hpp @@ -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" diff --git a/src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp b/src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp index ef2a2604f..2d44b1995 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #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 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::ClusterMath() : m_normQ(NULL), m_translation(NULL), init(false }; template -void ClusterMath::setParameterOffset(int offset) { - m_offset = offset; +void ClusterMath::setParameterOffset(int offset, dcm::ParameterType t) { + if(t == general) + m_offset = offset; + else + m_offset_rot = offset; }; template -int ClusterMath::getParameterOffset() { - return m_offset; +int ClusterMath::getParameterOffset(ParameterType t) { + if(t == general) + return m_offset; + else + return m_offset_rot; }; template @@ -243,7 +250,7 @@ void ClusterMath::finishCalculation() { #ifdef USE_LOGGING BOOST_LOG(log) << "Finish calculation"; #endif - + mapsToTransform(m_transform); init=false; @@ -419,7 +426,8 @@ void ClusterMath::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::Scalar ClusterMath::calcOnePoint(const typename template typename ClusterMath::Scalar ClusterMath::calcTwoPoints(const typename ClusterMath::Kernel::Vector3& p1, - const typename ClusterMath::Kernel::Vector3& p2) { + const typename ClusterMath::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 diff --git a/src/Mod/Assembly/App/opendcm/module3d/coincident.hpp b/src/Mod/Assembly/App/opendcm/module3d/coincident.hpp index e211a99c6..fd45c6b69 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/coincident.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/coincident.hpp @@ -28,7 +28,7 @@ namespace dcm { namespace details { //we need a custom orientation type to allow coincidents with points -struct ci_orientation : public Equation { +struct ci_orientation : public Equation { using Equation::operator=; ci_orientation() : Equation(parallel) {}; @@ -37,6 +37,11 @@ struct ci_orientation : public Equation { template< typename Kernel, typename Tag1, typename Tag2 > struct type : public PseudoScale { + 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 > Vec; @@ -126,6 +131,11 @@ struct ci_distance : public Equation { template< typename Kernel, typename Tag1, typename Tag2 > struct type : public PseudoScale { + 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 > Vec; diff --git a/src/Mod/Assembly/App/opendcm/module3d/defines.hpp b/src/Mod/Assembly/App/opendcm/module3d/defines.hpp index 60549e18c..fb49f2cd3 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/defines.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/defines.hpp @@ -20,6 +20,8 @@ #ifndef GCM_DEFINES_3D_H #define GCM_DEFINES_3D_H +#include + 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 solver_failure_type; +struct creation_error : virtual boost::exception {}; + } #endif diff --git a/src/Mod/Assembly/App/opendcm/module3d/module.hpp b/src/Mod/Assembly/App/opendcm/module3d/module.hpp index b21c84ff8..a511dbb1e 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/module.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/module.hpp @@ -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); diff --git a/src/Mod/Assembly/App/opendcm/module3d/solver.hpp b/src/Mod/Assembly/App/opendcm/module3d/solver.hpp index b767b4dae..edc8a6595 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/solver.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/solver.hpp @@ -20,12 +20,16 @@ #ifndef GCM_SOLVER_3D_H #define GCM_SOLVER_3D_H -#include - #include "defines.hpp" #include "clustermath.hpp" #include "opendcm/core/sheduler.hpp" #include "opendcm/core/traits.hpp" +#include +#include + +#include +#include +#include namespace dcm { namespace details { @@ -88,6 +92,23 @@ struct SystemSolver : public Job { Eigen::aligned_allocator >& 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 + void back_edge(Edge u, const Graph& g) { + m_dedected = true; + } + }; + SystemSolver(); virtual void execute(Sys& sys); void solveCluster(boost::shared_ptr cluster, Sys& sys); @@ -128,7 +149,7 @@ void MES::recalculate() { std::pair< oiter, oiter > oit = m_cluster->template getObjects(*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::Scalar SystemSolver::Rescaler::scaleClusters() template void SystemSolver::Rescaler::collectPseudoPoints( - boost::shared_ptr::Cluster> parent, - LocalVertex cluster, - std::vector::Kernel::Vector3, - Eigen::aligned_allocator::Kernel::Vector3> >& vec) { + boost::shared_ptr::Cluster> parent, + LocalVertex cluster, + std::vector::Kernel::Vector3, + Eigen::aligned_allocator::Kernel::Vector3> >& vec) { std::vector > vec2; typedef typename Cluster::template object_iterator c_iter; @@ -233,138 +254,202 @@ void SystemSolver::execute(Sys& sys) { template void SystemSolver::solveCluster(boost::shared_ptr cluster, Sys& sys) { - //set out and solve all relevant subclusters - typedef typename Cluster::cluster_iterator citer; - std::pair 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 cit = cluster->clusters(); + for(; cit.first != cit.second; cit.first++) { - boost::shared_ptr c = (*cit.first).second; - if(c->template getClusterProperty() && - c->template getClusterProperty() == details::cluster3D) - solveCluster(c, sys); - } + boost::shared_ptr c = (*cit.first).second; + if(c->template getClusterProperty() && + c->template getClusterProperty() == 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::vertex_iterator iter; - std::pair 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::vertex_iterator iter; + std::pair 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(*it.first)) { - params += 6; - } - } else { - params += cluster->template getObject(*it.first)->m_parameterCount; + //when cluster and not fixed it has trans and rot parameter + if(cluster->isCluster(*it.first)) { + if(!cluster->template getSubclusterProperty(*it.first)) { + params += 6; + } + } else { + params += cluster->template getObject(*it.first)->m_parameterCount; + }; + } + + //count the equations in the constraints + typedef typename Cluster::template object_iterator ocit; + typedef typename boost::graph_traits::edge_iterator e_iter; + std::pair e_it = boost::edges(*cluster); + for(; e_it.first != e_it.second; e_it.first++) { + std::pair< ocit, ocit > it = cluster->template getObjects(*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 ocit; - typedef typename boost::graph_traits::edge_iterator e_iter; - std::pair e_it = boost::edges(*cluster); - for(; e_it.first != e_it.second; e_it.first++) { - std::pair< ocit, ocit > it = cluster->template getObjects(*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 = "<template getObject(*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 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 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(*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: "<isCluster(*it.first)) { - boost::shared_ptr c = cluster->getVertexCluster(*it.first); - if(!cluster->template getSubclusterProperty(*it.first)) - c->template getClusterProperty().finishCalculation(); - else - c->template getClusterProperty().finishFixCalculation(); - - std::vector& vec = c->template getClusterProperty().getGeometry(); - for(typename std::vector::iterator vit = vec.begin(); vit != vec.end(); vit++) - (*vit)->finishCalculation(); - - } else { - Geom g = cluster->template getObject(*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: "<isCluster(*it.first)) { + boost::shared_ptr c = cluster->getVertexCluster(*it.first); + if(!cluster->template getSubclusterProperty(*it.first)) + c->template getClusterProperty().finishCalculation(); + else + c->template getClusterProperty().finishFixCalculation(); + + std::vector& vec = c->template getClusterProperty().getGeometry(); + for(typename std::vector::iterator vit = vec.begin(); vit != vec.end(); vit++) + (*vit)->finishCalculation(); + + } else { + Geom g = cluster->template getObject(*it.first); + g->scale(mes.Scaling); + g->finishCalculation(); + } + } + //we have solved this cluster + cluster->template setClusterProperty(false); + + } catch(boost::exception& x) { + throw; } - //we have solved this cluster - cluster->template setClusterProperty(false); }; }//details