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

add solver.hpp

Browse Source
This commit is contained in:
Stefan Tröger 2013-05-01 19:25:23 +02:00 committed by Stefan Tröger
parent b1d7dbe2f0
commit bf447d3a7c
7 changed files with 1021 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/Mod/Assembly/App/ConstraintGroup.cpp
View File

@ -66,7 +66,7 @@ void ConstraintGroup::addConstraint(Constraint* c)
//let's retrieve the solver if not already done
ItemAssembly* assembly = NULL;
if(!m_solver) {
if(!m_solver || !assembly) {
typedef std::vector<App::DocumentObject*>::iterator iter;
std::vector<App::DocumentObject*> vec = getInList();
@ -86,6 +86,10 @@ void ConstraintGroup::addConstraint(Constraint* c)
Base::Console().Message("ConstraintGroup: Unable to retrieve assembly solver\n");
return;
};
if(!assembly) {
Base::Console().Message("ConstraintGroup: Unable to retrieve assembly\n");
return;
};
//init the constraint
c->init(m_solver);

335
src/Mod/Assembly/App/Solver.h Normal file
View File

@ -0,0 +1,335 @@
/***************************************************************************
* Copyright (c) 2013 Stefan Tröger <stefantroeger@gmx.net> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef SOLVER_H
#define SOLVER_H
#include <Base/Placement.h>
#include <gp_Pnt.hxx>
#include <gp_Lin.hxx>
#include <gp_Pln.hxx>
#include <gp_Cylinder.hxx>
#include <Inventor/SbVec3f.h>
#include "opendcm/core.hpp"
#include "opendcm/module3d.hpp"
#include "opendcm/modulepart.hpp"
struct gp_pnt_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
switch(ID) {
case 0:
return t.X();
case 1:
return t.Y();
case 2:
return t.Z();
default:
return 0;
};
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
switch(ID) {
case 0:
t.SetX(value);
break;
case 1:
t.SetY(value);
break;
case 2:
t.SetZ(value);
break;
};
};
};
struct gp_lin_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
switch(ID) {
case 0:
return t.Location().X();
case 1:
return t.Location().Y();
case 2:
return t.Location().Z();
case 3:
return t.Direction().X();
case 4:
return t.Direction().Y();
case 5:
return t.Direction().Z();
default:
return 0;
};
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
gp_Pnt p = t.Location();
gp_Dir d = t.Direction();
switch(ID) {
case 0:
p.SetX(value);
break;
case 1:
p.SetY(value);
break;
case 2:
p.SetZ(value);
case 3:
d.SetX(value);
break;
case 4:
d.SetY(value);
break;
case 5:
d.SetZ(value);
break;
};
t.SetLocation(p);
t.SetDirection(d);
};
};
struct gp_pln_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
switch(ID) {
case 0:
return t.Axis().Location().X();
case 1:
return t.Axis().Location().Y();
case 2:
return t.Axis().Location().Z();
case 3:
return t.Axis().Direction().X();
case 4:
return t.Axis().Direction().Y();
case 5:
return t.Axis().Direction().Z();
default:
return 0;
};
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
gp_Pnt p = t.Axis().Location();
gp_Dir d = t.Axis().Direction();
switch(ID) {
case 0:
p.SetX(value);
break;
case 1:
p.SetY(value);
break;
case 2:
p.SetZ(value);
case 3:
d.SetX(value);
break;
case 4:
d.SetY(value);
break;
case 5:
d.SetZ(value);
break;
};
t.SetAxis(gp_Ax1(p,d));
};
};
struct gp_cylinder_accessor {
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
switch(ID) {
case 0:
return t.Axis().Location().X();
case 1:
return t.Axis().Location().Y();
case 2:
return t.Axis().Location().Z();
case 3:
return t.Axis().Direction().X();
case 4:
return t.Axis().Direction().Y();
case 5:
return t.Axis().Direction().Z();
case 6:
return t.Radius();
default:
return 0;
};
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
gp_Pnt p = t.Axis().Location();
gp_Dir d = t.Axis().Direction();
switch(ID) {
case 0:
p.SetX(value);
break;
case 1:
p.SetY(value);
break;
case 2:
p.SetZ(value);
case 3:
d.SetX(value);
break;
case 4:
d.SetY(value);
break;
case 5:
d.SetZ(value);
break;
case 6:
t.SetRadius(value);
};
t.SetAxis(gp_Ax1(p,d));
};
};
struct placement_accessor {
double q0, q1, q2, q3;
Base::Vector3d vec;
template<typename Scalar, int ID, typename T>
Scalar get(T& t) {
t.getRotation().getValue(q0,q1,q2,q3);
switch(ID) {
case 0:
return q3;
case 1:
return q0;
case 2:
return q1;
case 3:
return q2;
case 4:
return t.getPosition()[0];
case 5:
return t.getPosition()[1];
case 6:
return t.getPosition()[2];
default:
return 0;
};
};
template<typename Scalar, int ID, typename T>
void set(Scalar value, T& t) {
switch(ID) {
case 0:
q3 = value;
break;
case 1:
q0 = value;
break;
case 2:
q1 = value;
break;
case 3:
q2 = value;
break;
case 4:
vec[0] = value;
break;
case 5:
vec[1] = value;
break;
case 6:
vec[2] = value;
break;
};
};
template<typename T>
void finalize(T& t) {
//need to do it at once as setting every value step by step would always normalize the rotation and
//therefor give a false value
Base::Rotation rot(q0,q1,q2,q3);
t.setRotation(rot);
t.setPosition(vec);
};
};
//geometry_traits for opencascade
namespace dcm {
template<>
struct geometry_traits<gp_Pnt> {
typedef tag::point3D tag;
typedef modell::XYZ modell;
typedef gp_pnt_accessor accessor;
};
template<>
struct geometry_traits<gp_Lin> {
typedef tag::line3D tag;
typedef modell::XYZ2 modell;
typedef gp_lin_accessor accessor;
};
template<>
struct geometry_traits<gp_Pln> {
typedef tag::plane3D tag;
typedef modell::XYZ2 modell;
typedef gp_pln_accessor accessor;
};
template<>
struct geometry_traits<gp_Cylinder> {
typedef tag::cylinder3D tag;
typedef modell::XYZ2P modell;
typedef gp_cylinder_accessor accessor;
};
template<>
struct geometry_traits<Base::Placement> {
typedef tag::part tag;
typedef modell::quaternion_wxyz_vec3 modell;
typedef placement_accessor accessor;
};
template<>
struct geometry_traits<SbVec3f> {
typedef tag::point3D tag;
typedef modell::XYZ modell;
typedef orderd_bracket_accessor accessor;
};
}
//our constraint solving system
typedef dcm::Kernel<double> Kernel;
typedef dcm::Module3D< mpl::vector5< gp_Pnt, gp_Lin, gp_Pln, gp_Cylinder, SbVec3f>, std::string > Module3D;
typedef dcm::ModulePart< mpl::vector1< Base::Placement >, std::string > ModulePart;
typedef dcm::System<Kernel, Module3D, ModulePart> Solver;
typedef typename ModulePart::type<Solver>::Part Part3D;
typedef typename Module3D::type<Solver>::Geometry3D Geometry3D;
typedef typename Module3D::type<Solver>::Constraint3D Constraint3D;
#endif //SOLVER_H

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

@ -73,7 +73,7 @@ public:
protected:
template<typename ConstraintVector>
void initialize(typename fusion::result_of::as_vector<ConstraintVector>::type& obj);
void initialize(ConstraintVector& obj);
int equationCount();
@ -125,7 +125,7 @@ public:
mpl::push_back<mpl::_1, EquationSet<mpl::_2> > >::type eq_set_vector;
typedef typename fusion::result_of::as_vector<eq_set_vector>::type EquationSets;
typedef typename fusion::result_of::as_vector<ConstraintVector>::type Objects;
typedef ConstraintVector Objects;
template<typename T>
struct has_option {
@ -135,7 +135,7 @@ public:
typedef typename mpl::find<EquationVector, T>::type iterator;
typedef typename mpl::distance<typename mpl::begin<EquationVector>::type, iterator>::type distance;
BOOST_MPL_ASSERT((mpl::not_<boost::is_same<iterator, typename mpl::end<EquationVector>::type > >));
typedef typename mpl::at<ConstraintVector, distance>::type option_type;
typedef typename fusion::result_of::at<ConstraintVector, distance>::type option_type;
typedef mpl::not_<boost::is_same<option_type, no_option> > type;
};
@ -205,7 +205,7 @@ protected:
template< typename ConstraintVector >
struct creator : public boost::static_visitor<void> {
typedef typename fusion::result_of::as_vector<ConstraintVector>::type Objects;
typedef ConstraintVector Objects;
Objects& objects;
creator(Objects& obj);
@ -272,7 +272,7 @@ boost::shared_ptr<Derived> Constraint<Sys, Derived, Signals, MES, Geometry>::clo
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
template<typename ConstraintVector>
void Constraint<Sys, Derived, Signals, MES, Geometry>::initialize(typename fusion::result_of::as_vector<ConstraintVector>::type& obj) {
void Constraint<Sys, Derived, Signals, MES, Geometry>::initialize(ConstraintVector& obj) {
//first create the new placeholder
creator<ConstraintVector> c(obj);
@ -491,6 +491,7 @@ template<typename Sys, typename Derived, typename Signals, typename MES, typenam
template< typename ConstraintVector >
template<typename T1, typename T2>
void Constraint<Sys, Derived, Signals, MES, Geometry>::creator<ConstraintVector>::operator()(const T1&, const T2&) {
typedef tag_order< typename geometry_traits<T1>::tag, typename geometry_traits<T2>::tag > order;
//transform the constraints into eqautions with the now known types

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

@ -22,6 +22,20 @@
#include <assert.h>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/iterator_range.hpp>
#include <boost/fusion/include/copy.hpp>
namespace fusion = boost::fusion;
#include "kernel.hpp"
namespace dcm {
struct no_option {};
@ -44,17 +58,78 @@ struct PseudoScale {
void setScale(typename Kernel::number_type scale) {};
};
struct Distance {
//type to allow a metaprogramming check for a Equation
struct EQ {};
typedef double option_type;
template<typename Seq, typename T>
struct pushed_seq;
template<typename seq>
struct op_seq : public seq {
template<typename T>
typename boost::enable_if< boost::is_base_of< dcm::EQ, T>, typename pushed_seq<seq, T>::type >::type operator &(T val) {
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;
//create the new sequence
Sequence vec;
//copy the old values into the new sequence
Begin b(vec);
EndOld eo(vec);
fusion::iterator_range<Begin, EndOld> range(b, eo);
fusion::copy(*this, range);
//insert this object at the end of the sequence
fusion::back(vec) = val;
//and return our new extendet sequence
return vec;
};
};
template<typename Seq, typename T>
struct pushed_seq {
typedef typename boost::mpl::if_<boost::mpl::is_sequence<Seq>, Seq, fusion::vector1<Seq> >::type S;
typedef typename fusion::result_of::as_vector< typename boost::mpl::push_back<S, T>::type >::type vec;
typedef op_seq<vec> type;
};
template<typename Derived, typename Option>
struct Equation : public EQ {
typedef Option option_type;
option_type value;
Distance() : value(0) {};
Equation(option_type val = option_type()) : value(val) {};
Distance& operator=(const option_type val) {
Derived& operator()(const option_type val) {
value = val;
return *this;
return *(static_cast<Derived*>(this));
};
Derived& operator=(const option_type val) {
return operator()(val);
};
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 pushed_seq<T, Derived>::type vec;
fusion::at_c<0>(vec) = val;
fusion::at_c<1>(vec) = *(static_cast<Derived*>(this));
return vec;
};
};
struct Distance : public Equation<Distance, double> {
using Equation::operator=;
Distance() : Equation(0) {};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type {
@ -73,15 +148,15 @@ struct Distance {
};
Scalar calculate(Vector& param1, Vector& param2) {
assert(false);
return 0;
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
assert(false);
return 0;
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
assert(false);
return 0;
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
@ -93,19 +168,12 @@ struct Distance {
};
//the possible directions
enum Direction { Same, Opposite, Both };
enum Direction { equal, opposite, parallel, perpendicular };
struct Parallel {
struct Orientation : public Equation<Orientation, Direction> {
typedef Direction option_type;
option_type value;
Parallel() : value(Both) {};
Parallel& operator=(const option_type val) {
value = val;
return *this;
};
using Equation::operator=;
Orientation() : Equation(parallel) {};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -118,15 +186,15 @@ struct Parallel {
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
assert(false);
return 0;
return 0;
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
assert(false);
return 0;
return 0;
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
assert(false);
return 0;
return 0;
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
assert(false);
@ -137,17 +205,10 @@ struct Parallel {
};
};
struct Angle {
struct Angle : public Equation<Angle, double> {
typedef double option_type;
option_type value;
Angle() : value(0) {};
Angle& operator=(const option_type val) {
value = val;
return *this;
};
using Equation::operator=;
Angle() : Equation(0) {};
template< typename Kernel, typename Tag1, typename Tag2 >
struct type : public PseudoScale<Kernel> {
@ -180,7 +241,7 @@ struct Angle {
//every compiled file including this header would define these as global and the linker would find
//multiple definitions of the same objects
static Distance distance;
static Parallel parallel;
static Orientation orientation;
static Angle angle;
};

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

@ -56,6 +56,108 @@ struct Distance::type< Kernel, tag::point3D, tag::point3D > {
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::point3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value;
Vector3 diff, n, dist;
#ifdef USE_LOGGING
src::logger log;
attrs::mutable_constant< std::string > tag;
type() : tag("Distance point3D line3D") {
log.add_attribute("Tag", tag);
};
#endif
//template definition
void calculatePseudo(typename Kernel::Vector& point, Vec& v1, typename Kernel::Vector& line, Vec& v2) {
Vector3 pp = line.head(3) + (line.head(3)-point.head(3)).norm()*line.template segment<3>(3);
#ifdef USE_LOGGING
if(!boost::math::isnormal(pp.norm()))
BOOST_LOG(log) << "Unnormal pseudopoint detected";
#endif
v2.push_back(pp);
};
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& point, Vector& line) {
//diff = point1 - point2
n = line.template segment<3>(3);
diff = line.template head<3>() - point.template head<3>();
dist = diff - diff.dot(n)*n;
const Scalar res = dist.norm() - sc_value;
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
#endif
return res;
};
Scalar calculateGradientFirst(Vector& point, Vector& line, Vector& dpoint) {
if(dist.norm() == 0)
return 1.;
const Vector3 d_diff = -dpoint.template head<3>();
const Vector3 d_dist = d_diff - d_diff.dot(n)*n;
const Scalar res = dist.dot(d_dist)/dist.norm();
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<res
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
<<" and dpoint: "<<dpoint.transpose();
#endif
return res;
};
Scalar calculateGradientSecond(Vector& point, Vector& line, Vector& dline) {
if(dist.norm() == 0)
return 1.;
const Vector3 d_diff = dline.template head<3>();
const Vector3 d_n = dline.template segment<3>(3);
const Vector3 d_dist = d_diff - ((d_diff.dot(n)+diff.dot(d_n))*n + diff.dot(n)*d_n);
const Scalar res = dist.dot(d_dist)/dist.norm();
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal second cluster gradient detected: "<<res
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
<< "and dline: "<<dline.transpose();
#endif
return res;
};
void calculateGradientFirstComplete(Vector& point, Vector& line, Vector& gradient) {
if(dist.norm() == 0) {
gradient.head(3).setOnes();
return;
}
const Vector3 res = (n*n.transpose())*dist - dist;
gradient.head(3) = res/dist.norm();
};
void calculateGradientSecondComplete(Vector& point, Vector& line, Vector& gradient) {
if(dist.norm() == 0) {
gradient.head(6).setOnes();
return;
}
const Vector3 res = (-n*n.transpose())*dist + dist;
gradient.head(3) = res/dist.norm();
const Scalar mult = n.transpose()*dist;
gradient.template segment<3>(3) = -(mult*diff + diff.dot(n)*dist)/dist.norm();
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
typedef typename Kernel::number_type Scalar;
@ -139,7 +241,205 @@ struct Distance::type< Kernel, tag::point3D, tag::plane3D > {
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::point3D, tag::cylinder3D > : public Distance::type< Kernel, tag::point3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
#ifdef USE_LOGGING
type() {
Distance::type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance point3D cylinder3D");
};
#endif
Scalar calculate(Vector& param1, Vector& 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) {
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};
};
//TODO: this won't work for parallel lines. switch to point-line distance when lines are parallel
template<typename Kernel>
struct Distance::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value, cdn;
Vector3 c, n1, n2, nxn;
#ifdef USE_LOGGING
src::logger log;
attrs::mutable_constant< std::string > tag;
type() : tag("Distance line3D line3D") {
log.add_attribute("Tag", tag);
};
#endif
//template definition
void calculatePseudo(typename Kernel::Vector& line1, Vec& v1, typename Kernel::Vector& line2, Vec& v2) {
//add the line points of shortest distance as pseudopoints
const Vector3 c = line2.template head<3>() - line1.template head<3>();
const Vector3 n1 = line1.template segment<3>(3);
const Vector3 n2 = line2.template segment<3>(3);
const Vector3 nxn = n1.cross(n2);
const Vector3 r = c.cross(nxn)/nxn.squaredNorm();
Vector3 pp1 = line1.template head<3>() + r.dot(n2)*n1;
Vector3 pp2 = line2.template head<3>() + r.dot(n1)*n2;
#ifdef USE_LOGGING
if(!boost::math::isfinite(pp1.norm()) || !boost::math::isfinite(pp2.norm()))
BOOST_LOG(log) << "Unnormal pseudopoint detected";
#endif
v1.push_back(pp1);
v2.push_back(pp2);
};
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& line1, Vector& line2) {
//diff = point1 - point2
n1 = line1.template segment<3>(3);
n2 = line2.template segment<3>(3);
nxn = n1.cross(n2);
c = line2.template head<3>() - line1.template head<3>();
cdn = c.dot(nxn);
const Scalar res = std::abs(cdn) / nxn.norm();
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
#endif
return res;
};
Scalar calculateGradientFirst(Vector& line1, Vector& line2, Vector& dline1) {
if(nxn.norm() == 0)
return 1.;
const Vector3 nxn_diff = dline1.template segment<3>(3).cross(n2);
Scalar diff = (-dline1.template head<3>().dot(nxn)+c.dot(nxn_diff))*nxn.norm();
diff -= c.dot(nxn)*nxn.dot(nxn_diff)/nxn.norm();
//absoulute value requires diffrent differentation for diffrent results
if(cdn <= 0) diff *= -1;
diff /= std::pow(nxn.norm(),2);
#ifdef USE_LOGGING
if(!boost::math::isfinite(diff))
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<diff
<<" with line1: "<<line1.transpose()<<", line2: "<<line2.transpose()
<<" and dline1: "<<dline1.transpose();
#endif
return diff;
};
Scalar calculateGradientSecond(Vector& line1, Vector& line2, Vector& dline2) {
if(nxn.norm() == 0)
return 1.;
const Vector3 nxn_diff = n1.cross(dline2.template segment<3>(3));
Scalar diff = (dline2.template head<3>().dot(nxn)+c.dot(nxn_diff))*nxn.norm();
diff -= c.dot(nxn)*nxn.dot(nxn_diff)/nxn.norm();
//absoulute value requires diffrent differentation for diffrent results
if(cdn <= 0) diff *= -1;
diff /= std::pow(nxn.norm(),2);
#ifdef USE_LOGGING
if(!boost::math::isfinite(diff))
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<diff
<<" with line1: "<<line1.transpose()<<", line2: "<<line2.transpose()
<<" and dline2: "<<dline2.transpose();
#endif
return diff;
};
void calculateGradientFirstComplete(Vector& line1, Vector& line2, Vector& gradient) {
if(nxn.norm() == 0) {
gradient.head(3).setOnes();
return;
}
if(cdn >= 0) {
gradient.template head<3>() = -nxn/nxn.norm();
gradient.template segment<3>(3) = (c.cross(-n2)*nxn.norm()-c.dot(nxn)*n2.cross(nxn)/nxn.norm())/std::pow(nxn.norm(),2);
} else {
gradient.template head<3>() = nxn/nxn.norm();
gradient.template segment<3>(3) = (-c.cross(-n2)*nxn.norm()+c.dot(nxn)*n2.cross(nxn)/nxn.norm())/std::pow(nxn.norm(),2);
}
};
void calculateGradientSecondComplete(Vector& line1, Vector& line2, Vector& gradient) {
if(nxn.norm() == 0) {
gradient.head(3).setOnes();
return;
}
if(cdn >= 0) {
gradient.template head<3>() = nxn/nxn.norm();
gradient.template segment<3>(3) = (c.cross(n1)*nxn.norm()-c.dot(nxn)*((-n1).cross(nxn))/nxn.norm())/std::pow(nxn.norm(),2);
} else {
gradient.template head<3>() = -nxn/nxn.norm();
gradient.template segment<3>(3) = (-c.cross(n1)*nxn.norm()+c.dot(nxn)*((-n1).cross(nxn))/nxn.norm())/std::pow(nxn.norm(),2);
}
};
};
//only defined when line and plane are parallel, therefore it's the same as the point-plane distance
template<typename Kernel>
struct Distance::type< Kernel, tag::line3D, tag::plane3D > : public Distance::type< Kernel, tag::point3D, tag::plane3D > {
#ifdef USE_LOGGING
type() : Distance::type< Kernel, tag::point3D, tag::plane3D >() {
Distance::type< Kernel, tag::point3D, tag::plane3D >::tag.set("Distance line3D plane3D");
};
#endif
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p1,p2,g);
g.segment(3,3).setZero();
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::line3D, tag::cylinder3D > : public Distance::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
#ifdef USE_LOGGING
type() : Distance::type< Kernel, tag::line3D, tag::line3D >() {
Distance::type< Kernel, tag::line3D, tag::line3D >::tag.set("Distance line3D cylinder3D");
};
#endif
Scalar calculate(Vector& param1, Vector& 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) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};
};
//only defined when planes are parallel, therefore it's the same as the point-plane distance
template<typename Kernel>
struct Distance::type< Kernel, tag::plane3D, tag::plane3D > : public Distance::type< Kernel, tag::point3D, tag::plane3D > {
@ -155,135 +455,70 @@ struct Distance::type< Kernel, tag::plane3D, tag::plane3D > : public Distance::t
};
};
//only defined when plane and cylinder are parallel, therefore it's the same as the point-plane distance
template<typename Kernel>
struct Distance::type< Kernel, tag::point3D, tag::line3D > {
struct Distance::type< Kernel, tag::plane3D, tag::cylinder3D > : public Distance::type< Kernel, tag::point3D, tag::plane3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
typedef typename Kernel::Vector3 Vector3;
typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
Scalar value, sc_value;
Vector3 diff, n, dist;
#ifdef USE_LOGGING
src::logger log;
attrs::mutable_constant< std::string > tag;
type() : tag("Distance point3D line3D") {
log.add_attribute("Tag", tag);
type() : Distance::type< Kernel, tag::point3D, tag::plane3D >() {
Distance::type< Kernel, tag::point3D, tag::plane3D >::tag.set("Distance plane3D cylinder3D");
};
#endif
//template definition
void calculatePseudo(typename Kernel::Vector& point, Vec& v1, typename Kernel::Vector& line, Vec& v2) {
Vector3 pp = line.head(3) + (line.head(3)-point.head(3)).norm()*line.segment(3,3);
#ifdef USE_LOGGING
if(!boost::math::isnormal(pp.norm()))
BOOST_LOG(log) << "Unnormal pseudopoint detected";
#endif
v2.push_back(pp);
};
void setScale(Scalar scale) {
sc_value = value*scale;
};
Scalar calculate(Vector& point, Vector& line) {
//diff = point1 - point2
n = line.template segment<3>(3);
diff = line.template head<3>() - point.template head<3>();
dist = diff - diff.dot(n)*n;
const Scalar res = dist.norm() - sc_value;
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal residual detected: "<<res;
#endif
return res;
Scalar calculate(Vector& param1, Vector& 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& point, Vector& line, Vector& dpoint) {
if(dist.norm() == 0)
return 1.;
const Vector3 d_diff = -dpoint.template head<3>();
const Vector3 d_dist = d_diff - d_diff.dot(n)*n;
const Scalar res = dist.dot(d_dist)/dist.norm();
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal first cluster gradient detected: "<<res
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
<<" and dpoint: "<<dpoint.transpose();
#endif
return res;
Scalar calculateGradientFirst(Vector& p1, Vector& p2, Vector& dp1) {
return Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientSecond(p2,p1,dp1);
};
Scalar calculateGradientSecond(Vector& point, Vector& line, Vector& dline) {
if(dist.norm() == 0)
return 1.;
const Vector3 d_diff = dline.template head<3>();
const Vector3 d_n = dline.template segment<3>(3);
const Vector3 d_dist = d_diff - ((d_diff.dot(n)+diff.dot(d_n))*n + diff.dot(n)*d_n);
const Scalar res = dist.dot(d_dist)/dist.norm();
#ifdef USE_LOGGING
if(!boost::math::isfinite(res))
BOOST_LOG(log) << "Unnormal second cluster gradient detected: "<<res
<<" with point: "<<point.transpose()<<", line: "<<line.transpose()
<< "and dline: "<<dline.transpose();
#endif
return res;
Scalar calculateGradientSecond(Vector& p1, Vector& p2, Vector& dp2) {
return Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirst(p2,p1,dp2);
};
void calculateGradientFirstComplete(Vector& point, Vector& line, Vector& gradient) {
if(dist.norm() == 0) {
gradient.head(3).setOnes();
return;
}
const Vector3 res = (n*n.transpose())*dist - dist;
gradient.head(3) = res/dist.norm();
};
void calculateGradientSecondComplete(Vector& point, Vector& line, Vector& gradient) {
if(dist.norm() == 0) {
gradient.head(6).setOnes();
return;
}
const Vector3 res = (-n*n.transpose())*dist + dist;
gradient.head(3) = res/dist.norm();
const Scalar mult = n.transpose()*dist;
gradient.template segment<3>(3) = -(mult*diff + diff.dot(n)*dist)/dist.norm();
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::line3D, tag::line3D > : public Distance::type< Kernel, tag::point3D, tag::line3D > {
#ifdef USE_LOGGING
type() : Distance::type< Kernel, tag::point3D, tag::line3D >() {
Distance::type< Kernel, tag::point3D, tag::line3D >::tag.set("Distance line3D line3D");
};
#endif
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& p1, Vector& p2, Vector& g) {
Distance::type< Kernel, tag::point3D, tag::line3D >::calculateGradientFirstComplete(p1,p2,g);
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientSecondComplete(p2,p1,g);
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
Distance::type< Kernel, tag::point3D, tag::plane3D >::calculateGradientFirstComplete(p2,p1,g);
g.segment(3,3).setZero();
g(6) = -1;
};
};
template<typename Kernel>
struct Distance::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Distance::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
#ifdef USE_LOGGING
type() : Distance::type< Kernel, tag::line3D, tag::line3D >() {
Distance::type< Kernel, tag::line3D, tag::line3D >::tag.set("Distance cylinder3D cylinder3D");
};
#endif
typedef typename Kernel::VectorMap Vector;
Scalar calculate(Vector& param1, Vector& 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) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientFirstComplete(p1,p2,g);
g(6) = 0;
g(6) = -1;
};
void calculateGradientSecondComplete(Vector& p1, Vector& p2, Vector& g) {
Distance::type< Kernel, tag::line3D, tag::line3D >::calculateGradientSecondComplete(p1,p2,g);
g(6) = -1;
};
};

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

@ -77,8 +77,8 @@ struct Module3D {
struct Constraint3D;
struct Geometry3D;
struct vertex_prop;
struct inheriter_base;
struct inheriter_base;
typedef boost::shared_ptr<Geometry3D> Geom;
typedef boost::shared_ptr<Constraint3D> Cons;
@ -121,12 +121,12 @@ struct Module3D {
//allow accessing the internals by module3d classes but not by users
friend struct details::ClusterMath<Sys>;
friend struct details::ClusterMath<Sys>::map_downstream;
friend struct details::ClusterMath<Sys>::map_downstream;
friend struct details::SystemSolver<Sys>;
friend struct details::SystemSolver<Sys>::Rescaler;
friend class detail::Constraint<Sys, Constraint3D, ConsSignal, MES, Geometry3D>;
friend struct details::SystemSolver<Sys>::Rescaler;
friend class detail::Constraint<Sys, Constraint3D, ConsSignal, MES, Geometry3D>;
};
template<typename Derived>
class Constraint3D_id : public detail::Constraint<Sys, Derived, ConsSignal, MES, Geometry3D> {
@ -145,13 +145,36 @@ struct Module3D {
Constraint3D(Sys& system, Geom first, Geom second);
friend struct details::SystemSolver<Sys>;
friend struct details::SystemSolver<Sys>::Rescaler;
friend struct details::SystemSolver<Sys>::Rescaler;
friend struct details::MES<Sys>;
friend struct inheriter_base;
};
struct inheriter_base {
//build a constraint vector
template<typename T>
struct fusion_vec {
typedef typename mpl::if_< mpl::is_sequence<T>,
T, fusion::vector<T> >::type type;
};
struct set_constraint_option {
template<typename T>
typename boost::enable_if<mpl::is_sequence<T>, typename fusion_vec<T>::type >::type
operator()(T& val) {
return val;
};
template<typename T>
typename boost::disable_if<mpl::is_sequence<T>, typename fusion_vec<T>::type >::type
operator()(T& val) {
typename fusion_vec<T>::type vec;
fusion::at_c<0>(vec) = val;
return vec;
};
};
inheriter_base();
template<typename T>
@ -178,9 +201,9 @@ struct Module3D {
template<typename T>
Cons createConstraint3D(Identifier id, Geom first, Geom second, T constraint1);
void removeGeometry3D(Identifier id);
void removeGeometry3D(Identifier id);
void removeConstraint3D(Identifier id);
bool hasGeometry3D(Identifier id);
Geom getGeometry3D(Identifier id);
bool hasConstraint3D(Identifier id);
@ -406,27 +429,21 @@ template<typename T1>
typename Module3D<Typelist, ID>::template type<Sys>::Cons
Module3D<Typelist, ID>::type<Sys>::inheriter_base::createConstraint3D(Geom first, Geom second, T1 constraint1) {
//build a constraint vector
typedef mpl::vector<> cvec;
typedef typename mpl::if_< mpl::is_sequence<T1>,
typename mpl::fold< T1, cvec, mpl::push_back<mpl::_1,mpl::_2> >::type,
mpl::vector1<T1> >::type cvec1;
//get the fusion vector type
typedef typename fusion_vec<T1>::type covec;
//make a fusion sequence to hold the objects (as they hold the options)
typedef typename fusion::result_of::as_vector<cvec1>::type covec;
//set the objects
covec cv;
fusion::at_c<0>(cv) = constraint1;
covec cv = set_constraint_option()( constraint1 );
//now create the constraint
Cons c(new Constraint3D(*m_this, first, second));
//set the type and values
c->template initialize<cvec1>(cv);
c->template initialize<covec>(cv);
//add it to the clustergraph
fusion::vector<LocalEdge, GlobalEdge, bool, bool> res;
res = m_this->m_cluster->addEdge(first->template getProperty<vertex_prop>(),
second->template getProperty<vertex_prop>());
second->template getProperty<vertex_prop>());
if(!fusion::at_c<2>(res)) {
Cons rc;
return rc; //TODO: throw
@ -473,7 +490,7 @@ template<typename Sys>
void Module3D<Typelist, ID>::type<Sys>::inheriter_id::removeGeometry3D(Identifier id) {
if(hasGeometry3D(id))
inheriter_base::removeGeometry3D(getGeometry3D(id));
inheriter_base::removeGeometry3D(getGeometry3D(id));
};
template<typename Typelist, typename ID>
@ -492,7 +509,7 @@ template<typename Sys>
void Module3D<Typelist, ID>::type<Sys>::inheriter_id::removeConstraint3D(Identifier id) {
if(hasConstraint3D(id))
removeConstraint3D(getConstraint3D(id));
removeConstraint3D(getConstraint3D(id));
};

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

@ -25,12 +25,12 @@
#include "geometry.hpp"
#include <boost/math/special_functions/fpclassify.hpp>
using boost::math::isnormal;
using boost::math::isfinite;
namespace dcm {
//the calculations( same as we always calculate directions we can outsource the work to this functions)
namespace parallel_detail {
namespace orientation_detail {
template<typename Kernel, typename T>
inline typename Kernel::number_type calc(T d1,
@ -38,19 +38,18 @@ inline typename Kernel::number_type calc(T d1,
Direction dir) {
switch(dir) {
case Same:
case equal:
return (d1-d2).norm();
case Opposite:
case opposite:
return (d1+d2).norm();
case Both:
if(d1.dot(d2) >= 0) {
return (d1-d2).norm();
}
return (d1+d2).norm();
default:
assert(false);
case parallel:
return d1.dot(d2) - d1.norm()*d2.norm();
case perpendicular:
return d1.dot(d2);
default:
assert(false);
}
return 0;
return 0;
};
@ -62,21 +61,21 @@ inline typename Kernel::number_type calcGradFirst(T d1,
typename Kernel::number_type res;
switch(dir) {
case Same:
case equal:
res = ((d1-d2).dot(dd1) / (d1-d2).norm());
break;
case Opposite:
case opposite:
res= ((d1+d2).dot(dd1) / (d1+d2).norm());
break;
case Both:
if(d1.dot(d2) >= 0) {
res = (((d1-d2).dot(dd1) / (d1-d2).norm()));
break;
}
res = (((d1+d2).dot(dd1) / (d1+d2).norm()));
case parallel:
res = dd1.dot(d2) - d1.dot(dd1)/d1.norm()*d2.norm();
break;
case perpendicular:
res = dd1.dot(d2);
break;
}
if((isnormal)(res)) return res;
if(isfinite(res)) return res;
return 0;
};
@ -88,21 +87,20 @@ inline typename Kernel::number_type calcGradSecond(T d1,
typename Kernel::number_type res;
switch(dir) {
case Same:
case equal:
res = ((d1-d2).dot(-dd2) / (d1-d2).norm());
break;
case Opposite:
case opposite:
res = ((d1+d2).dot(dd2) / (d1+d2).norm());
break;
case Both:
if(d1.dot(d2) >= 0) {
res = (((d1-d2).dot(-dd2) / (d1-d2).norm()));
break;
}
res = (((d1+d2).dot(dd2) / (d1+d2).norm()));
case parallel:
res = d1.dot(dd2) - d2.dot(dd2)/d2.norm()*d1.norm();
break;
case perpendicular:
res = d1.dot(dd2);
break;
}
if((isnormal)(res)) return res;
if((isfinite)(res)) return res;
return 0;
};
@ -113,14 +111,18 @@ inline void calcGradFirstComp(T d1,
Direction dir) {
switch(dir) {
case Same:
case equal:
grad = (d1-d2) / (d1-d2).norm();
return;
case Opposite:
case opposite:
grad = (d1+d2) / (d1+d2).norm();
return;
case Both:
assert(false);
case parallel:
grad = d2 - d1/d1.norm()*d2.norm();
return;
case perpendicular:
grad = d2;
return;
}
};
@ -131,85 +133,198 @@ inline void calcGradSecondComp(T d1,
Direction dir) {
switch(dir) {
case Same:
case equal:
grad = (d2-d1) / (d1-d2).norm();
return;
case Opposite:
case opposite:
grad = (d2+d1) / (d1+d2).norm();
return;
case Both:
assert(false);
case parallel:
grad = d1 - d2/d2.norm()*d1.norm();
return;
case perpendicular:
grad = d1;
return;
}
};
}
template< typename Kernel >
struct Parallel::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale<Kernel> {
struct Orientation::type< Kernel, tag::direction3D, tag::direction3D > : public dcm::PseudoScale<Kernel> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
Direction value;
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
return parallel_detail::calc<Kernel>(param1.template tail<3>(), param2.template tail<3>(), value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
return parallel_detail::calcGradFirst<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam1.template tail<3>(), value);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
return parallel_detail::calcGradSecond<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam2.template tail<3>(), value);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
gradient.template head<3>().setZero();
parallel_detail::calcGradFirstComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), value);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
gradient.template head<3>().setZero();
parallel_detail::calcGradSecondComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), value);
};
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
return orientation_detail::calc<Kernel>(param1, param2, value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
return orientation_detail::calcGradFirst<Kernel>(param1, param2, dparam1, value);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
return orientation_detail::calcGradSecond<Kernel>(param1, param2, dparam2, value);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradFirstComp<Kernel>(param1, param2, gradient, value);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1, param2, gradient, value);
};
};
template< typename Kernel >
struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale<Kernel> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
//template definition
Scalar calculate(Vector& param1, Vector& 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) {
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) {
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) {
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) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
value);
};
};
template< typename Kernel >
struct Orientation::type< Kernel, tag::line3D, tag::plane3D > : public Orientation::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
option_type value;
option_type getValue() {
if(value==parallel)
return perpendicular;
if(value==perpendicular)
return parallel;
return value;
};
template< typename Kernel >
struct Parallel::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public dcm::PseudoScale<Kernel>{
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
Direction value;
Scalar calculate(Vector& param1, Vector& param2) {
return parallel_detail::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), value);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
return parallel_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) {
return parallel_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) {
gradient.template head<3>().setZero();
parallel_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) {
gradient.template head<3>().setZero();
parallel_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3),
gradient.template segment<3>(3), value);
};
//template definition
Scalar calculate(Vector& param1, Vector& 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) {
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) {
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) {
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) {
gradient.template head<3>().setZero();
orientation_detail::calcGradSecondComp<Kernel>(param1.template segment<3>(3),
param2.template segment<3>(3),
gradient.template segment<3>(3),
getValue());
};
};
template< typename Kernel >
struct Parallel::type< Kernel, tag::line3D, tag::plane3D > : public Parallel::type<Kernel, tag::line3D, tag::line3D> {};
struct Orientation::type< Kernel, tag::line3D, tag::cylinder3D > : public Orientation::type< Kernel, tag::line3D, tag::line3D > {};
template< typename Kernel >
struct Parallel::type< Kernel, tag::plane3D, tag::plane3D > : public Parallel::type<Kernel, tag::line3D, tag::line3D> {};
struct Orientation::type< Kernel, tag::plane3D, tag::plane3D > : public Orientation::type< Kernel, tag::line3D, tag::line3D > {};
template< typename Kernel >
struct Orientation::type< Kernel, tag::plane3D, tag::cylinder3D > : public Orientation::type<Kernel, tag::line3D, tag::plane3D> {
typedef typename Kernel::number_type Scalar;
typedef typename Kernel::VectorMap Vector;
using Orientation::type<Kernel, tag::line3D, tag::plane3D>::value;
//template definition
Scalar calculate(Vector& param1, Vector& param2) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculate(param1, param2);
};
Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientFirst(param1, param2, dparam1);
};
Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
return Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientSecond(param1, param2, dparam2);
};
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientFirstComplete(param1, param2, gradient);
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
Orientation::type<Kernel, tag::line3D, tag::plane3D>::calculateGradientSecondComplete(param1, param2, gradient);
gradient(6)=0;
};
};
template< typename Kernel >
struct Orientation::type< Kernel, tag::cylinder3D, tag::cylinder3D > : public Orientation::type< Kernel, tag::line3D, tag::line3D > {
typedef typename Kernel::VectorMap Vector;
void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& 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),
Orientation::type< Kernel, tag::line3D, tag::line3D >::value);
gradient(6) = 0;
};
void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
gradient.template head<3>().setZero();
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);
gradient(6) = 0;
};
};
}
#endif //GCM_ANGLE
#endif