sbpl: sbpl: heap.cpp Source File

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 #include <iostream>
00030 using namespace std;
00031 
00032 #include "../sbpl/headers.h"
00033 
00034 
00035 void heaperror(const char* ErrorString)
00036 {
00037   //need to send a message from here somehow
00038         SBPL_PRINTF("%s\n", ErrorString);
00039         throw new SBPL_Exception();
00040 }
00041 
00042 
00043 //returns an infinite key
00044 CKey InfiniteKey()
00045 {
00046         CKey key;
00047         key.SetKeytoInfinity();
00048         return key;
00049 
00050 }
00051 
00052 //---------------------------------normal (multi-priority) CHeap class---------------------------------------------------
00053 //constructors and destructors
00054 CHeap::CHeap()
00055 {
00056   percolates = 0;
00057   currentsize = 0;
00058   allocated = HEAPSIZE_INIT;
00059 
00060   heap = new heapelement[allocated];
00061 
00062 }
00063 
00064 CHeap::~CHeap()
00065 {
00066   int i;
00067   for (i=1; i<=currentsize; ++i)
00068     heap[i].heapstate->heapindex = 0;
00069 
00070   delete [] heap;
00071 }
00072 
00073 
00074 void CHeap::percolatedown(int hole, heapelement tmp)
00075 {
00076   int child;
00077 
00078   if (currentsize != 0)
00079   {
00080 
00081     for (; 2*hole <= currentsize; hole = child)
00082         {
00083           child = 2*hole;
00084 
00085           if (child != currentsize && heap[child+1].key < heap[child].key)
00086             ++child;
00087           if (heap[child].key < tmp.key)
00088             {
00089               percolates += 1;
00090               heap[hole] = heap[child];
00091               heap[hole].heapstate->heapindex = hole;
00092             }
00093           else
00094             break;
00095         }
00096       heap[hole] = tmp;
00097       heap[hole].heapstate->heapindex = hole;
00098    }
00099 }
00100 
00101 void CHeap::percolateup(int hole, heapelement tmp)
00102 {
00103   if (currentsize != 0)
00104     {
00105       for (; hole > 1 && tmp.key < heap[hole/2].key; hole /= 2)
00106           {
00107                 percolates += 1;
00108                 heap[hole] = heap[hole/2];
00109                 heap[hole].heapstate->heapindex = hole;
00110           }  
00111       heap[hole] = tmp;
00112       heap[hole].heapstate->heapindex = hole;
00113     }
00114 }
00115 
00116 void CHeap::percolateupordown(int hole, heapelement tmp)
00117 {
00118   if (currentsize != 0)
00119     {
00120       if (hole > 1 && heap[hole/2].key > tmp.key)
00121                 percolateup(hole, tmp);
00122       else
00123                 percolatedown(hole, tmp);
00124     }
00125 }
00126 
00127 bool CHeap::emptyheap()
00128 {
00129   return currentsize == 0;
00130 }
00131 
00132 
00133 bool CHeap::fullheap()
00134 {
00135   return currentsize == HEAPSIZE-1;
00136 }
00137 
00138 bool CHeap::inheap(AbstractSearchState *AbstractSearchState)
00139 {
00140   return (AbstractSearchState->heapindex != 0);
00141 }
00142 
00143 
00144 CKey CHeap::getkeyheap(AbstractSearchState *AbstractSearchState)
00145 {
00146   if (AbstractSearchState->heapindex == 0)
00147     heaperror("GetKey: AbstractSearchState is not in heap");
00148 
00149   return heap[AbstractSearchState->heapindex].key;
00150 }
00151 
00152 void CHeap::makeemptyheap()
00153 {
00154   int i;
00155 
00156   for (i=1; i<=currentsize; ++i)
00157     heap[i].heapstate->heapindex = 0;
00158   currentsize = 0;
00159 }
00160 
00161 void CHeap::makeheap()
00162 {
00163   int i;
00164 
00165   for (i = currentsize / 2; i > 0; i--)
00166     {
00167       percolatedown(i, heap[i]);
00168     }
00169 }
00170 
00171 void CHeap::growheap()
00172 {
00173   heapelement* newheap;
00174   int i;
00175 
00176   SBPL_PRINTF("growing heap size from %d ", allocated);
00177 
00178   allocated = 2*allocated;
00179   if(allocated > HEAPSIZE)
00180           allocated = HEAPSIZE;
00181 
00182   SBPL_PRINTF("to %d\n", allocated);
00183 
00184   newheap = new heapelement[allocated];
00185 
00186   for (i=0; i<=currentsize; ++i)
00187     newheap[i] = heap[i];
00188 
00189   delete [] heap;
00190 
00191   heap = newheap;
00192 }
00193 
00194 
00195 void CHeap::sizecheck()
00196 {
00197 
00198   if (fullheap())
00199     heaperror("insertheap: heap is full");
00200   else if(currentsize == allocated-1)
00201   {
00202         growheap();
00203   }
00204 }
00205 
00206 
00207 
00208 void CHeap::insertheap(AbstractSearchState *AbstractSearchState, CKey key)
00209 {
00210   heapelement tmp;
00211   char strTemp[100];
00212 
00213 
00214   sizecheck();
00215 
00216   if (AbstractSearchState->heapindex != 0)
00217     {
00218       sprintf(strTemp, "insertheap: AbstractSearchState is already in heap");
00219       heaperror(strTemp);
00220     }
00221   tmp.heapstate = AbstractSearchState;
00222   tmp.key = key;
00223   percolateup(++currentsize, tmp); 
00224 }
00225 
00226 void CHeap::deleteheap(AbstractSearchState *AbstractSearchState)
00227 {
00228   if (AbstractSearchState->heapindex == 0)
00229     heaperror("deleteheap: AbstractSearchState is not in heap");
00230   percolateupordown(AbstractSearchState->heapindex, heap[currentsize--]);
00231   AbstractSearchState->heapindex = 0;
00232 }
00233 
00234 void CHeap::updateheap(AbstractSearchState *AbstractSearchState, CKey NewKey)
00235 {
00236   if (AbstractSearchState->heapindex == 0)
00237     heaperror("Updateheap: AbstractSearchState is not in heap");
00238   if (heap[AbstractSearchState->heapindex].key != NewKey)
00239     {
00240       heap[AbstractSearchState->heapindex].key = NewKey;
00241       percolateupordown(AbstractSearchState->heapindex, heap[AbstractSearchState->heapindex]);
00242     }
00243 }
00244 
00245 AbstractSearchState* CHeap::getminheap()
00246 {
00247   if (currentsize == 0)
00248     heaperror("GetMinheap: heap is empty");
00249   return heap[1].heapstate;
00250 }
00251 
00252 AbstractSearchState* CHeap::getminheap(CKey& ReturnKey)
00253 {
00254   if (currentsize == 0)
00255     {
00256       heaperror("GetMinheap: heap is empty");
00257       ReturnKey = InfiniteKey();
00258     }
00259   ReturnKey = heap[1].key;
00260   return heap[1].heapstate;
00261 }
00262 
00263 CKey CHeap::getminkeyheap()
00264 {  
00265   CKey ReturnKey;
00266   if (currentsize == 0)
00267     return InfiniteKey();
00268   ReturnKey = heap[1].key;
00269   return ReturnKey;
00270 }
00271 
00272 AbstractSearchState* CHeap::deleteminheap()
00273 {
00274   AbstractSearchState *AbstractSearchState;
00275 
00276   if (currentsize == 0)
00277     heaperror("DeleteMin: heap is empty");
00278 
00279   AbstractSearchState = heap[1].heapstate;
00280   AbstractSearchState->heapindex = 0;
00281   percolatedown(1, heap[currentsize--]);
00282   return AbstractSearchState;
00283 }
00284 //---------------------------------end of normal (multi-priority) CHeap class---------------------------------------------------
00285 
00286 
00287 //---------------------------------single-priority CIntHeap class---------------------------------------------------
00288 //constructors and destructors
00289 CIntHeap::CIntHeap()
00290 {
00291   percolates = 0;
00292   currentsize = 0;
00293   allocated = HEAPSIZE_INIT;
00294 
00295   heap = new heapintelement[allocated];
00296 
00297 }
00298 
00299 CIntHeap::CIntHeap(int initial_size)
00300 {
00301   percolates = 0;
00302   currentsize = 0;
00303   allocated = initial_size;
00304 
00305   heap = new heapintelement[allocated];
00306 
00307 }
00308 
00309 
00310 
00311 CIntHeap::~CIntHeap()
00312 {
00313   int i;
00314   for (i=1; i<=currentsize; ++i)
00315     heap[i].heapstate->heapindex = 0;
00316 
00317   delete [] heap;
00318 }
00319 
00320 
00321 void CIntHeap::percolatedown(int hole, heapintelement tmp)
00322 {
00323   int child;
00324 
00325   if (currentsize != 0)
00326   {
00327 
00328     for (; 2*hole <= currentsize; hole = child)
00329         {
00330           child = 2*hole;
00331 
00332           if (child != currentsize && heap[child+1].key < heap[child].key)
00333             ++child;
00334           if (heap[child].key < tmp.key)
00335             {
00336               percolates += 1;
00337               heap[hole] = heap[child];
00338               heap[hole].heapstate->heapindex = hole;
00339             }
00340           else
00341             break;
00342         }
00343       heap[hole] = tmp;
00344       heap[hole].heapstate->heapindex = hole;
00345    }
00346 }
00347 
00348 void CIntHeap::percolateup(int hole, heapintelement tmp)
00349 {
00350   if (currentsize != 0)
00351     {
00352       for (; hole > 1 && tmp.key < heap[hole/2].key; hole /= 2)
00353           {
00354                 percolates += 1;
00355                 heap[hole] = heap[hole/2];
00356                 heap[hole].heapstate->heapindex = hole;
00357           }  
00358       heap[hole] = tmp;
00359       heap[hole].heapstate->heapindex = hole;
00360     }
00361 }
00362 
00363 void CIntHeap::percolateupordown(int hole, heapintelement tmp)
00364 {
00365   if (currentsize != 0)
00366     {
00367       if (hole > 1 && heap[hole/2].key > tmp.key)
00368                 percolateup(hole, tmp);
00369       else
00370                 percolatedown(hole, tmp);
00371     }
00372 }
00373 
00374 bool CIntHeap::emptyheap()
00375 {
00376   return currentsize == 0;
00377 }
00378 
00379 
00380 bool CIntHeap::fullheap()
00381 {
00382   return currentsize == HEAPSIZE-1;
00383 }
00384 
00385 bool CIntHeap::inheap(AbstractSearchState *AbstractSearchState)
00386 {
00387   return (AbstractSearchState->heapindex != 0);
00388 }
00389 
00390 
00391 int CIntHeap::getkeyheap(AbstractSearchState *AbstractSearchState)
00392 {
00393   if (AbstractSearchState->heapindex == 0)
00394     heaperror("GetKey: AbstractSearchState is not in heap");
00395 
00396   return heap[AbstractSearchState->heapindex].key;
00397 }
00398 
00399 void CIntHeap::makeemptyheap()
00400 {
00401   int i;
00402 
00403   for (i=1; i<=currentsize; ++i)
00404     heap[i].heapstate->heapindex = 0;
00405   currentsize = 0;
00406 }
00407 
00408 void CIntHeap::makeheap()
00409 {
00410   int i;
00411 
00412   for (i = currentsize / 2; i > 0; i--)
00413     {
00414       percolatedown(i, heap[i]);
00415     }
00416 }
00417 
00418 void CIntHeap::growheap()
00419 {
00420   heapintelement* newheap;
00421   int i;
00422 
00423   SBPL_PRINTF("growing heap size from %d ", allocated);
00424 
00425   allocated = 2*allocated;
00426   if(allocated > HEAPSIZE)
00427           allocated = HEAPSIZE;
00428 
00429   SBPL_PRINTF("to %d\n", allocated);
00430 
00431   newheap = new heapintelement[allocated];
00432 
00433   for (i=0; i<=currentsize; ++i)
00434     newheap[i] = heap[i];
00435 
00436   delete [] heap;
00437 
00438   heap = newheap;
00439 }
00440 
00441 
00442 void CIntHeap::sizecheck()
00443 {
00444 
00445   if (fullheap())
00446     heaperror("insertheap: heap is full");
00447   else if(currentsize == allocated-1)
00448   {
00449         growheap();
00450   }
00451 }
00452 
00453 
00454 
00455 void CIntHeap::insertheap(AbstractSearchState *AbstractSearchState, int key)
00456 {
00457   heapintelement tmp;
00458   char strTemp[100];
00459 
00460   sizecheck();
00461 
00462   if (AbstractSearchState->heapindex != 0)
00463     {
00464       sprintf(strTemp, "insertheap: AbstractSearchState is already in heap");
00465       heaperror(strTemp);
00466     }
00467   tmp.heapstate = AbstractSearchState;
00468   tmp.key = key;
00469   percolateup(++currentsize, tmp); 
00470 }
00471 
00472 void CIntHeap::deleteheap(AbstractSearchState *AbstractSearchState)
00473 {
00474   if (AbstractSearchState->heapindex == 0)
00475     heaperror("deleteheap: AbstractSearchState is not in heap");
00476   percolateupordown(AbstractSearchState->heapindex, heap[currentsize--]);
00477   AbstractSearchState->heapindex = 0;
00478 }
00479 
00480 void CIntHeap::updateheap(AbstractSearchState *AbstractSearchState, int NewKey)
00481 {
00482   if (AbstractSearchState->heapindex == 0)
00483     heaperror("Updateheap: AbstractSearchState is not in heap");
00484   if (heap[AbstractSearchState->heapindex].key != NewKey)
00485     {
00486       heap[AbstractSearchState->heapindex].key = NewKey;
00487       percolateupordown(AbstractSearchState->heapindex, heap[AbstractSearchState->heapindex]);
00488     }
00489 }
00490 
00491 AbstractSearchState* CIntHeap::getminheap()
00492 {
00493   if (currentsize == 0)
00494     heaperror("GetMinheap: heap is empty");
00495   return heap[1].heapstate;
00496 }
00497 
00498 AbstractSearchState* CIntHeap::getminheap(int& ReturnKey)
00499 {
00500   if (currentsize == 0)
00501     {
00502       heaperror("GetMinheap: heap is empty");
00503     }
00504   ReturnKey = heap[1].key;
00505   return heap[1].heapstate;
00506 }
00507 
00508 int CIntHeap::getminkeyheap()
00509 {  
00510   int ReturnKey;
00511   if (currentsize == 0)
00512     return INFINITECOST;
00513   ReturnKey = heap[1].key;
00514   return ReturnKey;
00515 }
00516 
00517 AbstractSearchState* CIntHeap::deleteminheap()
00518 {
00519   AbstractSearchState *AbstractSearchState;
00520 
00521   if (currentsize == 0)
00522     heaperror("DeleteMin: heap is empty");
00523 
00524   AbstractSearchState = heap[1].heapstate;
00525   AbstractSearchState->heapindex = 0;
00526   percolatedown(1, heap[currentsize--]);
00527   return AbstractSearchState;
00528 }
00529 //---------------------------------end of single-priority CIntHeap class---------------------------------------------------
00530 
00531