/// <summary> /// Tests relative position of other halfspace. /// There are four possibilities /// they intersect, they are disjoint, this halfspace /// contains the other, or it is contained in the other /// </summary> /// <param name="other"></param> /// <returns>Position (one of Intersecting, Disjoint, Contained, or Surrounds)</returns> public Position relativePosition(Halfspace other){ double phi; double dot; //double alter = 1.0; dot = this._SV.dot(other._SV); // // If the halfspace is "negative", then flip the // normal vector. This has the affect of negating the // dot product of the original two vectors // Note that (a. -b) == - (a.b) where . is vector inner product // // There was some old stuff here, that flipped // the angles, etc, when D < 0, but I don't think // that's right. The inclusion exclusion works // even if D < 0 // if (this._sign == Sign.Negative || other._sign == Sign.Negative){ // alter = -1.0; // } // // if one is positive and one is negative, then look for the relationship // between the hole of the negative halfspace and the positive halfspace // dot *= alter; // The following to keep the Acos library happy. Sometimes // it can be just a wee outside (-1, +1), and Acos is known // do blow up in the Windows environment, when something else // is in control of the math environs. if (dot <= -1.0 + SpatialVector.Epsilon){ phi = SpatialVector.Pi; } else if (dot >= 1.0 - SpatialVector.Epsilon){ phi = 0.0; } else { phi = Math.Acos(dot); } double a1 = this._hangle; double a2 = other._hangle; // if (this._sign != Sign.Positive) a1 = SpatialVector.Pi - a1; // if (other._sign != Sign.Positive) a2 = SpatialVector.Pi - a2; if (phi > a1 + a2) { return Position.Disjoint; } if (phi <= SpatialVector.Epsilon && Math.Abs(a1 - a2) <= SpatialVector.Epsilon) { return Position.Identical; } if (a1 > phi + a2) { return Position.Surrounds; } if (a2 > phi + a1) { return Position.Contained; } return Position.Intersecting; }
/// <summary> /// Copy given halfspace into this one. /// </summary> /// <param name="other">the halfspace to be copied</param> public void copy(Halfspace other){ _ID = other._ID; _D = other._D; _SV.x = other.sv.x; _SV.y = other.sv.y; _SV.z = other.sv.z; _hangle = other._hangle; _sign = other._sign; }
/// <summary> /// Test whether or not this halfspace completely surrounds /// (contains) another halfspace /// </summary> /// <param name="other">The other halfspace</param> /// <returns>true if this halfspace completely surrounds other</returns> public bool contains(Halfspace other){ return (this.relativePosition(other) == Position.Surrounds); }
/// <summary> /// Add a Halfspace to this Convex /// New Halfspace will be added to the end of the list /// </summary> /// <param name="hs">What to add to the convex</param> public void add(Halfspace hs){ _halfspaces.Add(hs); sortandsign(hs.sign); }
/// <summary> /// Add a new Halfspace specified by the four /// parameters /// </summary> /// <param name="x">X coordinate of vector</param> /// <param name="y">Y coordinate of vector</param> /// <param name="z">Z coordinate of vector</param> /// <param name="d">signed distance of cutting plane</param> public void add(double x, double y, double z, double d){ Halfspace hs = new Halfspace(x, y, z, d); _halfspaces.Add(hs); sortandsign(hs.sign); }
/// <summary> /// Maintain halfspaces in ascending opening angle order /// Called after each add, which puts a new halfspace at the /// end of the arraylist. "Bubble" the new addition to its proper /// place. Also assign a "Sign" from to this Convex /// One of: Positive, negative, zero, mixed. /// </summary> /// <param name="sign">The sign of the most recently added halfspace</param> private void sortandsign(Halfspace.Sign sign) { int i; int cnt; Halfspace hs; Halfspace hsprev; Object o; cnt = _halfspaces.Count; for(i = cnt-1; i>0; i--){ hs = (Halfspace) _halfspaces[i]; hsprev = (Halfspace) _halfspaces[i-1]; if (hs.hangle > hsprev.hangle){ // New halfspace has wider angle than previous one // so we are done! break; } else { // new halfspace needs to trade places with previous one o = hsprev; _halfspaces[i-1] = hs; _halfspaces[i] = o; } } // Done sorting, now update sign if(cnt == 1){ this._sign = sign; return; } switch(this._sign){ case Halfspace.Sign.Negative: if (sign == Halfspace.Sign.Positive) this._sign = Halfspace.Sign.Mixed; break; case Halfspace.Sign.Positive: if (sign == Halfspace.Sign.Negative) this._sign = Halfspace.Sign.Mixed; break; case Halfspace.Sign.Zero: this._sign = sign; break; case Halfspace.Sign.Mixed: // Mixed stays mixed break; } }
private void init(){ _halfspaces = new ArrayList(); // will change _corners = new ArrayList(); _sign = Halfspace.Sign.Zero; _boundingCircle = new Halfspace(); _htm = new HtmTrixel(); }
private void clear(){ _halfspaces = null; _corners = null; _sign = Halfspace.Sign.Zero; _boundingCircle = null; _htm = new HtmTrixel(); }
/////////////SIMPLIFY0//////////////////////////////////// // // /// <summary> /// 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. /// </summary> private void simplify0() { int i; int j,k; Cartesian vi1, vi2; //typedef std::vector<size_t> ValueVectorSzt; //ValueVectorSzt cornerConstr1, cornerConstr2, removeConstr; //ValueVectorSpvec corner; // bool ruledout; ArrayList corner = new ArrayList(); ArrayList cornerConstr1 = new ArrayList(); ArrayList cornerConstr2 = new ArrayList(); ArrayList removeConstr = new ArrayList(); bool vi1ok, vi2ok; bool[] ruledout = null; if (_halfspaces.Count == 1) { // for one constraint, it is itself the BC _boundingCircle.copy((Halfspace) _halfspaces[0]); return; } else if(_halfspaces.Count == 2) { // 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. // test for constraints being identical - rule 1 out Halfspace hs0, hs1; hs0 = (Halfspace) _halfspaces[0]; hs1 = (Halfspace) _halfspaces[1]; if(hs0.sv.eq(hs1.sv)){ // delete 1 _halfspaces[1] = null; // was:constraints_.erase(constraints_.end()-1); _boundingCircle.copy(hs0); return; } // test for constraints being two disjoint half spheres - empty convex! if(hs0.sv.opposite(hs1.sv)){ _halfspaces.Clear(); return; } _boundingCircle = new Halfspace(); // TODO: halfspace constructor from sum of two vectors _boundingCircle.d = 0; _boundingCircle.sv.assign(hs0.sv); _boundingCircle.sv.addMe(hs1.sv); _boundingCircle.sv.normalizeMe(); // SpatialConstraint(constraints_[0].v() + // constraints_[1].v(),0); return; } // Go over all pairs of constraints ruledout = new bool[_halfspaces.Count]; for(i=0; i<(int)_halfspaces.Count; i++){ ruledout[i] = true; } for(i = 0; i < _halfspaces.Count - 1; i++) { // ruledout = true; // was: Halfspace hi, hj; hi = (Halfspace) _halfspaces[i]; for(j = i+1; j < _halfspaces.Count; j ++) { hj = (Halfspace) _halfspaces[j]; // test for constraints being identical - rule i out if(hi.sv.eq(hj.sv)) //constraints_[i].a_ == constraints_[j].a_) break; // test for constraints being two disjoint half spheres - empty convex! if(hi.sv.opposite(hj.sv)){ _halfspaces.Clear(); return; } // vi1 and vi2 are their intersection points // vi1 = constraints_[i].a_ ^ constraints_[j].a_ ; // vi1.normalize(); // vi2 = (-1.0) * vi1; vi1 = new Cartesian(hi.sv); vi1.crossMe(hj.sv); vi1.normalizeMe(); vi2 = new Cartesian(vi1); vi2.scaleMe(-1.0); vi1ok = true; vi2ok = true; // // now test whether vi1 or vi2 or both are inside every constraint. // other than i or j // if yes, store them in the corner array. // vi1ok is false, if vi1 is outisde even one constraint // for(k = 0; k < _halfspaces.Count; k++) { Halfspace hk; if( k == i || k == j) continue; hk = (Halfspace)_halfspaces[k]; if(vi1ok && vi1.dot(hk.sv) <= 0.0) { vi1ok = false; } if(vi2ok && vi2.dot(hk.sv) <= 0.0) { vi2ok = false; } if(!vi1ok && !vi2ok) // don't look further break; } if(vi1ok) { // vi1 is inside all other constraints corner.Add(vi1); // was: corner.push_back(vi1); cornerConstr1.Add(i); cornerConstr2.Add(j); // was: cornerConstr1.push_back(i); // was: cornerConstr2.push_back(j); ruledout[i] = false; ruledout[j] = false; // ruledout = false; } if(vi2ok) { // corner.push_back(vi2); // cornerConstr1.push_back(i); // cornerConstr2.push_back(j); corner.Add(vi2); cornerConstr1.Add(i); cornerConstr2.Add(j); ruledout[i] = false; ruledout[j] = false; // ruledout = false; } } // END of J loop // is this constraint ruled out? i.e. none of its intersections // with other constraints are corners... remove it from constraints_ list. /******************** WAS: if(ruledout) { removeConstr.push_back(i); } **********************************************/ } // END of i loop // See if you can rule out a constraint // for(i=0; i< (int) _halfspaces.Count; i++){ if (ruledout[i]){ removeConstr.Add(i); } } ruledout = null; // delete ruledout; // 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. if (corner.Count == 0){ _halfspaces.Clear(); return; } _corners.Clear(); _corners.Add(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 = (int) cornerConstr1[0]; // the i'th constraint and j'th constraint j = (int) cornerConstr2[0]; // intersect at 0'th corner int 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. c1 = c2 = k1 = k2 = -1; vi1 = null; vi2 = null; for( k = 1; k < cornerConstr1.Count; k ++) { if((int) cornerConstr1[k] == i) { vi1 = (Cartesian) corner[k]; c1 = (int) cornerConstr2[k]; k1 = k; } if((int) cornerConstr2[k] == i) { vi1 = (Cartesian) corner[k]; c1 = (int) cornerConstr1[k]; k1 = k; } if((int) cornerConstr1[k] == j) { vi2 = (Cartesian) corner[k]; c2 = (int) cornerConstr2[k]; k2 = k; } if((int) cornerConstr2[k] == j) { vi2 = (Cartesian) corner[k]; c2 = (int) 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... // int c,currentCorner; Cartesian va; va = new Cartesian(vi1); va.subMe((Cartesian) corner[0]); va.crossMe( ((Halfspace) _halfspaces[i]).sv); if (va.dot((Cartesian) corner[0]) > 0.0){ if (k1 == -1) { throw (new Exception("Simplify0:k1 uninitialized")); } _corners.Add(vi1); c = c1; currentCorner = k1; } else { if (k2 == -1) { throw (new Exception("Simplify0:k2 uninitialized")); } _corners.Add(vi2); c = c2; currentCorner = k2; } // if( ((vi1 - corner[0]) ^ constraints_[i].a_) * corner[0] > 0 ) { // corners_.push_back(vi1); // c = c1; // currentCorner = k1; // } else { // corners_.push_back(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 > 0 ) { for (k = 0; k < cornerConstr1.Count; k++) { if(k == currentCorner)continue; if( (int) cornerConstr1[k] == c) { if( (currentCorner = k) == 0) break; _corners.Add(corner[k]); c = (int) cornerConstr2[k]; break; } if((int) cornerConstr2[k] == c) { if( (currentCorner = k) == 0) break; _corners.Add(corner[k]); c = (int) cornerConstr1[k]; break; } } } // Remove all redundant constraints for ( i = removeConstr.Count - 1; i>=0; i--){ //Start from END // WAS earlier: constraints_.erase(constraints_.end()-removeConstr[i]-1); /// in C++:constraints_.erase(constraints_.begin()+removeConstr[i]); _halfspaces.RemoveAt( (int) 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 (_halfspaces.Count >=3 ) { for(i = 0; i < (int) _corners.Count; i++){ for(j = i+1; j < _corners.Count; j++){ for(k = j+1; k < _corners.Count; k++) { Cartesian v = new Cartesian(); Cartesian tv = new Cartesian(); v.assign((Cartesian) _corners[j]); v.subMe((Cartesian) _corners[i]); tv.assign((Cartesian) _corners[k]); tv.subMe((Cartesian) _corners[j]); v.crossMe(tv); v.normalizeMe(); // 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 double d = v.dot((Cartesian) _corners[i]); if(_boundingCircle.d > d){ _boundingCircle.d = d; _boundingCircle.sv.assign(v); //new Halfspace(v,d); } } } } } }
public Polygon.Error add(double[] x, double[] y, double[] z, int len){ bool DIRECTION = false; bool FIRST_DIRECTION = false; // The constraint we have for each side is a 0-constraint (great circle) // passing through the 2 corners. Since we are in counterclockwise order, // the vector product of the two successive corners just gives the correct // constraint. // // Polygons should be counterclockwise // Polygons are assumed to be convex, otherwise windingerror is // computed wrong. int ix; Cartesian v = new Cartesian(); Convex cvx = new Convex(Convex.Mode.Normal); int i; /* PASS 1: check for winding error */ for(i = 0; i < len; i++) { // Keep track of winding direction. Should be positive // that is, CCW. ix = (i==len-1 ? 0 : i+1); if (i>0){ // test third corner against the constraint just formed // v is computed in the previous iteration // Look at a corner dot v if (v.dot(x[ix], y[ix], z[ix]) < Cartesian.Epsilon) { DIRECTION = true; if (i == 1) { FIRST_DIRECTION = true; } // break; // Move to pass 2 } else { DIRECTION = false; if (i == 1) { FIRST_DIRECTION = false; } } if (i > 1) { if (DIRECTION != FIRST_DIRECTION) { // C++: must clea up new Cartesian and convex // or better yet, do no allocate on top until you know // you need it return Polygon.Error.errBowtieOrConcave; // BOWTie error } } } // v = corners[i] ^ corners[ i == len-1 ? 0 : i + 1]; v.assign(x[i], y[i], z[i]); v.crossMe(x[ix], y[ix], z[ix]); if (v.isLength(0.0, Cartesian.Epsilon)) { return Polygon.Error.errZeroLength; } // WARNING! if v = zerovector, then edge error!!! } /* PASS 2: build convex in either original or reverse order */ /* forward: Go from 0 to len-1 by +1, cross i and i+1 (or 0) reverse: Go from len-1 to 0 by -1, cross i and i-1 (or len-1) */ if (DIRECTION) { for(i=len-1; i>=0; i--){ // v = corners[i] ^ corners[ i == 0 ? len-1 : i-1]; // v.normalize(); ix = (i==0? len-1 : i-1); v.assign(x[i], y[i], z[i]); v.crossMe(x[ix], y[ix], z[ix]); // WARNING! if v = zerovector, then edge error!!! v.normalizeMe(); Halfspace c = new Halfspace(v, 0.0); // SpatialConstraint c(v,0); cvx.add(c); } } else { for(i=0; i<len; i++){ //v = corners[i] ^ corners[ i == len-1 ? 0 : i+1]; // v.normalize(); ix = (i==len-1 ? 0 : i+1); v.assign(x[i], y[i], z[i]); v.crossMe(x[ix], y[ix], z[ix]); // WARNING! if v = zerovector, then edge error!!! v.normalizeMe(); Halfspace c = new Halfspace(v, 0.0); cvx.add(c); } } _reg.add(cvx); return Polygon.Error.Ok; }