kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

add opendcm constraint solver

Browse Source
This commit is contained in:
Stefan Tröger
2013-04-25 12:14:01 +02:00
committed by Stefan Tröger
parent d50f7f1787
commit 02bc130c42
53 changed files with 9940 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1520
src/Mod/Assembly/App/opendcm/core/clustergraph.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

512
src/Mod/Assembly/App/opendcm/core/constraint.hpp Normal file
View File

@@ -0,0 +1,512 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more detemplate tails.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_CONSTRAINT_H
#define GCM_CONSTRAINT_H
#include<Eigen/StdVector>
#include <assert.h>
#include <boost/variant.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/map.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/size.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/sequence/intrinsic/size.hpp>
#include <boost/fusion/include/size.hpp>
#include "traits.hpp"
#include "object.hpp"
#include "equations.hpp"
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
namespace dcm {
namespace detail {
//type erasure container for constraints
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
class Constraint : public Object<Sys, Derived, Signals > {
typedef typename system_traits<Sys>::Kernel Kernel;
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::DynStride DS;
typedef boost::shared_ptr<Geometry> geom_ptr;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
public:
Constraint(Sys& system, geom_ptr f, geom_ptr s);
~Constraint();
virtual boost::shared_ptr<Derived> clone(Sys& newSys);
protected:
template<typename ConstraintVector>
void initialize(typename fusion::result_of::as_vector<ConstraintVector>::type& obj);
int equationCount();
template< typename creator_type>
void resetType(creator_type& c);
void calculate(Scalar scale);
void setMaps(MES& mes);
void geometryReset(geom_ptr g) {
/* placeholder* p = content->resetConstraint(first, second);
delete content;
content = p;*/
};
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)),
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_residual;
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 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;
};
public:
template< typename ConstraintVector, typename EquationVector>
struct holder : public placeholder {
//create a vector of EquationSets with some mpl trickery
typedef typename mpl::fold< EquationVector, mpl::vector<>,
mpl::push_back<mpl::_1, EquationSet<mpl::_2> > >::type eq_set_vector;
typedef typename fusion::result_of::as_vector<eq_set_vector>::type EquationSets;
typedef typename fusion::result_of::as_vector<ConstraintVector>::type Objects;
template<typename T>
struct has_option {
//we get the index of the eqaution in the eqaution vector, and as it is the same
//as the index of the constraint in the constraint vector we can extract the
//option type and check if it is no_option
typedef typename mpl::find<EquationVector, T>::type iterator;
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 > >));
typedef typename mpl::at<ConstraintVector, distance>::type option_type;
typedef mpl::not_<boost::is_same<option_type, no_option> > type;
};
struct OptionSetter {
Objects& objects;
OptionSetter(Objects& val);
//only set the value if the equation has a option
template< typename T >
typename boost::enable_if<typename has_option<T>::type, void>::type
operator()(EquationSet<T>& val) const;
//if the equation has no otpion we do nothing!
template< typename T >
typename boost::enable_if<mpl::not_<typename has_option<T>::type>, void>::type
operator()(EquationSet<T>& val) const;
};
struct Calculater {
geom_ptr first, second;
Scalar scale;
Calculater(geom_ptr f, geom_ptr s, Scalar sc);
template< typename T >
void operator()(T& val) const;
};
struct MapSetter {
MES& mes;
geom_ptr first, second;
MapSetter(MES& m, geom_ptr f, geom_ptr s);
template< typename T >
void operator()(T& val) const;
};
struct PseudoCollector {
Vec& points1;
Vec& points2;
geom_ptr first,second;
PseudoCollector(geom_ptr f, geom_ptr s, Vec& vec1, Vec& vec2);
template< typename T >
void operator()(T& val) const;
};
holder(Objects& obj);
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale);
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);
virtual placeholder* clone();
virtual int equationCount() {
return mpl::size<EquationVector>::value;
};
EquationSets m_sets;
};
protected:
template< typename ConstraintVector >
struct creator : public boost::static_visitor<void> {
typedef typename fusion::result_of::as_vector<ConstraintVector>::type Objects;
Objects& objects;
creator(Objects& obj);
template<typename C, typename T1, typename T2>
struct equation {
typedef typename C::template type<Kernel, T1, T2> type;
};
template<typename T1, typename T2>
void operator()(const T1&, const T2&);
placeholder* p;
bool need_swap;
};
placeholder* content;
geom_ptr first, second;
Connection cf, cs;
};
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
Constraint<Sys, Derived, Signals, MES, Geometry>::Constraint(Sys& system, geom_ptr f, geom_ptr s)
: 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));
};
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);
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
boost::shared_ptr<Derived> Constraint<Sys, Derived, Signals, MES, Geometry>::clone(Sys& newSys) {
//copy the standart stuff
boost::shared_ptr<Derived> np = boost::shared_ptr<Derived>(new Derived(*static_cast<Derived*>(this)));
np->m_system = &newSys;
//copy the internals
np->content = content->clone();
//and get the geometry pointers right
if(first) {
GlobalVertex v = first->template getProperty<typename Geometry::vertex_propertie>();
np->first = newSys.m_cluster->template getObject<Geometry>(v);
}
if(second) {
GlobalVertex v = second->template getProperty<typename Geometry::vertex_propertie>();
np->second = newSys.m_cluster->template getObject<Geometry>(v);
}
return np;
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector>
void Constraint<Sys, Derived, Signals, MES, Geometry>::initialize(typename fusion::result_of::as_vector<ConstraintVector>::type& obj) {
//first create the new placeholder
creator<ConstraintVector> c(obj);
boost::apply_visitor(c, first->m_geometry, second->m_geometry);
//and now store it
content = c.p;
//geometry order needs to be the one needed by equations
if(c.need_swap) first.swap(second);
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
int Constraint<Sys, Derived, Signals, MES, Geometry>::equationCount() {
return content->equationCount();
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template< typename creator_type>
void Constraint<Sys, Derived, Signals, MES, Geometry>::resetType(creator_type& c) {
boost::apply_visitor(c, first->m_geometry, second->m_geometry);
content = c.p;
if(c.need_swap) first.swap(second);
};
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);
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
void Constraint<Sys, Derived, Signals, MES, Geometry>::setMaps(MES& mes) {
content->setMaps(mes, first, second);
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
void Constraint<Sys, Derived, Signals, MES, Geometry>::collectPseudoPoints(Vec& vec1, Vec& vec2) {
content->collectPseudoPoints(first, second, vec1, vec2);
};
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>::OptionSetter::OptionSetter(Objects& val) : objects(val) {};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
template<typename T>
typename boost::enable_if<typename Constraint<Sys, Derived, Signals, MES, Geometry>::template holder<ConstraintVector, EquationVector>::template has_option<T>::type, void>::type
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::OptionSetter::operator()(EquationSet<T>& val) const {
//get the index of the corresbonding equation
typedef typename mpl::find<EquationVector, T>::type iterator;
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;
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
template<typename T>
typename boost::enable_if<mpl::not_<typename Constraint<Sys, Derived, Signals, MES, Geometry>::template holder<ConstraintVector, EquationVector>::template has_option<T>::type>, void>::type
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::OptionSetter::operator()(EquationSet<T>& val) const {
};
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) {
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
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);
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<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);
}
}
} 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<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);
}
}
} 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);
}
}
};
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>::MapSetter::MapSetter(MES& m, geom_ptr f, geom_ptr s)
: mes(m), first(f), second(s) {
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
template< typename T >
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::MapSetter::operator()(T& val) const {
//when in cluster, there are 6 clusterparameter we differentiat for, if not we differentiat
//for every parameter in the geometry;
int equation = mes.setResidualMap(val.m_residual);
if(first->getClusterMode()) {
if(!first->isClusterFixed()) {
mes.setJacobiMap(equation, first->m_offset, 6, 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);
}
} else mes.setJacobiMap(equation, second->m_offset, second->m_parameterCount, val.m_diff_second);
};
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>::PseudoCollector::PseudoCollector(geom_ptr f, geom_ptr s, Vec& vec1, Vec& vec2)
: first(f), second(s), points1(vec1), points2(vec2) {
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
template< typename T >
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::PseudoCollector::operator()(T& val) const {
if(first->m_isInCluster && second->m_isInCluster) {
val.m_eq.calculatePseudo(first->m_rotated, points1, second->m_rotated, points2);
} else if(first->m_isInCluster) {
typename Kernel::Vector sec = second->m_parameter;
val.m_eq.calculatePseudo(first->m_rotated, points1, sec, points2);
} else if(second->m_isInCluster) {
typename Kernel::Vector fir = first->m_parameter;
val.m_eq.calculatePseudo(fir, points1, second->m_rotated, points2);
}
};
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>::holder(Objects& obj) {
//set the initial values in the equations
fusion::for_each(m_sets, OptionSetter(obj));
};
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));
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
typename Constraint<Sys, Derived, Signals, MES, Geometry>::placeholder*
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::resetConstraint(geom_ptr first, geom_ptr second) const {
//boost::apply_visitor(creator, first->m_geometry, second->m_geometry);
//if(creator.need_swap) first.swap(second);
return NULL;
};
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>::setMaps(MES& mes, geom_ptr first, geom_ptr second) {
fusion::for_each(m_sets, MapSetter(mes, first, second));
};
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>::collectPseudoPoints(geom_ptr f, geom_ptr s, Vec& vec1, Vec& vec2) {
fusion::for_each(m_sets, PseudoCollector(f, s, vec1, vec2));
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector, typename EquationVector>
typename Constraint<Sys, Derived, Signals, MES, Geometry>::placeholder*
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::clone() {
return new holder(*this);
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template< typename ConstraintVector >
Constraint<Sys, Derived, Signals, MES, Geometry>::creator<ConstraintVector>::creator(Objects& obj) : objects(obj) {
};
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template< typename ConstraintVector >
template<typename T1, typename T2>
void Constraint<Sys, Derived, Signals, MES, Geometry>::creator<ConstraintVector>::operator()(const T1&, const T2&) {
typedef tag_order< typename geometry_traits<T1>::tag, typename geometry_traits<T2>::tag > order;
//transform the constraints into eqautions with the now known types
typedef typename mpl::fold< ConstraintVector, mpl::vector<>,
mpl::push_back<mpl::_1, equation<mpl::_2, typename order::first_tag,
typename order::second_tag> > >::type EquationVector;
//and build the placeholder
p = new holder<ConstraintVector, EquationVector>(objects);
need_swap = order::swapt::value;
};
};//detail
};//dcm
#endif //GCM_CONSTRAINT_H

