/*
//#     Filename:       SpatialConvex.c
//#
//#     The SpatialConvex functions are here.
//#
//#     Author:         Peter Z. Kunszt based on A. Szalay's code
//#     
//#     Date:           October 23, 1998
//#
//#
//#
//# (c) Copyright The Johns Hopkins University 1998
//# All Rights Reserved
//#
//# The software and information contained herein are proprietary to The
//# Johns Hopkins University, Copyright 1998.  This software is furnished
//# pursuant to a written license agreement and may be used, copied,
//# transmitted, and stored only in accordance with the terms of such
//# license and with the inclusion of the above copyright notice.  This
//# software and information or any other copies thereof may not be
//# provided or otherwise made available to any other person.
//#
//#
//#     Modification History:
//#
*/
#define HTM_LIB
#include "SpatialConvex.h"
#include "stdlib.h"

#define N(n)	c->index_->nodes_.vector_[(n)]		 /* the node[n] */
#define NC(n,m)	c->index_->nodes_.vector_[(n)].childID_[(m)]/* children n->m */
#define NV(m)   c->index_->nodes_.vector_[nodeIndex].v_[(m)]/* vertices of n */
#define V(m)    c->index_->vertices_.vector_[(m)]         /* vertex vector m */
#define CONSTR(i) c->constraints_.vector_[(i)]   /* Constraint i */
#define CORNER(i) c->corners_.vector_[(i)]   /* Constraint i */

#define SGN(x) ( (x)<0? -1: ( (x)>0? 1:0 ) )		 /* signum */
#define IOFFSET 9
#define COMMENT '#'
/*
// ===========================================================================
//
// Member functions for class SpatialConvex
//
// ===========================================================================
*/
/* //////////////CONSTRUCTOR//////////////////////////////// */
SpatialConvex * spatialConvexNew()
{
  SpatialConvex *c = (SpatialConvex *)malloc(sizeof(SpatialConvex));
  spatialConvexInit(c);
  return c;
}

/* //////////////DESTRUCTOR///////////////////////////////// */
void spatialConvexDelete( SpatialConvex *c )
{
  free(c->constraints_.vector_);
  free(c->corners_.vector_);
}

/* /////////////INIT/////////////////////////////////////////
 * Initialize the convex */
void spatialConvexInit( SpatialConvex *c ) {
  c->index_     	= NULL;
  c->markup_		= NULL;
  c->constraints_.vector_ = NULL;
  c->constraints_.length_ = 0;
  c->constraints_.capacity_ = 0;
  c->corners_.vector_   = NULL;
  c->corners_.length_   = 0;
  c->corners_.capacity_ = 0;
  c->addlevel_          = 0;
  c->full_		= NULL;
  c->partial_		= NULL;
  c->flist_		= NULL;
  c->plist_		= NULL;
  c->sign_              = zERO;
  spatialConstraintInit(&(c->boundingCircle_));
}

/* /////////////COPY CONSTRUCTOR/////////////////////////////
*/
SpatialConvex * spatialConvexCopy( const SpatialConvex *cc )
{
  SpatialConvex *c = spatialConvexNew();

  if(cc==NULL)return NULL;

  c->index_     	= cc->index_;
  c->addlevel_   	= cc->addlevel_;
  c->sign_ 		= cc->sign_;
  c->bitresult_ 	= cc->bitresult_;
  c->markup_    	= spatialMarkupCopy(cc->markup_);

  /* copy bounding circle constraint */
  spatialConstraintAssign(&(c->boundingCircle_),&(cc->boundingCircle_));

  /* copy the constraints */
  c->constraints_.vector_ = (SpatialConstraint *)malloc(
		     sizeof(SpatialConstraint) * cc->constraints_.length_);
  memcpy(c->constraints_.vector_, cc->constraints_.vector_, 
	 cc->constraints_.length_);
  c->constraints_.length_ = cc->constraints_.length_;
  c->constraints_.capacity_ = cc->constraints_.capacity_;

  /* copy the corners */
  c->corners_.vector_ = (SpatialVector *)malloc(
		     sizeof(SpatialVector) * cc->corners_.length_);
  memcpy(c->corners_.vector_, cc->corners_.vector_, cc->corners_.length_);
  c->corners_.length_ = cc->corners_.length_;
  c->corners_.capacity_ = cc->corners_.capacity_;

  /* copy the bitlists : shallow copy */
  c->full_       	= cc->full_;
  c->partial_  		= cc->partial_;

  /* copy the lists : shallow copy */
  c->flist_      	= cc->flist_;
  c->plist_    		= cc->plist_;

  return c;
}

/* /////////////ASSIGNMENT///////////////////////////////////
 */
void spatialConvexAssign( SpatialConvex *c, const SpatialConvex *cc )
{
  if(c == cc)return;
  spatialConvexDelete ( c );
  spatialConvexInit( c );

  c->index_     	= cc->index_;
  c->addlevel_   	= cc->addlevel_;
  c->sign_ 		= cc->sign_;
  c->bitresult_ 	= cc->bitresult_;
  c->markup_    	= spatialMarkupCopy(cc->markup_);

  /* copy bounding circle constraint */
  spatialConstraintAssign(&(c->boundingCircle_),&(cc->boundingCircle_));

  /* copy the constraints */
  if( cc->constraints_.length_ ) {
    c->constraints_.vector_ = (SpatialConstraint *)malloc(
		     sizeof(SpatialConstraint) * cc->constraints_.length_);
    memcpy(c->constraints_.vector_, cc->constraints_.vector_, 
	 sizeof(SpatialConstraint) * cc->constraints_.length_);
    c->constraints_.length_ = cc->constraints_.length_;
    c->constraints_.capacity_ = cc->constraints_.capacity_;
  }

  /* copy the corners */
  if( cc->corners_.length_ ) {
    c->corners_.vector_ = (SpatialVector *)malloc(
		     sizeof(SpatialVector) * cc->corners_.length_);
    memcpy(c->corners_.vector_, cc->corners_.vector_, 
	 sizeof(SpatialVector) * cc->corners_.length_);
    c->corners_.length_ = cc->corners_.length_;
    c->corners_.capacity_ = cc->corners_.capacity_;
  }

  /* copy the bitlists : shallow copy */
  c->full_       	= cc->full_;
  c->partial_  		= cc->partial_;

  /* copy the lists : shallow copy */
  c->flist_      	= cc->flist_;
  c->plist_    		= cc->plist_;

}

/* /////////////CONSTRUCTOR FROM A TRIANGLE//////////////////
//
// Initialize domain from a triangle. The corners of these vectors
// form a triangle, so we just add three ZERO convexes to the domain
// where the direction is given by the cross product of the corners.
// Of course, the sign has to be determined (on which side of the triangle
// are we?) If the three points lie on one line, no convexes are added.
*/
SpatialConvex * spatialConvexNewTriangle( const SpatialVector * v1,
					  const SpatialVector * v2,
					  const SpatialVector * v3 )
{
  SpatialConvex *c = spatialConvexNew();
  SpatialVector a1, a2, a3;
  float64 s1, s2, s3, neg = -1.0L;

  /* set directions of half-spheres */
  spatialVectorCross( v2, v3, &a1 );
  spatialVectorCross( v3, v1, &a2 );
  spatialVectorCross( v1, v2, &a3 );
 
  /* we really need only the signs of these */
  s1 = spatialVectorDot( &a1, v1 );
  s2 = spatialVectorDot( &a2, v2 );
  s3 = spatialVectorDot( &a3, v3 );

  if(s1 * s2 * s3) {                /* this is nonzero if not on one line */
    /* change sign if necessary */
    if(s1 < 0.0L) spatialVectorMul( &a1, neg );
    if(s2 < 0.0L) spatialVectorMul( &a2, neg );
    if(s3 < 0.0L) spatialVectorMul( &a3, neg );
    spatialConvexAdd( c, &a1, 0.0 );
    spatialConvexAdd( c, &a2, 0.0 );
    spatialConvexAdd( c, &a3, 0.0 );
  }
  return c;
}

