planners/obsolete/PL_SimplexSubdivision.cpp

Go to the documentation of this file.

//## begin module%39864C6600AA.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%39864C6600AA.cm

//## begin module%39864C6600AA.cp preserve=no
//## end module%39864C6600AA.cp

//## Module: PL_SimplexSubdivision%39864C6600AA; Pseudo Package body
//## Source file: C:\project\mpk\code\Planners\PL_SimplexSubdivision.cpp

//## begin module%39864C6600AA.additionalIncludes preserve=no
//## end module%39864C6600AA.additionalIncludes

//## begin module%39864C6600AA.includes preserve=yes
#include <assert.h>
#include <math.h>
#pragma warning( disable : 4786 )
//## end module%39864C6600AA.includes

// PL_SimplexSubdivision
#include "synchronization/semaphore.h"
#include "PL_SimplexSubdivision.h"
#include "opengl/glos.h"
#include <gl/gl.h>

//=============================================================================
// Constructor
//
// description: constructor
//=============================================================================
PL_SimplexSubdivision::PL_SimplexSubdivision()
:
        m_NodeStart( 0 ),
        m_NodeGoal( 0 ),
        m_Compare( this ),
        m_SuspiciousNodesAndVolumes( m_Compare )
{
}


//=============================================================================
// Destructor
//
// description: Destructor
//=============================================================================
PL_SimplexSubdivision::~PL_SimplexSubdivision()
{
        //nothing
}

//=============================================================================
// AddEdge
//
// description: Adds an edge between two nodes
//=============================================================================
void PL_SimplexSubdivision::AddEdge( const int node0, const int node1 )
{
        if( !IsAdjacent( node0, node1 ) )
        {
                return;
        }

        Node& n0 = m_Nodes[ node0 ];
        Node& n1 = m_Nodes[ node1 ];
        n0.AddNeighbor( node1 );
        n1.AddNeighbor( node0 );
}

//=============================================================================
// AddNode
//
// description: Adds a node to the vector of nodes
//=============================================================================
int PL_SimplexSubdivision::AddNode
( 
        const Configuration& min, 
        const Configuration& max 
)
{
        Node addme;
#pragma message( "##### Magic numbers are bad" )
        addme.m_DistanceFromStart = 99999999;   
        addme.m_Min = min;
        addme.m_Max = max;
        addme.m_Alive = true;
        addme.m_NodesAdjacent.clear();
        addme.m_LocationOfCollision = ( min + max ) / 2;
        this->m_Nodes.push_back( addme );
        int returnMe = m_Nodes.size() - 1;
        this->m_OpenList.insert( returnMe );
        return returnMe;
}

//=============================================================================
// AddSuspiciousNode
//
// description: adds a suspicious node to the set
//=============================================================================
void PL_SimplexSubdivision::AddSuspiciousNode( const int nodeNum )
{
        m_SuspiciousNodesAndVolumes.insert( nodeNum );
}


//=============================================================================
// AddToOpenList
//
// description: adds a node to the open list - if it's not in the closed list 
// already
//=============================================================================
void PL_SimplexSubdivision::AddToOpenList( const int nodeNum )
{
        //
        // is the node in the closed list
        //
        //if( m_ClosedList.find( nodeNum ) == m_ClosedList.end() )
        //{
        //      return;
        //}
        //else
        {
                m_OpenList.insert( nodeNum );
        }
}


