suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

windows adoptions

Browse Source
This commit is contained in:
Stefan Tröger 2013-10-03 10:14:47 +00:00
parent 7981edd639
commit b3058a4f5a
15 changed files with 605 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Mod/Assembly/App/opendcm/core.hpp
View File

@ -23,6 +23,8 @@
#ifdef _WIN32
//warning about to long decoraded names, won't affect the code correctness
#pragma warning( disable : 4503 )
//warning about changed pod initalising behaviour (boost blank in variant)
#pragma warning( disable : 4345 )
//disable boost concept checks, as some of them have alignment problems which bring msvc to an error
//(for example DFSvisitor check in boost::graph::depht_first_search)

34
src/Mod/Assembly/App/opendcm/core/constraint.hpp
View File

@ -51,7 +51,6 @@
#include "equations.hpp"
#include "geometry.hpp"
class T;
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
@ -59,6 +58,13 @@ namespace dcm {
namespace detail {
//metafunction to avoid ot-of-range access of mpl sequences
template<typename Sequence, int Value>
struct in_range_value {
typedef typename mpl::prior<mpl::size<Sequence> >::type last_id;
typedef typename mpl::min< mpl::int_<Value>, last_id>::type type;
};
//type erasure container for constraints
template<typename Sys, int Dim>
class Constraint {
@ -152,7 +158,7 @@ protected:
virtual ~placeholder() {}
virtual placeholder* resetConstraint(geom_ptr first, geom_ptr second) const = 0;
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false) = 0;
virtual void treatLGZ(geom_ptr first, geom_ptr second) = 0;
virtual void treatLGZ(geom_ptr first, geom_ptr second) = 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;
@ -297,7 +303,7 @@ public:
holder(Objects& obj);
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false);
virtual void treatLGZ(geom_ptr first, geom_ptr second);
virtual void treatLGZ(geom_ptr first, geom_ptr second);
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);
@ -370,7 +376,7 @@ template<typename ConstraintVector>
void Constraint<Sys, Dim>::initialize(ConstraintVector& cv) {
//use the compile time unrolling to retrieve the geometry tags
initializeFirstGeometry<mpl::int_<0>, ConstraintVector>(cv, mpl::true_());
initializeFirstGeometry<mpl::int_<0> >(cv, mpl::true_());
};
template<typename Sys, int Dim>
@ -507,15 +513,13 @@ void Constraint<Sys, Dim>::holder<ConstraintVector, EquationVector>::Calculater:
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
for(int i=0; i<3; i++) {
typename Kernel::VectorMap block(&first->m_diffparam(0,i),first->m_parameterCount,1, DS(1,1));
val.m_diff_first_rot(i) = val.m_eq.calculateGradientFirst(first->m_parameter,
second->m_parameter, block);
second->m_parameter, first->m_diffparam.col(i));
}
//and now with the translations
for(int i=0; i<3; i++) {
typename Kernel::VectorMap block(&first->m_diffparam(0,i+3),first->m_parameterCount,1, DS(1,1));
val.m_diff_first(i) = val.m_eq.calculateGradientFirst(first->m_parameter,
second->m_parameter, block);
second->m_parameter, first->m_diffparam.col(i+3));
}
}
}
@ -530,15 +534,13 @@ void Constraint<Sys, Dim>::holder<ConstraintVector, EquationVector>::Calculater:
//cluster mode, so we do a full calculation with all 3 rotation diffparam vectors
for(int i=0; i<3; i++) {
typename Kernel::VectorMap block(&second->m_diffparam(0,i),second->m_parameterCount,1, DS(1,1));
val.m_diff_second_rot(i) = val.m_eq.calculateGradientSecond(first->m_parameter,
second->m_parameter, block);
second->m_parameter, second->m_diffparam.col(i));
}
//and the translation seperated
for(int i=0; i<3; i++) {
typename Kernel::VectorMap block(&second->m_diffparam(0,i+3),second->m_parameterCount,1, DS(1,1));
val.m_diff_second(i) = val.m_eq.calculateGradientSecond(first->m_parameter,
second->m_parameter, block);
second->m_parameter, second->m_diffparam.col(i+3));
}
}
}
@ -632,7 +634,7 @@ void Constraint<Sys, Dim>::holder<ConstraintVector, EquationVector>::LGZ::operat
if(!val.enabled)
return;
//to treat local gradient zeros we calculate a approximate second derivative of the equations
//only do that if neseccary: residual is not zero
if(val.m_residual(0) > 1e-7) { //TODO: use exact precission and scale value
@ -845,14 +847,14 @@ template<typename WhichType, typename ConstraintVector>
void Constraint<Sys, Dim>::initializeFirstGeometry(ConstraintVector& cv, boost::mpl::true_ /*unrolled*/) {
typedef typename Sys::geometries geometries;
switch(first->getExactType()) {
#ifdef BOOST_PP_LOCAL_ITERATE
#define BOOST_PP_LOCAL_MACRO(n) \
case (WhichType::value + n): \
return initializeSecondGeometry<boost::mpl::int_<0>,\
typename mpl::at_c<geometries, WhichType::value + n >::type,\
typename mpl::at<geometries, typename in_range_value<geometries, WhichType::value + n>::type >::type,\
ConstraintVector>(cv, typename boost::mpl::less<boost::mpl::int_<WhichType::value + n>, boost::mpl::size<geometries> >::type()); \
break;
#define BOOST_PP_LOCAL_LIMITS (0, 10)
@ -883,7 +885,7 @@ void Constraint<Sys, Dim>::initializeSecondGeometry(ConstraintVector& cv, boost:
#define BOOST_PP_LOCAL_MACRO(n) \
case (WhichType::value + n): \
return intitalizeFinalize<FirstType, \
typename mpl::at_c<geometries, WhichType::value + n >::type,\
typename mpl::at<geometries, typename in_range_value<geometries, WhichType::value + n>::type >::type,\
ConstraintVector>(cv, typename boost::mpl::less<boost::mpl::int_<WhichType::value + n>, boost::mpl::size<geometries> >::type()); \
break;
#define BOOST_PP_LOCAL_LIMITS (0, 10)

80
src/Mod/Assembly/App/opendcm/core/equations.hpp
View File

@ -49,7 +49,9 @@ 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 DerivedA,typename DerivedB>
void calculatePseudo(const E::MatrixBase<DerivedA>& param1, Vec& v1, const E::MatrixBase<DerivedB>& param2, Vec& v2) {};
};
template<typename Kernel>
@ -60,7 +62,9 @@ struct 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) {};
template <typename DerivedA,typename DerivedB>
void calculatePseudo(const E::MatrixBase<DerivedA>& param1, Vec& v1, const E::MatrixBase<DerivedB>& param2, Vec& v2) {};
void setScale(typename Kernel::number_type scale) {};
};
@ -218,28 +222,42 @@ struct Distance : public Equation<Distance, double> {
Scalar value;
//template definition
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {
template <typename DerivedA,typename DerivedB>
void calculatePseudo(const E::MatrixBase<DerivedA>& param1, Vec& v1, const E::MatrixBase<DerivedB>& param2, Vec& v2) {
assert(false);
};
void setScale(Scalar scale) {
assert(false);
};
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
};
@ -267,22 +285,35 @@ struct Orientation : public Equation<Orientation, Direction, true> {
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
};
@ -307,22 +338,35 @@ struct Angle : public Equation<Angle, double, true> {
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
};

68
src/Mod/Assembly/App/opendcm/core/kernel.hpp
View File

@ -30,6 +30,7 @@
#include <boost/exception/exception.hpp>
#include <boost/exception/errinfo_errno.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/mpl/vector.hpp>
#include <time.h>
@ -37,11 +38,12 @@
#include "logging.hpp"
#include "defines.hpp"
#include "multimap.hpp"
namespace dcm {
#include "property.hpp"
namespace E = Eigen;
namespace mpl= boost::mpl;
namespace dcm {
struct nothing {
void operator()() {};
@ -54,6 +56,18 @@ enum ParameterType {
complete //all parameter
};
//solver settings
struct precision {
typedef double type;
typedef setting_property kind;
struct default_value {
double operator()() {
return 1e-6;
};
};
};
template<typename Kernel>
struct Dogleg {
@ -62,9 +76,10 @@ struct Dogleg {
#endif
typedef typename Kernel::number_type number_type;
number_type tolg, tolx, tolf;
number_type tolg, tolx;
Kernel* m_kernel;
Dogleg() : tolg(1e-40), tolx(1e-20), tolf(1e-6) {
Dogleg(Kernel* k) : m_kernel(k), tolg(1e-40), tolx(1e-20){
#ifdef USE_LOGGING
log.add_attribute("Tag", attrs::constant< std::string >("Dogleg"));
@ -190,7 +205,7 @@ struct Dogleg {
while(!stop) {
// check if finished
if(fx_inf <= tolf*sys.Scaling) // Success
if(fx_inf <= m_kernel->template getProperty<precision>()*sys.Scaling) // Success
stop = 1;
else
if(g_inf <= tolg)
@ -318,7 +333,7 @@ struct Dogleg {
};
template<typename Scalar, template<class> class Nonlinear = Dogleg>
struct Kernel {
struct Kernel : public PropertyOwner< mpl::vector<precision> > {
//basics
typedef Scalar number_type;
@ -489,34 +504,49 @@ struct Kernel {
};
virtual void recalculate() = 0;
virtual void removeLocalGradientZeros() = 0;
virtual void removeLocalGradientZeros() = 0;
};
Kernel() {};
//static comparison versions
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(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2, number_type precission) {
return ((p1-p2).squaredNorm() < precission);
}
static bool isSame(number_type t1, number_type t2) {
return (std::abs(t1-t2) < 0.00001);
static bool isSame(number_type t1, number_type t2, number_type precission) {
return (std::abs(t1-t2) < precission);
}
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 bool isOpposite(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2, number_type precission) {
return ((p1+p2).squaredNorm() < precission);
}
static int solve(MappedEquationSystem& mes) {
//runtime comparison versions (which use user settings for precission)
template <typename DerivedA,typename DerivedB>
bool isSame(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2) {
return ((p1-p2).squaredNorm() < getProperty<precision>());
}
bool isSame(number_type t1, number_type t2) {
return (std::abs(t1-t2) < getProperty<precision>());
}
template <typename DerivedA,typename DerivedB>
bool isOpposite(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2) {
return ((p1+p2).squaredNorm() < getProperty<precision>());
}
int solve(MappedEquationSystem& mes) {
nothing n;
return Nonlinear< Kernel<Scalar, Nonlinear> >().solve(mes, n);
return NonlinearSolver(this).solve(mes, n);
};
template<typename Functor>
static int solve(MappedEquationSystem& mes, Functor& f) {
return Nonlinear< Kernel<Scalar, Nonlinear> >().solve(mes, f);
int solve(MappedEquationSystem& mes, Functor& f) {
return NonlinearSolver(this).solve(mes, f);
};
typedef mpl::vector1<precision> properties;
};
}

13
src/Mod/Assembly/App/opendcm/core/property.hpp
View File

@ -23,17 +23,24 @@
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/container/vector.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/void.hpp>
#include <boost/mpl/filter_view.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/fusion/mpl.hpp>
#include <boost/fusion/include/sequence.hpp>
#include <boost/fusion/include/container.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/size.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/property_map/property_map.hpp>
#include "kernel.hpp"
#include <boost/exception/errinfo_errno.hpp>
#include "defines.hpp"
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;

33
src/Mod/Assembly/App/opendcm/core/system.hpp
View File

@ -161,7 +161,7 @@ public:
typedef typename details::vertex_fold< properties,
mpl::vector1<vertex_index_prop> >::type vertex_properties;
typedef typename details::cluster_fold< properties,
mpl::vector2<changed_prop, type_prop> >::type cluster_properties;
mpl::vector2<changed_prop, type_prop> >::type cluster_properties;
protected:
//object storage
@ -277,15 +277,41 @@ public:
return s;
};
//a kernel has it's own settings, therefore we need to decide which is accessed
template<typename Setting>
typename Setting::type& getSetting() {
typename boost::enable_if< boost::is_same< typename mpl::find<typename Kernel::properties, Setting>::type,
typename mpl::end<typename Kernel::properties>::type >, typename Setting::type& >::type getSetting() {
return m_settings.template getProperty<Setting>();
};
template<typename Setting>
void setSetting(typename Setting::type value){
typename boost::disable_if< boost::is_same< typename mpl::find<typename Kernel::properties, Setting>::type,
typename mpl::end<typename Kernel::properties>::type >, typename Setting::type& >::type getSetting() {
return m_kernel.template getProperty<Setting>();
};
template<typename Setting>
typename boost::enable_if< boost::is_same< typename mpl::find<typename Kernel::properties, Setting>::type,
typename mpl::end<typename Kernel::properties>::type >, void >::type setSetting(typename Setting::type value){
m_settings.template setProperty<Setting>(value);
};
template<typename Setting>
typename boost::disable_if< boost::is_same< typename mpl::find<typename Kernel::properties, Setting>::type,
typename mpl::end<typename Kernel::properties>::type >, void >::type setSetting(typename Setting::type value){
m_kernel.template setProperty<Setting>(value);
};
//convinience function
template<typename Setting>
typename Setting::type& setting() {
return getSetting<Setting>();
};
//let evryone access and use our math kernel
Kernel& kernel() {
return m_kernel;
};
private:
struct cloner {
@ -344,3 +370,4 @@ public:

87
src/Mod/Assembly/App/opendcm/module3d/angle.hpp
View File

@ -28,49 +28,49 @@ namespace dcm {
//the calculations( same as we always calculate directions we can outsource the work to this functions)
namespace angle_detail {
template<typename Kernel, typename T>
inline typename Kernel::number_type calc(const T& d1,
const T& d2,
template<typename Kernel, typename T1, typename T2>
inline typename Kernel::number_type calc(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const typename Kernel::number_type& angle) {
return d1.dot(d2) / (d1.norm()*d2.norm()) - std::cos(angle);
};
template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradFirst(const T& d1,
const T& d2,
const T& dd1) {
template<typename Kernel, typename T1, typename T2, typename T3>
inline typename Kernel::number_type calcGradFirst(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& dd1) {
typename Kernel::number_type norm = d1.norm()*d2.norm();
return dd1.dot(d2)/norm - (d1.dot(d2) * (d1.dot(dd1)/d1.norm())*d2.norm()) / std::pow(norm,2);
};
template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradSecond(const T& d1,
const T& d2,
const T& dd2) {
template<typename Kernel, typename T1, typename T2, typename T3>
inline typename Kernel::number_type calcGradSecond(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& dd2) {
typename Kernel::number_type norm = d1.norm()*d2.norm();
return d1.dot(dd2)/norm - (d1.dot(d2) * (d2.dot(dd2)/d2.norm())*d1.norm()) / std::pow(norm,2);
};
template<typename Kernel, typename T>
inline void calcGradFirstComp(const T& d1,
const T& d2,
const T& grad) {
template<typename Kernel, typename T1, typename T2, typename T3>
inline void calcGradFirstComp(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& grad) {
typename Kernel::number_type norm = d1.norm()*d2.norm();
const_cast< T& >(grad) = d2/norm - d1.dot(d2)*d1/(std::pow(d1.norm(),3)*d2.norm());
const_cast< E::MatrixBase<T3>& >(grad) = d2/norm - d1.dot(d2)*d1/(std::pow(d1.norm(),3)*d2.norm());
};
template<typename Kernel, typename T>
inline void calcGradSecondComp(const T& d1,
const T& d2,
const T& grad) {
template<typename Kernel, typename T1, typename T2, typename T3>
inline void calcGradSecondComp(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& grad) {
typename Kernel::number_type norm = d1.norm()*d2.norm();
const_cast< T& >(grad) = d1/norm - d1.dot(d2)*d2/(std::pow(d2.norm(),3)*d1.norm());
const_cast< E::MatrixBase<T3>& >(grad) = d1/norm - d1.dot(d2)*d2/(std::pow(d2.norm(),3)*d1.norm());
};
}
@ -85,20 +85,33 @@ struct Angle::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return angle_detail::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return angle_detail::calcGradFirst<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam1.template segment<3>(3));
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return angle_detail::calcGradSecond<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam2.template segment<3>(3));
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
angle_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3));
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
angle_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3));
};
@ -112,7 +125,10 @@ struct Angle::type< Kernel, tag::line3D, tag::cylinder3D > : public Angle::type<
typedef typename Kernel::VectorMap Vector;
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Angle::type<Kernel, tag::line3D, tag::line3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};
@ -126,7 +142,10 @@ struct Angle::type< Kernel, tag::plane3D, tag::cylinder3D > : public Angle::type
typedef typename Kernel::VectorMap Vector;
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Angle::type<Kernel, tag::line3D, tag::line3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};
@ -137,12 +156,18 @@ struct Angle::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Angle::t
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Angle::type<Kernel, tag::line3D, tag::line3D>::calculateGradientFirstComplete(param1, param2, gradient);
gradient(6)=0;
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Angle::type<Kernel, tag::line3D, tag::line3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};

18
src/Mod/Assembly/App/opendcm/module3d/clustermath.hpp
View File

@ -492,7 +492,7 @@ typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calculateClusterScale() {
const typename Kernel::Vector3 p1 = m_points[0];
const typename Kernel::Vector3 p2 = m_points[1];
if(Kernel::isSame((p1-p2).norm(), 0.))
if(Kernel::isSame((p1-p2).norm(), 0., 1e-10))
return calcOnePoint(p1);
return calcTwoPoints(p1, p2);
@ -505,16 +505,16 @@ typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calculateClusterScale() {
const typename Kernel::Vector3 d = p2-p1;
const typename Kernel::Vector3 e = p3-p1;
if(Kernel::isSame(d.norm(), 0.)) {
if(Kernel::isSame(d.norm(), 0., 1e-10)) {
if(Kernel::isSame(e.norm(), 0.))
if(Kernel::isSame(e.norm(), 0., 1e-10))
return calcOnePoint(p1);
return calcTwoPoints(p1, p3);
} else if(Kernel::isSame(e.norm(), 0.)) {
} else if(Kernel::isSame(e.norm(), 0., 1e-10)) {
return calcTwoPoints(p1, p2);
} else if(!Kernel::isSame((d/d.norm() - e/e.norm()).norm(), 0.) &&
!Kernel::isSame((d/d.norm() + e/e.norm()).norm(), 0.)) {
} else if(!Kernel::isSame((d/d.norm() - e/e.norm()).norm(), 0., 1e-10) &&
!Kernel::isSame((d/d.norm() + e/e.norm()).norm(), 0., 1e-10)) {
return calcThreePoints(p1, p2, p3);
}
//three points on a line need to be treaded as multiple points
@ -633,12 +633,12 @@ void ClusterMath<Sys>::applyClusterScale(Scalar scale, bool isFixed) {
}
//if this is our scale then just applie the midpoint as shift
if(Kernel::isSame(scale, m_scale)) {
if(Kernel::isSame(scale, m_scale, 1e-10)) {
}
//if only one point exists we extend the origin-point-line to match the scale
else if(mode==details::one) {
if(Kernel::isSame(midpoint.norm(),0))
if(Kernel::isSame(midpoint.norm(),0, 1e-10))
midpoint << scale, 0, 0;
else midpoint += scale*scale_dir;
}
@ -735,7 +735,7 @@ typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcTwoPoints(const typename
midpoint = p1+(p2-p1)/2.;
scale_dir = (p2-p1).cross(midpoint);
scale_dir = scale_dir.cross(p2-p1);
if(!Kernel::isSame(scale_dir.norm(),0)) scale_dir.normalize();
if(!Kernel::isSame(scale_dir.norm(),0, 1e-10)) scale_dir.normalize();
else scale_dir(0) = 1;
mode = details::two;
m_scale = (p2-p1).norm()/2.;

69
src/Mod/Assembly/App/opendcm/module3d/coincident.hpp
View File

@ -47,22 +47,35 @@ struct ci_orientation : public Equation<ci_orientation, Direction, true> {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
option_type value;
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
};
@ -75,19 +88,32 @@ struct ci_orientation::type< Kernel, tag::point3D, tag::point3D > : public dcm::
typedef typename Kernel::VectorMap Vector;
option_type value;
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.setZero();
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.setZero();
};
};
@ -138,22 +164,35 @@ struct ci_distance : public Equation<ci_distance, double> {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
option_type value;
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
assert(false);
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
assert(false);
};
};

178
src/Mod/Assembly/App/opendcm/module3d/distance.hpp
View File

@ -39,19 +39,32 @@ struct Distance::type< Kernel, tag::point3D, tag::point3D > {
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return (param1-param2).norm() - sc_value;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return (param1-param2).dot(dparam1) / (param1-param2).norm();
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return (param1-param2).dot(-dparam2) / (param1-param2).norm();
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient = (param1-param2) / (param1-param2).norm();
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient = (param2-param1) / (param1-param2).norm();
};
};
@ -88,7 +101,8 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& point, Vector& line) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& point, const E::MatrixBase<DerivedB>& line) {
//diff = point1 - point2
n = line.template segment<3>(3);
diff = line.template head<3>() - point.template head<3>();
@ -101,7 +115,10 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
return res;
};
Scalar calculateGradientFirst(Vector& point, Vector& line, Vector& dpoint) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& point,
const E::MatrixBase<DerivedB>& line,
const E::MatrixBase<DerivedC>& dpoint) {
if(dist.norm() == 0)
return 1.;
@ -117,7 +134,10 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
return res;
};
Scalar calculateGradientSecond(Vector& point, Vector& line, Vector& dline) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& point,
const E::MatrixBase<DerivedB>& line,
const E::MatrixBase<DerivedC>& dline) {
if(dist.norm() == 0)
return 1.;
@ -134,7 +154,10 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
return res;
};
void calculateGradientFirstComplete(Vector& point, Vector& line, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& point,
const E::MatrixBase<DerivedB>& line,
E::MatrixBase<DerivedC>& gradient) {
if(dist.norm() == 0) {
gradient.head(3).setOnes();
return;
@ -144,7 +167,10 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
gradient.head(3) = res/dist.norm();
};
void calculateGradientSecondComplete(Vector& point, Vector& line, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& point,
const E::MatrixBase<DerivedB>& line,
E::MatrixBase<DerivedC>& gradient) {
if(dist.norm() == 0) {
gradient.head(6).setOnes();
return;
@ -191,7 +217,8 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
//(p1-p2)°n / |n| - distance
const Scalar res = (param1.head(3)-param2.head(3)).dot(param2.tail(3)) / param2.tail(3).norm() - sc_value;
#ifdef USE_LOGGING
@ -201,7 +228,10 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
return res;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
//dp1°n / |n|
//if(dparam1.norm()!=1) return 0;
const Scalar res = (dparam1.head(3)).dot(param2.tail(3)) / param2.tail(3).norm();
@ -212,7 +242,10 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
return res;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
const typename Kernel::Vector3 p1 = param1.head(3);
const typename Kernel::Vector3 p2 = param2.head(3);
@ -228,11 +261,17 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
return res;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient = param2.tail(3) / param2.tail(3).norm();
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
const typename Kernel::Vector3 p1m2 = param1.head(3) - param2.head(3);
const typename Kernel::Vector3 n = param2.tail(3);
@ -252,13 +291,17 @@ struct Distance::type< Kernel, tag::point3D, tag::cylinder3D > : public Distance
Distance::type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance point3D cylinder3D");
};
#endif
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
//(p1-p2)°n / |n| - distance
const Scalar res = Distance::type< Kernel, tag::point3D, tag::line3D >::calculate(param1, param2);
return res - param2(6);
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};
@ -309,12 +352,13 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& line1, Vector& line2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& line1, const E::MatrixBase<DerivedB>& line2) {
//diff = point1 - point2
n1 = line1.template segment<3>(3);
n2 = line2.template segment<3>(3);
nxn = n1.cross(n2);
nxn_n = nxn.norm();
nxn_n = nxn.norm();
c = line2.template head<3>() - line1.template head<3>();
cdn = c.dot(nxn);
const Scalar res = std::abs(cdn) / nxn.norm();
@ -325,7 +369,10 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
return res;
};
Scalar calculateGradientFirst(Vector& line1, Vector& line2, Vector& dline1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& line1,
const E::MatrixBase<DerivedB>& line2,
const E::MatrixBase<DerivedC>& dline1) {
if(nxn_n == 0)
return 1.;
@ -334,7 +381,8 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
diff -= c.dot(nxn)*nxn.dot(nxn_diff)/nxn_n;
//absoulute value requires diffrent differentation for diffrent results
if(cdn <= 0) diff *= -1;
if(cdn <= 0)
diff *= -1;
diff /= std::pow(nxn_n,2);
@ -347,7 +395,10 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
return diff;
};
Scalar calculateGradientSecond(Vector& line1, Vector& line2, Vector& dline2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& line1,
const E::MatrixBase<DerivedB>& line2,
const E::MatrixBase<DerivedC>& dline2) {
if(nxn_n == 0)
return 1.;
@ -356,7 +407,8 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
diff -= c.dot(nxn)*nxn.dot(nxn_diff)/nxn_n;
//absoulute value requires diffrent differentation for diffrent results
if(cdn <= 0) diff *= -1;
if(cdn <= 0)
diff *= -1;
diff /= std::pow(nxn_n,2);
@ -369,7 +421,10 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
return diff;
};
void calculateGradientFirstComplete(Vector& line1, Vector& line2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& line1,
const E::MatrixBase<DerivedB>& line2,
E::MatrixBase<DerivedC>& gradient) {
if(nxn_n == 0) {
gradient.head(3).setOnes();
return;
@ -378,13 +433,17 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
if(cdn >= 0) {
gradient.template head<3>() = -nxn/nxn_n;
gradient.template segment<3>(3) = (c.cross(-n2)*nxn_n-c.dot(nxn)*n2.cross(nxn)/nxn_n)/std::pow(nxn_n,2);
} else {
}
else {
gradient.template head<3>() = nxn/nxn_n;
gradient.template segment<3>(3) = (-c.cross(-n2)*nxn_n+c.dot(nxn)*n2.cross(nxn)/nxn_n)/std::pow(nxn_n,2);
}
};
void calculateGradientSecondComplete(Vector& line1, Vector& line2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& line1,
const E::MatrixBase<DerivedB>& line2,
E::MatrixBase<DerivedC>& gradient) {
if(nxn_n == 0) {
gradient.head(3).setOnes();
return;
@ -393,7 +452,8 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
if(cdn >= 0) {
gradient.template head<3>() = nxn/nxn_n;
gradient.template segment<3>(3) = (c.cross(n1)*nxn_n-c.dot(nxn)*((-n1).cross(nxn))/nxn_n)/std::pow(nxn_n,2);
} else {
}
else {
gradient.template head<3>() = -nxn/nxn_n;
gradient.template segment<3>(3) = (-c.cross(n1)*nxn_n+c.dot(nxn)*((-n1).cross(nxn))/nxn_n)/std::pow(nxn_n,2);
}
@ -410,10 +470,14 @@ struct Distance::type< Kernel, tag::line3D, tag::plane3D > : public Distance::ty
};
#endif
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p1,p2,grad);
g.segment(3,3).setZero();
g.segment(3,3).setZero();
};
};
@ -429,13 +493,17 @@ struct Distance::type< Kernel, tag::line3D, tag::cylinder3D > : public Distance:
};
#endif
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
//(p1-p2)°n / |n| - distance
const Scalar res = Distance::type< Kernel, tag::line3D, tag::line3D >::calculate(param1, param2);
return res - param2(6);
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};
@ -451,8 +519,12 @@ struct Distance::type< Kernel, tag::plane3D, tag::plane3D > : public Distance::t
};
#endif
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p1,p2,grad);
g.segment(3,3).setZero();
};
@ -471,26 +543,39 @@ struct Distance::type< Kernel, tag::plane3D, tag::cylinder3D > : public Distance
};
#endif
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
//(p1-p2)°n / |n| - distance
const Scalar res = Distance::type< Kernel, tag::point3D, tag::plane3D >::calculate(param2, param1);
return res - param2(6);
};
Scalar calculateGradientFirst(Vector& p1, Vector& p2, Vector& dp1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
const E::MatrixBase<DerivedC>& dp1) {
return Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientSecond(p2,p1,dp1);
};
Scalar calculateGradientSecond(Vector& p1, Vector& p2, Vector& dp2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
const E::MatrixBase<DerivedC>& dp2) {
return Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirst(p2,p1,dp2);
};
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientSecondComplete(p2,p1,g);
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
typename Kernel::VectorMap grad(&g(0), 3, typename Kernel::DynStride(1,1));
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p2,p1,grad);
g.segment(3,3).setZero();
g(6) = -1;
@ -509,18 +594,25 @@ struct Distance::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Dista
};
#endif
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
//(p1-p2)°n / |n| - distance
const Scalar res = Distance::type< Kernel, tag::line3D, tag::line3D >::calculate(param1, param2);
return res - param1(6) - param2(6);
};
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientFirstComplete(p1,p2,g);
g(6) = -1;
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& p1,
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};