189
src/Mod/Assembly/App/opendcm/core/equations.hpp Normal file
View File

@@ -0,0 +1,189 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more detemplate tails.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_EQUATIONS_H
#define GCM_EQUATIONS_H
#include <assert.h>
namespace dcm {
struct no_option {};
template<typename Kernel>
struct Pseudo {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {};
};
template<typename Kernel>
struct Scale {
void setScale(typename Kernel::number_type scale) {};
};
template<typename Kernel>
struct PseudoScale {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {};
void setScale(typename Kernel::number_type scale) {};
};
struct Distance {
typedef double option_type;
option_type value;
Distance() : value(0) {};
Distance& operator=(const option_type val) {
value = val;
return *this;
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value;
//template definition
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {
assert(false);
};
void setScale(Scalar scale) {
assert(false);
};
Scalar calculate(Vector& param1, Vector& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
};
};
//the possible directions
enum Direction { Same, Opposite, Both };
struct Parallel {
typedef Direction option_type;
option_type value;
Parallel() : value(Both) {};
Parallel& operator=(const option_type val) {
value = val;
return *this;
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
};
};
struct Angle {
typedef double option_type;
option_type value;
Angle() : value(0) {};
Angle& operator=(const option_type val) {
value = val;
return *this;
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
assert(false);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
assert(false);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
assert(false);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
};
};
};
//static is needed to restrain the scope of the objects to the current compilation unit. Without it
//every compiled file including this header would define these as global and the linker would find
//multiple definitions of the same objects
static Distance distance;
static Parallel parallel;
static Angle angle;
};
#endif //GCM_EQUATIONS_H

481
src/Mod/Assembly/App/opendcm/core/geometry.hpp Normal file
View File