//=============================================================================
// CollisionCheck
//
// description: checks two nodes for collisions between them
//=============================================================================
PL_SimplexSubdivision::CollisionType PL_SimplexSubdivision::CollisionCheck( const int n0, const int n1 )
{
        Node& node0 = m_Nodes[ n0 ];
        Node& node1 = m_Nodes[ n1 ];

        //
        // figure out the index to the neighbors for each of the nodes
        //

        int n0Neighbor = -1;
        int n1Neighbor = -1;

        int i;
        int size = node0.m_NodesAdjacent.size();
        for( i = 0; i < size; i++ )
        {
                if( node0.m_NodesAdjacent[ i ] == n1 )
                {
                        n0Neighbor = i;
                        break;
                }
        }
        assert( n0Neighbor != -1 );

        size = node1.m_NodesAdjacent.size();
        for( i = 0; i < size; i++ )
        {
                if( node1.m_NodesAdjacent[ i ] == n0 )
                {
                        n1Neighbor = i;
                        break;
                }
        }
        assert( n1Neighbor != -1 );

        //
        // Has the collision check already been done?
        //
        if( node0.m_LinkType[ n0Neighbor ] != CT_UNKNOWN )
        {
                return node0.m_LinkType[ n0Neighbor ];
        }

        //
        // perform collision detection
        //

        Configuration c0 = node0.Center();
        Configuration c1 = node1.Center();
        bool collision = collisionDetector->IsInterfering( c0 );
        collision = ( collision || collisionDetector->IsInterfering( c1 ) );
        collision = ( collision || collisionDetector->IsInterferingLinear( c0, c1 ) );
        CollisionType ct;
        if( collision )
        {
                ct = CT_COLLISION;

                //get the location of the collision
                Configuration c = collisionDetector->GetLastValid();
                bool n0contains = node0.ContainsCompletely( c );
                bool n1contains = node1.ContainsCompletely( c );

                if( n0contains && !n1contains )
                {
                        double minx = node0.m_Min[ 0 ];
                        double miny = node0.m_Min[ 1 ];
                        double maxx = node0.m_Max[ 0 ];
                        double maxy = node0.m_Max[ 1 ];
                        double cx = c[ 0 ];
                        double cy = c[ 1 ];
                        node0.m_LocationOfCollision = c;
                }
                else if( n1contains && !n0contains )
                {
                        node1.m_LocationOfCollision = c;
                }
                else
                {
                        //assert( false );
                }
        }
        else
        {
                ct = CT_FREE;
        }
        node0.m_LinkType[ n0Neighbor ] = ct;
        node1.m_LinkType[ n1Neighbor ] = ct;
        return ct;
}

//=============================================================================
// ConnectStartAndGoal
//
// description: connects the start and the goal to the tree
//=============================================================================
void PL_SimplexSubdivision::ConnectStartAndGoal()
{
        //
        // Connect the start
        //
        bool collision = true;
        if( collision )
        {
                Configuration startNode = m_Nodes[ m_NodeStart ].Center();
                collision = collisionDetector->IsInterferingLinear( GetStartConfig(), startNode );
                if( collision )
                {
                        SubdivideNode( m_NodeStart );
                }
                else
                {
                        m_Nodes[ m_NodeStart ].m_DistanceFromStart = 0;
                }
        }

        //
        // Connect the goal
        //
        collision = true;
        if( collision )
        {
                Configuration goalNode = m_Nodes[ m_NodeGoal ].Center();
                collision = collisionDetector->IsInterferingLinear( GetGoalConfig(), goalNode );
                if( collision )
                {
                        SubdivideNode( m_NodeGoal );
                }
        }
}


//=============================================================================
// DeleteEdge
//
// description: deletes an already existing edge
//=============================================================================
void PL_SimplexSubdivision::DeleteEdge( const int node0, const int node1 )
{
        Node& n0 = m_Nodes[ node0 ];
        Node& n1 = m_Nodes[ node1 ];
        n0.RemoveNeighbor( node1 );
        n1.RemoveNeighbor( node0 );
}

//=============================================================================
// DeleteNode
//
// description: deletes an already existing node
//=============================================================================
void PL_SimplexSubdivision::DeleteNode( const int nodeNum )
{
        Node& node = m_Nodes[ nodeNum ];
        node.m_Alive = false;

        //delete all the links associated with this node
        int i;
        int size = node.m_NodesAdjacent.size();
        for( i = size - 1; i >= 0; i-- )
        {
                int neighbor = node.m_NodesAdjacent[ i ];
                DeleteEdge( nodeNum, neighbor );
        }

        m_OpenList.erase( nodeNum );
        m_ClosedList.erase( nodeNum );
}

