TAPsBVHTree.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsBVHTree.cpp

BVHTree class is a class for any Boundary Volume Hierarchy Tree.

SUKITTI PUNAK   (01/17/2006)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsBVHTree.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs
//=============================================================================
// Constructor and Destructor
//-----------------------------------------------------------------------------
template <typename T>
BVHTree<T>::BVHTree ( 
    TransformationSupport<T> & transform, 
    Enum::CD type, 
    int numOfNodes, 
    BVHNode<T> * rootNode )
    : m_eType( type ), 
      m_iTotalNodes( numOfNodes ), 
      m_pRoot( rootNode ), 
      m_Transform( transform )
{}
//-----------------------------------------------------------------------------
template <typename T>
BVHTree<T>::~BVHTree ()
{
    if ( m_pRoot ) {
        delete m_pRoot;
        m_pRoot = NULL;
    }
}
//-----------------------------------------------------------------------------




#ifdef  TAPs_DEBUG_COLLISION_DETECTION
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::ClearFlags ()
{
    if ( m_pRoot == NULL )  return;
    //---------------------------------------------------------------
    ClearFlags( m_pRoot );
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::ClearFlags ( BVHNode<T> * const node )
{
    node->flag = false;
    //---------------------------------------------------------------
    for ( int i = 0; i < node->GetNumOfChildren(); ++i ) {
        if ( node->Child(i) != NULL ) {
            ClearFlags( node->Child(i) );
        }
    }
}
//-----------------------------------------------------------------------------
#endif//TAPs_DEBUG_COLLISION_DETECTION




//=============================================================================
// Tree Operations
//-----------------------------------------------------------------------------
// CountNodes
template <typename T>
int BVHTree<T>::CountNodes () const
{
    if ( m_pRoot == NULL )      return 0;
    //---------------------------------------------------------------
    return CountNodes( m_pRoot );
}
//-----------------------------------------------------------------------------
// CountNodes
template <typename T>
int BVHTree<T>::CountNodes ( BVHNode<T> const * const node ) const
{
    //---------------------------------------------------------------
    int numOfNodes = 1;
    for ( int i = 0; i < node->GetNumOfChildren(); ++i ) {
        if ( node->Child(i) != NULL ) {
            numOfNodes += CountNodes( node->Child(i) );
        }
    }
    return numOfNodes;
}
//-----------------------------------------------------------------------------
// CountLeafNodes
template <typename T>
int BVHTree<T>::CountLeafNodes () const
{
    if ( m_pRoot == NULL )      return 0;
    //---------------------------------------------------------------
    return CountLeafNodes( m_pRoot );
}
//-----------------------------------------------------------------------------
// CountLeafNodes
template <typename T>
int BVHTree<T>::CountLeafNodes ( BVHNode<T> const * const node ) const
{
    //if ( node->listHEFace.size() == 1 ) {
    if ( node->IsLeaf() ) {
        return 1;
    }
    //---------------------------------------------------------------
    int numOfLeafNodes = 0;
    for ( int i = 0; i < node->GetNumOfChildren(); ++i ) {
        if ( node->Child(i) != NULL ) {
            numOfLeafNodes += CountLeafNodes( node->Child(i) );
        }
    }
    return numOfLeafNodes;
}
//-----------------------------------------------------------------------------
// FindListOfLeafNodes
template <typename T>
void BVHTree<T>::FindListOfLeafNodes ( 
        BVHNode<T> * node, std::set< BVHNode<T> * > & leafNodeSet )
{
    if ( node->IsLeaf() ) {
        leafNodeSet.insert( node );
        return;
    }
    //---------------------------------------------------------------
    for ( int i = 0; i < node->GetNumOfChildren(); ++i ) {
        if ( node->Child(i) != NULL ) {
            FindListOfLeafNodes( node->Child(i), leafNodeSet );
        }
    }
}
//-----------------------------------------------------------------------------
// Display all nodes as string
template <typename T>
void BVHTree<T>::DisplayAllNodesAsString ( BVHNode<T> * node, std::string & str, int level ) const
{
    std::stringstream ss;
    //---------------------------------------------------------------
    for ( int i = 0; i < node->GetNumOfChildren(); ++i ) {
        if ( node->Child(i) != NULL ) {
            DisplayAllNodesAsString( node->Child(i), str, level+1 );
        }
    }
    ss << "[Level "<<level<<"]" << "\tID: " << node->GetID();
    if ( node->IsLeaf() ) {
        ss << "\ta leaf node";
    }
    ss << "\n";
    str += ss.str();
}
//-----------------------------------------------------------------------------
// Update
template <typename T>
void BVHTree<T>::Update ()
{
    //std::cout << "BVHTree<T>::Update()\n";

    std::set< BVHNode<T> * > leafNodeSet;
    if ( !m_pRoot ) return;
    //FindListOfLeafNodes( m_pRoot, leafNodeSet );
    //Update( leafNodeSet );
    Update( GetListOfLeafNodes() );
}
//-----------------------------------------------------------------------------
// Update
template <typename T>
void BVHTree<T>::Update ( std::set< BVHNode<T> * > & nodeSet )
{
    if ( nodeSet.empty() )  return;
    std::set< BVHNode<T> * >::iterator nodeSetIterator;
    std::set< BVHNode<T> * > parentNodeSet;
    for ( nodeSetIterator = nodeSet.begin(); nodeSetIterator != nodeSet.end(); ++nodeSetIterator ) {
        (*nodeSetIterator)->Update();
        if ( (*nodeSetIterator)->Parent() ) {
            parentNodeSet.insert( (*nodeSetIterator)->Parent() );
        }
    }
    Update( parentNodeSet );
}
//-----------------------------------------------------------------------------


/*
//=============================================================================
// CALCULATE AND SAVE TRANSFORMATIONS
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::CalAndSaveTransformations ( 
    BoundingVolume<T> const * const pBV )
{
    // m_FwdTransformMatrix[0]
    m_FwdTransformMatrix[0].MakeIdentity();

    // m_FwdTransformMatrix[1]
    m_FwdTransformMatrix[1] = pBV->GetTransform().GetMatrixTransform();

    // Inversed Matrices
    m_InvTransformMatrix[0].MakeIdentity();
    m_InvTransformMatrix[1] = m_FwdTransformMatrix[1].GetInverse();
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::CalAndSaveTransformations ( 
    BoundingVolume<T> const * const pBV, 
    Matrix4x4<T> const & transformB )
{
    // m_FwdTransformMatrix[0]
    m_FwdTransformMatrix[0].MakeIdentity();

    // m_FwdTransformMatrix[1]
    m_FwdTransformMatrix[1] = pBV->GetTransform().GetMatrixTransform();

    // Inversed Matrices
    m_InvTransformMatrix[0].MakeIdentity();
    m_InvTransformMatrix[1] = m_FwdTransformMatrix[1].GetInverse();
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::CalAndSaveTransformations ( 
    Matrix4x4<T> const & transformA, 
    BoundingVolume<T> const * const pBV )
{
    // m_FwdTransformMatrix[0]
    m_FwdTransformMatrix[0].MakeIdentity();

    // m_FwdTransformMatrix[1]
    m_FwdTransformMatrix[1] = pBV->GetTransform().GetMatrixTransform();

    // Inversed Matrices
    m_InvTransformMatrix[0] = transformA.GetInverse();
    m_InvTransformMatrix[1] = m_FwdTransformMatrix[1].GetInverse();
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::CalAndSaveTransformations ( 
    Matrix4x4<T> const & transformA, 
    BoundingVolume<T> const * const pBV, 
    Matrix4x4<T> const & transformB )
{
    // m_FwdTransformMatrix[0]
    m_FwdTransformMatrix[0] = transformB;

    // m_FwdTransformMatrix[1]
    m_FwdTransformMatrix[1] = m_FwdTransformMatrix[0] * pBV->GetTransform().GetMatrixTransform();

    // Inversed Matrices
    m_InvTransformMatrix[0] = transformA.GetInverse();
    m_InvTransformMatrix[1] = m_FwdTransformMatrix[1].GetInverse();
}
//-----------------------------------------------------------------------------
// CALCULATE AND SAVE TRANSFORMATIONS
//=============================================================================
//*/



//*
//=============================================================================
// BINARY AABB BOUNDING VOLUME HIERARCHY TREE
//-----------------------------------------------------------------------------
template <typename T>
BVHTree<T> * BVHTree<T>::BAABBBuild ( 
        TransformationSupport<T> & transform,   // I/O: Transform
        const std::list< HEFace<T> * > & heFaceList,    // I/P half-edge face list
        BVHTree<T> * cdtree )           // O/P pointer to BVHTree
{
    //std::cout << "BAABBBuild (before): " << cdtree << std::endl;
    BVHNode<T> *node = CreateBVHNodeBAABB( NULL, heFaceList );
    cdtree = new BVHTree<T>( transform, Enum::BVH_TREE_BINARY_AABB, 1, node );
    //FindOBBOfFaceList( heFaceList, node );
    Vector3<T> U, V, W;
    FindOBBOfFaceList( heFaceList, node->GetCenter(), U, V, W );
//  node->listHEFace  = heFaceList;         // DEBUG
    if ( node ) BAABBBuildChildren( node, heFaceList, U );
    //std::cout << "BAABBBuild (after): " << cdtree << std::endl;
    return cdtree;
}
//-----------------------------------------------------------------------------
// Create binary child AABBs
template <typename T>
void BVHTree<T>::BAABBBuildChildren ( 
    BVHNode<T> * pParentNode, 
    const std::list< HEFace<T> * > & heFaceList, 
    Vector3<T> const & principleVector )
{
    int size = static_cast<int>( heFaceList.size() );
    std::list< HEFace<T> * > leftGroup, rightGroup;
    //---------------------------------------------------------------
    assert( pParentNode );          // ASSERT
    BVHNode<T> * leftChild  = NULL;
    BVHNode<T> * rightChild = NULL;
    //---------------------------------------------------------------
    // CASE: one face
    // This is a leaf node.
    assert( size > 0 );         // ASSERT
    if ( 1 == size ) {
        return;
    }
    //---------------------------------------------------------------
    // CASE: two faces
    // Create Two Leaf Nodes
    if ( 2 == size ) {
        //-------------------------------------------------
        // Separate two faces into left and right groups
        std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
        leftGroup.push_back( *face );
        ++face;
        rightGroup.push_back( *face );
        //-------------------------------------------------
        // Create Left Leaf Node
        leftChild  = CreateBVHNodeBAABB( pParentNode, leftGroup );
//      leftChild->listHEFace  = leftGroup;         // DEBUG
        pParentNode->Child( 0, leftChild );
        //-------------------------------------------------
        // Create Right Leaf Node
        rightChild = CreateBVHNodeBAABB( pParentNode, rightGroup );
//      rightChild->listHEFace = rightGroup;        // DEBUG
        pParentNode->Child( 1, rightChild );
        return;
    }
    //---------------------------------------------------------------
    // CASE: more than two faces
    // Didive into two groups
    BAABBDivideToFaceGroups(    heFaceList, leftGroup, rightGroup, 
                                pParentNode->GetCenter(), 
                                principleVector );
    //---------------------------------------------------------------
    // CASE: more than two faces and the divider failed to divide the face list
    if (   static_cast<int>( leftGroup.size() )  == size 
        || static_cast<int>( rightGroup.size() ) == size ) {
        //std::cout << "Total    : " << size;
        //std::cout << "; Left  Grp: " << leftGroup.size();
        //std::cout << "; Right Grp: " << rightGroup.size() << std::endl;
        leftGroup.clear();
        rightGroup.clear();
        std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
        int i = 0;
        for ( ; i < size/2; ++i ) {
            leftGroup.push_back( *face );
            ++face;
        }
        for ( ; i < size; ++i ) {
            rightGroup.push_back( *face );
            ++face;
        }
    }
    Vector3<T> U, V, W;
    //---------------------------------------------------------------
    // Create Left Child Node
    leftChild  = CreateBVHNodeBAABB( pParentNode, leftGroup );
    FindOBBOfFaceList( leftGroup, leftChild->GetCenter(), U, V, W );
//  leftChild->listHEFace  = leftGroup;         // DEBUG
    pParentNode->Child( 0, leftChild );
    BAABBBuildChildren( leftChild, leftGroup, U );
    //---------------------------------------------------------------
    // Create Right Child Node
    rightChild = CreateBVHNodeBAABB( pParentNode, rightGroup );
    FindOBBOfFaceList( rightGroup, rightChild->GetCenter(), U, V, W );
//  rightChild->listHEFace = rightGroup;        // DEBUG
    pParentNode->Child( 1, rightChild );
    BAABBBuildChildren( rightChild, rightGroup, U );

    /*
    int size = static_cast<int>( heFaceList.size() );
    std::list< HEFace<T> * > leftGroup, rightGroup;
    //---------------------------------------------------------------
    // Didive into two groups
    BAABBDivideToFaceGroups(    heFaceList, leftGroup, rightGroup, 
                                pParentNode->GetCenter(), 
                                principleVector );
    //---------------------------------------------------------------
    assert( pParentNode );          // ASSERT
    BVHNode<T> * leftChild = NULL;
    BVHNode<T> * rightChild = NULL;
    if ( 2 < static_cast<int>( leftGroup.size() ) && static_cast<int>( leftGroup.size() ) < size ) {
        leftChild  = CreateBVHNodeBAABB( pParentNode, leftGroup );
        //FindOBBOfFaceList( leftGroup, leftChild );
        Vector3<T> U, V, W;
        FindOBBOfFaceList( leftGroup, leftChild->GetCenter(), U, V, W );
        leftChild->listHEFace  = leftGroup;         // DEBUG
        pParentNode->Child( 0, leftChild );
        BAABBBuildChildren( leftChild, leftGroup, U );
    }
    if ( 2 < static_cast<int>( rightGroup.size() ) && static_cast<int>( rightGroup.size() ) < size ) {
        rightChild = CreateBVHNodeBAABB( pParentNode, rightGroup );
        //FindOBBOfFaceList( rightGroup, rightChild );
        Vector3<T> U, V, W;
        FindOBBOfFaceList( rightGroup, rightChild->GetCenter(), U, V, W );
        rightChild->listHEFace = rightGroup;        // DEBUG
        pParentNode->Child( 1, rightChild );
        BAABBBuildChildren( rightChild, rightGroup, U );
    }
    //*/
}
//-----------------------------------------------------------------------------
// Find the group that this face belongs to
template <typename T>
int BVHTree<T>::BAABBDetermineFaceGroup ( HEFace<T> * face, 
                                      const Vector3<T> & center, 
                                      const Vector3<T> & direction )
{
    //-------------------------------------------------------------------------
    // Divided into an axis positive and negative
    //---------------------------------------------------------------
    // REMARK:
    //   To speed thing up, we can evaluate only the first vertex of the polygon.
    //---------------------------------------------------------------
    // Determine from all vertices
    int value = 0;
    HEHalfEdge<T> * firstHalfEdge = face->IncidentHalfEdge();
    HEHalfEdge<T> * halfEdge = firstHalfEdge;
    Vector3<T> vertex;
    do {
        //vertex = halfEdge->Vertex()->GetPosition() - center;
        //if ( vertex.GetX() >= 0 ) ++value;
        //else                      --value;
        if ( (halfEdge->Vertex()->GetPosition() - center) * direction >= 0 )
            ++value;
        else
            --value;
        halfEdge = halfEdge->Next();
    } while ( halfEdge != firstHalfEdge );
    //---------------------------------------------------------------
    // Determine the group (left or right)
    return  value >= 0 ? 1 : 0;
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::BAABBDivideToFaceGroups ( 
                const std::list< HEFace<T> * > & inputFaces,    // input
                std::list< HEFace<T> * > & group1,              // output group1
                std::list< HEFace<T> * > & group2,              // output group2
                const Vector3<T> & center, 
                const Vector3<T> & direction )
{
    std::list< HEFace<T> * >::const_iterator face;
    for ( face = inputFaces.begin(); face != inputFaces.end(); ++face ) {
        if ( BAABBDetermineFaceGroup( *face, center, direction ) == 1 )
            group1.push_back( *face );
        else
            group2.push_back( *face );
    }
}
//-----------------------------------------------------------------------------
// Create a new sphere bounding volume node from the face list
template <typename T>
BVHNode<T> * BVHTree<T>::CreateBVHNodeBAABB ( 
                        BVHNode<T> * pParentNode, 
                        const std::list< HEFace<T> * > & heFaceList )
{
    //*
    Vector3<T> AABB[2];
    //HEVertex<T> * vertex = m_listVertex->Head();
    std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
    HEHalfEdge<T> * firstHalfEdge = (*face)->IncidentHalfEdge();
    HEHalfEdge<T> * halfEdge = firstHalfEdge;
    Vector3<T> vertex = halfEdge->Vertex()->GetPosition();
    AABB[0] = AABB[1] = vertex;
    //---------------------------------------------------------------
    // For Each Face
    for ( ; face != heFaceList.end(); ++face ) {
        halfEdge = firstHalfEdge = (*face)->IncidentHalfEdge();
        //-------------------------------------------------
        // For Each Vertex
        do {
            vertex = halfEdge->Vertex()->GetPosition();
            // Find the lowest an the highest of x, y, and z
            // AABB[0] is min and AABB[1] is max
            if      ( AABB[0][0] > vertex[0] )  AABB[0][0] = vertex[0];
            else if ( AABB[1][0] < vertex[0] )  AABB[1][0] = vertex[0];
            if      ( AABB[0][1] > vertex[1] )  AABB[0][1] = vertex[1];
            else if ( AABB[1][1] < vertex[1] )  AABB[1][1] = vertex[1];
            if      ( AABB[0][2] > vertex[2] )  AABB[0][2] = vertex[2];
            else if ( AABB[1][2] < vertex[2] )  AABB[1][2] = vertex[2];
            // Next vertex
            halfEdge = halfEdge->Next();
        } while ( halfEdge != firstHalfEdge );
    }
    /*
    //---------------------------------------------------------------
    // Find the bounding volume center from the AABB
    Vector3<T> vCenter( ( AABB[0][0] + AABB[1][0] ) / 2.0,
                        ( AABB[0][1] + AABB[1][1] ) / 2.0,
                        ( AABB[0][2] + AABB[1][2] ) / 2.0 );
    //*/
    /*
    //---------------------------------------------------------------
    // Find the Sphere Bounding Volume
    T radius = 0, squaredLength;
    for ( face = heFaceList.begin(); face != heFaceList.end(); ++face ) {
        halfEdge = firstHalfEdge = (*face)->IncidentHalfEdge();
        //-------------------------------------------------
        do {
            vertex = halfEdge->Vertex()->GetPosition();
            squaredLength = (vertex - vCenter).SquaredLength();
            if ( squaredLength > radius )   radius = squaredLength;
            halfEdge = halfEdge->Next();
        } while ( halfEdge != firstHalfEdge );
    }
    radius = sqrt( radius );
    //*/
    //---------------------------------------------------------------
    // Make the root node
    //-------------------------------------------
    // BVHNode Constructor
    //   BVHNode<T>(    Enum::CD type,
    //                  int id, 
    //                  BVHNode<T> * parent, 
    //                  BVHNode<T> ** children )
    //-------------------------------------------
    BVHNode<T> * node = new BVHNode<T>( Enum::BVH_NODE_BINARY_AABB, 
                                -1, pParentNode, NULL );
    //std::cout << node << "\n";
    //std::cout << node->GetCenter() << std::endl;
    //node->SetCenter( vCenter );
    node->SetMinPt( AABB[0] );
    node->SetMaxPt( AABB[1] );
    //std::cout << node->GetCenter() << std::endl;
    //node->SetRadius( radius );
    return node;
}
//*/
//-----------------------------------------------------------------------------
//=============================================================================










//*
//=============================================================================
// BINARY OBB BOUNDING VOLUME HIERARCHY TREE
//-----------------------------------------------------------------------------
template <typename T>
BVHTree<T> * BVHTree<T>::BOBBBuild ( 
        TransformationSupport<T> & transform,   // I/O: Transform
        const std::list< HEFace<T> * > & heFaceList,    // I/P half-edge face list
        BVHTree<T> * cdtree )           // O/P pointer to BVHTree
{
    //std::cout << "BOBBBuild (before): " << cdtree << std::endl;
    BVHNode<T> *node = CreateBVHNodeBOBB( NULL, heFaceList );
    cdtree = new BVHTree<T>( transform, Enum::BVH_TREE_BINARY_OBB, 1, node );
    //FindOBBOfFaceList( heFaceList, node );
    Vector3<T> U, V, W;
    FindOBBOfFaceList( heFaceList, node->GetCenter(), U, V, W );
    node->SetPrincipleComponents( U, V, W );
//  node->listHEFace  = heFaceList;         // DEBUG
    if ( node ) BOBBBuildChildren( node, heFaceList, U );
    //std::cout << "BOBBBuild (after): " << cdtree << std::endl;

    FindListOfLeafNodes();

    return cdtree;
}
//-----------------------------------------------------------------------------
// Create binary child OBBs
template <typename T>
void BVHTree<T>::BOBBBuildChildren ( 
    BVHNode<T> * pParentNode, 
    const std::list< HEFace<T> * > & heFaceList, 
    Vector3<T> const & principleVector )
{
    int size = static_cast<int>( heFaceList.size() );
    std::list< HEFace<T> * > leftGroup, rightGroup;
    //---------------------------------------------------------------
    assert( pParentNode );          // ASSERT
    BVHNode<T> * leftChild  = NULL;
    BVHNode<T> * rightChild = NULL;
    //---------------------------------------------------------------
    // CASE: one face
    // This is a leaf node.
    assert( size > 0 );         // ASSERT
    if ( 1 == size ) {
        return;
    }
    //---------------------------------------------------------------
    // CASE: two faces
    // Create Two Leaf Nodes
    if ( 2 == size ) {
        //-------------------------------------------------
        // Separate two faces into left and right groups
        std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
        leftGroup.push_back( *face );
        ++face;
        rightGroup.push_back( *face );
        //-------------------------------------------------
        // Create Left Leaf Node
        leftChild  = CreateBVHNodeBOBB( pParentNode, leftGroup );
//      leftChild->listHEFace  = leftGroup;         // DEBUG
        pParentNode->Child( 0, leftChild );
        //-------------------------------------------------
        // Create Right Leaf Node
        rightChild = CreateBVHNodeBOBB( pParentNode, rightGroup );
//      rightChild->listHEFace = rightGroup;        // DEBUG
        pParentNode->Child( 1, rightChild );
        return;
    }
    //---------------------------------------------------------------
    // Didive into two groups
    // CASE: more than two faces
    BOBBDivideToFaceGroups( heFaceList, leftGroup, rightGroup, 
                                pParentNode->GetCenter(), 
                                principleVector );
    //---------------------------------------------------------------
    // CASE: more than two faces and the divider failed to divide the face list
    if (   static_cast<int>( leftGroup.size() )  == size 
        || static_cast<int>( rightGroup.size() ) == size ) {
        //std::cout << "Total    : " << size;
        //std::cout << "; Left  Grp: " << leftGroup.size();
        //std::cout << "; Right Grp: " << rightGroup.size() << std::endl;
        leftGroup.clear();
        rightGroup.clear();
        std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
        int i = 0;
        for ( ; i < size/2; ++i ) {
            leftGroup.push_back( *face );
            ++face;
        }
        for ( ; i < size; ++i ) {
            rightGroup.push_back( *face );
            ++face;
        }
    }
    Vector3<T> U, V, W;
    //---------------------------------------------------------------
    // Create Left Child Node
    leftChild  = CreateBVHNodeBOBB( pParentNode, leftGroup );
    FindOBBOfFaceList( leftGroup, leftChild->GetCenter(), U, V, W );
    leftChild->SetPrincipleComponents( U, V, W );
//  leftChild->listHEFace  = leftGroup;         // DEBUG
    pParentNode->Child( 0, leftChild );
    BOBBBuildChildren( leftChild, leftGroup, U );
    //---------------------------------------------------------------
    // Create Right Child Node
    rightChild = CreateBVHNodeBOBB( pParentNode, rightGroup );
    FindOBBOfFaceList( rightGroup, rightChild->GetCenter(), U, V, W );
    rightChild->SetPrincipleComponents( U, V, W );
//  rightChild->listHEFace = rightGroup;        // DEBUG
    pParentNode->Child( 1, rightChild );
    BOBBBuildChildren( rightChild, rightGroup, U );

    /*
    int size = static_cast<int>( heFaceList.size() );
    std::list< HEFace<T> * > leftGroup, rightGroup;
    //---------------------------------------------------------------
    // Didive into two groups
    BOBBDivideToFaceGroups( heFaceList, leftGroup, rightGroup, 
                                pParentNode->GetCenter(), 
                                principleVector );
    //---------------------------------------------------------------
    assert( pParentNode );          // ASSERT
    BVHNode<T> * leftChild = NULL;
    BVHNode<T> * rightChild = NULL;
    if ( 2 < static_cast<int>( leftGroup.size() ) && static_cast<int>( leftGroup.size() ) < size ) {
        leftChild  = CreateBVHNodeBOBB( pParentNode, leftGroup );
        //FindOBBOfFaceList( leftGroup, leftChild );
        Vector3<T> U, V, W;
        FindOBBOfFaceList( leftGroup, leftChild->GetCenter(), U, V, W );
        leftChild->SetPrincipleComponents( U, V, W );
        leftChild->listHEFace  = leftGroup;         // DEBUG
        pParentNode->Child( 0, leftChild );
        BOBBBuildChildren( leftChild, leftGroup, U );
    }
    if ( 2 < static_cast<int>( rightGroup.size() ) && static_cast<int>( rightGroup.size() ) < size ) {
        rightChild = CreateBVHNodeBOBB( pParentNode, rightGroup );
        //FindOBBOfFaceList( rightGroup, rightChild );
        Vector3<T> U, V, W;
        FindOBBOfFaceList( rightGroup, rightChild->GetCenter(), U, V, W );
        rightChild->SetPrincipleComponents( U, V, W );
        rightChild->listHEFace = rightGroup;        // DEBUG
        pParentNode->Child( 1, rightChild );
        BOBBBuildChildren( rightChild, rightGroup, U );
    }
    //*/
}
//-----------------------------------------------------------------------------
// Find the group that this face belongs to
template <typename T>
int BVHTree<T>::BOBBDetermineFaceGroup ( HEFace<T> * face, 
                                      const Vector3<T> & center, 
                                      const Vector3<T> & direction )
{
    //-------------------------------------------------------------------------
    // Divided into an axis positive and negative
    //---------------------------------------------------------------
    // REMARK:
    //   To speed thing up, we can evaluate only the first vertex of the polygon.
    //---------------------------------------------------------------
    // Determine from all vertices
    int value = 0;
    HEHalfEdge<T> * firstHalfEdge = face->IncidentHalfEdge();
    HEHalfEdge<T> * halfEdge = firstHalfEdge;
    Vector3<T> vertex;
    do {
        //vertex = halfEdge->Vertex()->GetPosition() - center;
        //if ( vertex.GetX() >= 0 ) ++value;
        //else                      --value;
        if ( (halfEdge->Vertex()->GetPosition() - center) * direction >= 0 )
            ++value;
        else
            --value;
        halfEdge = halfEdge->Next();
    } while ( halfEdge != firstHalfEdge );
    //---------------------------------------------------------------
    // Determine the group (left or right)
    return  value >= 0 ? 1 : 0;
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::BOBBDivideToFaceGroups ( 
                const std::list< HEFace<T> * > & inputFaces,    // input
                std::list< HEFace<T> * > & group1,              // output group1
                std::list< HEFace<T> * > & group2,              // output group2
                const Vector3<T> & center, 
                const Vector3<T> & direction )
{
    std::list< HEFace<T> * >::const_iterator face;
    for ( face = inputFaces.begin(); face != inputFaces.end(); ++face ) {
        if ( BOBBDetermineFaceGroup( *face, center, direction ) == 1 )
            group1.push_back( *face );
        else
            group2.push_back( *face );
    }
}
//-----------------------------------------------------------------------------
// Create a new sphere bounding volume node from the face list
template <typename T>
BVHNode<T> * BVHTree<T>::CreateBVHNodeBOBB ( 
                        BVHNode<T> * pParentNode, 
                        const std::list< HEFace<T> * > & heFaceList )
{
    //*
    Vector3<T> AABB[2];
    //HEVertex<T> * vertex = m_listVertex->Head();
    std::list< HEFace<T> * >::const_iterator face = heFaceList.begin();
    HEHalfEdge<T> * firstHalfEdge = (*face)->IncidentHalfEdge();
    HEHalfEdge<T> * halfEdge = firstHalfEdge;
    Vector3<T> vertex = halfEdge->Vertex()->GetPosition();
    AABB[0] = AABB[1] = vertex;
    //---------------------------------------------------------------
    // For Each Face
    for ( ; face != heFaceList.end(); ++face ) {
        halfEdge = firstHalfEdge = (*face)->IncidentHalfEdge();
        //-------------------------------------------------
        // For Each Vertex
        do {
            vertex = halfEdge->Vertex()->GetPosition();
            // Find the lowest an the highest of x, y, and z
            // AABB[0] is min and AABB[1] is max
            if      ( AABB[0][0] > vertex[0] )  AABB[0][0] = vertex[0];
            else if ( AABB[1][0] < vertex[0] )  AABB[1][0] = vertex[0];
            if      ( AABB[0][1] > vertex[1] )  AABB[0][1] = vertex[1];
            else if ( AABB[1][1] < vertex[1] )  AABB[1][1] = vertex[1];
            if      ( AABB[0][2] > vertex[2] )  AABB[0][2] = vertex[2];
            else if ( AABB[1][2] < vertex[2] )  AABB[1][2] = vertex[2];
            // Next vertex
            halfEdge = halfEdge->Next();
        } while ( halfEdge != firstHalfEdge );
    }
    //---------------------------------------------------------------
    // Find the bounding volume center from the AABB
    Vector3<T> vCenter( ( AABB[0][0] + AABB[1][0] ) / 2.0,
                        ( AABB[0][1] + AABB[1][1] ) / 2.0,
                        ( AABB[0][2] + AABB[1][2] ) / 2.0 );
    /*
    //---------------------------------------------------------------
    // Find the Sphere Bounding Volume
    T radius = 0, squaredLength;
    for ( face = heFaceList.begin(); face != heFaceList.end(); ++face ) {
        halfEdge = firstHalfEdge = (*face)->IncidentHalfEdge();
        //-------------------------------------------------
        do {
            vertex = halfEdge->Vertex()->GetPosition();
            squaredLength = (vertex - vCenter).SquaredLength();
            if ( squaredLength > radius )   radius = squaredLength;
            halfEdge = halfEdge->Next();
        } while ( halfEdge != firstHalfEdge );
    }
    radius = sqrt( radius );
    //*/
    //---------------------------------------------------------------
    // Make the root node
    //-------------------------------------------
    // BVHNode Constructor
    //   BVHNode<T>(    Enum::CD type,
    //                  int id, 
    //                  BVHNode<T> * parent, 
    //                  BVHNode<T> ** children )
    //-------------------------------------------
    BVHNode<T> * node = new BVHNode<T>( Enum::BVH_NODE_BINARY_OBB, 
                                -1, pParentNode, NULL );
    //std::cout << node << "\n";
    //std::cout << node->GetCenter() << std::endl;
    node->SetCenter( vCenter );
    //std::cout << node->GetCenter() << std::endl;
    //node->SetRadius( radius );
    return node;
}
//*/
//-----------------------------------------------------------------------------
//=============================================================================















//=============================================================================
//-----------------------------------------------------------------------------
// Find OOB (Oriented Bounding Box) (see OBB of Gottschalk, et al.)
// Gottschalk, et al. triangulate all polygons composed of more than three edges.
// Then find 'mean' and 3-by-3 'covariance matrix'
// This 'covariance matrix' is a symmetric matrix, hence all of its eigenvalue 
// are real and its eigenvectors are mutually orthogonal.
// The normalized eigenvectors are used as a basis for OBB.
// Then find the extremal vertices along each axis of this basis, and size the 
// bounding box, oriented with the basis vectors.
// m = 1/(3n) * Sum_{i=0}^n ( p^i + q^i + r^i )
// C_{jk} = 1/(3n) * Sum_{i=0}^n ( (p_j^ihat)*(p_k^ihat) + (q_j^ihat)*(q_k^ihat) + (r_j^ihat)*(r_k^ihat) )
// where
//   n is the number of triangles
//   p^i, q^i, and r^i are the first, second, and third vertex of triangle i.
//   p_j^ihat is the j component of first vertex subtracted by m_j
//   remark: (i=x, j=y, k=z)
//*****************************************************************************
// m = 1/(3n) * Sum_{i=0}^n ( p^i + q^i + r^i )
// C_{11} = 1/(3n) * Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_x - m_x)^2 ) )
// C_{22} = 1/(3n) * Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_y - m_y)^2 ) )
// C_{33} = 1/(3n) * Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_z - m_y)^2 ) )
// C_{12} = C_{21} = Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_x - m_x)*(v_i_y - m_y) ) )
// C_{13} = C_{31} = Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_x - m_x)*(v_i_z - m_z) ) )
// C_{23} = C_{32} = Sum_{i=0}^n ( Sum_{v=0}^3 ( (v_i_y - m_y)*(v_i_z - m_z) ) )
// For a polygon with more than three vertices,
//   the polygon is triangulated at its centroid.
template <typename T>
void BVHTree<T>::FindOBBOfFaceList ( 
            const std::list< HEFace<T> * > & heFaceList,    // I/P: face list
            Vector3<T> & center,    // O/P: center
            Vector3<T> & U,         // O/P: 1st principle component
            Vector3<T> & V,         // O/P: 2nd principle component
            Vector3<T> & W )        // O/P: 3rd principle component
{
    //if ( heFaceList.size() == 0 ) return;
    assert( heFaceList.size() > 0 );            // ASSERT
    Vector3<T>      M( 0,0,0 );                 // Mean
    Matrix3x3<T>    C( 0,0,0,0,0,0,0,0,0 );     // Covariance Matrix
    //---------------------------------------------------------------
    std::list< HEFace<T> * >::const_iterator face;
    HEHalfEdge<T> * firstHalfEdge;
    HEHalfEdge<T> * halfEdge;
    Vector3<T>      vertex;
    Vector3<T>      centroid;
    int count, totalVertices = 0;
    //---------------------------------------------------------------
    // Calculate MEAN
    // For each face (polygon)
    for ( face = heFaceList.begin(); face != heFaceList.end(); ++face ) {
        //-------------------------------------------------
        // Determine the number of vertices of the face
        firstHalfEdge = halfEdge = (*face)->IncidentHalfEdge();
        count = 0;
        do {
            ++count;
            halfEdge = halfEdge->Next();    // next halfedge
        } while ( halfEdge != firstHalfEdge );
        //-------------------------------------------------
        // Triangle (or lower) Case
        if ( count <= 3 ) {
            totalVertices += count;
            do {
                //-------------------------------
                // Find Mean
                M += halfEdge->Vertex()->GetPosition();
                //-------------------------------
                halfEdge = halfEdge->Next();    // next halfedge
            } while ( halfEdge != firstHalfEdge );
        }
        //-------------------------------------------------
        // Four or More Vertices Case
        else {
            totalVertices += 3*count;   // increased by 3 * count
            centroid.SetXYZ( 0,0,0 );
            do {
                vertex = halfEdge->Vertex()->GetPosition();
                centroid += vertex;
                //-------------------------------
                // Find Mean
                M += vertex*2;
                //-------------------------------
                halfEdge = halfEdge->Next();    // next halfedge
            } while ( halfEdge != firstHalfEdge );
            //centroid /= count;
            M += centroid;
        }
        //-------------------------------------------------
    }
    M /= T(totalVertices);      // Final Mean
    //---------------------------------------------------------------
    // Calculate COVARIANCE MATRIX
    // For each face (polygon)
    for ( face = heFaceList.begin(); face != heFaceList.end(); ++face ) {
        //-------------------------------------------------
        // Determine the number of vertices of the face
        firstHalfEdge = halfEdge = (*face)->IncidentHalfEdge();
        count = 0;
        do {
            ++count;
            halfEdge = halfEdge->Next();    // next halfedge
        } while ( halfEdge != firstHalfEdge );
        //-------------------------------------------------
        // Triangle (or lower) Case
        if ( count <= 3 ) {
            //totalVertices += count;
            do {
                vertex = halfEdge->Vertex()->GetPosition() - M;
                //-------------------------------
                // Covariance Matrix is a 3x3 symmatric matrix
                C[0] += vertex.GetX() * vertex.GetX() * 2;  // C_{11}
                C[4] += vertex.GetY() * vertex.GetY() * 2;  // C_{22}
                C[8] += vertex.GetZ() * vertex.GetZ() * 2;  // C_{33}
                C[1] += vertex.GetX() * vertex.GetY() * 2;  // C_{12} = C_{21}
                C[2] += vertex.GetX() * vertex.GetZ() * 2;  // C_{13} = C_{31}
                C[5] += vertex.GetY() * vertex.GetZ() * 2;  // C_{23} = C_{32}
                //-------------------------------
                halfEdge = halfEdge->Next();    // next halfedge
            } while ( halfEdge != firstHalfEdge );
        }
        //-------------------------------------------------
        // Four or More Vertices Case
        else {
            //totalVertices += 3*count; // increased by 3 * count
            centroid.SetXYZ( 0,0,0 );
            do {
                vertex = halfEdge->Vertex()->GetPosition() - M;
                centroid += vertex;
                //-------------------------------
                // Covariance Matrix is a 3x3 symmatric matrix
                C[0] += vertex.GetX() * vertex.GetX();      // C_{11}
                C[4] += vertex.GetY() * vertex.GetY();      // C_{22}
                C[8] += vertex.GetZ() * vertex.GetZ();      // C_{33}
                C[1] += vertex.GetX() * vertex.GetY();      // C_{12} = C_{21}
                C[2] += vertex.GetX() * vertex.GetZ();      // C_{13} = C_{31}
                C[5] += vertex.GetY() * vertex.GetZ();      // C_{23} = C_{32}
                //-------------------------------
                halfEdge = halfEdge->Next();    // next halfedge
            } while ( halfEdge != firstHalfEdge );
            vertex = centroid/T(count) - M;
            C[0] += vertex.GetX() * vertex.GetX();      // C_{11}
            C[4] += vertex.GetY() * vertex.GetY();      // C_{22}
            C[8] += vertex.GetZ() * vertex.GetZ();      // C_{33}
            C[1] += vertex.GetX() * vertex.GetY();      // C_{12} = C_{21}
            C[2] += vertex.GetX() * vertex.GetZ();      // C_{13} = C_{31}
            C[5] += vertex.GetY() * vertex.GetZ();      // C_{23} = C_{32}
        }
        //-------------------------------------------------
    }
    //---------------------------------------------------------------
    C[3] = C[1];
    C[6] = C[2];
    C[7] = C[5];
    C /= T(totalVertices);      // Final Covariance Matrix
    //---------------------------------------------------------------
    T l[3];
    CGMath<T>::EigValsVecsOf3x3SymMatrix( 
        C, l[0], l[1], l[2], U, V, W );
    //---------------------------------------------------------------
    // Find the length of each principle component
    Vector3<T> vec;
    T length, positive[3] = {0,0,0}, negative[3] = {0,0,0};
    //---------------------------------------------------------------
    // For each face (polygon)
    for ( face = heFaceList.begin(); face != heFaceList.end(); ++face ) {
        //-------------------------------------------------
        // For each vertex of the face
        firstHalfEdge = halfEdge = (*face)->IncidentHalfEdge();
        do {
            vec = halfEdge->Vertex()->GetPosition() - center;
            length = vec * U;
            if      ( length > positive[0] )    positive[0] = length;
            else if ( length < negative[0] )    negative[0] = length;
            length = vec * V;
            if      ( length > positive[1] )    positive[1] = length;
            else if ( length < negative[1] )    negative[1] = length;
            length = vec * W;
            if      ( length > positive[2] )    positive[2] = length;
            else if ( length < negative[2] )    negative[2] = length;
            halfEdge = halfEdge->Next();    // next halfedge
        } while ( halfEdge != firstHalfEdge );
        //-------------------------------------------------
    }
    //---------------------------------------------------------------
    // Adjust the center and the length of principle components
    T adjustedValue;
    length = (positive[0] - negative[0]) / T(2.0);
    adjustedValue = positive[0] - length;
    center = center + U*adjustedValue;
    U *= length;
    length = (positive[1] - negative[1]) / T(2.0);
    adjustedValue = positive[1] - length;
    center = center + V*adjustedValue;
    V *= length;
    length = (positive[2] - negative[2]) / T(2.0);
    adjustedValue = positive[2] - length;
    center = center + W*adjustedValue;
    W *= length;
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
//=============================================================================

//-----------------------------------------------------------------------------
#if defined(__gl_h_) || defined(__GL_H__)
//=============================================================================
// DrawByOpenGL
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::DrawByOpenGL () const
{
    if ( m_pRoot ) {
        glPushMatrix();
            GetTransform().TransformByOpenGLForDrawing();
            m_pRoot->DrawByOpenGL();
        glPopMatrix();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::DrawByOpenGL ( int currentLevel, int startLevel, int endLevel ) const
{
    if ( m_pRoot ) {
        glPushMatrix();
            GetTransform().TransformByOpenGLForDrawing();
            m_pRoot->DrawByOpenGL( currentLevel, startLevel, endLevel );
        glPopMatrix();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::DrawByOpenGLOnlyEndLevel () const
{
    if ( m_pRoot ) {
        glPushMatrix();
            GetTransform().TransformByOpenGLForDrawing();
            m_pRoot->DrawByOpenGLOnlyEndLevel();
        glPopMatrix();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::DrawByOpenGLCollidedNodes () const
{
    glPushMatrix();
        GetTransform().TransformByOpenGLForDrawing();
        for ( int i = 0; i < static_cast<int>( m_svCollidedNode.size() ); ++i ) {
            m_svCollidedNode[i]->DrawBV();
            m_svCollidedNode[i]->DrawPrim();
        }
    glPopMatrix();
}
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree<T>::DrawByOpenGLCollidedNodesThat ( 
                    TransformationSupport<T> const & transformSupport ) const
{
    glPushMatrix();
        transformSupport.TransformByOpenGLForDrawing();
        for ( int i = 0; i < static_cast<int>( m_svCollidedNode.size() ); ++i ) {
            m_svCollidedNodeThat[i]->DrawBV();
            m_svCollidedNodeThat[i]->DrawPrim();
        }
    glPopMatrix();
}
//-----------------------------------------------------------------------------
#endif
//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----