@@ -0,0 +1,481 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_GEOMETRY_H
#define GCM_GEOMETRY_H
#include <iostream>
#include <Eigen/Core>
#include <boost/type_traits.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/map.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/mpl.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/function.hpp>
#include <boost/variant.hpp>
#include "object.hpp"
#include "traits.hpp"
#include "logging.hpp"
#include "transformation.hpp"
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
namespace dcm {
namespace tag {
struct undefined {
typedef mpl::int_<0> parameters;
typedef mpl::int_<0> transformations;
};
//we need to order tags, this values make it easy for module tags
namespace weight {
struct direction : mpl::int_<0> {};
struct point : mpl::int_<1> {};
struct line : mpl::int_<2> {};
struct plane : mpl::int_<3> {};
struct cylinder : mpl::int_<4> {};
}
}
struct orderd_bracket_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
return t[ID];
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
t[ID] = value;
};
};
struct orderd_roundbracket_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
return t(ID);
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
t(ID) = value;
};
};
//tag ordering (smaller weight is first tag)
template<typename T1, typename T2>
struct tag_order {
BOOST_MPL_ASSERT((mpl::not_< mpl::or_<
boost::is_same< typename T1::weight, mpl::int_<0> >,
boost::is_same< typename T2::weight, mpl::int_<0> > > >));
typedef typename mpl::less<typename T2::weight, typename T1::weight>::type swapt;
typedef typename mpl::if_<swapt, T2, T1>::type first_tag;
typedef typename mpl::if_<swapt, T1, T2>::type second_tag;
};
//template<typename T1, typename T2>
//struct type_order : public tag_order< typename geometry_traits<T1>::tag, typename geometry_traits<T2>::tag > {};
template< typename T>
struct geometry_traits {
BOOST_MPL_ASSERT_MSG(false, NO_GEOMETRY_TRAITS_SPECIFIED_FOR_TYPE, (T));
};
template< typename T>
struct geometry_clone_traits {
T operator()(T& val) {
return T(val);
};
};
struct reset {}; //signal namespace
namespace detail {
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
class Geometry : public Object<Sys, Derived,
mpl::map3< mpl::pair<reset, boost::function<void (boost::shared_ptr<Derived>) > >,
mpl::pair<remove, boost::function<void (boost::shared_ptr<Derived>) > > ,
mpl::pair<recalculated, boost::function<void (boost::shared_ptr<Derived>)> > > > {
typedef mpl::map3< mpl::pair<reset, boost::function<void (boost::shared_ptr<Derived>) > >,
mpl::pair<remove, boost::function<void (boost::shared_ptr<Derived>) > >,
mpl::pair<recalculated, boost::function<void (boost::shared_ptr<Derived>)> > > Signals;
typedef typename boost::make_variant_over< GeometrieTypeList >::type Variant;
typedef Object<Sys, Derived, Signals> Base;
typedef typename system_traits<Sys>::Kernel Kernel;
typedef typename system_traits<Sys>::Cluster Cluster;
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::DynStride DS;
typedef typename Kernel::template transform_type<Dim>::type Transform;
typedef typename Kernel::template transform_type<Dim>::diff_type DiffTransform;
struct cloner : boost::static_visitor<void> {
typedef typename boost::make_variant_over< GeometrieTypeList >::type Variant;
Variant variant;
cloner(Variant& v) : variant(v) {};
template<typename T>
void operator()(T& t) {
variant = geometry_clone_traits<T>()(t);
};
};
#ifdef USE_LOGGING
protected:
src::logger log;
#endif
public:
typedef mpl::int_<Dim> Dimension;
template<typename T>
Geometry(T geometry, Sys& system);
template<typename T>
void set(T geometry);
template<typename Visitor>
typename Visitor::result_type apply(Visitor& vis) {
return boost::apply_visitor(vis, m_geometry);
};
//basic ation
void transform(const Transform& t);
virtual boost::shared_ptr<Derived> clone(Sys& newSys);
//allow accessing the internal values in unittests without making them public,
//so that access control of the internal classes is not changed and can be tested
#ifdef TESTING
typename Kernel::Vector& toplocal() {
return m_toplocal;
};
typename Kernel::Vector& rotated() {
return m_rotated;
};
int& offset() {
return m_offset;
};
void clusterMode(bool iscluster, bool isFixed) {
setClusterMode(iscluster, isFixed);
};
void trans(const Transform& t) {
transform(t);
};
void recalc(DiffTransform& trans) {
recalculate(trans);
};
typename Kernel::Vector3 point() {
return getPoint();
};
int parameterCount() {
return m_parameterCount;
};
#endif
//protected would be the right way, however, visual studio 10 does not find a way to access them even when constraint::holder structs
//are declared friend
//protected:
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_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;
typename Kernel::Vector m_toplocal; //the local value in the toplevel cluster used for cuttent solving
typename Kernel::Vector m_global; //the global value outside of all clusters
typename Kernel::Vector m_rotated; //the global value as the rotation of toplocal (used as temp)
typename Kernel::Matrix m_diffparam; //gradient vectors combined as matrix when in cluster
typename Kernel::VectorMap m_parameter; //map to the parameters in the solver
template<typename T>
void init(T& t);
void normalize();
typename Sys::Kernel::VectorMap& getParameterMap();
void initMap();
void setClusterMode(bool iscluster, bool isFixed);
bool getClusterMode() {
return m_isInCluster;
};
bool isClusterFixed() {
return m_clusterFixed;
};
void recalculate(DiffTransform& trans);
typename Kernel::Vector3 getPoint() {
return m_toplocal.template segment<Dim>(0);
};
//visitor to write the calculated value into the variant
struct apply_visitor : public boost::static_visitor<void> {
apply_visitor(typename Kernel::Vector& v) : value(v) {};
template <typename T>
void operator()(T& t) const {
(typename geometry_traits<T>::modell()).template inject<typename Kernel::number_type,
typename geometry_traits<T>::accessor >(t, value);
}
typename Kernel::Vector& value;
};
//use m_value or parametermap as new value, dependend on the solving mode
void finishCalculation();
template<typename VectorType>
void transform(const Transform& t, VectorType& vec);
void scale(Scalar value);
};
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
template<typename T>
Geometry<Sys, Derived, GeometrieTypeList, Dim>::Geometry(T geometry, Sys& system)
: m_isInCluster(false), m_geometry(geometry), m_parameter(NULL,0,DS(0,0)),
m_clusterFixed(false), m_init(false) {
#ifdef USE_LOGGING
log.add_attribute("Tag", attrs::constant< std::string >("Geometry3D"));
#endif
this->m_system = &system;
init<T>(geometry);
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
template<typename T>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::set(T geometry) {
m_geometry = geometry;
init<T>(geometry);
//Base::template emitSignal<reset>( ((Derived*)this)->shared_from_this() );
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::transform(const Transform& t) {
if(m_isInCluster)
transform(t, m_toplocal);
else if(m_init)
transform(t, m_rotated);
else
transform(t, m_global);
};
template<typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
boost::shared_ptr<Derived> Geometry<Sys, Derived, GeometrieTypeList, Dim>::clone(Sys& newSys) {
//copy the standart stuff
boost::shared_ptr<Derived> np = boost::shared_ptr<Derived>(new Derived(*static_cast<Derived*>(this)));
np->m_system = &newSys;
//it's possible that the variant contains pointers, so we need to clone them
cloner clone_fnc(np->m_geometry);
boost::apply_visitor(clone_fnc, m_geometry);
return np;
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
template<typename T>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::init(T& t) {
m_BaseParameterCount = geometry_traits<T>::tag::parameters::value;
m_parameterCount = m_BaseParameterCount;
m_rotations = geometry_traits<T>::tag::rotations::value;
m_translations = geometry_traits<T>::tag::translations::value;
m_toplocal.setZero(m_parameterCount);
m_global.resize(m_parameterCount);
m_rotated.resize(m_parameterCount);
m_rotated.setZero();
m_diffparam.resize(m_parameterCount,6);
m_diffparam.setZero();
(typename geometry_traits<T>::modell()).template extract<Scalar,
typename geometry_traits<T>::accessor >(t, m_global);
normalize();
//new value which is not set into parameter, so init is false
m_init = false;
#ifdef USE_LOGGING
BOOST_LOG(log) << "Init: "<<m_global.transpose();
#endif
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::normalize() {
//directions are not nessessarily normalized, but we need to ensure this in cluster mode
for(int i=m_translations; i!=m_rotations; i++)
m_global.template segment<Dim>(i*Dim).normalize();
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
typename Sys::Kernel::VectorMap& Geometry<Sys, Derived, GeometrieTypeList, Dim>::getParameterMap() {
m_isInCluster = false;
m_parameterCount = m_BaseParameterCount;
return m_parameter;
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::initMap() {
//when direct parameter solving the global value is wanted (as it's the initial rotation*toplocal)
m_parameter = m_global;
m_init = true;
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::setClusterMode(bool iscluster, bool isFixed) {
m_isInCluster = iscluster;
m_clusterFixed = isFixed;
if(iscluster) {
//we are in cluster, therfore the parameter map should not point to a solver value but to
//the rotated original value;
new(&m_parameter) typename Sys::Kernel::VectorMap(&m_rotated(0), m_parameterCount, DS(1,1));
//the local value is the global one as no transformation was applied yet
m_toplocal = m_global;
m_rotated = m_global;
} else new(&m_parameter) typename Sys::Kernel::VectorMap(&m_global(0), m_parameterCount, DS(1,1));
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::recalculate(DiffTransform& trans) {
if(!m_isInCluster) return;
for(int i=0; i!=m_rotations; i++) {
//first rotate the original to the transformed value
m_rotated.block(i*Dim,0,Dim,1) = trans.rotation()*m_toplocal.template segment<Dim>(i*Dim);
//now calculate the gradient vectors and add them to diffparam
for(int j=0; j<Dim; j++)
m_diffparam.block(i*Dim,j,Dim,1) = trans.differential().block(0,j*3,Dim,Dim) * m_toplocal.template segment<Dim>(i*Dim);
}
//after rotating the needed parameters we translate the stuff that needs to be moved
for(int i=0; i!=m_translations; i++) {
m_rotated.block(i*Dim,0,Dim,1) += trans.translation().vector();
m_rotated.block(i*Dim,0,Dim,1) *= trans.scaling().factor();
//calculate the gradient vectors and add them to diffparam
m_diffparam.block(i*Dim,Dim,Dim,Dim).setIdentity();
}
#ifdef USE_LOGGING
if(!boost::math::isnormal(m_rotated.norm()) || !boost::math::isnormal(m_diffparam.norm())) {
BOOST_LOG(log) << "Unnormal recalculated value detected: "<<m_rotated.transpose()<<std::endl
<< "or unnormal recalculated diff detected: "<<std::endl<<m_diffparam<<std::endl
<<" with Transform: "<<std::endl<<trans;
}
#endif
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::finishCalculation() {
//if fixed nothing needs to be changed
if(m_isInCluster) {
//recalculate(1.); //remove scaling to get right global value
m_global = m_rotated;
#ifdef USE_LOGGING
BOOST_LOG(log) << "Finish cluster calculation";
#endif
}
//TODO:non cluster paramter scaling
else {
m_global = m_parameter;
normalize();
#ifdef USE_LOGGING
BOOST_LOG(log) << "Finish calculation";
#endif
};
apply_visitor v(m_global);
apply(v);
m_init = false;
m_isInCluster = false;
//emit the signal for recalculation
Base::template emitSignal<recalculated>( ((Derived*)this)->shared_from_this() );
};
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
template<typename VectorType>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::transform(const Transform& t, VectorType& vec) {
//everything that needs to be translated needs to be fully transformed
for(int i=0; i!=m_translations; i++) {
typename Kernel::Vector3 v = vec.template segment<Dim>(i*Dim);
vec.template segment<Dim>(i*Dim) = t*v;
}
for(int i=m_translations; i!=m_rotations; i++) {
typename Kernel::Vector3 v = vec.template segment<Dim>(i*Dim);
vec.template segment<Dim>(i*Dim) = t.rotate(v);
}
#ifdef USE_LOGGING
BOOST_LOG(log) << "Transformed with cluster: "<<m_isInCluster
<< ", init: "<<m_init<<" into: "<< vec.transpose();
#endif
}
template< typename Sys, typename Derived, typename GeometrieTypeList, int Dim>
void Geometry<Sys, Derived, GeometrieTypeList, Dim>::scale(Scalar value) {
for(int i=0; i!=m_translations; i++)
m_parameter.template segment<Dim>(i*Dim) *= 1./value;
};
}
}
#endif // GCM_GEOMETRY_H

425
src/Mod/Assembly/App/opendcm/core/kernel.hpp Normal file
View File

@@ -0,0 +1,425 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_KERNEL_H
#define GCM_KERNEL_H
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <boost/math/special_functions/fpclassify.hpp>
#include <time.h>
#include "transformation.hpp"
#include "logging.hpp"
namespace dcm {
namespace E = Eigen;
struct nothing {
void operator()() {};
};
template<typename Kernel>
struct Dogleg {
#ifdef USE_LOGGING
src::logger log;
#endif
typedef typename Kernel::number_type number_type;
number_type tolg, tolx, tolf;
Dogleg() : tolg(1e-40), tolx(1e-20), tolf(1e-5) {
#ifdef USE_LOGGING
log.add_attribute("Tag", attrs::constant< std::string >("Dogleg"));
#endif
};
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) {
// get the steepest descent stepsize and direction
const double alpha(g.squaredNorm()/(jacobi*g).squaredNorm());
const typename Kernel::Vector h_sd = -g;
// get the gauss-newton step
const typename Kernel::Vector h_gn = (jacobi).fullPivLu().solve(-residual);
#ifdef USE_LOGGING
if(!boost::math::isfinite(h_gn.norm())) {
BOOST_LOG(log)<< "Unnormal gauss-newton detected: "<<h_gn.norm();
}
if(!boost::math::isfinite(h_sd.norm())) {
BOOST_LOG(log)<< "Unnormal steepest descent detected: "<<h_sd.norm();
}
if(!boost::math::isfinite(alpha)) {
BOOST_LOG(log)<< "Unnormal alpha detected: "<<alpha;
}
#endif
// compute the dogleg step
if(h_gn.norm() <= delta) {
h_dl = h_gn;
} else if((alpha*h_sd).norm() >= delta) {
//h_dl = alpha*h_sd;
h_dl = (delta/(h_sd.norm()))*h_sd;
#ifdef USE_LOGGING
if(!boost::math::isfinite(h_dl.norm())) {
BOOST_LOG(log)<< "Unnormal dogleg descent detected: "<<h_dl.norm();
}
#endif
} else {
//compute beta
number_type beta = 0;
typename Kernel::Vector a = alpha*h_sd;
typename Kernel::Vector b = h_gn;
number_type c = a.transpose()*(b-a);
number_type bas = (b-a).squaredNorm(), as = a.squaredNorm();
if(c<0) {
beta = -c+std::sqrt(std::pow(c,2)+bas*(std::pow(delta,2)-as));
beta /= bas;
} else {
beta = std::pow(delta,2)-as;
beta /= c+std::sqrt(std::pow(c,2) + bas*(std::pow(delta,2)-as));
};
// and update h_dl and dL with beta
h_dl = alpha*h_sd + beta*(b-a);
#ifdef USE_LOGGING
if(!boost::math::isfinite(c)) {
BOOST_LOG(log)<< "Unnormal dogleg c detected: "<<c;
}
if(!boost::math::isfinite(bas)) {
BOOST_LOG(log)<< "Unnormal dogleg bas detected: "<<bas;
}
if(!boost::math::isfinite(beta)) {
BOOST_LOG(log)<< "Unnormal dogleg beta detected: "<<beta;
}
#endif
}
return 0;
};
int solve(typename Kernel::MappedEquationSystem& sys) {
nothing n;
return solve(sys, n);
};
template<typename Functor>
int solve(typename Kernel::MappedEquationSystem& sys, Functor& rescale) {
clock_t start = clock();
clock_t inc_rec = clock();
if(!sys.isValid()) return 5;
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);
sys.recalculate();
#ifdef USE_LOGGING
BOOST_LOG(log)<< "initial jacobi: "<<std::endl<<sys.Jacobi<<std::endl
<< "residual: "<<sys.Residual.transpose()<<std::endl
<< "maximal differential: "<<sys.Jacobi.template lpNorm<Eigen::Infinity>();
#endif
number_type err = sys.Residual.norm();
F_old = sys.Residual;
J_old = sys.Jacobi;
g = sys.Jacobi.transpose()*(sys.Residual);
// get the infinity norm fx_inf and g_inf
number_type g_inf = g.template lpNorm<E::Infinity>();
number_type fx_inf = sys.Residual.template lpNorm<E::Infinity>();
int maxIterNumber = 10000;//MaxIterations * xsize;
number_type diverging_lim = 1e6*err + 1e12;
number_type delta=5;
number_type nu=2.;
int iter=0, stop=0, reduce=0, unused=0, counter=0;
while(!stop) {
// check if finished
if(fx_inf <= tolf*sys.Scaling) // Success
stop = 1;
else if(g_inf <= tolg)
stop = 2;
else if(delta <= tolx)
stop = 3;
else if(iter >= maxIterNumber)
stop = 4;
else if(!boost::math::isfinite(err))
stop = 5;
else if(err > diverging_lim)
stop = 6;
// see if we are already finished
if(stop)
break;
number_type err_new;
number_type dF=0, dL=0;
number_type rho;
//get the update step
calculateStep(g, sys.Jacobi, sys.Residual, h_dl, delta);
// calculate the linear model
dL = 0.5*sys.Residual.norm() - 0.5*(sys.Residual + sys.Jacobi*h_dl).norm();
// get the new values
sys.Parameter += h_dl;
clock_t start_rec = clock();
sys.recalculate();
clock_t end_rec = clock();
inc_rec += end_rec-start_rec;
#ifdef USE_LOGGING
if(!boost::math::isfinite(sys.Residual.norm())) {
BOOST_LOG(log)<< "Unnormal residual detected: "<<sys.Residual.norm();
}
if(!boost::math::isfinite(sys.Jacobi.sum())) {
BOOST_LOG(log)<< "Unnormal jacobi detected: "<<sys.Jacobi.sum();
}
#endif
//calculate the translation update ratio
err_new = sys.Residual.norm();
dF = err - err_new;
rho = dF/dL;
if(dF<=0 || dL<=0) rho = -1;
// update delta
if(rho>0.85) {
delta = std::max(delta,2*h_dl.norm());
nu = 2;
} else if(rho < 0.25) {
delta = delta/nu;
nu = 2*nu;
}
if(dF > 0 && dL > 0) {
//see if we got too high differentials
if(sys.Jacobi.template lpNorm<Eigen::Infinity>() > 2) {
#ifdef USE_LOGGING
BOOST_LOG(log)<< "High differential detected: "<<sys.Jacobi.template lpNorm<Eigen::Infinity>()<<" in iteration: "<<iter;
#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();
sys.recalculate();
counter = 0;
}
F_old = sys.Residual;
J_old = sys.Jacobi;
err = sys.Residual.norm();
g = sys.Jacobi.transpose()*(sys.Residual);
// get infinity norms
g_inf = g.template lpNorm<E::Infinity>();
fx_inf = sys.Residual.template lpNorm<E::Infinity>();
} else {
sys.Residual = F_old;
sys.Jacobi = J_old;
sys.Parameter -= h_dl;
unused++;
#ifdef USE_LOGGING
BOOST_LOG(log)<< "Reject step in iter "<<iter;
#endif
}
iter++;
counter++;
}
/*
clock_t end = clock();
double ms = (double(end-start) * 1000.) / double(CLOCKS_PER_SEC);
double ms_rec = (double(inc_rec-start) * 1000.) / double(CLOCKS_PER_SEC);
*/
#ifdef USE_LOGGING
BOOST_LOG(log) <<"Done solving: "<<err<<", iter: "<<iter<<", unused: "<<unused<<", reason:"<< stop;
BOOST_LOG(log)<< "final jacobi: "<<std::endl<<sys.Jacobi;
#endif
return stop;
}
};
template<typename Scalar, template<class> class Nonlinear = Dogleg>
struct Kernel {
//basics
typedef Scalar number_type;
//linear algebra types 2D
typedef E::Matrix<Scalar, 2, 1> Vector2;
//Linear algebra types 3D
typedef E::Matrix<Scalar, 3, 1> Vector3;
typedef E::Matrix<Scalar, 1, 3> CVector3;
typedef E::Matrix<Scalar, 3, 3> Matrix3;
typedef E::Matrix<Scalar, E::Dynamic, 1> Vector;
typedef E::Matrix<Scalar, 1, E::Dynamic> CVector;
typedef E::Matrix<Scalar, E::Dynamic, E::Dynamic> Matrix;
//mapped types
typedef E::Stride<E::Dynamic, E::Dynamic> DynStride;
typedef E::Map< Vector3 > Vector3Map;
typedef E::Map< CVector3> CVector3Map;
typedef E::Map< Matrix3 > Matrix3Map;
typedef E::Map< Vector, 0, DynStride > VectorMap;
typedef E::Map< CVector, 0, DynStride > CVectorMap;
typedef E::Map< Matrix, 0, DynStride > MatrixMap;
//Special types
typedef E::Quaternion<Scalar> Quaternion;
typedef E::Matrix<Scalar, 3, 9> Matrix39;
typedef E::Map< Matrix39 > Matrix39Map;
typedef E::Block<Matrix> MatrixBlock;
typedef detail::Transform<Scalar, 3> Transform3D;
typedef detail::DiffTransform<Scalar, 3> DiffTransform3D;
typedef detail::Transform<Scalar, 2> Transform2D;
typedef detail::DiffTransform<Scalar, 2> DiffTransform2D;
typedef Nonlinear< Kernel<Scalar, Nonlinear> > NonlinearSolver;
template<int Dim>
struct transform_type {
typedef typename boost::mpl::if_c<Dim==2, Transform2D, Transform3D>::type type;
typedef typename boost::mpl::if_c<Dim==2, DiffTransform2D, DiffTransform3D>::type diff_type;
};
template<int Dim>
struct vector_type {
typedef E::Matrix<Scalar, Dim, 1> type;
};
struct MappedEquationSystem {
Matrix Jacobi;
Vector Parameter;
Vector Residual;
number_type Scaling;
int m_params, m_eqns; //total amount
int m_param_offset, m_eqn_offset; //current positions while creation
MappedEquationSystem(int params, int equations)
: Jacobi(equations, params),
Parameter(params), Residual(equations),
m_params(params), m_eqns(equations), Scaling(1.) {
m_param_offset = 0;
m_eqn_offset = 0;
Jacobi.setZero(); //important as some places are never written
};
int setParameterMap(int number, VectorMap& map) {
new(&map) VectorMap(&Parameter(m_param_offset), number, DynStride(1,1));
m_param_offset += number;
return m_param_offset-number;
};
int setParameterMap(Vector3Map& map) {
new(&map) Vector3Map(&Parameter(m_param_offset));
m_param_offset += 3;
return m_param_offset-3;
};
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));
};
void setJacobiMap(int eqn, int offset, int number, VectorMap& map) {
new(&map) VectorMap(&Jacobi(eqn, offset), number, DynStride(0,m_eqns));
};
bool isValid() {
if(!m_params || !m_eqns) return false;
return true;
};
virtual void recalculate() = 0;
};
Kernel() {};
template <typename DerivedA,typename DerivedB>
static bool isSame(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2) {
return ((p1-p2).squaredNorm() < 0.00001);
}
static bool isSame(number_type t1, number_type t2) {
return (std::abs(t1-t2) < 0.00001);
}
template <typename DerivedA,typename DerivedB>
static bool isOpposite(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2) {
return ((p1+p2).squaredNorm() < 0.00001);
}
static int solve(MappedEquationSystem& mes) {
nothing n;
return Nonlinear< Kernel<Scalar, Nonlinear> >().solve(mes, n);
};
template<typename Functor>
static int solve(MappedEquationSystem& mes, Functor& f) {
return Nonlinear< Kernel<Scalar, Nonlinear> >().solve(mes, f);
};
};
}
#endif //GCM_KERNEL_H

94
src/Mod/Assembly/App/opendcm/core/logging.hpp Normal file
View File

@@ -0,0 +1,94 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_LOGGING_H
#define GCM_LOGGING_H
#ifdef USE_LOGGING
#include <boost/log/core.hpp>
#include <boost/log/sinks.hpp>
#include <boost/log/attributes.hpp>
#include <boost/log/attributes/attribute_def.hpp>
#include <boost/log/utility/init/to_file.hpp>
#include <boost/log/utility/init/common_attributes.hpp>
#include <boost/log/sources/logger.hpp>
#include <boost/log/sources/record_ostream.hpp>
#include <boost/log/formatters.hpp>
#include <boost/log/filters.hpp>
#include <boost/shared_ptr.hpp>
namespace logging = boost::log;
namespace sinks = boost::log::sinks;
namespace src = boost::log::sources;
namespace fmt = boost::log::formatters;
namespace flt = boost::log::filters;
namespace attrs = boost::log::attributes;
namespace keywords = boost::log::keywords;
namespace dcm {
static int counter = 0;
typedef sinks::synchronous_sink< sinks::text_file_backend > sink_t;
inline boost::shared_ptr< sink_t > init_log() {
//create the filename
std::stringstream str;
str<<"openDCM_"<<counter<<"_%N.log";
counter++;
boost::shared_ptr< logging::core > core = logging::core::get();
boost::shared_ptr< sinks::text_file_backend > backend =
boost::make_shared< sinks::text_file_backend >(
keywords::file_name = str.str(), //file name pattern
keywords::rotation_size = 10 * 1024 * 1024 //Rotation size is 10MB
);
// Wrap it into the frontend and register in the core.
// The backend requires synchronization in the frontend.
boost::shared_ptr< sink_t > sink(new sink_t(backend));
sink->set_formatter(
fmt::stream <<"[" << fmt::attr<std::string>("Tag") <<"] "
<< fmt::if_(flt::has_attr<std::string>("ID")) [
fmt::stream << "["<< fmt::attr< std::string >("ID")<<"] "]
<< fmt::message()
);
core->add_sink(sink);
return sink;
};
inline void stop_log(boost::shared_ptr< sink_t >& sink) {
boost::shared_ptr< logging::core > core = logging::core::get();
// Remove the sink from the core, so that no records are passed to it
core->remove_sink(sink);
sink.reset();
};
}; //dcm
#endif //USE_LOGGING
#endif //GCM_LOGGING_H

269
src/Mod/Assembly/App/opendcm/core/object.hpp Normal file
View File

@@ -0,0 +1,269 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_OBJECT_H
#define GCM_OBJECT_H
#include <iostream>
#include <list>
#include <boost/mpl/at.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/void.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/key_type.hpp>
#include <boost/mpl/value_type.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/preprocessor.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/repetition/enum.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/enum_trailing_params.hpp>
#include <boost/preprocessor/repetition/enum_binary_params.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/any.hpp>
#include "property.hpp"
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
#define EMIT_ARGUMENTS(z, n, data) \
BOOST_PP_CAT(data, n)
#define EMIT_CALL_DEF(z, n, data) \
template < \
typename S \
BOOST_PP_ENUM_TRAILING_PARAMS(n, typename Arg) \
> \
void emitSignal( \
BOOST_PP_ENUM_BINARY_PARAMS(n, Arg, const& arg) \
);
#define EMIT_CALL_DEC(z, n, data) \
template<typename Sys, typename Derived, typename Sig> \
template < \
typename S \
BOOST_PP_ENUM_TRAILING_PARAMS(n, typename Arg) \
> \
void Object<Sys, Derived, Sig>::emitSignal( \
BOOST_PP_ENUM_BINARY_PARAMS(n, Arg, const& arg) \
) \
{ \
typedef typename mpl::find<sig_name, S>::type iterator; \
typedef typename mpl::distance<typename mpl::begin<sig_name>::type, iterator>::type distance; \
typedef typename fusion::result_of::value_at<Signals, distance>::type list_type; \
list_type& list = fusion::at<distance>(m_signals); \
for (typename list_type::iterator it=list.begin(); it != list.end(); it++) \
(*it)(BOOST_PP_ENUM(n, EMIT_ARGUMENTS, arg)); \
};
namespace dcm {
//few standart signal names
struct remove {};
struct recalculated {};
typedef boost::any Connection;
/**
* @brief Base class for all object types
*
* This class add's property and signal capabilitys to all deriving classes. For properties it is tigthly
* integrated with the system class: It searches systems property list for the derived class as specified by
* the second template parameter and makes it accessible via appopriate functions. Signals are speciefied by a
* mpl::map with signal name type as key and a boost::function as values.
*
* \tparam Sys class of type System of which the properties are taken
* \tparam Obj the type of the derived object
* \tparam Sig a mpl::map specifing the object's signals by (type - boost::function) pairs
**/
template<typename Sys, typename Derived, typename Sig>
struct Object : public boost::enable_shared_from_this<Derived> {
Object() {};
Object(Sys& system);
/**
* @brief Create a new object of the same type with the same values
*
* Returns a new shared_ptr for the Drived type with the same properties as the initial one. If
* the new pointer should be used in a new system the parameter \param newSys needs to be that
* new system. Overload this function if you have datamembers in any derived class wich are not
* copyconstructable.
* @tparam Prop property type which should be accessed
* @return Prop::type& a reference to the properties actual value.
**/
virtual boost::shared_ptr<Derived> clone(Sys& newSys);
/**
* @brief Access properties
*
* Returns a reference to the propertys actual value. The property type has to be registerd to the
* System type which was given as template parameter to this object.
* @tparam Prop property type which should be accessed
* @return Prop::type& a reference to the properties actual value.
**/
template<typename Prop>
typename Prop::type& getProperty();
/**
* @brief Set properties
*
* Set'S the value of a specified property. The property type has to be registerd to the
* System type which was given as template parameter to this object. Note that setProperty(value)
* is equivalent to getProperty() = value.
* @tparam Prop property type which should be setProperty
* @param value value of type Prop::type which should be set in this object
**/
template<typename Prop>
void setProperty(typename Prop::type value);
/**
* @brief Connects a slot to a specified signal.
*
* Slots are boost::functions which get called when the signal is emitted. Any valid boost::function
* which ressembles the signal tyes signature can be registert. It is important that the signal type
* was registerd to this object on creation by the appropriate template parameter.
*
* @tparam S the signal which should be intercepted
* @param function boost::function which resembles the signal type's signature
* @return void
**/
template<typename S>
Connection connectSignal(typename mpl::at<Sig, S>::type function);
/**
* @brief Disconnects a slot for a specific signal.
*
* Disconnects a slot so that it dosn't get called at signal emittion. It's important to
* disconnect the slot by the same boost:function it was connected with.
*
* @tparam S the signal type of interest
* @param function boost::function with which the slot was connected
* @return void
**/
template<typename S>
void disconnectSignal(Connection c);
/*properties
* search the property map of the system class and get the mpl::vector of properties for the
* derived type. It's imortant to not store the properties but their types. These types are
* stored and accessed as fusion vector.
* */
typedef typename mpl::at<typename Sys::object_properties, Derived>::type Mapped;
typedef typename mpl::if_< boost::is_same<Mapped, mpl::void_ >, mpl::vector0<>, Mapped>::type Sequence;
typedef typename details::pts<Sequence>::type Properties;
Properties m_properties;
Sys* m_system;
protected:
/*signal handling
* extract all signal types to allow index search (inex search on signal functions would fail as same
* signatures are supported for multiple signals). Create std::vectors to allow multiple slots per signal
* and store these vectors in a fusion::vector for easy access.
* */
typedef typename mpl::fold< Sig, mpl::vector<>,
mpl::push_back<mpl::_1, mpl::key_type<Sig, mpl::_2> > >::type sig_name;
typedef typename mpl::fold< Sig, mpl::vector<>,
mpl::push_back<mpl::_1, mpl::value_type<Sig, mpl::_2> > >::type sig_functions;
typedef typename mpl::fold< sig_functions, mpl::vector<>,
mpl::push_back<mpl::_1, std::list<mpl::_2> > >::type sig_vectors;
typedef typename fusion::result_of::as_vector<sig_vectors>::type Signals;
Signals m_signals;
public:
//with no vararg templates before c++11 we need preprocessor to create the overloads of emit signal we need
BOOST_PP_REPEAT(5, EMIT_CALL_DEF, ~)
};
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
template<typename Sys, typename Derived, typename Sig>
Object<Sys, Derived, Sig>::Object(Sys& system) : m_system(&system) {};
template<typename Sys, typename Derived, typename Sig>
boost::shared_ptr<Derived> Object<Sys, Derived, Sig>::clone(Sys& newSys) {
boost::shared_ptr<Derived> np = boost::shared_ptr<Derived>(new Derived(*static_cast<Derived*>(this)));
np->m_system = &newSys;
return np;
};
template<typename Sys, typename Derived, typename Sig>
template<typename Prop>
typename Prop::type& Object<Sys, Derived, Sig>::getProperty() {
typedef typename mpl::find<Sequence, Prop>::type iterator;
typedef typename mpl::distance<typename mpl::begin<Sequence>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<Sequence>::type > >));
return fusion::at<distance>(m_properties);
};
template<typename Sys, typename Derived, typename Sig>
template<typename Prop>
void Object<Sys, Derived, Sig>::setProperty(typename Prop::type value) {
typedef typename mpl::find<Sequence, Prop>::type iterator;
typedef typename mpl::distance<typename mpl::begin<Sequence>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<Sequence>::type > >));
fusion::at<distance>(m_properties) = value;
};
template<typename Sys, typename Derived, typename Sig>
template<typename S>
Connection Object<Sys, Derived, Sig>::connectSignal(typename mpl::at<Sig, S>::type function) {
typedef typename mpl::find<sig_name, S>::type iterator;
typedef typename mpl::distance<typename mpl::begin<sig_name>::type, iterator>::type distance;
typedef typename fusion::result_of::value_at<Signals, distance>::type list_type;
list_type& list = fusion::at<distance>(m_signals);
return list.insert(list.begin(),function);
};
template<typename Sys, typename Derived, typename Sig>
template<typename S>
void Object<Sys, Derived, Sig>::disconnectSignal(Connection c) {
typedef typename mpl::find<sig_name, S>::type iterator;
typedef typename mpl::distance<typename mpl::begin<sig_name>::type, iterator>::type distance;
typedef typename fusion::result_of::value_at<Signals, distance>::type list_type;
list_type& list = fusion::at<distance>(m_signals);
list.erase(boost::any_cast<typename list_type::iterator>(c));
};
BOOST_PP_REPEAT(5, EMIT_CALL_DEC, ~)
}
#endif //GCM_OBJECT_H

