00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
#ifndef __RACEGROUND_H
00051
#define __RACEGROUND_H
00052
00053
00054
#include <stdio.h>
00055
#include <math.h>
00056
#include <list>
00057
00058
#include "abstractobstacle.h"
00059
#include <drawstuff/drawstuff.h>
00060
#include "stl_adds.h"
00061
#include "matrix.h"
00062
using namespace matrix;
00063
#include "mathutils.h"
00064
#include "abstracttracksection.h"
00065
#include "straightline.h"
00066
#include "degreesegment.h"
00067
00068
00069
00070 class RaceGround :
public AbstractObstacle {
00071
00072
00073 list<AbstractTrackSection*>
SegmentList;
00074
00075
00076 bool obstacle_exists;
00077
00078
public:
00079
00080 RaceGround(
const OdeHandle& odehandle):
00081 AbstractObstacle(odehandle) {
00082
setParameters(
pose);
00083 };
00084
00085
00086 RaceGround(
const OdeHandle& odehandle,
const Matrix& pose):
00087 AbstractObstacle(odehandle) {
00088
setParameters(pose);
00089 };
00090
00091 RaceGround(
const OdeHandle& odehandle,
const Position& pos,
double angle):
00092 AbstractObstacle(odehandle) {
00093
setParameters(getTranslationRotationMatrix(pos,angle));
00094 };
00095
00096
00097
00098
00099
00100 ~RaceGround() {}
00101
00102
00103
00104
00105 void setNumberOfSegments(
int number) {
00106
numberOfBarcodes=number;
00107 }
00108
00109
00110
00111
00112
00113 pair<double, double>
getPositionOnTrack(
const Position& p) {
00114
00115
double passedLength=0.0f;
00116
double segmentNumber=-1.0f;
00117
double width =-1.0f;
00118
for(list<AbstractTrackSection*>::iterator it =
SegmentList.begin();
00119 it!=
SegmentList.end(); ++it) {
00120
if((*it)->isInside(p)){
00121
double sectionLength = (*it)->getSectionIdValue(p);
00122 width = (*it)->getWidthIdValue(p);
00123
if (sectionLength<0.0f ) {
00124 printf(
"Weird! We should be in the segment!\n");
00125 }
00126 segmentNumber=
numberOfBarcodes*
00127 (passedLength+sectionLength)/
trackLength;
00128
return pair<double, double> (segmentNumber, width);
00129 }
00130 passedLength+=(*it)->getLength();
00131 }
00132
return pair<double, double> (segmentNumber, width);
00133 }
00134
00135
00136
00137
00138 void addSegments(list<AbstractTrackSection*> listToAdd) {
00139
SegmentList+=listToAdd;
00140 }
00141
00142
00143
00144
00145 void addSegment(
AbstractTrackSection* Segment) {
00146
SegmentList+=Segment;
00147 }
00148
00149
00150
00151
00152
00153
00154
00155
00156 void addSegment(string& name) {
00157
00158
Matrix newPose(
pose);
00159
if (!
SegmentList.empty()) {
00160
Matrix pos =
SegmentList.back()->getPoseMatrix();
00161
Matrix end =
SegmentList.back()->getTransformedEndMatrix();
00162 newPose=pos * end;
00163 }
00164
00165
if (name==
"straightline") {
00166
AbstractTrackSection* segment =
new StraightLine(newPose);
00167
SegmentList += segment;
00168
trackLength+=segment->getLength();
00169 }
else if (name.find(
"degree")==0) {
00170
DegreeSegment* segment =
new DegreeSegment(newPose);
00171
SegmentList+=(
AbstractTrackSection*) segment;
00172
00173
char* d = (
char*)malloc(name.length()*
sizeof(
char));
00174
double angle, radius;
00175
if (sscanf(name.c_str(),
"%s %lf %lf",d,&angle,&radius)!=3)
00176 std::cout <<
"parameter parsing invalid!: " << name <<
"\n";
00177
else {
00178 std::cout <<
"parameters " << d <<
"," << angle <<
" " << radius <<
"\n";
00179
00180 segment->
setCurveAngle(angle/180.0*M_PI);
00181 segment->
setRadius(radius);
00182
trackLength+=segment->
getLength();
00183 }
00184 free(d);
00185 }
00186 }
00187
00188
00189
00190
00191
00192
00193
00194 void addSegments(list<string> names) {
00195
for(list<string>::iterator it = names.begin(); it!=names.end(); ++it) {
00196
addSegment(*it);
00197 }
00198 }
00199
00200
00201
00202
00203 virtual void draw(){
00204
00205 dsSetTexture (DS_NONE);
00206 dsSetColor (color.r, color.g, color.b);
00207
for(list<AbstractTrackSection*>::iterator it =
SegmentList.begin();
00208 it!=
SegmentList.end(); ++it) {
00209
00210 (*it)->draw();
00211 }
00212 };
00213
00214
00215 virtual void setPosition(
double x,
double y,
double z){
00216
if (
obstacle_exists){
00217
destroy();
00218 }
00219
create();
00220 };
00221
00222 virtual void getPosition(
double& x,
double& y,
double& z){
00223 x=0.0f;
00224 y=0.0f;
00225 z=0.0f;
00226 };
00227
00228
00229
00230 virtual void setGeometry(
double length_,
double width_,
double height_){
00231
length=length_;
00232
width=width_;
00233
height =height_;
00234 };
00235
00236 virtual void setColor(
double r,
double g,
double b){
00237 color.r=r;
00238 color.g=
g;
00239 color.b=b;
00240 };
00241
00242
00243
protected:
00244 double length;
00245 double trackLength;
00246 double width;
00247 double height;
00248 Matrix pose;
00249 double numberOfBarcodes;
00250
00251 dSpaceID
raceground_space;
00252
00253 virtual void setParameters(
const Matrix& initpose) {
00254
pose=initpose;
00255
obstacle_exists=
false;
00256
setColor(226 / 255.0, 103 / 255.0, 66 / 255.0);
00257
numberOfBarcodes=256.0f;
00258
trackLength=0.0;
00259 }
00260
00261 virtual void create(){
00262
00263
raceground_space = space;
00264
00265
00266
00267
00268
00269
for(list<AbstractTrackSection*>::iterator it =
SegmentList.begin();it!=
SegmentList.end(); ++it) {
00270
00271 (*it)->create(
raceground_space);
00272 }
00273 };
00274
00275
00276 virtual void destroy(){
00277
for(list<AbstractTrackSection*>::iterator it =
SegmentList.begin();it!=
SegmentList.end(); ++it) {
00278
00279 (*it)->destroy();
00280 }
00281
obstacle_exists=
false;
00282 };
00283
00284 };
00285
00286
#endif