4
src/Mod/Assembly/App/opendcm/module3d/dof.hpp
View File

@ -69,7 +69,7 @@ public:
tp1 = std::make_pair(v, constraint);
} else if(m_translation == plane) {
if(K::isSame(tp1.first, v) || K::isOpposite(tp1.first, v)) {
if(K::isSame(tp1.first, v, 1e-6) || K::isOpposite(tp1.first, v, 1e-6)) {
ConstraintVector cv;
cv.push_back(tp1.second);
return std::make_pair(false,cv);
@ -107,7 +107,7 @@ public:
return std::make_pair(false, ConstraintVector()); //error as every function call removes 2 dof's
} else if(m_rotation == line) {
if(K::isSame(rp1.first, v) || K::isOpposite(rp1.first, v)) {
if(K::isSame(rp1.first, v, 1e-6) || K::isOpposite(rp1.first, v, 1e-6)) {
ConstraintVector cv;
cv.push_back(rp1.second);
return std::make_pair(false, cv);

4
src/Mod/Assembly/App/opendcm/module3d/module.hpp
View File

@ -171,7 +171,7 @@ struct Module3D {
void recalculated();
void removed();
friend class Constraint3D;
friend struct Constraint3D;
};
template<typename Derived>
@ -213,7 +213,7 @@ struct Module3D {
friend struct details::ClusterMath<Sys>::map_downstream;
friend struct details::SystemSolver<Sys>;
friend struct details::SystemSolver<Sys>::Rescaler;
friend class inheriter_base;
friend struct inheriter_base;
public:
//the geometry class itself does not hold an aligned eigen object, but maybe the variant

271
src/Mod/Assembly/App/opendcm/module3d/parallel.hpp
View File

@ -32,128 +32,130 @@ namespace dcm {
//the calculations( same as we always calculate directions we can outsource the work to this functions)
namespace orientation_detail {
template<typename Kernel, typename T>
inline typename Kernel::number_type calc(const T& d1,
const T& d2,
template<typename Kernel, typename T1, typename T2>
inline typename Kernel::number_type calc(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const Direction& dir) {
switch(dir) {
case parallel:
if(d1.dot(d2) < 0)
return (d1+d2).norm();
case equal:
return (d1-d2).norm();
case opposite:
case parallel:
if(d1.dot(d2) < 0)
return (d1+d2).norm();
case perpendicular:
return d1.dot(d2);
default:
assert(false);
case equal:
return (d1-d2).norm();
case opposite:
return (d1+d2).norm();
case perpendicular:
return d1.dot(d2);
default:
assert(false);
}
return 0;
};
template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradFirst(const T& d1,
const T& d2,
const T& dd1,
template<typename Kernel, typename T1, typename T2, typename T3>
inline typename Kernel::number_type calcGradFirst(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& dd1,
const Direction& dir) {
typename Kernel::number_type res;
switch(dir) {
case parallel:
if(d1.dot(d2) < 0) {
res= ((d1+d2).dot(dd1) / (d1+d2).norm());
break;
}
case equal:
res = ((d1-d2).dot(dd1) / (d1-d2).norm());
break;
case opposite:
case parallel:
if(d1.dot(d2) < 0) {
res= ((d1+d2).dot(dd1) / (d1+d2).norm());
break;
case perpendicular:
res = dd1.dot(d2);
break;
}
case equal:
res = ((d1-d2).dot(dd1) / (d1-d2).norm());
break;
case opposite:
res= ((d1+d2).dot(dd1) / (d1+d2).norm());
break;
case perpendicular:
res = dd1.dot(d2);
break;
}
if(isfinite(res)) return res;
if(isfinite(res))
return res;
return 0;
};
template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradSecond(const T& d1,
const T& d2,
const T& dd2,
template<typename Kernel, typename T1, typename T2, typename T3>
inline typename Kernel::number_type calcGradSecond(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& dd2,
const Direction& dir) {
typename Kernel::number_type res;
switch(dir) {
case parallel:
if(d1.dot(d2) < 0) {
res = ((d1+d2).dot(dd2) / (d1+d2).norm());
break;
}
case equal:
res = ((d1-d2).dot(-dd2) / (d1-d2).norm());
break;
case opposite:
case parallel:
if(d1.dot(d2) < 0) {
res = ((d1+d2).dot(dd2) / (d1+d2).norm());
break;
case perpendicular:
res = d1.dot(dd2);
break;
}
case equal:
res = ((d1-d2).dot(-dd2) / (d1-d2).norm());
break;
case opposite:
res = ((d1+d2).dot(dd2) / (d1+d2).norm());
break;
case perpendicular:
res = d1.dot(dd2);
break;
}
if((isfinite)(res)) return res;
if((isfinite)(res))
return res;
return 0;
};
template<typename Kernel, typename T>
inline void calcGradFirstComp(const T& d1,
const T& d2,
const T& grad,
template<typename Kernel, typename T1, typename T2, typename T3>
inline void calcGradFirstComp(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& grad,
const Direction& dir) {
switch(dir) {
case parallel:
if(d1.dot(d2) < 0) {
const_cast< T& >(grad) = (d1+d2) / (d1+d2).norm();
return;
}
case equal:
const_cast< T& >(grad) = (d1-d2) / (d1-d2).norm();
return;
case opposite:
const_cast< T& >(grad) = (d1+d2) / (d1+d2).norm();
return;
case perpendicular:
const_cast< T& >(grad) = d2;
case parallel:
if(d1.dot(d2) < 0) {
const_cast< E::MatrixBase<T3>& >(grad) = (d1+d2) / (d1+d2).norm();
return;
}
case equal:
const_cast< E::MatrixBase<T3>& >(grad) = (d1-d2) / (d1-d2).norm();
return;
case opposite:
const_cast< E::MatrixBase<T3>& >(grad) = (d1+d2) / (d1+d2).norm();
return;
case perpendicular:
const_cast< E::MatrixBase<T3>& >(grad) = d2;
return;
}
};
template<typename Kernel, typename T>
inline void calcGradSecondComp(const T& d1,
const T& d2,
const T& grad,
template<typename Kernel, typename T1, typename T2, typename T3>
inline void calcGradSecondComp(const E::MatrixBase<T1>& d1,
const E::MatrixBase<T2>& d2,
const E::MatrixBase<T3>& grad,
const Direction& dir) {
switch(dir) {
case parallel:
if(d1.dot(d2) < 0) {
const_cast< T& >(grad) = (d2+d1) / (d1+d2).norm();
return;
}
case equal:
const_cast< T& >(grad) = (d2-d1) / (d1-d2).norm();
return;
case opposite:
const_cast< T& >(grad) = (d2+d1) / (d1+d2).norm();
return;
case perpendicular:
const_cast< T& >(grad) = d1;
case parallel:
if(d1.dot(d2) < 0) {
const_cast< E::MatrixBase<T3>& >(grad) = (d2+d1) / (d1+d2).norm();
return;
}
case equal:
const_cast< E::MatrixBase<T3>& >(grad) = (d2-d1) / (d1-d2).norm();
return;
case opposite:
const_cast< E::MatrixBase<T3>& >(grad) = (d2+d1) / (d1+d2).norm();
return;
case perpendicular:
const_cast< E::MatrixBase<T3>& >(grad) = d1;
return;
}
};
@ -168,20 +170,33 @@ struct Orientation::type< Kernel, tag::direction3D, tag::direction3D > : public
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return orientation_detail::calc<Kernel>(param1, param2, value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return orientation_detail::calcGradFirst<Kernel>(param1, param2, dparam1, value);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return orientation_detail::calcGradSecond<Kernel>(param1, param2, dparam2, value);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradFirstComp<Kernel>(param1, param2, gradient, value);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1, param2, gradient, value);
};
@ -196,31 +211,44 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return orientation_detail::calc<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return orientation_detail::calcGradFirst<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam1.template segment<3>(3),
value);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return orientation_detail::calcGradSecond<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam2.template segment<3>(3),
value);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
value);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
@ -246,31 +274,44 @@ struct Orientation::type< Kernel, tag::line3D, tag::plane3D > : public Orientati
};
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return orientation_detail::calc<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
getValue());
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return orientation_detail::calcGradFirst<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam1.template segment<3>(3),
getValue());
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return orientation_detail::calcGradSecond<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam2.template segment<3>(3),
getValue());
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
getValue());
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
@ -285,7 +326,10 @@ struct Orientation::type< Kernel, tag::line3D, tag::cylinder3D > : public Orient
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Orientation::type<Kernel, tag::line3D, tag::line3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};
@ -303,19 +347,32 @@ struct Orientation::type< Kernel, tag::plane3D, tag::cylinder3D > : public Orien
using Orientation::type<Kernel, tag::line3D, tag::plane3D>::value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculate(param1, param2);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientFirst(param1, param2, dparam1);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam2) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientSecond(param1, param2, dparam2);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientFirstComplete(param1, param2, gradient);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};
@ -326,7 +383,10 @@ struct Orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public O
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
@ -334,7 +394,10 @@ struct Orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public O
Orientation::type< Kernel, tag::line3D, tag::line3D >::value);
gradient(6) = 0;
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),