167
src/Mod/Assembly/App/opendcm/core/property.hpp Normal file
View File

@@ -0,0 +1,167 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_PROPERTY_H
#define GCM_PROPERTY_H
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/container/vector.hpp>
#include <boost/mpl/find.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/property_map/property_map.hpp>
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
namespace dcm {
struct vertex_property {};
struct edge_property {};
struct cluster_property {};
struct object_property {};
namespace details {
template<typename Graph>
struct vertex_selector {
typedef typename boost::graph_traits<Graph>::vertex_descriptor key_type;
typedef typename Graph::vertex_properties sequence_type;
};
template<typename Graph>
struct edge_selector {
typedef typename boost::graph_traits<Graph>::edge_descriptor key_type;
typedef typename Graph::edge_properties sequence_type;
};
template< typename Kind, typename Graph>
struct property_selector : public mpl::if_<boost::is_same<Kind, vertex_property>,
vertex_selector<Graph>, edge_selector<Graph> >::type {};
template<typename T>
struct property_type {
typedef typename T::type type;
};
template<typename T>
struct property_kind {
typedef typename T::kind type;
};
//property vector to a fusion sequence of the propety types
template<typename T>
struct pts { //property type sequence
typedef typename mpl::transform<T, details::property_type<mpl::_1> >::type ptv;
typedef typename fusion::result_of::as_vector< ptv >::type type;
};
}
template<typename T>
struct is_edge_property : boost::is_same<typename T::kind,edge_property> {};
template<typename T>
struct is_vertex_property : boost::is_same<typename T::kind,vertex_property> {};
template<typename T>
struct is_cluster_property : boost::is_same<typename T::kind,cluster_property> {};
template<typename T>
struct is_object_property : boost::is_same<typename T::kind,object_property> {};
template <typename Property, typename Graph>
class property_map {
public:
typedef typename dcm::details::property_selector<typename Property::kind, Graph>::key_type key_type;
typedef typename Property::type value_type;
typedef typename Property::type& reference;
typedef boost::lvalue_property_map_tag category;
typedef Property property;
typedef typename dcm::details::property_selector<typename Property::kind, Graph>::sequence_type sequence;
property_map(Graph& g)
: m_graph(g) { }
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
//***********************************
struct empty_prop {
typedef int kind;
typedef int type;
};
struct type_prop {
//states the type of a cluster
typedef cluster_property kind;
typedef int type;
};
struct changed_prop {
typedef cluster_property kind;
typedef bool type;
};
template<typename T>
struct id_prop {
typedef object_property kind;
typedef T type;
};
}
#endif //GCM_PROPERTY_H

100
src/Mod/Assembly/App/opendcm/core/sheduler.hpp Normal file
View File

@@ -0,0 +1,100 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_SHEDULER_H
#define GCM_SHEDULER_H
#include <set>
#include <algorithm>
namespace dcm {
template<typename Sys>
struct Job {
virtual ~Job() {};
virtual void execute(Sys& sys) = 0;
bool operator<(const Job<Sys>& j) const {
return priority < j.priority;
};
int priority;
};
template<typename functor, typename System, int prior>
struct FunctorJob : Job<System> {
FunctorJob(functor f) : m_functor(f) {
Job<System>::priority = prior;
};
virtual void execute(System& sys) {
m_functor(sys);
};
functor m_functor;
};
template<typename Sys>
class Sheduler {
public:
Sheduler() {};
~Sheduler() {
std::for_each(m_preprocess.begin(), m_preprocess.end(), Deleter());
std::for_each(m_process.begin(), m_process.end(), Deleter());
std::for_each(m_postprocess.begin(), m_postprocess.end(), Deleter());
};
void addPreprocessJob(Job<Sys>* j) {
m_preprocess.insert(j);
};
void addPostprocessJob(Job<Sys>* j) {
m_postprocess.insert(j);
};
void addProcessJob(Job<Sys>* j) {
m_process.insert(j);
};
void execute(Sys& system) {
std::for_each(m_preprocess.begin(), m_preprocess.end(), Executer(system));
std::for_each(m_process.begin(), m_process.end(), Executer(system));
std::for_each(m_postprocess.begin(), m_postprocess.end(), Executer(system));
};
protected:
struct Executer {
Executer(Sys& s) : m_system(s) {};
void operator()(Job<Sys>* j) {
j->execute(m_system);
};
Sys& m_system;
};
struct Deleter {
void operator()(Job<Sys>* j) {
delete j;
};
};
std::set< Job<Sys>* > m_preprocess, m_process, m_postprocess;
};
}
#endif //GCM_SHEDULER_H

327
src/Mod/Assembly/App/opendcm/core/system.hpp Normal file
View File

@@ -0,0 +1,327 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_SYSTEM_H
#define GCM_SYSTEM_H
#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector/vector0.hpp>
#include <boost/mpl/map.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/count.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/less_equal.hpp>
#include <boost/mpl/print.hpp>
#include <boost/mpl/and.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <iostream>
#include "property.hpp"
#include "clustergraph.hpp"
#include "sheduler.hpp"
#include "logging.hpp"
#include "traits.hpp"
namespace mpl = boost::mpl;
namespace dcm {
struct No_Identifier {};
struct Unspecified_Identifier {};
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>,
mpl::push_back<mpl::_1,mpl::_2>, mpl::_1 > > {};
template<typename seq, typename state>
struct vertex_fold : mpl::fold< seq, state,
mpl::if_< is_vertex_property<mpl::_2>,
mpl::push_back<mpl::_1,mpl::_2>, mpl::_1 > > {};
template<typename seq, typename state>
struct cluster_fold : mpl::fold< seq, state,
mpl::if_< is_cluster_property<mpl::_2>,
mpl::push_back<mpl::_1,mpl::_2>, mpl::_1 > > {};
template<typename seq, typename state, typename obj>
struct obj_fold : mpl::fold< seq, state,
mpl::if_< mpl::or_<
boost::is_same< details::property_kind<mpl::_2>, obj>, is_object_property<mpl::_2> >,
mpl::push_back<mpl::_1,mpl::_2>, mpl::_1 > > {};
template<typename objects, typename properties>
struct property_map {
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;
};
template<typename T>
struct get_identifier {
typedef typename T::Identifier type;
};
template<typename seq, typename state>
struct map_fold : mpl::fold< seq, state, mpl::insert<mpl::_1,mpl::_2> > {};
struct nothing {};
template<int v>
struct EmptyModule {
template<typename T>
struct type {
struct inheriter {};
typedef mpl::vector<> properties;
typedef mpl::vector<> objects;
typedef Unspecified_Identifier Identifier;
static void system_init(T& sys) {};
static void system_copy(const T& from, T& into) {};
};
};
template <class T>
struct is_shared_ptr : boost::mpl::false_ {};
template <class T>
struct is_shared_ptr<boost::shared_ptr<T> > : boost::mpl::true_ {};
}
template< typename KernelType,
typename T1 = details::EmptyModule<1>,
typename T2 = details::EmptyModule<2>,
typename T3 = details::EmptyModule<3> >
class System : public T1::template type< System<KernelType,T1,T2,T3> >::inheriter,
public T2::template type< System<KernelType,T1,T2,T3> >::inheriter,
public T3::template type< System<KernelType,T1,T2,T3> >::inheriter {
typedef System<KernelType,T1,T2,T3> BaseType;
public:
typedef typename T1::template type< BaseType > Type1;
typedef typename T2::template type< BaseType > Type2;
typedef typename T3::template type< BaseType > Type3;
typedef mpl::vector3<Type1, Type2, Type3> TypeVector;
//Check if all Identifiers are the same and find out which type it is
typedef typename mpl::fold<TypeVector, mpl::vector<>,
mpl::if_<boost::is_same<details::get_identifier<mpl::_2>, Unspecified_Identifier>,
mpl::_1, mpl::push_back<mpl::_1, details::get_identifier<mpl::_2> > > >::type Identifiers;
BOOST_MPL_ASSERT((mpl::or_<
mpl::less_equal<typename mpl::size<Identifiers>::type, mpl::int_<1> >,
mpl::equal< typename mpl::count<Identifiers,
typename mpl::at_c<Identifiers,0> >::type, typename mpl::size<Identifiers>::type > >));
typedef typename mpl::if_< mpl::empty<Identifiers>,
No_Identifier, typename mpl::at_c<Identifiers, 0>::type >::type Identifier;
public:
//get all module objects and properties
typedef typename details::vector_fold<typename Type3::objects,
typename details::vector_fold<typename Type2::objects,
typename details::vector_fold<typename Type1::objects,
mpl::vector<> >::type >::type>::type objects;
typedef typename details::vector_fold<typename Type3::properties,
typename details::vector_fold<typename Type2::properties,
typename details::vector_fold<typename Type1::properties,
mpl::vector<id_prop<Identifier> > >::type >::type>::type properties;
//make the subcomponent lists of objects and properties
typedef typename details::edge_fold< properties, mpl::vector<> >::type edge_properties;
typedef typename details::vertex_fold< properties, mpl::vector<> >::type vertex_properties;
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;
protected:
//object storage
typedef typename mpl::transform<objects, boost::shared_ptr<mpl::_1> >::type sp_objects;
typedef typename mpl::fold< sp_objects, mpl::vector<>,
mpl::push_back<mpl::_1, std::vector<mpl::_2> > >::type object_vectors;
typedef typename fusion::result_of::as_vector<object_vectors>::type Storage;
template<typename FT1, typename FT2, typename FT3>
friend struct Object;
struct clearer {
template<typename T>
void operator()(T& vector) const {
vector.clear();
};
};
#ifdef USE_LOGGING
boost::shared_ptr< sink_t > sink;
#endif
public:
typedef ClusterGraph<edge_properties, vertex_properties, cluster_properties, objects> Cluster;
typedef Sheduler< BaseType > Shedule;
typedef KernelType Kernel;
public:
System() : m_cluster(new Cluster), m_storage(new Storage)
#ifdef USE_LOGGING
, sink(init_log())
#endif
{
Type1::system_init(*this);
Type2::system_init(*this);
Type3::system_init(*this);
};
~System() {
#ifdef USE_LOGGING
stop_log(sink);
#endif
};
void clear() {
m_cluster->clearClusters();
m_cluster->clear();
fusion::for_each(*m_storage, clearer());
};
template<typename Object>
typename std::vector< boost::shared_ptr<Object> >::iterator begin() {
typedef typename mpl::find<objects, Object>::type iterator;
typedef typename mpl::distance<typename mpl::begin<objects>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<objects>::type > >));
return fusion::at<distance>(*m_storage).begin();
};
template<typename Object>
typename std::vector< boost::shared_ptr<Object> >::iterator end() {
typedef typename mpl::find<objects, Object>::type iterator;
typedef typename mpl::distance<typename mpl::begin<objects>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<objects>::type > >));
return fusion::at<distance>(*m_storage).end();
};
template<typename Object>
std::vector< boost::shared_ptr<Object> >& objectVector() {
typedef typename mpl::find<objects, Object>::type iterator;
typedef typename mpl::distance<typename mpl::begin<objects>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<objects>::type > >));
return fusion::at<distance>(*m_storage);
};
template<typename Object>
void push_back(boost::shared_ptr<Object> ptr) {
objectVector<Object>().push_back(ptr);
};
template<typename Object>
void erase(boost::shared_ptr<Object> ptr) {
std::vector< boost::shared_ptr<Object> >& vec = objectVector<Object>();
vec.erase(std::remove(vec.begin(), vec.end(), ptr), vec.end());
};
void solve() {
clock_t start = clock();
m_sheduler.execute(*this);
clock_t end = clock();
double ms = (double(end-start)* 1000.) / double(CLOCKS_PER_SEC);
//Base::Console().Message("overall solving time in ms: %f\n", ms);
};
System* createSubsystem() {
System* s = new System();
s->m_cluster = m_cluster->createCluster().first;
s->m_storage = m_storage;
s->m_cluster->template setClusterProperty<dcm::type_prop>(details::subcluster);
return s;
};
private:
struct cloner {
System& newSys;
cloner(System& ns) : newSys(ns) {};
template<typename T>
struct test : mpl::and_<details::is_shared_ptr<T>,
mpl::not_<boost::is_same<T, boost::shared_ptr<typename System::Cluster> > > > {};
template<typename T>
typename boost::enable_if< test<T>, void>::type operator()(T& p) const {
p = p->clone(newSys);
newSys.push_back(p);
};
template<typename T>
typename boost::enable_if< mpl::not_<test<T> >, void>::type operator()(const T& p) const {};
};
public:
void copyInto(System& into) const {
//copy the clustergraph and clone all objects while at it. They are also pushed to the storage
cloner cl(into);
m_cluster->copyInto(into.m_cluster, cl);
//notify all modules that they are copied
Type1::system_copy(*this, into);
Type2::system_copy(*this, into);
Type3::system_copy(*this, into);
};
System* clone() const {
System* ns = new System();
this->copyInto(*ns);
return ns;
};
boost::shared_ptr<Cluster> m_cluster;
boost::shared_ptr<Storage> m_storage;
Shedule m_sheduler;
Kernel m_kernel;
};
}
#endif //GCM_SYSTEM_H

