MBSim

Project Page
functions_contact.h
1 /* Copyright (C) 2004-2010 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  * rzander@users.berlios.de
19  */
20 
21 #ifndef FUNCTIONS_CONTACT_H_
22 #define FUNCTIONS_CONTACT_H_
23 
24 #include <mbsim/contour.h>
25 #include <mbsim/contours/contour1s.h>
26 #include <mbsim/contours/point.h>
27 #include "mbsim/contours/line.h"
28 #include <mbsim/contours/circle_solid.h>
29 #include "mbsim/contours/circle_hollow.h"
30 #include "mbsim/contours/circle.h"
31 #include "mbsim/contours/frustum2d.h"
32 #include "mbsim/contours/edge.h"
33 #include "mbsim/contours/face.h"
34 #include "mbsim/contours/frustum.h"
35 #include "mbsim/contours/plane.h"
36 #include "mbsim/contours/planewithfrustum.h"
37 #include "mbsim/contours/contour_interpolation.h"
38 #include "mbsim/contours/contour_quad.h"
39 #include "mbsim/contours/cuboid.h"
40 #include "mbsim/contours/compound_contour.h"
41 #include "mbsim/utils/function.h"
42 #include "mbsim/mbsim_event.h"
43 
44 namespace MBSim {
45 
51  template<class Ret, class Arg>
52  class DistanceFunction : public Function1<Ret,Arg> {
53  public:
54  /* INTERFACE FOR DERIVED CLASSES */
59  virtual Ret operator()(const Arg& x, const void * =NULL) = 0;
60 
65  virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); };
66 
71  virtual fmatvec::Vec computeWrD(const Arg& x) = 0;
72  /*************************************************/
73  };
74 
82  class FuncPairContour1sPoint : public DistanceFunction<double,double> {
83  public:
89  FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp(fmatvec::Vec(1,fmatvec::INIT,0.)) {}
90 
91  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
92  double operator()(const double &alpha, const void * =NULL) {
93  fmatvec::Vec Wd = computeWrD(alpha);
94  fmatvec::Vec Wt = cp.getFrameOfReference().getOrientation().col(1);
95  return Wt.T()*Wd;
96  }
97 
98  fmatvec::Vec computeWrD(const double &alpha) {
99  //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
100  cp.getLagrangeParameterPosition()(0) = alpha;
102  contour->computeRootFunctionFirstTangent(cp);
103  //}
104  return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
105  }
106  /*************************************************/
107 
108  private:
113  Point *point;
114 
119  };
120 
128  class FuncPairContour1sCircleHollow : public DistanceFunction<double,double> {
129  public:
135  FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
136 
137  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
138  double operator()(const double &alpha, const void * =NULL) {
139  fmatvec::Vec Wd = computeWrD(alpha);
140  return circle->getReferenceOrientation().col(2).T()*Wd;
141  }
142 
143  fmatvec::Vec computeWrD(const double &alpha) {
144  //if(fabs(alpha-cp.getLagrangeParameterPosition()(0))>epsroot()) { TODO this is not working in all cases
145  cp.getLagrangeParameterPosition()(0) = alpha;
147  //}
148  return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
149  }
150  /*************************************************/
151 
152  private:
157  CircleHollow *circle;
158 
163  };
164 
165 
171  class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::Vec,fmatvec::Vec> {
172  public:
178  FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
179 
180  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
181  fmatvec::Vec operator()(const fmatvec::Vec &alpha, const void * =NULL) {
182  fmatvec::Mat Wt = contour->computeWt(alpha);
183  fmatvec::Vec WrOC[2];
184  WrOC[0] = point->getFrame()->getPosition();
185  WrOC[1] = contour->computeWrOC(alpha);
186  return Wt.T() * ( WrOC[1] - WrOC[0] );
187  }
188 
189  fmatvec::Vec computeWrD(const fmatvec::Vec &alpha) {
190  return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
191  }
192  /*************************************************/
193 
194  private:
199  Point *point;
200  };
201 
207  class FuncPairConeSectionCircle : public DistanceFunction<double,double> {
208  public:
216  FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
217 
225  FuncPairConeSectionCircle(double R_,double a_,double b_,bool sec_IN_ci_) : R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {}
226 
227  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
228  virtual double operator()(const double &phi, const void * =NULL) = 0;
229  double operator[](const double &phi);
230  virtual fmatvec::Vec computeWrD(const double &phi) = 0;
231  /*************************************************/
232 
233  /* GETTER / SETTER */
234  void setDiffVec(fmatvec::Vec d_);
235 
236  void setSectionCOS(fmatvec::Vec b1_,fmatvec::Vec b2_);
237  /*************************************************/
238 
239  protected:
243  double R, a, b;
244 
248  bool sec_IN_ci;
249 
253  fmatvec::Vec b1, b2;
254 
258  fmatvec::Vec d;
259  };
260 
261  inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec d_) { d=d_; }
262  inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec b1_,fmatvec::Vec b2_) { b1=b1_; b2=b2_; }
263  inline double FuncPairConeSectionCircle::operator[](const double &phi) { if(sec_IN_ci) return R - nrm2(computeWrD(phi)); else return nrm2(computeWrD(phi)) - R; }
264 
272  public:
280  FuncPairEllipseCircle(double R_,double a_,double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
281 
289  FuncPairEllipseCircle(double R_,double a_,double b_,bool el_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,el_IN_ci_) {}
290 
291  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
292  double operator()(const double &phi, const void * =NULL);
293  fmatvec::Vec computeWrD(const double &phi);
294  /*************************************************/
295 
296  /* GETTER / SETTER */
297  void setEllipseCOS(fmatvec::Vec b1e_,fmatvec::Vec b2e_);
298  /*************************************************/
299  };
300 
301  inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec b1e_,fmatvec::Vec b2e_) {setSectionCOS(b1e_,b2e_);}
302  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); }
303  inline fmatvec::Vec FuncPairEllipseCircle::computeWrD(const double &phi) { return d + b1*a*cos(phi) + b2*b*sin(phi); }
304 
311  public:
319  FuncPairHyperbolaCircle(double R_, double a_, double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
320 
328  FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,hy_IN_ci_) {}
329 
330  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
331  double operator()(const double &phi, const void * =NULL);
332  fmatvec::Vec computeWrD(const double &phi);
333  /*************************************************/
334  };
335 
336  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); }
337  inline fmatvec::Vec FuncPairHyperbolaCircle::computeWrD(const double &phi) { return d + b1*a*cosh(phi) + b2*b*sinh(phi); }
338 
344  class JacobianPairConeSectionCircle : public Function1<double,double> {
345  public:
351  JacobianPairConeSectionCircle(double a_, double b_) : a(a_), b(b_) {}
352 
353  /* GETTER / SETTER */
354  void setDiffVec(fmatvec::Vec d_);
355  void setSectionCOS(fmatvec::Vec b1_,fmatvec::Vec b2_);
356  /*************************************************/
357 
358  protected:
362  double a, b;
363 
367  fmatvec::Vec b1, b2;
368 
372  fmatvec::Vec d;
373  };
374 
375  inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec d_) { d=d_; }
376  inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec b1_,fmatvec::Vec b2_) { b1=b1_; b2=b2_; }
377 
384  public:
391 
392  /* INHERITED INTERFACE OF FUNCTION */
393  double operator()(const double &phi, const void * =NULL);
394  /*************************************************/
395  };
396 
397  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)); }
398 
405  public:
412 
413  /* INHERITED INTERFACE OF FUNCTION */
414  double operator()(const double &phi, const void * =NULL);
415  /*************************************************/
416  };
417 
418  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));}
419 
426  class FuncPairContour1sLine : public DistanceFunction<double,double> {
427  public:
433  FuncPairContour1sLine(Line* line_, Contour1s *contour_) : contour(contour_), line(line_) {}
434 
435  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
436  virtual double operator()(const double &s, const void * =NULL) {
437  throw MBSimError("ERROR (FuncPairContour1sLine::operator): Not implemented!");
438  //fmatvec::Vec WtC = (contour->computeWt(s)).col(0);
439  //fmatvec::Vec WnL = line->computeWn();
440  //return trans(WtC)*WnL;
441  }
442 
443  virtual fmatvec::Vec computeWrD(const double &s) {
444  throw MBSimError("ERROR (FuncPairContour1sLine::computeWrD): Not implemented!");
445  //fmatvec::Vec WrOCContour = contour->computeWrOC(s);
446  //fmatvec::Vec Wn = contour->computeWn(s);
447  //double g =trans(Wn)*(WrOCContour-line->getFrame()->getPosition());
448  //return Wn*g;
449  }
450 
451  virtual double operator[](const double &s) {
452  return nrm2(computeWrD(s));
453  }
454  /*************************************************/
455 
456  private:
457  Contour1s *contour;
458  Line *line;
459  };
460 
466  class FuncPairContour1sCircleSolid : public DistanceFunction<double,double> {
467  public:
473  FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
474 
475  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
476  double operator()(const double &alpha, const void * =NULL) {
477  cp.getLagrangeParameterPosition() = fmatvec::Vec(1, fmatvec::INIT, alpha);
478  fmatvec::Vec Wd = computeWrD(alpha);
479  fmatvec::Vec Wt = contour->computeTangent(cp);
480  return Wt.T()*Wd;
481  }
482 
483  fmatvec::Vec computeWrD(const double &alpha) {
484  cp.getLagrangeParameterPosition() = fmatvec::Vec(1, fmatvec::INIT, alpha);
486  contour->computeRootFunctionFirstTangent(cp);
487  contour->computeRootFunctionNormal(cp);
488  WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius()*cp.getFrameOfReference().getOrientation().col(0);
489  WrOC[1] = cp.getFrameOfReference().getPosition();
490  return WrOC[1] - WrOC[0];
491  }
492  /***************************************************/
493 
494  private:
499  CircleSolid *circle;
500 
505 
509  fmatvec::Vec WrOC[2];
510  };
511 
523  public:
531 
539 
540  /* GETTER / SETTER */
541  void setInitialValue(const double &s0_ ) { s0=s0_; }
542  void setNodes(const fmatvec::Vec &nodes_) {nodes=nodes_;}
543  void setSearchAll(bool searchAll_) { searchAll=searchAll_; }
544  /*************************************************/
545 
552  void setEqualSpacing(const int &n, const double &x0, const double &dx);
553 
558  double slv();
563  fmatvec::Mat slvAll();
564 
565 
566  private:
571 
576 
580  double s0;
581 
585  fmatvec::Vec nodes;
586 
590  bool searchAll;
591  };
592 
593 }
594 
595 #endif /* FUNCTIONS_CONTACT_H_ */
596