Robot Simulator of the Robotics Group for Self-Organization of Control  0.8.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
raceground.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2005-2011 LpzRobots development team *
3  * Georg Martius <georg dot martius at web dot de> *
4  * Frank Guettler <guettler at informatik dot uni-leipzig dot de *
5  * Frank Hesse <frank at nld dot ds dot mpg dot de> *
6  * Ralf Der <ralfder at mis dot mpg dot de> *
7  * *
8  * This program is free software; you can redistribute it and/or modify *
9  * it under the terms of the GNU General Public License as published by *
10  * the Free Software Foundation; either version 2 of the License, or *
11  * (at your option) any later version. *
12  * *
13  * This program is distributed in the hope that it will be useful, *
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
16  * GNU General Public License for more details. *
17  * *
18  * You should have received a copy of the GNU General Public License *
19  * along with this program; if not, write to the *
20  * Free Software Foundation, Inc., *
21  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
22  * *
23  ***************************************************************************/
24 #ifndef __RACEGROUND_H
25 #define __RACEGROUND_H
26 
27 
28 #include <stdio.h>
29 #include <cmath>
30 #include <list>
31 
32 #include "abstractobstacle.h"
33 #include <drawstuff/drawstuff.h>
34 #include "stl_adds.h"
35 #include "matrix.h"
36 using namespace matrix;
37 #include "mathutils.h"
38 #include "abstracttracksection.h"
39 #include "straightline.h"
40 #include "degreesegment.h"
41 
42 
43 //Fixme: raceground creates collisions with ground and itself
44 class RaceGround : public AbstractObstacle {
45 
46  // the list that contains all segments
47  list<AbstractTrackSection*> SegmentList;
48 
49 
50  bool obstacle_exists;
51 
52  public:
53 
54  RaceGround(const OdeHandle& odehandle):
55  AbstractObstacle(odehandle), length(0), width(0), height(0) {
56  setParameters(pose);
57  };
58 
59 
60  RaceGround(const OdeHandle& odehandle,const Matrix& pose):
61  AbstractObstacle(odehandle), length(0), width(0), height(0) {
62  setParameters(pose);
63  };
64 
65  RaceGround(const OdeHandle& odehandle,const Position& pos, double angle):
66  AbstractObstacle(odehandle), length(0), width(0), height(0) {
67  setParameters(getTranslationRotationMatrix(pos,angle));
68  };
69 
70 
71  /**
72  * Destructor
73  */
75 
76  /**
77  * you set the number of segments of the track
78  */
79  void setNumberOfSegments(int number) {
80  numberOfBarcodes=number;
81  }
82 
83  /**
84  * returns the barcode number of the given point
85  * returns (length,width) (-1,-1) if point is not on the track
86  */
87  pair<double, double> getPositionOnTrack(const Position& p) {
88 
89  double passedLength=0.0f;
90  double segmentNumber=-1.0f;
91  double width =-1.0f;
92  for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();
93  it!= SegmentList.end(); ++it) {
94  if((*it)->isInside(p)){
95  double sectionLength = (*it)->getSectionIdValue(p); // between 0..length
96  width = (*it)->getWidthIdValue(p);
97  if (sectionLength<0.0f ) { // weird isegment is found
98  printf("Weird! We should be in the segment!\n");
99  }
100  segmentNumber=numberOfBarcodes*
101  (passedLength+sectionLength)/trackLength;
102  return pair<double, double> (segmentNumber, width);
103  }
104  passedLength+=(*it)->getLength();
105  }
106  return pair<double, double> (segmentNumber, width);
107  }
108 
109  /**
110  * adds the segments in the list to the SegmentList
111  */
112  void addSegments(list<AbstractTrackSection*> listToAdd) {
113  SegmentList+=listToAdd;
114  }
115 
116  /**
117  * adds the segment to the SegmentList
118  */
120  SegmentList+=Segment;
121  }
122 
123 
124  /**
125  * adds the named segment to the SegmentList
126  * names are:
127  * straightline: StraightLine
128  * 90degree : DegreeSegment
129  */
130  void addSegment(string& name) {
131  // get first pose from last stored segment
132  Matrix newPose(pose); // this is the initial pose
133  if (!SegmentList.empty()) {
134  Matrix pos = SegmentList.back()->getPoseMatrix();
135  Matrix end = SegmentList.back()->getTransformedEndMatrix();
136  newPose=pos * end;
137  }
138 
139  if (name=="straightline") {
140  AbstractTrackSection* segment = new StraightLine(newPose);
141  SegmentList += segment;
142  trackLength+=segment->getLength();
143  } else if (name.find("degree")==0) {
144  DegreeSegment* segment = new DegreeSegment(newPose);
145  SegmentList+=(AbstractTrackSection*) segment;
146  // now get the angle and the radius
147  char* d = (char*)malloc(name.length()*sizeof(char));
148  double angle, radius;
149  if (sscanf(name.c_str(),"%1000s %6lf %6lf",d,&angle,&radius)!=3)
150  std::cout << "parameter parsing invalid!: " << name << "\n";
151  else {
152  std::cout << "parameters " << d << "," << angle << " " << radius << "\n";
153  // parsing was ok
154  segment->setCurveAngle(angle/180.0*M_PI);
155  segment->setRadius(radius);
156  trackLength+=segment->getLength();
157  }
158  free(d);
159  }
160  }
161 
162  /**
163  * adds the named segments in the list to the SegmentList
164  * names are:
165  * straightline: StraightLine
166  * 90degree : 90DegreeSegment
167  */
168  void addSegments(list<string> names) {
169  for(list<string>::iterator it = names.begin(); it!=names.end(); ++it) {
170  addSegment(*it);
171  }
172  }
173 
174 /**
175  * draws all the segments stored in SegmentList
176  */
177  virtual void draw(){
178  // double box[3];
179  dsSetTexture (DS_NONE);
180  dsSetColor (color.r, color.g, color.b);
181  for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();
182  it!= SegmentList.end(); ++it) {
183  // call the create function of the segment
184  (*it)->draw();
185  }
186  };
187 
188 
189  virtual void setPosition(double x, double y, double z){
190  if (obstacle_exists){
191  destroy();
192  }
193  create();
194  };
195 
196  virtual void getPosition(double& x, double& y, double& z){
197  x=0.0f;
198  y=0.0f;
199  z=0.0f;
200  };
201 
202 
203  // normally we don't need this function
204  virtual void setGeometry(double length_, double width_, double height_){
205  length=length_;
206  width=width_;
207  height =height_;
208  };
209 
210  virtual void setColor(double r, double g, double b){
211  color.r=r;
212  color.g=g;
213  color.b=b;
214  };
215 
216 
217  protected:
218  double length;
219  double trackLength;
220  double width;
221  double height;
224 
226 
227  virtual void setParameters(const Matrix& initpose) {
228  pose=initpose;
229  obstacle_exists=false;
230  setColor(226 / 255.0, 103 / 255.0, 66 / 255.0);
231  numberOfBarcodes=256.0f;
232  trackLength=0.0;
233  }
234 
235  virtual void create(){
236  // create raceground space and add it to the top level space
237  raceground_space = space;
238  //raceground_space = dSimpleSpaceCreate (space);
239  // dSpaceSetCleanup (raceground_space,0);
240  // todo:
241  // go through all list elements
242  // draw them all by there own draw function
243  for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();it!= SegmentList.end(); ++it) {
244  // call the create function of the segment
245  (*it)->create(raceground_space);
246  }
247  };
248 
249 
250  virtual void destroy(){
251  for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();it!= SegmentList.end(); ++it) {
252  // call the create function of the segment
253  (*it)->destroy();
254  }
255  obstacle_exists=false;
256  };
257 
258 };
259 
260 #endif
pair< double, double > getPositionOnTrack(const Position &p)
returns the barcode number of the given point returns (length,width) (-1,-1) if point is not on the t...
Definition: raceground.h:87
Matrix type.
Definition: matrix.h:65
Abstract class (interface) for obstacles.
Definition: abstracttracksection.h:37
virtual void setParameters(const Matrix &initpose)
Definition: raceground.h:227
virtual void setGeometry(double length_, double width_, double height_)
Definition: raceground.h:204
virtual void setColor(double r, double g, double b)
Definition: raceground.h:210
RaceGround(const OdeHandle &odehandle)
Definition: raceground.h:54
dSpaceID raceground_space
Definition: raceground.h:225
~RaceGround()
Destructor.
Definition: raceground.h:74
Matrix pose
Definition: raceground.h:222
virtual double getLength()=0
returns the length of the segment, here it is the length of the arc formula is: radius * angle; ...
virtual void destroy()
Definition: raceground.h:250
virtual double getLength()
returns the length of the segment, here it is the length of the arc
Definition: degreesegment.cpp:261
void addSegments(list< AbstractTrackSection * > listToAdd)
adds the segments in the list to the SegmentList
Definition: raceground.h:112
double width
Definition: raceground.h:220
Definition: position.h:30
class for degree segments
Definition: degreesegment.h:32
virtual void setPosition(double x, double y, double z)
Definition: raceground.h:189
void addSegment(AbstractTrackSection *Segment)
adds the segment to the SegmentList
Definition: raceground.h:119
Abstract class (interface) for obstacles.
Definition: straightline.h:32
void addSegments(list< string > names)
adds the named segments in the list to the SegmentList names are: straightline: StraightLine 90degree...
Definition: raceground.h:168
double g(double z)
neuron transfer function
Definition: regularisation.h:35
virtual void draw()
draws all the segments stored in SegmentList
Definition: raceground.h:177
virtual void setRadius(const double &rad)
Definition: degreesegment.cpp:64
virtual void setCurveAngle(const double &alpha)
Definition: degreesegment.cpp:56
Definition: raceground.h:44
virtual void create()
Definition: raceground.h:235
double height
Definition: raceground.h:221
void addSegment(string &name)
adds the named segment to the SegmentList names are: straightline: StraightLine 90degree : DegreeSegm...
Definition: raceground.h:130
RaceGround(const OdeHandle &odehandle, const Position &pos, double angle)
Definition: raceground.h:65
RaceGround(const OdeHandle &odehandle, const Matrix &pose)
Definition: raceground.h:60
void setNumberOfSegments(int number)
you set the number of segments of the track
Definition: raceground.h:79
double trackLength
Definition: raceground.h:219
double numberOfBarcodes
Definition: raceground.h:223
virtual void getPosition(double &x, double &y, double &z)
Definition: raceground.h:196