MBSim

Project Page
constitutive_laws.h
1 /* Copyright (C) 2004-2009 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  */
19 
20 #ifndef _CONSTITUTIVE_LAWS_H_
21 #define _CONSTITUTIVE_LAWS_H_
22 
23 #include "fmatvec.h"
24 #include "mbsim/utils/function.h"
25 
26 namespace MBSim {
27 
34  public:
38  GeneralizedForceLaw() : forceFunc(NULL) {};
39 
40  GeneralizedForceLaw(Function2<double,double,double> *forceFunc_) : forceFunc(forceFunc_) {};
41 
45  virtual ~GeneralizedForceLaw() { if(forceFunc) delete forceFunc; forceFunc = NULL; };
46 
47  /* INTERFACE FOR DERIVED CLASSES */
53  virtual bool isActive(double g, double gTol) { return true; }
54  virtual bool remainsActive(double s, double sTol) { return true; }
55  virtual double project(double la, double gdn, double r, double laMin=0) { return 0; }
56  virtual fmatvec::Vec diff(double la, double gdn, double r, double laMin=0) { return fmatvec::Vec(2, fmatvec::INIT, 0); }
57  virtual double solve(double G, double gdn) { return 0; }
58 
66  virtual bool isFulfilled(double la, double gdn, double tolla, double tolgd, double laMin=0) { return true; }
67 
71  virtual bool isSetValued() const = 0;
72 
77  virtual void initializeUsingXML(TiXmlElement *element) {}
78  /***************************************************/
79 
87  double operator()(double g, double gd, const void * additional=NULL) { assert(forceFunc); return (*forceFunc)(g,gd,additional); }
88 
94  void setForceFunction(Function2<double,double,double> *forceFunc_) { forceFunc=forceFunc_; }
95 
96  protected:
98  };
99 
106  public:
111 
115  virtual ~UnilateralConstraint() {};
116 
117  /* INHERITED INTERFACE */
118  virtual bool isActive(double g, double gTol) { return g<=gTol; }
119  virtual bool remainsActive(double gd, double gdTol) { return gd<=gdTol; }
120  virtual double project(double la, double gdn, double r, double laMin=0);
121  virtual fmatvec::Vec diff(double la, double gdn, double r, double laMin=0);
122  virtual double solve(double G, double gdn);
123  virtual bool isFulfilled(double la, double gdn, double tolla, double tolgd, double laMin=0);
124  virtual bool isSetValued() const { return true; }
125  /***************************************************/
126 
127  bool remainsClosed(double s, double sTol) { return s<=sTol; } // s = gd/gdd
128  };
129 
137  public:
142 
146  virtual ~BilateralConstraint() {};
147 
148  /* INHERITED INTERFACE */
149  virtual bool isActive(double g, double gTol) { return true; }
150  virtual double project(double la, double gdn, double r, double laMin=0);
151  virtual fmatvec::Vec diff(double la, double gdn, double r, double laMin=0);
152  virtual double solve(double G, double gdn);
153  virtual bool isFulfilled(double la, double gdn, double tolla, double tolgd, double laMin=0);
154  virtual bool isSetValued() const { return true; }
155  /***************************************************/
156 
157  bool remainsClosed(double s, double sTol) { return true; }
158  };
159 
166  public:
171 
175  virtual ~GeneralizedImpactLaw() {};
176 
177  /* INTERFACE FOR DERIVED CLASSES */
178  virtual double project(double la, double gdn, double gda, double r, double laMin=0) = 0;
179  virtual fmatvec::Vec diff(double la, double gdn, double gda, double r, double laMin=0) = 0;
180  virtual double solve(double G, double gdn, double gda) = 0;
181  virtual bool isFulfilled(double la, double gdn, double gda, double tolla, double tolgd, double laMin=0) = 0;
182  virtual void initializeUsingXML(TiXmlElement *element) {}
183  /***************************************************/
184  };
185 
193  public:
197  UnilateralNewtonImpact() : epsilon(0), gd_limit(1e-2) {};
198 
202  UnilateralNewtonImpact(double epsilon_) : epsilon(epsilon_), gd_limit(1e-2) {};
203 
207  UnilateralNewtonImpact(double epsilon_, double gd_limit_) : epsilon(epsilon_), gd_limit(gd_limit_) {};
208 
213 
214  /* INHERITED INTERFACE */
215  virtual double project(double la, double gdn, double gda, double r, double laMin=0);
216  virtual fmatvec::Vec diff(double la, double gdn, double gda, double r, double laMin=0);
217  virtual double solve(double G, double gdn, double gda);
218  virtual bool isFulfilled(double la, double gdn, double gda, double tolla, double tolgd, double laMin=0);
219  virtual void initializeUsingXML(TiXmlElement *element);
220  /***************************************************/
221 
222  protected:
223  double epsilon, gd_limit;
224  };
225 
232  public:
237 
241  virtual ~BilateralImpact() {};
242 
243  /* INHERITED INTERFACE */
244  virtual double project(double la, double gdn, double gda, double r, double laMin=0);
245  virtual fmatvec::Vec diff(double la, double gdn, double gda, double r, double laMin=0);
246  virtual double solve(double G, double gdn, double gda);
247  virtual bool isFulfilled(double la, double gdn, double gda, double tolla, double tolgd, double laMin=0);
248  /***************************************************/
249  };
250 
257  public:
261  FrictionForceLaw() : frictionForceFunc(NULL) {};
262 
263  FrictionForceLaw(Function2<fmatvec::Vec,fmatvec::Vec,double> *frictionForceFunc_) : frictionForceFunc(frictionForceFunc_) {};
264 
268  virtual ~FrictionForceLaw() { if(frictionForceFunc) delete frictionForceFunc; frictionForceFunc = NULL; };
269 
270  /* INTERFACE FOR DERIVED CLASSES */
271  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r) { return fmatvec::Vec(2); }
272  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r) { return fmatvec::Mat(2,2); }
273  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, double laN) { return fmatvec::Vec(2); }
274  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double tolla, double tolgd) { return true; }
275  virtual fmatvec::Vec dlaTdlaN(const fmatvec::Vec& gd, double laN) { return fmatvec::Vec(2); }
276  virtual int getFrictionDirections() = 0;
277  virtual bool isSticking(const fmatvec::Vec& s, double sTol) = 0;
278  virtual double getFrictionCoefficient(double gd) { return 0; }
279  virtual bool isSetValued() const = 0;
280  virtual void initializeUsingXML(TiXmlElement *element) {}
281  /***************************************************/
282 
283  fmatvec::Vec operator()(const fmatvec::Vec &gd, double laN) { assert(frictionForceFunc); return (*frictionForceFunc)(gd,laN); }
284 
290  void setFrictionForceFunction(Function2<fmatvec::Vec,fmatvec::Vec,double> *frictionForceFunc_) { frictionForceFunc=frictionForceFunc_; }
291 
292  protected:
294  };
295 
302  public:
306  PlanarCoulombFriction() : mu(0) {};
307 
311  PlanarCoulombFriction(double mu_) : mu(mu_) {};
312 
317 
318  /* INHERITED INTERFACE */
319  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
320  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
321  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, double laN);
322  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double tolla, double tolgd);
323  virtual fmatvec::Vec dlaTdlaN(const fmatvec::Vec& gd, double laN);
324  virtual int getFrictionDirections() { return 1; }
325  virtual bool isSticking(const fmatvec::Vec& s, double sTol) { return fabs(s(0)) <= sTol; }
326  virtual double getFrictionCoefficient(double gd) { return mu; }
327  virtual bool isSetValued() const { return true; }
328  virtual void initializeUsingXML(TiXmlElement *element);
329  /***************************************************/
330 
331  void setFrictionCoefficient(double mu_) { mu = mu_; }
332 
333  protected:
334  double mu;
335  };
336 
343  public:
348 
352  SpatialCoulombFriction(double mu_) : mu(mu_) {};
353 
358 
359  /* INHERITED INTERFACE */
360  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
361  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
362  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, double laN);
363  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double tolla, double tolgd);
364  virtual fmatvec::Vec dlaTdlaN(const fmatvec::Vec& gd, double laN);
365  virtual int getFrictionDirections() { return 2; }
366  virtual bool isSticking(const fmatvec::Vec& s, double sTol) { return nrm2(s(0,1)) <= sTol; }
367  virtual double getFrictionCoefficient(double gd) { return mu; }
368  virtual bool isSetValued() const { return true; }
369  virtual void initializeUsingXML(TiXmlElement *element);
370  /***************************************************/
371 
372  void setFrictionCoefficient(double mu_) { mu = mu_; }
373 
374  protected:
375  double mu;
376  };
377 
385  public:
390 
395 
399  virtual ~PlanarStribeckFriction() { if(fmu) delete fmu; fmu=0; }
400 
401  /* INHERITED INTERFACE */
402  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
403  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
404  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, double laN);
405  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double tolla, double tolgd);
406  virtual fmatvec::Vec dlaTdlaN(const fmatvec::Vec& gd, double laN);
407  virtual int getFrictionDirections() { return 1; }
408  virtual bool isSticking(const fmatvec::Vec& s, double sTol) { return fabs(s(0)) <= sTol; }
409  virtual double getFrictionCoefficient(double gd) { return (*fmu)(gd); }
410  virtual bool isSetValued() const { return true; }
411  /***************************************************/
412 
413  protected:
418  };
419 
427  public:
432 
437 
441  virtual ~SpatialStribeckFriction() { if(fmu) delete fmu; fmu=0; }
442 
443  /* INHERITED INTERFACE */
444  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
445  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double r);
446  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, double laN);
447  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, double laN, double tolla, double tolgd);
448  virtual fmatvec::Vec dlaTdlaN(const fmatvec::Vec& gd, double laN);
449  virtual int getFrictionDirections() { return 2; }
450  virtual bool isSticking(const fmatvec::Vec& s, double sTol) { return nrm2(s(0,1)) <= sTol; }
451  virtual double getFrictionCoefficient(double gd) { return (*fmu)(gd); }
452  virtual bool isSetValued() const { return true; }
453  /***************************************************/
454 
455  protected:
460  };
461 
468  public:
473 
477  virtual ~FrictionImpactLaw() {};
478 
479  /* INTERFACE FOR DERIVED CLASSES */
480  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r) = 0;
481  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r) = 0;
482  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN) = 0;
483  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double tolla, double tolgd) = 0;
484  virtual int isSticking(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double laTol, double gdTol) = 0;
485  virtual int getFrictionDirections() = 0;
486  virtual void initializeUsingXML(TiXmlElement *element) {}
487  /***************************************************/
488  };
489 
496  public:
500  PlanarCoulombImpact() : mu(0) {};
501 
505  PlanarCoulombImpact(double mu_) : mu(mu_) {};
506 
510  virtual ~PlanarCoulombImpact() {}
511 
512  /* INHERITED INTERFACE */
513  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
514  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
515  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN);
516  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double tolla, double tolgd);
517  virtual int isSticking(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double laTol, double gdTol);
518  virtual int getFrictionDirections() { return 1; }
519  virtual void initializeUsingXML(TiXmlElement *element);
520  /***************************************************/
521 
522  void setFrictionCoefficient(double mu_) { mu = mu_; }
523  double getFrictionCoefficient(double gd) { return mu; }
524 
525  protected:
526  double mu;
527  };
528 
535  public:
539  SpatialCoulombImpact() : mu(0) {};
540 
544  SpatialCoulombImpact(double mu_) : mu(mu_) {};
545 
550 
551  /* INHERITED INTERFACE */
552  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
553  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
554  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN);
555  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double tolla, double tolgd);
556  virtual int isSticking(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double laTol, double gdTol);
557  virtual int getFrictionDirections() { return 2; }
558  virtual void initializeUsingXML(TiXmlElement *element);
559  /***************************************************/
560 
561  void setFrictionCoefficient(double mu_) { mu = mu_; }
562  double getFrictionCoefficient(double gd) { return mu; }
563 
564  protected:
565  double mu;
566  };
567 
575  public:
580 
585 
589  virtual ~PlanarStribeckImpact() { if(fmu) delete fmu; fmu=0; }
590 
591  /* INHERITED INTERFACE */
592  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
593  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
594  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN);
595  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double tolla, double tolgd);
596  virtual int isSticking(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double laTol, double gdTol);
597  virtual int getFrictionDirections() { return 1; }
598  /***************************************************/
599 
600  double getFrictionCoefficient(double gd) { return (*fmu)(gd); }
601 
602  protected:
607  };
608 
616  public:
621 
626 
630  virtual ~SpatialStribeckImpact() { if(fmu) delete fmu; fmu=0; }
631 
632  /* INHERITED INTERFACE */
633  virtual fmatvec::Vec project(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
634  virtual fmatvec::Mat diff(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double r);
635  virtual fmatvec::Vec solve(const fmatvec::SqrMat& G, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN);
636  virtual bool isFulfilled(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double tolla, double tolgd);
637  virtual int isSticking(const fmatvec::Vec& la, const fmatvec::Vec& gdn, const fmatvec::Vec& gda, double laN, double laTol, double gdTol);
638  virtual int getFrictionDirections() { return 2; }
639  /***************************************************/
640 
641  double getFrictionCoefficient(double gd) { return (*fmu)(gd); }
642 
643  protected:
648  };
649 
656  public:
661 
663 
668 
669  /* INHERITED INTERFACE */
670  virtual bool isActive(double g, double gTol) { return g<=gTol; }
671  virtual bool remainsActive(double s, double sTol) { return s<=sTol; }
672  virtual bool isSetValued() const { return false; }
673  /***************************************************/
674 
675  virtual void initializeUsingXML(TiXmlElement *element);
676  };
677 
684  public:
689 
691 
696 
697  /* INHERITED INTERFACE */
698  virtual bool isActive(double g, double gTol) { return true; }
699  virtual bool remainsActive(double s, double sTol) { return true; }
700  virtual bool isSetValued() const { return false; }
701  /***************************************************/
702 
703  virtual void initializeUsingXML(TiXmlElement *element);
704  };
705 
707  public:
709  RegularizedPlanarFriction(Function2<fmatvec::Vec,fmatvec::Vec,double> *frictionForceFunc_) : FrictionForceLaw(frictionForceFunc_) {};
710  virtual ~RegularizedPlanarFriction() {}
711  int getFrictionDirections() { return 1; }
712  bool isSticking(const fmatvec::Vec& s, double sTol) { return fabs(s(0)) <= sTol; }
713  bool isSetValued() const { return false; }
714  virtual void initializeUsingXML(TiXmlElement *element);
715  };
716 
718  public:
721  virtual ~RegularizedSpatialFriction() {}
722  int getFrictionDirections() { return 2; }
723  bool isSticking(const fmatvec::Vec& s, double sTol) { return nrm2(s(0,1)) <= sTol; }
724  bool isSetValued() const { return false; }
725  virtual void initializeUsingXML(TiXmlElement *element);
726  };
727 
728 }
729 
730 #endif /* _CONSTITUTIVE_LAWS_H_ */
731