//=============================================================================
// DrawExplicit
//
// description: Draws the current snapshot of the planner
//=============================================================================
bool PL_SimplexSubdivision::DrawExplicit () const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

        //
        // Disable lighting and depth test
        //
        glDisable( GL_LIGHTING );
        glDisable( GL_DEPTH_TEST );

        int i = 0;
        glColor4d( 1.0, 1.0, 1.0, 1.0 );
        glPointSize( 3.0 );

        int dof = GetGoalConfig().DOF();

        //draw all the nodes
        int size = m_Nodes.size();
        for( i = 0; i < size; i++ )
        {
                //
                // draw an individual node
                //
                const Node& node = m_Nodes[ i ];
                node.DrawExplicit();
        }
        
        //
        // Draw all the connections
        //

        //
        // Connections
        //
        glBegin( GL_LINES );
        glLineWidth( 2.0 );

        int n;
        int nSize = m_Nodes.size();
        for( n = 0; n < nSize; n++ )
        {
                const Node& me = m_Nodes[ n ];
                if( !me.m_Alive )
                {
                        continue;
                }
                int size = me.m_NodesAdjacent.size();
                for( i = 0; i < size; i++ )
                {
                        int neighborInt = me.m_NodesAdjacent[ i ];
                        assert( neighborInt >= 0 );
                        CollisionType ct = me.m_LinkType[ i ];
                        switch( ct )
                        {
                        case CT_FREE:
                                {
                                        glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );
                                        break;
                                }
                        case CT_COLLISION:
                                {
                                        glColor4f( 1.0f, 0.0f, 0.0f, 1.0f );
                                        break;
                                }
                        default:
                                {
                                        glColor4f( 0.5f, 0.5f, 0.5f, 0.3f );
                                        break;
                                }
                        }
                        const Node& neighborNode = m_Nodes[ neighborInt ];
                        Configuration neighbor = neighborNode.Center();
                        Configuration myC = me.Center();
                        double mX = myC[ 0 ];
                        double mY = myC[ 1 ];
                        double nX = neighbor[ 0 ];
                        double nY = neighbor[ 1 ];
                        glVertex3d( mX, mY, 0 );
                        glVertex3d( nX, nY, 0 );
                }
        }
        glEnd();

        //
        // draw the start/goal
        //
        glColor3f( 1.0f, 1.0f, 1.0f );
        glBegin( GL_POINTS );
                double n0 = 0;
                double n1 = 0;
                double n2 = 0;
                if( dof > 0 )
                {
                        n0 = GetGoalConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        n1 = GetGoalConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        n2 = GetGoalConfig()[ 2 ];
                }
                glVertex3f( n0, n1, n2 );
                if( dof > 0 )
                {
                        n0 = GetStartConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        n1 = GetStartConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        n2 = GetStartConfig()[ 2 ];
                }
                glVertex3f( n0, n1, n2 );
        glEnd();

        semaphore.Unlock();
        return true;
}

//=============================================================================
// IsAdjacent
//
// description: test if two nodes are adjacent 
//=============================================================================
bool PL_SimplexSubdivision::IsAdjacent( int node0, int node1 )
{
        int overlapping = 0;
        int adjacent = 0;

        Node& n0 = m_Nodes[ node0 ];
        Node& n1 = m_Nodes[ node1 ];
        int i;
        int size = n0.m_Max.DOF();
        for( i = 0; i < size; i++ )
        {
                double aMin = n0.m_Min[ i ];
                double aMax = n0.m_Max[ i ];
                double bMin = n1.m_Min[ i ];
                double bMax = n1.m_Max[ i ];

                if( ( aMin <= bMin ) && ( aMax > bMin ) )
                {
                        overlapping++;
                        continue;
                }
                if( ( bMin <= aMin ) && ( bMax > aMin ) )
                {
                        overlapping++;
                        continue;
                }
                if( ( aMax == bMin ) || ( bMax == aMin ) )
                {
                        adjacent++;
                        continue;
                }
        }
        if( ( adjacent == 1 ) && ( overlapping + adjacent == size ) )
        {
                return true;
        }
        return false;
}

