/// <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;
}
