add solver.hpp
This commit is contained in:
Stefan Tröger
committed by
Stefan Tröger
Stefan Tröger
parent
36c474e8f4
commit
7a95658cdb
@@ -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;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -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));
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user