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

new dcm version

Browse Source
This commit is contained in:
Stefan Tröger 2013-10-18 19:02:54 +00:00
parent 5376e3a3e5
commit 3abda4dd67
9 changed files with 290 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -447,7 +447,7 @@ Constraint<Sys, Dim>::holder<ConstraintVector, EquationVector>::OptionSetter::op
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;
fusion::copy(fusion::at<distance>(objects).values, val.m_eq.values);
val.pure_rotation = fusion::at<distance>(objects).pure_rotation;
};

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

@ -34,6 +34,10 @@
#include <boost/fusion/include/advance.hpp>
#include <boost/fusion/include/back.hpp>
#include <boost/fusion/include/iterator_range.hpp>
#include <boost/fusion/include/nview.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/as_map.hpp>
#include <boost/exception/exception.hpp>
@ -44,12 +48,18 @@ namespace mpl = boost::mpl;
namespace dcm {
//the possible directions
enum Direction { parallel, equal, opposite, perpendicular };
//the possible solution spaces
enum SolutionSpace {unidirectional, positiv_directional, negative_directional};
struct no_option {};
template<typename Kernel>
struct Pseudo {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
template <typename DerivedA,typename DerivedB>
void calculatePseudo(const E::MatrixBase<DerivedA>& param1, Vec& v1, const E::MatrixBase<DerivedB>& param2, Vec& v2) {};
};
@ -62,7 +72,7 @@ struct Scale {
template<typename Kernel>
struct PseudoScale {
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
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) {};
@ -83,8 +93,8 @@ struct constraint_sequence : public seq {
typedef typename pushed_seq<seq, T>::type Sequence;
typedef typename fusion::result_of::begin<Sequence>::type Begin;
typedef typename fusion::result_of::end<Sequence>::type End;
typedef typename fusion::result_of::prior<End>::type EndOld;
typedef typename fusion::result_of::find<Sequence, typename fusion::result_of::back<typename pushed_seq<seq, T>::S1>::type >::type EndOld;
//create the new sequence
Sequence vec;
@ -97,7 +107,7 @@ struct constraint_sequence : public seq {
fusion::copy(*this, range);
//insert this object at the end of the sequence
fusion::back(vec) = val;
*fusion::find<T>(vec) = val;
//and return our new extendet sequence
return vec;
@ -114,27 +124,31 @@ struct constraint_sequence : public seq {
typedef typename pushed_seq<T, seq>::type Sequence;
typedef typename fusion::result_of::begin<Sequence>::type Begin;
typedef typename fusion::result_of::end<Sequence>::type End;
typedef typename fusion::result_of::find<Sequence, typename fusion::result_of::back<typename pushed_seq<T, seq>::S1>::type >::type EndF;
typedef typename mpl::distance< typename mpl::begin<T>::type, typename mpl::end<T>::type >::type distanceF;
typedef typename fusion::result_of::advance<Begin, distanceF>::type EndF;
//create the new sequence
Sequence vec;
//copy the given values into the new sequence
Begin b(vec);
EndF ef(vec);
fusion::iterator_range<Begin, EndF> range(b, ef);
fusion::copy(val, range);
//copy the objects value into the new sequence
EndF bb(vec);
End e(vec);
fusion::iterator_range<EndF, End> range2(bb, e);
fusion::copy(*this, range2);
//to copy the types of the second sequence is not as easy as before. If types were already present in
//the original sequence they are not added again. therefore we need to find all types of the second sequence
//in the new one and assign the objects to this positions.
//get a index vector for all second-sequence-elements
typedef typename mpl::transform<typename pushed_seq<T, seq>::S2,
fusion::result_of::distance<typename fusion::result_of::begin<Sequence>::type,
fusion::result_of::find<Sequence, mpl::_1> > >::type position_vector;
//and copy the types in
fusion::nview<Sequence, position_vector> view(vec);
fusion::copy(*this, view);
//and return our new extendet sequence
return vec;
@ -145,45 +159,82 @@ template<typename Seq, typename T>
struct pushed_seq {
typedef typename mpl::if_<mpl::is_sequence<Seq>, Seq, fusion::vector1<Seq> >::type S1;
typedef typename mpl::if_<mpl::is_sequence<T>, T, fusion::vector1<T> >::type S2;
typedef typename fusion::result_of::as_vector<typename mpl::fold< S2, S1, mpl::push_back<mpl::_1,mpl::_2> >::type >::type vec;
typedef typename mpl::fold< S2, S1, mpl::if_< boost::is_same<
mpl::find<mpl::_1, mpl::_2>, mpl::end<mpl::_1> >, mpl::push_back<mpl::_1,mpl::_2>, mpl::_1> >::type unique_vector;
typedef typename fusion::result_of::as_vector< unique_vector >::type vec;
typedef constraint_sequence<vec> type;
};
template<typename Derived, typename Option, bool rotation_only = false>
struct Equation : public EQ {
typedef Option option_type;
option_type value;
typedef typename mpl::if_<mpl::is_sequence<Option>, Option, mpl::vector<Option> >::type option_sequence;
typedef typename mpl::fold<option_sequence, fusion::map<>, fusion::result_of::push_back<mpl::_1, fusion::pair<mpl::_2, std::pair<bool, mpl::_2> > > > ::type option_set_map;
typedef typename fusion::result_of::as_map<option_set_map>::type options;
options values;
bool pure_rotation;
Equation(option_type val = option_type()) : value(val), pure_rotation(rotation_only) {};
struct option_copy {
Derived& operator()(const option_type val) {
value = val;
options* values;
option_copy(options& op) : values(&op) {};
template<typename T>
void operator()(const T& val) const {
if(val.second.first)
fusion::at_key<typename T::first_type>(*values) = val.second;
};
};
Equation() : pure_rotation(rotation_only) {};
//assign option
template<typename T>
typename boost::enable_if<fusion::result_of::has_key<options, T>, Derived&>::type operator()(const T& val) {
fusion::at_key<T>(values).second = val;
fusion::at_key<T>(values).first = true;
return *(static_cast<Derived*>(this));
};
Derived& operator=(const option_type val) {
//assign option
template<typename T>
typename boost::enable_if<fusion::result_of::has_key<options, T>, Derived&>::type operator=(const T& val) {
return operator()(val);
};
//assign complete equation
template<typename T>
typename boost::enable_if<boost::is_base_of<EQ, T>, Derived& >::type
operator=(T& eq) {
//we only copy the values which were set and are therefore valid
option_copy oc(values);
fusion::for_each(eq.values, oc);
//the assigned eqution can be set back to default for convinience in further usage
eq.setDefault();
return *static_cast<Derived*>(this);
};
//an equation gets added to this equation
template<typename T>
typename boost::enable_if< boost::is_base_of< dcm::EQ, T>, typename pushed_seq<T, Derived>::type >::type operator &(T val) {
typename boost::enable_if< boost::is_base_of< dcm::EQ, T>, typename pushed_seq<T, Derived>::type >::type operator &(const T& val) {
typename pushed_seq<T, Derived>::type vec;
fusion::at_c<0>(vec) = val;
fusion::at_c<1>(vec) = *(static_cast<Derived*>(this));
*fusion::find<T>(vec) = val;
*fusion::find<Derived>(vec) = *(static_cast<Derived*>(this));
return vec;
};
//an sequence gets added to this equation (happens only if sequenced equations like coincident are used)
template<typename T>
typename boost::enable_if< mpl::is_sequence<T>, typename pushed_seq<T, Derived>::type >::type operator &(T val) {
typename boost::enable_if< mpl::is_sequence<T>, typename pushed_seq<T, Derived>::type >::type operator &(const T& val) {
typedef typename pushed_seq<T, Derived>::type Sequence;
typedef typename fusion::result_of::begin<Sequence>::type Begin;
typedef typename fusion::result_of::end<Sequence>::type End;
typedef typename fusion::result_of::prior<End>::type EndOld;
typedef typename fusion::result_of::find<Sequence, typename fusion::result_of::back<typename pushed_seq<T, Derived>::S1>::type >::type EndOld;
//create the new sequence
Sequence vec;
@ -196,17 +247,28 @@ struct Equation : public EQ {
fusion::copy(val, range);
//insert this object at the end of the sequence
fusion::back(vec) = *static_cast<Derived*>(this);
*fusion::find<Derived>(vec) = *static_cast<Derived*>(this);
//and return our new extendet sequence
return vec;
};
//set default option values, neeeded for repedability and to prevent unexpected behaviour
virtual void setDefault() = 0;
};
struct Distance : public Equation<Distance, double> {
struct Distance : public Equation<Distance, mpl::vector2<double, SolutionSpace> > {
using Equation::operator=;
Distance() : Equation(0) {};
using Equation::options;
Distance() : Equation() {
setDefault();
};
void setDefault() {
fusion::at_key<double>(values) = std::make_pair(false, 0.);
fusion::at_key<SolutionSpace>(values) = std::make_pair(false, unidirectional);
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type {
@ -220,7 +282,7 @@ struct Distance : public Equation<Distance, double> {
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value;
options values;
//template definition
template <typename DerivedA,typename DerivedB>
void calculatePseudo(const E::MatrixBase<DerivedA>& param1, Vec& v1, const E::MatrixBase<DerivedB>& param2, Vec& v2) {
@ -263,13 +325,17 @@ struct Distance : public Equation<Distance, double> {
};
};
//the possible directions
enum Direction { parallel=0, equal, opposite, perpendicular };
struct Orientation : public Equation<Orientation, Direction, true> {
using Equation::operator=;
Orientation() : Equation(parallel) {};
using Equation::options;
Orientation() : Equation() {
setDefault();
};
void setDefault() {
fusion::at_key<Direction>(values) = std::make_pair(false, parallel);
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -282,7 +348,7 @@ struct Orientation : public Equation<Orientation, Direction, true> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
options values;
//template definition
template <typename DerivedA,typename DerivedB>
@ -290,27 +356,27 @@ struct Orientation : public Equation<Orientation, Direction, true> {
assert(false);
return 0;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
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;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
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;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
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);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
@ -319,10 +385,17 @@ struct Orientation : public Equation<Orientation, Direction, true> {
};
};
struct Angle : public Equation<Angle, double, true> {
struct Angle : public Equation<Angle, mpl::vector2<double, SolutionSpace>, true> {
using Equation::operator=;
Angle() : Equation(0) {};
Angle() : Equation() {
setDefault();
};
void setDefault() {
fusion::at_key<double>(values) = std::make_pair(false, 0.);
fusion::at_key<SolutionSpace>(values) = std::make_pair(false, unidirectional);
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -335,35 +408,35 @@ struct Angle : public Equation<Angle, double, true> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
options values;
//template definition
template <typename DerivedA,typename DerivedB>
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
return 0;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
assert(false);
return 0;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
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;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
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);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {

16
src/Mod/Assembly/App/opendcm/module3d/alignment.hpp
View File

@ -28,29 +28,29 @@ namespace dcm {
struct Alignment : public dcm::constraint_sequence< fusion::vector2< Distance, Orientation > > {
//allow to set the distance
Alignment& operator()(Direction val) {
fusion::at_c<1>(*this) = val;
// fusion::at_c<1>(*this) = val;
return *this;
};
Alignment& operator()(double val) {
fusion::at_c<0>(*this) = val;
//fusion::at_c<0>(*this) = val;
return *this;
};
Alignment& operator()(double val1, Direction val2) {
fusion::at_c<0>(*this) = val1;
fusion::at_c<1>(*this) = val2;
// fusion::at_c<0>(*this) = val1;
// fusion::at_c<1>(*this) = val2;
return *this;
};
Alignment& operator()(Direction val1, double val2) {
fusion::at_c<0>(*this) = val2;
fusion::at_c<1>(*this) = val1;
// fusion::at_c<0>(*this) = val2;
// fusion::at_c<1>(*this) = val1;
return *this;
};
Alignment& operator=(Direction val) {
fusion::at_c<1>(*this) = val;
// fusion::at_c<1>(*this) = val;
return *this;
};
Alignment& operator=(double val) {
fusion::at_c<0>(*this) = val;
// fusion::at_c<0>(*this) = val;
return *this;
};
};

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

@ -82,12 +82,12 @@ struct Angle::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
typename Angle::options values;
//template definition
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);
return angle_detail::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), fusion::at_key<double>(values).second);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,

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

@ -31,8 +31,14 @@ namespace details {
struct ci_orientation : public Equation<ci_orientation, Direction, true> {
using Equation::operator=;
ci_orientation() : Equation(parallel) {};
using Equation::options;
ci_orientation() : Equation() {
setDefault();
};
void setDefault() {
fusion::at_key<Direction>(values) = std::make_pair(false, parallel);
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -46,7 +52,7 @@ struct ci_orientation : public Equation<ci_orientation, Direction, true> {
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
option_type value;
typename ci_orientation::options values;
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);
@ -87,7 +93,7 @@ struct ci_orientation::type< Kernel, tag::point3D, tag::point3D > : public dcm::
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
typename ci_orientation::options values;
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
return 0;
@ -145,11 +151,18 @@ struct ci_orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public
//we need a custom distance type to use point-distance functions instead of real geometry distance
struct ci_distance : public Equation<ci_distance, double> {
struct ci_distance : public Equation<ci_distance, mpl::vector2<double, SolutionSpace> > {
using Equation::operator=;
ci_distance() : Equation(0) {};
using Equation::options;
ci_distance() : Equation() {
setDefault();
};
void setDefault() {
fusion::at_key<double>(values) = std::make_pair(false, 0.);
fusion::at_key<SolutionSpace>(values) = std::make_pair(false, unidirectional);
};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -163,7 +176,7 @@ struct ci_distance : public Equation<ci_distance, double> {
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
option_type value;
typename ci_distance::options values;
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
assert(false);

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

@ -32,12 +32,13 @@ struct Distance::type< Kernel, tag::point3D, tag::point3D > {
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value;
Scalar sc_value;
typename Distance::options values;
//template definition
void calculatePseudo(typename Kernel::Vector& param1, Vec& v1, typename Kernel::Vector& param2, Vec& v2) {};
void setScale(Scalar scale) {
sc_value = value*scale;
sc_value = fusion::at_key<double>(values).second*scale;
};
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& param1, const E::MatrixBase<DerivedB>& param2) {
@ -77,7 +78,8 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value;
Scalar sc_value;
typename Distance::options values;
Vector3 diff, n, dist;
#ifdef USE_LOGGING
@ -99,7 +101,7 @@ struct Distance::type< Kernel, tag::point3D, tag::line3D > {
v2.push_back(pp);
};
void setScale(Scalar scale) {
sc_value = value*scale;
sc_value = fusion::at_key<double>(values).second*scale;
};
template <typename DerivedA,typename DerivedB>
Scalar calculate(const E::MatrixBase<DerivedA>& point, const E::MatrixBase<DerivedB>& line) {
@ -190,7 +192,9 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
typedef typename Kernel::VectorMap Vector;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value;
Scalar sc_value, result;
SolutionSpace sspace;
typename Distance::options values;
#ifdef USE_LOGGING
src::logger log;
@ -215,17 +219,27 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
#endif
};
void setScale(Scalar scale) {
sc_value = value*scale;
sc_value = fusion::at_key<double>(values).second*scale;
sspace = fusion::at_key<SolutionSpace>(values).second;
};
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;
result = (param1.head(3)-param2.head(3)).dot(param2.tail(3)) / param2.tail(3).norm();
if(sspace == unidirectional)
return std::abs(result) - sc_value;
if(sspace==positiv_directional)
return result - sc_value;
if(sspace ==negative_directional)
return result + sc_value;
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
BOOST_LOG(log) << "Unnormal residual detected: " << result;
#endif
return res;
return result;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
@ -239,6 +253,13 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<res;
#endif
//r = sqrt(x^2) = (x^2)^(1/2)
//r' = 1/2(x^2)^(-1/2) * (x^2)'
//r' = 1/sqrt(x^2) * x * x'
//r' = sign(x)*x'
if(sspace == unidirectional && result<0.)
return -res;
return res;
};
@ -258,6 +279,9 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal second cluster gradient detected: "<<res;
#endif
if(sspace == unidirectional && result<0.)
return -res;
return res;
};
@ -266,6 +290,9 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
gradient = param2.tail(3) / param2.tail(3).norm();
if(sspace == unidirectional && result<0.)
gradient *= -1.;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
@ -277,6 +304,9 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
gradient.head(3) = -n / n.norm();
gradient.tail(3) = (p1m2)/n.norm() - (p1m2).dot(n)*n/std::pow(n.norm(),3);
if(sspace == unidirectional && result<0.)
gradient *= -1.;
};
};
@ -286,16 +316,68 @@ struct Distance::type< Kernel, tag::point3D, tag::cylinder3D > : public Distance
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
Scalar result;
SolutionSpace sspace;
using Distance::template type<Kernel, tag::point3D, tag::line3D>::values;
#ifdef USE_LOGGING
type() {
Distance::type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance point3D cylinder3D");
Distance::template type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance point3D cylinder3D");
};
#endif
void setScale(Scalar scale) {
Distance::template type<Kernel, tag::point3D, tag::line3D>::setScale(scale);
sspace = fusion::at_key<SolutionSpace>(values).second;
};
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);
result = Distance::type< Kernel, tag::point3D, tag::line3D >::calculate(param1, param2);
if(sspace == unidirectional)
return std::abs(result) - param2(6);
if(sspace==positiv_directional)
return result - param2(6);
return result + param2(6);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
const E::MatrixBase<DerivedC>& dparam1) {
const Scalar res = Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientFirst(param1,param2,dparam1);
if(sspace == unidirectional && result<0.)
return -res;
return res;
};
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 Scalar res = Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientSecond(param1,param2,dparam2);
if(sspace == unidirectional && result<0.)
return -res;
return res;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
const E::MatrixBase<DerivedB>& param2,
E::MatrixBase<DerivedC>& gradient) {
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientFirstComplete(param1,param2,gradient);
if(sspace == unidirectional && result<0.)
gradient *= -1;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
@ -303,7 +385,13 @@ struct Distance::type< Kernel, tag::point3D, tag::cylinder3D > : public Distance
const E::MatrixBase<DerivedB>& p2,
E::MatrixBase<DerivedC>& g) {
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
if(sspace == negative_directional)
g(6) = 1;
else
g(6) = -1;
if(sspace == unidirectional && result<0.)
g *= -1;
};
};
//TODO: this won't work for parallel lines. switch to point-line distance when lines are parallel
@ -315,7 +403,8 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value, cdn, nxn_n;
Scalar sc_value, cdn, nxn_n;
typename Distance::options values;
Vector3 c, n1, n2, nxn;
//if the lines are parallel we need to fall back to point-line distance
@ -360,8 +449,8 @@ struct Distance::type< Kernel, tag::line3D, tag::line3D > {
};
void setScale(Scalar scale) {
sc_value = value*scale;
pl_eqn.value = value;
sc_value = fusion::at_key<double>(values).second*scale;
fusion::copy(values, pl_eqn.values);
pl_eqn.setScale(scale);
};
template <typename DerivedA,typename DerivedB>
@ -637,3 +726,4 @@ struct Distance::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Dista
}//namespace dcm
#endif //GCM_DISTANCE3D_H

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

@ -167,38 +167,38 @@ struct Orientation::type< Kernel, tag::direction3D, tag::direction3D > : public
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
typename Orientation::options values;
//template definition
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);
return orientation_detail::calc<Kernel>(param1, param2, fusion::at_key<Direction>(values).second);
};
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);
return orientation_detail::calcGradFirst<Kernel>(param1, param2, dparam1, fusion::at_key<Direction>(values).second);
};
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);
return orientation_detail::calcGradSecond<Kernel>(param1, param2, dparam2, fusion::at_key<Direction>(values).second);
};
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);
orientation_detail::calcGradFirstComp<Kernel>(param1, param2, gradient, fusion::at_key<Direction>(values).second);
};
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);
orientation_detail::calcGradSecondComp<Kernel>(param1, param2, gradient, fusion::at_key<Direction>(values).second);
};
};
@ -208,14 +208,14 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
typename Orientation::options values;
//template definition
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);
fusion::at_key<Direction>(values).second);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientFirst(const E::MatrixBase<DerivedA>& param1,
@ -224,7 +224,7 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
return orientation_detail::calcGradFirst<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam1.template segment<3>(3),
value);
fusion::at_key<Direction>(values).second);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
Scalar calculateGradientSecond(const E::MatrixBase<DerivedA>& param1,
@ -233,7 +233,7 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
return orientation_detail::calcGradSecond<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
dparam2.template segment<3>(3),
value);
fusion::at_key<Direction>(values).second);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientFirstComplete(const E::MatrixBase<DerivedA>& param1,
@ -243,7 +243,7 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
orientation_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
value);
fusion::at_key<Direction>(values).second);
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
void calculateGradientSecondComplete(const E::MatrixBase<DerivedA>& param1,
@ -253,7 +253,7 @@ struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::Pseud
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
value);
fusion::at_key<Direction>(values).second);
};
};
@ -263,9 +263,10 @@ struct Orientation::type< Kernel, tag::line3D, tag::plane3D > : public Orientati
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
options values;
option_type getValue() {
dcm::Direction getValue() {
dcm::Direction value = fusion::at_key<Direction>(values).second;
if(value==parallel)
return perpendicular;
if(value==perpendicular)
@ -344,7 +345,7 @@ struct Orientation::type< Kernel, tag::plane3D, tag::cylinder3D > : public Orien
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
using Orientation::type<Kernel, tag::line3D, tag::plane3D>::value;
using Orientation::type<Kernel, tag::line3D, tag::plane3D>::values;
//template definition
template <typename DerivedA,typename DerivedB>
@ -391,7 +392,7 @@ struct Orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public O
orientation_detail::calcGradFirstComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
Orientation::type< Kernel, tag::line3D, tag::line3D >::value);
fusion::at_key<dcm::Direction>(Orientation::type< Kernel, tag::line3D, tag::line3D >::values).second);
gradient(6) = 0;
};
template <typename DerivedA,typename DerivedB, typename DerivedC>
@ -402,7 +403,7 @@ struct Orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public O
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
Orientation::type< Kernel, tag::line3D, tag::line3D >::value);
fusion::at_key<dcm::Direction>(Orientation::type< Kernel, tag::line3D, tag::line3D >::values).second);
gradient(6) = 0;
};
};

