//# Filename: SpatialConvex.cpp //# //# The SpatialConvex //# classes are defined here. //# //# Author: Peter Z. Kunszt based on A. Szalay's code //# //# Date: October 23, 1998 //# //# //# //# Copyright (C) 2000 Peter Z. Kunszt, Alex S. Szalay, Aniruddha R. Thakar //# The Johns Hopkins University //# //# This program is free software; you can redistribute it and/or //# modify it under the terms of the GNU General Public License //# as published by the Free Software Foundation; either version 2 //# of the License, or (at your option) any later version. //# //# This program is distributed in the hope that it will be useful, //# but WITHOUT ANY WARRANTY; without even the implied warranty of //# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //# GNU General Public License for more details. //# //# You should have received a copy of the GNU General Public License //# along with this program; if not, write to the Free Software //# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. //# //# //# //# Modification History: //# //#define DIAGNOSE #include "VarVecDef.h" #include "SpatialConvex.h" #define N(n) index_->nodes_.vector_[(n)] // the node[n] #define NC(n,m) index_->nodes_.vector_[(n)].childID_[(m)]// the children n->m #define NV(m) index_->nodes_.vector_[id].v_[(m)] // the vertices of n #define V(m) index_->vertices_.vector_[(m)] // the vertex vector m #define SGN(x) ( (x)<0? -1: ( (x)>0? 1:0 ) ) // signum #define IOFFSET 9 #define COMMENT '#' // =========================================================================== // // Member functions for class SpatialConvex // // =========================================================================== SpatialConvex::SpatialConvex() { } /////////////COPY CONSTRUCTOR///////////////////////////// // SpatialConvex::SpatialConvex(const SpatialConvex & c) : index_(c.index_), addlevel_(c.addlevel_), full_(c.full_), partial_(c.partial_), flist_(c.flist_), plist_(c.plist_), boundingCircle_(c.boundingCircle_) { constraints_ = c.constraints_; corners_ = c.corners_; bitresult_ = c.bitresult_; sign_ = c.sign_; } /////////////ASSIGNMENT/////////////////////////////////// // SpatialConvex& SpatialConvex::operator =(const SpatialConvex & c) { if(&c == this)return *this; index_ = c.index_; addlevel_ = c.addlevel_; full_ = c.full_; partial_ = c.partial_; flist_ = c.flist_; plist_ = c.plist_; boundingCircle_ = c.boundingCircle_; constraints_ = c.constraints_; corners_ = c.corners_; bitresult_ = c.bitresult_; sign_ = c.sign_; return *this; } /////////////CONSTRUCTOR FROM A TRIANGLE////////////////// // // Initialize domain from a triangle. The corners of these vectors // form a triangle, so we just add three ZERO convexes to the domain // where the direction is given by the cross product of the corners. // Of course, the sign has to be determined (on which side of the triangle // are we?) If the three points lie on one line, no convexes are added. // SpatialConvex::SpatialConvex(const SpatialVector * v1, const SpatialVector * v2, const SpatialVector * v3) { SpatialVector a1 = (*v2) ^ (*v3); // set directions of half-spheres SpatialVector a2 = (*v3) ^ (*v1); SpatialVector a3 = (*v1) ^ (*v2); float64 s1 = a1 * (*v1); // we really need only the signs of these float64 s2 = a2 * (*v2); float64 s3 = a3 * (*v3); if(s1 * s2 * s3) { // this is nonzero if not on one line if(s1 < 0.0L) a1 = (-1) * a1 ; // change sign if necessary if(s2 < 0.0L) a2 = (-1) * a2 ; if(s3 < 0.0L) a3 = (-1) * a3 ; constraints_.append(SpatialConstraint(a1,0.0)); // we don't care about the constraints_.append(SpatialConstraint(a2,0.0)); // order since all angles are constraints_.append(SpatialConstraint(a3,0.0)); // 90 degrees. } sign_ = zERO; } /////////////CONSTRUCTOR FROM A RECTANGLE///////////////// // // Initialize convex from a rectangle. The vectors that form a rectangle // may be in any order, the code finds the edges by itself. // If one of the vectors lies within the triangle formed by the // other three vectors, the previous constructor is used. // SpatialConvex::SpatialConvex(const SpatialVector * v1, const SpatialVector * v2, const SpatialVector * v3, const SpatialVector * v4) { int i,j,k,l,m; // indices // to simplify things, copy input into a 4-array const SpatialVector *v[4] = {v1,v2,v3,v4}; SpatialVector d[6]; float64 s[6][2]; for (i = 0, k = 0; i < 4 ; i++) for (j = i+1; j < 4; j++, k++) { // set directions of half-spheres d[k] = (*v[i]) ^ (*v[j]); // two of these are diagonals. d[k].normalize(); for (l = 0, m = 0; l < 4; l++) if(l != i && l != j)s[k][m++] = d[k] * (*v[l]); // set the 'sign' } // the sides are characterized by having both other corners // to the same (inner) side. so it is easy to find the edges. // again, the sign has to be taken care of -> direction of d // the nice thing here is that if one of the corners is inside // a triangles formed by the other three, only 3 constraints get // added. for(i = 0; i < 6; i++) if(s[i][0] * s[i][1] > 0.0) // not >= because we don't want aligned corners constraints_.append(SpatialConstraint((s[i][0] > 0.0 ? d[i] : (-1 * d[i])), 0.0)); // Special cases: 1 // if three of the corners are aligned, we end up with // only two constraints. Find the third and append it. // Indeed, there are 3 identical constraints among the d[], // so the first that qualifies gets appended. if(constraints_.length() == 2) { for(i = 0; i < 6; i++) if(s[i][0] == 0.0 || s[i][1] == 0.0) { constraints_.append(SpatialConstraint( ((s[i][0]+s[i][1]) > 0.0 ? d[i] : (-1 * d[i])), 0.0)); break; } } // Special cases: 2 // if all four corners are aligned, no constraints have been appended. sign_ = zERO; } /////////////ADD////////////////////////////////////////// // void SpatialConvex::add(SpatialConstraint & c) { constraints_.append(c); // order constraints: by ascending opening angle. Since we append // always at the end, we only need one ordering sweep starting at // the end for ( size_t i = constraints_.length() - 1; i > 0; i-- ) { if ( constraints_.vector_[i].s_ < constraints_.vector_[i-1].s_ ) { SpatialConstraint tmp( constraints_.vector_[i] ); constraints_.vector_[i] = constraints_.vector_[i-1]; constraints_.vector_[i-1] = tmp; } } if(constraints_.length() == 1) { // first constraint sign_ = c.sign_; return; } switch (sign_) { case nEG: if(c.sign_ == pOS) sign_ = mIXED; break; case pOS: if(c.sign_ == nEG) sign_ = mIXED; break; case zERO: sign_ = c.sign_; break; } } /////////////SIMPLIFY0//////////////////////////////////// // simplify0: simplify zERO convexes. calculate corners of convex // and the bounding circle. // // zERO convexes are made up of constraints which are all great // circles. It can happen that some of the constraints are redundant. // For example, if 3 of the great circles define a triangle as the convex // which lies fully inside the half sphere of the fourth constraint, // that fourth constraint is redundant and will be removed. // // The algorithm is the following: // // zero-constraints are half-spheres, defined by a single normalized // vector v, pointing in the direction of that half-sphere. // // Two zero-constraints intersect at // // i = +- v x v // 1,2 1 2 // // the vector cross product of their two defining vectors. // // The two vectors i1,2 are tested against every other constraint in // the convex if they lie within their half-spheres. Those // intersections i which lie within every other constraint, are stored // into corners_. // // Constraints that do not have a single corner on them, are dropped. // void SpatialConvex::simplify0() { size_t i,j,k; SpatialVector vi1, vi2; ValVec cornerConstr1, cornerConstr2, removeConstr; ValVec corner; if (constraints_.length() == 1) { // for one constraint, it is itself the BC boundingCircle_ = constraints_(0); return; // For 2 constraints, take the bounding circle a 0-constraint... // this is by no means optimal, but the code is optimized for at least // 3 zERO constraints... so this is acceptable. } else if(constraints_.length() == 2) { // test for constraints being identical - rule 1 out if(constraints_.vector_[0].a_ == constraints_.vector_[1].a_){ constraints_.cut(1); boundingCircle_ = constraints_(0); return; } // test for constraints being two disjoint half spheres - empty convex! if(constraints_.vector_[0].a_ == (-1.0)*constraints_.vector_[1].a_){ constraints_.cut(constraints_.length()); return; } boundingCircle_ = SpatialConstraint(constraints_(0).v() + constraints_(1).v(),0); return; } // Go over all pairs of constraints for(i = 0; i < constraints_.length() - 1; i++) { bool ruledout = true; for(j = i+1; j < constraints_.length(); j ++) { // test for constraints being identical - rule i out if(constraints_.vector_[i].a_ == constraints_.vector_[j].a_)break; // test for constraints being two disjoint half spheres - empty convex! if(constraints_.vector_[i].a_ == (-1.0)*constraints_.vector_[j].a_){ constraints_.cut(constraints_.length()); return; } // vi1 and vi2 are their intersection points vi1 = constraints_.vector_[i].a_ ^ constraints_.vector_[j].a_ ; vi1.normalize(); vi2 = (-1.0) * vi1; bool vi1ok = true, vi2ok = true; // now test whether vi1 or vi2 or both are inside every other constraint. // if yes, store them in the corner array. for(k = 0; k < constraints_.length(); k++) { if(k == i || k == j) continue; if(vi1ok && vi1 * constraints_.vector_[k].a_ <= 0.0) vi1ok = false; if(vi2ok && vi2 * constraints_.vector_[k].a_ <= 0.0) vi2ok = false; if(!vi1ok && !vi2ok)break; } if(vi1ok) { corner.append(vi1); cornerConstr1.append(i); cornerConstr2.append(j); ruledout = false; } if(vi2ok) { corner.append(vi2); cornerConstr1.append(i); cornerConstr2.append(j); ruledout = false; } } // is this constraint ruled out? i.e. none of its intersections // with other constraints are corners... remove it from constraints_ list. if(ruledout) removeConstr.append(i); } // Now set the corners into their correct order, which is an // anti-clockwise walk around the polygon. // // start at any corner. so take the first. corners_.cut(corners_.length()); corners_.append(corner(0)); // The trick is now to start off into the correct direction. // this corner has two edges it can walk. we have to take the // one where the convex lies on its left side. i = cornerConstr1(0); // the i'th constraint and j'th constraint j = cornerConstr2(0); // intersect at 0'th corner size_t c1,c2,k1,k2; // Now find the other corner where the i'th and j'th constraints intersect. // Store the corner in vi1 and vi2, and the other constraint indices // in c1,c2. for( k = 1; k < cornerConstr1.length(); k ++) { if(cornerConstr1(k) == i) { vi1 = corner(k); c1 = cornerConstr2(k); k1 = k; } if(cornerConstr2(k) == i) { vi1 = corner(k); c1 = cornerConstr1(k); k1 = k; } if(cornerConstr1(k) == j) { vi2 = corner(k); c2 = cornerConstr2(k); k2 = k; } if(cornerConstr2(k) == j) { vi2 = corner(k); c2 = cornerConstr1(k); k2 = k; } } // Now test i'th constraint-edge ( corner 0 and corner k ) whether // it is on the correct side (left) // // ( (corner(k) - corner(0)) x constraint(i) ) * corner(0) // // is >0 if yes, <0 if no... // size_t c,currentCorner; if( ((vi1 - corner(0)) ^ constraints_(i).a_) * corner(0) > 0 ) { corners_.append(vi1); c = c1; currentCorner = k1; } else { corners_.append(vi2); c = c2; currentCorner = k2; } // now append the corners that match the index c until we got corner 0 again // currentCorner holds the current corners index // c holds the index of the constraint that has just been intersected with // So: // x We are on a constraint now (i or j from before), the second corner // is the one intersecting with constraint c. // x Find other corner for constraint c. // x Save that corner, and set c to the constraint that intersects with c // at that corner. Set currentcorner to that corners index. // x Loop until 0th corner reached. while( currentCorner ) { for (k = 0; k < cornerConstr1.length(); k++) { if(k == currentCorner)continue; if(cornerConstr1(k) == c) { if( (currentCorner = k) == 0) break; corners_.append(corner(k)); c = cornerConstr2(k); break; } if(cornerConstr2(k) == c) { if( (currentCorner = k) == 0) break; corners_.append(corner(k)); c = cornerConstr1(k); break; } } } // Remove all redundant constraints for ( i = 0; i < removeConstr.length(); i++) constraints_.remove(removeConstr(i)); // Now calculate the bounding circle for the convex. // We take it as the bounding circle of the triangle with // the widest opening angle. All triangles made out of 3 corners // are considered. boundingCircle_.d_ = 1.0; if (constraints_.length() >=3 ) { for(i = 0; i < corners_.length(); i++) for(j = i+1; j < corners_.length(); j++) for(k = j+1; k < corners_.length(); k++) { SpatialVector v = ( corners_(j) - corners_(i) ) ^ ( corners_(k) - corners_(j) ); v.normalize(); // Set the correct opening angle: Since the plane cutting // out the triangle also correctly cuts out the bounding cap // of the triangle on the sphere, we can take any corner to // calculate the opening angle float64 d = v * corners_(i); if(boundingCircle_.d_ > d) boundingCircle_ = SpatialConstraint(v,d); } } #ifdef DIAGNOSE for(i = 0; i < corners_.length(); i++) { cout << corners_(i).ra() << "," << corners_(i).dec() << ":" << corners_(i) << endl; } #endif } /////////////SIMPLIFY///////////////////////////////////// // simplify: We have the following decision tree for the // simplification of convexes: // // Always test two constraints against each other. We have // // * If both constraints are pOS // // # If they intersect: keep both // // # If one lies in the other: drop the larger one // // # Else: disjunct. Empty convex, stop. // // * If both constraints are nEG // // # If they intersect or are disjunct: ok // // # Else: one lies in the other, drop smaller 'hole' // // * Mixed: one pOS, one nEG // // # No intersection, disjunct: pOS is redundant // // # Intersection: keep both // // # pOS within nEG: empty convex, stop. // // # nEG within pOS: keep both. // void SpatialConvex::simplify() { if(sign_ == zERO) { simplify0(); // treat zERO convexes separately return; } size_t i,j; size_t clen; bool redundancy = true; while(redundancy) { redundancy = false; clen = constraints_.length(); for(i = 0; i < clen; i++) { for(j = 0; j < i; j++) { int test; // don't bother with two zero constraints if( constraints_[i].sign_ == zERO && constraints_[j].sign_ == zERO) continue; // both pos or zero if( ( constraints_[i].sign_ == pOS || constraints_[i].sign_ == zERO ) && ( constraints_[j].sign_ == pOS || constraints_[j].sign_ == zERO ) ) { if ( (test = testConstraints(i,j)) == 0 ) continue; // intersection if ( test < 0 ) { // disjoint ! convex is empty constraints_.cut(constraints_.length()); return; } // one is redundant if(test == 1) constraints_.cut(1, clen - 1 - i); else if(test==2)constraints_.cut(1, clen - 1 - j); else continue; // intersection redundancy = true; // we did cut out a constraint -> do the loop again break; } // both neg or zero if( ( constraints_[i].sign_ == nEG ) && ( constraints_[j].sign_ == nEG ) ) { if ( (test = testConstraints(i,j)) <= 0 ) continue; // ok // one is redundant if(test == 1) constraints_.cut(1, clen - 1 - j); else if(test==2)constraints_.cut(1, clen - 1 - i); else continue; // intersection redundancy = true; // we did cut out a constraint -> do the loop again break; } // one neg, one pos/zero if( (test = testConstraints(i,j)) == 0) continue; // ok: intersect if( test < 0 ) { // neg is redundant if ( constraints_[i].sign_ == nEG ) constraints_.cut(1, clen - 1 - i); else constraints_.cut(1, clen - 1 - j); redundancy = true; // we did cut out a constraint -> do the loop again break; } // if the negative constraint is inside the positive: continue if ( (constraints_[i].sign_ == nEG && test == 2) || (constraints_[j].sign_ == nEG && test == 1) )continue; // positive constraint in negative: convex is empty! constraints_.cut(constraints_.length()); return; } if(redundancy)break; } } // reset the sign of the convex sign_ = constraints_[0].sign_; for(i = 1; i < constraints_.length(); i++) { switch (sign_) { case nEG: if(constraints_[i].sign_ == pOS) sign_ = mIXED; break; case pOS: if(constraints_[i].sign_ == nEG) sign_ = mIXED; break; case zERO: sign_ = constraints_[i].sign_; break; } } if (constraints_.length() == 1) // for one constraint, it is itself the BC boundingCircle_ = constraints_(0); else if (sign_ == pOS) boundingCircle_ = constraints_(0); } /////////////TESTCONSTRAINTS////////////////////////////// // testConstraints: Test for the relative position of two constraints. // Returns 0 if they intersect // Returns -1 if they are disjoint // Returns 1 if j is in i // Returns 2 if i is in j // int SpatialConvex::testConstraints(size_t i, size_t j) { float64 phi = ( (constraints_[i].sign_ == nEG ? (-1 * constraints_[i].a_): constraints_[i].a_ ) * (constraints_[j].sign_ == nEG ? (-1 * constraints_[j].a_): constraints_[j].a_ ) ); phi = (phi <= -1.0L + gEpsilon ? gPi : acos(phi)) ; // correct for math lib -1.0 float64 a1 = (constraints_[i].sign_ == pOS ? constraints_[i].s_ : gPi-constraints_[i].s_); float64 a2 = (constraints_[j].sign_ == pOS ? constraints_[j].s_ : gPi-constraints_[j].s_); if ( phi > a1 + a2 ) return -1; if ( a1 > phi + a2 ) return 1; if ( a2 > phi + a1 ) return 2; return 0; } /////////////INTERSECT//////////////////////////////////// // void SpatialConvex::intersect(const SpatialIndex * idx, BitList * partial, BitList * full) { index_ = idx; addlevel_ = idx->maxlevel_ - idx->buildlevel_; partial_ = partial; full_ = full; bitresult_ = true; range_ = false; doIntersect(); } /////////////INTERSECT//////////////////////////////////// // void SpatialConvex::intersect(const SpatialIndex * idx, ValVec * partial, ValVec * full) { index_ = idx; addlevel_ = idx->maxlevel_ - idx->buildlevel_; plist_ = partial; flist_ = full; bitresult_ = false; range_ = false; doIntersect(); } /////////////INTERSECT//////////////////////////////////// // void SpatialConvex::intersect(const SpatialIndex * idx, ValVec * idList) { index_ = idx; addlevel_ = idx->maxlevel_ - idx->buildlevel_; plist_ = idList; bitresult_ = false; range_ = true; doIntersect(); } /////////////DOINTERSECT////////////////////////////////// // void SpatialConvex::doIntersect() { simplify(); // don't work too hard... if(constraints_.length()==0)return; // nothing to intersect!! // Start with root nodes (index = 1-8) and intersect triangles for(uint32 i = 1; i <= 8; i++) triangleTest(i); } /////////////TRIANGLETEST///////////////////////////////// // triangleTest: this is the main test of a triangle vs a Convex. It // will properly mark up the flags for the triangular node[index], and // all its children SpatialMarkup SpatialConvex::triangleTest(uint64 id) { SpatialMarkup mark; // // do the face test on the triangle mark = testNode(V(NV(0)),V(NV(1)),V(NV(2))); // do we have a final result code? // if rEJECT, fULL then return if(mark > fULL) return mark; if(mark == fULL) { fillChildren(id); // propagate final result to children return mark; } // if pARTIAL or dONTKNOW, then continue, test children, // but do not reach beyond the leaf nodes. // If Convex is fully contained within one (sWALLOWED), // we can stop looking further in another child if (NC(id,0)!=0) { triangleTest(NC(id,0)); triangleTest(NC(id,1)); triangleTest(NC(id,2)); triangleTest(NC(id,3)); // we are at the leafnodes // If we have to recurse further, calculate intersections one by one // If not, just set the proper bit in partial_ or append id to plist_. } else { if(addlevel_) { // from now on, continue to build the triangles dynamically. // until maxlevel_ levels depth. testPartial(addlevel_, N(id).id_, V(NV(0)), V(NV(1)), V(NV(2))); } else { if(bitresult_) partial_->set((uint32)index_->leafNumberById(N(id).id_),true); else plist_->append(N(id).id_); } } return mark; } /////////////FILLCHILDREN///////////////////////////////// // fillChildren: mark children as full // void SpatialConvex::fillChildren(uint64 id) { if(range_) plist_->append(N(id).id_); else { if(NC(id,0)!=0) { for(size_t i = 0; i < 4; i++) { fillChildren(NC(id,i)); } } else { // we are at the leaf. If we still have levels to recurse, // fill them. If not, just set the full_ bitlist's or flist_ list's // value correctly. if(addlevel_) setfull(N(id).id_,addlevel_); else { if(bitresult_) full_->set((uint32)index_->leafNumberById(N(id).id_), true); else flist_->append(N(id).id_); } } } } /////////////SETFULL////////////////////////////////////// // setfull: set the bitlist leaves at level maxlevel to full. // if we have still levels to go, recurse. Use the id to get // the leaf node's index. See idbyname and namebyid for explanations. // void SpatialConvex::setfull(uint64 id, size_t level) { if(level--) { setfull(id << 2 , level); setfull((id << 2) + 1, level); setfull((id << 2) + 2, level); setfull((id << 2) + 3, level); } else { if(bitresult_) full_->set((uint32)index_->leafNumberById(id), true); else flist_->append(id); } } /////////////TESTPARTIAL////////////////////////////////// // testPartial: test a triangle's subtriangle whether they are partial. // if level is nonzero, recurse. // void SpatialConvex::testPartial(size_t level, uint64 id, const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { // if there is still a level to go, subdivide the // triangle according to our rules and test each subdivision. // (our rules are: each subdivided triangle has to be given // ordered counter-clockwise, 0th index starts of new 0-node, // 1st index starts off new 1-node, 2nd index starts off new 2-node // middle triangle gives new 3-node. // if we are at the bottom, set this id to partial. if(level--) { SpatialVector w0 = v1 + v2; w0.normalize(); SpatialVector w1 = v0 + v2; w1.normalize(); SpatialVector w2 = v1 + v0; w2.normalize(); testSubTriangle(level, (id << 2) , v0, w2, w1); testSubTriangle(level, (id << 2) + 1, v1, w0, w2); testSubTriangle(level, (id << 2) + 2, v2, w1, w0); testSubTriangle(level, (id << 2) + 3, w0, w1, w2); } else { if(bitresult_) partial_->set((uint32)index_->leafNumberById(id), true); else plist_->append(id); } } /////////////TESTSUBTRIANGLE//////////////////////////////// // testSubTriangle: call full or partial depending on result of testNode. // void SpatialConvex::testSubTriangle(size_t level, uint64 id, const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { // test this triangle. SpatialMarkup mark = testNode(v0, v1, v2); // if it is full, set all fulls below this level, too // else if it is partial or unknown or swallowed call testpartial // with this new level. if(mark == fULL) { if(range_) plist_->append(id); else setfull(id , level); } else if(mark < fULL) testPartial(level, id, v0, v1, v2); } /////////////TESTNODE///////////////////////////////////// // testNode: tests the QuadNodes for intersections. // SpatialMarkup SpatialConvex::testNode(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { // Start with testing the vertices for the QuadNode with this convex. int vsum = testVertex(v0) + testVertex(v1) + testVertex(v2); #ifdef DIAGNOSE char name[10]; SpatialVector v = v0 + v1 + v2; cout << index_->nameById(index_->idByPoint(v),name) << " " << vsum << " " << endl; #endif SpatialMarkup mark = testTriangle( v0, v1, v2, vsum); #ifdef DIAGNOSE cout << ( mark == pARTIAL ? " partial " : ( mark == fULL ? " full " : ( mark == rEJECT ? " reject " : " dontknow " ) ) ) << name << endl; /* << v0 << "," << v1 << "," << v2 << " " << endl; << V(NV(0)) << " , " << V(NV(1)) << " , " << V(NV(2)) << endl << " (" << V(NV(0)).ra() << "," << V(NV(0)).dec() << ")" << " (" << V(NV(1)).ra() << "," << V(NV(1)).dec() << ")" << " (" << V(NV(2)).ra() << "," << V(NV(2)).dec() << ")" << endl; */ #endif // since we cannot play games using the on-the-fly triangles, // substitute dontknow with partial. if (mark == dONTKNOW) mark = pARTIAL; return mark; } /////////////TESTTRIANGLE////////////////////////////////// // testTriangle: tests a triangle given by 3 vertices if // it intersects the convex. // SpatialMarkup SpatialConvex::testTriangle(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2, int vsum) { if(vsum == 1 || vsum == 2) return pARTIAL; // If vsum = 3 then we have all vertices inside the convex. // Now use the following decision tree: // // * If the sign of the convex is pOS or zERO : mark as fULL intersection. // // * Else, test for holes inside the triangle. A 'hole' is a nEG constraint // that has its center inside the triangle. If there is such a hole, // return pARTIAL intersection. // // * Else (no holes, sign nEG or mIXED) test for intersection of nEG // constraints with the edges of the triangle. If there are such, // return pARTIAL intersection. // // * Else return fULL intersection. if(vsum == 3) { if(sign_ == pOS || sign_ == zERO) return fULL; if ( testHole(v0,v1,v2) ) return pARTIAL; if ( testEdge(v0,v1,v2) ) return pARTIAL; return fULL; } // If we have reached that far, we have vsum=0. There is no definite // decision making possible here with our methods, the markup may result // in dONTKNOW. The decision tree is the following: // // * Test with bounding circle of the triangle. // // # If the sign of the convex zERO test with the precalculated // bounding circle of the convex. If it does not intersect with the // triangle's bounding circle, rEJECT. // // # If the sign of the convex is nonZERO: if the bounding circle // lies outside of one of the constraints, rEJECT. // // * Else: there was an intersection with the bounding circle. // // # For zERO convexes, test whether the convex intersects the edges. // If none of the edges of the convex intersects with the edges of // the triangle, we have a rEJECT. Else, pARTIAL. // // # If sign of convex is pOS, or miXED and the smallest constraint does // not intersect the edges and has its center inside the triangle, // return sWALLOW. If no intersection of edges and center outside // triangle, return rEJECT. // // # So the smallest constraint DOES intersect with the edges. If // there is another pOS constraint which does not intersect with // the edges, and has its center outside the triangle, return // rEJECT. If its center is inside the triangle return sWALLOW. // Else, return pARTIAL for pOS and dONTKNOW for mIXED signs. // // * If we are here, return dONTKNOW. There is an intersection with // the bounding circle, none of the vertices is inside the convex and // we have very strange possibilities left for pOS and mIXED signs. For // nEG, i.e. all constraints negative, we also have some complicated // things left for which we cannot test further. if ( !testBoundingCircle(v0,v1,v2) ) return rEJECT; if ( sign_ == pOS || sign_ == mIXED || (sign_ == zERO && constraints_.length() <= 2)) { // Does the smallest constraint intersect with the edges? if ( testEdgeConstraint(v0,v1,v2,0) ) { // Is there another positive constraint that does NOT intersect with // the edges? size_t cIndex; if ( cIndex = testOtherPosNone(v0,v1,v2) ) { // Does that constraint lie inside or outside of the triangle? if ( testConstraintInside(v0,v1,v2, cIndex) ) return pARTIAL; // Does the triangle lie completely within that constr? else if( constraints_.vector_[cIndex].contains(v0) ) return pARTIAL; else return rEJECT; } else { if(sign_ == pOS || sign_ == zERO) return pARTIAL; else return dONTKNOW; } } else { if (sign_ == pOS || sign_ == zERO) { // Does the smallest lie inside or outside the triangle? if( testConstraintInside(v0,v1,v2, 0) ) return pARTIAL; else return rEJECT; } else return dONTKNOW; } } else if (sign_ == zERO) { if ( corners_.length() > 0 && testEdge0(v0,v1,v2) ) return pARTIAL; else return rEJECT; } return pARTIAL; } /////////////TESTVERTEX///////////////////////////////////// // testVertex: same as above, but for any spatialvector, no markup speedup // int SpatialConvex::testVertex(const SpatialVector & v) { for ( size_t i = 0; i < constraints_.length(); i++) if ( (constraints_.vector_[i].a_ * v ) < constraints_.vector_[i].d_ ) return 0; return 1; } /////////////TESTHOLE///////////////////////////////////// // testHole: test for holes. If there is a negative constraint whose center // is inside the triangle, we speak of a hole. Returns true if // found one. // bool SpatialConvex::testHole(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { bool test = false; for(size_t i = 0; i < constraints_.length(); i++) { if ( constraints_.vector_[i].sign_ == nEG ) { // test only 'holes' // If (a ^ b * c) < 0, vectors abc point clockwise. // -> center c not inside triangle, since vertices a,b are ordered // counter-clockwise. The comparison here is the other way // round because c points into the opposite direction as the hole if ( ( ( v0 ^ v1 ) * constraints_.vector_[i].a_) > 0.0L ) continue; if ( ( ( v1 ^ v2 ) * constraints_.vector_[i].a_) > 0.0L ) continue; if ( ( ( v2 ^ v0 ) * constraints_.vector_[i].a_) > 0.0L ) continue; test = true; break; } } return test; } /////////////TESTEDGE0//////////////////////////////////// // testEdge0: test if the edges intersect with the zERO convex. // The edges are given by the vertex vectors e[0-2] // All constraints are great circles, so test if their intersect // with the edges is inside or outside the convex. // If any edge intersection is inside the convex, return true. // If all edge intersections are outside, check whether one of // the corners is inside the triangle. If yes, all of them must be // inside -> return true. // bool SpatialConvex::testEdge0(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { // We have constructed the corners_ array in a certain direction. // now we can run around the convex, check each side against the 3 // triangle edges. If any of the sides has its intersection INSIDE // the side, return true. At the end test if a corner lies inside // (because if we get there, none of the edges intersect, but it // can be that the convex is fully inside the triangle. so to test // one single edge is enough) struct edgeStruct { SpatialVector e; // The half-sphere this edge delimits float64 l; // length of edge const SpatialVector *e1; // first end const SpatialVector *e2; // second end } edge[3]; // fill the edge structure for each side of this triangle edge[0].e = v0 ^ v1; edge[0].e1 = &v0; edge[0].e2 = &v1; edge[1].e = v1 ^ v2; edge[1].e1 = &v1; edge[1].e2 = &v2; edge[2].e = v2 ^ v0; edge[2].e1 = &v2; edge[2].e2 = &v0; edge[0].l = acos(v0 * v1); edge[1].l = acos(v1 * v2); edge[2].l = acos(v2 * v0); for(size_t i = 0; i < corners_.length(); i++) { size_t j = 0; if(i < corners_.length() - 1) j = i+1; SpatialVector a1; float64 l1,l2; // lengths of the arcs from intersection to edge corners float64 cedgelen = acos(corners_(i) * corners_(j)); // length of edge of convex // calculate the intersection - all 3 edges for (size_t iedge = 0; iedge < 3; iedge++) { a1 = ( edge[iedge].e ) ^ ( corners_(i) ^ corners_(j) ); a1.normalize(); // if the intersection a1 is inside the edge of the convex, // its distance to the corners is smaller than the edgelength. // this test has to be done for both the edge of the convex and // the edge of the triangle. for(size_t k = 0; k < 2; k++) { l1 = acos(corners_(i) * a1); l2 = acos(corners_(j) * a1); if( l1 - cedgelen <= gEpsilon && l2 - cedgelen <= gEpsilon ) { l1 = acos( *(edge[iedge].e1) * a1 ); l2 = acos( *(edge[iedge].e2) * a1 ); if( l1 - edge[iedge].l <= gEpsilon && l2 - edge[iedge].l <= gEpsilon ) return true; } a1 *= -1.0; // do the same for the other intersection } } } return testVectorInside(v0,v1,v2,corners_(0)); } /////////////TESTEDGE///////////////////////////////////// // testEdge: test if edges intersect with constraint. This problem // is solved by a quadratic equation. Return true if there is // an intersection. // bool SpatialConvex::testEdge(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { for(size_t i = 0; i < constraints_.length(); i++) { if ( constraints_.vector_[i].sign_ == nEG ) { // test only 'holes' if ( eSolve(v0, v1, i) ) return true; if ( eSolve(v1, v2, i) ) return true; if ( eSolve(v2, v0, i) ) return true; } } return false; } /////////////ESOLVE/////////////////////////////////////// // eSolve: solve the quadratic eq. for intersection of an edge with a circle // constraint. Edge given by grand circle running through v1, v2 // Constraint given by cIndex. bool SpatialConvex::eSolve(const SpatialVector & v1, const SpatialVector & v2, size_t cIndex) { float64 gamma1 = v1 * constraints_.vector_[cIndex].a_ ; float64 gamma2 = v2 * constraints_.vector_[cIndex].a_ ; float64 mu = v1 * v2; float64 u2 = (1 - mu) / (1 + mu); float64 a = - u2 * (gamma1 + constraints_.vector_[cIndex].d_); float64 b = gamma1 * ( u2 - 1 ) + gamma2 * ( u2 + 1 ); float64 c = gamma1 - constraints_.vector_[cIndex].d_; float64 D = b * b - 4 * a * c; if( D < 0.0L ) return false; // no intersection // calculate roots a'la Numerical Recipes float64 q = -0.5L * ( b + ( SGN(b) * sqrt(D) ) ); float64 root1, root2; int i = 0; if ( a > gEpsilon || a < -gEpsilon ) { root1 = q / a; i++; } if ( q > gEpsilon || q < -gEpsilon ) { root2 = c / q; i++; } // Check whether the roots lie within [0,1]. If not, the intersection // is outside the edge. if (i == 0) return false; // no solution if ( root1 >= 0.0L && root1 <= 1.0L ) return true; if ( i == 2 && ( (root1 >= 0.0L && root1 <= 1.0L ) || (root2 >= 0.0L && root2 <= 1.0L ) ) ) return true; return false; } /////////////TESTBOUNDINGCIRCLE/////////////////////////// // testBoundingCircle: test for boundingCircles intersecting with constraint // bool SpatialConvex::testBoundingCircle(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { // Set the correct direction: The normal vector to the triangle plane SpatialVector c = ( v1 - v0 ) ^ ( v2 - v1 ); c.normalize(); // Set the correct opening angle: Since the plane cutting out the triangle // also correctly cuts out the bounding cap of the triangle on the sphere, // we can take any corner to calculate the opening angle float64 d = acos (c * v0); // for zero convexes, we have calculated a bounding circle for the convex. // only test with this single bounding circle. if(sign_ == zERO) { float64 tst; if ( ( (tst = c * boundingCircle_.a_) < -1.0L + gEpsilon ? gPi : acos(tst) ) > ( d + boundingCircle_.s_) ) return false; return true; } // for all other convexes, test every constraint. If the bounding // circle lies completely outside of one of the constraints, reject. // else, accept. size_t i; for(i = 0; i < constraints_.length(); i++) { if ( ( (c * constraints_.vector_[i].a_) < -1.0L + gEpsilon ? gPi : acos(c * constraints_.vector_[i].a_) ) > ( d + constraints_.vector_[i].s_) ) return false; } return true; } /////////////TESTEDGECONSTRAINT/////////////////////////// // testEdgeConstraint: test if edges intersect with a given constraint. // bool SpatialConvex::testEdgeConstraint(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2, size_t cIndex) { if ( eSolve(v0, v1, cIndex) ) return true; if ( eSolve(v1, v2, cIndex) ) return true; if ( eSolve(v2, v0, cIndex) ) return true; return false; } /////////////TESTOTHERPOSNONE///////////////////////////// // testOtherPosNone: test for other positive constraints that do // not intersect with an edge. Return its index // size_t SpatialConvex::testOtherPosNone(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2) { size_t i = 1; while ( i < constraints_.length() && constraints_.vector_[i].sign_ == pOS ) { if ( !testEdgeConstraint ( v0,v1,v2, i ) ) return i; i++; } return 0; } /////////////TESTCONSTRAINTINSIDE///////////////////////// // testConstraintInside: look if a constraint is inside the triangle // bool SpatialConvex::testConstraintInside(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2, size_t i) { return testVectorInside(v0,v1,v2, constraints_.vector_[i].a_); } /////////////TESTVECTORINSIDE//////////////////////////// // testVectorInside: look if a vector is inside the triangle // bool SpatialConvex::testVectorInside(const SpatialVector & v0, const SpatialVector & v1, const SpatialVector & v2, SpatialVector & v) { // If (a ^ b * c) < 0, vectors abc point clockwise. // -> center c not inside triangle, since vertices are ordered // counter-clockwise. if( ( (( v0 ^ v1 ) * v) < 0 ) || ( (( v1 ^ v2 ) * v) < 0 ) || ( (( v2 ^ v0 ) * v) < 0 ) ) return false; return true; } /////////////READ///////////////////////////////////////// // void SpatialConvex::read(istream &in) { size_t nconstr; SpatialConstraint constr; in.setf(ios::skipws); while(in.peek() == COMMENT) // ignore comments in.ignore(10000,'\n'); in >> nconstr ; in.ignore(); // ignore endl if(!in.good()) throw SpatialFailure("SpatialConvex:read: Could not read constraint"); for(size_t i = 0; i < nconstr; i++) { if(in.eof()) throw SpatialFailure("SpatialConvex:read: Premature end-of-file"); in >> constr; if(!in.good()) throw SpatialFailure("SpatialConvex:read: Could not read constraint"); add(constr); } } /////////////READ///////////////////////////////////////// // void SpatialConvex::readRaDec(istream &in) { size_t nconstr; SpatialConstraint constr; while(in.peek() == COMMENT) // ignore comments in.ignore(10000,'\n'); in >> nconstr ; in.ignore(); // ignore endl for(size_t i = 0; i < nconstr; i++) { constr.readRaDec(in); add(constr); } } /////////////WRITE//////////////////////////////////////// // void SpatialConvex::write(ostream &out) const { out << "#CONVEX" << endl; out << constraints_.length() << endl; for (size_t i = 0; i < constraints_.length() ; i++) out << constraints_[i]; } /////////////>>/////////////////////////////////////////// // read from istream // istream& operator >>( istream& in, SpatialConvex & c) { c.read(in); return(in); } /////////////<