1 /***************************************************************************
2
3 file : pathfinder.h
4 created : Tue Oct 9 16:52:00 CET 2001
5 copyright : (C) 2001-2002 by Bernhard Wymann
6 email : berniw@bluewin.ch
7 version : $Id: pathfinder.h,v 1.1.2.1 2008/11/09 17:50:19 berniw Exp $
8
9 ***************************************************************************/
10
11 /***************************************************************************
12 * *
13 * This program is free software; you can redistribute it and/or modify *
14 * it under the terms of the GNU General Public License as published by *
15 * the Free Software Foundation; either version 2 of the License, or *
16 * (at your option) any later version. *
17 * *
18 ***************************************************************************/
19
20 /*
21 should (!) compute a good path
22 */
23
24 #ifndef _PATHFINDER_H_
25 #define _PATHFINDER_H_
26
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <cstring>
30
31 #include <tgf.h>
32 #include <track.h>
33 #include <car.h>
34 #include <raceman.h>
35 #include <robot.h>
36 #include <robottools.h>
37 #include <math.h>
38 #include "trackdesc.h"
39 #include "mycar.h"
40 #include "spline.h"
41 #include "linalg.h"
42
43 #define FNPF "drivers/berniw/parameter.dat"
44 #define FNIS "drivers/berniw/intsinsqr.dat"
45 #define FNIC "drivers/berniw/intcossqr.dat"
46
47 /* how far we look forward for overtaking, collisions, etc */
48 #define AHEAD 500
49
50 /* how many segments can i pass per simulation step, depends on TRACKRES, simulation->_deltaTime and speed */
51 #define SEGRANGE 3
52
53 /* choose old path */
54 //#define PATH_BERNIW
55
56 /* choose k1999 path */
57 #define PATH_K1999
58
59 class MyCar;
60 class OtherCar;
61
62
63 /* holds a point of datafile from clothoid */
64 typedef struct {
65 double x;
66 double pd;
67 double is;
68 double ic;
69 } tParam;
70
71
72 /* holds data relative to my car */
73 typedef struct {
74 double speedsqr; /* on track direction projected speed squared of opponent */
75 double speed; /* same, but not squared */
76 double time; /* estimate of time to catch up the car */
77 double cosalpha; /* cos(alpha) from angle between my ond opponent */
78 double disttomiddle; /* distance to middle (for prediction) */
79 int catchdist;
80 int catchsegid; /* segment, where i expect (or better guess!) to catch opponent */
81 double dist; /* #segments from me to the other car */
82 OtherCar* collcar; /* pointers to the cars */
83 bool overtakee; /* is this the guy to overtake? */
84 double disttopath; /* distance to my path */
85 double brakedist; /* distance needed for braking to the speed of this car */
86 double mincorner; /* corner nearest to my car */
87 double minorthdist; /* minimal distance relative to my car */
88 } tOCar;
89
90
91 /* holds data needed for let pass opponents */
92 typedef struct {
93 double time; /* how long is the opponent "in range" to overlap me */
94 } tOverlapTimer;
95
96
97
98 class PathSeg
99 {
100 public:
101 void set(tdble ispeedsqr, tdble ilength, v3d* ip, v3d* id);
102 void set(tdble ispeedsqr, tdble ilength, v3d* id);
setLoc(v3d * ip)103 inline void setLoc(v3d* ip) { p = (*ip); }
setOptLoc(v3d * ip)104 inline void setOptLoc(v3d* ip) { o = (*ip); }
setPitLoc(v3d * ip)105 inline void setPitLoc(v3d* ip) { l = ip; }
106
setSpeedsqr(tdble spsqr)107 inline void setSpeedsqr(tdble spsqr) { speedsqr = spsqr; }
setWeight(tdble w)108 inline void setWeight(tdble w) { weight = w; }
setRadius(tdble r)109 inline void setRadius(tdble r) { radius = r; }
110
getSpeedsqr()111 inline tdble getSpeedsqr() { return speedsqr; }
getLength()112 inline tdble getLength() { return length; }
getWeight()113 inline tdble getWeight() { return weight; }
getRadius()114 inline tdble getRadius() { return radius; }
115
getOptLoc()116 inline v3d* getOptLoc() { return &o; }
getPitLoc()117 inline v3d* getPitLoc() { return l; }
getLoc()118 inline v3d* getLoc() { return &p; }
getDir()119 inline v3d* getDir() { return &d; }
120
121 private:
122 tdble speedsqr; /* max possible speed sqared (speed ist therefore sqrt(speedsqr) */
123 tdble length; /* dist to the next pathseg */
124 tdble weight; /* weight function value for superposition */
125 tdble radius; /* radius of current segment */
126 v3d p; /* position in space, dynamic trajectory */
127 v3d o; /* position in space, static trajectory */
128 v3d d; /* direction vector of dynamic trajectory */
129 v3d* l; /* trajectory for pit lane */
130 };
131
132
133 class Pathfinder
134 {
135 public:
136 Pathfinder(TrackDesc* itrack, tCarElt* car, tSituation *situation);
137 ~Pathfinder();
138 void plan(int trackSegId, tCarElt* car, tSituation* situation, MyCar* myc, OtherCar* ocar);
139 void plan(MyCar* myc);
140
141 void initPit(tCarElt* car);
isPitAvailable()142 inline bool isPitAvailable() { return pit; }
getPitSegId()143 inline int getPitSegId() { return pitSegId; }
144 void setPitStop(bool p, int id);
getPitStop()145 inline bool getPitStop() { return pitStop; }
146 int segmentsToPit(int id);
147 void plotPitStopPath(char* filename);
148 void plotPath(char* filename);
149
sqr(double a)150 inline double sqr(double a) { return a*a; };
dist(v3d * a,v3d * b)151 inline double dist(v3d* a, v3d* b) { return sqrt(sqr(a->x-b->x) + sqr(a->y-b->y) + sqr(a->z-b->z)); }
dist2D(v3d * a,v3d * b)152 inline double dist2D(v3d* a, v3d* b) { return sqrt(sqr(a->x-b->x) + sqr(a->y-b->y)); }
getPathSeg(int pathSegId)153 inline PathSeg* getPathSeg(int pathSegId) { return &ps[pathSegId]; }
154 int getCurrentSegment(tCarElt* car);
155 int getCurrentSegment(tCarElt* car, int range);
getnPathSeg()156 inline int getnPathSeg() { return nPathSeg; }
getPitSpeedSqrLimit()157 inline double getPitSpeedSqrLimit() { return pitspeedsqrlimit; }
158 double distToPath(int trackSegId, v3d* p);
159
160 private:
161 static const double COLLDIST; /* up to this distance do we consider other cars as dangerous */
162 static const double TPRES; /* resolution of the steps */
163 enum { PITPOINTS = 7 }; /* # points for pit spline */
164 enum { NTPARAMS = 1001 }; /* # entries in dat files */
165 tParam cp[NTPARAMS]; /* holds values needed for clothiod */
166
167 TrackDesc* track; /* pointer to track data */
168 int lastId; /* segment id of the last call */
169 PathSeg* ps; /* array with the plan */
170 int nPathSeg; /* # of PathSeg's */
171 int lastPlan; /* start of the last plan */
172 int lastPlanRange; /* range of the last plan */
173 bool pitStop; /* pitstop ? */
174 bool inPit; /* internal pit state */
175
176 int s1, s3; /* pitentrystart, pitentryend */
177 int e1, e3; /* pitexitstart, pitexitend */
178
179 v3d pitLoc; /* location of pit */
180 int pitSegId; /* segment id of pit */
181 bool pit;
182 int changed;
183 double pitspeedsqrlimit; /* speed limit for pit lane squared */
184
185 int collcars;
186 tOCar* o;
187 tOverlapTimer* overlaptimer;
188 v3d* pitcord;
189
190 void initPitStopPath(void);
191 void getPitPoint(int j, int k, double slope, double dist, v3d* r);
192 int collision(int trackSegId, tCarElt* mycar, tSituation *s, MyCar* myc, OtherCar* ocar);
193 int overtake(int trackSegId, tSituation *s, MyCar* myc, OtherCar* ocar);
194 double curvature(double xp, double yp, double x, double y, double xn, double yn);
195 void adjustRadius(int s, int p, int e, double c, double carwidth);
196 void stepInterpolate(int iMin, int iMax, int Step);
197 void interpolate(int Step);
198 void smooth(int Step);
199
200 int correctPath(int id, tCarElt* car, MyCar* myc);
201
202 bool loadClothoidParams(tParam* p);
203 double intsinsqr(double alpha);
204 double intcossqr(double alpha);
205 double clothparam(double alpha);
206 double clothsigma(double beta, double y);
207 double clothlength(double beta, double y);
208
209 int findStartSegId(int id);
210 int findEndSegId(int id);
211 int initStraight(int id, double w);
212 int initLeft(int id, double w);
213 int initRight(int id, double w);
214 double computeWeight(double x, double len);
215 void setLocWeighted(int id, double newweight, v3d* newp);
216 void smooth(int s, int e, int p, double w);
217 void smooth(int id, double delta, double w);
218 void optimize(int start, int range, double w);
219 void optimize2(int start, int range, double w);
220 void optimize3(int start, int range, double w);
221 int updateOCar(int trackSegId, tSituation *s, MyCar* myc, OtherCar* ocar, tOCar* o);
222 void updateOverlapTimer(int trackSegId, tSituation *s, MyCar* myc, OtherCar* ocar, tOCar* o, tOverlapTimer* ov);
223 int letoverlap(int trackSegId, tSituation *s, MyCar* myc, OtherCar* ocar, tOverlapTimer* ov);
224 double pathSlope(int id);
225 int countSegments(int from, int to);
226 };
227
228
distToPath(int trackSegId,v3d * p)229 inline double Pathfinder::distToPath(int trackSegId, v3d* p)
230 {
231 v3d *toright = track->getSegmentPtr(trackSegId)->getToRight();
232 v3d *pathdir = ps[trackSegId].getDir();
233 v3d n1, torightpath;
234 toright->crossProduct(pathdir, &n1);
235 pathdir->crossProduct(&n1, &torightpath);
236 return ((*p - *ps[trackSegId].getLoc())*torightpath)/torightpath.len();
237 }
238
239
set(tdble ispeedsqr,tdble ilength,v3d * ip,v3d * id)240 inline void PathSeg::set(tdble ispeedsqr, tdble ilength, v3d* ip, v3d* id) {
241 speedsqr = ispeedsqr;
242 length = ilength;
243 p = (*ip);
244 d = (*id);
245 }
246
247
set(tdble ispeedsqr,tdble ilength,v3d * id)248 inline void PathSeg::set(tdble ispeedsqr, tdble ilength, v3d* id) {
249 speedsqr = ispeedsqr;
250 length = ilength;
251 d = (*id);
252 }
253
254
setPitStop(bool p,int id)255 inline void Pathfinder::setPitStop(bool p, int id) {
256 if (isPitAvailable() && track->isBetween(e3, (s1 - AHEAD + nPathSeg) % nPathSeg, id) && p) {
257 pitStop = true ;
258 } else {
259 pitStop = false;
260 }
261 }
262
263
segmentsToPit(int id)264 inline int Pathfinder::segmentsToPit(int id) {
265 if (id <= pitSegId) {
266 return (pitSegId - id);
267 } else {
268 return (track->getnTrackSegments() - id + pitSegId);
269 }
270 }
271
272
pathSlope(int id)273 inline double Pathfinder::pathSlope(int id) {
274 int nextid = (id + 1) % nPathSeg;
275 v3d dir = *ps[nextid].getLoc() - *ps[id].getLoc();
276 double dp = dir*(*track->getSegmentPtr(id)->getToRight())/dir.len();
277 double alpha = PI/2.0 - acos(dp);
278 return tan(alpha);
279 }
280
281
282 /* get the segment on which the car is, searching ALL the segments */
getCurrentSegment(tCarElt * car)283 inline int Pathfinder::getCurrentSegment(tCarElt* car)
284 {
285 lastId = track->getCurrentSegment(car);
286 return lastId;
287 }
288
289
290 /* get the segment on which the car is, searching from the position of the last call within range */
getCurrentSegment(tCarElt * car,int range)291 inline int Pathfinder::getCurrentSegment(tCarElt* car, int range)
292 {
293 lastId = track->getCurrentSegment(car, lastId, range);
294 return lastId;
295 }
296
297
countSegments(int from,int to)298 inline int Pathfinder::countSegments(int from, int to)
299 {
300 if ( to >= from) {
301 return to - from;
302 } else {
303 return nPathSeg - from + to;
304 }
305 }
306
307 #endif // _PATHFINDER_H_
308
309