MBSim

Project Page
functions_contact.h
00001 /* Copyright (C) 2004-2010 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  *          rzander@users.berlios.de
00019  */
00020 
00021 #ifndef FUNCTIONS_CONTACT_H_
00022 #define FUNCTIONS_CONTACT_H_
00023 
00024 #include <mbsim/contour.h>
00025 #include <mbsim/contours/contour1s.h>
00026 #include <mbsim/contours/point.h>
00027 #include "mbsim/contours/line.h"
00028 #include <mbsim/contours/circle_solid.h>
00029 #include "mbsim/contours/circle_hollow.h"
00030 #include "mbsim/contours/circle.h"
00031 #include "mbsim/contours/frustum2d.h"
00032 #include "mbsim/contours/edge.h"
00033 #include "mbsim/contours/frustum.h"
00034 #include "mbsim/contours/plane.h"
00035 #include "mbsim/contours/contour2s.h"
00036 #include "mbsim/contours/planewithfrustum.h"
00037 #include "mbsim/contours/contour_interpolation.h"
00038 #include "mbsim/contours/contour_quad.h"
00039 #include "mbsim/contours/cuboid.h"
00040 #include "mbsim/contours/compound_contour.h"
00041 #include "mbsim/mbsim_event.h"
00042 #include <mbsim/functions/function.h>
00043 
00044 namespace MBSim {
00045 
00046   template <typename Sig> class DistanceFunction;
00047 
00053   template <typename Ret, typename Arg>
00054   class DistanceFunction<Ret(Arg)> : public Function<Ret(Arg)> {
00055     public:
00056       virtual ~DistanceFunction() {
00057       }
00058       /* INTERFACE FOR DERIVED CLASSES */
00063       virtual Ret operator()(const Arg &x) = 0;
00064 
00069         virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); }
00070 
00075       virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00076       /*************************************************/
00077   };
00078 
00086   class FuncPairContour1sPoint : public DistanceFunction<double(double)> {
00087     public:
00093       FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp() {}
00094 
00095       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00096       double operator()(const double &alpha) {
00097         fmatvec::Vec3 Wd = computeWrD(alpha);
00098         fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00099         return Wt.T() * Wd;
00100       }
00101 
00102       fmatvec::Vec3 computeWrD(const double &alpha) {
00103         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00104         cp.getLagrangeParameterPosition()(0) = alpha;
00105         contour->computeRootFunctionPosition(cp);
00106         contour->computeRootFunctionFirstTangent(cp);
00107         //}
00108         return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00109       }
00110       /*************************************************/
00111 
00112     private:
00116       Contour1s *contour;
00117       Point *point;
00118 
00122       ContourPointData cp;
00123   };
00124 
00130   class FuncPairContour2sPoint : public DistanceFunction<fmatvec::Vec(fmatvec::Vec)> {
00131     public:
00137       FuncPairContour2sPoint(Point* point_, Contour2s *contour_) :
00138           contour(contour_), point(point_), cp() {
00139       }
00140 
00141       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00142       fmatvec::Vec operator()(const fmatvec::Vec &alpha) {  // Vec2: U and V direction
00143         fmatvec::Vec3 Wd = computeWrD(alpha);
00144         fmatvec::Vec3 Wt1 = cp.getFrameOfReference().getOrientation().col(1);
00145         fmatvec::Vec3 Wt2 = cp.getFrameOfReference().getOrientation().col(2);
00146         fmatvec::Vec Wt(2, fmatvec::NONINIT);  // TODO:: check this?
00147         Wt(0) = Wt1.T() * Wd; // the projection of distance vector Wd into the first tangent direction: scalar value
00148         Wt(1) = Wt2.T() * Wd; // the projection of distance vector Wd into the second tangent direction: scalar value
00149         return Wt;
00150       }
00151 
00152       fmatvec::Vec3 computeWrD(const fmatvec::Vec &alpha) {
00153         cp.getLagrangeParameterPosition()(0) = alpha(0);
00154         cp.getLagrangeParameterPosition()(1) = alpha(1);
00155         contour->computeRootFunctionPosition(cp);
00156         contour->computeRootFunctionFirstTangent(cp); // TODO:: move these two line into operator() ?? maybe not possible, as when opertor[] is called, the orientation need to be updated
00157         contour->computeRootFunctionSecondTangent(cp);
00158         return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00159       }
00160       /*************************************************/
00161 
00162     private:
00166       Contour2s *contour;
00167       Point *point;
00171       ContourPointData cp;
00172   };
00173 
00181   class FuncPairContour1sCircleHollow : public DistanceFunction<double(double)> {
00182     public:
00188       FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00189 
00190       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00191       double operator()(const double &alpha) {
00192         fmatvec::Vec3 Wd = computeWrD(alpha);
00193         return circle->getReferenceOrientation().col(2).T() * Wd;
00194       }
00195 
00196       fmatvec::Vec3 computeWrD(const double &alpha) {
00197         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00198         cp.getLagrangeParameterPosition()(0) = alpha;
00199         contour->computeRootFunctionPosition(cp);
00200         //}
00201         return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00202       }
00203       /*************************************************/
00204 
00205     private:
00209       Contour1s *contour;
00210       CircleHollow *circle;
00211 
00215       ContourPointData cp;
00216   };
00217 
00223   class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV(fmatvec::VecV)> {
00224     public:
00230       FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00231 
00232       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00233       fmatvec::VecV operator()(const fmatvec::VecV &alpha) {
00234         fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00235         fmatvec::Vec3 WrOC[2];
00236         WrOC[0] = point->getFrame()->getPosition();
00237         WrOC[1] = contour->computeWrOC(alpha);
00238         return Wt.T() * (WrOC[1] - WrOC[0]);
00239       }
00240 
00241       fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00242         return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00243       }
00244       /*************************************************/
00245 
00246     private:
00250       ContourInterpolation *contour;
00251       Point *point;
00252   };
00253 
00259   class FuncPairConeSectionCircle : public DistanceFunction<double(double)> {
00260     public:
00268       FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00269 
00277       FuncPairConeSectionCircle(double R_, double a_, double b_, bool sec_IN_ci_) :
00278           R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {
00279       }
00280 
00281       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00282       virtual double operator()(const double &phi) = 0;
00283       double operator[](const double &phi);
00284       virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00285       /*************************************************/
00286 
00287       /* GETTER / SETTER */
00288       void setDiffVec(fmatvec::Vec3 d_);
00289 
00290       void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
00291       /*************************************************/
00292 
00293     protected:
00297       double R, a, b;
00298 
00302       bool sec_IN_ci;
00303 
00307       fmatvec::Vec3 b1, b2;
00308 
00312       fmatvec::Vec3 d;
00313   };
00314 
00315   inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
00316     d = d_;
00317   }
00318   inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
00319     b1 = b1_;
00320     b2 = b2_;
00321   }
00322   inline double FuncPairConeSectionCircle::operator[](const double &phi) {
00323     if (sec_IN_ci)
00324       return R - nrm2(computeWrD(phi));
00325     else
00326       return nrm2(computeWrD(phi)) - R;
00327   }
00328 
00335   class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00336     public:
00344       FuncPairEllipseCircle(double R_, double a_, double b_) :
00345           FuncPairConeSectionCircle(R_, a_, b_) {
00346       }
00347 
00355       FuncPairEllipseCircle(double R_, double a_, double b_, bool el_IN_ci_) :
00356           FuncPairConeSectionCircle(R_, a_, b_, el_IN_ci_) {
00357       }
00358 
00359       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00360       double operator()(const double &phi);
00361       fmatvec::Vec3 computeWrD(const double &phi);
00362       /*************************************************/
00363 
00364       /* GETTER / SETTER */
00365       void setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_);
00366       /*************************************************/
00367   };
00368 
00369   inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_) {
00370     setSectionCOS(b1e_, b2e_);
00371   }
00372   inline double FuncPairEllipseCircle::operator()(const double &phi) {
00373     return -2 * b * (b2(0) * d(0) + b2(1) * d(1) + b2(2) * d(2)) * cos(phi) + 2 * a * (b1(0) * d(0) + b1(1) * d(1) + b1(2) * d(2)) * sin(phi) + ((a * a) - (b * b)) * sin(2 * phi);
00374   }
00375   inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) {
00376     return d + b1 * a * cos(phi) + b2 * b * sin(phi);
00377   }
00378 
00384   class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00385     public:
00393       FuncPairHyperbolaCircle(double R_, double a_, double b_) :
00394           FuncPairConeSectionCircle(R_, a_, b_) {
00395       }
00396 
00404       FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) :
00405           FuncPairConeSectionCircle(R_, a_, b_, hy_IN_ci_) {
00406       }
00407 
00408       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00409       double operator()(const double &phi);
00410       fmatvec::Vec3 computeWrD(const double &phi);
00411       /*************************************************/
00412   };
00413 
00414   inline double FuncPairHyperbolaCircle::operator()(const double &phi) {
00415     return -2 * b * (b2(0) * d(0) + b2(1) * d(1) + b2(2) * d(2)) * cosh(phi) - 2 * a * (b1(0) * d(0) + b1(1) * d(1) + b1(2) * d(2)) * sinh(phi) - ((a * a) + (b * b)) * sinh(2 * phi);
00416   }
00417   inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) {
00418     return d + b1 * a * cosh(phi) + b2 * b * sinh(phi);
00419   }
00420 
00426   class JacobianPairConeSectionCircle : public Function<double(double)> {
00427     public:
00433       JacobianPairConeSectionCircle(double a_, double b_) :
00434           a(a_), b(b_) {
00435       }
00436 
00437       /* GETTER / SETTER */
00438       void setDiffVec(fmatvec::Vec3 d_);
00439       void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
00440       /*************************************************/
00441 
00442     protected:
00446       double a, b;
00447 
00451       fmatvec::Vec3 b1, b2;
00452 
00456       fmatvec::Vec3 d;
00457   };
00458 
00459   inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
00460     d = d_;
00461   }
00462   inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
00463     b1 = b1_;
00464     b2 = b2_;
00465   }
00466 
00472   class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00473     public:
00479       JacobianPairEllipseCircle(double a_, double b_) :
00480           JacobianPairConeSectionCircle(a_, b_) {
00481       }
00482 
00483       /* INHERITED INTERFACE OF FUNCTION */
00484       double operator()(const double &phi);
00485       /*************************************************/
00486   };
00487 
00488   inline double JacobianPairEllipseCircle::operator()(const double &phi) {
00489     return 2. * (b * (b2(0) * d(0) + b2(1) * d(1) + b2(2) * d(2)) * sin(phi) + a * (b1(0) * d(0) + b1(1) * d(1) + b1(2) * d(2)) * cos(phi) + ((a * a) - (b * b)) * cos(2 * phi));
00490   }
00491 
00497   class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00498     public:
00504       JacobianPairHyperbolaCircle(double a_, double b_) :
00505           JacobianPairConeSectionCircle(a_, b_) {
00506       }
00507 
00508       /* INHERITED INTERFACE OF FUNCTION */
00509       double operator()(const double &phi);
00510       /*************************************************/
00511   };
00512 
00513   inline double JacobianPairHyperbolaCircle::operator()(const double &phi) {
00514     return -2 * (b * (b2(0) * d(0) + b2(1) * d(1) + b2(2) * d(2)) * sinh(phi) + a * (b1(0) * d(0) + b1(1) * d(1) + b1(2) * d(2)) * cosh(phi) + ((a * a) + (b * b)) * cosh(2 * phi));
00515   }
00516 
00523   class FuncPairContour1sLine : public DistanceFunction<double(double)> {
00524     public:
00530       FuncPairContour1sLine(Line* line_, Contour1s *contour_) :
00531           contour(contour_), line(line_) {
00532       }
00533 
00534       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00535       virtual double operator()(const double &s) {
00536         THROW_MBSIMERROR("(FuncPairContour1sLine::operator): Not implemented!");
00537         //fmatvec::Vec WtC = (contour->computeWt(s)).col(0);
00538         //fmatvec::Vec WnL = line->computeWn();
00539         //return trans(WtC)*WnL;
00540       }
00541 
00542       virtual fmatvec::Vec3 computeWrD(const double &s) {
00543         THROW_MBSIMERROR("(FuncPairContour1sLine::computeWrD): Not implemented!");
00544         //fmatvec::Vec WrOCContour =  contour->computeWrOC(s);
00545         //fmatvec::Vec Wn = contour->computeWn(s);
00546         //double g =trans(Wn)*(WrOCContour-line->getFrame()->getPosition()); 
00547         //return Wn*g;
00548       }
00549 
00550       virtual double operator[](const double &s) {
00551         return nrm2(computeWrD(s));
00552       }
00553       /*************************************************/
00554 
00555     private:
00556       Contour1s *contour;
00557       Line *line;
00558   };
00559 
00565   class FuncPairContour1sCircleSolid : public DistanceFunction<double(double)> {
00566     public:
00572       FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) :
00573           contour(contour_), circle(circle_) {
00574       }
00575 
00576       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00577       double operator()(const double &alpha) {
00578         cp.getLagrangeParameterPosition()(0) = alpha;
00579         fmatvec::Vec3 Wd = computeWrD(alpha);
00580         fmatvec::Vec3 Wt = contour->computeTangent(cp);
00581         return Wt.T() * Wd;
00582       }
00583 
00584       fmatvec::Vec3 computeWrD(const double &alpha) {
00585         cp.getLagrangeParameterPosition()(0) = alpha;
00586         contour->computeRootFunctionPosition(cp);
00587         contour->computeRootFunctionFirstTangent(cp);
00588         contour->computeRootFunctionNormal(cp);
00589         WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius() * cp.getFrameOfReference().getOrientation().col(0);
00590         WrOC[1] = cp.getFrameOfReference().getPosition();
00591         return WrOC[1] - WrOC[0];
00592       }
00593       /***************************************************/
00594 
00595     private:
00599       Contour1s *contour;
00600       CircleSolid *circle;
00601 
00605       ContourPointData cp;
00606 
00610       fmatvec::Vec3 WrOC[2];
00611   };
00612 
00623   class Contact1sSearch {
00624     public:
00628       Contact1sSearch() :
00629           func(0), jac(0), s0(0.), searchAll(false) {
00630       }
00631 
00638       Contact1sSearch(DistanceFunction<double(double)> *func_) :
00639           func(func_), jac(0), s0(0.), searchAll(false) {
00640       }
00641 
00648       Contact1sSearch(DistanceFunction<double(double)> *func_, Function<double(double)> *jac_) :
00649           func(func_), jac(jac_), s0(0.), searchAll(false) {
00650       }
00651 
00652       /* GETTER / SETTER */
00653       void setFunction(DistanceFunction<double(double)> *func_) {
00654         func = func_;
00655       }
00656 
00657       void setJacobianFunction(Function<double(double)> *jac_) {
00658         jac = jac_;
00659       }
00660 
00661       void setInitialValue(const double &s0_) {
00662         s0 = s0_;
00663       }
00664       void setNodes(const fmatvec::Vec &nodes_) {
00665         nodes = nodes_;
00666       }
00667       void setSearchAll(bool searchAll_) {
00668         searchAll = searchAll_;
00669       }
00670       /*************************************************/
00671 
00678       void setEqualSpacing(const int &n, const double &x0, const double &dx);
00679 
00684       double slv();
00689       fmatvec::Mat slvAll();
00690 
00691     private:
00695       DistanceFunction<double(double)> *func;
00696 
00700       Function<double(double)> *jac;
00701 
00705       double s0;
00706 
00710       fmatvec::Vec nodes;
00711 
00715       bool searchAll;
00716   };
00717 
00726   class Contact2sSearch {
00727     public:
00734       Contact2sSearch(DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func_) :
00735           func(func_), jac(0), s0(2), searchAll(false) {
00736       }
00737 
00744       Contact2sSearch(DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func_, Function<fmatvec::Mat(fmatvec::Mat)> *jac_) :
00745           func(func_), jac(jac_), s0(2), searchAll(false) {
00746       }
00747 
00748       /* GETTER / SETTER */
00749       void setInitialValue(const fmatvec::Vec2 &s0_) {
00750         s0 = s0_;
00751       }
00752       void setNodes(const fmatvec::Vec &nodesU_, const fmatvec::Vec &nodesV_) {
00753         nodesU = nodesU_;
00754         nodesV = nodesV_;
00755       }
00756       void setSearchAll(bool searchAll_) {
00757         searchAll = searchAll_;
00758       }
00759       /*************************************************/
00760 
00770       void setEqualSpacing(const int nU, const int nV, const double U0, const double V0, const double dU, const double dV);
00771 
00776       fmatvec::Vec2 slv();
00777 
00778 //      /*!
00779 //       * \brief solve for all potential contact points
00780 //       * \return matrix holding LagrangeParameterPosition in col(0) and respective distances in col(1)
00781 //       */
00782 //      fmatvec::Mat slvAll();
00783 
00784     protected:
00788       std::vector<double> searchVdirection(double u);
00789 
00790     private:
00794       DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func;
00795 
00799       Function<fmatvec::Mat(fmatvec::Mat)> *jac;  // TODO::check the template type
00800 
00804       fmatvec::Vec2 s0;
00805 
00809       fmatvec::Vec nodesU, nodesV;
00810 
00814       bool searchAll;
00815   };
00816 }
00817 
00818 #endif /* FUNCTIONS_CONTACT_H_ */
00819