001 #ifndef PATHFINDER_HH
002 #define PATHFINDER_HH 21
003 #include "LevelEditor";
004 #include "Set";
005 #define PF_CELL_DIST 64
006 class CMaxMovePair {
007   CLevel *l;
008   I64 around_x,around_y;
009   I64 x,y;
010   I64 x2,y2;
011   F64 best;
012 };
013 Bool MaxMoveDistPlot(CMaxMovePair *mp,I64 x,I64 y,I64 z) {
014   Bool hit=FALSE;
015   CI64Set *lines=LevelGetLinesInRadius(mp->l,x,y,32.);
016   CLine *line;
017   CD2 *st,*en,at,hitat,to;
018   I64 idx;
019   F64 best,d;
020   at.x=mp->x;
021   at.y=mp->y;
022   to.x=mp->x2;
023   to.y=mp->y2;
024   if(lines->cnt) {
025     for(idx=0;idx!=lines->cnt;++idx) {
026       line=lines->body[idx];
027       st=&line->start->x;
028       en=&line->end->x;
029       if(PlaneIntersect(&hitat,&at,&to,st,en)) {
030         d=Sqrt(Sqr(hitat.x-mp->x)+Sqr(hitat.y-mp->y));
031         if(d<mp->best||!hit)
032           hit=TRUE,mp->best=d;
033       }
034     }
035   } else {
036     mp->around_x=x;
037     mp->around_y=y;
038   }
039   I64SetDel(lines);
040   return !hit;
041 }
042 F64 MaxMoveDist(CLevel *l,F64 x,F64 y,F64 x2,F64 y2) {
043 //TODO something more clever
044   CD2 *st,*en;
045   CD2 at,to,hit;
046   CLine *line;
047   CI64Set *lines;
048   CMaxMovePair mp;
049   Bool passed=FALSE;
050   F64 best=I16_MAX,d;
051   I64 idx;
052   at.x=x,at.y=y;
053   mp.l=l;
054   mp.best=I16_MAX;
055   mp.around_x=x;
056   mp.x=x;
057   mp.y=y;
058   mp.x2=x2;
059   mp.y2=y2;
060   mp.around_y=y;
061   to.x=x2;
062   to.y=y2;
063   if(!Line(&mp,x,y,0,x2,y2,0,&MaxMoveDistPlot,32)) {
064     passed=TRUE;
065     best=mp.best;
066   }
067   if(!passed) return I16_MAX;
068   return best;
069 }
070 I64 class CMove {
071   I32 x,y;
072 };
073 //CI64Set of CMove's
074 CI64Set *PathFind(CLevel *l,F64 x,F64 y,F64 to_x,F64 to_y) {
075   LockWorld;
076   CHashTable *ass=HashTableNew(0x100,Fs),*old=Fs->hash_table;
077   CHashGeneric *gen;
078   U8 buf[STR_LEN];
079   Fs->hash_table=ass;
080   CI64Set *canidates=I64SetNew;
081   CI64Set *done;
082   CI64Set *ret=I64SetNew;
083   I64 wave=0,dir,idx,idx2,old_wave;
084   F64 a,d,best,global_best_d=I16_MAX*I16_MAX;
085   CMove global_best;
086   CMove tmp,tmp2;
087   tmp.x=x;
088   tmp.y=y;
089   global_best=0;
090   I64SetAdd(canidates,tmp);
091   while(canidates->cnt&&wave<128) {
092 //Pick best 21
093     idx=canidates->cnt;
094     tmp2=canidates->body[0];
095     best=Sqr(tmp2.x-to_x)+Sqr(tmp2.y-to_y);
096     while(--idx>=0) {
097       tmp=canidates->body[idx];
098       d=Sqr(tmp.x-to_x)+Sqr(tmp.y-to_y);
099       if(d<=best) {
100         best=d,tmp2=tmp;
101       }
102       if(d<=global_best_d) {
103         global_best_d=d;
104         global_best=tmp;
105       }
106     }
107     I64SetRem(canidates,tmp2);
108 //Make child nodes
109     for(idx=0;idx!=8;idx++) {
110       tmp=tmp2;
111       switch(idx) {
112         case 0:
113           tmp.x+=PF_CELL_DIST;
114           break;
115         case 1:
116           tmp.x+=PF_CELL_DIST;
117           tmp.y+=PF_CELL_DIST;
118           break;
119         case 2:
120           tmp.y+=PF_CELL_DIST;
121           break;
122         case 3:
123           tmp.x-=PF_CELL_DIST;
124           tmp.y+=PF_CELL_DIST;
125           break;
126         case 4:
127           tmp.x-=PF_CELL_DIST;
128           break;
129         case 5:
130           tmp.x-=PF_CELL_DIST;
131           tmp.y-=PF_CELL_DIST;
132           break;
133         case 6:
134           tmp.y-=PF_CELL_DIST;
135           break;
136         case 7:
137           tmp.x+=PF_CELL_DIST;
138           tmp.y-=PF_CELL_DIST;
139           break;
140       }
141       if(MaxMoveDist(l,tmp2.x,tmp2.y,tmp.x,tmp.y)>50+PF_CELL_DIST) {
142         StrPrint(buf,"(%d,%d)",tmp.x,tmp.y);
143         old_wave=FramePtr(buf);
144         if(old_wave>wave||old_wave==0) {
145           HashGenericAdd(buf,HTT_FRAME_PTR,wave+1,tmp2.x,tmp2.y);
146           I64SetAdd(canidates,tmp);
147         }
148       }
149     }
150     ++wave;
151   }
152   I64SetDel(canidates);
153 
154   ret=I64SetNew;
155   tmp.x=to_x;
156   tmp.y=to_y;
157   I64SetAdd(ret,tmp);
158   tmp=global_best;
159   I64SetAdd(ret,tmp);
160   while(TRUE) {
161     StrPrint(buf,"(%d,%d)",tmp.x,tmp.y);
162     gen=HashFind(buf,Fs->hash_table,HTT_FRAME_PTR);
163     if(!gen) {
164       break;
165     }
166     if(gen->user_data0>wave)
167       break;
168     wave=gen->user_data0;
169     tmp2=tmp;
170     tmp.x=gen->user_data1;
171     tmp.y=gen->user_data2;
172     if(tmp==tmp2)
173         break;
174     I64SetAdd(ret,tmp); 
175   }
176   //Reverse the result
177   done=I64SetNew;
178   for(idx=ret->cnt-1;idx>=0;--idx) {
179     I64SetAdd(done,ret->body[idx]);
180   }
181   HashTableDel(Fs->hash_table);
182   Fs->hash_table=old;
183   I64SetDel(ret);
184   UnlockWorld;
185   return done; 
186 
187 }
188 U0 PathFinderGo(CLevel *l,CI64Set *moves,F64 force,F64 max_force,CD2 *pos,CD2 *deltas) {
189   I64 idx=0;
190   CMove best=moves->body[0],cur;
191   CI64Set *new;
192   F64 d,best_d=0,a,d2;
193 again:;
194   for(idx=0;idx!=moves->cnt;++idx) {
195     cur=moves->body[idx];
196     d=Sqrt(Sqr(cur.x-pos->x)+Sqr(cur.y-pos->y))+.01;
197     
198     if(MaxMoveDist(l,pos->x,pos->y,cur.x,cur.y)>d) {
199       if(best_d<d) {
200         best_d=d;
201         best=cur;
202       }
203     }
204   }
205   while(moves->body[0]!=best&&moves->cnt>1)
206     I64SetRem(moves,moves->body[0]);
207   a=Arg(best.x-pos->x,best.y-pos->y);
208   if(moves->cnt==1) {
209   } else 
210     best_d=force;
211   deltas->x=Cos(a)*Clamp(best_d,force,max_force);
212   deltas->y=Sin(a)*Clamp(best_d,force,max_force);
213 }
214 # endif