$search
00001 /* 00002 * Copyright (c) 2008, Maxim Likhachev 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the University of Pennsylvania nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 #ifndef __LIST_H_ 00030 #define __LIST_H_ 00031 00032 //the maximum size of the heap 00033 #define LISTSIZE 5000000 00034 00035 struct listelement 00036 { 00037 AbstractSearchState *liststate; 00038 struct listelement *prev; 00039 struct listelement *next; 00040 }; 00041 typedef struct listelement listelement; 00042 00043 class CList 00044 { 00045 00046 //data 00047 public: 00048 00049 listelement* firstelement; 00050 listelement* lastelement; 00051 int currentsize; 00052 00053 //constructors 00054 public: 00055 CList() 00056 { 00057 firstelement = NULL; 00058 lastelement = NULL; 00059 currentsize = 0; 00060 }; 00061 ~CList() 00062 {}; 00063 00064 //functions 00065 public: 00066 bool empty() 00067 {return (currentsize == 0);}; 00068 bool full() 00069 {return (currentsize >= LISTSIZE);}; 00070 bool in(AbstractSearchState *AbstractSearchState1, int listindex) 00071 { 00072 return (AbstractSearchState1->listelem[listindex] != NULL); 00073 }; 00074 void makeemptylist(int listindex) 00075 { 00076 while(firstelement != NULL) 00077 remove(firstelement->liststate, listindex); 00078 }; 00079 00080 void insert(AbstractSearchState *AbstractSearchState1, int listindex) 00081 { 00082 if(currentsize >= LISTSIZE) 00083 { 00084 SBPL_ERROR("ERROR: list is full\n"); 00085 throw new SBPL_Exception(); 00086 } 00087 if(AbstractSearchState1->listelem[listindex] != NULL) 00088 { 00089 SBPL_ERROR("ERROR: insert: element is already in the list\n"); 00090 throw new SBPL_Exception(); 00091 } 00092 listelement *insertelem = (listelement*)malloc(sizeof(listelement)); 00093 insertelem->liststate = AbstractSearchState1; 00094 insertelem->prev = NULL; 00095 insertelem->next = firstelement; 00096 if(firstelement != NULL) 00097 firstelement->prev = insertelem; 00098 firstelement = insertelem; 00099 if(lastelement == NULL) //if this is the first element to be inserted into the list 00100 lastelement = insertelem; 00101 AbstractSearchState1->listelem[listindex] = insertelem; 00102 currentsize++; 00103 }; 00104 00105 void insertinfront(AbstractSearchState *AbstractSearchState1, int listindex) 00106 { 00107 insert(AbstractSearchState1, listindex); 00108 }; 00109 00110 void remove(AbstractSearchState *AbstractSearchState1, int listindex) 00111 { 00112 if(currentsize == 0 || AbstractSearchState1->listelem[listindex] == NULL) 00113 { 00114 SBPL_ERROR("ERROR: delete: list does not contain the element\n"); 00115 throw new SBPL_Exception(); 00116 } 00117 if(AbstractSearchState1->listelem[listindex]->prev != NULL && 00118 AbstractSearchState1->listelem[listindex]->next != NULL) 00119 { 00120 //in the middle of the list 00121 AbstractSearchState1->listelem[listindex]->prev->next = 00122 AbstractSearchState1->listelem[listindex]->next; 00123 AbstractSearchState1->listelem[listindex]->next->prev = 00124 AbstractSearchState1->listelem[listindex]->prev; 00125 } 00126 else if(AbstractSearchState1->listelem[listindex]->prev != NULL) 00127 { 00128 //at the end of the list 00129 AbstractSearchState1->listelem[listindex]->prev->next = NULL; 00130 lastelement = AbstractSearchState1->listelem[listindex]->prev; 00131 } 00132 else if(AbstractSearchState1->listelem[listindex]->next != NULL) 00133 { 00134 //at the beginning of the list 00135 AbstractSearchState1->listelem[listindex]->next->prev = NULL; 00136 firstelement = AbstractSearchState1->listelem[listindex]->next; 00137 } 00138 else 00139 { 00140 //the only element in the list 00141 firstelement = NULL; 00142 lastelement = NULL; 00143 } 00144 //delete 00145 free(AbstractSearchState1->listelem[listindex]); 00146 AbstractSearchState1->listelem[listindex] = NULL; 00147 currentsize--; 00148 }; 00149 00150 00151 AbstractSearchState *getfirst() 00152 { 00153 if(firstelement == NULL) 00154 return NULL; 00155 else 00156 return firstelement->liststate; 00157 }; 00158 00159 AbstractSearchState *getlast() 00160 { 00161 if(lastelement == NULL) 00162 return NULL; 00163 else 00164 return lastelement->liststate; 00165 }; 00166 00167 00168 AbstractSearchState *getnext(AbstractSearchState* AbstractSearchState1, int listindex) 00169 { 00170 if(AbstractSearchState1->listelem[listindex]->next == NULL) 00171 return NULL; 00172 else 00173 return AbstractSearchState1->listelem[listindex]->next->liststate; 00174 }; 00175 00176 private: 00177 00178 00179 00180 //operators 00181 public: 00182 00183 00184 }; 00185 00186 00187 class CBucket 00188 { 00189 00190 //data 00191 public: 00192 //buckets are from first_priority to numofbuckets-3. Bucket numofbucket-2 contains elements with higher finite priorities (unordered). Last bucket contains infinite 00193 //priority elements 00194 vector<AbstractSearchState *>* bucketV; 00195 vector<int> assortedpriorityV; //contains the priorities of elements in the numofbuckets-2 bucket (mixed priorities) 00196 int firstpriority; 00197 int numofbuckets; 00198 int currentminelement_bucketind; 00199 int currentminelement_priority; 00200 int currentminelement_bucketVind; //index within the bucket 00201 00202 int maxassortedpriorityVsize; 00203 00204 //constructors 00205 public: 00206 CBucket(int first_priority, int max_bucketed_priority) 00207 { 00208 bucketV = NULL; 00209 firstpriority = 0; 00210 numofbuckets = 0; 00211 currentminelement_bucketind = INFINITECOST; 00212 currentminelement_priority = INFINITECOST; 00213 currentminelement_bucketVind = INFINITECOST; 00214 maxassortedpriorityVsize = 0; 00215 00216 reset(first_priority, max_bucketed_priority); 00217 }; 00218 ~CBucket() 00219 { 00220 if(bucketV != NULL) 00221 { 00222 makeemptybucketV(); 00223 00224 delete [] bucketV; 00225 bucketV = NULL; 00226 firstpriority = 0; 00227 numofbuckets = 0; 00228 00229 } 00230 }; 00231 00232 //functions 00233 public: 00234 bool empty() 00235 { 00236 return (currentminelement_bucketind == INFINITECOST); 00237 }; 00238 00239 bool reset(int first_priority, int max_bucketed_priority) 00240 { 00241 //delete the old ones 00242 if(numofbuckets != 0) 00243 { 00244 makeemptybucketV(); 00245 00246 delete [] bucketV; 00247 bucketV = NULL; 00248 firstpriority = 0; 00249 numofbuckets = 0; 00250 } 00251 00252 //compute the number of buckets 00253 numofbuckets = max_bucketed_priority - first_priority + 1; //this is how many priorities are one per buckets 00254 numofbuckets += 2; //one bucket for (max_bucketed_priority; infinity) and one bucket for infinity 00255 00256 //allocate memory 00257 bucketV = new vector<AbstractSearchState *>[numofbuckets]; 00258 00259 //currently all empty 00260 currentminelement_bucketind = INFINITECOST; 00261 currentminelement_priority = INFINITECOST; 00262 currentminelement_bucketVind = INFINITECOST; 00263 00264 //reset statistics 00265 maxassortedpriorityVsize = 0; 00266 00267 return true; 00268 }; 00269 00270 void makeemptybucketV() 00271 { 00272 for(int i = 0; i < numofbuckets; i++) 00273 { 00274 for(int eind = 0; eind < (int)bucketV[i].size(); eind++) 00275 { 00276 bucketV[i].at(eind)->heapindex = -1; 00277 } 00278 } 00279 assortedpriorityV.clear(); //clear the assorted priorities array 00280 00281 //currently all empty 00282 currentminelement_bucketind = INFINITECOST; 00283 currentminelement_priority = INFINITECOST; 00284 currentminelement_bucketVind = INFINITECOST; 00285 00286 }; 00287 00288 AbstractSearchState *getminelement() 00289 { 00290 if(currentminelement_bucketind == INFINITECOST) 00291 return NULL; 00292 #if DEBUG 00293 else if(currentminelement_bucketind >= numofbuckets) 00294 { 00295 SBPL_ERROR("ERROR: currentminelement_bucketind is invalid\n"); 00296 throw new SBPL_Exception(); 00297 } 00298 else if((int)bucketV[currentminelement_bucketind].size() <= currentminelement_bucketVind) 00299 { 00300 SBPL_ERROR("ERROR: failed to get minelement\n"); 00301 throw new SBPL_Exception(); 00302 } 00303 #endif 00304 else 00305 { 00306 return bucketV[currentminelement_bucketind].at(currentminelement_bucketVind); 00307 } 00308 00309 }; 00310 00311 AbstractSearchState *popminelement() 00312 { 00313 if(currentminelement_bucketind == INFINITECOST) 00314 return NULL; 00315 #if DEBUG 00316 else if(currentminelement_bucketind >= numofbuckets) 00317 { 00318 SBPL_ERROR("ERROR: currentminelement_bucketind is invalid\n"); 00319 throw new SBPL_Exception(); 00320 } 00321 else if((int)bucketV[currentminelement_bucketind].size() <= currentminelement_bucketVind) 00322 { 00323 SBPL_ERROR("ERROR: failed to get minelement\n"); 00324 throw new SBPL_Exception(); 00325 } 00326 #endif 00327 else 00328 { 00329 AbstractSearchState* AbstractSearchState1 = bucketV[currentminelement_bucketind].at(currentminelement_bucketVind); 00330 removestategivenbucketindex(AbstractSearchState1, currentminelement_bucketind); 00331 return AbstractSearchState1; 00332 } 00333 00334 }; 00335 00336 00337 int getminpriority() 00338 { 00339 return currentminelement_priority; 00340 }; 00341 void remove(AbstractSearchState *AbstractSearchState1, int priorityattimeofinsertion) 00342 { 00343 //compute bucketindex 00344 int bucketindex = priorityattimeofinsertion - firstpriority; 00345 if(priorityattimeofinsertion == INFINITECOST) 00346 bucketindex = numofbuckets-1; 00347 else if (bucketindex > numofbuckets-2) 00348 bucketindex = numofbuckets-2; 00349 00350 removestategivenbucketindex(AbstractSearchState1, bucketindex); 00351 }; 00352 00353 void insert(AbstractSearchState *AbstractSearchState1, int priority) 00354 { 00355 //compute bucket index 00356 int bucketindex = priority - firstpriority; 00357 if(priority == INFINITECOST) 00358 { 00359 bucketindex = numofbuckets - 1; 00360 } 00361 else if(bucketindex >= numofbuckets - 2) 00362 { 00363 bucketindex = numofbuckets - 2; //overflow bucket 00364 } 00365 00366 //insert the element itself 00367 bucketV[bucketindex].push_back(AbstractSearchState1); 00368 AbstractSearchState1->heapindex = (int)bucketV[bucketindex].size()-1; 00369 00370 if(bucketindex == numofbuckets - 2) 00371 { 00372 assortedpriorityV.push_back(priority); 00373 00374 if(maxassortedpriorityVsize < (int)assortedpriorityV.size()) 00375 maxassortedpriorityVsize = (int)assortedpriorityV.size(); 00376 } 00377 00378 //re-compute minelement if necessary 00379 if(priority < currentminelement_priority || currentminelement_priority == INFINITECOST) 00380 { 00381 recomputeminelementupfrombucket(bucketindex); 00382 } 00383 }; 00384 00385 00386 private: 00387 00388 void removestategivenbucketindex(AbstractSearchState *AbstractSearchState1, int bucketindex) 00389 { 00390 00391 int removeelemind = AbstractSearchState1->heapindex; 00392 int lastelemind = (int)bucketV[bucketindex].size()-1; 00393 00394 //put into its place the last element in the bucket 00395 if(bucketindex == numofbuckets-2) 00396 assortedpriorityV.at(removeelemind) = assortedpriorityV.at(lastelemind); 00397 bucketV[bucketindex].at(lastelemind)->heapindex = removeelemind; 00398 bucketV[bucketindex].at(removeelemind) = bucketV[bucketindex].at(lastelemind); 00399 00400 //clear the element itself 00401 AbstractSearchState1->heapindex = -1; 00402 00403 //remove the last element 00404 bucketV[bucketindex].pop_back(); 00405 00406 //re-compute minelement if necessary 00407 if(bucketindex == currentminelement_bucketind && ((int)bucketV[bucketindex].size() == 0 || currentminelement_bucketind == numofbuckets - 2)) 00408 { 00409 recomputeminelementupfrombucket(bucketindex); 00410 } 00411 }; 00412 00413 00414 void recomputeminelementupfrombucket(int startbucketindex) 00415 { 00416 //first find non-empty bucket 00417 int bind = -1; 00418 for (bind = startbucketindex; bind < numofbuckets; bind++) 00419 { 00420 if((int)bucketV[bind].size() != 0) 00421 break; 00422 } 00423 00424 if(bind < numofbuckets - 2) 00425 { 00426 //normal bucket 00427 currentminelement_bucketind = bind; 00428 currentminelement_priority = firstpriority + bind; 00429 currentminelement_bucketVind = 0; 00430 } 00431 else if(bind == (numofbuckets -2)) 00432 { 00433 //assorted priorities 00434 currentminelement_bucketind = bind; 00435 currentminelement_priority = INFINITECOST; 00436 currentminelement_bucketVind = INFINITECOST; 00437 for(int eind = 0; eind < (int)bucketV[bind].size(); eind++) 00438 { 00439 if(currentminelement_priority > assortedpriorityV.at(eind)) 00440 { 00441 currentminelement_priority = assortedpriorityV.at(eind); 00442 currentminelement_bucketVind = eind; 00443 } 00444 } 00445 if(currentminelement_priority == INFINITECOST) 00446 { 00447 SBPL_ERROR("ERROR: in recomputemin in buckets\n"); 00448 throw new SBPL_Exception(); 00449 } 00450 } 00451 else if(bind == (numofbuckets - 1)) 00452 { 00453 //bucket with infinite priorities 00454 currentminelement_bucketind = bind; 00455 currentminelement_priority = INFINITECOST; 00456 currentminelement_bucketVind = 0; 00457 } 00458 else 00459 { 00460 currentminelement_bucketind = INFINITECOST; 00461 currentminelement_priority = INFINITECOST; 00462 currentminelement_bucketVind = INFINITECOST; 00463 } 00464 00465 }; 00466 00467 }; 00468 00469 00470 class CSlidingBucket 00471 { 00472 00473 //data 00474 public: 00475 //fixed size buckets. Each bucket is of size bucketsize, and there are numofbuckets buckets. 00476 //all unoccupied buckets are supposed to be set to NULL 00477 AbstractSearchState*** bucketV; 00478 int* lastelementindexV; //index of the last element for each bucket 00479 int numofbuckets; 00480 int bucketsize; 00481 int currentminelement_bindex; //index of the bucket of the current min element 00482 int currentminelement_index; //index of the current minelement in the bucket 00483 int currentmaxelement_priority; //the maxelement priority in the list 00484 int currentminelement_priority; //the priority of the current minelement 00485 int currentfirstbucket_bindex; //index of the bucket that corresponds to the first bucket in the list (lowest priority) 00486 int currentfirstbucket_priority; //priority of the first bucket in the list 00487 00488 //constructors 00489 public: 00490 CSlidingBucket(int num_of_buckets, int bucket_size) 00491 { 00492 numofbuckets = num_of_buckets; 00493 bucketsize = bucket_size; 00494 00495 //allocate memory 00496 bucketV = new AbstractSearchState** [numofbuckets]; 00497 lastelementindexV = new int[numofbuckets]; 00498 for(int i = 0; i < numofbuckets; i++) 00499 { 00500 lastelementindexV[i] = -1; 00501 bucketV[i] = NULL; 00502 } 00503 00504 currentminelement_bindex = currentfirstbucket_bindex = 0; 00505 currentminelement_index = -1; 00506 currentmaxelement_priority = currentminelement_priority = currentfirstbucket_priority = 0; 00507 }; 00508 ~CSlidingBucket() 00509 { 00510 for(int i = 0; i < numofbuckets; i++) 00511 { 00512 if(bucketV[i] != NULL) 00513 { 00514 delete [] bucketV[i]; 00515 bucketV[i] = NULL; 00516 } 00517 } 00518 delete [] bucketV; 00519 bucketV = NULL; 00520 delete [] lastelementindexV; 00521 }; 00522 00523 //functions 00524 public: 00525 00526 inline bool empty() 00527 {return (currentminelement_index == -1 && currentmaxelement_priority == currentminelement_priority);}; 00528 00529 inline int getminkey() 00530 {return currentminelement_priority;}; 00531 00532 void reset() 00533 { 00534 currentminelement_bindex = currentfirstbucket_bindex = 0; 00535 currentminelement_index = -1; 00536 currentmaxelement_priority = currentminelement_priority = currentfirstbucket_priority = 0; 00537 for(int i = 0; i < numofbuckets; i++) 00538 { 00539 lastelementindexV[i] = -1; 00540 if(bucketV[i] == NULL) 00541 continue; 00542 for(int eind = 0; eind < bucketsize; eind++) 00543 bucketV[i][eind] = NULL; 00544 } 00545 00546 }; 00547 00548 AbstractSearchState *popminelement() 00549 { 00550 if(currentminelement_index == -1 && currentmaxelement_priority == currentminelement_priority) 00551 return NULL; 00552 else 00553 { 00554 AbstractSearchState* currentelement = NULL; 00555 if(currentminelement_index == -1 || bucketV[currentminelement_bindex] == NULL || bucketV[currentminelement_bindex][currentminelement_index] == NULL) 00556 currentelement = recomputeandreturnmin(); 00557 else 00558 currentelement = bucketV[currentminelement_bindex][currentminelement_index]; 00559 00560 //delete the element 00561 bucketV[currentminelement_bindex][currentminelement_index] = NULL; 00562 00563 //reset the first bucket to the element that was just delete 00564 currentfirstbucket_bindex = currentminelement_bindex; 00565 currentfirstbucket_priority = currentminelement_priority; 00566 00567 //recomputemin 00568 recomputeandreturnmin(); 00569 00570 return currentelement; 00571 } 00572 }; 00573 00574 AbstractSearchState *getminelement() 00575 { 00576 if(currentminelement_index == -1 && currentmaxelement_priority == currentminelement_priority) 00577 return NULL; 00578 else 00579 { 00580 AbstractSearchState* currentelement = NULL; 00581 if(currentminelement_index == -1 || bucketV[currentminelement_bindex] == NULL || bucketV[currentminelement_bindex][currentminelement_index] == NULL) 00582 currentelement = recomputeandreturnmin(); 00583 else 00584 currentelement = bucketV[currentminelement_bindex][currentminelement_index]; 00585 00586 return currentelement; 00587 } 00588 }; 00589 00590 void insert(AbstractSearchState *AbstractSearchState1, int priority) 00591 { 00592 //compute the index of the bucket where to put it in 00593 int bucket_increment = (priority - currentfirstbucket_priority); 00594 int bucket_index = (currentfirstbucket_bindex + bucket_increment)%numofbuckets; 00595 00596 if(bucket_increment >= numofbuckets || bucket_increment < 0) 00597 { 00598 SBPL_ERROR("ERROR: invalid priority=%d (currentfirstbucket_priority=%d) used with sliding buckets\n", priority, currentfirstbucket_priority); 00599 throw new SBPL_Exception(); 00600 } 00601 00602 //insert the element 00603 lastelementindexV[bucket_index]++; 00604 00605 if(lastelementindexV[bucket_index] == bucketsize) 00606 { 00607 SBPL_ERROR("ERROR: bucket %d is full (size=%d)\n", bucket_index, bucketsize); 00608 throw new SBPL_Exception(); 00609 } 00610 00611 if(bucketV[bucket_index] == NULL) 00612 createbucket(bucket_index); 00613 00614 bucketV[bucket_index][lastelementindexV[bucket_index]] = AbstractSearchState1; 00615 00616 //make sure maximum and minimum is correct 00617 if(priority > currentmaxelement_priority) 00618 currentmaxelement_priority = priority; 00619 if(priority < currentminelement_priority) 00620 { 00621 currentminelement_priority = priority; 00622 currentminelement_bindex = bucket_index; 00623 } 00624 00625 //special case for the only entry 00626 if(currentminelement_bindex == bucket_index && currentminelement_index == -1) 00627 { 00628 currentminelement_index = 0; 00629 } 00630 }; 00631 00632 private: 00633 00634 AbstractSearchState *recomputeandreturnmin() 00635 { 00636 if(currentminelement_index == -1 && currentmaxelement_priority == currentminelement_priority) 00637 return NULL; 00638 while(currentminelement_index == -1 || bucketV[currentminelement_bindex] == NULL || bucketV[currentminelement_bindex][currentminelement_index] == NULL) 00639 { 00640 //try incrementing element index 00641 if (currentminelement_index < lastelementindexV[currentminelement_bindex]) 00642 currentminelement_index++; 00643 else 00644 { 00645 //there are no more elements left in this bucket 00646 lastelementindexV[currentminelement_bindex] = -1; 00647 00648 //if it was the last bucket, then that is it 00649 if(currentminelement_priority == currentmaxelement_priority) 00650 { 00651 currentminelement_index = -1; 00652 currentmaxelement_priority = currentminelement_priority; 00653 return NULL; 00654 } 00655 00656 //try incrementing bucket index 00657 currentminelement_bindex = (currentminelement_bindex + 1)%numofbuckets; 00658 currentminelement_index = 0; 00659 currentminelement_priority++; 00660 } 00661 } 00662 return bucketV[currentminelement_bindex][currentminelement_index]; 00663 }; 00664 00665 void createbucket(int bucketindex) 00666 { 00667 if(bucketV[bucketindex] != NULL) 00668 { 00669 SBPL_ERROR("ERROR: trying to create a non-null bucket\n"); 00670 throw new SBPL_Exception(); 00671 } 00672 00673 bucketV[bucketindex] = new AbstractSearchState* [bucketsize]; 00674 for(int eind = 0; eind < bucketsize; eind++) 00675 bucketV[bucketindex][eind] = NULL; 00676 }; 00677 00678 }; 00679 00680 00681 #endif