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/planewithfrustum.h"
00036 #include "mbsim/contours/contour_interpolation.h"
00037 #include "mbsim/contours/contour_quad.h"
00038 #include "mbsim/contours/cuboid.h"
00039 #include "mbsim/contours/compound_contour.h"
00040 #include "mbsim/mbsim_event.h"
00041 #include <fmatvec/function.h>
00042 
00043 namespace MBSim {
00044 
00045   template<typename Sig> class DistanceFunction;
00046 
00052   template<typename Ret, typename Arg>
00053     class DistanceFunction<Ret(Arg)> : public fmatvec::Function<Ret(Arg)> {
00054       public:
00055       virtual ~DistanceFunction() {
00056       }
00057         /* INTERFACE FOR DERIVED CLASSES */
00062         virtual Ret operator()(const Arg &x) = 0;
00063 
00068         virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); };        
00069 
00074         virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00075         /*************************************************/
00076     };
00077 
00085   class FuncPairContour1sPoint : public DistanceFunction<double(double)> {
00086     public:
00092       FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp() {}
00093 
00094       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00095       double operator()(const double &alpha) {
00096         fmatvec::Vec3 Wd = computeWrD(alpha);
00097         fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00098         return Wt.T()*Wd;
00099       }
00100 
00101       fmatvec::Vec3 computeWrD(const double &alpha) {
00102         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00103         cp.getLagrangeParameterPosition()(0) = alpha;
00104         contour->computeRootFunctionPosition(cp);
00105         contour->computeRootFunctionFirstTangent(cp);
00106         //}
00107         return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00108       }
00109       /*************************************************/
00110 
00111     private:
00115       Contour1s *contour;
00116       Point *point;
00117 
00121       ContourPointData cp;
00122   };
00123 
00131   class FuncPairContour1sCircleHollow : public DistanceFunction<double(double)> {
00132     public:
00138       FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00139 
00140       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00141       double operator()(const double &alpha) {
00142         fmatvec::Vec3 Wd = computeWrD(alpha);
00143         return circle->getReferenceOrientation().col(2).T()*Wd;
00144       }
00145 
00146       fmatvec::Vec3 computeWrD(const double &alpha) {
00147         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00148         cp.getLagrangeParameterPosition()(0) = alpha;
00149         contour->computeRootFunctionPosition(cp);
00150         //}
00151         return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00152       }
00153       /*************************************************/
00154 
00155     private:
00159       Contour1s *contour;      
00160       CircleHollow *circle;
00161 
00165       ContourPointData cp;
00166   };
00167 
00168 
00174   class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV(fmatvec::VecV)> {
00175     public:
00181       FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00182 
00183       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00184       fmatvec::VecV operator()(const fmatvec::VecV &alpha) {
00185         fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00186         fmatvec::Vec3 WrOC[2];
00187         WrOC[0] = point->getFrame()->getPosition();
00188         WrOC[1] = contour->computeWrOC(alpha);
00189         return Wt.T() * ( WrOC[1] - WrOC[0] ); 
00190       }
00191 
00192       fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00193         return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00194       }
00195       /*************************************************/
00196 
00197     private:
00201       ContourInterpolation *contour;
00202       Point *point;
00203   };
00204 
00210   class FuncPairConeSectionCircle : public DistanceFunction<double(double)> {
00211     public:
00219       FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00220 
00228       FuncPairConeSectionCircle(double R_,double a_,double b_,bool sec_IN_ci_) : R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {}
00229 
00230       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00231       virtual double operator()(const double &phi) = 0;
00232       double operator[](const double &phi);
00233       virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00234       /*************************************************/
00235 
00236       /* GETTER / SETTER */
00237       void setDiffVec(fmatvec::Vec3 d_);
00238 
00239       void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00240       /*************************************************/
00241 
00242     protected:
00246       double R, a, b;
00247 
00251       bool sec_IN_ci;
00252 
00256       fmatvec::Vec3 b1, b2;
00257 
00261       fmatvec::Vec3 d;  
00262   };
00263 
00264   inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00265   inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00266   inline double FuncPairConeSectionCircle::operator[](const double &phi) { if(sec_IN_ci) return R - nrm2(computeWrD(phi)); else return nrm2(computeWrD(phi)) - R; }
00267 
00274   class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00275     public:
00283       FuncPairEllipseCircle(double R_,double a_,double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00284 
00292       FuncPairEllipseCircle(double R_,double a_,double b_,bool el_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,el_IN_ci_) {}
00293 
00294       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00295       double operator()(const double &phi);
00296       fmatvec::Vec3 computeWrD(const double &phi);
00297       /*************************************************/
00298 
00299       /* GETTER / SETTER */
00300       void setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_);
00301       /*************************************************/
00302   };
00303 
00304   inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_) {setSectionCOS(b1e_,b2e_);}
00305   inline double FuncPairEllipseCircle::operator()(const double &phi) { 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); }
00306   inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) { return d + b1*a*cos(phi) + b2*b*sin(phi); }
00307 
00313   class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00314     public:
00322       FuncPairHyperbolaCircle(double R_, double a_, double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00323 
00331       FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,hy_IN_ci_) {}
00332 
00333       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00334       double operator()(const double &phi);
00335       fmatvec::Vec3 computeWrD(const double &phi);
00336       /*************************************************/
00337   };
00338 
00339   inline double FuncPairHyperbolaCircle::operator()(const double &phi) { 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); }
00340   inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) { return d + b1*a*cosh(phi) + b2*b*sinh(phi); }
00341 
00347   class JacobianPairConeSectionCircle : public fmatvec::Function<double(double)> {
00348     public:
00354       JacobianPairConeSectionCircle(double a_, double b_) : a(a_), b(b_) {}
00355 
00356       /* GETTER / SETTER */
00357       void setDiffVec(fmatvec::Vec3 d_);
00358       void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00359       /*************************************************/
00360 
00361     protected:
00365       double a, b;
00366 
00370       fmatvec::Vec3 b1, b2;
00371 
00375       fmatvec::Vec3 d;
00376   };
00377 
00378   inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00379   inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00380 
00386   class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00387     public:
00393       JacobianPairEllipseCircle(double a_, double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00394 
00395       /* INHERITED INTERFACE OF FUNCTION */
00396       double operator()(const double &phi);
00397       /*************************************************/
00398   };
00399 
00400   inline double JacobianPairEllipseCircle::operator()(const double &phi) { 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)); }
00401 
00407   class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00408     public:
00414       JacobianPairHyperbolaCircle(double a_,double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00415 
00416       /* INHERITED INTERFACE OF FUNCTION */
00417       double operator()(const double &phi);
00418       /*************************************************/
00419   };
00420 
00421   inline double JacobianPairHyperbolaCircle::operator()(const double &phi) { 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));}
00422 
00429   class FuncPairContour1sLine : public DistanceFunction<double(double)> {
00430     public:
00436       FuncPairContour1sLine(Line* line_, Contour1s *contour_) : contour(contour_), line(line_) {}
00437 
00438       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00439       virtual double operator()(const double &s) {
00440         throw MBSimError("ERROR (FuncPairContour1sLine::operator): Not implemented!");
00441         //fmatvec::Vec WtC = (contour->computeWt(s)).col(0);
00442         //fmatvec::Vec WnL = line->computeWn();
00443         //return trans(WtC)*WnL;
00444       }
00445 
00446       virtual fmatvec::Vec3 computeWrD(const double &s) {
00447         throw MBSimError("ERROR (FuncPairContour1sLine::computeWrD): Not implemented!");
00448         //fmatvec::Vec WrOCContour =  contour->computeWrOC(s);
00449         //fmatvec::Vec Wn = contour->computeWn(s);
00450         //double g =trans(Wn)*(WrOCContour-line->getFrame()->getPosition()); 
00451         //return Wn*g;
00452       }
00453 
00454       virtual double operator[](const double &s) {
00455         return nrm2(computeWrD(s));
00456       }
00457       /*************************************************/
00458 
00459     private:
00460       Contour1s *contour;
00461       Line *line;
00462   };
00463 
00469   class FuncPairContour1sCircleSolid : public DistanceFunction<double(double)> {
00470     public:
00476       FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00477 
00478       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00479       double operator()(const double &alpha) {
00480         cp.getLagrangeParameterPosition()(0) = alpha;
00481         fmatvec::Vec3 Wd = computeWrD(alpha);
00482         fmatvec::Vec3 Wt = contour->computeTangent(cp);
00483         return Wt.T()*Wd;
00484       }
00485 
00486       fmatvec::Vec3 computeWrD(const double &alpha) {
00487         cp.getLagrangeParameterPosition()(0) = alpha;
00488         contour->computeRootFunctionPosition(cp);
00489         contour->computeRootFunctionFirstTangent(cp);
00490         contour->computeRootFunctionNormal(cp);
00491         WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius()*cp.getFrameOfReference().getOrientation().col(0);
00492         WrOC[1] = cp.getFrameOfReference().getPosition();
00493         return WrOC[1] - WrOC[0];
00494       }
00495       /***************************************************/
00496 
00497     private:
00501       Contour1s *contour;
00502       CircleSolid *circle;
00503 
00507       ContourPointData cp;
00508 
00512       fmatvec::Vec3 WrOC[2];
00513   };
00514 
00525   class Contact1sSearch {
00526     public:
00533       Contact1sSearch(DistanceFunction<double(double)> *func_) : func(func_), jac(0), s0(0.), searchAll(false) {}
00534 
00541       Contact1sSearch(DistanceFunction<double(double)> *func_,fmatvec::Function<double(double)> *jac_) : func(func_), jac(jac_), s0(0.), searchAll(false) {}
00542 
00543       /* GETTER / SETTER */
00544       void setInitialValue(const double &s0_ ) { s0=s0_; }
00545       void setNodes(const fmatvec::Vec &nodes_) {nodes=nodes_;}
00546       void setSearchAll(bool searchAll_) { searchAll=searchAll_; }
00547       /*************************************************/
00548 
00555       void setEqualSpacing(const int &n, const double &x0, const double &dx);
00556 
00561       double slv();
00566       fmatvec::Mat slvAll();
00567 
00568 
00569     private:
00573       DistanceFunction<double(double)> *func;
00574 
00578       fmatvec::Function<double(double)> *jac;      
00579 
00583       double s0;
00584 
00588       fmatvec::Vec nodes;
00589 
00593       bool searchAll;
00594   };
00595 
00596 }
00597 
00598 #endif /* FUNCTIONS_CONTACT_H_ */
00599