From bf447d3a7ca581199d435cc4e8ebbaff7f85af10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20Tr=C3=B6ger?= Date: Wed, 1 May 2013 19:25:23 +0200 Subject: [PATCH] add solver.hpp --- src/Mod/Assembly/App/ConstraintGroup.cpp | 6 +- src/Mod/Assembly/App/Solver.h | 335 ++++++++++++++ .../Assembly/App/opendcm/core/constraint.hpp | 11 +- .../Assembly/App/opendcm/core/equations.hpp | 127 +++-- .../App/opendcm/module3d/distance.hpp | 433 ++++++++++++++---- .../Assembly/App/opendcm/module3d/module.hpp | 61 ++- .../App/opendcm/module3d/parallel.hpp | 301 ++++++++---- 7 files changed, 1021 insertions(+), 253 deletions(-) create mode 100644 src/Mod/Assembly/App/Solver.h diff --git a/src/Mod/Assembly/App/ConstraintGroup.cpp b/src/Mod/Assembly/App/ConstraintGroup.cpp index 71fe93f0a..2241be58e 100644 --- a/src/Mod/Assembly/App/ConstraintGroup.cpp +++ b/src/Mod/Assembly/App/ConstraintGroup.cpp @@ -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::iterator iter; std::vector 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); diff --git a/src/Mod/Assembly/App/Solver.h b/src/Mod/Assembly/App/Solver.h new file mode 100644 index 000000000..5948b7167 --- /dev/null +++ b/src/Mod/Assembly/App/Solver.h @@ -0,0 +1,335 @@ +/*************************************************************************** + * Copyright (c) 2013 Stefan Tröger * + * * + * 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 + +#include +#include +#include +#include +#include + +#include "opendcm/core.hpp" +#include "opendcm/module3d.hpp" +#include "opendcm/modulepart.hpp" + +struct gp_pnt_accessor { + + template + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 { + typedef tag::point3D tag; + typedef modell::XYZ modell; + typedef gp_pnt_accessor accessor; +}; +template<> +struct geometry_traits { + typedef tag::line3D tag; + typedef modell::XYZ2 modell; + typedef gp_lin_accessor accessor; +}; +template<> +struct geometry_traits { + typedef tag::plane3D tag; + typedef modell::XYZ2 modell; + typedef gp_pln_accessor accessor; +}; +template<> +struct geometry_traits { + typedef tag::cylinder3D tag; + typedef modell::XYZ2P modell; + typedef gp_cylinder_accessor accessor; +}; +template<> +struct geometry_traits { + typedef tag::part tag; + typedef modell::quaternion_wxyz_vec3 modell; + typedef placement_accessor accessor; +}; +template<> +struct geometry_traits { + typedef tag::point3D tag; + typedef modell::XYZ modell; + typedef orderd_bracket_accessor accessor; +}; +} + + +//our constraint solving system +typedef dcm::Kernel 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 Solver; + +typedef typename ModulePart::type::Part Part3D; +typedef typename Module3D::type::Geometry3D Geometry3D; +typedef typename Module3D::type::Constraint3D Constraint3D; + + +#endif //SOLVER_H \ No newline at end of file diff --git a/src/Mod/Assembly/App/opendcm/core/constraint.hpp b/src/Mod/Assembly/App/opendcm/core/constraint.hpp index c2f74117f..6cb91602d 100644 --- a/src/Mod/Assembly/App/opendcm/core/constraint.hpp +++ b/src/Mod/Assembly/App/opendcm/core/constraint.hpp @@ -73,7 +73,7 @@ public: protected: template - void initialize(typename fusion::result_of::as_vector::type& obj); + void initialize(ConstraintVector& obj); int equationCount(); @@ -125,7 +125,7 @@ public: mpl::push_back > >::type eq_set_vector; typedef typename fusion::result_of::as_vector::type EquationSets; - typedef typename fusion::result_of::as_vector::type Objects; + typedef ConstraintVector Objects; template struct has_option { @@ -135,7 +135,7 @@ public: typedef typename mpl::find::type iterator; typedef typename mpl::distance::type, iterator>::type distance; BOOST_MPL_ASSERT((mpl::not_::type > >)); - typedef typename mpl::at::type option_type; + typedef typename fusion::result_of::at::type option_type; typedef mpl::not_ > type; }; @@ -205,7 +205,7 @@ protected: template< typename ConstraintVector > struct creator : public boost::static_visitor { - typedef typename fusion::result_of::as_vector::type Objects; + typedef ConstraintVector Objects; Objects& objects; creator(Objects& obj); @@ -272,7 +272,7 @@ boost::shared_ptr Constraint::clo template template -void Constraint::initialize(typename fusion::result_of::as_vector::type& obj) { +void Constraint::initialize(ConstraintVector& obj) { //first create the new placeholder creator c(obj); @@ -491,6 +491,7 @@ template template void Constraint::creator::operator()(const T1&, const T2&) { + typedef tag_order< typename geometry_traits::tag, typename geometry_traits::tag > order; //transform the constraints into eqautions with the now known types diff --git a/src/Mod/Assembly/App/opendcm/core/equations.hpp b/src/Mod/Assembly/App/opendcm/core/equations.hpp index 0e391f48e..4e078bf55 100644 --- a/src/Mod/Assembly/App/opendcm/core/equations.hpp +++ b/src/Mod/Assembly/App/opendcm/core/equations.hpp @@ -22,6 +22,20 @@ #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 +struct pushed_seq; + +template +struct op_seq : public seq { + + template + typename boost::enable_if< boost::is_base_of< dcm::EQ, T>, typename pushed_seq::type >::type operator &(T val) { + + typedef typename pushed_seq::type Sequence; + typedef typename fusion::result_of::begin::type Begin; + typedef typename fusion::result_of::end::type End; + typedef typename fusion::result_of::prior::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 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 +struct pushed_seq { + typedef typename boost::mpl::if_, Seq, fusion::vector1 >::type S; + typedef typename fusion::result_of::as_vector< typename boost::mpl::push_back::type >::type vec; + typedef op_seq type; +}; + +template +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(this)); }; + Derived& operator=(const option_type val) { + return operator()(val); + }; + + template + typename boost::enable_if< boost::is_base_of< dcm::EQ, T>, typename pushed_seq::type >::type operator &(T val) { + + typename pushed_seq::type vec; + fusion::at_c<0>(vec) = val; + fusion::at_c<1>(vec) = *(static_cast(this)); + return vec; + }; +}; + +struct Distance : public Equation { + + 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 { - 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 { @@ -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 { - 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 { @@ -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; }; diff --git a/src/Mod/Assembly/App/opendcm/module3d/distance.hpp b/src/Mod/Assembly/App/opendcm/module3d/distance.hpp index b36d30ad0..88fe8ab3e 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/distance.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/distance.hpp @@ -56,6 +56,108 @@ struct Distance::type< Kernel, tag::point3D, tag::point3D > { }; }; +template +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 > 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: "<(); + 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: "<(); + 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: "<(3) = -(mult*diff + diff.dot(n)*dist)/dist.norm(); + }; +}; + template 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 +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 +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 > 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: "<(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: "<(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: "<= 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 +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 +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 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 -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 > 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: "<::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: "<::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: "<::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 -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 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; }; }; diff --git a/src/Mod/Assembly/App/opendcm/module3d/module.hpp b/src/Mod/Assembly/App/opendcm/module3d/module.hpp index 73623f39f..f274fac1f 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/module.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/module.hpp @@ -77,8 +77,8 @@ struct Module3D { struct Constraint3D; struct Geometry3D; struct vertex_prop; - struct inheriter_base; - + struct inheriter_base; + typedef boost::shared_ptr Geom; typedef boost::shared_ptr Cons; @@ -121,12 +121,12 @@ struct Module3D { //allow accessing the internals by module3d classes but not by users friend struct details::ClusterMath; - friend struct details::ClusterMath::map_downstream; + friend struct details::ClusterMath::map_downstream; friend struct details::SystemSolver; - friend struct details::SystemSolver::Rescaler; - friend class detail::Constraint; + friend struct details::SystemSolver::Rescaler; + friend class detail::Constraint; }; - + template class Constraint3D_id : public detail::Constraint { @@ -145,13 +145,36 @@ struct Module3D { Constraint3D(Sys& system, Geom first, Geom second); friend struct details::SystemSolver; - friend struct details::SystemSolver::Rescaler; + friend struct details::SystemSolver::Rescaler; friend struct details::MES; friend struct inheriter_base; }; struct inheriter_base { + //build a constraint vector + template + struct fusion_vec { + typedef typename mpl::if_< mpl::is_sequence, + T, fusion::vector >::type type; + }; + + struct set_constraint_option { + + template + typename boost::enable_if, typename fusion_vec::type >::type + operator()(T& val) { + return val; + }; + template + typename boost::disable_if, typename fusion_vec::type >::type + operator()(T& val) { + typename fusion_vec::type vec; + fusion::at_c<0>(vec) = val; + return vec; + }; + }; + inheriter_base(); template @@ -178,9 +201,9 @@ struct Module3D { template 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 Module3D::template type::Cons Module3D::type::inheriter_base::createConstraint3D(Geom first, Geom second, T1 constraint1) { - //build a constraint vector - typedef mpl::vector<> cvec; - typedef typename mpl::if_< mpl::is_sequence, - typename mpl::fold< T1, cvec, mpl::push_back >::type, - mpl::vector1 >::type cvec1; + //get the fusion vector type + typedef typename fusion_vec::type covec; - //make a fusion sequence to hold the objects (as they hold the options) - typedef typename fusion::result_of::as_vector::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(cv); + c->template initialize(cv); //add it to the clustergraph fusion::vector res; res = m_this->m_cluster->addEdge(first->template getProperty(), - second->template getProperty()); + second->template getProperty()); if(!fusion::at_c<2>(res)) { Cons rc; return rc; //TODO: throw @@ -473,7 +490,7 @@ template void Module3D::type::inheriter_id::removeGeometry3D(Identifier id) { if(hasGeometry3D(id)) - inheriter_base::removeGeometry3D(getGeometry3D(id)); + inheriter_base::removeGeometry3D(getGeometry3D(id)); }; template @@ -492,7 +509,7 @@ template void Module3D::type::inheriter_id::removeConstraint3D(Identifier id) { if(hasConstraint3D(id)) - removeConstraint3D(getConstraint3D(id)); + removeConstraint3D(getConstraint3D(id)); }; diff --git a/src/Mod/Assembly/App/opendcm/module3d/parallel.hpp b/src/Mod/Assembly/App/opendcm/module3d/parallel.hpp index 440fa1032..9aad3e8f5 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/parallel.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/parallel.hpp @@ -25,12 +25,12 @@ #include "geometry.hpp" #include -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 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 { +struct Orientation::type< Kernel, tag::direction3D, tag::direction3D > : public dcm::PseudoScale { - 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(param1.template tail<3>(), param2.template tail<3>(), value); - }; - Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { - return parallel_detail::calcGradFirst(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(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(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(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(param1, param2, value); + }; + Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { + return orientation_detail::calcGradFirst(param1, param2, dparam1, value); + }; + Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) { + return orientation_detail::calcGradSecond(param1, param2, dparam2, value); + }; + void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) { + gradient.template head<3>().setZero(); + orientation_detail::calcGradFirstComp(param1, param2, gradient, value); + }; + void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) { + gradient.template head<3>().setZero(); + orientation_detail::calcGradSecondComp(param1, param2, gradient, value); + }; +}; + +template< typename Kernel > +struct Orientation::type< Kernel, tag::line3D, tag::line3D > : public dcm::PseudoScale { + + 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(param1.template segment<3>(3), + param2.template segment<3>(3), + value); + }; + Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { + return orientation_detail::calcGradFirst(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(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(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(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{ - - typedef typename Kernel::number_type Scalar; - typedef typename Kernel::VectorMap Vector; - - Direction value; - - Scalar calculate(Vector& param1, Vector& param2) { - return parallel_detail::calc(param1.template segment<3>(3), param2.template segment<3>(3), value); - }; - Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { - return parallel_detail::calcGradFirst(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(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(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(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(param1.template segment<3>(3), + param2.template segment<3>(3), + getValue()); }; + Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { + return orientation_detail::calcGradFirst(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(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(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(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 {}; +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 {}; +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 { + + typedef typename Kernel::number_type Scalar; + typedef typename Kernel::VectorMap Vector; + + using Orientation::type::value; + + //template definition + Scalar calculate(Vector& param1, Vector& param2) { + return Orientation::type::calculate(param1, param2); + }; + Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) { + return Orientation::type::calculateGradientFirst(param1, param2, dparam1); + }; + Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) { + return Orientation::type::calculateGradientSecond(param1, param2, dparam2); + }; + void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) { + Orientation::type::calculateGradientFirstComplete(param1, param2, gradient); + }; + void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) { + Orientation::type::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(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(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