/Users/craigcornelius/Projects/SPRING Mac Release 0.2/boundingsphere.cpp

Go to the documentation of this file.

// $Id: boundingsphere.cpp,v 1.30 2006/05/24 16:52:40 sean Exp $
#include "boundingsphere.h"
#include "object.h"

#ifdef _WIN32
#include <glut.h>
#else
#include <GLUT/glut.h>
#include <OpenGL/gl.h>
#endif

//#include <queue.h>

const char* BoundingSphere::rcsid = "@(#) $Id: boundingsphere.cpp,v 1.30 2006/05/24 16:52:40 sean Exp $ $Copyright: (c)2001 National Biocomputation Center, Stanford University $";

// OLDUPDATE uses a leaf set instead of a priority queue (PQ is 10-20% faster)
#define OLDUPDATE 
#ifdef OLDUPDATE
BSLset BoundingSphereLeaf::sphereLeafSet;
#else
BSqueue BoundingSphere::spherePQ;
#endif
int bncount = 0;

int BoundingSphere::debug = 0;
int BoundingSphere::return_first_collision_only = 0;

// Helper functions

bool scompare::operator() (const BoundingSphere* s1, 
                           const BoundingSphere* s2) const
{ return (s1->level < s2->level); }

void swapptr(void **a, void **b)
{
    void* temp = *a;
    *a = *b;
    *b = temp;
}


//  BoundingSphere functions

// constructor for BoundingSphere superclass
BoundingSphere::BoundingSphere(BoundingSphere *parent_)
{
    parent = parent_;
    
    if (parent) level = parent->level + 1;
    else level = 0;     // root
    
    // center and radius set in subclasses
    
    inQueue = false;
    enabled = 1;
}

// destructor for BoundingSphere superclass
BoundingSphere::~BoundingSphere()
{

    parent = NULL;
    center = Point3D(-1, -1, -1);
    radius = -1;
    level = -1;
    inQueue = false;
}

// threshold prevents the old sphere from being excessively big
// radius must be bigger than newRadius for threshold to matter
// threshold should be >= 1.
bool BoundingSphere::remainsValid(Point3D newCenter, double newRadius, 
                                  double threshold)
{
    if ((center.Dist(newCenter) + newRadius) > radius)
        return false;
    
    double ratio = radius/newRadius;
    
    if (ratio > threshold)
        return false;
    
    return true;
}

// this returns the Node* covered by given bounding sphere tree which is
// closest to pt (and not more than thresh away from pt)
Node* BoundingSphere::findClosestNode(BoundingSphere *s, Point3D pt, 
                                      double *thresh)
{
    if (!s->isLeaf())
        return(((BoundingSphereNode *)s)->findClosestNode(pt, thresh));
    else
        return(((BoundingSphereLeaf *)s)->findClosestNode(pt, thresh));
}

Node* BoundingSphereNode::findClosestNode(Point3D pt, double *thresh)
{
    // is pt close enough to this sphere
    double squaredSep = center.SquaredDist(pt);
    if (squaredSep >= (radius + *thresh)*(radius + *thresh))
        return(NULL);
    
    // call for left child
    Node *nl = BoundingSphere::findClosestNode(left, pt, thresh);
    
    // call for right child if it exists
    if (right) {
        Node *nr = BoundingSphere::findClosestNode(right, pt, thresh);
        if (nr)
            return(nr);
    }
    return(nl);
}

