/// <summary>
/// Look if a vector is inside the triangle
/// returns true if vector is inside
/// </summary>
/// <param name="v0"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="v"></param>
/// <returns>true if vector is inside the triangle</returns>
private bool testVectorInside( // SUSPECT
Cartesian v0, Cartesian v1, Cartesian v2, Cartesian v){
// If (a ^ b * c) < 0, vectors abc point clockwise.
// -> center c not inside triangle, since vertices are ordered
// counter-clockwise.
Cartesian c01 = new Cartesian(v0);
Cartesian c12 = new Cartesian(v1);
Cartesian c20 = new Cartesian(v2);
c01.crossMe(v1);
c12.crossMe(v2);
c20.crossMe(v0);
// if (((( v0 ^ v1 ) * v) < 0 ) ||
// ( (( v1 ^ v2 ) * v) < 0 ) ||
// ( (( v2 ^ v0 ) * v) < 0 ) )
// return false;
// return true;
if (c01.dot(v) < 0)
return false;
if (c12.dot(v) < 0)
return false;
if (c20.dot(v) < 0)
return false;
return true;
}
} // 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
/////////////TESTEDGE0////////////////////////////////////
// testEdge0:
//
/// <summary>
/// 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.
/// </summary>
/// <param name="v0">vertex vector e[0]</param>
/// <param name="v1">vertex vector e[1]</param>
/// <param name="v2">vertex vector e[2]</param>
/// <returns></returns>
private bool testEdge0(Cartesian v0, Cartesian v1, Cartesian 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)
edgeStruct[] edge = new edgeStruct[3];
if (_corners.Count < 1) {
throw new Exception("testEdge0: There are no corners");
}
// fill the edge structure for each side of this triangle
Cartesian c01 = new Cartesian(v0);
Cartesian c12 = new Cartesian(v1);
Cartesian c20 = new Cartesian(v2);
c01.crossMe(v1);
c12.crossMe(v2);
c20.crossMe(v0);
edge[0].e = c01;
edge[0].e1 = v0;
edge[0].e2 = v1;
edge[0].l = Math.Acos(v0.dot(v1));
edge[1].e = c12;
edge[1].e1 = v1;
edge[1].e2 = v2;
edge[1].l = Math.Acos(v1.dot(v2));
edge[2].e = c20;
edge[2].e1 = v2;
edge[2].e2 = v0;
edge[2].l = Math.Acos(v2.dot(v0));
// 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(int i = 0; i < _corners.Count; i++) {
int j = 0;
if(i < _corners.Count - 1){
j = i+1;
}
Cartesian a1 = new Cartesian();
Cartesian tmp;
double arg1, arg2;
double l1,l2; // lengths of the arcs from intersection to edge corners
double cedgelen = Math.Acos(
((Cartesian) _corners[i]).dot
((Cartesian) _corners[j])); // length of edge of convex
// calculate the intersection - all 3 edges
for (int iedge = 0; iedge < 3; iedge++) {
a1.assign(edge[iedge].e);
tmp =
((Cartesian) _corners[i]).cross
((Cartesian) _corners[j]);
a1.crossMe(tmp);
if (a1.normalizeMeSafely()){
// WARNING
// Keep your eye on this, used to crash here...
//
// 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(int k = 0; k < 2; k++) {
l1 = Math.Acos(((Cartesian) _corners[i]).dot(a1));
l2 = Math.Acos(((Cartesian) _corners[j]).dot(a1));
if( l1 - cedgelen <= Cartesian.Epsilon &&
l2 - cedgelen <= Cartesian.Epsilon){
arg1 = (edge[iedge].e1).dot(a1);
arg2 = (edge[iedge].e2).dot(a1);
if (arg1 > 1.0) arg1 = 1.0;
if (arg2 > 1.0) arg2 = 1.0;
l1 = Math.Acos(arg1);
l2 = Math.Acos(arg2);
if( l1 - edge[iedge].l <= Cartesian.Epsilon &&
l2 - edge[iedge].l <= Cartesian.Epsilon)
return true;
}
a1.scaleMe(-1.0); // do the same for the other intersection
}
}
}
}
return testVectorInside(v0,v1,v2,(Cartesian) _corners[0]);
}
private bool testHole(Cartesian v0, Cartesian v1, Cartesian v2){
Cartesian r1, r2, r3;
bool test = false;
Halfspace hs;
r1 = new Cartesian();
r2 = new Cartesian();
r3 = new Cartesian();
for(int i = 0; i < _halfspaces.Count; i++) {
hs = (Halfspace) _halfspaces[i];
if ( hs.sign < 0) {
// 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
//
// The old code said this:
// if ( ( ( v0 ^ v1 ) *
// _halfspaces[i].a_) > 0.0L ) continue;
// if ( ( ( v1 ^ v2 ) *
// _halfspaces[i].a_) > 0.0L ) continue;
// if ( ( ( v2 ^ v0 ) *
// _halfspaces[i].a_) > 0.0L ) continue;
r1.assign(v0);
r1.crossMe(v1);
if ( r1.dot( hs.sv) > 0.0) continue;
r1.assign(v1);
r1.crossMe(v2);
if ( r1.dot( hs.sv) > 0.0) continue;
r1.assign(v2);
r1.crossMe(v0);
if ( r1.dot( hs.sv) > 0.0) continue;
test = true;
break;
}
}
return test;
}
/////////////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;
}
