Ejemplo n.º 1
0
		/// <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;
		}
Ejemplo n.º 2
0
		/// <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;
		}
Ejemplo n.º 3
0
		/// <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);
		}
Ejemplo n.º 4
0
		/// <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);
		}
Ejemplo n.º 5
0
		/// <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);
		}
Ejemplo n.º 6
0
		/// <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;
			}

		}
Ejemplo n.º 7
0
		private void init(){
			_halfspaces     = new ArrayList(); // will change
			_corners        = new ArrayList();
			_sign           = Halfspace.Sign.Zero;
			_boundingCircle = new Halfspace();
			_htm		    = new HtmTrixel();
		}
Ejemplo n.º 8
0
		private  void clear(){
			_halfspaces     = null;
			_corners        = null;
			_sign           = Halfspace.Sign.Zero;
			_boundingCircle = null;
			_htm		    = new HtmTrixel();
		}
Ejemplo n.º 9
0
		/////////////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);
							}
						}
					}
				}
			}
		}
Ejemplo n.º 10
0
		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;
		}