//=============================================================================
// Plan
//
// description: Does the planning
//=============================================================================
bool PL_SimplexSubdivision::Plan ()
{
        // Start the timer
        StartTimer();

        m_Nodes.clear();

        //
        // Set up min and max configurations
        //
        Configuration min;
        Configuration max;
        min = GetStartConfig();
        max = GetStartConfig();
        int i;
        int size = min.DOF();
        for( i = 0; i < size; i++ )
        {
                min[ i ] = collisionDetector->JointMin( i );
                max[ i ] = collisionDetector->JointMax( i );
        }

        //
        // Add the 0th node
        //
        int firstNode = AddNode( min, max );
        m_NodeStart = firstNode;
        m_NodeGoal = firstNode;

        //
        // Connect the start and the goal to the 0th node
        //
        ConnectStartAndGoal();

        // Add the start node to the open list
        AddToOpenList( m_NodeStart );

        bool foundGoal = SearchForGoal();
        Semaphore semaphore( this->guid );
        //while( !foundGoal && !( HasTimeLimitExpired() ) )
        while( !foundGoal )
        {
                if( HasTimeLimitExpired() )
                {
                        return false;
                }
                if( this->m_UseSemaphores )
                {
                        semaphore.Lock();
                }
                SubdivideLargestNode();
                foundGoal = SearchForGoal();

                if( this->m_UseSemaphores )
                {
                        semaphore.Unlock();
                }
        }

        //
        // Walk backwards from the goal to the start to get the path
        //
        /*
        path.Clear();
        path.AppendPointToBeginning( goalConfig );
        int currentNodeInt = m_NodeGoal;
        while( currentNodeInt != m_NodeStart )
        {
                Node& currentNode = m_Nodes[ currentNodeInt ];
                int bestNeighbor = currentNode.m_NodesAdjacent[ -1 ];
                double bestNeighborValue = 9999999;

                //
                // found the next step in the path
                //
                path.AppendPointToBeginning( currentNode.Center() );

                int i;
                int size = currentNode.m_NodesAdjacent.size();
                for( i = 0; i < size; i++ )
                {
                        int neighborInt = currentNode.m_NodesAdjacent[ i ];
                        CollisionType ct = currentNode.m_LinkType[ i ];
                        if( ct == CT_FREE )
                        {
                                Node& neighbor = m_Nodes[ neighborInt ];
                                if( neighbor.m_DistanceFromStart < bestNeighborValue )
                                {
                                        bestNeighborValue = neighbor.m_DistanceFromStart;
                                        bestNeighbor = neighborInt;
                                }
                        }
                }
                assert( bestNeighbor != 1 );
                currentNodeInt = bestNeighbor;
        }

        //
        // Add the start point to the path
        //
        Node& currentNode = m_Nodes[ currentNodeInt ];
        path.AppendPointToBeginning( currentNode.Center() );
        path.AppendPointToBeginning( startConfig );
        */
        //StopTimer();
        return true;
}

