Choreonoid  1.1
World.h
説明を見る。
1 
6 #ifndef CNOID_BODY_WORLD_H_INCLUDED
7 #define CNOID_BODY_WORLD_H_INCLUDED
8 
9 #include "Body.h"
10 #include "ForwardDynamics.h"
11 #include <map>
12 #include "exportdecl.h"
13 
14 namespace cnoid {
15 
16  class Link;
17 
19  {
20  public:
21  WorldBase();
22  virtual ~WorldBase();
23 
28  inline int numBodies() { return bodyInfoArray.size(); }
29 
35  BodyPtr body(int index);
36 
42  BodyPtr body(const std::string& name);
43 
49  inline ForwardDynamicsPtr forwardDynamics(int index) {
50  return bodyInfoArray[index].forwardDynamics;
51  }
52 
58  int bodyIndex(const std::string& name);
59 
66  int addBody(BodyPtr body);
67 
71  void clearBodies();
72 
76  void clearCollisionPairs();
77 
82  void setTimeStep(double dt);
83 
88  double timeStep(void) const { return timeStep_; }
89 
94  void setCurrentTime(double tm);
95 
100  double currentTime(void) const { return currentTime_; }
101 
106  void setGravityAcceleration(const Vector3& g);
107 
112  inline const Vector3& gravityAcceleration() { return g; }
113 
119  void enableSensors(bool on);
120 
124  void setEulerMethod();
125 
129  void setRungeKuttaMethod();
130 
134  virtual void initialize();
135 
139  virtual void calcNextState();
140 
147  std::pair<int,bool> getIndexOfLinkPairs(Link* link1, Link* link2);
148 
149  protected:
150 
151  double currentTime_;
152  double timeStep_;
153 
154  struct BodyInfo {
157  };
158  std::vector<BodyInfo> bodyInfoArray;
159 
161 
162  private:
163  typedef std::map<std::string, int> NameToIndexMap;
164  NameToIndexMap nameToBodyIndexMap;
165 
166  typedef std::map<BodyPtr, int> BodyToIndexMap;
167  BodyToIndexMap bodyToIndexMap;
168 
169  struct LinkPairKey {
170  Link* link1;
171  Link* link2;
172  bool operator<(const LinkPairKey& pair2) const;
173  };
174  typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
175  LinkPairKeyToIndexMap linkPairKeyToIndexMap;
176 
177  int numRegisteredLinkPairs;
178 
179  Vector3 g;
180 
181  bool isEulerMethod; // Euler or Runge Kutta ?
182 
183  };
184 
185 
186  template <class TConstraintForceSolver> class World : public WorldBase
187  {
188  public:
189  TConstraintForceSolver constraintForceSolver;
190 
192 
193  virtual void initialize() {
195  constraintForceSolver.initialize();
196  }
197 
198  virtual void calcNextState(){
199  constraintForceSolver.solve();
201  }
202  };
203 
204 };
205 
206 #endif