00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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
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
00293 e->reg[re]->enclosingSet.insert(e->reg[le]->addr);
00294 e->reg[le]->enclosingSet.insert(e->reg[re]->addr);
00295
00296
00297 if(e->reg[le]->type == THIS) {
00298 e->reg[re]->type |= ENCLOSING;
00299
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
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
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
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
00471 ELhash[b] = NULL;
00472 return NULL;
00473 }
00474
00475
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
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
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
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 }