//=============================================================================
// SearchForGoal
//
// description: searches for the goal until no nodes remain on the open list
//=============================================================================
bool PL_SimplexSubdivision::SearchForGoal()
{
        //
        // Take the top node off the open list + try to expand it
        //
        while( m_OpenList.size() != 0 )
        {
                int nodesRemainingInOpenList = m_OpenList.size();
                int nodeToExpand = *( m_OpenList.begin() );
                m_OpenList.erase( m_OpenList.begin() );
                Node& n = m_Nodes[ nodeToExpand ];

                //check if any of the neighbors of this node can reach the start
                int i;
                int size = n.m_NodesAdjacent.size();
                for( i = 0; i < size; i++ )
                {
                        int neighbor = n.m_NodesAdjacent[ i ];
                        Node& neighborNode = m_Nodes[ neighbor ];
                        if( neighborNode.m_DistanceFromStart >= n.m_DistanceFromStart )
                        {
                                continue;
                        }
                        m_ClosedList.insert( nodeToExpand );

                        //test the link for collisions
                        PL_SimplexSubdivision::CollisionType ct = CollisionCheck( nodeToExpand, neighbor );
                        if( ct == CT_FREE )
                        {
                                //
                                // Update the distance from the start
                                //
                                double distance = ( n.Center() - neighborNode.Center() ).Magnitude();
                                double newDistance = neighborNode.m_DistanceFromStart + distance;
                                if( newDistance >= n.m_DistanceFromStart )
                                {
                                        continue;
                                }
                                n.m_DistanceFromStart = newDistance;

                                if( nodeToExpand == m_NodeGoal )
                                {
                                        return true;
                                }

                                //
                                // Add all the neighbors to the open list
                                //
                                int j;
                                int size = n.m_NodesAdjacent.size();
                                for( j = 0; j < size; j++ )
                                {
                                        int neighbor = n.m_NodesAdjacent[ j ];
                                        AddToOpenList( neighbor );
                                }
                        }
                        else
                        {
                                Configuration collision = collisionDetector->GetLastIntersection();
                                Node& n0 = m_Nodes[ neighbor ];
                                Node& n1 = m_Nodes[ nodeToExpand ];
                                bool n0contains = n0.Contains( collision );
                                bool n1contains = n0.Contains( collision );
                                if( n0contains )
                                {
                                        AddSuspiciousNode( neighbor );
                                }
                                if( n1contains )
                                {
                                        AddSuspiciousNode( nodeToExpand );
                                }
                                if( !n0contains && !n1contains )
                                {
                                        AddSuspiciousNode( neighbor );
                                        AddSuspiciousNode( nodeToExpand );
                                }
                        }
                }
        }
        return false;
}

//=============================================================================
// SubdivideLargestNode
//
// description: Subdivides the largest node in the suspicious list
//=============================================================================
void PL_SimplexSubdivision::SubdivideLargestNode()
{
        //
        // Are there any suspicious nodes?
        //
        int size = m_SuspiciousNodesAndVolumes.size();
        assert( size != 0 );

        INTSET::value_type value = *( m_SuspiciousNodesAndVolumes.begin() );
        m_SuspiciousNodesAndVolumes.erase( m_SuspiciousNodesAndVolumes.begin() );
        int mostSuspiciousNode = value;
        double howBigWasIt = Volume( mostSuspiciousNode );
        SubdivideNode( mostSuspiciousNode );
}

//=============================================================================
// SubdivideNode
//
// description: Subdivides a specific node along the longest side
//=============================================================================
void PL_SimplexSubdivision::SubdivideNode( int nodeNum )
{
        Node subdivideMe = m_Nodes[ nodeNum ];
        Configuration min = subdivideMe.m_Min;
        Configuration max = subdivideMe.m_Max;

        //
        // Figure out the longest edge of the node
        //
        int longestDimension = 0;
        double longestSize = 0;
        int i;
        int size = subdivideMe.m_Min.Length();
        for( i = 0; i < size; i++ )
        {
                double size = max[ i ] - min[ i ];
                if( size > longestSize )
                {
                        longestSize = size; 
                        longestDimension = i;
                }
        }

        double maxDim = max[ longestDimension ];
        double minDim = min[ longestDimension ];
        double halfWayPoint = subdivideMe.m_LocationOfCollision[ longestDimension ];
        Configuration minHi = max;
        minHi[ longestDimension ] = halfWayPoint;
        Configuration maxLo = min;
        maxLo[ longestDimension ] = halfWayPoint;
        
        int loNumber = AddNode( min, minHi );
        int hiNumber = AddNode( maxLo, max );
        AddEdge( loNumber, hiNumber );

        size = subdivideMe.m_NodesAdjacent.size();
        assert( size >= 0 );
        for( i = 0; i < size; i++ )
        {
                int originalNeighbor = subdivideMe.m_NodesAdjacent[ i ];
                AddEdge( loNumber, originalNeighbor );
                AddEdge( hiNumber, originalNeighbor );
        }

        //
        // if this was the start node or the goal node, there is more to do
        //
        bool needReconnect = false;
        if( nodeNum == m_NodeStart )
        {
                Node& a = m_Nodes[ loNumber ];
                Node& b = m_Nodes[ hiNumber ];
                if( a.Contains( GetStartConfig() ) )
                {
                        m_NodeStart = loNumber;
                }
                else
                {
                        m_NodeStart = hiNumber;
                        assert( b.Contains( GetStartConfig() ) );
                }
                needReconnect = true;
        }

        if( nodeNum == m_NodeGoal )
        {
                Node& a = m_Nodes[ loNumber ];
                Node& b = m_Nodes[ hiNumber ];
                if( a.Contains( GetGoalConfig()) )
                {
                        m_NodeGoal = loNumber;
                }
                else
                {
                        m_NodeGoal = hiNumber;
                        assert( b.Contains( GetGoalConfig() ) );
                }
                needReconnect = true;
        }
        DeleteNode( nodeNum );
        if( needReconnect )
        {
                ConnectStartAndGoal();
        }
}

