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/face.h"
00034 #include "mbsim/contours/frustum.h"
00035 #include "mbsim/contours/plane.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/utils/function.h"
00042 #include "mbsim/mbsim_event.h"
00043 
00044 namespace MBSim {
00045 
00051   template<class Ret, class Arg>
00052     class DistanceFunction : public Function1<Ret,Arg> {
00053       public:
00054         /* INTERFACE FOR DERIVED CLASSES */
00059         virtual Ret operator()(const Arg& x, const void * =NULL) = 0;
00060 
00065         virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); };        
00066 
00071         virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00072         /*************************************************/
00073     };
00074 
00082   class FuncPairContour1sPoint : public DistanceFunction<double,double> {
00083     public:
00089       FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp(fmatvec::VecV(1)) {}
00090 
00091       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00092       double operator()(const double &alpha, const void * =NULL) {
00093         fmatvec::Vec3 Wd = computeWrD(alpha);
00094         fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00095         return Wt.T()*Wd;
00096       }
00097 
00098       fmatvec::Vec3 computeWrD(const double &alpha) {
00099         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00100         cp.getLagrangeParameterPosition()(0) = alpha;
00101         contour->computeRootFunctionPosition(cp);
00102         contour->computeRootFunctionFirstTangent(cp);
00103         //}
00104         return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00105       }
00106       /*************************************************/
00107 
00108     private:
00112       Contour1s *contour;
00113       Point *point;
00114 
00118       ContourPointData cp;
00119   };
00120 
00128   class FuncPairContour1sCircleHollow : public DistanceFunction<double,double> {
00129     public:
00135       FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00136 
00137       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00138       double operator()(const double &alpha, const void * =NULL) {
00139         fmatvec::Vec3 Wd = computeWrD(alpha);
00140         return circle->getReferenceOrientation().col(2).T()*Wd;
00141       }
00142 
00143       fmatvec::Vec3 computeWrD(const double &alpha) {
00144         //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
00145         cp.getLagrangeParameterPosition()(0) = alpha;
00146         contour->computeRootFunctionPosition(cp);
00147         //}
00148         return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00149       }
00150       /*************************************************/
00151 
00152     private:
00156       Contour1s *contour;      
00157       CircleHollow *circle;
00158 
00162       ContourPointData cp;
00163   };
00164 
00165 
00171   class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV,fmatvec::VecV> {
00172     public:
00178       FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00179 
00180       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00181       fmatvec::VecV operator()(const fmatvec::VecV &alpha, const void * =NULL) {
00182         fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00183         fmatvec::Vec3 WrOC[2];
00184         WrOC[0] = point->getFrame()->getPosition();
00185         WrOC[1] = contour->computeWrOC(alpha);
00186         return Wt.T() * ( WrOC[1] - WrOC[0] ); 
00187       }
00188 
00189       fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00190         return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00191       }
00192       /*************************************************/
00193 
00194     private:
00198       ContourInterpolation *contour;
00199       Point *point;
00200   };
00201 
00207   class FuncPairConeSectionCircle : public DistanceFunction<double,double> {
00208     public:
00216       FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00217 
00225       FuncPairConeSectionCircle(double R_,double a_,double b_,bool sec_IN_ci_) : R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {}
00226 
00227       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00228       virtual double operator()(const double &phi, const void * =NULL) = 0;
00229       double operator[](const double &phi);
00230       virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00231       /*************************************************/
00232 
00233       /* GETTER / SETTER */
00234       void setDiffVec(fmatvec::Vec3 d_);
00235 
00236       void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00237       /*************************************************/
00238 
00239     protected:
00243       double R, a, b;
00244 
00248       bool sec_IN_ci;
00249 
00253       fmatvec::Vec3 b1, b2;
00254 
00258       fmatvec::Vec3 d;  
00259   };
00260 
00261   inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00262   inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00263   inline double FuncPairConeSectionCircle::operator[](const double &phi) { if(sec_IN_ci) return R - nrm2(computeWrD(phi)); else return nrm2(computeWrD(phi)) - R; }
00264 
00271   class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00272     public:
00280       FuncPairEllipseCircle(double R_,double a_,double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00281 
00289       FuncPairEllipseCircle(double R_,double a_,double b_,bool el_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,el_IN_ci_) {}
00290 
00291       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00292       double operator()(const double &phi, const void * =NULL);
00293       fmatvec::Vec3 computeWrD(const double &phi);
00294       /*************************************************/
00295 
00296       /* GETTER / SETTER */
00297       void setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_);
00298       /*************************************************/
00299   };
00300 
00301   inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_) {setSectionCOS(b1e_,b2e_);}
00302   inline double FuncPairEllipseCircle::operator()(const double &phi, const void *) { 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); }
00303   inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) { return d + b1*a*cos(phi) + b2*b*sin(phi); }
00304 
00310   class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00311     public:
00319       FuncPairHyperbolaCircle(double R_, double a_, double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00320 
00328       FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,hy_IN_ci_) {}
00329 
00330       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00331       double operator()(const double &phi, const void * =NULL);
00332       fmatvec::Vec3 computeWrD(const double &phi);
00333       /*************************************************/
00334   };
00335 
00336   inline double FuncPairHyperbolaCircle::operator()(const double &phi, const void *) { 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); }
00337   inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) { return d + b1*a*cosh(phi) + b2*b*sinh(phi); }
00338 
00344   class JacobianPairConeSectionCircle : public Function1<double,double> {
00345     public:
00351       JacobianPairConeSectionCircle(double a_, double b_) : a(a_), b(b_) {}
00352 
00353       /* GETTER / SETTER */
00354       void setDiffVec(fmatvec::Vec3 d_);
00355       void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00356       /*************************************************/
00357 
00358     protected:
00362       double a, b;
00363 
00367       fmatvec::Vec3 b1, b2;
00368 
00372       fmatvec::Vec3 d;
00373   };
00374 
00375   inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00376   inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00377 
00383   class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00384     public:
00390       JacobianPairEllipseCircle(double a_, double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00391 
00392       /* INHERITED INTERFACE OF FUNCTION */
00393       double operator()(const double &phi, const void * =NULL);
00394       /*************************************************/
00395   };
00396 
00397   inline double JacobianPairEllipseCircle::operator()(const double &phi, const void *) { 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)); }
00398 
00404   class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00405     public:
00411       JacobianPairHyperbolaCircle(double a_,double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00412 
00413       /* INHERITED INTERFACE OF FUNCTION */
00414       double operator()(const double &phi, const void * =NULL);
00415       /*************************************************/
00416   };
00417 
00418   inline double JacobianPairHyperbolaCircle::operator()(const double &phi, const void *) { 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));}
00419 
00426   class FuncPairContour1sLine : public DistanceFunction<double,double> {
00427     public:
00433       FuncPairContour1sLine(Line* line_, Contour1s *contour_) : contour(contour_), line(line_) {}
00434 
00435       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00436       virtual double operator()(const double &s, const void * =NULL) {
00437         throw MBSimError("ERROR (FuncPairContour1sLine::operator): Not implemented!");
00438         //fmatvec::Vec WtC = (contour->computeWt(s)).col(0);
00439         //fmatvec::Vec WnL = line->computeWn();
00440         //return trans(WtC)*WnL;
00441       }
00442 
00443       virtual fmatvec::Vec3 computeWrD(const double &s) {
00444         throw MBSimError("ERROR (FuncPairContour1sLine::computeWrD): Not implemented!");
00445         //fmatvec::Vec WrOCContour =  contour->computeWrOC(s);
00446         //fmatvec::Vec Wn = contour->computeWn(s);
00447         //double g =trans(Wn)*(WrOCContour-line->getFrame()->getPosition()); 
00448         //return Wn*g;
00449       }
00450 
00451       virtual double operator[](const double &s) {
00452         return nrm2(computeWrD(s));
00453       }
00454       /*************************************************/
00455 
00456     private:
00457       Contour1s *contour;
00458       Line *line;
00459   };
00460 
00466   class FuncPairContour1sCircleSolid : public DistanceFunction<double,double> {
00467     public:
00473       FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00474 
00475       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00476       double operator()(const double &alpha, const void * =NULL) {
00477         cp.getLagrangeParameterPosition() = fmatvec::VecV(1,fmatvec::INIT,alpha);
00478         fmatvec::Vec3 Wd = computeWrD(alpha);
00479         fmatvec::Vec3 Wt = contour->computeTangent(cp);
00480         return Wt.T()*Wd;
00481       }
00482 
00483       fmatvec::Vec3 computeWrD(const double &alpha) {
00484         cp.getLagrangeParameterPosition() = fmatvec::VecV(1,fmatvec::INIT,alpha);
00485         contour->computeRootFunctionPosition(cp);
00486         contour->computeRootFunctionFirstTangent(cp);
00487         contour->computeRootFunctionNormal(cp);
00488         WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius()*cp.getFrameOfReference().getOrientation().col(0);
00489         WrOC[1] = cp.getFrameOfReference().getPosition();
00490         return WrOC[1] - WrOC[0];
00491       }
00492       /***************************************************/
00493 
00494     private:
00498       Contour1s *contour;
00499       CircleSolid *circle;
00500 
00504       ContourPointData cp;
00505 
00509       fmatvec::Vec3 WrOC[2];
00510   };
00511 
00522   class Contact1sSearch {
00523     public:
00530       Contact1sSearch(DistanceFunction<double,double> *func_) : func(func_), jac(0), s0(0.), searchAll(false) {}
00531 
00538       Contact1sSearch(DistanceFunction<double,double> *func_,Function1<double,double> *jac_) : func(func_), jac(jac_), s0(0.), searchAll(false) {}
00539 
00540       /* GETTER / SETTER */
00541       void setInitialValue(const double &s0_ ) { s0=s0_; }
00542       void setNodes(const fmatvec::Vec &nodes_) {nodes=nodes_;}
00543       void setSearchAll(bool searchAll_) { searchAll=searchAll_; }
00544       /*************************************************/
00545 
00552       void setEqualSpacing(const int &n, const double &x0, const double &dx);
00553 
00558       double slv();
00563       fmatvec::Mat slvAll();
00564 
00565 
00566     private:
00570       DistanceFunction<double,double> *func;
00571 
00575       Function1<double,double> *jac;      
00576 
00580       double s0;
00581 
00585       fmatvec::Vec nodes;
00586 
00590       bool searchAll;
00591   };
00592 
00593 }
00594 
00595 #endif /* FUNCTIONS_CONTACT_H_ */
00596