101
src/Mod/Assembly/App/opendcm/core/traits.hpp Normal file
View File

@@ -0,0 +1,101 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2012 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef GCM_TRAITS_H
#define GCM_TRAITS_H
#include <boost/type_traits.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/void.hpp>
#include <boost/preprocessor/repetition/enum_shifted.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>
#include <boost/preprocessor/repetition/enum.hpp>
#include <string.h>
namespace mpl = boost::mpl;
namespace dcm {
template< typename T >
struct system_traits {
typedef typename T::Kernel Kernel;
typedef typename T::Cluster Cluster;
template<typename M>
struct getModule {
typedef typename mpl::if_< boost::is_base_of<M, typename T::Type1>, typename T::Type1, typename T::Type2 >::type test1;
typedef typename mpl::if_< boost::is_base_of<M, test1>, test1, typename T::Type3 >::type test2;
typedef typename mpl::if_< boost::is_base_of<M, test2>, test2, mpl::void_ >::type type;
typedef boost::is_same<type, mpl::void_> has_module;
};
};
template<typename T>
struct compare_traits {
BOOST_MPL_ASSERT_MSG((mpl::not_<boost::is_same<T, const char*> >::value),
YOU_SHOULD_NOT_USE_THIS_TYPE_AS_IDENTIFIER,
(const char*));
static bool compare(T& first, T& second) {
return first == second;
};
};
template<>
struct compare_traits<std::string> {
static bool compare(std::string& first, std::string& second) {
return !(first.compare(second));
};
};
template<typename seq, int size>
struct vector_shrink {
typedef typename mpl::copy< seq, mpl::back_inserter< mpl::vector<> > >::type type;
};
template<typename seq>
struct vector_shrink< seq, 0 > {
typedef mpl::vector0<> type;
};
template<typename seq>
struct vector_shrink< seq, 1 > {
typedef mpl::vector1< typename mpl::at_c< seq, 0 >::type > type;
};
#define SEQ_TYPES(z, n, data) \
typename mpl::at_c< seq, n >::type
#define VECTOR_SHRINK(z, n, data) \
template< typename seq> \
struct vector_shrink<seq, n> { \
typedef BOOST_PP_CAT(mpl::vector, n) < \
BOOST_PP_ENUM(n, SEQ_TYPES, ~) > type; \
};
BOOST_PP_REPEAT_FROM_TO(2,10, VECTOR_SHRINK, ~)
}//namespace dcm
#endif //GCM_TRAITS_H