//=============================================================================
// Volume
//
// description: returns the volume of a node
//=============================================================================
double PL_SimplexSubdivision::Volume( const int i ) const
{
        return m_Nodes[ i ].Volume();
}


//=============================================================================
// AddNeighbor
//
// description: Adds a neighbor to a node
//=============================================================================
void PL_SimplexSubdivision::Node::AddNeighbor( const int node )
{
        //
        // check if the neighbor already exists
        //
        //
        // find the neighbor
        //
        std::vector< int >::iterator it;
        for( it = m_NodesAdjacent.begin(); it != m_NodesAdjacent.end(); it++ )
        {
                if( *it == node )
                {
                        return;
                }
        }


        m_NodesAdjacent.push_back( node );
        m_LinkType.push_back( CT_UNKNOWN );
}

//=============================================================================
// Center
//
// description: gets the center of the node
//=============================================================================
Configuration PL_SimplexSubdivision::Node::Center() const
{
        assert( m_Alive );
        return ( m_Max + m_Min ) / 2;
}

//=============================================================================
// Contains
//
// description: does the node contain this point?
//=============================================================================
bool PL_SimplexSubdivision::Node::Contains( const Configuration& c ) const
{
        bool returnMe = true;
        int i;
        int size = m_Min.Length();
        for( i = 0; i < size; i++ )
        {
                if( m_Min[ i ] > c[ i ] )
                {
                        return false;
                }

                if( m_Max[ i ] < c[ i ] )
                {
                        return false;
                }
        }
        return true;
}

//=============================================================================
// ContainsCompletely
//
// description: does the node contain this point?
//=============================================================================
bool PL_SimplexSubdivision::Node::ContainsCompletely( const Configuration& c ) const
{
        bool returnMe = true;
        int i;
        int size = m_Min.Length();
        for( i = 0; i < size; i++ )
        {
                if( m_Min[ i ] >= c[ i ] )
                {
                        return false;
                }

                if( m_Max[ i ] <= c[ i ] )
                {
                        return false;
                }
        }
        return true;
}

//=============================================================================
// DrawExplicit
//
// description: Draws the node to the screen using OpenGL
//=============================================================================
void PL_SimplexSubdivision::Node::DrawExplicit() const
{
        if( !m_Alive )
        {
                return;
        }

        ::glLineWidth( 1.0 );

        double xMin = m_Min[ 0 ];
        double yMin = m_Min[ 1 ];
        double xMax = m_Max[ 0 ];
        double yMax = m_Max[ 1 ];

        GlColor();

        glBegin( GL_QUADS );
                glVertex3d( xMin, yMin, 0 );
                glVertex3d( xMax, yMin, 0 );
                glVertex3d( xMax, yMax, 0 );
                glVertex3d( xMin, yMax, 0 );
        glEnd();

        glBegin( GL_LINE_LOOP );
                glVertex3d( xMin, yMin, 0 );
                glVertex3d( xMax, yMin, 0 );
                glVertex3d( xMax, yMax, 0 );
                glVertex3d( xMin, yMax, 0 );
        glEnd();

        glBegin( GL_POINTS );
                glVertex3d( xMin, yMin, 0 );
                glVertex3d( xMax, yMin, 0 );
                glVertex3d( xMax, yMax, 0 );
                glVertex3d( xMin, yMax, 0 );
        glEnd();
}