5
src/Mod/Assembly/App/opendcm/moduleShape3d/distance.hpp
View File

@ -36,7 +36,8 @@ struct Distance::type< Kernel, tag::point3D, tag::segment3D > {
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value, cross_n, cross_v12_n, v12_n;
Scalar sc_value, cross_n, cross_v12_n, v12_n;
typename Distance::options values;
Vector3 v01, v02, v12, cross;
#ifdef USE_LOGGING
@ -59,7 +60,7 @@ struct Distance::type< Kernel, tag::point3D, tag::segment3D > {
v2.push_back(pp);
};
void setScale(Scalar scale) {
sc_value = value*scale;
sc_value = fusion::at_key<double>(values).second*scale;
};
template <typename DerivedA,typename DerivedB>

15
src/Mod/Assembly/App/opendcm/moduleShape3d/fixed.hpp
View File

@ -28,13 +28,18 @@
namespace dcm {
namespace details {
//this equation is for internal use only. one needs to ensure the constraint is disabled after adding
//this fixed equation
struct Fixed : public Equation<Orientation, Direction, true> {
using Equation::operator=;
Fixed() : Equation(parallel) {};
using Equation::options;
Fixed() : Equation() {
setDefault();
};
void setDefault() {};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -42,7 +47,7 @@ struct Fixed : public Equation<Orientation, Direction, true> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
typename Fixed::options values;
//we shall not use this equation, warn the user about wrong usage
template <typename DerivedA,typename DerivedB>
@ -80,8 +85,8 @@ struct Fixed : public Equation<Orientation, Direction, true> {
};
static Fixed fixed;
}//details
} //dcm
#endif
#endif