/* /////////////CONSTRUCTOR FROM A RECTANGLE/////////////////
//
// Initialize convex from a rectangle. The vectors that form a rectangle
// may be in any order, the code finds the edges by itself.
// If one of the vectors lies within the triangle formed by the
// other three vectors, the previous constructor is used.
//
*/
SpatialConvex * spatialConvexNewRectangle( const SpatialVector * v1,
					   const SpatialVector * v2,
					   const SpatialVector * v3,
					   const SpatialVector * v4 )
{
  SpatialConvex *c = spatialConvexNew();
  int i,j,k,l,m;  /* indices */
  /* to simplify things, copy input into a 4-array */
  const SpatialVector *v[4];
  SpatialVector d[6];
  float64 s[6][2], neg = -1.0;

  v[0] = v1; v[1] = v2; v[2] = v3; v[3] = v4;
  for (i = 0, k = 0; i < 4 ; i++)
    for (j = i+1; j < 4; j++, k++) {    /* set directions of half-spheres */
      spatialVectorCrossNorm( v[i], v[j], &d[k] );/* two are diagonals. */
      for (l = 0, m = 0; l < 4; l++) /* set the 'sign' */
	if(l != i && l != j)s[k][m++] = spatialVectorDot( &d[k], v[l] );
    }

  /* the sides are characterized by having both other corners
     to the same (inner) side. so it is easy to find the edges.
     again, the sign has to be taken care of -> direction of d
     the nice thing here is that if one of the corners is inside
     a triangles formed by the other three, only 3 constraints get
     added. */
  for(i = 0; i < 6; i++)
    if(s[i][0] * s[i][1] > 0.0) { /* not >=  we don't want aligned corners */
      if(s[i][0] < 0.0) spatialVectorMul( &d[i], neg );
      spatialConvexAdd( c, &d[i], 0.0 );
    }

  /* Special cases: 1
     if three of the corners are aligned, we end up with
     only two constraints. Find the third and append it.
     Indeed, there are 3 identical constraints among the d[],
     so the first that qualifies gets appended. */
  if(c->constraints_.length_ == 2) {
    for(i = 0; i < 6; i++)
      if(s[i][0] == 0.0 || s[i][1] == 0.0) {
	if( (s[i][0]+s[i][1]) < 0.0 ) spatialVectorMul( &d[i], neg );
	spatialConvexAdd( c, &d[i], 0.0);
	break;
      }
  }
  /* Special cases: 2
     if all four corners are aligned, no constraints have been appended. */
  return c;
}


/* /////////////ADD//////////////////////////////////////////
 */
void spatialConvexAdd( SpatialConvex *c, SpatialVector *v, float64 d )
{
  SpatialConstraint tmp0, tmp;
  size_t j,i;
  spatialConstraintSet( &tmp, v, d );
  i = spatialConstraintVecAppend( &(c->constraints_), &tmp );

  /* order constraints: by ascending opening angle. Since we append
     always at the end, we only need one ordering sweep starting at
     the end */
  for ( j = c->constraints_.length_ - 1; j > 0; j-- ) {
    if ( CONSTR(j).s_ <  CONSTR(j-1).s_ ) {
      spatialConstraintAssign( &tmp0, &(CONSTR(j)) );
      spatialConstraintAssign( &(CONSTR(j)), &(CONSTR(j-1)));
      spatialConstraintAssign( &(CONSTR(j-1)), &tmp0 );
    }
  }

  if(c->constraints_.length_ == 1) {  /* first constraint */
    c->sign_ = tmp.sign_;
    return;
  }

  switch (c->sign_) {
  case nEG:
    if(tmp.sign_ == pOS) c->sign_ = mIXED;
    break;
  case pOS:
    if(tmp.sign_ == nEG) c->sign_ = mIXED;
    break;
  case zERO:
    c->sign_ = tmp.sign_;
    break;
  }
}


/* /////////////SIMPLIFY0////////////////////////////////////
// simplify0: 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.
*/