Node* BoundingSphereLeaf::findClosestNode(Point3D pt, double *thresh)
{
    Node *ret_node = NULL;
    
    // is pt close enough to this sphere
    double squaredSep = center.SquaredDist(pt);
    if (squaredSep >= (radius + *thresh)*(radius + *thresh))
        return(ret_node);
    
    // are any of the Nodes associated with the primitive close enough to pt?
    // if so, modify the return node and the current threshold
    double squaredThresh = (*thresh)*(*thresh);
    switch (feature_type) {
        case tetra_ptr:
        squaredSep = pt.SquaredDist(getTetra()->getNodeLink(0)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getTetra()->getNodeLink(0);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getTetra()->getNodeLink(1)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getTetra()->getNodeLink(1);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getTetra()->getNodeLink(2)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getTetra()->getNodeLink(2);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getTetra()->getNodeLink(3)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getTetra()->getNodeLink(2);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        break;
        
        case face_ptr:
        squaredSep = pt.SquaredDist(getFace()->getNodeLink(0)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getFace()->getNodeLink(0);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getFace()->getNodeLink(1)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getFace()->getNodeLink(1);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getFace()->getNodeLink(2)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getFace()->getNodeLink(2);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        break;
    case edge_ptr:
        squaredSep = pt.SquaredDist(getEdge()->getNodeLink(0)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getEdge()->getNodeLink(0);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        squaredSep = pt.SquaredDist(getEdge()->getNodeLink(1)->p);
        if (squaredSep < squaredThresh) {
            ret_node = getEdge()->getNodeLink(1);
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        break;
    case node_ptr:
        squaredSep = pt.SquaredDist(getNode()->p);
        if (squaredSep < squaredThresh) {
            ret_node = getNode();
            *thresh = sqrt(squaredSep);
            squaredThresh = squaredSep;
        }
        break;
    }
    
    // now return the closest node (if any are closer than current thresh)
    //  pertaining to this feature
    return(ret_node);
}

int BoundingSphere::hasAncestor(BoundingSphere* b)
{
    BoundingSphere* ptr = this;
    
    while (ptr != NULL) {
        if (ptr == b) return(1);        // found it!
        ptr = ptr->getParent();
    }
    
    // didn't find it
    return(0);
}

int BoundingSphere::getDepth()
{
    BoundingSphere* parent = getParent();
    
    if (parent == NULL) return(1);
    else return ( 1+ parent->getDepth() );
}

BoundingSphereNode* BoundingSphere::findCommonAncestor(BoundingSphere* b[], 
                                                       int num)
{
    if (num <= 0) return(NULL);
    
    // start off with the 0th guy's parent
    BoundingSphere* cur = b[0]->getParent();
    
    // loop over all the other ones
    for (int i=0; i<num; i++) {
        // if b[i] doesn't have cur as an ancestor, move cur up until it is
        while (!b[i]->hasAncestor(cur) && (cur != NULL)) {
            cur = cur->getParent();
        }
    }
    
    return((BoundingSphereNode*)cur);
}

// sets value of enabled variable for subtree under us
void BoundingSphere::markTreeEnabled(int enabled_)
{
    // set ours
    enabled = enabled_;
    
    // and recurse with children if we have any
    if (!isLeaf()) {
        BoundingSphereNode* me = (BoundingSphereNode*)this;
        BoundingSphere* my_left = me->getLeft();
        BoundingSphere* my_right = me->getRight();
        if (my_left) my_left->markTreeEnabled(enabled_);
        if (my_right) my_right->markTreeEnabled(enabled_);
    }
}

// propagates enabled value up tree from here to root
void BoundingSphere::propEnabled(int enabled_)
{
    // set ours
    enabled = enabled_;
    
    // and propagate up tree
    BoundingSphere* ptr = getParent();
    while (ptr) {
        ptr->enabled = enabled_;
        ptr = ptr->getParent();
    }
}

int BoundingSphere::numLeafs()
{
    // trivial case
    if (isLeaf()) 
        return(1);
    
    // otherwise, I'm a BSN
    BoundingSphereNode* me = (BoundingSphereNode*)this;
    
    // get my left and right children
    BoundingSphere* left = me->getLeft();
    BoundingSphere* right = me->getRight();
    
    // recurse 
    int num = 0;
    if (left) num += left->numLeafs();
    if (right) num += right->numLeafs();
    
    // and return
    return(num);
}

int BoundingSphere::numEnabledLeafs()
{
    // trivial case
    if (isLeaf()) 
        return( enabled );
    
    // otherwise, I'm a BSN
    BoundingSphereNode* me = (BoundingSphereNode*)this;
    
    // get my left and right children
    BoundingSphere* left = me->getLeft();
    BoundingSphere* right = me->getRight();
    
    // recurse 
    int num = 0;
    if (left) num += left->numEnabledLeafs();
    if (right) num += right->numEnabledLeafs();
    
    // and return
    return(num);
}

int BoundingSphere::dist(BoundingSphere *sa, BoundingSphere *sb, double error, 
                         double init)
{
    if (debug==2) 
        cerr << " BoundingSphere::dist(" << sa << ", " << sb
        << ", " << error << ", " << init << ")\n";
    
    // sanity check
    if (!sa || !sb) return(0);
    
    // performance optimization
    if (!sa->enabled) return(0);
    if (!sb->enabled) return(0);
    
    if (!sa->isLeaf()) {
        if (debug==3) cerr << "sa is not a leaf" << endl;
        return (BoundingSphereNode::dist((BoundingSphereNode *)sa, sb, error, 
            init));
    }
    if (debug==3) cerr << "sa is a leaf" << endl;
    
    if (!sb->isLeaf()) {
        if (debug==3) cerr << "sb is not a leaf" << endl;
        return (BoundingSphereNode::dist((BoundingSphereNode *)sb, sa, error, 
            init));
    }
    if (debug==3) cerr << "sb is a leaf" << endl;
    
    return (BoundingSphereLeaf::dist((BoundingSphereLeaf *)sa, 
        (BoundingSphereLeaf *)sb, error, init));
}

// will I need a couple of these with different arguments depending 
// on the node type being traversed??
int BoundingSphere::addLeaf(BoundingSphereLeaf *sa, BoundingSphere *sb)
{
    if (debug==2) cerr << "BoundingSphere::addLeaf " << endl;
    
    if (!sa->isLeaf()) {
        if (debug==3) cerr << "sa is not a leaf" << endl;
        return (0);
    }
    
    if (!sb->isLeaf()) {
        
        int split = 0;
        
        const Point3D &acenter = sa->getCenter();
        
        double sacenter[3];
        sacenter[0] = acenter.x;
        sacenter[1] = acenter.y;
        sacenter[2] = acenter.z;
        
        const Point3D &bcenter = ((BoundingSphereNode*)sb)->getCenter();
        
        double sbcenter[3];
        sbcenter[0] = bcenter.x;
        sbcenter[1] = bcenter.y;
        sbcenter[2] = bcenter.z;
        
        if ((sbcenter[1] - sacenter[1]) > (sbcenter[split] - sacenter[split]))
            split = 1;
        if ((sbcenter[2] - sacenter[2]) > (sbcenter[split] - sacenter[split]))
            split = 2;
        
        if (sacenter[split] < sbcenter[split]) {
            return(BoundingSphere::addLeaf(sa, ((BoundingSphereNode*)sb)->getLeft() ));
        } 
        else {
            return(BoundingSphere::addLeaf(sa, ((BoundingSphereNode*)sb)->getRight() ));
        }
    }   
    
    
    // if you get here then that means sb is a leaf
    // the node above the leaf may have 1 or 2 children
    BoundingSphereNode* sc = ((BoundingSphereNode*)sb->parent); 
    
    // can sa and sb also be split on some axis to know
    // which side to add it to??
    
    if ( sc->getLeft() == sb ) { 
        BoundingSphereNode* mid = new BoundingSphereNode(sb,sa,sc);
        sc->setLeft(mid);
                mid->setParent(sc);
    }
    else {
        BoundingSphereNode* mid = new BoundingSphereNode(sa,sb,sc);
        sc->setRight(mid);
                mid->setParent(sc);
    }
    
    return (1);
}


int BoundingSphere::removeLeaf(BoundingSphereLeaf *sa)
{
    if (debug==1) cerr << "BoundingSphere::removeLeaf " << endl;
    
    if (!sa->isLeaf()) {
        if (debug==1) cerr << "sa is not a leaf" << endl;
        return (0);
    }
    
    BoundingSphereLeaf *buddy = NULL; 
    BoundingSphereNode *oneUp = NULL;
        oneUp = (BoundingSphereNode*)sa->parent;
    
    if (oneUp) {
                if (sa == oneUp->getLeft()){
                        buddy = (BoundingSphereLeaf*)((BoundingSphereNode*)oneUp)->getRight();
                        oneUp->setRight(NULL); // this is so at the sa destructor, we dont remove buddy
                }
                else {
                        buddy = (BoundingSphereLeaf*)((BoundingSphereNode*)oneUp)->getLeft();
                        oneUp->setLeft(NULL); 
                }
    
    
                BoundingSphereNode *twoUp = NULL;
                twoUp = (BoundingSphereNode*)oneUp->parent;
    
                if (twoUp) {
                        if (oneUp == twoUp->getLeft()){
                                twoUp->setLeft(buddy);
                                buddy->setParent(twoUp);
                        }
                        else{
                                twoUp->setRight(buddy);
                                buddy->setParent(twoUp);
                        }                       
                        delete ((BoundingSphereNode *)oneUp); // atthis destructor, buddy remains intact, but sa is deleted
                }
                else {
                        if(buddy == NULL){
                                delete ((BoundingSphereNode *)oneUp);           
                                return (1);
                        } else {
                                sa->getFeatureObject()->setBoundingSphereRoot((BoundingSphereNode *)buddy); // make buddy the root of the tree
                                buddy->setParent(NULL);
                                delete ((BoundingSphereNode *)oneUp);
                                return (1);
                        }
                }

    }
        else {
                // this leaf is the root, just remove the whole tree
                sa->getFeatureObject()->setBoundingSphereRoot(NULL);
                delete ((BoundingSphereLeaf *)sa);
        }
    
    return (1);
    
}


//  BoundingSphereNode functions


void BoundingSphereNode::drawSubtree()
{
    // recurse
    if (left)
        if (left->isLeaf())
            ((BoundingSphereLeaf *)left)->draw();
        else
            ((BoundingSphereNode *)left)->drawSubtree();
        if (right)
            if (right->isLeaf())
                ((BoundingSphereLeaf *)right)->draw();
            else
                ((BoundingSphereNode *)right)->drawSubtree();
            
            // and draw myself
            glColor4f((level+1)%2, ((level+1)%3)/2.0, ((level+1)%5)/4.0, 0.2);
            glTranslated(center.x, center.y, center.z);
            glutSolidSphere(radius, 8, 8);
            glTranslated(-center.x, -center.y, -center.z);
}

// no recursion
BoundingSphereNode::BoundingSphereNode(BoundingSphere *left_, 
                                       BoundingSphere *right_, BoundingSphere *parent_)
                                       : BoundingSphere(parent_)
{
    // BoundingSphere superclass sets its own values
    
    left = left_;
    right = right_;
    
    if (left) {
        left->parent = this;
        left->level = level + 1;
    }
    if (right) {
        right->parent = this;
        right->level = level + 1;
    }
    
    bncount++;
    if (debug==3) 
        cerr << "making node " << this << " with " << left << " and " 
        << right << endl;
    
    procChange();
}

// creates root node and recurses on sphereList to build tree
BoundingSphereNode::BoundingSphereNode(BSlist &sphereList, 
                                       BoundingSphere *parent_)
                                       : BoundingSphere(parent_)
{
    bncount++;
    if (debug==3) {
        cerr << "making node " << this << " with " << sphereList.NumElements() 
            << " children to be sorted" << endl;
    }
    
    // find max/min etc. 
    BSlist leftList, rightList;    
    
    double min[3] = { LARGE,  LARGE,  LARGE};
    double max[3] = {-LARGE, -LARGE, -LARGE};
    
    double mid;
    
    int split = 0;
    
    {   // compute center and min/max values for the spheres
        for (int sphereIter=0; sphereIter<sphereList.NumElements(); sphereIter++) {
            
            BoundingSphere *cur = sphereList[sphereIter];    
            const Point3D &center = cur->getCenter();
            
            double ncenter[3];
            ncenter[0] = center.x;
            ncenter[1] = center.y;
            ncenter[2] = center.z;
            
            for (int i = 0; i < 3; i++) {
                if (ncenter[i] < min[i])
                    min[i] = ncenter[i];
                if (ncenter[i] > max[i])
                    max[i] = ncenter[i];
            }
        }
    }
    
    if ((max[1] - min[1]) > (max[split] - min[split]))
        split = 1;    
    if ((max[2] - min[2]) > (max[split] - min[split]))
        split = 2;
    
    mid = (min[split] + max[split]) / 2;
    
    
    // use this info to split the data 
    for (int sphereIter=0; sphereIter < sphereList.NumElements(); sphereIter++) {
        BoundingSphere *cur = sphereList[sphereIter];
        const Point3D &center = cur->getCenter();
        
        double ncenter[3];
        ncenter[0] = center.x;
        ncenter[1] = center.y;
        ncenter[2] = center.z;
        
        if (ncenter[split] < mid) {
            leftList.Append(cur);
        } else {
            rightList.Append(cur);
        }
    }
    
    // if left list ended up empty, copy at least one element over (first one)
    if ( leftList.NumElements() == 0 && rightList.NumElements() ) {
        // copy over 1 item from leftlist to rightlist
        leftList.Append(rightList[0]);
        rightList.DeleteElement(rightList[0]);
    }
    
    // now, if leftlist only has 1 element, take it out and we're done
    if (leftList.NumElements() == 1) {
        left = leftList[0];
        // sets the leaf node's parent
        left->parent = this;
        left->level = level + 1;
        if (debug==2) cerr << "left: " << left << " parent: " << this << endl;
    } 
    else        // more than 1 element, recurse
        left = new BoundingSphereNode(leftList, this);
    
    // now, if rightlist only has 1 element, take it out and we're done
    if (rightList.NumElements() == 1) {
        right = rightList[0];
        // sets the leaf node's parent
        right->parent = this;
        right->level = level + 1;
        if (debug==2) cerr << "right: " << right << " parent: " << this << endl;
    } 
    else        // more than 1 element, recurse
        if (rightList.NumElements()) right = new BoundingSphereNode(rightList, this);
        
        procChange();
}

// node destructor, recursively deletes its subtree
BoundingSphereNode::~BoundingSphereNode()
{
    if (left) {
        if (left->isLeaf())
            delete ((BoundingSphereLeaf *)left);
        else
            delete ((BoundingSphereNode *)left);
        }
        if (right) {
            if (right->isLeaf())
                delete ((BoundingSphereLeaf *)right);
            else
                delete ((BoundingSphereNode *)right);
        }
            
    if (debug && !parent)
       cout << "deleting sphere tree\n";

    left = NULL;
    right = NULL;
}

int BoundingSphereNode::dist(BoundingSphereNode *sa, BoundingSphere *sb, 
                             double error, double init)
{
    if (debug==2) 
        cerr << " BoundingSphereNode::dist(" << sa << ", " << sb
        << ", " << error << ", " << init << ")\n";
    
    // sb is also not a leaf, and sa is smaller, swap so sa is bigger
    if (!sb->isLeaf() && (sa->getRadius() < sb->getRadius())) {
        swapptr((void **)&sa, (void **)&sb);
    }
    
    // get locals
    Point3D sa_center = sa->getCenter(); Point3D sb_center = sb->getCenter();
    double sa_radius = sa->getRadius(); double sb_radius = sb->getRadius();
    double squaredSep = sa_center.SquaredDist(sb_center);
    
    if (debug==3) {
        cerr << "tester " << sa << " center " << sa_center << endl;
        cerr << "tester " << sa << " radius " << sa_radius << endl;
        cerr << "testee " << sb << " center " << sb_center << endl;
        cerr << "testee " << sb << " radius " << sb_radius << endl;
        cerr << "squaredSep " << squaredSep << endl;
        cerr << "test " << (squaredSep - (sa_radius + sb_radius)) << endl;
    }
    
    double radius_sum = sa_radius + sb_radius;
    if (squaredSep > (radius_sum * radius_sum)) {
        if (debug==3) cerr << "earliest cut out " << endl;
        return 0; // they do not overlap
    }
    
    // do left child
    int ldist = BoundingSphere::dist(sa->left, sb, error, init);
    
    // if our option says to only return the first collision, 
    // and if we have one, return
    if (return_first_collision_only && ldist)
        return ldist;
    
    if (debug==3) cerr << "returning control to dist2 testing right" << endl;
    
    // do right child
    int rdist = 0;
    if (sa->right) {
        rdist = BoundingSphere::dist(sa->right, sb, error, init);
        
        // if our option says to only return the first collision, 
        // and if we have one now, return
        if (return_first_collision_only && rdist)
            return rdist;
    }
    
    // return values
    if (ldist || rdist) return 1;
    return 0;
}

bool BoundingSphereNode::procChange()
{
    if (debug)
        cerr << level << " BSN::procChange " << this << endl;
    
    // get current info for left side
    Point3D lc = left->getCenter();
    double lr = left->getRadius();
    
    if (debug==3) {
        cerr << left << " left center " << lc << " radius " << lr << endl;
        if (left->isLeaf()) cerr << "leaf index " << 
            ((BoundingSphereLeaf *)left)->getFeatureIndex() 
            << endl;
    }
    
    // our new info
    double newRadius;
    Point3D newCenter;
    
    if (right) {
        Point3D rc = right->getCenter();
        double rr = right->getRadius();
        
        if (debug==3) {
            cerr << right << " right center " << rc << " radius " << rr << endl;
            if (right->isLeaf()) cerr << "leaf index " << 
                ((BoundingSphereLeaf *)right)->getFeatureIndex()
                << endl;
        }
        
        // get distance between lc and rc
        double dist = lc.Dist(rc);
        
        if (debug==3) {
            cerr << " dist " << dist << endl;
            cerr << " dist plus left " << dist + lr << endl;
            cerr << " dist plus right " << dist + rr << endl;
        }
        
        
        if ( rr > (dist + lr) ) {
            if (debug==1) cerr << " left inside right sphere" << endl;
            newRadius = rr;
            newCenter = rc;
        }
        else if ( lr > (dist + rr) ) {
            if (debug==1) cerr << " right inside left sphere" << endl;
            newRadius = lr;
            newCenter = lc;
        }
        else {
            if (debug==3) 
                cerr << " i'm not in either, just going to add them " << endl;
            newRadius = (dist + lr + rr) / 2.0;
            
            if (dist != 0)
                newCenter.interp(lc, rc, (newRadius - lr) / dist); 
            else { // they are circumspheres
                if (lr > rr) { // take the larger one
                    newCenter = lc; 
                    // before steve just took the left, how did he 
                    //    know it was truly larger?
                    newRadius = lr;
                }
                else {
                    newCenter = rc;
                    newRadius = rr;
                }
            }
        }
    }    // right
    else {
        if (debug==3) cerr << "no right " << endl;
        newCenter = lc;
        newRadius = lr;
    }
    
    // set our new values
    this->center = newCenter;
    this->radius = newRadius;
    if (debug==3) 
        cerr << this << " my center " << center << " radius " << radius << endl;
    
    // and get outta here
    return true;
}


//  BoundingSphereLeaf functions
BoundingSphereLeaf::BoundingSphereLeaf(Tetra *tetra_, BoundingSphere *parent_)
: BoundingSphere(parent_)
{
  if (debug == 2) {
                cout << "BoundingSphereLeaf" << endl;
                cout << "parent: " << parent << endl;
        }
        
  feature_ptr.tetra = tetra_;
  feature_type = tetra_ptr;     
        if (debug==2) cout << "tetra: " << feature_ptr.tetra << endl;

  feature_ptr.tetra->setBoundingSphere(this);

  setRadiusAndCenterFromFeature();

  if (debug==3) {
    cout << "making leaf: " << this << endl;    
    cout << "tetra: " << getFeatureIndex() << endl;
    cout << "radius: " << radius << endl;
    cout << "center: " << center << endl;
  }

  inQueue = false;
}

BoundingSphereLeaf::BoundingSphereLeaf(Face *face_, BoundingSphere *parent_)
: BoundingSphere(parent_)
{
    if (debug==2) cerr << "BoundingSphereLeaf" << endl;
    
    feature_ptr.face = face_;
    feature_type = face_ptr;
    if (debug==2) cerr << "face: " << feature_ptr.face << endl;
    
    feature_ptr.face->setBoundingSphere(this);
    
    setRadiusAndCenterFromFeature();
    
    if (debug==3) {
        cerr << "making leaf: " << this << endl;    
        cerr << "face: " << getFeatureIndex() << endl;
        cerr << "radius: " << radius << endl;
        cerr << "center: " << center << endl;
            cerr << "feature:" << feature_ptr.face->getIndex() << endl;
    }
}

BoundingSphereLeaf::BoundingSphereLeaf(Edge *edge_, BoundingSphere *parent_)
: BoundingSphere(parent_)
{
    if (debug==2) cerr << "BoundingSphereLeaf" << endl;
    
    feature_ptr.edge = edge_;
    feature_type = edge_ptr;
    if (debug==2) cerr << "edge: " << feature_ptr.edge << endl;
    
    feature_ptr.edge->setBoundingSphere(this);
    
    setRadiusAndCenterFromFeature();
    
    if (debug==3) {
        cerr << "making leaf: " << this << endl;    
        cerr << "edge: " << getFeatureIndex() << endl;
        cerr << "radius: " << radius << endl;
        cerr << "center: " << center << endl;
    }
}

BoundingSphereLeaf::BoundingSphereLeaf(Node *node_, BoundingSphere *parent_)
: BoundingSphere(parent_)
{
    if (debug==2) cerr << "BoundingSphereLeaf" << endl;
    
    feature_ptr.node = node_;
    feature_type = node_ptr;
    if (debug==2) cerr << "node: " << feature_ptr.node << endl;
    
    feature_ptr.node->setBoundingSphere(this);
    
    setRadiusAndCenterFromFeature();
    
    if (debug==3) {
        cerr << "making leaf: " << this << endl;    
        cerr << "node: " << getFeatureIndex() << endl;
        cerr << "radius: " << radius << endl;
        cerr << "center: " << center << endl;
    }
}

// leaf destructor
BoundingSphereLeaf::~BoundingSphereLeaf()
{
    feature_type = face_ptr;
    feature_ptr.face = NULL;
        sphereLeafSet.DeleteAll();
}

void BoundingSphereLeaf::draw()
{
    // draw a sphere around myself
    glColor4f(1.0, 1.0, 1.0, 0.2);
    glTranslated(center.x, center.y, center.z);
    glutSolidSphere(radius, 8, 8);
    glTranslated(-center.x, -center.y, -center.z);
}

int BoundingSphereLeaf::dist(BoundingSphereLeaf *sa, BoundingSphereLeaf *sb, 
                             double error, double init)
{
    if (debug==2) 
        cerr << " BoundingSphereLeaf::dist(" << sa << ", " << sb
        << ", " << error << ", " << init << ")\n";
    
    // first check if these 2 leaf spheres have any overlap
    Point3D sbcent = sb->getCenter();
    double squared = sa->getCenter().SquaredDist(sbcent);
    double sarad = sa->getRadius(); double sbrad = sb->getRadius();
    if (squared > (sarad+sbrad)*(sarad+sbrad))
        return 0;
    
    // now see if the underlying features are colliding
    Point3D int_p;
    int ret_val = 1;  // assume there is a collision, if not make it 0
    double dist = 0.0;
    double param1 = 0.0, param2 = 0.0;
    Point3D nearpt;
    
    //
    // face-face potential collision
    //
    if (sa->feature_type == face_ptr && sb->feature_type == face_ptr) {
        
        // same object, and same spheres (ie same face) or adjacent faces
        if (sa->getFeatureObject() == sb->getFeatureObject() &&
            (sa == sb || sa->getFace()->isAdjacent(sb->getFace())))
            return(0);
        
        // if error value is >0, then assume we're trying to do fast but
        // less accurate tests, so just assume we have a collision if the
        // leaf-node bounding spheres (bsphere of faces) are intersecting
        if (error > 0.0)
            ret_val = 1;
        else  // do the test
            ret_val = sa->feature_ptr.face->intersectsFace(sb->feature_ptr.face, 
            &int_p);
        // if we failed, cut out early
        if (!ret_val) return(0);
    }  // end face-face
    
    //
    // edge-edge potential collision
    //
    else if (sa->feature_type == edge_ptr && sb->feature_type == edge_ptr) {
      int i1 = sa->getEdge()->getIndex();
      int i2 = sb->getEdge()->getIndex();
#define SEPARATION 3  // don't want edges closer than this to cause collisions
      // same object, and nearly adjacent edges
      if (sa->getFeatureObject() == sb->getFeatureObject() &&
          i1 < i2+SEPARATION && i1 > i2-SEPARATION)
        return(0);
        
      // otherwise, do the test
      dist = sa->feature_ptr.edge->EdgeDistanceToEdge(sb->feature_ptr.edge,
                                                      &param1, &param2);
        
      // too far- cut out early (should use error value here someday)
      if (dist > 0.5*(sa->getFeatureObject()->getEdgeThresh() + 
                      sb->getFeatureObject()->getEdgeThresh()))
        return(0);//must return here!, not set ret_val
    }  // end edge-edge
    
    //
    // face-edge or edge-face potential collision
    //
    else if ((sa->feature_type == face_ptr && sb->feature_type == edge_ptr) ||
             (sa->feature_type == edge_ptr && sb->feature_type == face_ptr)) {

      // swap the (local) sa and sb pointers so that sa is edge, sb is face
      // ok b/c sa is local ptr, and &sa is its address, so contents of the
      //   spheres don't change, just their local names.
      if (sa->feature_type == face_ptr)
        swapptr((void **)&sa, (void **)&sb);

      dist = sa->feature_ptr.edge->EdgeDistanceToFaceNoIntersect
        (sb->feature_ptr.face, &param1, &nearpt);

      // too far -- cut out early
      if (dist > 0.5*(sa->getFeatureObject()->getEdgeThresh()))
        return(0);

      //testing:
      //cout << "Edge: " << sa->feature_ptr.edge->getIndex() << "  Face: " <<
      //sb->feature_ptr.face->getIndex() << "  Dist: " << dist << "  pt: " <<
      //nearpt << endl;
      // old way: finding edge/face intersection
      //ret_val = sa->feature_ptr.face->intersectsEdge(sb->feature_ptr.edge, 
      //                                               &int_p);
      //if (!ret_val) return(0);
    }  // end face-edge and edge-face
    
    // some other feature pair that's not handled yet, assume no collision
    else { 
        return(0);
    }
    
    // now we add the collisions to the appropriate objects
    Collision_pair pair;
    Object *myObj = sa->getFeatureObject();
    Object *otherObj = sb->getFeatureObject();

    // for collisions of edge objects
    if (sa->feature_type == edge_ptr && sb->feature_type == edge_ptr) {
      int append_flag = 1;
      if (myObj == otherObj) { // self-collision
        // here's some code to avoid adding the same collision twice, which
        //     is a problem for self-collisions
        // NB: i1 and i2 are already guaranteed to be non-= and non-adjacent
        //     based on SEPARATION above
        int i1 = sa->getEdge()->getIndex();
        int i2 = sb->getEdge()->getIndex();
        for (int k = 0; k < myObj->CollisionPairGetNumElements(); k++) {
          int j1 =
            myObj->CollisionPairGet(k).mySphere->getEdge()->getIndex();
          int j2 =
            myObj->CollisionPairGet(k).otherSphere->getEdge()->getIndex();
          if ((i1 == j1 && i2 == j2) || (i1 == j2 && i2 == j1))
            append_flag = 0;
        } // for each collision
      } // self-collision
      if (append_flag) {
        pair.mySphere = sa;
        pair.otherSphere = sb;
        pair.near1 = param1;
        pair.near2 = param2;
        pair.dist = dist;
        myObj->CollisionPairAdd(pair);
      }
      if (myObj != otherObj) { // add collisions for other edge object
        // switch spheres, switch params
        pair.mySphere = sb;
        pair.otherSphere = sa;
        pair.near1 = param2;
        pair.near2 = param1;
        pair.dist = dist;
        otherObj->CollisionPairAdd(pair);
      }
    }
    // for edge-face collisions
    else if (sa->feature_type == edge_ptr && sb->feature_type == face_ptr) {
      pair.mySphere = sa;
      pair.otherSphere = sb;
      pair.near1 = param1;
      pair.int_pt = nearpt;
      pair.dist = dist;
      myObj->CollisionPairAdd(pair);
      // switch spheres, add to other object
      pair.mySphere = sb;
      pair.otherSphere = sa;
      pair.near1 = param1;
      pair.int_pt = nearpt;
      pair.dist = dist;
      otherObj->CollisionPairAdd(pair);
    }
    // don't have face-edge collisions: pointers swapped to make edge-face

    // for other collisions (e.g. face-face)
    else {
      // save this collision_pair structure and add to this object
      pair.mySphere = sa;
      pair.otherSphere = sb;
      pair.int_pt = int_p;
      myObj->CollisionPairAdd(pair);

      // now swich MY and OTHER, and add it to other object
      pair.mySphere = sb;
      pair.otherSphere = sa;
      otherObj->CollisionPairAdd(pair);
    }


    // debugging
    if (debug == 2) {
        cout << "2:I'm colliding over here: [" 
            << myObj->getName() << "]-" << sa->getFeatureIndex() << " - [" 
            << otherObj->getName() << "]-" << sb->getFeatureIndex() << endl;
    }
    
    // return value indicating there was a collision
    return ret_val;
}

bool BoundingSphereLeaf::procChange()
{
    if (debug)
        cerr << getFeatureObject()->getName() << " index:" << getFeatureIndex()
        << " level:" << level << " BSL::procChange" << endl;
    
    // got rid of processed variable, if we call this on a leaf, it's because 
    // the leaf is in the update list, so we always update and return true
    // the return value is only used in the old 'propChange' method
    setRadiusAndCenterFromFeature();
    
    if (debug==3) {
        cerr << "updating leaf: " << this << endl;    
        switch (feature_type) {
        case tetra_ptr:
            cerr << "tetra: " << getFeatureIndex() << endl;
            break;
        case face_ptr:
            cerr << "face: " << getFeatureIndex() << endl;
            break;
        case edge_ptr:
            cerr << "edge: " << getFeatureIndex() << endl;
            break;
        case node_ptr:
            cerr << "node: " << getFeatureIndex() << endl;
            break;
        }
        cerr << "radius: " << radius << endl;
        cerr << "center: " << center << endl;
    }
    return true;
}

void BoundingSphereLeaf::insertInUpdateList()
{
    // performance optmization
    if (!enabled) return;
    
    // note the sphereLeafSet or spherePQ is static, just one exists, so we
    // do all our sphere updates for all objects at once
#ifdef OLDUPDATE
    sphereLeafSet.AddElement(this);
#else
    if (inQueue == false) {
        // avoid pushing a leaf sphere into the priority queue more than once
        spherePQ.push(this);
        inQueue = true;
    }
#endif
}

void BoundingSphereLeaf::processUpdate()
{
    if (debug==2) cerr << "BoundingSphereLeaf::processUpdate()" << endl;
    
#ifdef OLDUPDATE    
    // old method: go through the leaf set, propagate each change upwards
    if (debug==3) cerr << " set size: " << sphereLeafSet.NumElements() << endl;
    
    for (int slsIter=0; slsIter<sphereLeafSet.NumElements(); slsIter++)
        (sphereLeafSet[slsIter])->propChange();
    
    sphereLeafSet.DeleteAll();
#else
    // new method: go through the priority queue, starts out with just leaves,
    // then each adds its parent if the parent is not already in the queue
    while (!spherePQ.empty()) {
        // get and pop the highest priority sphere (eg closest to leaf)
        BoundingSphere *top = spherePQ.top();
        spherePQ.pop();
        // process it and mark it as no longer in batch
        top->procChange();
        top->inQueue = false;
        // and stick its parent in queue if not there already
        if (top->parent && !top->parent->inQueue) {
            spherePQ.push(top->parent);
            top->parent->inQueue = true;
            assert(top->parent->level < top->level);
        }
    }
#endif
}

void BoundingSphereLeaf::setRadiusAndCenterFromFeature()
{
    switch (feature_type) {
    case tetra_ptr:
        this->radius = feature_ptr.tetra->getRadius(); 
        this->center = feature_ptr.tetra->getCenter(); 
        break;
    case face_ptr:
        this->radius = feature_ptr.face->getRadius(); 
        this->center = feature_ptr.face->getCenter(); 
        break;
    case edge_ptr:
      // use MAX so that we find collisions as soon as inside threshold
        this->radius = MAX(getFeatureObject()->getEdgeThresh(),
                           feature_ptr.edge->getRadius()); 
        this->center = feature_ptr.edge->getCenter(); 
        break;
    case node_ptr:
        this->radius = feature_ptr.node->getRadius(); 
        this->center = feature_ptr.node->getCenter(); 
        break;
    }
}

Object* BoundingSphereLeaf::getFeatureObject()
{
    Object *obj = NULL;
    switch (feature_type) {
     case tetra_ptr:
        obj = feature_ptr.tetra->getTetraArray()->getObject();
        break;
    case face_ptr:
        obj = feature_ptr.face->getFaceArray()->getObject();
        break;
    case edge_ptr:
        obj = feature_ptr.edge->getEdgeArray()->getObject();
        break;
    case node_ptr:
        obj = feature_ptr.node->getNodeArray()->getObject();
        break;
    }
    return (obj);
}

int BoundingSphereLeaf::getFeatureIndex()
{
    int index = 0;
    switch (feature_type) {
     case tetra_ptr:
        index = feature_ptr.tetra->getIndex();
        break;
    case face_ptr:
        index = feature_ptr.face->getIndex();
        break;
    case edge_ptr:
        index = feature_ptr.edge->getIndex();
        break;
    case node_ptr:
        index = feature_ptr.node->getIndex();
        break;
    }
    return(index);
}

---