File indexing completed on 2024-03-24 03:46:45
0001 //# Filename: RangeConvex.cpp # # The RangeConvex # classes are 0002 //defined here. # # Author: Peter Z. Kunszt based on A. Szalay's code 0003 //# # Date: October 23, 1998 # # SPDX-FileCopyrightText: 2000 Peter Z. Kunszt 0004 //Alex S. Szalay, Aniruddha R. Thakar # The Johns Hopkins University # 0005 //# Modification History: # # Oct 18, 2001 : Dennis C. Dinge -- 0006 //Replaced ValVec with std::vector 0007 0008 #include "RangeConvex.h" 0009 0010 #include "SpatialGeneral.h" 0011 0012 #define N(n) index_->nodes_[(n)] 0013 #define NC(n, m) index_->nodes_[(n)].childID_[(m)] // the children n->m 0014 #define NV(m) index_->nodes_[id].v_[(m)] // the vertices of n 0015 #define V(m) index_->vertices_[(m)] // the vertex vector m 0016 0017 #define SGN(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0)) // signum 0018 0019 // =========================================================================== 0020 // 0021 // Member functions for class RangeConvex 0022 // 0023 // =========================================================================== 0024 0025 RangeConvex::RangeConvex() 0026 { 0027 } 0028 0029 /////////////CONSTRUCTOR FROM A TRIANGLE////////////////// 0030 // 0031 // Initialize convex from a triangle. The corners of these vectors 0032 // form a triangle, so we just add three ZERO constraints to the convex 0033 // where the direction is given by the cross product of the corners. 0034 // Of course, the sign has to be determined (on which side of the triangle 0035 // are we?) If the three points lie on one line, no constraints are added. 0036 // 0037 RangeConvex::RangeConvex(const SpatialVector *v1, const SpatialVector *v2, const SpatialVector *v3) 0038 { 0039 SpatialVector a1 = (*v2) ^ (*v3); // set directions of half-spheres 0040 SpatialVector a2 = (*v3) ^ (*v1); 0041 SpatialVector a3 = (*v1) ^ (*v2); 0042 float64 s1 = a1 * (*v1); // we really need only the signs of these 0043 float64 s2 = a2 * (*v2); 0044 float64 s3 = a3 * (*v3); 0045 0046 if (s1 * s2 * s3 != 0) // this is nonzero if not on one line 0047 { 0048 if (s1 < 0.0L) 0049 a1 = (-1) * a1; // change sign if necessary 0050 if (s2 < 0.0L) 0051 a2 = (-1) * a2; 0052 if (s3 < 0.0L) 0053 a3 = (-1) * a3; 0054 constraints_.push_back(SpatialConstraint(a1, 0.0)); // we don't care about the 0055 constraints_.push_back(SpatialConstraint(a2, 0.0)); // order since all angles are 0056 constraints_.push_back(SpatialConstraint(a3, 0.0)); // 90 degrees. 0057 } 0058 sign_ = zERO; 0059 } 0060 0061 /////////////CONSTRUCTOR FROM A RECTANGLE///////////////// 0062 // 0063 // Initialize convex from a rectangle. The vectors that form a rectangle 0064 // may be in any order, the code finds the edges by itself. 0065 // If one of the vectors lies within the triangle formed by the 0066 // other three vectors, the previous constructor is used. 0067 // 0068 RangeConvex::RangeConvex(const SpatialVector *v1, const SpatialVector *v2, const SpatialVector *v3, 0069 const SpatialVector *v4) 0070 { 0071 int i, j, k, l, m; // indices 0072 // to simplify things, copy input into a 4-array 0073 const SpatialVector *v[4] = { v1, v2, v3, v4 }; 0074 SpatialVector d[6]; 0075 float64 s[6][2]; 0076 for (i = 0, k = 0; i < 4; i++) 0077 for (j = i + 1; j < 4; j++, k++) // set directions of half-spheres 0078 { 0079 d[k] = (*v[i]) ^ (*v[j]); // two of these are diagonals. 0080 d[k].normalize(); 0081 for (l = 0, m = 0; l < 4; l++) 0082 if (l != i && l != j) 0083 s[k][m++] = d[k] * (*v[l]); // set the 'sign' 0084 } 0085 0086 // the sides are characterized by having both other corners 0087 // to the same (inner) side. so it is easy to find the edges. 0088 // again, the sign has to be taken care of -> direction of d 0089 // the nice thing here is that if one of the corners is inside 0090 // a triangles formed by the other three, only 3 constraints get 0091 // added. 0092 for (i = 0; i < 6; i++) 0093 if (s[i][0] * s[i][1] > 0.0) // not >= because we don't want aligned corners 0094 constraints_.push_back(SpatialConstraint((s[i][0] > 0.0 ? d[i] : (-1 * d[i])), 0.0)); 0095 0096 // Special cases: 1 0097 // if three of the corners are aligned, we end up with 0098 // only two constraints. Find the third and append it. 0099 // Indeed, there are 3 identical constraints among the d[], 0100 // so the first that qualifies gets appended. 0101 if (constraints_.size() == 2) 0102 { 0103 for (i = 0; i < 6; i++) 0104 if (s[i][0] == 0.0 || s[i][1] == 0.0) 0105 { 0106 constraints_.push_back(SpatialConstraint(((s[i][0] + s[i][1]) > 0.0 ? d[i] : (-1 * d[i])), 0.0)); 0107 break; 0108 } 0109 } 0110 // Special cases: 2 0111 // if all four corners are aligned, no constraints have been appended. 0112 sign_ = zERO; 0113 } 0114 0115 /////////////ADD////////////////////////////////////////// 0116 // 0117 void RangeConvex::add(SpatialConstraint &c) 0118 { 0119 constraints_.push_back(c); 0120 // order constraints: by ascending opening angle. Since we append 0121 // always at the end, we only need one ordering sweep starting at 0122 // the end 0123 for (size_t i = constraints_.size() - 1; i > 0; i--) 0124 { 0125 if (constraints_[i].s_ < constraints_[i - 1].s_) 0126 { 0127 SpatialConstraint tmp(constraints_[i]); 0128 constraints_[i] = constraints_[i - 1]; 0129 constraints_[i - 1] = tmp; 0130 } 0131 } 0132 0133 if (constraints_.size() == 1) // first constraint 0134 { 0135 sign_ = c.sign_; 0136 return; 0137 } 0138 0139 switch (sign_) 0140 { 0141 case nEG: 0142 if (c.sign_ == pOS) 0143 sign_ = mIXED; 0144 break; 0145 case pOS: 0146 if (c.sign_ == nEG) 0147 sign_ = mIXED; 0148 break; 0149 case zERO: 0150 sign_ = c.sign_; 0151 break; 0152 case mIXED: 0153 break; 0154 } 0155 } 0156 0157 /////////////SIMPLIFY0//////////////////////////////////// 0158 // simplify0: simplify zERO convexes. calculate corners of convex 0159 // and the bounding circle. 0160 // 0161 // zERO convexes are made up of constraints which are all great 0162 // circles. It can happen that some of the constraints are redundant. 0163 // For example, if 3 of the great circles define a triangle as the convex 0164 // which lies fully inside the half sphere of the fourth constraint, 0165 // that fourth constraint is redundant and will be removed. 0166 // 0167 // The algorithm is the following: 0168 // 0169 // zero-constraints are half-spheres, defined by a single normalized 0170 // vector v, pointing in the direction of that half-sphere. 0171 // 0172 // Two zero-constraints intersect at 0173 // 0174 // i = +- v x v 0175 // 1,2 1 2 0176 // 0177 // the vector cross product of their two defining vectors. 0178 // 0179 // The two vectors i1,2 are tested against every other constraint in 0180 // the convex if they lie within their half-spheres. Those 0181 // intersections i which lie within every other constraint, are stored 0182 // into corners_. 0183 // 0184 // Constraints that do not have a single corner on them, are dropped. 0185 // 0186 0187 void RangeConvex::simplify0() 0188 { 0189 size_t i, j, k; 0190 SpatialVector vi1, vi2; 0191 std::vector<size_t> cornerConstr1, cornerConstr2, removeConstr; 0192 std::vector<SpatialVector> corner; 0193 if (constraints_.size() == 1) // for one constraint, it is itself the BC 0194 { 0195 boundingCircle_ = constraints_[0]; 0196 return; 0197 // For 2 constraints, take the bounding circle a 0-constraint... 0198 // this is by no means optimal, but the code is optimized for at least 0199 // 3 zERO constraints... so this is acceptable. 0200 } 0201 else if (constraints_.size() == 2) 0202 { 0203 // test for constraints being identical - rule 1 out 0204 if (constraints_[0].a_ == constraints_[1].a_) 0205 { 0206 constraints_.erase(constraints_.end() - 1); 0207 boundingCircle_ = constraints_[0]; 0208 return; 0209 } 0210 // test for constraints being two disjoint half spheres - empty convex! 0211 if (constraints_[0].a_ == (-1.0) * constraints_[1].a_) 0212 { 0213 constraints_.clear(); 0214 return; 0215 } 0216 boundingCircle_ = SpatialConstraint(constraints_[0].v() + constraints_[1].v(), 0); 0217 return; 0218 } 0219 0220 // Go over all pairs of constraints 0221 for (i = 0; i < constraints_.size() - 1; i++) 0222 { 0223 bool ruledout = true; 0224 for (j = i + 1; j < constraints_.size(); j++) 0225 { 0226 // test for constraints being identical - rule i out 0227 if (constraints_[i].a_ == constraints_[j].a_) 0228 break; 0229 // test for constraints being two disjoint half spheres - empty convex! 0230 if (constraints_[i].a_ == (-1.0) * constraints_[j].a_) 0231 { 0232 constraints_.clear(); 0233 return; 0234 } 0235 // vi1 and vi2 are their intersection points 0236 vi1 = constraints_[i].a_ ^ constraints_[j].a_; 0237 vi1.normalize(); 0238 vi2 = (-1.0) * vi1; 0239 bool vi1ok = true, vi2ok = true; 0240 // now test whether vi1 or vi2 or both are inside every other constraint. 0241 // if yes, store them in the corner array. 0242 for (k = 0; k < constraints_.size(); k++) 0243 { 0244 if (k == i || k == j) 0245 continue; 0246 if (vi1ok && vi1 * constraints_[k].a_ <= 0.0) 0247 vi1ok = false; 0248 if (vi2ok && vi2 * constraints_[k].a_ <= 0.0) 0249 vi2ok = false; 0250 if (!vi1ok && !vi2ok) 0251 break; 0252 } 0253 if (vi1ok) 0254 { 0255 corner.push_back(vi1); 0256 cornerConstr1.push_back(i); 0257 cornerConstr2.push_back(j); 0258 ruledout = false; 0259 } 0260 if (vi2ok) 0261 { 0262 corner.push_back(vi2); 0263 cornerConstr1.push_back(i); 0264 cornerConstr2.push_back(j); 0265 ruledout = false; 0266 } 0267 } 0268 // is this constraint ruled out? i.e. none of its intersections 0269 // with other constraints are corners... remove it from constraints_ list. 0270 if (ruledout) 0271 removeConstr.push_back(i); 0272 } 0273 0274 // Now set the corners into their correct order, which is an 0275 // anti-clockwise walk around the polygon. 0276 // 0277 // start at any corner. so take the first. 0278 0279 corners_.clear(); 0280 corners_.push_back(corner[0]); 0281 0282 // The trick is now to start off into the correct direction. 0283 // this corner has two edges it can walk. we have to take the 0284 // one where the convex lies on its left side. 0285 i = cornerConstr1[0]; // the i'th constraint and j'th constraint 0286 j = cornerConstr2[0]; // intersect at 0'th corner 0287 size_t c1(0), c2(0), k1(0), k2(0); 0288 // Now find the other corner where the i'th and j'th constraints intersect. 0289 // Store the corner in vi1 and vi2, and the other constraint indices 0290 // in c1,c2. 0291 for (k = 1; k < cornerConstr1.size(); k++) 0292 { 0293 if (cornerConstr1[k] == i) 0294 { 0295 vi1 = corner[k]; 0296 c1 = cornerConstr2[k]; 0297 k1 = k; 0298 } 0299 if (cornerConstr2[k] == i) 0300 { 0301 vi1 = corner[k]; 0302 c1 = cornerConstr1[k]; 0303 k1 = k; 0304 } 0305 if (cornerConstr1[k] == j) 0306 { 0307 vi2 = corner[k]; 0308 c2 = cornerConstr2[k]; 0309 k2 = k; 0310 } 0311 if (cornerConstr2[k] == j) 0312 { 0313 vi2 = corner[k]; 0314 c2 = cornerConstr1[k]; 0315 k2 = k; 0316 } 0317 } 0318 // Now test i'th constraint-edge ( corner 0 and corner k ) whether 0319 // it is on the correct side (left) 0320 // 0321 // ( (corner(k) - corner(0)) x constraint(i) ) * corner(0) 0322 // 0323 // is >0 if yes, <0 if no... 0324 // 0325 size_t c, currentCorner; 0326 if (((vi1 - corner[0]) ^ constraints_[i].a_) * corner[0] > 0) 0327 { 0328 corners_.push_back(vi1); 0329 c = c1; 0330 currentCorner = k1; 0331 } 0332 else 0333 { 0334 corners_.push_back(vi2); 0335 c = c2; 0336 currentCorner = k2; 0337 } 0338 // now append the corners that match the index c until we got corner 0 again 0339 // currentCorner holds the current corners index 0340 // c holds the index of the constraint that has just been intersected with 0341 // So: 0342 // x We are on a constraint now (i or j from before), the second corner 0343 // is the one intersecting with constraint c. 0344 // x Find other corner for constraint c. 0345 // x Save that corner, and set c to the constraint that intersects with c 0346 // at that corner. Set currentcorner to that corners index. 0347 // x Loop until 0th corner reached. 0348 while (currentCorner) 0349 { 0350 for (k = 0; k < cornerConstr1.size(); k++) 0351 { 0352 if (k == currentCorner) 0353 continue; 0354 if (cornerConstr1[k] == c) 0355 { 0356 if ((currentCorner = k) == 0) 0357 break; 0358 corners_.push_back(corner[k]); 0359 c = cornerConstr2[k]; 0360 break; 0361 } 0362 if (cornerConstr2[k] == c) 0363 { 0364 if ((currentCorner = k) == 0) 0365 break; 0366 corners_.push_back(corner[k]); 0367 c = cornerConstr1[k]; 0368 break; 0369 } 0370 } 0371 } 0372 // Remove all redundant constraints 0373 for (i = 0; i < removeConstr.size(); i++) 0374 constraints_.erase(constraints_.end() - removeConstr[i] - 1); 0375 0376 // Now calculate the bounding circle for the convex. 0377 // We take it as the bounding circle of the triangle with 0378 // the widest opening angle. All triangles made out of 3 corners 0379 // are considered. 0380 boundingCircle_.d_ = 1.0; 0381 if (constraints_.size() >= 3) 0382 { 0383 for (i = 0; i < corners_.size(); i++) 0384 for (j = i + 1; j < corners_.size(); j++) 0385 for (k = j + 1; k < corners_.size(); k++) 0386 { 0387 SpatialVector v = (corners_[j] - corners_[i]) ^ (corners_[k] - corners_[j]); 0388 v.normalize(); 0389 // Set the correct opening angle: Since the plane cutting 0390 // out the triangle also correctly cuts out the bounding cap 0391 // of the triangle on the sphere, we can take any corner to 0392 // calculate the opening angle 0393 float64 d = v * corners_[i]; 0394 if (boundingCircle_.d_ > d) 0395 boundingCircle_ = SpatialConstraint(v, d); 0396 } 0397 } 0398 } 0399 0400 /////////////SIMPLIFY///////////////////////////////////// 0401 // simplify: We have the following decision tree for the 0402 // simplification of convexes: 0403 // 0404 // Always test two constraints against each other. We have 0405 // 0406 // * If both constraints are pOS 0407 // # If they intersect: keep both 0408 // # If one lies in the other: drop the larger one 0409 // # Else: disjunct. Empty convex, stop. 0410 // 0411 // * If both constraints are nEG 0412 // # If they intersect or are disjunct: ok 0413 // # Else: one lies in the other, drop smaller 'hole' 0414 // 0415 // * Mixed: one pOS, one nEG 0416 // # No intersection, disjunct: pOS is redundant 0417 // # Intersection: keep both 0418 // # pOS within nEG: empty convex, stop. 0419 // # nEG within pOS: keep both. 0420 // 0421 0422 void RangeConvex::simplify() 0423 { 0424 if (sign_ == zERO) 0425 { 0426 simplify0(); // treat zERO convexes separately 0427 return; 0428 } 0429 0430 size_t i, j; 0431 size_t clen; 0432 bool redundancy = true; 0433 0434 while (redundancy) 0435 { 0436 redundancy = false; 0437 clen = constraints_.size(); 0438 0439 for (i = 0; i < clen; i++) 0440 { 0441 for (j = 0; j < i; j++) 0442 { 0443 int test; 0444 0445 // don't bother with two zero constraints 0446 if (constraints_[i].sign_ == zERO && constraints_[j].sign_ == zERO) 0447 continue; 0448 0449 // both pos or zero 0450 if ((constraints_[i].sign_ == pOS || constraints_[i].sign_ == zERO) && 0451 (constraints_[j].sign_ == pOS || constraints_[j].sign_ == zERO)) 0452 { 0453 if ((test = testConstraints(i, j)) == 0) 0454 continue; // intersection 0455 if (test < 0) // disjoint ! convex is empty 0456 { 0457 constraints_.clear(); 0458 return; 0459 } 0460 // one is redundant 0461 if (test == 1) 0462 constraints_.erase(constraints_.end() - i - 1); 0463 else if (test == 2) 0464 constraints_.erase(constraints_.end() - j - 1); 0465 else 0466 continue; // intersection 0467 redundancy = true; // we did cut out a constraint -> do the loop again 0468 break; 0469 } 0470 0471 // both neg or zero 0472 if ((constraints_[i].sign_ == nEG) && (constraints_[j].sign_ == nEG)) 0473 { 0474 if ((test = testConstraints(i, j)) <= 0) 0475 continue; // ok 0476 // one is redundant 0477 if (test == 1) 0478 constraints_.erase(constraints_.end() - 1 - j); 0479 else if (test == 2) 0480 constraints_.erase(constraints_.end() - 1 - i); 0481 else 0482 continue; // intersection 0483 redundancy = true; // we did cut out a constraint -> do the loop again 0484 break; 0485 } 0486 0487 // one neg, one pos/zero 0488 if ((test = testConstraints(i, j)) == 0) 0489 continue; // ok: intersect 0490 if (test < 0) // neg is redundant 0491 { 0492 if (constraints_[i].sign_ == nEG) 0493 constraints_.erase(constraints_.end() - 1 - i); 0494 else 0495 constraints_.erase(constraints_.end() - 1 - j); 0496 redundancy = true; // we did cut out a constraint -> do the loop again 0497 break; 0498 } 0499 // if the negative constraint is inside the positive: continue 0500 if ((constraints_[i].sign_ == nEG && test == 2) || (constraints_[j].sign_ == nEG && test == 1)) 0501 continue; 0502 // positive constraint in negative: convex is empty! 0503 constraints_.clear(); 0504 return; 0505 } 0506 if (redundancy) 0507 break; 0508 } 0509 } 0510 0511 // reset the sign of the convex 0512 sign_ = constraints_[0].sign_; 0513 for (i = 1; i < constraints_.size(); i++) 0514 { 0515 switch (sign_) 0516 { 0517 case nEG: 0518 if (constraints_[i].sign_ == pOS) 0519 sign_ = mIXED; 0520 break; 0521 case pOS: 0522 if (constraints_[i].sign_ == nEG) 0523 sign_ = mIXED; 0524 break; 0525 case zERO: 0526 sign_ = constraints_[i].sign_; 0527 break; 0528 case mIXED: 0529 break; 0530 } 0531 } 0532 0533 if (constraints_.size() == 1) // for one constraint, it is itself the BC 0534 boundingCircle_ = constraints_[0]; 0535 else if (sign_ == pOS) 0536 boundingCircle_ = constraints_[0]; 0537 } 0538 0539 /////////////TESTCONSTRAINTS////////////////////////////// 0540 // testConstraints: Test for the relative position of two constraints. 0541 // Returns 0 if they intersect 0542 // Returns -1 if they are disjoint 0543 // Returns 1 if j is in i 0544 // Returns 2 if i is in j 0545 // 0546 int RangeConvex::testConstraints(size_t i, size_t j) 0547 { 0548 float64 phi = ((constraints_[i].sign_ == nEG ? (-1 * constraints_[i].a_) : constraints_[i].a_) * 0549 (constraints_[j].sign_ == nEG ? (-1 * constraints_[j].a_) : constraints_[j].a_)); 0550 phi = (phi <= -1.0L + gEpsilon ? gPi : acos(phi)); // correct for math lib -1.0 0551 float64 a1 = (constraints_[i].sign_ == pOS ? constraints_[i].s_ : gPi - constraints_[i].s_); 0552 float64 a2 = (constraints_[j].sign_ == pOS ? constraints_[j].s_ : gPi - constraints_[j].s_); 0553 0554 if (phi > a1 + a2) 0555 return -1; 0556 if (a1 > phi + a2) 0557 return 1; 0558 if (a2 > phi + a1) 0559 return 2; 0560 return 0; 0561 } 0562 0563 /////////////INTERSECT//////////////////////////////////// 0564 // used by intersect.cpp application 0565 // 0566 void RangeConvex::intersect(const SpatialIndex *idx, HtmRange *htmrange) 0567 { 0568 hr = htmrange; 0569 index_ = idx; 0570 addlevel_ = idx->maxlevel_ - idx->buildlevel_; 0571 0572 simplify(); // don't work too hard... 0573 0574 if (constraints_.empty()) 0575 return; // nothing to intersect!! 0576 0577 // Start with root nodes (index = 1-8) and intersect triangles 0578 for (uint32 i = 1; i <= 8; i++) 0579 { 0580 testTrixel(i); 0581 } 0582 } 0583 0584 ////////////SAVETRIXEL 0585 // 0586 inline void RangeConvex::saveTrixel(uint64 htmid) 0587 { 0588 // Some application want a complete htmid20 range 0589 // 0590 int level, i, shifts; 0591 uint64 lo, hi; 0592 #ifdef _WIN32 0593 uint64 IDHIGHBIT = 1; 0594 uint64 IDHIGHBIT2 = 1; 0595 IDHIGHBIT = IDHIGHBIT << 63; 0596 IDHIGHBIT2 = IDHIGHBIT2 << 62; 0597 #endif 0598 0599 for (i = 0; i < IDSIZE; i += 2) 0600 { 0601 if ((htmid << i) & IDHIGHBIT) 0602 break; 0603 } 0604 0605 level = (IDSIZE - i) >> 1; 0606 level -= 2; 0607 if (level < olevel) 0608 { 0609 /* Size is the length of the string representing the name of the 0610 trixel, the level is size - 2 0611 */ 0612 shifts = (olevel - level) << 1; 0613 lo = htmid << shifts; 0614 hi = lo + ((uint64)1 << shifts) - 1; 0615 } 0616 else 0617 { 0618 lo = hi = htmid; 0619 } 0620 hr->mergeRange(lo, hi); 0621 0622 return; 0623 } 0624 0625 /////////////TRIANGLETEST///////////////////////////////// 0626 // testTrixel: this is the main test of a triangle vs a Convex. It 0627 // will properly mark up the flags for the triangular node[index], and 0628 // all its children 0629 SpatialMarkup RangeConvex::testTrixel(uint64 id) 0630 { 0631 SpatialMarkup mark; 0632 uint64 childID; 0633 uint64 tid; 0634 0635 // const struct SpatialIndex::QuadNode &indexNode = index_->nodes_[id]; 0636 const struct SpatialIndex::QuadNode *indexNode = &index_->nodes_[id]; 0637 0638 // 0639 // do the face test on the triangle 0640 0641 // was: mark = testNode(V(NV(0)),V(NV(1)),V(NV(2))); 0642 // changed to by Gyorgy Fekete. Overall Speedup approx. 2% 0643 0644 mark = testNode(id); // was:(indexNode or id); 0645 0646 switch (mark) 0647 { 0648 case fULL: 0649 tid = N(id).id_; 0650 0651 saveTrixel(tid); //was: plist_->push_back(tid); 0652 0653 return mark; 0654 case rEJECT: 0655 tid = N(id).id_; 0656 return mark; 0657 default: 0658 // if pARTIAL or dONTKNOW, then continue, test children, 0659 // but do not reach beyond the leaf nodes. 0660 // If Convex is fully contained within one (sWALLOWED), 0661 // we can stop looking further in another child 0662 0663 // #define NC(n,m) index_->nodes_[(n)].childID_[(m)] // the children n->m 0664 // childID = index_->nodes_[id].childID_[0]; 0665 0666 // Test how many of the four children are rejected? 0667 // 0668 // [ed:algo] 0669 // 0670 break; 0671 } 0672 0673 // NEW NEW algorithm Disabled when enablenew is 0 0674 // 0675 { 0676 childID = indexNode->childID_[0]; 0677 if (childID != 0) 0678 { 0679 ////////////// [ed:split] 0680 tid = N(id).id_; 0681 childID = indexNode->childID_[0]; 0682 testTrixel(childID); 0683 childID = indexNode->childID_[1]; 0684 testTrixel(childID); 0685 childID = indexNode->childID_[2]; 0686 testTrixel(childID); 0687 childID = indexNode->childID_[3]; 0688 testTrixel(childID); 0689 } 0690 else /// No children... 0691 { 0692 if (addlevel_) 0693 { 0694 testPartial(addlevel_, N(id).id_, V(NV(0)), V(NV(1)), V(NV(2)), 0); 0695 } 0696 else 0697 { 0698 saveTrixel(N(id).id_); // was: plist_->push_back(N(id).id_); 0699 } 0700 } 0701 } ///////////////////////////// END OF NEW ALGORITHM returns mark below; 0702 0703 /* NEW NEW NEW 0704 If rejected, then we return [done] 0705 If full, then we list the id (propagate to children) [done] 0706 0707 If partial, then we look ahead to see how many children are rejected. 0708 But ah, next iteration could benefit from having computed this already. 0709 0710 If two children are rejected, then we stop 0711 If one or 0 nodes are rejected, then we 0712 */ 0713 return mark; 0714 } 0715 0716 /////////////TESTPARTIAL////////////////////////////////// 0717 // testPartial: test a triangle's subtriangle whether they are partial. 0718 // if level is nonzero, recurse. 0719 // 0720 void RangeConvex::testPartial(size_t level, uint64 id, const SpatialVector &v0, const SpatialVector &v1, 0721 const SpatialVector &v2, int PPrev) 0722 { 0723 uint64 ids[4], id0; 0724 SpatialMarkup m[4]; 0725 int P, F; // count number of partials and fulls 0726 0727 SpatialVector w0 = v1 + v2; 0728 w0.normalize(); 0729 SpatialVector w1 = v0 + v2; 0730 w1.normalize(); 0731 SpatialVector w2 = v1 + v0; 0732 w2.normalize(); 0733 0734 ids[0] = id0 = id << 2; 0735 ids[1] = id0 + 1; 0736 ids[2] = id0 + 2; 0737 ids[3] = id0 + 3; 0738 0739 m[0] = testNode(v0, w2, w1); 0740 m[1] = testNode(v1, w0, w2); 0741 m[2] = testNode(v2, w1, w0); 0742 m[3] = testNode(w0, w1, w2); 0743 0744 F = (m[0] == fULL) + (m[1] == fULL) + (m[2] == fULL) + (m[3] == fULL); 0745 P = (m[0] == pARTIAL) + (m[1] == pARTIAL) + (m[2] == pARTIAL) + (m[3] == pARTIAL); 0746 0747 // 0748 // Several interesting cases for saving this (the parent) trixel. 0749 // Case P==4, all four children are partials, so pretend parent is full, we save and return 0750 // Case P==3, and F==1, most of the parent is in, so pretend that parent is full again 0751 // Case P==2 or 3, but the previous testPartial had three partials, so parent was in an arc 0752 // as opposed to previous partials being fewer, so parent was in a tiny corner... 0753 0754 if ((level-- <= 0) || ((P == 4) || (F >= 2) || (P == 3 && F == 1) || (P > 1 && PPrev == 3))) 0755 { 0756 saveTrixel(id); 0757 return; 0758 } 0759 else 0760 { 0761 // look at each child, see if some need saving; 0762 for (int i = 0; i < 4; i++) 0763 { 0764 if (m[i] == fULL) 0765 { 0766 saveTrixel(ids[i]); 0767 } 0768 } 0769 // look at the four kids again, for partials 0770 0771 if (m[0] == pARTIAL) 0772 testPartial(level, ids[0], v0, w2, w1, P); 0773 if (m[1] == pARTIAL) 0774 testPartial(level, ids[1], v1, w0, w2, P); 0775 if (m[2] == pARTIAL) 0776 testPartial(level, ids[2], v2, w1, w0, P); 0777 if (m[3] == pARTIAL) 0778 testPartial(level, ids[3], w0, w1, w2, P); 0779 } 0780 } 0781 0782 /////////////TESTNODE///////////////////////////////////// 0783 // testNode: tests the QuadNodes for intersections. 0784 // 0785 SpatialMarkup RangeConvex::testNode(uint64 id) 0786 //uint64 id) 0787 // const struct SpatialIndex::QuadNode *indexNode) 0788 { 0789 const SpatialVector *v0, *v1, *v2; 0790 // const struct SpatialIndex::QuadNode &indexNode = index_->nodes_[id]; 0791 const struct SpatialIndex::QuadNode *indexNode = &index_->nodes_[id]; 0792 int m; 0793 m = indexNode->v_[0]; 0794 v0 = &index_->vertices_[m]; // the vertex vector m 0795 0796 m = indexNode->v_[1]; 0797 v1 = &index_->vertices_[m]; 0798 0799 m = indexNode->v_[2]; 0800 v2 = &index_->vertices_[m]; 0801 0802 // Start with testing the vertices for the QuadNode with this convex. 0803 int vsum = testVertex(v0) + testVertex(v1) + testVertex(v2); 0804 0805 SpatialMarkup mark = testTriangle(*v0, *v1, *v2, vsum); 0806 0807 // since we cannot play games using the on-the-fly triangles, 0808 // substitute dontknow with partial. 0809 if (mark == dONTKNOW) 0810 mark = pARTIAL; 0811 0812 return mark; 0813 } 0814 SpatialMarkup RangeConvex::testNode(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 0815 { 0816 // Start with testing the vertices for the QuadNode with this convex. 0817 0818 int vsum = testVertex(v0) + testVertex(v1) + testVertex(v2); 0819 0820 SpatialMarkup mark = testTriangle(v0, v1, v2, vsum); 0821 0822 // since we cannot play games using the on-the-fly triangles, 0823 // substitute dontknow with partial. 0824 if (mark == dONTKNOW) 0825 mark = pARTIAL; 0826 0827 return mark; 0828 } 0829 0830 /////////////TESTTRIANGLE////////////////////////////////// 0831 // testTriangle: tests a triangle given by 3 vertices if 0832 // it intersects the convex. 0833 // 0834 SpatialMarkup RangeConvex::testTriangle(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2, 0835 int vsum) 0836 { 0837 if (vsum == 1 || vsum == 2) 0838 return pARTIAL; 0839 0840 // If vsum = 3 then we have all vertices inside the convex. 0841 // Now use the following decision tree: 0842 // 0843 // * If the sign of the convex is pOS or zERO : mark as fULL intersection. 0844 // 0845 // * Else, test for holes inside the triangle. A 'hole' is a nEG constraint 0846 // that has its center inside the triangle. If there is such a hole, 0847 // return pARTIAL intersection. 0848 // 0849 // * Else (no holes, sign nEG or mIXED) test for intersection of nEG 0850 // constraints with the edges of the triangle. If there are such, 0851 // return pARTIAL intersection. 0852 // 0853 // * Else return fULL intersection. 0854 0855 if (vsum == 3) 0856 { 0857 if (sign_ == pOS || sign_ == zERO) 0858 return fULL; 0859 if (testHole(v0, v1, v2)) 0860 return pARTIAL; 0861 if (testEdge(v0, v1, v2)) 0862 return pARTIAL; 0863 return fULL; 0864 } 0865 0866 // If we have reached that far, we have vsum=0. There is no definite 0867 // decision making possible here with our methods, the markup may result 0868 // in dONTKNOW. The decision tree is the following: 0869 // 0870 // * Test with bounding circle of the triangle. 0871 // 0872 // # If the sign of the convex zERO test with the precalculated 0873 // bounding circle of the convex. If it does not intersect with the 0874 // triangle's bounding circle, rEJECT. 0875 // 0876 // # If the sign of the convex is nonZERO: if the bounding circle 0877 // lies outside of one of the constraints, rEJECT. 0878 // 0879 // * Else: there was an intersection with the bounding circle. 0880 // 0881 // # For zERO convexes, test whether the convex intersects the edges. 0882 // If none of the edges of the convex intersects with the edges of 0883 // the triangle, we have a rEJECT. Else, pARTIAL. 0884 // 0885 // # If sign of convex is pOS, or miXED and the smallest constraint does 0886 // not intersect the edges and has its center inside the triangle, 0887 // return sWALLOW. If no intersection of edges and center outside 0888 // triangle, return rEJECT. 0889 // 0890 // # So the smallest constraint DOES intersect with the edges. If 0891 // there is another pOS constraint which does not intersect with 0892 // the edges, and has its center outside the triangle, return 0893 // rEJECT. If its center is inside the triangle return sWALLOW. 0894 // Else, return pARTIAL for pOS and dONTKNOW for mIXED signs. 0895 // 0896 // * If we are here, return dONTKNOW. There is an intersection with 0897 // the bounding circle, none of the vertices is inside the convex and 0898 // we have very strange possibilities left for pOS and mIXED signs. For 0899 // nEG, i.e. all constraints negative, we also have some complicated 0900 // things left for which we cannot test further. 0901 0902 if (!testBoundingCircle(v0, v1, v2)) 0903 return rEJECT; 0904 0905 if (sign_ == pOS || sign_ == mIXED || (sign_ == zERO && constraints_.size() <= 2)) 0906 { 0907 // Does the smallest constraint intersect with the edges? 0908 if (testEdgeConstraint(v0, v1, v2, 0)) 0909 { 0910 // Is there another positive constraint that does NOT intersect with 0911 // the edges? 0912 size_t cIndex; 0913 if ((cIndex = testOtherPosNone(v0, v1, v2))) 0914 { 0915 // Does that constraint lie inside or outside of the triangle? 0916 if (testConstraintInside(v0, v1, v2, cIndex)) 0917 return pARTIAL; 0918 // Does the triangle lie completely within that constr? 0919 else if (constraints_[cIndex].contains(v0)) 0920 return pARTIAL; 0921 else 0922 return rEJECT; 0923 } 0924 else 0925 { 0926 if (sign_ == pOS || sign_ == zERO) 0927 return pARTIAL; 0928 else 0929 return dONTKNOW; 0930 } 0931 } 0932 else 0933 { 0934 if (sign_ == pOS || sign_ == zERO) 0935 { 0936 // Does the smallest lie inside or outside the triangle? 0937 if (testConstraintInside(v0, v1, v2, 0)) 0938 return pARTIAL; 0939 else 0940 return rEJECT; 0941 } 0942 else 0943 return dONTKNOW; 0944 } 0945 } 0946 else if (sign_ == zERO) 0947 { 0948 if (corners_.size() > 0 && testEdge0(v0, v1, v2)) 0949 return pARTIAL; 0950 else 0951 return rEJECT; 0952 } 0953 return pARTIAL; 0954 } 0955 0956 /////////////TESTVERTEX///////////////////////////////////// 0957 // testVertex: same as above, but for any spatialvector, no markup speedup 0958 // 0959 int RangeConvex::testVertex(const SpatialVector &v) 0960 { 0961 for (size_t i = 0; i < constraints_.size(); i++) 0962 if ((constraints_[i].a_ * v) < constraints_[i].d_) 0963 return 0; 0964 0965 return 1; 0966 } 0967 int RangeConvex::testVertex(const SpatialVector *v) 0968 { 0969 for (size_t i = 0; i < constraints_.size(); i++) 0970 if ((constraints_[i].a_ * *v) < constraints_[i].d_) 0971 return 0; 0972 0973 return 1; 0974 } 0975 0976 /////////////TESTHOLE///////////////////////////////////// 0977 // testHole: test for holes. If there is a negative constraint whose center 0978 // is inside the triangle, we speak of a hole. Returns true if 0979 // found one. 0980 // 0981 bool RangeConvex::testHole(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 0982 { 0983 bool test = false; 0984 0985 for (size_t i = 0; i < constraints_.size(); i++) 0986 { 0987 if (constraints_[i].sign_ == nEG) // test only 'holes' 0988 { 0989 // If (a ^ b * c) < 0, vectors abc point clockwise. 0990 // -> center c not inside triangle, since vertices a,b are ordered 0991 // counter-clockwise. The comparison here is the other way 0992 // round because c points into the opposite direction as the hole 0993 0994 if (((v0 ^ v1) * constraints_[i].a_) > 0.0L) 0995 continue; 0996 if (((v1 ^ v2) * constraints_[i].a_) > 0.0L) 0997 continue; 0998 if (((v2 ^ v0) * constraints_[i].a_) > 0.0L) 0999 continue; 1000 test = true; 1001 break; 1002 } 1003 } 1004 return test; 1005 } 1006 1007 /////////////TESTEDGE0//////////////////////////////////// 1008 // testEdge0: test if the edges intersect with the zERO convex. 1009 // The edges are given by the vertex vectors e[0-2] 1010 // All constraints are great circles, so test if their intersect 1011 // with the edges is inside or outside the convex. 1012 // If any edge intersection is inside the convex, return true. 1013 // If all edge intersections are outside, check whether one of 1014 // the corners is inside the triangle. If yes, all of them must be 1015 // inside -> return true. 1016 // 1017 bool RangeConvex::testEdge0(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 1018 { 1019 // We have constructed the corners_ array in a certain direction. 1020 // now we can run around the convex, check each side against the 3 1021 // triangle edges. If any of the sides has its intersection INSIDE 1022 // the side, return true. At the end test if a corner lies inside 1023 // (because if we get there, none of the edges intersect, but it 1024 // can be that the convex is fully inside the triangle. so to test 1025 // one single edge is enough) 1026 1027 struct edgeStruct 1028 { 1029 SpatialVector e; // The half-sphere this edge delimits 1030 float64 l; // length of edge 1031 const SpatialVector *e1; // first end 1032 const SpatialVector *e2; // second end 1033 } edge[3]; 1034 1035 // fill the edge structure for each side of this triangle 1036 edge[0].e = v0 ^ v1; 1037 edge[0].e1 = &v0; 1038 edge[0].e2 = &v1; 1039 edge[1].e = v1 ^ v2; 1040 edge[1].e1 = &v1; 1041 edge[1].e2 = &v2; 1042 edge[2].e = v2 ^ v0; 1043 edge[2].e1 = &v2; 1044 edge[2].e2 = &v0; 1045 edge[0].l = acos(v0 * v1); 1046 edge[1].l = acos(v1 * v2); 1047 edge[2].l = acos(v2 * v0); 1048 1049 for (size_t i = 0; i < corners_.size(); i++) 1050 { 1051 size_t j = 0; 1052 if (i < corners_.size() - 1) 1053 j = i + 1; 1054 SpatialVector a1; 1055 float64 l1, l2; // lengths of the arcs from intersection to edge corners 1056 float64 cedgelen = acos(corners_[i] * corners_[j]); // length of edge of convex 1057 1058 // calculate the intersection - all 3 edges 1059 for (size_t iedge = 0; iedge < 3; iedge++) 1060 { 1061 a1 = (edge[iedge].e) ^ (corners_[i] ^ corners_[j]); 1062 a1.normalize(); 1063 // if the intersection a1 is inside the edge of the convex, 1064 // its distance to the corners is smaller than the edgelength. 1065 // this test has to be done for both the edge of the convex and 1066 // the edge of the triangle. 1067 for (size_t k = 0; k < 2; k++) 1068 { 1069 l1 = acos(corners_[i] * a1); 1070 l2 = acos(corners_[j] * a1); 1071 if (l1 - cedgelen <= gEpsilon && l2 - cedgelen <= gEpsilon) 1072 { 1073 l1 = acos(*(edge[iedge].e1) * a1); 1074 l2 = acos(*(edge[iedge].e2) * a1); 1075 if (l1 - edge[iedge].l <= gEpsilon && l2 - edge[iedge].l <= gEpsilon) 1076 return true; 1077 } 1078 a1 *= -1.0; // do the same for the other intersection 1079 } 1080 } 1081 } 1082 return testVectorInside(v0, v1, v2, corners_[0]); 1083 } 1084 1085 /////////////TESTEDGE///////////////////////////////////// 1086 // testEdge: test if edges intersect with constraint. This problem 1087 // is solved by a quadratic equation. Return true if there is 1088 // an intersection. 1089 // 1090 bool RangeConvex::testEdge(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 1091 { 1092 for (size_t i = 0; i < constraints_.size(); i++) 1093 { 1094 if (constraints_[i].sign_ == nEG) // test only 'holes' 1095 { 1096 if (eSolve(v0, v1, i)) 1097 return true; 1098 if (eSolve(v1, v2, i)) 1099 return true; 1100 if (eSolve(v2, v0, i)) 1101 return true; 1102 } 1103 } 1104 return false; 1105 } 1106 1107 /////////////ESOLVE/////////////////////////////////////// 1108 // eSolve: solve the quadratic eq. for intersection of an edge with a circle 1109 // constraint. Edge given by grand circle running through v1, v2 1110 // Constraint given by cIndex. 1111 bool RangeConvex::eSolve(const SpatialVector &v1, const SpatialVector &v2, size_t cIndex) 1112 { 1113 float64 gamma1 = v1 * constraints_[cIndex].a_; 1114 float64 gamma2 = v2 * constraints_[cIndex].a_; 1115 float64 mu = v1 * v2; 1116 float64 u2 = (1 - mu) / (1 + mu); 1117 1118 float64 a = -u2 * (gamma1 + constraints_[cIndex].d_); 1119 float64 b = gamma1 * (u2 - 1) + gamma2 * (u2 + 1); 1120 float64 c = gamma1 - constraints_[cIndex].d_; 1121 1122 float64 D = b * b - 4 * a * c; 1123 1124 if (D < 0.0L) 1125 return false; // no intersection 1126 1127 // calculate roots a'la Numerical Recipes 1128 1129 float64 q = -0.5L * (b + (SGN(b) * sqrt(D))); 1130 1131 float64 root1(0), root2(0); 1132 int i = 0; 1133 1134 if (a > gEpsilon || a < -gEpsilon) 1135 { 1136 root1 = q / a; 1137 i++; 1138 } 1139 if (q > gEpsilon || q < -gEpsilon) 1140 { 1141 root2 = c / q; 1142 i++; 1143 } 1144 1145 // Check whether the roots lie within [0,1]. If not, the intersection 1146 // is outside the edge. 1147 1148 if (i == 0) 1149 return false; // no solution 1150 if (root1 >= 0.0L && root1 <= 1.0L) 1151 return true; 1152 if (i == 2 && ((root1 >= 0.0L && root1 <= 1.0L) || (root2 >= 0.0L && root2 <= 1.0L))) 1153 return true; 1154 1155 return false; 1156 } 1157 1158 /////////////TESTBOUNDINGCIRCLE/////////////////////////// 1159 // testBoundingCircle: test for boundingCircles intersecting with constraint 1160 // 1161 bool RangeConvex::testBoundingCircle(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 1162 { 1163 // Set the correct direction: The normal vector to the triangle plane 1164 SpatialVector c = (v1 - v0) ^ (v2 - v1); 1165 c.normalize(); 1166 1167 // Set the correct opening angle: Since the plane cutting out the triangle 1168 // also correctly cuts out the bounding cap of the triangle on the sphere, 1169 // we can take any corner to calculate the opening angle 1170 float64 d = acos(c * v0); 1171 1172 // for zero convexes, we have calculated a bounding circle for the convex. 1173 // only test with this single bounding circle. 1174 1175 if (sign_ == zERO) 1176 { 1177 float64 tst; 1178 if (((tst = c * boundingCircle_.a_) < -1.0L + gEpsilon ? gPi : acos(tst)) > (d + boundingCircle_.s_)) 1179 return false; 1180 return true; 1181 } 1182 1183 // for all other convexes, test every constraint. If the bounding 1184 // circle lies completely outside of one of the constraints, reject. 1185 // else, accept. 1186 1187 size_t i; 1188 for (i = 0; i < constraints_.size(); i++) 1189 { 1190 if (((c * constraints_[i].a_) < -1.0L + gEpsilon ? gPi : acos(c * constraints_[i].a_)) > 1191 (d + constraints_[i].s_)) 1192 return false; 1193 } 1194 return true; 1195 } 1196 1197 /////////////TESTEDGECONSTRAINT/////////////////////////// 1198 // testEdgeConstraint: test if edges intersect with a given constraint. 1199 // 1200 bool RangeConvex::testEdgeConstraint(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2, 1201 size_t cIndex) 1202 { 1203 if (eSolve(v0, v1, cIndex)) 1204 return true; 1205 if (eSolve(v1, v2, cIndex)) 1206 return true; 1207 if (eSolve(v2, v0, cIndex)) 1208 return true; 1209 return false; 1210 } 1211 1212 /////////////TESTOTHERPOSNONE///////////////////////////// 1213 // testOtherPosNone: test for other positive constraints that do 1214 // not intersect with an edge. Return its index 1215 // 1216 size_t RangeConvex::testOtherPosNone(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2) 1217 { 1218 size_t i = 1; 1219 while (i < constraints_.size() && constraints_[i].sign_ == pOS) 1220 { 1221 if (!testEdgeConstraint(v0, v1, v2, i)) 1222 return i; 1223 i++; 1224 } 1225 return 0; 1226 } 1227 1228 /////////////TESTCONSTRAINTINSIDE///////////////////////// 1229 // testConstraintInside: look if a constraint is inside the triangle 1230 // 1231 bool RangeConvex::testConstraintInside(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2, 1232 size_t i) 1233 { 1234 return testVectorInside(v0, v1, v2, constraints_[i].a_); 1235 } 1236 1237 /////////////TESTVECTORINSIDE//////////////////////////// 1238 // testVectorInside: look if a vector is inside the triangle 1239 // 1240 bool RangeConvex::testVectorInside(const SpatialVector &v0, const SpatialVector &v1, const SpatialVector &v2, 1241 SpatialVector &v) 1242 { 1243 // If (a ^ b * c) < 0, vectors abc point clockwise. 1244 // -> center c not inside triangle, since vertices are ordered 1245 // counter-clockwise. 1246 1247 if ((((v0 ^ v1) * v) < 0) || (((v1 ^ v2) * v) < 0) || (((v2 ^ v0) * v) < 0)) 1248 return false; 1249 return true; 1250 }