VastDefs.cc

Go to the documentation of this file.
00001 //
00002 // Copyright (C) 2006 Institut fuer Telematik, Universitaet Karlsruhe (TH)
00003 //
00004 // This program is free software; you can redistribute it and/or
00005 // modify it under the terms of the GNU General Public License
00006 // as published by the Free Software Foundation; either version 2
00007 // of the License, or (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00017 //
00018 
00024 #include "VastDefs.h"
00025 
00026 Site::Site()
00027 {
00028     type = UNDEF;
00029     innerEdge[0] = false;
00030     innerEdge[1] = false;
00031     innerEdge[2] = false;
00032     outerEdge = false;
00033     isAdded = false;
00034     neighborCount = 0;
00035     addr = NodeHandle::UNSPECIFIED_NODE;
00036     tstamp = 0.0;
00037 }
00038 
00039 std::ostream& operator<<(std::ostream& Stream, const Site s)
00040 {
00041     Stream  << "Type: ";
00042     if(s.type & UNDEF) Stream << "Undefined ";
00043     if(s.type & THIS) Stream << "This ";
00044     if(s.type & ENCLOSING) Stream << "Enclosing ";
00045     if(s.type & NEIGHBOR) Stream << "Inner ";
00046     if(s.type & BOUNDARY) Stream << "Boundary ";
00047     if(s.type & NEW) Stream << "Discovered ";
00048     return Stream << "  IP: " << s.addr.getAddress();
00049 }
00050 
00051 Edge::Edge()
00052 {
00053     a = 0.0;
00054     b = 0.0;
00055     c = 0.0;
00056     ep[0] = NULL;
00057     ep[1] = NULL;
00058     reg[0] = NULL;
00059     reg[1] = NULL;
00060 }
00061 
00062 Halfedge::Halfedge()
00063 {
00064     ELleft = NULL;
00065     ELright = NULL;
00066     ELedge = NULL;
00067     ELpm = 0;
00068     vertex = NULL;
00069     ystar = 0.0;
00070     PQnext = NULL;
00071 }
00072 
00073 HeapPQ::HeapPQ()
00074 {
00075     PQhash = NULL;
00076 }
00077 
00078 void HeapPQ::PQinitialize(int sqrt_nsites, double ymin, double deltay)
00079 {
00080     PQcount = 0;
00081     PQmin = 0;
00082     PQhashsize = 4 * sqrt_nsites;
00083     PQhash = new Halfedge[PQhashsize];
00084     this->ymin = ymin;
00085     this->deltay = deltay;
00086 }
00087 
00088 void HeapPQ::PQreset()
00089 {
00090     delete[] PQhash;
00091 }
00092 
00093 void HeapPQ::PQinsert(Halfedge *he, Site *v, double offset)
00094 {
00095     Halfedge *last, *next;
00096 
00097     he->vertex = v;
00098     he->ystar = v->coord.y + offset;
00099     last = &PQhash[PQbucket(he)];
00100     while((next = last->PQnext) != NULL && (he->ystar  > next->ystar  || (he->ystar == next->ystar && v->coord.x > next->vertex->coord.x))) {
00101         last = next;
00102     }
00103     he->PQnext = last->PQnext;
00104     last->PQnext = he;
00105     PQcount++;
00106 }
00107 
00108 void HeapPQ::PQdelete(Halfedge *he)
00109 {
00110     Halfedge *last;
00111 
00112     if(he->vertex != NULL) {
00113         last = &PQhash[PQbucket(he)];
00114         while(last->PQnext != he) last = last->PQnext;
00115 
00116         last->PQnext = he->PQnext;
00117         PQcount--;
00118         he->vertex = NULL;
00119     }
00120 }
00121 
00122 int HeapPQ::PQbucket(Halfedge *he)
00123 {
00124     int bucket;
00125 
00126     bucket = (int)((he->ystar - ymin)/deltay) * PQhashsize;
00127     if(bucket < 0) bucket = 0;
00128     if(bucket >= PQhashsize) bucket = PQhashsize-1;
00129     if(bucket < PQmin) PQmin = bucket;
00130     return bucket;
00131 }
00132 
00133 int HeapPQ::PQempty()
00134 {
00135     return PQcount==0;
00136 }
00137 
00138 Vector2D HeapPQ::PQ_min()
00139 {
00140     Vector2D answer;
00141 
00142     while(PQhash[PQmin].PQnext == NULL) PQmin++;
00143 
00144     answer.x = PQhash[PQmin].PQnext->vertex->coord.x;
00145     answer.y = PQhash[PQmin].PQnext->ystar;
00146     return answer;
00147 }
00148 
00149 Halfedge* HeapPQ::PQextractmin()
00150 {
00151     Halfedge *curr;
00152 
00153     curr = PQhash[PQmin].PQnext;
00154     PQhash[PQmin].PQnext = curr->PQnext;
00155     PQcount--;
00156     return curr;
00157 }
00158 
00159 void Geometry::initialize(double deltax, double deltay, Vector2D center, Vector2D old_pos, Vector2D new_pos, double radius)
00160 {
00161     this->deltay = deltay;
00162     this->deltax = deltax;
00163     this->center[0] = center;
00164     this->center[1] = old_pos;
00165     this->center[2] = new_pos;
00166     if((old_pos.x != new_pos.x) || (old_pos.y != new_pos.y)) doDiscovery = true;
00167     else doDiscovery = false;
00168     this->sq_radius = radius*radius;
00169 }
00170 
00171 void Geometry::reset()
00172 {
00173     for(std::vector<Site*>::iterator itTemp = SITEVector.begin(); itTemp != SITEVector.end(); ++itTemp) {
00174         delete *itTemp;
00175     }
00176     for(std::vector<Edge*>::iterator itTemp = EDGEVector.begin(); itTemp != EDGEVector.end(); ++itTemp) {
00177         delete *itTemp;
00178     }
00179     SITEVector.clear();
00180     EDGEVector.clear();
00181 }
00182 
00183 void Geometry::setDebug(bool debugOutput)
00184 {
00185     this->debugOutput = debugOutput;
00186 }
00187 
00188 bool Geometry::intersectCircleSite(Site *s, Vector2D center)
00189 {
00190     double sq_distance;
00191     Vector2D temp;
00192     temp.x = s->coord.x - center.x;
00193     temp.y = s->coord.y - center.y;
00194     sq_distance = temp.x*temp.x + temp.y*temp.y;
00195     return sq_distance < sq_radius ? true : false;
00196 }
00197 
00198 bool Geometry::intersectCircleLine(Vector2D start, Vector2D dir, Vector2D center, bool lowerBound, bool upperBound)
00199 {
00200     Vector2D StartMinusCenter;
00201     double DirDotStartMinusCenter, DirSq, StartMinusCenterSq, discriminant;
00202     StartMinusCenter.x = start.x - center.x;
00203     StartMinusCenter.y = start.y - center.y;
00204     StartMinusCenterSq = StartMinusCenter.x * StartMinusCenter.x + StartMinusCenter.y * StartMinusCenter.y;
00205 
00206     DirDotStartMinusCenter = dir.x * StartMinusCenter.x + dir.y * StartMinusCenter.y;
00207     DirSq = dir.x * dir.x + dir.y * dir.y;
00208 
00209     discriminant = DirDotStartMinusCenter * DirDotStartMinusCenter - DirSq * (StartMinusCenterSq - sq_radius);
00210 
00211     if(discriminant <= 0.0f) return false;
00212     else if(lowerBound) {
00213         double s = (-DirDotStartMinusCenter - sqrtf(discriminant)) / DirSq;
00214         if(s < 0.0f) return false;
00215         else if(upperBound && s > 1.0f) return false;
00216     }
00217     return true;
00218 }
00219 
00220 void Geometry::processEdge(Edge *e)
00221 {
00222     bool leftEndpoint_In[3], rightEndpoint_In[3];
00223     int i, numTest;
00224     // test the edge just against our own AOI or also against AOI's of a moving neighbor
00225     numTest = doDiscovery ? 3 : 1;
00226 
00227     for(i = 0; i < numTest; i++) {
00228         if(e->ep[le]) leftEndpoint_In[i] = intersectCircleSite(e->ep[le], center[i]);
00229         else leftEndpoint_In[i] = false;
00230         if(e->ep[re]) rightEndpoint_In[i] = intersectCircleSite(e->ep[re], center[i]);
00231         else rightEndpoint_In[i] = false;
00232     }
00233     for(i = 0; i < numTest; i++) {
00234         if(leftEndpoint_In[i] || rightEndpoint_In[i]) {
00235             if(!e->reg[le]->innerEdge[i]) e->reg[le]->innerEdge[i] = true;
00236             if(!e->reg[re]->innerEdge[i]) e->reg[re]->innerEdge[i] = true;
00237         }
00238     }
00239     if(!leftEndpoint_In[0] || !rightEndpoint_In[0]) {
00240         if(!e->reg[le]->outerEdge) e->reg[le]->outerEdge = true;
00241         if(!e->reg[re]->outerEdge) e->reg[re]->outerEdge = true;
00242     }
00243     for(i = 0; i < numTest; i++) {
00244         if(!(leftEndpoint_In[i] || rightEndpoint_In[i])) {
00245             bool lineTest = false;
00246             if(e->ep[le] && e->ep[re]) {
00247                 Vector2D t_dir;
00248                 t_dir.x = e->ep[re]->coord.x - e->ep[le]->coord.x;
00249                 t_dir.y = e->ep[re]->coord.y - e->ep[le]->coord.y;
00250                 lineTest = intersectCircleLine(e->ep[le]->coord, t_dir, center[i], true, true);
00251             }
00252             if((e->ep[le] && !e->ep[re]) || (!e->ep[le] && e->ep[re])) {
00253                 Vector2D t_dir;
00254                 t_dir.x = e->b;
00255                 t_dir.y = -(e->a);
00256                 if(e->ep[le]) {
00257                     if(t_dir.x < 0.0f) {
00258                         t_dir.x = -t_dir.x;
00259                         t_dir.y = -t_dir.y;
00260                     }
00261                     lineTest = intersectCircleLine(e->ep[le]->coord, t_dir, center[i], true, false);
00262                 }
00263                 else {
00264                     if(t_dir.x >= 0.0f) {
00265                         t_dir.x = -t_dir.x;
00266                         t_dir.y = -t_dir.y;
00267                     }
00268                     lineTest = intersectCircleLine(e->ep[re]->coord, t_dir, center[i], true, false);
00269                 }
00270             }
00271             if(!(e->ep[le] || e->ep[re])) {
00272                 Vector2D t_start, t_dir;
00273                 if(e->b == 0.0f) {
00274                     t_start.x = e->c / e->a;
00275                     t_start.y = 0.0f;
00276                 }
00277                 else {
00278                     t_start.x = 0.0f;
00279                     t_start.y = e->c / e->b;
00280                 }
00281                 t_dir.x = e->b;
00282                 t_dir.y = -(e->a);
00283                 lineTest = intersectCircleLine(t_start, t_dir, center[i], false, false);
00284             }
00285 
00286             if(lineTest) {
00287                 if(!e->reg[le]->innerEdge[i]) e->reg[le]->innerEdge[i] = true;
00288                 if(!e->reg[re]->innerEdge[i]) e->reg[re]->innerEdge[i] = true;
00289             }
00290         }
00291     }
00292     // test if one of the nodes bisected by the edge is an enclosing neighbor
00293     if(e->reg[le]->type == THIS) {
00294         e->reg[re]->type |= ENCLOSING;
00295         // Debug output
00296         if(debugOutput) EV << "[Geometry::processEdge()]\n"
00297                            << "    Site at [" << e->reg[re]->coord.x << ", " << e->reg[re]->coord.y << "] is a enclosing neighbor."
00298                            << endl;
00299     }
00300     if(e->reg[re]->type == THIS) {
00301         e->reg[le]->type |= ENCLOSING;
00302         // Debug output
00303         if(debugOutput) EV << "[Geometry::processEdge()]\n"
00304                            << "    Site at [" << e->reg[le]->coord.x << ", " << e->reg[le]->coord.y << "] is a enclosing neighbor."
00305                            << endl;
00306     }
00307 }
00308 
00309 Edge* Geometry::bisect(Site *s1, Site *s2)
00310 {
00311     double dx, dy, adx, ady;
00312     Edge *newedge;
00313 
00314     newedge = new Edge;
00315 
00316     newedge->reg[0] = s1;
00317     newedge->reg[1] = s2;
00318     newedge->ep[0] = NULL;
00319     newedge->ep[1] = NULL;
00320 
00321     dx = s2->coord.x - s1->coord.x;
00322     dy = s2->coord.y - s1->coord.y;
00323     adx = dx > 0 ? dx : -dx;
00324     ady = dy > 0 ? dy : -dy;
00325     newedge->c = s1->coord.x * dx + s1->coord.y * dy + (dx*dx + dy*dy)*0.5;
00326     if(adx>ady) {
00327         newedge->a = 1.0;
00328         newedge->b = dy/dx;
00329         newedge->c /= dx;
00330     }
00331     else {
00332         newedge->b = 1.0;
00333         newedge->a = dx/dy;
00334         newedge->c /= dy;
00335     }
00336 
00337     EDGEVector.push_back(newedge);
00338     return newedge;
00339 }
00340 
00341 Site* Geometry::intersect(Halfedge *el1, Halfedge *el2)
00342 {
00343     Edge *e1, *e2, *e;
00344     Halfedge *el;
00345     double d, xint, yint;
00346     int right_of_site;
00347     Site *v;
00348 
00349     e1 = el1->ELedge;
00350     e2 = el2->ELedge;
00351     if(e1 == NULL || e2 == NULL) return NULL;
00352     if(e1->reg[1] == e2->reg[1]) return NULL;
00353 
00354     d = e1->a * e2->b - e1->b * e2->a;
00355     if(-1.0e-10 < d && d < 1.0e-10) return NULL;
00356 
00357     xint = (e1->c * e2->b - e2->c * e1->b)/d;
00358     yint = (e2->c * e1->a - e1->c * e2->a)/d;
00359 
00360     if((e1->reg[1]->coord.y < e2->reg[1]->coord.y) || (e1->reg[1]->coord.y == e2->reg[1]->coord.y && e1->reg[1]->coord.x < e2->reg[1]->coord.x)) {
00361         el = el1;
00362         e = e1;
00363     }
00364     else {
00365         el = el2;
00366         e = e2;
00367     }
00368 
00369     right_of_site = xint >= e->reg[1]->coord.x;
00370     if((right_of_site && el->ELpm == le) || (!right_of_site && el->ELpm == re)) return NULL;
00371 
00372     v = new Site;
00373 
00374     v->coord.x = xint;
00375     v->coord.y = yint;
00376 
00377     SITEVector.push_back(v);
00378     return v;
00379 }
00380 
00381 void Geometry::endpoint(Edge *e, int lr, Site *s)
00382 {
00383     e->ep[lr] = s;
00384     if(e->ep[re-lr] == NULL) return;
00385     processEdge(e);
00386 }
00387 
00388 double Geometry::dist(Site *s, Site *t)
00389 {
00390     double dx, dy;
00391     dx = s->coord.x - t->coord.x;
00392     dy = s->coord.y - t->coord.y;
00393     return sqrt(dx*dx + dy*dy);
00394 }
00395 
00396 EdgeList::EdgeList()
00397 {
00398     ELhash = NULL;
00399 }
00400 
00401 void EdgeList::initialize(int sqrt_nsites, double xmin, double deltax, Site *bottomsite)
00402 {
00403     int i;
00404 
00405     HEcount = 0;
00406     ELhashsize = 2 * sqrt_nsites;
00407 
00408     HEmemmap = new Halfedge*[ELhashsize];
00409 
00410     ELhash = new Halfedge*[ELhashsize];
00411     for(i=0; i<ELhashsize; i++) ELhash[i] = NULL;
00412     ELleftend = HEcreate(NULL, 0);
00413     ELrightend = HEcreate(NULL, 0);
00414     ELleftend->ELleft = NULL;
00415     ELleftend->ELright = ELrightend;
00416     ELrightend->ELleft = ELleftend;
00417     ELrightend->ELright = NULL;
00418     ELhash[0] = ELleftend;
00419     ELhash[ELhashsize-1] = ELrightend;
00420     this->xmin = xmin;
00421     this->deltax = deltax;
00422     this->bottomsite = bottomsite;
00423 }
00424 
00425 void EdgeList::reset()
00426 {
00427     delete[] ELhash;
00428     // free all allocated memory
00429     for(int i=0; i<HEcount; i++) delete HEmemmap[i];
00430     delete[] HEmemmap;
00431 }
00432 
00433 Halfedge* EdgeList::HEcreate(Edge *e, int pm)
00434 {
00435     Halfedge *answer = new Halfedge;
00436     answer->ELedge = e;
00437     answer->ELpm = pm;
00438 
00439     HEmemmap[HEcount++] = answer;
00440     if(HEcount%ELhashsize == 0) {
00441         Halfedge **Temp = new Halfedge*[HEcount + ELhashsize];
00442         for(int i=0; i<HEcount; i++) Temp[i] = HEmemmap[i];
00443         delete[] HEmemmap;
00444         HEmemmap = Temp;
00445     }
00446     return answer;
00447 }
00448 
00449 void EdgeList::ELinsert(Halfedge *lb, Halfedge *new_he)
00450 {
00451     new_he->ELleft = lb;
00452     new_he->ELright = lb->ELright;
00453     (lb->ELright)->ELleft = new_he;
00454     lb->ELright = new_he;
00455 }
00456 
00457 /* Get entry from hash table, pruning any deleted nodes */
00458 Halfedge* EdgeList::ELgethash(int b)
00459 {
00460     Halfedge *he;
00461 
00462     if(b < 0 || b >= ELhashsize) return NULL;
00463     he = ELhash[b];
00464     if(he == NULL || he->ELedge != (Edge*)DELETED) return he;
00465 
00466     /* Hash table points to deleted half edge. */
00467     ELhash[b] = NULL;
00468     return NULL;
00469 }
00470 
00471 /* returns 1 if p is to right of halfedge e */
00472 int EdgeList::right_of(Halfedge *el, Vector2D *p)
00473 {
00474     Edge *e;
00475     Site *topsite;
00476     int right_of_site, above, fast;
00477     double dxp, dyp, dxs, t1, t2, t3, yl;
00478 
00479     e = el->ELedge;
00480     topsite = e->reg[1];
00481     right_of_site = p->x > topsite->coord.x;
00482     if(right_of_site && el->ELpm == le) return 1;
00483     if(!right_of_site && el->ELpm == re) return 0;
00484 
00485     if(e->a == 1.0) {
00486         dyp = p->y - topsite->coord.y;
00487         dxp = p->x - topsite->coord.x;
00488         fast = 0;
00489         if((!right_of_site && (e->b < 0.0)) || (right_of_site && (e->b >= 0.0))) {
00490             above = dyp >= e->b * dxp;
00491             fast = above;
00492         }
00493         else {
00494             above = p->x + p->y * e->b > e->c;
00495             if(e->b < 0.0) above = !above;
00496             if(!above) fast = 1;
00497         }
00498         if(!fast) {
00499             dxs = topsite->coord.x - (e->reg[0])->coord.x;
00500             above = e->b * (dxp*dxp - dyp*dyp) < dxs * dyp * (1.0 + 2.0*dxp/dxs + e->b*e->b);
00501             if(e->b < 0.0) above = !above;
00502         }
00503     }
00504     else {
00505         yl = e->c - e->a * p->x;
00506         t1 = p->y - yl;
00507         t2 = p->x - topsite->coord.x;
00508         t3 = yl - topsite->coord.y;
00509         above = t1*t1 > t2*t2 + t3*t3;
00510     }
00511     return el->ELpm == le ? above : !above;
00512 }
00513 
00514 Halfedge* EdgeList::ELleftbnd(Vector2D *p)
00515 {
00516     int i, bucket;
00517     Halfedge *he;
00518 
00519     /* Use hash table to get close to desired halfedge */
00520     bucket = (int)((p->x - xmin) / deltax) * ELhashsize;
00521     if(bucket < 0) bucket =0;
00522     if(bucket >= ELhashsize) bucket = ELhashsize - 1;
00523     he = ELgethash(bucket);
00524     if(he == NULL) {
00525         for(i=1; 1; i++) {
00526             if((he = ELgethash(bucket-i)) != NULL) break;
00527             if((he = ELgethash(bucket+i)) != NULL) break;
00528         }
00529     totalsearch++;
00530     }
00531     ntry++;
00532     /* Now search linear list of halfedges for the corect one */
00533     if(he == ELleftend  || (he != ELrightend && right_of(he, p))) {
00534         do {he = he->ELright;} while(he != ELrightend && right_of(he, p));
00535         he = he->ELleft;
00536     }
00537     else do {he = he->ELleft;} while(he != ELleftend && !right_of(he, p));
00538 
00539     /* Update hash table and reference counts */
00540     if(bucket > 0 && bucket < ELhashsize-1) {
00541         ELhash[bucket] = he;
00542     }
00543     return he;
00544 }
00545 
00546 void EdgeList::ELdelete(Halfedge *he)
00547 {
00548     (he->ELleft)->ELright = he->ELright;
00549     (he->ELright)->ELleft = he->ELleft;
00550     he->ELedge = (Edge*)DELETED;
00551 }
00552 
00553 Halfedge* EdgeList::ELright(Halfedge *he)
00554 {
00555     return he->ELright;
00556 }
00557 
00558 Halfedge* EdgeList::ELleft(Halfedge *he)
00559 {
00560     return he->ELleft;
00561 }
00562 
00563 
00564 Site* EdgeList::leftreg(Halfedge *he)
00565 {
00566     if(he->ELedge == NULL) return(bottomsite);
00567     return he->ELpm == le ? he->ELedge->reg[le] : he->ELedge->reg[re];
00568 }
00569 
00570 Site* EdgeList::rightreg(Halfedge *he)
00571 {
00572     if(he->ELedge == NULL) return(bottomsite);
00573     return he->ELpm == le ? he->ELedge->reg[re] : he->ELedge->reg[le];
00574 }

Generated on Tue Sep 8 17:26:54 2009 for OverSim by  doxygen 1.5.8