//=============================================================================
// GlColor
//
// description: sets the color of this node
//=============================================================================
void PL_SimplexSubdivision::Node::GlColor() const
{
        bool isUnknown = true;
        bool isFree = true;
        bool isColliding = true;
        int i;
        int size = m_LinkType.size();
        for( i = 0; i < size; i++ )
        {
                if( m_LinkType[ i ] == CT_COLLISION )
                {
                        isFree = false;
                }

                if( m_LinkType[ i ] != CT_UNKNOWN )
                {
                        isUnknown = false;
                }

                if( m_LinkType[ i ] == CT_FREE )
                {
                        isColliding = false;
                }
        }

        if( isUnknown )
        {
                ::glColor4d( 1.0, 1.0, 1.0, 0.3 );
        }
        else if( isFree )
        {
                ::glColor4d( 0.0, 1.0, 0.0, 0.3 );
        }
        else if( isColliding )
        {
                ::glColor4d( 1.0, 0.0, 0.0, 0.3 );
        }
        else
        {
                ::glColor4d( 1.0, 1.0, 0.0, 0.3 );
        }
}


//=============================================================================
// IsClean
//
// description: determines if the node is free of any colliding neighbors
//=============================================================================
bool PL_SimplexSubdivision::Node::IsClean() const
{
        int i;
        int size = m_LinkType.size();
        for( i = 0; i < size; i++ )
        {
                if( m_LinkType[ i ] != CT_FREE )
                {
                        return false;
                }
        }
        return true;
}


//=============================================================================
// RemoveNeighbor
//
// description: removes a neighbor
//=============================================================================
void PL_SimplexSubdivision::Node::RemoveNeighbor( const int node )
{
        //
        // find the neighbor
        //
        std::vector< int >::iterator it;
        std::vector< CollisionType >::iterator ctIt = m_LinkType.begin();
        for( it = m_NodesAdjacent.begin(); it != m_NodesAdjacent.end(); it++, ctIt++ )
        {
                if( *it == node )
                {
                        m_NodesAdjacent.erase( it );
                        m_LinkType.erase( ctIt );
                        return;
                }
        }

        //assert( false );
        //
        // the neighbor wasn't there to begin with
        //
}

//=============================================================================
// Volume
//
// description: returns the size of the node
//=============================================================================
double PL_SimplexSubdivision::Node::Volume() const
{
        double runningTotal = 1.0;
        int i;
        int size = m_Max.Length();
        for( i = 0; i < size; i++ )
        {
                double length = m_Max[ i ] - m_Min[ i ];
                runningTotal *= length;
        }
        return runningTotal;
}

//=============================================================================
// PL_SimplexSubdivision::ComparisonFunction::ComparisonFunction();
//
// description: constructor
//=============================================================================
PL_SimplexSubdivision::ComparisonFunction::ComparisonFunction( const PL_SimplexSubdivision* planner  )
:
        m_Planner( planner )
{
}

//=============================================================================
// PL_SimplexSubdivision::ComparisonFunction::ComparisonFunction( const ComparisonFunction& right )
//
// description: Copy constructor
//=============================================================================
PL_SimplexSubdivision::ComparisonFunction::ComparisonFunction( const ComparisonFunction& right )
{
        m_Planner = right.m_Planner;
}

//=============================================================================
// PL_SimplexSubdivision::ComparisonFunction::ComparisonFunction
// ( 
//    const PL_SimplexSubdivision* planner 
// )
//
// description: compares two nodes, and sorts based on volume
//=============================================================================
bool PL_SimplexSubdivision::ComparisonFunction::operator()
( 
        const int a, 
        const int b 
)

{
        assert( m_Planner != NULL );
        double volA = m_Planner->Volume( a );
        double volB = m_Planner->Volume( b );

        //yeah, i know this looks backwards, i want the sort in that order
        if( volA > volB ) return true;
        if( volB > volA ) return false;
        if( a > b ) return true;
        return false;
}

---