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     // enhanced enclosing test
00293     e->reg[re]->enclosingSet.insert(e->reg[le]->addr);
00294     e->reg[le]->enclosingSet.insert(e->reg[re]->addr);
00295     
00296     // test if one of the nodes bisected by the edge is an enclosing neighbor
00297     if(e->reg[le]->type == THIS) {
00298         e->reg[re]->type |= ENCLOSING;
00299         // Debug output
00300         if(debugOutput) EV << "[Geometry::processEdge()]\n"
00301                            << "    Site at [" << e->reg[re]->coord.x << ", " << e->reg[re]->coord.y << "] is an enclosing neighbor."
00302                            << endl;
00303     }
00304     if(e->reg[re]->type == THIS) {
00305         e->reg[le]->type |= ENCLOSING;
00306         // Debug output
00307         if(debugOutput) EV << "[Geometry::processEdge()]\n"
00308                            << "    Site at [" << e->reg[le]->coord.x << ", " << e->reg[le]->coord.y << "] is an enclosing neighbor."
00309                            << endl;
00310     }
00311 }
00312 
00313 Edge* Geometry::bisect(Site *s1, Site *s2)
00314 {
00315     double dx, dy, adx, ady;
00316     Edge *newedge;
00317 
00318     newedge = new Edge;
00319 
00320     newedge->reg[0] = s1;
00321     newedge->reg[1] = s2;
00322     newedge->ep[0] = NULL;
00323     newedge->ep[1] = NULL;
00324 
00325     dx = s2->coord.x - s1->coord.x;
00326     dy = s2->coord.y - s1->coord.y;
00327     adx = dx > 0 ? dx : -dx;
00328     ady = dy > 0 ? dy : -dy;
00329     newedge->c = s1->coord.x * dx + s1->coord.y * dy + (dx*dx + dy*dy)*0.5;
00330     if(adx>ady) {
00331         newedge->a = 1.0;
00332         newedge->b = dy/dx;
00333         newedge->c /= dx;
00334     }
00335     else {
00336         newedge->b = 1.0;
00337         newedge->a = dx/dy;
00338         newedge->c /= dy;
00339     }
00340 
00341     EDGEVector.push_back(newedge);
00342     return newedge;
00343 }
00344 
00345 Site* Geometry::intersect(Halfedge *el1, Halfedge *el2)
00346 {
00347     Edge *e1, *e2, *e;
00348     Halfedge *el;
00349     double d, xint, yint;
00350     int right_of_site;
00351     Site *v;
00352 
00353     e1 = el1->ELedge;
00354     e2 = el2->ELedge;
00355     if(e1 == NULL || e2 == NULL) return NULL;
00356     if(e1->reg[1] == e2->reg[1]) return NULL;
00357 
00358     d = e1->a * e2->b - e1->b * e2->a;
00359     if(-1.0e-10 < d && d < 1.0e-10) return NULL;
00360 
00361     xint = (e1->c * e2->b - e2->c * e1->b)/d;
00362     yint = (e2->c * e1->a - e1->c * e2->a)/d;
00363 
00364     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)) {
00365         el = el1;
00366         e = e1;
00367     }
00368     else {
00369         el = el2;
00370         e = e2;
00371     }
00372 
00373     right_of_site = xint >= e->reg[1]->coord.x;
00374     if((right_of_site && el->ELpm == le) || (!right_of_site && el->ELpm == re)) return NULL;
00375 
00376     v = new Site;
00377 
00378     v->coord.x = xint;
00379     v->coord.y = yint;
00380 
00381     SITEVector.push_back(v);
00382     return v;
00383 }
00384 
00385 void Geometry::endpoint(Edge *e, int lr, Site *s)
00386 {
00387     e->ep[lr] = s;
00388     if(e->ep[re-lr] == NULL) return;
00389     processEdge(e);
00390 }
00391 
00392 double Geometry::dist(Site *s, Site *t)
00393 {
00394     double dx, dy;
00395     dx = s->coord.x - t->coord.x;
00396     dy = s->coord.y - t->coord.y;
00397     return sqrt(dx*dx + dy*dy);
00398 }
00399 
00400 EdgeList::EdgeList()
00401 {
00402     ELhash = NULL;
00403 }
00404 
00405 void EdgeList::initialize(int sqrt_nsites, double xmin, double deltax, Site *bottomsite)
00406 {
00407     int i;
00408 
00409     HEcount = 0;
00410     ELhashsize = 2 * sqrt_nsites;
00411 
00412     HEmemmap = new Halfedge*[ELhashsize];
00413 
00414     ELhash = new Halfedge*[ELhashsize];
00415     for(i=0; i<ELhashsize; i++) ELhash[i] = NULL;
00416     ELleftend = HEcreate(NULL, 0);
00417     ELrightend = HEcreate(NULL, 0);
00418     ELleftend->ELleft = NULL;
00419     ELleftend->ELright = ELrightend;
00420     ELrightend->ELleft = ELleftend;
00421     ELrightend->ELright = NULL;
00422     ELhash[0] = ELleftend;
00423     ELhash[ELhashsize-1] = ELrightend;
00424     this->xmin = xmin;
00425     this->deltax = deltax;
00426     this->bottomsite = bottomsite;
00427 }
00428 
00429 void EdgeList::reset()
00430 {
00431     delete[] ELhash;
00432     // free all allocated memory
00433     for(int i=0; i<HEcount; i++) delete HEmemmap[i];
00434     delete[] HEmemmap;
00435 }
00436 
00437 Halfedge* EdgeList::HEcreate(Edge *e, int pm)
00438 {
00439     Halfedge *answer = new Halfedge;
00440     answer->ELedge = e;
00441     answer->ELpm = pm;
00442 
00443     HEmemmap[HEcount++] = answer;
00444     if(HEcount%ELhashsize == 0) {
00445         Halfedge **Temp = new Halfedge*[HEcount + ELhashsize];
00446         for(int i=0; i<HEcount; i++) Temp[i] = HEmemmap[i];
00447         delete[] HEmemmap;
00448         HEmemmap = Temp;
00449     }
00450     return answer;
00451 }
00452 
00453 void EdgeList::ELinsert(Halfedge *lb, Halfedge *new_he)
00454 {
00455     new_he->ELleft = lb;
00456     new_he->ELright = lb->ELright;
00457     (lb->ELright)->ELleft = new_he;
00458     lb->ELright = new_he;
00459 }
00460 
00461 /* Get entry from hash table, pruning any deleted nodes */
00462 Halfedge* EdgeList::ELgethash(int b)
00463 {
00464     Halfedge *he;
00465 
00466     if(b < 0 || b >= ELhashsize) return NULL;
00467     he = ELhash[b];
00468     if(he == NULL || he->ELedge != (Edge*)DELETED) return he;
00469 
00470     /* Hash table points to deleted half edge. */
00471     ELhash[b] = NULL;
00472     return NULL;
00473 }
00474 
00475 /* returns 1 if p is to right of halfedge e */
00476 int EdgeList::right_of(Halfedge *el, Vector2D *p)
00477 {
00478     Edge *e;
00479     Site *topsite;
00480     int right_of_site, above, fast;
00481     double dxp, dyp, dxs, t1, t2, t3, yl;
00482 
00483     e = el->ELedge;
00484     topsite = e->reg[1];
00485     right_of_site = p->x > topsite->coord.x;
00486     if(right_of_site && el->ELpm == le) return 1;
00487     if(!right_of_site && el->ELpm == re) return 0;
00488 
00489     if(e->a == 1.0) {
00490         dyp = p->y - topsite->coord.y;
00491         dxp = p->x - topsite->coord.x;
00492         fast = 0;
00493         if((!right_of_site && (e->b < 0.0)) || (right_of_site && (e->b >= 0.0))) {
00494             above = dyp >= e->b * dxp;
00495             fast = above;
00496         }
00497         else {
00498             above = p->x + p->y * e->b > e->c;
00499             if(e->b < 0.0) above = !above;
00500             if(!above) fast = 1;
00501         }
00502         if(!fast) {
00503             dxs = topsite->coord.x - (e->reg[0])->coord.x;
00504             above = e->b * (dxp*dxp - dyp*dyp) < dxs * dyp * (1.0 + 2.0*dxp/dxs + e->b*e->b);
00505             if(e->b < 0.0) above = !above;
00506         }
00507     }
00508     else {
00509         yl = e->c - e->a * p->x;
00510         t1 = p->y - yl;
00511         t2 = p->x - topsite->coord.x;
00512         t3 = yl - topsite->coord.y;
00513         above = t1*t1 > t2*t2 + t3*t3;
00514     }
00515     return el->ELpm == le ? above : !above;
00516 }
00517 
00518 Halfedge* EdgeList::ELleftbnd(Vector2D *p)
00519 {
00520     int i, bucket;
00521     Halfedge *he;
00522 
00523     /* Use hash table to get close to desired halfedge */
00524     bucket = (int)((p->x - xmin) / deltax) * ELhashsize;
00525     if(bucket < 0) bucket =0;
00526     if(bucket >= ELhashsize) bucket = ELhashsize - 1;
00527     he = ELgethash(bucket);
00528     if(he == NULL) {
00529         for(i=1; 1; i++) {
00530             if((he = ELgethash(bucket-i)) != NULL) break;
00531             if((he = ELgethash(bucket+i)) != NULL) break;
00532         }
00533     totalsearch++;
00534     }
00535     ntry++;
00536     /* Now search linear list of halfedges for the corect one */
00537     if(he == ELleftend  || (he != ELrightend && right_of(he, p))) {
00538         do {he = he->ELright;} while(he != ELrightend && right_of(he, p));
00539         he = he->ELleft;
00540     }
00541     else do {he = he->ELleft;} while(he != ELleftend && !right_of(he, p));
00542 
00543     /* Update hash table and reference counts */
00544     if(bucket > 0 && bucket < ELhashsize-1) {
00545         ELhash[bucket] = he;
00546     }
00547     return he;
00548 }
00549 
00550 void EdgeList::ELdelete(Halfedge *he)
00551 {
00552     (he->ELleft)->ELright = he->ELright;
00553     (he->ELright)->ELleft = he->ELleft;
00554     he->ELedge = (Edge*)DELETED;
00555 }
00556 
00557 Halfedge* EdgeList::ELright(Halfedge *he)
00558 {
00559     return he->ELright;
00560 }
00561 
00562 Halfedge* EdgeList::ELleft(Halfedge *he)
00563 {
00564     return he->ELleft;
00565 }
00566 
00567 
00568 Site* EdgeList::leftreg(Halfedge *he)
00569 {
00570     if(he->ELedge == NULL) return(bottomsite);
00571     return he->ELpm == le ? he->ELedge->reg[le] : he->ELedge->reg[re];
00572 }
00573 
00574 Site* EdgeList::rightreg(Halfedge *he)
00575 {
00576     if(he->ELedge == NULL) return(bottomsite);
00577     return he->ELpm == le ? he->ELedge->reg[re] : he->ELedge->reg[le];
00578 }
Generated on Wed May 26 16:21:15 2010 for OverSim by  doxygen 1.6.3