} // METHOD
/// <summary>
/// Test for boundingCircles intersecting with halfspace
/// </summary>
/// <param name="v0"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
private bool testBoundingCircle(Cartesian v0, Cartesian v1, Cartesian v2){
// Set the correct direction: The normal vector to the triangle plane
//Cartesian c = ( v1 - v0 ) ^ ( v2 - v1 );
//c.normalize();
Cartesian c = new Cartesian(v1);
c.subMe(v0);
Cartesian t = new Cartesian(v2);
t.subMe(v1);
c.crossMe(t);
c.normalizeMe();
// 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 = Math.Acos (c.dot(v0));
// for zero convexes, we have calculated a bounding circle for the convex.
// only test with this single bounding circle.
//
// NOTE! Bounding circle gets changed by simplify0
// If you did not run simplify, bounding circle may contain
// unpredictable numbers.
//
if(_sign == Halfspace.Sign.Zero && _boundingCircle != null) {
double tst;
tst = (tst = c.dot(_boundingCircle.sv));
if ((tst < -1.0 + Cartesian.Epsilon ? Cartesian.Pi : Math.Acos(tst)) >
(d + _boundingCircle.hangle)) 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.
int i;
double ftmp;
Halfspace hs;
for(i = 0; i < _halfspaces.Count; i++) {
hs = (Halfspace) _halfspaces[i];
if (c.dot(hs.sv) < -1.0 + Cartesian.Epsilon){
ftmp = Cartesian.Pi;
} else {
ftmp = Math.Acos(c.dot(hs.sv));
}
if (ftmp > (d + hs.hangle)){
return false;
}
}
return true;
} // end METHOD testBoundingCircle
/////////////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);
}
}
}
}
}
}