iCub-main
world.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Vadim Tikhanoff
6 * email: vadim.tikhanoff@iit.it
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 
29 #ifndef ICUBSIMULATION_WORLD_INC
30 #define ICUBSIMULATION_WORLD_INC
31 
32 #include "SDL.h"
33 #include "SDL_opengl.h"
34 #include "rendering.h"
35 #include <ode/ode.h>
36 #include <string>
37 #include "RobotConfig.h"
38 #include "WorldOp.h"
39 
40 #define DENSITY (1.0) // density of all objects
41 
42 #ifdef _MSC_VER
43 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
44 #endif
45 
46 class WorldObject {
47 public:
48  virtual dBodyID getBody() const = 0;
49  virtual dGeomID getGeometry() const = 0;
50  virtual bool create(const WorldOp& op, WorldResult& result, int idx) = 0;
51  virtual bool takeColor() { return true; }
52 };
53 
54 typedef dReal real3[3];
55 
57 public:
58  int len;
59  int *counter;
61 
62  WorldObjectList(int len, int& counter, real3 *colors) :
63  len(len),
64  counter(&counter),
65  colors(colors) {
66  }
67 
68  virtual WorldObject& get(int index) = 0;
69  virtual const WorldObject& get(int index) const = 0;
70 
71  int length() {
72  return *counter;
73  }
74 
75  virtual bool create(const WorldOp& op, WorldResult& result) {
76  if (counter==NULL) return false;
77 
78  if (!op.location.isValid()) {
79  result.setFail("location not set");
80  return false;
81  }
82 
83  int at = *counter;
84  if (at>=len-1) {
85  result.setFail("too many objects");
86  return false;
87  }
88  WorldObject& obj = get(at);
89  if (!obj.create(op,result,at)) return false;
90  (*counter)++;
91 
92  if (op.dynamic.get()) {
93  dBodySetPosition(obj.getBody(),
94  op.location.get(0),
95  op.location.get(1),
96  op.location.get(2));
97  } else {
98  dGeomSetPosition(obj.getGeometry(),
99  op.location.get(0),
100  op.location.get(1),
101  op.location.get(2));
102  }
103 
104  if (op.color.isValid()) {
105  if (colors!=NULL) {
106  colors[at][0] = op.color.get(0);
107  colors[at][1] = op.color.get(1);
108  colors[at][2] = op.color.get(2);
109  }
110  }
111 
112 
113  return true;
114  }
115 
116  bool inRange(int index) const {
117  return index>=0 && index<(*counter);
118  }
119 
120  void clear() {
121  for (int i=0; i<*counter; i++) {
122  // shouldn't bodies be destroyed as well?
123  // original code doesn't do this, so maybe it is ok?
124  dGeomDestroy(get(i).getGeometry());
125  }
126  *counter = 0;
127  }
128 };
129 
130 template <class T>
132 public:
134 
135  WorldObjectListOf(T *store, int len, int& counter, real3 *colors) :
136  WorldObjectList(len,counter,colors),
137  store(store)
138  {
139  }
140 
141  virtual WorldObject& get(int index) {
142  return store[index];
143  }
144 
145  virtual const WorldObject& get(int index) const {
146  return store[index];
147  }
148 
149 };
150 
152 public:
153 #define MAXNUM 100
154 #define GPB 3// maximum number of geometries per body
155  double l_massobj0;
156  double l_massobj1;
157  double l_massobj2;
158  double l_massobj3;
159 
160  int OBJNUM;
161  int waitOBJ;
162  int S_OBJNUM;
163 
164  int SPHNUM;
165  int waitSPH;
166  int S_SPHNUM;
167 
169  int waitOBJ1;
171 
172  int waitMOD;
176 
177  dReal color[100][3];
178  dReal s_color[100][3];
179  dReal color1[100][3];
180  dReal s_color1[100][3];
181  dReal color2[100][3];
182  dReal s_color2[100][3];
183  worldSimData();
184 };
185 
186 class worldSim : public worldSimData {
187 public:
188  static const bool textured = true;
189  std::string actWorld;
190  dTriMeshDataID TriData[100];
191  dTriMeshX trimesh[100];
192 
193  dTriMeshDataID s_TriData[100];
194  dTriMeshX s_trimesh[100];
195 
196  // max number of objects
197 #define numObjJoints 5 //define Joints
198 
199  dJointID joint [numObjJoints];
200  dReal speed [numObjJoints];
201  dJointID j;
202 
203  dBodyID tableBody[5];
204  dGeomID tableGeom[5];
205 
206  dBodyID tempBody;
207  dGeomID tempGeom[2];
208 
209  dBodyID ballBody;
210  dGeomID ballGeom;
211 
212  //init. encapsulated object - the Box
213  dGeomID box_part[14];
214  dGeomID box_geom[14];
215  dBodyID Box;
216 
217  //Geometry group for the bodies of the Object
218  dSpaceID boxObj;
219 
220  dBodyID box;
221  dGeomID boxgeom;
222 
225  class MyObject : public WorldObject {
226  public:
227  dBodyID boxbody; // the body
228  dGeomID geom[GPB]; // geometries representing this body
229  dReal size[3];
230 
231  virtual dBodyID getBody() const { return boxbody; }
232  virtual dGeomID getGeometry() const { return geom[0]; }
233 
234  virtual bool create(const WorldOp& op, WorldResult& result, int idx);
235  };
236 
238  MyObject s_obj[MAXNUM];
239 
242 
243  class MyObject1 : public WorldObject {
244  public:
245  dBodyID cylbody; // the body
246  dGeomID cylgeom[GPB]; // geometries representing this body
247  dReal radius;
248  dReal length;
249 
250  virtual dBodyID getBody() const { return cylbody; }
251  virtual dGeomID getGeometry() const { return cylgeom[0]; }
252  virtual bool create(const WorldOp& op, WorldResult& result, int idx);
253  };
254 
255  MyObject1 cyl_obj[MAXNUM];
256  MyObject1 s_cyl_obj[MAXNUM];
257 
260 
261  int modelTexture[100];
262  int s_modelTexture[100];
263 
264  class MyObject2 : public WorldObject {
265  public:
266  dBodyID body; // the body
267  dGeomID geom; // geometries representing this body
268  virtual dBodyID getBody() const { return body; }
269  virtual dGeomID getGeometry() const { return geom; }
270  virtual bool create(const WorldOp& op, WorldResult& result, int idx);
271  virtual bool takeColor() { return false; }
272  };
273  MyObject2 ThreeD_obj[100];
274  MyObject2 s_ThreeD_obj[100];
275 
278 
279  class MyObject3 : public WorldObject {
280  public:
281  dBodyID sphbody; // the body
282  dGeomID sphgeom[GPB]; // geometries representing this body
283  dReal radius;
284  virtual dBodyID getBody() const { return sphbody; }
285  virtual dGeomID getGeometry() const { return sphgeom[0]; }
286  virtual bool create(const WorldOp& op, WorldResult& result, int idx);
287  };
288 
291 
294 
295  std::string texture;
296  std::string model_DIR;
297 public:
298 
299  void syncAngles();
300  void ballDamping();
301  void draw();
302  void drawGeom(dGeomID g, const dReal *pos, const dReal *rot);//, float red = 0.0f, float green = 128.5f, float blue = 255.0f);
303  void setPosition(dReal agent1X, dReal agent1Z, dReal agent1Y );
304  void activateWorld(RobotConfig& config);
305  void init( dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
306  RobotConfig& config);
307  void loadTexture(std::string texture, int numTexture);
308 
309 
310  ~worldSim();
311 
312  worldSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
313  RobotConfig& config);
314 };
315 
316 
317 #endif
dBodyID sphbody
Definition: world.h:281
int waitOBJ
Definition: world.h:161
int waitMOD
Definition: world.h:172
virtual dGeomID getGeometry() const
Definition: world.h:251
#define numObjJoints
Definition: world.h:197
WorldObjectListOf< MyObject2 > model_dynamic
Definition: world.h:277
dBodyID body
Definition: world.h:266
virtual bool takeColor()
Definition: world.h:271
dGeomID ballGeom
Definition: world.h:210
static uint32_t idx[BOARD_NUM]
int MODEL_NUM
Definition: world.h:174
dGeomID boxgeom
Definition: world.h:221
int S_SPHNUM
Definition: world.h:166
bool WAITLOADING
Definition: world.h:223
bool static_model
Definition: world.h:224
const dReal * pos
Definition: iCub_Sim.cpp:62
double l_massobj1
Definition: world.h:156
dBodyID cylbody
Definition: world.h:245
double l_massobj3
Definition: world.h:158
virtual bool create(const WorldOp &op, WorldResult &result)
Definition: world.h:75
int OBJNUM
Definition: world.h:160
double get(int offset) const
Definition: WorldOp.h:110
WorldOpTriplet color
Definition: WorldOp.h:122
bool get() const
Definition: WorldOp.h:72
dBodyID box
Definition: world.h:220
virtual dBodyID getBody() const =0
WorldObjectListOf< MyObject > box_static
Definition: world.h:240
dGeomID geom
Definition: world.h:267
int S_cylOBJNUM
Definition: world.h:170
virtual dBodyID getBody() const
Definition: world.h:250
WorldOpFlag dynamic
Definition: WorldOp.h:127
WorldObjectListOf< MyObject3 > sphere_static
Definition: world.h:292
dBodyID ballBody
Definition: world.h:209
#define GPB
Definition: world.h:154
virtual dBodyID getBody() const
Definition: world.h:268
WorldObjectListOf< MyObject1 > cylinder_dynamic
Definition: world.h:259
WorldObjectListOf< MyObject3 > sphere_dynamic
Definition: world.h:293
bool inRange(int index) const
Definition: world.h:116
int * counter
Definition: world.h:59
virtual dGeomID getGeometry() const
Definition: world.h:232
int length()
Definition: world.h:71
WorldObjectListOf< MyObject2 > model_static
Definition: world.h:276
int cylOBJNUM
Definition: world.h:168
virtual dGeomID getGeometry() const
Definition: world.h:269
double l_massobj0
Definition: world.h:155
double l_massobj2
Definition: world.h:157
dBodyID boxbody
Definition: world.h:227
virtual dBodyID getBody() const
Definition: world.h:284
int SPHNUM
Definition: world.h:164
Header file for the rendering.
std::string actWorld
Definition: world.h:189
dReal real3[3]
Definition: world.h:54
virtual dGeomID getGeometry() const =0
std::string texture
Definition: world.h:295
bool isValid() const
Definition: WorldOp.h:46
WorldObjectListOf< MyObject1 > cylinder_static
Definition: world.h:258
void clear()
Definition: world.h:120
WorldOpTriplet location
Definition: WorldOp.h:120
real3 * colors
Definition: world.h:60
int s_MODEL_NUM
Definition: world.h:175
int waitOBJ1
Definition: world.h:169
int S_OBJNUM
Definition: world.h:162
std::string model_DIR
Definition: world.h:296
virtual dBodyID getBody() const
Definition: world.h:231
WorldObjectListOf< MyObject > box_dynamic
Definition: world.h:241
dJointID j
Definition: world.h:201
WorldObjectList(int len, int &counter, real3 *colors)
Definition: world.h:62
dSpaceID boxObj
Definition: world.h:218
int waitSPH
Definition: world.h:165
void setFail(const char *msg)
Definition: WorldOp.h:158
dBodyID Box
Definition: world.h:215
#define MAXNUM
Definition: world.h:153
virtual dGeomID getGeometry() const
Definition: world.h:285
int s_waitMOD
Definition: world.h:173
virtual bool takeColor()
Definition: world.h:51
virtual bool create(const WorldOp &op, WorldResult &result, int idx)=0
dBodyID tempBody
Definition: world.h:206
WorldObjectListOf(T *store, int len, int &counter, real3 *colors)
Definition: world.h:135