MBSim

Project Page
contact.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 _CONTACT_H_
21 #define _CONTACT_H_
22 
23 #include <mbsim/link_mechanics.h>
24 #ifdef HAVE_OPENMBVCPPINTERFACE
25 namespace OpenMBV {
26  class Frame;
27  class Arrow;
28 }
29 #endif
30 
31 namespace MBSim {
32 
33  class ContactKinematics;
34  class GeneralizedForceLaw;
35  class GeneralizedImpactLaw;
36  class FrictionForceLaw;
37  class FrictionImpactLaw;
38  class ContourPointData;
39 
53  class Contact: public LinkMechanics {
54  public:
59  Contact(const std::string &name);
60 
64  virtual ~Contact();
65 
66  /* INHERITED INTERFACE OF LINKINTERFACE */
67  virtual void updatewb(double t, int i=0);
68  virtual void updateW(double t, int i=0);
69  virtual void updateV(double t, int i=0);
70  virtual void updateh(double t, int i=0);
71  virtual void updateg(double t);
72  virtual void updategd(double t);
73  virtual void updateStopVector(double t);
74  virtual void updateJacobians(double t, int j=0);
75  /***************************************************/
76 
77  /* INHERITED INTERFACE OF LINK */
78  virtual void updateWRef(const fmatvec::Mat &ref, int j=0);
79  virtual void updateVRef(const fmatvec::Mat &ref, int j=0);
80  virtual void updatehRef(const fmatvec::Vec &hRef, int j=0);
81  virtual void updatewbRef(const fmatvec::Vec &ref);
82  virtual void updatelaRef(const fmatvec::Vec& ref);
83  virtual void updategRef(const fmatvec::Vec& ref);
84  virtual void updategdRef(const fmatvec::Vec& ref);
85  virtual void updaterFactorRef(const fmatvec::Vec &ref);
86  virtual void updatesvRef(const fmatvec::Vec &ref);
87  virtual void updatejsvRef(const fmatvec::Vector<int> &ref);
88  virtual void calcxSize();
89  virtual void calclaSize(int j);
90  virtual void calcgSize(int j);
91  virtual void calcgdSize(int j);
92  virtual void calcrFactorSize(int j);
93  virtual void calcsvSize();
94  virtual void calcLinkStatusSize();
95  virtual void calcLinkStatusRegSize();
96  virtual void init(InitStage stage);
97  virtual bool isSetValued() const;
98  virtual void updateLinkStatus(double dt);
99  virtual void updateLinkStatusReg(double dt);
100  virtual bool isActive() const;
101  virtual bool gActiveChanged();
102  virtual void solveImpactsFixpointSingle(double dt);
103  virtual void solveConstraintsFixpointSingle();
104  virtual void solveImpactsGaussSeidel(double dt);
105  virtual void solveConstraintsGaussSeidel();
106  virtual void solveImpactsRootFinding(double dt);
107  virtual void solveConstraintsRootFinding();
108  virtual void jacobianConstraints();
109  virtual void jacobianImpacts();
110  virtual void updaterFactors();
111  virtual void checkConstraintsForTermination();
112  virtual void checkImpactsForTermination(double dt);
114  virtual void checkActive(int j);
115  virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_);
116  virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
117 
118  /***************************************************/
119 
120  /* INHERITED INTERFACE OF ELEMENT */
121  virtual std::string getType() const { return "Contact"; }
122  virtual void plot(double t, double dt = 1);
123  virtual void closePlot();
124  /***************************************************/
125 
126 #ifdef HAVE_OPENMBVCPPINTERFACE
127 
133  void enableOpenMBVContactPoints(double size=1.,bool enable=true) { openMBVContactFrameSize=size; openMBVContactFrameEnabled=enable; }
134 
140  void setOpenMBVNormalForceArrow(OpenMBV::Arrow *arrow) { contactArrow=arrow; }
141 
149  void setOpenMBVFrictionArrow(OpenMBV::Arrow *arrow) { frictionArrow=arrow; }
150 #endif
151 
152  /* GETTER / SETTER */
153  void setContactForceLaw(GeneralizedForceLaw *fcl_) { fcl = fcl_; }
154  GeneralizedForceLaw * getContactForceLaw() const {return fcl; }
155  void setContactImpactLaw(GeneralizedImpactLaw *fnil_) { fnil = fnil_; }
156  void setFrictionForceLaw(FrictionForceLaw *fdf_) { fdf = fdf_; }
157  void setFrictionImpactLaw(FrictionImpactLaw *ftil_) { ftil = ftil_; }
158  void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
159  ContactKinematics* getContactKinematics() const { return contactKinematics; }
160  /***************************************************/
161 
165  virtual int getFrictionDirections();
166 
171  void connect(Contour *contour1, Contour* contour2);
172 
173  void computeCurvatures(fmatvec::Vec & r) const;
174 
175  virtual void initializeUsingXML(TiXmlElement *element);
176 
177  void calccorrSize(int j);
178  void updatecorr(int j);
179  void updatecorrRef(const fmatvec::Vec& ref);
180 
181  void checkRoot();
182 
183  protected:
188 
193 
198 
203 
208 
212  std::vector<ContourPointData*> cpData;
213 
217  std::vector<unsigned int> gActive, gActive0;
218 
222  std::vector<unsigned int*> gdActive;
223 
227  std::vector<unsigned int*> gddActive;
228 
232  fmatvec::Index iT;
233 
237  fmatvec::Vec gdn, gdd;
238 
242  std::vector<fmatvec::Vec> gk, gdk, gdnk, gddk, lak, wbk, svk, rFactork;
243 
247  std::vector<fmatvec::Vector<int> > jsvk;
248 
252  std::vector<fmatvec::Mat*> fF;
253 
257  std::vector<fmatvec::Vec*> WF;
258 
262  std::vector<fmatvec::Mat*> Vk[2], Wk[2];
263 
267  std::vector<int> laSizek, laIndk, gSizek, gIndk, gdSizek, gdIndk, svSizek, svIndk, rFactorSizek, rFactorIndk;
268 
269 #ifdef HAVE_OPENMBVCPPINTERFACE
270 
273  OpenMBV::Group * openMBVContactGrp;
274 
278  std::vector<std::vector<OpenMBV::Frame*> > openMBVContactFrame;
279 
283  std::vector<OpenMBV::Arrow *> openMBVNormalForceArrow, openMBVFrictionArrow;
284 
288  double openMBVContactFrameSize;
289 
293  bool openMBVContactFrameEnabled;
294 
298  OpenMBV::Arrow *contactArrow, *frictionArrow;
299 #endif
300 
306  std::vector<fmatvec::Vec> corrk;
307 
311  std::vector<int> corrSizek, corrIndk;
312 
316  std::vector<int> rootID;
317 
321  std::vector<fmatvec::Vec> gddkBuf;
322 
323  private:
324  std::string saved_ref1, saved_ref2;
325  };
326 
327 }
328 
329 #endif /* _CONTACT_H_ */
330