292
src/Mod/Assembly/App/opendcm/core/transformation.hpp Normal file
View File

@@ -0,0 +1,292 @@
/*
openDCM, dimensional constraint manager
Copyright (C) 2013 Stefan Troeger <stefantroeger@gmx.net>
This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along
with this library; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#ifndef DCM_TRANSFORMATION_H
#define DCM_TRANSFORMATION_H
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/mpl/if.hpp>
namespace dcm {
namespace detail {
template<typename Scalar, int Dim>
class Transform {
public:
typedef Eigen::Matrix<Scalar, Dim, 1> Vector;
typedef typename boost::mpl::if_c< Dim == 3,
Eigen::Quaternion<Scalar>,
Eigen::Rotation2D<Scalar> >::type Rotation;
typedef Eigen::Translation<Scalar, Dim> Translation;
typedef Eigen::UniformScaling<Scalar> Scaling;
typedef typename Rotation::RotationMatrixType RotationMatrix;
protected:
Rotation m_rotation;
Translation m_translation;
Scaling m_scale;
public:
Transform() : m_rotation(Rotation::Identity()),
m_translation(Translation::Identity()),
m_scale(Scaling(1.)) { };
Transform(const Rotation& r) : m_rotation(r),
m_translation(Translation::Identity()),
m_scale(Scaling(1.)) {
m_rotation.normalize();
};
Transform(const Rotation& r, const Translation& t) : m_rotation(r),
m_translation(t),
m_scale(Scaling(1.)) {
m_rotation.normalize();
};
Transform(const Rotation& r, const Translation& t, const Scaling& s) : m_rotation(r),
m_translation(t),
m_scale(s) {
m_rotation.normalize();
};
//access the single parts and manipulate them
//***********************
const Rotation& rotation() const {
return m_rotation;
}
template<typename Derived>
Transform& rotate(const Eigen::RotationBase<Derived,Dim>& rotation) {
m_rotation = rotation.derived().normalized()*m_rotation;
return *this;
}
const Translation& translation() const {
return m_translation;
}
Transform& translate(const Translation& translation) {
m_translation = m_translation*translation;
return *this;
}
const Scaling& scaling() const {
return m_scale;
}
Transform& scale(const Scalar& scaling) {
m_scale *= Scaling(scaling);
return *this;
}
Transform& scale(const Scaling& scaling) {
m_scale.factor() *= scaling.factor();
return *this;
}
Transform& invert() {
m_rotation = m_rotation.inverse();
m_translation.vector() = (m_rotation*m_translation.vector()) * (-m_scale.factor());
m_scale = Scaling(1./m_scale.factor());
return *this;
};
Transform inverse() {
Transform res(*this);
res.invert();
return res;
};
//operators for value manipulation
//********************************
inline Transform& operator=(const Translation& t) {
m_translation = t;
m_rotation = Rotation::Identity();
m_scale = Scaling(1.);
return *this;
}
inline Transform operator*(const Translation& t) const {
Transform res = *this;
res.translate(t);
return res;
}
inline Transform& operator*=(const Translation& t) {
return translate(t);
}
inline Transform& operator=(const Scaling& s) {
m_scale = s;
m_translation = Translation::Identity();
m_rotation = Rotation::Identity();
return *this;
}
inline Transform operator*(const Scaling& s) const {
Transform res = *this;
res.scale(s);
return res;
}
inline Transform& operator*=(const Scaling& s) {
return scale(s);
}
template<typename Derived>
inline Transform& operator=(const Eigen::RotationBase<Derived,Dim>& r) {
m_rotation = r.derived();
m_rotation.normalize();
m_translation = Translation::Identity();
m_scale = Scaling(1);
return *this;
}
template<typename Derived>
inline Transform operator*(const Eigen::RotationBase<Derived,Dim>& r) const {
Transform res = *this;
res.rotate(r.derived());
return res;
}
template<typename Derived>
inline Transform& operator*=(const Eigen::RotationBase<Derived,Dim>& r) {
return rotate(r.derived());
}
inline Transform operator* (const Transform& other) const {
Transform res(*this);
res*= other;
return res;
}
inline Transform& operator*= (const Transform& other) {
rotate(other.rotation());
other.rotate(m_translation.vector());
m_translation.vector() += other.translation().vector()/m_scale.factor();
m_scale.factor() *= other.scaling().factor();
return *this;
}
//transform Vectors
//*****************
template<typename Derived>
inline Derived& rotate(Eigen::MatrixBase<Derived>& vec) const {
vec = m_rotation*vec;
return vec.derived();
}
template<typename Derived>
inline Derived& translate(Eigen::MatrixBase<Derived>& vec) const {
vec = m_translation*vec;
return vec.derived();
}
template<typename Derived>
inline Derived& scale(Eigen::MatrixBase<Derived>& vec) const {
vec*=m_scale.factor();
return vec.derived();
}
template<typename Derived>
inline Derived& transform(Eigen::MatrixBase<Derived>& vec) const {
vec = (m_rotation*vec + m_translation.vector())*m_scale.factor();
return vec.derived();
}
template<typename Derived>
inline Derived operator*(const Eigen::MatrixBase<Derived>& vec) const {
return (m_rotation*vec + m_translation.vector())*m_scale.factor();
}
template<typename Derived>
inline void operator()(Eigen::MatrixBase<Derived>& vec) const {
transform(vec);
}
//Stuff
//*****
bool isApprox(const Transform& other, Scalar prec) const {
return m_rotation.isApprox(other.rotation(), prec)
&& ((m_translation.vector()- other.translation().vector()).norm() < prec)
&& (std::abs(m_scale.factor()-other.scaling().factor()) < prec);
};
void setIdentity() {
m_rotation.setIdentity();
m_translation = Translation::Identity();
m_scale = Scaling(1.);
}
static const Transform Identity() {
return Transform(Rotation::Identity(), Translation::Identity(), Scaling(1));
}
Transform& normalize() {
m_rotation.normalize();
return *this;
}
};
template<typename Scalar, int Dim>
class DiffTransform : public Transform<Scalar, Dim> {
typedef typename Transform<Scalar, Dim>::Rotation Rotation;
typedef typename Transform<Scalar, Dim>::Translation Translation;
typedef typename Transform<Scalar, Dim>::Scaling Scaling;
typedef Eigen::Matrix<Scalar, Dim, 3*Dim> DiffMatrix;
DiffMatrix m_diffMatrix;
public:
DiffTransform() : Transform<Scalar, Dim>() { };
DiffTransform(const Rotation& r) : Transform<Scalar, Dim>(r) {};
DiffTransform(const Rotation& r, const Translation& t) : Transform<Scalar, Dim>(r,t) {};
DiffTransform(const Rotation& r, const Translation& t, const Scaling& s) : Transform<Scalar, Dim>(r,t,s) {};
DiffTransform(Transform<Scalar, Dim>& trans)
: Transform<Scalar, Dim>(trans.rotation(), trans.translation(), trans.scaling()) {
m_diffMatrix.setZero();
};
const DiffMatrix& differential() {
return m_diffMatrix;
};
inline Scalar& operator()(int f, int s) {
return m_diffMatrix(f,s);
};
inline Scalar& at(int f, int s) {
return m_diffMatrix(f,s);
};
};
}//detail
}//DCM
/*When you overload a binary operator as a member function of a class the overload is used
* when the first operand is of the class type.For stream operators, the first operand
* is the stream and not (usually) the custom class.
*/
template<typename Kernel, int Dim>
std::ostream& operator<<(std::ostream& os, const dcm::detail::Transform<Kernel, Dim>& t) {
os << "Rotation: " << t.rotation().coeffs().transpose() << std::endl
<< "Translation: " << t.translation().vector().transpose() <<std::endl
<< "Scale: " << t.scaling().factor();
return os;
}
template<typename Kernel, int Dim>
std::ostream& operator<<(std::ostream& os, dcm::detail::DiffTransform<Kernel, Dim>& t) {
os << "Rotation: " << t.rotation().coeffs().transpose() << std::endl
<< "Translation: " << t.translation().vector().transpose() <<std::endl
<< "Scale: " << t.scaling().factor() << std::endl
<< "Differential:" << std::endl<<t.differential();
return os;
}
#endif //DCM_TRANSFORMATION