void spatialConvexSimplify0( SpatialConvex *c )
{

  size_t i,j,k;
  size_t c1,c2,k1,k2;
  size_t ic,currentCorner;
  float64 neg = -1.0L, d;
  SpatialVector vi1, vi2, tmp1, tmp2;
  VarVecInt cornerConstr1, cornerConstr2, removeConstr;
  SpatialVectorVec corner;
  bool ruledout, vi1ok, vi2ok;

  spatialVectorVecInit( &corner );
  varVecIntInit( &cornerConstr1 );
  varVecIntInit( &cornerConstr2 );
  varVecIntInit( &removeConstr );
  /* for one constraint, it is itself the BC */
  if (c->constraints_.length_ == 1) {
    spatialConstraintAssign( &(c->boundingCircle_), &(CONSTR(0)));
    return;

  /* 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. */
  } else if(c->constraints_.length_ == 2) {
    /* test for constraints being identical - rule 1 out */
    if(spatialVectorEQ( &(CONSTR(0).a_), &(CONSTR(1).a_)) ) {
      c->constraints_.length_--;
      spatialConstraintAssign( &(c->boundingCircle_), &(CONSTR(0)));
      return;
    }
    /* test for constraints being two disjoint half spheres - empty convex! */
    if(CONSTR(0).a_.x_ == (-1.0)*CONSTR(1).a_.x_ &&
       CONSTR(0).a_.y_ == (-1.0)*CONSTR(1).a_.y_ &&
       CONSTR(0).a_.z_ == (-1.0)*CONSTR(1).a_.z_ ){
      spatialConstraintVecClear( &(c->constraints_), false );
      return;
    }
    spatialVectorAddNorm( &(CONSTR(0).a_), &(CONSTR(1).a_), &tmp1 );

    spatialConstraintSet( &c->boundingCircle_, &tmp1, 0.0);
    return;
  }

  /* Go over all pairs of constraints */
  for(i = 0; i < c->constraints_.length_ - 1; i++) {
    ruledout = true;
    for(j = i+1; j < c->constraints_.length_; j ++) {
      /* test for constraints being identical - rule i out */
      if(spatialVectorEQ( &(CONSTR(i).a_), &(CONSTR(j).a_)) )break;
      /* test for constraints being two disjoint half spheres -empty convex! */
      if(CONSTR(i).a_.x_ == (-1.0)*CONSTR(j).a_.x_ &&
	 CONSTR(i).a_.y_ == (-1.0)*CONSTR(j).a_.y_ &&
	 CONSTR(i).a_.z_ == (-1.0)*CONSTR(j).a_.z_ ) {
	spatialConstraintVecClear( &(c->constraints_), false );
	return;
      }
      /* vi1 and vi2 are their intersection points */
      spatialVectorCrossNorm( &(CONSTR(i).a_), &(CONSTR(j).a_), &vi1) ;
      spatialVectorAssign( &vi2, &vi1); 
      spatialVectorMul( &vi2, neg );
      vi1ok = true; vi2ok = true;
      /* now test whether vi1 or vi2 or both are inside every other constraint.
	 if yes, store them in the corner array. */
      for(k = 0; k < c->constraints_.length_; k++) {
	if(k == i || k == j) continue;
	if(vi1ok && 
	   spatialVectorDot( &vi1, &(CONSTR(k).a_) ) <= 0.0)
	  vi1ok = false;
	if(vi2ok && 
	   spatialVectorDot( &vi2, &(CONSTR(k).a_) ) <= 0.0)
	  vi2ok = false;
	if(!vi1ok && !vi2ok)break;
      }
      if(vi1ok) { 
	spatialVectorVecAppend( &corner, &vi1);
	varVecIntAppend( &cornerConstr1, i );
	varVecIntAppend( &cornerConstr2, j );
	ruledout = false; 
      }
      if(vi2ok) { 
	spatialVectorVecAppend( &corner, &vi2);
	varVecIntAppend( &cornerConstr1, i );
	varVecIntAppend( &cornerConstr2, j );
	ruledout = false; 
      }
    }
    /* is this constraint ruled out? i.e. none of its intersections
       with other constraints are corners... remove it from constraints_ list.
    */
    if(ruledout) varVecIntAppend( &removeConstr, i );
  }

  /* 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. */

  spatialVectorVecClear( &(c->corners_), false );
  spatialVectorVecAppend( &(c->corners_), &(corner.vector_[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 = cornerConstr1.vector_[0];	/* the i'th constraint and j'th constraint */
  j = cornerConstr2.vector_[0];	/* intersect at 0'th corner */
  /* 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. */
  for( k = 1; k < cornerConstr1.length_; k ++) {
    if(cornerConstr1.vector_[k] == i) {
      spatialVectorAssign( &vi1, &(corner.vector_[k]) );
      c1 = cornerConstr2.vector_[k];
      k1 = k;
    }
    if(cornerConstr2.vector_[k] == i) {
      spatialVectorAssign( &vi1, &(corner.vector_[k]) );
      c1 = cornerConstr1.vector_[k];
      k1 = k;
    }
    if(cornerConstr1.vector_[k] == j) {
      spatialVectorAssign( &vi2, &(corner.vector_[k]) );
      c2 = cornerConstr2.vector_[k];
      k2 = k;
    }
    if(cornerConstr2.vector_[k] == j) {
      spatialVectorAssign( &vi2, &(corner.vector_[k]) );
      c2 = cornerConstr1.vector_[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...
  */
  spatialVectorSub( &vi1, &(corner.vector_[0]), &tmp1 );
  spatialVectorCross( &tmp1, &(CONSTR(i).a_), &tmp2);
  if( spatialVectorDot( &tmp2, &(corner.vector_[0]) ) > 0 ) {
    spatialVectorVecAppend( &(c->corners_), &vi1 );
    ic = c1;
    currentCorner = k1;
  } else {
    spatialVectorVecAppend( &(c->corners_), &vi2 );
    ic = 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 ) {
    for (k = 0; k < cornerConstr1.length_; k++) {
      if(k == currentCorner)continue;
      if(cornerConstr1.vector_[k] == ic) {
	if( (currentCorner = k) == 0) break;
	spatialVectorVecAppend( &(c->corners_), &(corner.vector_[k]) );
	ic = cornerConstr2.vector_[k];
	break;
      }
      if(cornerConstr2.vector_[k] == ic) {
	if( (currentCorner = k) == 0) break;
	spatialVectorVecAppend( &(c->corners_), &(corner.vector_[k]) );
	ic = cornerConstr1.vector_[k];
	break;
      }
    }
  }
  /* Remove all redundant constraints */
  qsort( &(removeConstr.vector_), removeConstr.length_, sizeof(int32),
	 varVecIntOrderDescending );
  for ( i = 0; i < removeConstr.length_; i++)
    spatialConstraintVecRemove( &(c->constraints_), removeConstr.vector_[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. */
  c->boundingCircle_.d_ = 1.0;
  if (c->constraints_.length_ >=3 ) {
    for(i = 0; i < c->corners_.length_; i++)
      for(j = i+1; j < c->corners_.length_; j++)
	for(k = j+1; k < c->corners_.length_; k++) {
	  spatialVectorSub( &(c->corners_.vector_[j]), 
			    &(c->corners_.vector_[i]), &tmp1 );
	  spatialVectorSub( &(c->corners_.vector_[k]), 
			    &(c->corners_.vector_[j]), &tmp2 );
	  spatialVectorCrossNorm( &tmp1, &tmp2, &vi1 );
	  /* 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 */
	  d = spatialVectorDot( &vi1, &(c->corners_.vector_[i]) );
	  if(c->boundingCircle_.d_ > d) 
	    spatialConstraintSet( &(c->boundingCircle_), &vi1, d);
	}
  }

  /* DEBUG CODE
  for(i = 0; i < corners_.length(); i++) {
    cout << corners_(i).ra() << "," << corners_(i).dec() << ":" << corners_(i) << endl;

  }
  */
    
}
/*
/////////////SIMPLIFY/////////////////////////////////////
// simplify: We have the following decision tree for the 
//           simplification of convexes:
//
//  Always test two constraints against each other. We have
//
//  * If both constraints are pOS
//
//     # If they intersect: keep both
//
//     # If one lies in the other: drop the larger one
//
//     # Else: disjunct. Empty convex, stop.
//
//  * If both constraints are nEG
//
//     # If they intersect or are disjunct: ok
//
//     # Else: one lies in the other, drop smaller 'hole'
//
//  * Mixed: one pOS, one nEG
//
//     # No intersection, disjunct: pOS is redundant
//
//     # Intersection: keep both
//
//     # pOS within nEG: empty convex, stop.
//
//     # nEG within pOS: keep both.
//
*/

void spatialConvexSimplify( SpatialConvex *c )
{
  size_t i,j;
  size_t clen;
  bool redundancy = true;

  if(c->sign_ == zERO) {
    spatialConvexSimplify0(c);	/* treat zERO convexes separately */
    return;
  }

  while(redundancy) {
    redundancy = false;
    clen = c->constraints_.length_;

  for(i = 0; i < clen; i++) {
    for(j = 0; j < i; j++) {
      int test;

      /* don't bother with two zero constraints */
      if( CONSTR(i).sign_ == zERO && CONSTR(j).sign_ == zERO)
	continue;

      /* both pos or zero */
      if( ( CONSTR(i).sign_ == pOS || CONSTR(i).sign_ == zERO ) &&
	  ( CONSTR(j).sign_ == pOS || CONSTR(j).sign_ == zERO ) ) {
	/* intersection */
	if ( (test = spatialConvexTestConstraints(c,i,j)) == 0 ) continue;
	if ( test < 0 ) { /* disjoint ! convex is empty */
	  spatialConstraintVecClear(&(c->constraints_), false);
	  return;
	}
	/* one is redundant */
	if(test == 1) spatialConstraintVecRemove( &(c->constraints_), i );
	else if(test==2)spatialConstraintVecRemove( &(c->constraints_), j );
	else continue;     /* intersection */
	redundancy = true; /* we did cut out a constraint; do the loop again */
	break;
      }

      /* both neg or zero */
      if( ( CONSTR(i).sign_ == nEG ) && ( CONSTR(j).sign_ == nEG ) ) {
	if ( (test = spatialConvexTestConstraints(c,i,j)) <= 0 ) 
	  continue; /* ok */
	/* one is redundant */
	if(test == 1) spatialConstraintVecRemove( &(c->constraints_), j );
	else if(test==2) spatialConstraintVecRemove( &(c->constraints_), i );
	else continue; /* intersection */
	redundancy = true; /* we did cut out a constraint; do the loop again */
	break;
      }

      /* one neg, one pos/zero */
      if( (test = spatialConvexTestConstraints(c,i,j)) == 0) 
	continue; /* ok: intersect */
      if( test < 0 ) { /* neg is redundant */
	if ( CONSTR(i).sign_ == nEG )
	  spatialConstraintVecRemove( &(c->constraints_), i );
	else spatialConstraintVecRemove( &(c->constraints_), j );
	redundancy = true; /* we did cut out a constraint; do the loop again */
	break;
      }
      /* if the negative constraint is inside the positive: continue */
      if ( (CONSTR(i).sign_ == nEG && test == 2) || 
	   (CONSTR(j).sign_ == nEG && test == 1) )continue;
      /* positive constraint in negative: convex is empty! */
      spatialConstraintVecClear( &(c->constraints_), false);
      return;
    }
    if(redundancy)break;
  }

  }

  /* reset the sign of the convex */
  c->sign_ = CONSTR(0).sign_;
  for(i = 1; i < c->constraints_.length_; i++) {
    switch (c->sign_) {
    case nEG:
      if(CONSTR(i).sign_ == pOS) c->sign_ = mIXED;
      break;
    case pOS:
      if(CONSTR(i).sign_ == nEG) c->sign_ = mIXED;
      break;
    case zERO:
      c->sign_ = CONSTR(i).sign_;
      break;
    }
  }

}

/* /////////////TESTCONSTRAINTS//////////////////////////////
// testConstraints: Test for the relative position of two constraints.
//                  Returns 0  if they intersect
//                  Returns -1 if they are disjoint
//                  Returns 1  if j is in i
//                  Returns 2  if i is in j
*/
int spatialConvexTestConstraints( SpatialConvex *c, size_t i, size_t j )
{

  SpatialVector vi;
  SpatialVector vj;
  float64 phi, neg = -1.0L, a1, a2;
  spatialVectorAssign( &vi, &(CONSTR(i).a_));
  spatialVectorAssign( &vj, &(CONSTR(j).a_));
  if ( CONSTR(i).sign_ == nEG ) spatialVectorMul ( &vi, neg );
  if ( CONSTR(j).sign_ == nEG ) spatialVectorMul ( &vj, neg );

  /* correct for math lib -1.0 */
  phi = spatialVectorDot( &vi, &vj );
  phi = ( (phi <= -1.0L + gEpsilon) ? gPi : acos(phi)) ;
  a1 = ( (CONSTR(i).sign_ == pOS) ? 
		CONSTR(i).s_ : gPi-CONSTR(i).s_);
  a2 = ( (CONSTR(j).sign_ == pOS) ? 
		    CONSTR(j).s_ : gPi-CONSTR(j).s_);

  if ( phi > a1 + a2 ) return -1;
  if ( a1 > phi + a2 ) return 1;
  if ( a2 > phi + a1 ) return 2;
  return 0;
}

/* /////////////INTERSECT////////////////////////////////////
 */
void spatialConvexIntersectBit( SpatialConvex *c, 
				const SpatialIndex * idx, 
				SpatialMarkup * markup,
				BitList * partial, BitList * full )
{
  c->index_ = idx;
  c->addlevel_ = idx->maxlevel_ - idx->buildlevel_;
  c->markup_ = markup;
  c->partial_ = partial;
  c->full_ = full;
  c->bitresult_ = true;

  spatialConvexDoIntersect(c);
}

/* /////////////INTERSECT////////////////////////////////////
 */
void spatialConvexIntersect( SpatialConvex *c, 
			     const SpatialIndex * idx, 
			     SpatialMarkup * markup,
			     SpatialIDVec * partial, SpatialIDVec * full )
{
  c->index_ = idx;
  c->addlevel_ = idx->maxlevel_ - idx->buildlevel_;
  c->markup_ = markup;
  c->plist_ = partial;
  c->flist_ = full;
  c->bitresult_ = false;

  spatialConvexDoIntersect(c);
}

/* /////////////DOINTERSECT//////////////////////////////////
 */
void spatialConvexDoIntersect( SpatialConvex *c ) {

  IDTYPE i;

  spatialMarkupClearVertex(c->markup_);	/* initalize to UNSET */
  spatialConvexSimplify(c);		/* don't work too hard... */

  /*  spatialConvexWrite(c,stdout); */
  if(c->constraints_.length_==0)return;   /* nothing to intersect!! */

  /* Start with root nodes (index = 1-8) and intersect triangles */
  for(i = 1; i <= 8; i++)
    spatialConvexTriangleTest(c,i);

}

/* /////////////TRIANGLETEST/////////////////////////////////
// triangleTest: this is the main test of a triangle vs a Convex.  It
// will properly mark up the flags for the triangular node[index], and
// all its children
*/
uint8 spatialConvexTriangleTest( SpatialConvex *c, IDTYPE nodeIndex )
{
  uint8 mark;

  /* do the face test on the triangle */

  /* don't change node markup if we already have it accepted
     (we already have had a convex markup) */
  if (c->markup_->mark_[nodeIndex] == fULL) 
    return c->markup_->mark_[nodeIndex];

  mark =  spatialConvexTestNodeIdx(c,nodeIndex);

  /* do we have a final result code?
     if bREJECT, rEJECT, fULL then return */

  if(mark >= fULL && c->addlevel_ == 0) {
    c->markup_->mark_[nodeIndex] = mark;
    if(mark == fULL)  /* propagate final result to children */
      spatialConvexFillChildren(c,nodeIndex);

  } else {

    /* if sWALLOWED, pARTIAL, or dONTKNOW, then continue, test children,
       but do not reach beyond the leaf nodes.
       If Convex is fully contained within one (sWALLOWED),
       we can stop looking further in another child */

    if (NC(nodeIndex,0)!=0) {
      spatialConvexTriangleTest(c,NC(nodeIndex,0));
      spatialConvexTriangleTest(c,NC(nodeIndex,1));
      spatialConvexTriangleTest(c,NC(nodeIndex,2));
      spatialConvexTriangleTest(c,NC(nodeIndex,3));
   /* we are at the leafnodes
      If we have to recurse further, calculate intersections one by one
      If not, just set the proper bit in partial_ or append id to plist_.
   */
    } else {
      if(c->addlevel_) {
	/* from now on, continue to build the triangles dynamically.
	   until maxlevel_ levels depth. */
	spatialConvexTestPartial(c, c->addlevel_, 
				 N(nodeIndex).id_, 
				 &(V(NV(0))), &(V(NV(1))), &(V(NV(2))));

      } else {
	if(c->bitresult_)
	  bitListSet( c->partial_,
		      (uint32)spatialIndexLeafNumberById( c->index_, 
							  N(nodeIndex).id_),
		      true );
	else
	  spatialIDVecAppend(c->plist_, N(nodeIndex).id_);
      }
    }
  }
  return mark;
}

/* /////////////FILLCHILDREN/////////////////////////////////
// fillChildren: mark children as full
*/
void spatialConvexFillChildren( SpatialConvex *c, IDTYPE nodeIndex )
{
  size_t i;
  if(NC(nodeIndex,0)!=0) {
    for(i = 0; i < 4; i++) {
      c->markup_->mark_[NC(nodeIndex,i)] = fULL;
      spatialConvexFillChildren(c,NC(nodeIndex,i));
    }
  } else {
    /* we are at the leaf. If we still have levels to recurse,
       fill them. If not, just set the full_ bitlist's or flist_ list's
       value correctly. */
    if(c->addlevel_)
      spatialConvexSetfull(c,N(nodeIndex).id_,c->addlevel_);
    else {
      if(c->bitresult_)
	bitListSet( c->full_,
		    (uint32)spatialIndexLeafNumberById( c->index_,
							N(nodeIndex).id_ ), 
		   true );
      else
	spatialIDVecAppend( c->flist_, N(nodeIndex).id_ );
    }
  }
}

/* /////////////SETFULL//////////////////////////////////////
// setfull: set the bitlist leaves at level maxlevel to full.
// if we have still levels to go, recurse. Use the id to get
// the leaf node's index. See idbyname and namebyid for explanations.
*/
void spatialConvexSetfull( SpatialConvex *c, IDTYPE id, size_t level )
{
  if(level--) {
    spatialConvexSetfull( c, id << 2      , level );
    spatialConvexSetfull( c, (id << 2) + 1, level );
    spatialConvexSetfull( c, (id << 2) + 2, level );
    spatialConvexSetfull( c, (id << 2) + 3, level );
  } else {
    if(c->bitresult_)
      bitListSet( c->full_,
		  (uint32)spatialIndexLeafNumberById( c->index_, id ), 
		  true );
    else
      spatialIDVecAppend( c->flist_, id );
  }
}

/* /////////////TESTPARTIAL//////////////////////////////////
// testPartial: test a triangle's subtriangle whether they are partial.
// if level is nonzero, recurse.
*/
void spatialConvexTestPartial( SpatialConvex *c,
			       size_t level, IDTYPE id,
			       const SpatialVector * v0, 
			       const SpatialVector * v1, 
			       const SpatialVector * v2 )
{
  SpatialVector w0, w1 ,w2;
  /* if there is still a level to go, subdivide the
     triangle according to our rules and test each subdivision.
     (our rules are: each subdivided triangle has to be given
     ordered counter-clockwise, 0th index starts of new 0-node,
     1st index starts off new 1-node, 2nd index starts off new 2-node
     middle triangle gives new 3-node.
     if we are at the bottom, set this id to partial. */
  if(level--) {
    spatialVectorAddNorm( v1, v2, &w0 );
    spatialVectorAddNorm( v0, v2, &w1 );
    spatialVectorAddNorm( v1, v0, &w2 );

    spatialConvexTestSubTriangle( c, level, (id << 2)    , v0, &w2, &w1 );
    spatialConvexTestSubTriangle( c, level, (id << 2) + 1, v1, &w0, &w2 );
    spatialConvexTestSubTriangle( c, level, (id << 2) + 2, v2, &w1, &w0 );
    spatialConvexTestSubTriangle( c, level, (id << 2) + 3, &w0, &w1, &w2 );
  } else {
    if(c->bitresult_)
      bitListSet( c->partial_,
		  (uint32)spatialIndexLeafNumberById( c->index_, id ), 
		  true );
    else
      spatialIDVecAppend( c->plist_, id );
  }
}

/* /////////////TESTSUBTRIANGLE////////////////////////////////
// testSubTriangle: call full or partial depending on result of testNode.
*/
void spatialConvexTestSubTriangle( SpatialConvex *c,
				   size_t level, IDTYPE id,
				   const SpatialVector * v0, 
				   const SpatialVector * v1, 
				   const SpatialVector * v2 )
{
  /* test this triangle. */
  uint8 mark = spatialConvexTestNode( c, v0, v1, v2 );

  /* if it is full, set all fulls below this level, too
     else if it is partial or unknown or swallowed call testpartial
     with this new level. */
  if(mark == fULL)
    spatialConvexSetfull( c, id , level );
  else if(mark < fULL)
    spatialConvexTestPartial( c, level, id, v0, v1, v2 );
}

/* /////////////TESTNODE/////////////////////////////////////
// testNode: tests the QuadNodes for intersections.
*/
uint8 spatialConvexTestNode( SpatialConvex *c, 
			     const SpatialVector * v0, 
			     const SpatialVector * v1, 
			     const SpatialVector * v2 )
{
  int vsum;
  uint8 mark;
  char name[10];
  SpatialVector v,w;

  /* Start with testing the vertices for the QuadNode with this convex. */

  vsum = spatialConvexTestVertex( c, v0 ) + 
         spatialConvexTestVertex( c, v1 ) + 
         spatialConvexTestVertex( c, v2 );

  /* DEBUG OUTPUT
  spatialVectorAdd( v0, v1, &v );
  spatialVectorAdd( &v, v2, &w );
  spatialIndexNameById( spatialIndexIdByPoint(c->index_,&w), name );
  printf("%s %d\n",name, vsum);
  */

  mark = spatialConvexTestTriangle( c, v0, v1, v2, vsum );

  /* DEBUG OUTPUT 
     printf(" %s%s\n", ( mark == pARTIAL ? "partial " : 
	    ( mark ==  fULL ? "full " :
	      ( mark == rEJECT ? "reject " :
		( mark == bREJECT ? "breject " :
		  ( mark == sWALLOWED ? "swallowed " :
		    "dontknow " ) ) ) ) ), name );

     spatialVectorWrite( &V(NV(0)), stdout ); printf(",");
     spatialVectorWrite( &V(NV(1)), stdout ); printf(",");
     spatialVectorWrite( &V(NV(2)), stdout ); printf("\n");
     printf(" ( %f, %f ),  ( %f, %f ),  ( %f, %f ) \n",
             V(NV(0)).ra_, V(NV(0)).dec_,
	     V(NV(1)).ra_, V(NV(1)).dec_,
	     V(NV(2)).ra_, V(NV(2)).dec_);
  */

  /* since we cannot play games using the on-the-fly triangles,
     substitute dontknow with partial. */
  if (mark == dONTKNOW) 
    mark = pARTIAL;

  return mark;
}

/* /////////////TESTNODE/////////////////////////////////////
// testNode: tests the QuadNodes for intersections.
/*/
uint8 spatialConvexTestNodeIdx( SpatialConvex *c, IDTYPE nodeIndex )
{
  int vsum;
  uint8 mark;
  char name[10];
  /* Start with testing the vertices for the QuadNode with this convex. */

  vsum = spatialConvexTestVertexIdx( c, NV(0) ) 
       + spatialConvexTestVertexIdx( c, NV(1) ) 
       + spatialConvexTestVertexIdx( c, NV(2) ) ;

  /* DEBUG OUTPUT
  spatialIndexNameById( N(nodeIndex).id_, name );
  printf("%s %d\n",name, vsum);
  */

  mark = spatialConvexTestTriangle( c, 
				    &(V(NV(0))), 
				    &(V(NV(1))), 
				    &(V(NV(2))), vsum);

  /* DEBUG OUTPUT 
     printf(" %s%d,%d,%d\n", ( mark == pARTIAL ? "partial " : 
	    ( mark ==  fULL ? "full " :
	      ( mark == rEJECT ? "reject " :
		( mark == bREJECT ? "breject " :
		  ( mark == sWALLOWED ? "swallowed " :
		    "dontknow " ) ) ) ) ), NV(0), NV(1), NV(2) );

     spatialVectorWrite( &V(NV(0)), stdout ); printf(",");
     spatialVectorWrite( &V(NV(1)), stdout ); printf(",");
     spatialVectorWrite( &V(NV(2)), stdout ); printf("\n");
     printf(" ( %f, %f ),  ( %f, %f ),  ( %f, %f ) \n",
             V(NV(0)).ra_, V(NV(0)).dec_,
	     V(NV(1)).ra_, V(NV(1)).dec_,
	     V(NV(2)).ra_, V(NV(2)).dec_);
  */

  /* If we are down at the leaf nodes here, it is dONTKNOW,
     really.. but since these are the leaf nodes here and we want to
     be on the safe side, mark them as partial. */
  if (NC(nodeIndex,0) == 0 && mark == dONTKNOW) 
    mark = pARTIAL;

  return mark;
}

/* /////////////TESTTRIANGLE//////////////////////////////////
// testTriangle: tests a triangle given by 3 vertices if
// it intersects the convex.
*/
uint8 spatialConvexTestTriangle( SpatialConvex *c,
				 const SpatialVector * v0, 
				 const SpatialVector * v1, 
				 const SpatialVector * v2,
				 int vsum )
{
  size_t cIndex;

  if(vsum == 1 || vsum == 2) return pARTIAL;

  /* If vsum = 3 then we have all vertices inside the convex.
     Now use the following decision tree:
  
     * If the sign of the convex is pOS or zERO : mark as fULL intersection.
  
     * Else, test for holes inside the triangle. A 'hole' is a nEG constraint
       that has its center inside the triangle. If there is such a hole,
       return pARTIAL intersection.
  
     * Else (no holes, sign nEG or mIXED) test for intersection of nEG
       constraints with the edges of the triangle. If there are such,
       return pARTIAL intersection.
  
     * Else return fULL intersection. */

  if(vsum == 3) {
    if(c->sign_ == pOS || c->sign_ == zERO) return fULL;
    if ( spatialConvexTestHole( c, v0, v1, v2 ) ) return pARTIAL;
    if ( spatialConvexTestEdge( c, v0, v1, v2 ) ) return pARTIAL;
    return fULL;
  }

  /* If we have reached that far, we have vsum=0. There is no definite
     decision making possible here with our methods, the markup may result
     in dONTKNOW. The decision tree is the following:
  
     * Test with bounding circle of the triangle.
  
       # If the sign of the convex zERO test with the precalculated
         bounding circle of the convex. If it does not intersect with the
	 triangle's bounding circle, bREJECT.
  
       # If the sign of the convex is nonZERO: if the bounding circle
         lies outside of one of the constraints, bREJECT.
  
     * Else: there was an intersection with the bounding circle.
  
       # For zERO convexes, test whether the convex intersects the edges.
         If none of the edges of the convex intersects with the edges of
         the triangle, we have a rEJECT. Else, pARTIAL.
  
       # If sign of convex is pOS, or miXED and the smallest constraint does
         not intersect the edges and has its center inside the triangle,
	 return sWALLOW. If no intersection of edges and center outside
	 triangle, return rEJECT.
  
       # So the smallest constraint DOES intersect with the edges. If
         there is another pOS constraint which does not intersect with
	 the edges, and has its center outside the triangle, return
	 rEJECT. If its center is inside the triangle return sWALLOW. 
	 Else, return pARTIAL for pOS and dONTKNOW for mIXED signs. 
  
     * If we are here, return dONTKNOW. There is an intersection with 
       the bounding circle, none of the vertices is inside the convex and
       we have very strange possibilities left for pOS and mIXED signs. For
       nEG, i.e. all constraints negative, we also have some complicated
       things left for which we cannot test further.
  */

  if ( !spatialConvexTestBoundingCircle( c, v0, v1, v2 ) ) return bREJECT;

  if ( c->sign_ == pOS || c->sign_ == mIXED || 
       (c->sign_ == zERO && c->constraints_.length_ <= 2)) {
    /* Does the smallest constraint intersect with the edges? */
    if ( spatialConvexTestEdgeConstraint( c, v0, v1, v2, 0 ) ) {
      /* Is there another positive constraint that does NOT intersect with
	 the edges? */
      if ( cIndex = spatialConvexTestOtherPosNone(c,v0,v1,v2) ) {
	/* Does that constraint lie inside or outside of the triangle? */
	if ( spatialConvexTestConstraintInside(c,v0,v1,v2, cIndex) )
	  return pARTIAL;
	/* Does the triangle lie completely within that constr? */
	else if( spatialConstraintContains( &(CONSTR(cIndex)), v0 ) )
	  return pARTIAL;
	else return rEJECT;

      } else {
	if(c->sign_ == pOS || c->sign_ == zERO) return pARTIAL;
	else return dONTKNOW;
      }	
    } else {
      if (c->sign_ == pOS || c->sign_ == zERO) {
	/* Does the smallest lie inside or outside the triangle? */
	if( spatialConvexTestConstraintInside( c, v0, v1, v2, 0 ) ) 
	  return pARTIAL;
	else return rEJECT;
      } else return dONTKNOW;
    }
  } else if (c->sign_ == zERO) {
    if ( c->corners_.length_ > 0 && spatialConvexTestEdge0( c, v0, v1, v2 ) ) 
      return pARTIAL;
    else return rEJECT;
  }
  return pARTIAL;
}

/* /////////////TESTVERTEX/////////////////////////////////////
// testVertex: test for vertex being inside or outside convex.
*/
int spatialConvexTestVertexIdx( SpatialConvex *c, size_t vIndex )
{
  int i;

  if ( c->markup_->vmark_[vIndex] == vTRUE ) return 1;
  if ( c->markup_->vmark_[vIndex] == vFALSE ) return 0;

  i = spatialConvexTestVertex( c, &(V(vIndex)) );

  if (i) c->markup_->vmark_[vIndex] = vTRUE;
  else c->markup_->vmark_[vIndex] = vFALSE;
  return i;
}

/* /////////////TESTVERTEX/////////////////////////////////////
// testVertex: same as above, but for any spatialvector, no markup speedup
*/
int spatialConvexTestVertex( SpatialConvex *c, const SpatialVector * v )
{
  size_t i;
  for ( i = 0; i < c->constraints_.length_; i++) 
    if ( spatialVectorDot( &(CONSTR(i).a_), v )  < CONSTR(i).d_ )
      return 0;

  return 1;
}

/* ////////////TESTHOLE/////////////////////////////////////
// testHole: test for holes. If there is a negative constraint whose center
//           is inside the triangle, we speak of a hole. Returns true if
//	     found one.
*/
bool spatialConvexTestHole( SpatialConvex *c, 
			    const SpatialVector * v0, 
			    const SpatialVector * v1, 
			    const SpatialVector * v2 )
{
  size_t i;
  SpatialVector w;
  bool test = false;

  for(i = 0; i < c->constraints_.length_; i++) {

    if ( CONSTR(i).sign_ == nEG ) {  /* 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 */

      spatialVectorCross( v0, v1, &w );
      if ( spatialVectorDot( &w, &(CONSTR(i).a_)) > 0.0L ) continue;
      spatialVectorCross( v1, v2, &w );
      if ( spatialVectorDot( &w, &(CONSTR(i).a_)) > 0.0L ) continue;
      spatialVectorCross( v2, v0, &w );
      if ( spatialVectorDot( &w, &(CONSTR(i).a_)) > 0.0L ) continue;
      test = true;
      break;
    }
  }
  return test;
}

/* /////////////TESTEDGE0////////////////////////////////////
// testEdge0: 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.
*/
bool spatialConvexTestEdge0( SpatialConvex *c,
			     const SpatialVector * v0, 
			     const SpatialVector * v1, 
			     const SpatialVector * 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) */

  size_t i,j,k,iedge;
  float64 l1,l2;    /* lengths of the arcs from intersection to edge corners */
  float64 cedgelen, neg = -1.0;
  SpatialVector tmp, a1;

  struct edgeStruct {
    SpatialVector e;		/* The half-sphere this edge delimits */
    float64	  l;		/* length of edge */
    const SpatialVector *e1;	/* first end */
    const SpatialVector *e2;	/* second end */
  } edge[3];

  /* fill the edge structure for each side of this triangle */
  spatialVectorCross( v0, v1, &(edge[0].e) );
  spatialVectorCross( v1, v2, &(edge[1].e) );
  spatialVectorCross( v2, v0, &(edge[2].e) );
  edge[0].e1 = v0; edge[0].e2 = v1;
  edge[1].e1 = v1; edge[1].e2 = v2;
  edge[2].e1 = v2; edge[2].e2 = v0;
  edge[0].l = acos( spatialVectorDot( v0, v1 ));
  edge[1].l = acos( spatialVectorDot( v1, v2 ));
  edge[2].l = acos( spatialVectorDot( v2, v0 ));

  for(i = 0; i < c->corners_.length_; i++) {
    j = 0;
    if(i < c->corners_.length_ - 1) j = i+1;
    /* length of edge of convex */
    cedgelen = acos( spatialVectorDot( &(c->corners_.vector_[i]),
				       &(c->corners_.vector_[j]) ));

    /* calculate the intersection - all 3 edges */
    for (iedge = 0; iedge < 3; iedge++) {
      spatialVectorCrossNorm( &(c->corners_.vector_[i]),  
			      &(c->corners_.vector_[j]), &tmp );
      spatialVectorCrossNorm( &(edge[iedge].e), &tmp, &a1 );
      /* 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(k = 0; k < 2; k++) {
	l1 = acos( spatialVectorDot( &(c->corners_.vector_[i]), &a1 ));
	l2 = acos( spatialVectorDot( &(c->corners_.vector_[j]), &a1 ));
	if( l1 - cedgelen <= gEpsilon && l2 - cedgelen <= gEpsilon ) {
	  l1 = acos( spatialVectorDot( edge[iedge].e1, &a1 ));
	  l2 = acos( spatialVectorDot( edge[iedge].e2, &a1 ));
	  if( l1 - edge[iedge].l <= gEpsilon && 
	      l2 - edge[iedge].l <= gEpsilon ) 
	    return true;
	}
	/* do the same for the other intersection */
	spatialVectorMul( &a1, neg );
      }
    }
  }
  return spatialConvexTestVectorInside( c, v0, v1, v2, 
					&(c->corners_.vector_[0]));
}

/* /////////////TESTEDGE/////////////////////////////////////
// testEdge: test if edges intersect with constraint. This problem
//           is solved by a quadratic equation. Return true if there is
//	     an intersection.
*/
bool spatialConvexTestEdge( SpatialConvex *c,
			    const SpatialVector * v0, 
			    const SpatialVector * v1, 
			    const SpatialVector * v2 )
{
  size_t i;
  for(i = 0; i < c->constraints_.length_; i++) {

    if ( CONSTR(i).sign_ == nEG ) {  /* test only 'holes' */
      if ( spatialConvexESolve( c, v0, v1, i ) ) return true;
      if ( spatialConvexESolve( c, v1, v2, i ) ) return true;
      if ( spatialConvexESolve( c, v2, v0, i ) ) return true;
    }
  }
  return false;
}

/* /////////////ESOLVE///////////////////////////////////////
// eSolve: solve the quadratic eq. for intersection of an edge with a circle
//         constraint. Edge given by grand circle running through v1, v2
//        Constraint given by cIndex. 
*/
bool spatialConvexESolve( SpatialConvex *c,
			  const SpatialVector * v1, 
			  const SpatialVector * v2, size_t cIndex )
{
  float64 gamma1, gamma2, mu, u2, a, b, cc, D, q, root1, root2;
  int i = 0;

  gamma1 = spatialVectorDot( v1, &(CONSTR(cIndex).a_) );
  gamma2 = spatialVectorDot( v2, &(CONSTR(cIndex).a_) );
  mu     = spatialVectorDot( v1, v2 );
  u2     = (1 - mu) / (1 + mu);

  a      = - u2 * (gamma1 + CONSTR(cIndex).d_);
  b      = gamma1 * ( u2 - 1 ) + gamma2 * ( u2 + 1 );
  cc     = gamma1 - CONSTR(cIndex).d_;

  D      = b * b - 4 * a * cc;

  if( D < 0.0L ) return false; /* no intersection */

  /* calculate roots a'la Numerical Recipes */

  q      = -0.5L * ( b + ( SGN(b) * sqrt(D) ) );

  if ( a > gEpsilon || a < -gEpsilon ) { root1 = q / a; i++; }
  if ( q > gEpsilon || q < -gEpsilon ) { root2 = cc / q; i++; }

  /* Check whether the roots lie within [0,1]. If not, the intersection
     is outside the edge. */

  if (i == 0) return false; /* no solution */
  if ( root1 >= 0.0L && root1 <= 1.0L ) return true;
  if ( i == 2 && ( (root1 >= 0.0L && root1 <= 1.0L ) ||
		   (root2 >= 0.0L && root2 <= 1.0L ) ) ) return true;

  return false;
}

/* ////////////TESTBOUNDINGCIRCLE///////////////////////////
// testBoundingCircle: test for boundingCircles intersecting with constraint
*/
bool spatialConvexTestBoundingCircle( SpatialConvex *c,
				      const SpatialVector * v0, 
				      const SpatialVector * v1, 
				      const SpatialVector * v2 )
{

  SpatialVector cc, s1, s2;
  float64 d, tst;
  size_t i;

  /* Set the correct direction: The normal vector to the triangle plane */
  spatialVectorSub( v1, v0, &s1 );
  spatialVectorSub( v2, v1, &s2 );
  spatialVectorCrossNorm( &s1, &s2, &cc );

  /* 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 */
  d = acos ( spatialVectorDot( &cc, v0 ) );

  /* for zero convexes, we have calculated a bounding circle for the convex.
     only test with this single bounding circle. */

  if(c->sign_ == zERO) {
    if ( ( ( (tst = spatialVectorDot( &cc, &(c->boundingCircle_.a_)) ) 
	                < -1.0L + gEpsilon ) ? gPi : acos(tst) ) > 
	 ( d + c->boundingCircle_.s_) ) 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. */

  for(i = 0; i < c->constraints_.length_; i++) {
    tst = spatialVectorDot( &cc, &(CONSTR(i).a_) );
      if ( ( ( tst < -1.0L + gEpsilon ) ? gPi : acos(tst) ) > 
	   ( d + CONSTR(i).s_) ) return false;
  }
  return true;
}

/* /////////////TESTEDGECONSTRAINT///////////////////////////
// testEdgeConstraint: test if edges intersect with a given constraint.
*/
bool spatialConvexTestEdgeConstraint( SpatialConvex *c,
				      const SpatialVector * v0, 
				      const SpatialVector * v1, 
				      const SpatialVector * v2, 
				      size_t cIndex )
{
  if ( spatialConvexESolve( c, v0, v1, cIndex) ) return true;
  if ( spatialConvexESolve( c, v1, v2, cIndex) ) return true;
  if ( spatialConvexESolve( c, v2, v0, cIndex) ) return true;
  return false;
}

/* /////////////TESTOTHERPOSNONE/////////////////////////////
// testOtherPosNone: test for other positive constraints that do
//                   not intersect with an edge. Return its index
*/
size_t spatialConvexTestOtherPosNone( SpatialConvex *c,
				      const SpatialVector * v0, 
				      const SpatialVector * v1, 
				      const SpatialVector * v2 )
{
  size_t i = 1;
  while ( i < c->constraints_.length_ && CONSTR(i).sign_ == pOS ) {
    if ( !spatialConvexTestEdgeConstraint ( c, v0, v1, v2, i ) ) return i;
    i++;
  }
  return 0;
}

/* /////////////TESTCONSTRAINTINSIDE/////////////////////////
// testConstraintInside: look if a constraint is inside the triangle
*/
bool spatialConvexTestConstraintInside( SpatialConvex *c,
					const SpatialVector * v0, 
					const SpatialVector * v1, 
					const SpatialVector * v2, 
					size_t i )
{
  return spatialConvexTestVectorInside( c, v0, v1, v2, &(CONSTR(i).a_));
}

/* /////////////TESTVECTORINSIDE////////////////////////////
// testVectorInside: look if a vector is inside the triangle
*/
bool spatialConvexTestVectorInside( SpatialConvex *c,
				    const SpatialVector * v0, 
				    const SpatialVector * v1, 
				    const SpatialVector * v2, 
				    SpatialVector * v )
{

  /* If (a ^ b * c) < 0, vectors abc point clockwise.
     -> center c not inside triangle, since vertices are ordered
     counter-clockwise. */

  SpatialVector tmp;
  spatialVectorCross( v0, v1, &tmp );
  if( spatialVectorDot( &tmp, v ) < 0 ) return false ;
  spatialVectorCross( v1, v2, &tmp );
  if( spatialVectorDot( &tmp, v ) < 0 ) return false ;
  spatialVectorCross( v2, v0, &tmp );
  if( spatialVectorDot( &tmp, v ) < 0 ) return false ;

  return true;
}

/* /////////////READ/////////////////////////////////////////
 */
void spatialConvexRead( SpatialConvex *c, FILE *in )
{
  size_t nconstr,i;
  SpatialConstraint constr;
  char buffer[HTM_READBUFSIZE];
  char first;
  buffer[0] = '\0';

  spatialConstraintInit( &constr );
  /* ignore lines that begin with the comment character */
  first = (char)getc(in);
  while(first == COMMENT) {
    fgets(buffer, HTM_READBUFSIZE, in);
    first = (char)getc(in);
  }
  ungetc( (int)first, in );

  getStreamToken(buffer,in);
  nconstr = atoi(buffer);

  for(i = 0; i < nconstr; i++) {
    spatialConstraintRead( &constr, in );
    spatialConvexAdd( c, &constr.a_, constr.d_ );
  }
}

/* /////////////READ/////////////////////////////////////////
 */
void spatialConvexReadRaDec( SpatialConvex *c, FILE *in )
{
  size_t nconstr,i;
  SpatialConstraint constr;
  char buffer[HTM_READBUFSIZE];
  char first;
  buffer[0] = '\0';

  /* ignore lines that begin with the comment character */
  first = (char)getc(in);
  while(first == COMMENT) {
    fgets(buffer, HTM_READBUFSIZE, in);
    first = (char)getc(in);
  }
  ungetc( (int)first, in );

  getStreamToken(buffer,in);
  nconstr = atoi(buffer);

  for(i = 0; i < nconstr; i++) {
    spatialConstraintReadRaDec( &constr, in );
    spatialConvexAdd( c, &constr.a_, constr.d_ );
  }
}

/* /////////////WRITE////////////////////////////////////////
 */
void spatialConvexWrite( SpatialConvex *c, FILE *out ) {
  size_t i;
  fprintf(out,"#CONVEX\n%d\n",c->constraints_.length_);
  for (i = 0; i < c->constraints_.length_ ; i++)
    spatialConstraintWrite( &(CONSTR(i)), out);
}

size_t varVecIntAppend( VarVecInt *v, int32 i ) {

  size_t newsize = v->capacity_ * 2;
  if(v->length_ == v->capacity_) {
    if(newsize == 0)newsize = 2;
    if(v->vector_ == NULL)
      v->vector_ = (int32 *)malloc(newsize * sizeof(int32));
    else
      v->vector_ = (int32 *)realloc( v->vector_, newsize * sizeof(int32) );
    v->capacity_ = newsize;
  }
  v->vector_[v->length_] = i;
  return v->length_++;
}

void varVecIntInit( VarVecInt *v ) {
  v->vector_ = NULL;
  v->capacity_= 0;
  v->length_ = 0;
}

void varVecIntClear( VarVecInt *v , bool keep ) {
  if(!keep){
    free(v->vector_);
    v->capacity_ = 0;
  }
  v->length_ = 0;
}

int varVecIntOrderDescending ( const void *a, const void *b ) {
  int32 ia = *((int32 *)a);
  int32 ib = *((int32 *)b);

  if (ia == ib) return 0;
  return ia < ib ? 1 : -1 ;
}

void spatialIDVecInit( SpatialIDVec *v ) {
  v->vector_ = NULL;
  v->length_ = 0;
  v->capacity_ = 0;
}

size_t spatialIDVecAppend( SpatialIDVec *v, IDTYPE i ) {

  size_t newsize = v->capacity_ * 2,j;
  IDTYPE *tmp;
  if(v->length_ == v->capacity_) {
    if(newsize == 0)newsize = 2;
    if(v->vector_ == NULL)
      v->vector_ = (IDTYPE *)malloc( newsize * sizeof(IDTYPE) );
    else {
      v->vector_ = (IDTYPE *)realloc( v->vector_, newsize * sizeof(IDTYPE) );
    }
    v->capacity_ = newsize;
  }
  v->vector_[v->length_] = i;

  /* DEBUG OUTPUT
  printf("partial = ");
  for(j = 0; j <= v->length_; j++)
    printf("%llu ",v->vector_[j]);
  printf("\n");
  */

  return v->length_++;
}

void spatialIDVecClear( SpatialIDVec *v , bool keep ) {
  if(!keep){
    free(v->vector_);
    v->capacity_ = 0;
  }
  v->length_ = 0;
}

void spatialConvexVecInit( SpatialConvexVec *v ) {
  v->vector_ = NULL;
  v->length_ = 0;
  v->capacity_ = 0;
}

size_t spatialConvexVecAppend( SpatialConvexVec *v, 
			       const SpatialConvex *c )
{

  size_t newsize = v->capacity_ * 2;
  if(v->length_ == v->capacity_) {
    if(newsize == 0){
      newsize = 2;
      v->vector_ = (SpatialConvex *)malloc( newsize * sizeof(SpatialConvex) );
    } else
      v->vector_ = (SpatialConvex *)realloc( v->vector_, newsize * sizeof(SpatialConvex) );
    v->capacity_ = newsize;
  }
  spatialConvexInit( &(v->vector_[v->length_]) );
  spatialConvexAssign( &(v->vector_[v->length_]), c );
  return v->length_++;
}

void spatialConvexVecClear( SpatialConvexVec *v , bool keep ) {
  if(!keep){
    free(v->vector_);
    v->capacity_ = 0;
  }
  v->length_ = 0;
}