12
src/Mod/Assembly/App/opendcm/module3d/solver.hpp
View File

@ -209,7 +209,7 @@ typename SystemSolver<Sys>::Scalar SystemSolver<Sys>::Rescaler::scaleClusters()
sc = (s>sc) ? s : sc;
}
//if no scaling-value returned we can use 1
sc = (Kernel::isSame(sc,0)) ? 1. : sc;
sc = (Kernel::isSame(sc,0, 1e-10)) ? 1. : sc;
typedef typename boost::graph_traits<Cluster>::vertex_iterator iter;
std::pair<iter, iter> it = boost::vertices(*cluster);
@ -241,7 +241,7 @@ void SystemSolver<Sys>::Rescaler::collectPseudoPoints(
std::pair<e_iter, e_iter> it = boost::out_edges(cluster, *parent);
for(; it.first != it.second; it.first++) {
std::pair< c_iter, c_iter > cit = parent->template getGlobalEdges(*it.first);
std::pair< c_iter, c_iter > cit = parent->getGlobalEdges(*it.first);
for(; cit.first != cit.second; cit.first++) {
Cons c = parent->template getObject<Constraint3D>(*cit.first);
@ -390,7 +390,7 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
BOOST_LOG(log)<< "No rotation parameters in system, solve without scaling";
#endif
DummyScaler re;
Kernel::solve(mes, re);
sys.kernel().solve(mes, re);
}
else {
@ -422,7 +422,7 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
//solve can be done without catching exceptions, because this only fails if the system in
//unsolvable
DummyScaler re;
Kernel::solve(mes, re);
sys.kernel().solve(mes, re);
//now let's see if we have to go on with the translations
mes.setAccess(general);
@ -434,7 +434,7 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
//let's try translation only
try {
DummyScaler re;
Kernel::solve(mes, re);
sys.kernel().solve(mes, re);
done=true;
}
catch(boost::exception&) {
@ -451,7 +451,7 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
#endif
Rescaler re(cluster, mes);
re();
Kernel::solve(mes, re);
sys.kernel().solve(mes, re);
#ifdef USE_LOGGING
BOOST_LOG(log)<< "Numbers of rescale: "<<re.rescales;
#endif

6
src/Mod/Assembly/App/opendcm/modulePart/module.hpp
View File

@ -309,7 +309,7 @@ ModulePart<Typelist, ID>::type<Sys>::Part_base::addGeometry3D(const T& geom, Coo
Geom g(new Geometry3D(geom, *m_system));
if(frame == Local) {
//we need to collect all transforms up to this part!
Transform t;//(m_transform);
Transform t;
transform_traverse(t, m_cluster);
g->transform(t);
@ -325,8 +325,8 @@ ModulePart<Typelist, ID>::type<Sys>::Part_base::addGeometry3D(const T& geom, Coo
template<typename Typelist, typename ID>
template<typename Sys>
void ModulePart<Typelist, ID>::type<Sys>::Part_base::transform_traverse(ModulePart<Typelist, ID>::type<Sys>::Part_base::Transform& t,
boost::shared_ptr<ModulePart<Typelist, ID>::type<Sys>::Part_base::Cluster> c) {
void ModulePart<Typelist, ID>::type<Sys>::Part_base::transform_traverse(typename ModulePart<Typelist, ID>::type<Sys>::Part_base::Transform& t,
boost::shared_ptr<typename ModulePart<Typelist, ID>::type<Sys>::Part_base::Cluster> c) {
t *= c->template getProperty<typename Part_base::module3d::math_prop>().m_transform;