TAPsBVHTree_BinarySphere.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsBVHTree_BinarySphere.hpp
******************************************************************************/
// See comment below
/******************************************************************************
SUKITTI PUNAK   (07/11/2008)
UPDATE          (10/10/2010)
******************************************************************************/
#include "TAPsBVHTree_BinarySphere.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_BinarySphere<T>::BVHTree_BinarySphere ( 
    TransformationSupport<T> & transform, 
    int numOfNodes, 
    BVHNode<T> * rootNode )
    : BVHTree<T>( transform, Enum::BVH_TREE_BINARY_SPHERE, numOfNodes, rootNode )
{}
//-----------------------------------------------------------------------------
template <typename T>
BVHTree_BinarySphere<T>::BVHTree_BinarySphere ( 
    TransformationSupport<T> & transform,   
    HEFaceList<T> * heFaceList              
)
    : BVHTree<T>( transform )
{
    //-----------------------------------------------------
    // Copy original face list (heFaceList) to a list 
    // of face (faceList)
    std::list< HEFace<T> * > faceList;
    HEFace<T> * face = heFaceList->Head();
    while ( face ) {
        faceList.push_back( face );
        face = face->Next();
    }

    m_eType = Enum::BVH_TREE_BINARY_SPHERE;
    m_iTotalNodes = 1;
    m_Transform = transform;
    m_pRoot = CreateBVHNodeBSphere( NULL, faceList );

    Vector3<T> U, V, W;
    FindOBBOfFaceList( faceList, m_pRoot->GetCenter(), U, V, W );
    if ( m_pRoot )  BSphereBuildChildren( m_pRoot, faceList, U );

    FindListOfLeafNodes();

    SetNumOfNodes( CountNodes() );
}
//-----------------------------------------------------------------------------
template <typename T>
BVHTree_BinarySphere<T>::~BVHTree_BinarySphere ()
{}
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// TestOverlapWith a BVHTree
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWith ( BVHTree<T> const * const that )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWith( BVHTree<T> const * const " << that << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    m_svCollidedNode.clear();
    m_svCollidedNodeThat.clear();

    //---------------------------------------------------------------
    switch ( that->GetType() ) {
        case Enum::BVH_TREE_BINARY_SPHERE:
        case Enum::BVH_NODE_LEAF_SPHERE:
            return TestOverlap_vs_BinarySphere( m_pRoot, GetTransform().GetMatrixTransform(), that->Root(), that->GetTransform().GetMatrixTransform() );
            break;

        case Enum::BVH_TREE_QUAD_SPHERE:
        case Enum::BVH_TREE_OCTANT_SPHERE:
        case Enum::BVH_TREE_GENERIC_SPHERE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree_BinarySphere<T> 002)" << std::endl;
            break;

        default:
            std::cout << "Error: BVHTree_BinarySphere<T>::TestOverlapWith Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------
// TestOverlapWith a BVHNode
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWith ( BVHNode<T> const * const node )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWith( BVHNode<T> const * const " << node << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    //std::cout << "\tNOT IMPLEMENTED YET!\n";

    m_svCollidedNode.clear();
    m_svCollidedNodeThat.clear();
    //---------------------------------------------------------------
    switch ( node->GetType() ) {
        case Enum::BVH_NODE_BINARY_SPHERE:
        case Enum::BVH_NODE_LEAF_SPHERE:
            return TestOverlap_vs_BinarySphere( m_pRoot, GetTransform().GetMatrixTransform(), node, CGMath<T>::IdentityMatrix4x4 );
            break;

        case Enum::BVH_NODE_QUAD_SPHERE:
        case Enum::BVH_NODE_OCTANT_SPHERE:
        case Enum::BVH_NODE_GENERIC_SPHERE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree_BinarySphere<T> 004)" << std::endl;
            break;
        default:
            std::cout << "Error: BVHTree_BinarySphere<T>::TestOverlapWith Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------
// TestOverlapWith a MultiBoundingVolume
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWith ( MultiBoundingVolume<T> const * const pMBV )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWith( MultiBoundingVolume<T> const * const " << pMBV << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    //m_svCollidedNodeThat.clear();
    m_svCollidedBVAndNodeList.clear();

    bool bResult = false;
    //---------------------------------------------------------------
    // Traverse the list of bounding volumes
    std::vector< BoundingVolume<T> * >::iterator pos = const_cast< MultiBoundingVolume<T> * >( pMBV )->GetBoundingVolumeList().begin();

    while ( pos != pMBV->GetBoundingVolumeList().end() ) {
        m_svCollidedNode.clear();
        assert( *pos != NULL );
        // Store the transformation of the bounding volume
        TransformationSupport<T> trx = (*pos)->GetTransform();
        (*pos)->GetTransform().SetMatrixTransform( pMBV->GetTransform().GetMatrixTransform() * trx.GetMatrixTransform() );

        // Collision Detection
        bool result = TestOverlap_vs_BV( m_pRoot, GetTransform().GetMatrixTransform(), *pos, (*pos)->GetTransform().GetMatrixTransform() );

        // Restore the transformation of the bounding volume
        (*pos)->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() );
        // Set result and keep records
        if ( result ) {
            bResult = true;
            //for ( int i = 0; i < static_cast<int>( m_svCollidedNodeThat.size() ); ++i ) {
            //  m_svCollidedNode.push_back( m_svCollidedNodeThat[i] );
            //  m_svCollidedBVList.push_back( *pos );
            //}
            m_svCollidedBVAndNodeList.push_back( BVsAndNodesList<T>( *pos, m_svCollidedNode ) );
        }
        // Next Bounding Volume
        ++pos;

        //pos = const_cast< MultiBoundingVolume<T> * >( pMBV )->GetBoundingVolumeList().end();
    }

    return bResult;
}
//-----------------------------------------------------------------------------
// TestOverlapWith a BoundingVolume
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWith ( BoundingVolume<T> const * const pBV )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWith( BoundingVolume<T> const * const " << pBV << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    // After finish the content in m_svCollidedNodeThat will be moved into m_svCollidedNode.
    // m_svCollidedNode must be priorly cleared by a caller function.

    m_svCollidedNode.clear();
    //m_svCollidedNodeThat.clear();
    // m_svCollidedBVAndNodeList is not cleared, so that this fn can be used for overlap test with MBV.

    return TestOverlap_vs_BV( m_pRoot, GetTransform().GetMatrixTransform(), pBV, pBV->GetTransform().GetMatrixTransform() );
}



//-----------------------------------------------------------------------------
// TestOverlapWithTillLeafNodes with a BVHTree
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes ( BVHTree<T> const * const that )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes( BVHTree<T> const * const " << that << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    m_svCollidedNode.clear();
    m_svCollidedNodeThat.clear();

    //---------------------------------------------------------------
    switch ( that->GetType() ) {
        case Enum::BVH_TREE_BINARY_SPHERE:
        case Enum::BVH_NODE_LEAF_SPHERE:
            return TestOverlap_vs_BinarySphere_TillLeafNodes( m_pRoot, GetTransform().GetMatrixTransform(), that->Root(), that->GetTransform().GetMatrixTransform() );
            break;

        case Enum::BVH_TREE_QUAD_SPHERE:
        case Enum::BVH_TREE_OCTANT_SPHERE:
        case Enum::BVH_TREE_GENERIC_SPHERE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree_BinarySphere<T> 010)" << std::endl;
            break;

        default:
            std::cout << "Error: BVHNode_BinarySphere<T>::TestOverlapWithTillLeafNodes Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------
// TestOverlapWithTillLeafNodes with a BVHNode
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes ( BVHNode<T> const * const node )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes( BVHNode<T> const * const " << node << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    //std::cout << "\tNOT IMPLEMENTED YET!\n";

    m_svCollidedNode.clear();
    m_svCollidedNodeThat.clear();
    //---------------------------------------------------------------
    switch ( node->GetType() ) {
        case Enum::BVH_NODE_BINARY_SPHERE:
        case Enum::BVH_NODE_LEAF_SPHERE:
            return TestOverlap_vs_BinarySphere_TillLeafNodes( m_pRoot, GetTransform().GetMatrixTransform(), node, CGMath<T>::IdentityMatrix4x4 );
            break;

        case Enum::BVH_NODE_QUAD_SPHERE:
        case Enum::BVH_NODE_OCTANT_SPHERE:
        case Enum::BVH_NODE_GENERIC_SPHERE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree<T> 012)" << std::endl;
            break;
        default:
            std::cout << "Error: BVHNode<T>::TestOverlapWithTillLeafNodes Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------
// TestOverlapWithTillLeafNodes with a MultiBoundingVolume
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes ( MultiBoundingVolume<T> const * const pMBV )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    //std::cout << "BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes( MultiBoundingVolume<T> const * const " << pMBV << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    // After finish the content in m_svCollidedNodeThat will be moved into m_svCollidedNode.
    // m_svCollidedNode must be priorly cleared by a caller function.

    //m_svCollidedNodeThat.clear();
    m_svCollidedBVAndNodeList.clear();

    bool bResult = false;
    //---------------------------------------------------------------
    // Traverse the list of bounding volumes
    std::vector< BoundingVolume<T> * >::iterator pos = const_cast< MultiBoundingVolume<T> * >( pMBV )->GetBoundingVolumeList().begin();

    while ( pos != pMBV->GetBoundingVolumeList().end() ) {
        m_svCollidedNode.clear();
        assert( *pos != NULL );
        // Store the transformation of the bounding volume
        TransformationSupport<T> trx = (*pos)->GetTransform();
        (*pos)->GetTransform().SetMatrixTransform( pMBV->GetTransform().GetMatrixTransform() * trx.GetMatrixTransform() );

        // Collision Detection
        bool result = TestOverlap_vs_BV_TillLeafNodes( m_pRoot, GetTransform().GetMatrixTransform(), *pos, (*pos)->GetTransform().GetMatrixTransform() );

        // Restore the transformation of the bounding volume
        (*pos)->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() );
        // Set result and keep records
        if ( result ) {
            bResult = true;
            //for ( int i = 0; i < static_cast<int>( m_svCollidedNodeThat.size() ); ++i ) {
            //  m_svCollidedNode.push_back( m_svCollidedNodeThat[i] );
            //  m_svCollidedBVList.push_back( *pos );
            //}
            m_svCollidedBVAndNodeList.push_back( BVsAndNodesList<T>( *pos, m_svCollidedNode ) );
        }
        // Next Bounding Volume
        ++pos;
    }

    return bResult;
}
//-----------------------------------------------------------------------------
// TestOverlapWithTillLeafNodes with a BoundingVolume
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes ( BoundingVolume<T> const * const pBV )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes( BoundingVolume<T> const * const " << pBV << " )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR

    // After finish the content in m_svCollidedNodeThat will be moved into m_svCollidedNode.
    // m_svCollidedNode must be priorly cleared by a caller function.

    m_svCollidedNode.clear();
    //m_svCollidedNodeThat.clear();
    // m_svCollidedBVAndNodeList is not cleared, so that this fn can be used for overlap test with MBV.

    return TestOverlap_vs_BV_TillLeafNodes( m_pRoot, GetTransform().GetMatrixTransform(), pBV, pBV->GetTransform().GetMatrixTransform() );
}
//-----------------------------------------------------------------------------




//=============================================================================
// START: Test Intersection with a Line Segment
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestIntersectionWithLineSegmentTillLeafNodes ( 
    Vector3<T> const * const ptA, Vector3<T> const * const ptB )
{
    // Transform ptA and ptB to the local coordinate system of the CD tree
    TAPs::Matrix4x4<T> invTrx( GetTransform().RefToMatrixTransform().GetInverse() );
    Vector3<T> A = invTrx * *ptA;
    Vector3<T> B = invTrx * *ptB;

    m_svCollidedNode.clear();
    return TestIntersectionWithLineSegmentTillLeafNodesRecursive( Root(), &A, &B );
}
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestIntersectionWithLineSegmentTillLeafNodesRecursive ( 
    BVHNode<T> const * const node, 
    Vector3<T> const * const ptA, 
    Vector3<T> const * const ptB )
{
    // Need to add transformation of the node into the computation !!!

    T ratio, distance;
    Vector3<T> projPt;
    CGMath<T>::FindProjectedPointOnALine ( 
            node->GetCenter(),  
            *ptA,   
            *ptB,   
            ratio,      
            projPt,     
            distance    
    );
    //---------------------------------------------------------------
    // Test overlapping
    if ( distance > node->GetRadius() ) {
        return false;
    }
    //---------------------------------------------------------------
    // If node A is leaf
    else if ( node->IsLeaf() ) {
        if ( ratio < 0 || 1 < ratio ) {
            return false;
        }
        m_svCollidedNode.push_back( const_cast< BVHNode<T> * >( node ) );
        return true;
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        bool overlap1 = false, overlap2 = false;
        if ( node->Child(0) ) {
            overlap1 = TestIntersectionWithLineSegmentTillLeafNodesRecursive( node->Child(0), ptA, ptB );
        }
        if ( node->Child(1) ) {
            overlap2 = TestIntersectionWithLineSegmentTillLeafNodesRecursive( node->Child(1), ptA, ptB );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( node )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
}
//-----------------------------------------------------------------------------
// END: Test Intersection with a Line Segment
//=============================================================================




//=============================================================================
// START: Test Intersection with a circle
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestIntersectionWithCircleTillLeafNodes ( 
    Vector3<T> const * const circleNormalPlane, Vector3<T> const * const circleCenter, T circleRadius )
{
    // Transform the circle's center to the local coordinate system of the CD tree
    TAPs::Matrix4x4<T> invTrx( GetTransform().RefToMatrixTransform().GetInverse() );
    Vector3<T> C = invTrx * *circleCenter;
    Vector3<T> N = invTrx * (*circleNormalPlane + *circleCenter) - C;

    // Calculate the transform that transform the circle to lie flat 
    // on the x-y plane with normal plane point to z-positive.
    Matrix4x4<T> Translation = CGMath<T>::MatrixTranslation( -C );
    Matrix4x4<T> Rotation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( N, Vector3<T>(0,0,1) );
    Matrix4x4<T> Trx = Rotation * Translation;  // first translate the center to the origin, then rotate the normal plane to <0,0,1>
    //Trx.Inversed();
    //C = Trx * C;

    m_svCollidedNode.clear();
    return TestIntersectionWithCircleTillLeafNodesRecursive( Trx, Root(), circleRadius );
}
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestIntersectionWithCircleTillLeafNodesRecursive (
    Matrix4x4<T> Trx,   
    BVHNode<T> const * const node,
    T circleRadius )
{
    // The circle's center is assumed to be at the origin
    // The circle is assumed to lie flat on the xy-plane, i.e. its normal plane point to the z-positive direction.

    Vector3<T> center = Trx * node->GetCenter();

    //---------------------------------------------------------------
    // Test overlapping
    T offset = fabs(center[2]);
    if ( offset > node->GetRadius() ) {
        return false;
    }
    else {
        T ratio = offset/node->GetRadius();
        offset = sqrt( 1 - ratio*ratio ) * node->GetRadius();

        //offset = node->GetRadius();
    }
    //---------------------------------------------------------------
    if ( center[0]*center[0] + center[1]*center[1] - offset > circleRadius*circleRadius ) {
        return false;
    }
    //---------------------------------------------------------------
    // If node A is leaf
    else if ( node->IsLeaf() ) {
        //if ( ratio < 0 || 1 < ratio ) {
        //  return false;
        //}
        m_svCollidedNode.push_back( const_cast< BVHNode<T> * >( node ) );
        return true;
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        bool overlap1 = false, overlap2 = false;
        if ( node->Child(0) ) {
            overlap1 = TestIntersectionWithCircleTillLeafNodesRecursive( Trx, node->Child(0), circleRadius );
        }
        if ( node->Child(1) ) {
            overlap2 = TestIntersectionWithCircleTillLeafNodesRecursive( Trx, node->Child(1), circleRadius );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( node )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
}
//-----------------------------------------------------------------------------
// END: Test Intersection with a circle
//=============================================================================




//=============================================================================
// START: Test Intersection with an interaction point group
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWith (
    InteractionPointGroup<T> * const    pIPG,           
    std::vector< PointForce<T> > & ListOfPointForces    
)
{
    m_svCollidedNode.clear();

    unsigned int countPF = ListOfPointForces.size();

    // Transform the interaction point group to the body space of BVHTree
    InteractionPointGroup<T> IPG_Local( *pIPG );
    Matrix4x4<T> invTrx = GetTransform().GetMatrixTransform().GetInverse();
    IPG_Local.TransformedBy( invTrx );

    bool bResult = TestOverlapWithIPGRecursive( Root(), &IPG_Local, ListOfPointForces );

    // Transform the point forces to world space
    if ( bResult ) {
        for ( unsigned int i = countPF; i < ListOfPointForces.size(); ++i ) {
            ListOfPointForces[i].RefToPosition() = GetTransform().GetMatrixTransform() * ListOfPointForces[i].RefToPosition();

            //ListOfPointForces[i].RefToForce() = GetTransform().GetMatrixTransform() * ListOfPointForces[i].RefToForce() - ListOfPointForces[i].RefToPosition();
        }
    }

    return bResult;
}
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithTillLeafNodes (
    InteractionPointGroup<T> * const    pIPG,           
    std::vector< PointForce<T> > & ListOfPointForces    
)
{
    m_svCollidedNode.clear();

    unsigned int countPF = ListOfPointForces.size();

    // Transform the interaction point group to the body space of BVHTree
    InteractionPointGroup<T> IPG_Local( *pIPG );
    Matrix4x4<T> invTrx = GetTransform().GetMatrixTransform().GetInverse();
    IPG_Local.TransformedBy( invTrx );

    bool bResult = TestOverlapWithIPGTillLeafNodesRecursive( Root(), &IPG_Local, ListOfPointForces );

    // Transform the point forces to world space
    if ( bResult ) {
        for ( unsigned int i = countPF; i < ListOfPointForces.size(); ++i ) {
            ListOfPointForces[i].RefToPosition() = GetTransform().GetMatrixTransform() * ListOfPointForces[i].RefToPosition();

            //ListOfPointForces[i].RefToForce() = GetTransform().GetMatrixTransform() * ListOfPointForces[i].RefToForce() - ListOfPointForces[i].RefToPosition();
        }
    }

    return bResult;
}
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithIPGRecursive (
    BVHNode<T> const * const node,
    InteractionPointGroup<T> * const    pIPG,           
    std::vector< PointForce<T> > & ListOfPointForces    
)
{
    bool bCD = false;

    // Node is not leaf
    if ( !node->IsLeaf() ) {
        for ( std::vector< InteractionPoint<T> >::iterator it = pIPG->GetTheSetOfInteractionPoints().begin();
            it != pIPG->GetTheSetOfInteractionPoints().end(); ++it )
        {
            Vector3<T> separationCenter = node->GetCenter() - it->GetPosition();
            T length = separationCenter.Length() - ( node->GetRadius() + it->GetSize() );
            if ( length < 0 ) {
                bCD = true;
                break;
            }
        }

        if ( bCD == false ) return false;

        // Traverse to child nodes
        bool overlap1 = false, overlap2 = false;
        if ( node->Child(0) ) {
            overlap1 = TestOverlapWithIPGRecursive( node->Child(0), pIPG, ListOfPointForces );
        }
        if ( node->Child(1) ) {
            overlap2 = TestOverlapWithIPGRecursive( node->Child(1), pIPG, ListOfPointForces );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( node )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
    // Node is leaf
    else {
        for ( std::vector< InteractionPoint<T> >::iterator it = pIPG->GetTheSetOfInteractionPoints().begin();
            it != pIPG->GetTheSetOfInteractionPoints().end(); ++it )
        {
            Vector3<T> separation = node->GetCenter() - it->GetPosition();
            T len = separation.Length();
            T length = len - ( node->GetRadius() + it->GetSize() );
            if ( length < 0 ) {

                // Test with primitive (a triangle)
                HEFace<T> * pHEFace = node->GetAPrimitiveHalfEdgeFace();
                std::vector< HEVertex<T> * const >  listOfHEVertices = pHEFace->GetPtrsToVertices();
                Vector3<T> outVec[3];
                bCD = CGMath<T>::FindIntersectionSphereTriangle(
                    it->GetPosition(),  // sphere's center
                    it->GetSize(),      // sphere's radius
                    listOfHEVertices[0]->GetPosition(), listOfHEVertices[1]->GetPosition(), listOfHEVertices[2]->GetPosition(), // triangle's vertices
                    outVec[0], outVec[1], outVec[2] // out vectors for resolving the CD
                    //, &contactPt      // the contact point on the triangle
                );

                if ( bCD ) {
                    //separation *= length/len;
                    separation = ( outVec[0] + outVec[1] + outVec[2] ) / T(-3);
                    ListOfPointForces.push_back( PointForce<T>( it->GetPosition(), separation ) );
                }
            }
        }
    }

    return bCD;
}
//-----------------------------------------------------------------------------
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlapWithIPGTillLeafNodesRecursive (
    BVHNode<T> const * const node,
    InteractionPointGroup<T> * const    pIPG,           
    std::vector< PointForce<T> > & ListOfPointForces    
)
{
    bool bCD = false;

    // Node is not leaf
    if ( !node->IsLeaf() ) {
        for ( std::vector< InteractionPoint<T> >::iterator it = pIPG->GetTheSetOfInteractionPoints().begin();
            it != pIPG->GetTheSetOfInteractionPoints().end(); ++it )
        {
            Vector3<T> separationCenter = node->GetCenter() - it->GetPosition();
            T length = separationCenter.Length() - ( node->GetRadius() + it->GetSize() );
            if ( length < 0 ) {
                bCD = true;
                break;
            }
        }

        if ( bCD == false ) return false;

        // Traverse to child nodes
        bool overlap1 = false, overlap2 = false;
        if ( node->Child(0) ) {
            overlap1 = TestOverlapWithIPGTillLeafNodesRecursive( node->Child(0), pIPG, ListOfPointForces );
        }
        if ( node->Child(1) ) {
            overlap2 = TestOverlapWithIPGTillLeafNodesRecursive( node->Child(1), pIPG, ListOfPointForces );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( node )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
    // Node is leaf
    else {
        for ( std::vector< InteractionPoint<T> >::iterator it = pIPG->GetTheSetOfInteractionPoints().begin();
            it != pIPG->GetTheSetOfInteractionPoints().end(); ++it )
        {
            Vector3<T> separation = node->GetCenter() - it->GetPosition();
            T len = separation.Length();
            T length = len - ( node->GetRadius() + it->GetSize() );
            if ( length < 0 ) {
                bCD = true;
                separation *= length/len;
                ListOfPointForces.push_back( PointForce<T>( it->GetPosition(), separation ) );
            }
        }
    }

    return bCD;
}

//-----------------------------------------------------------------------------
// END: Test Intersection with an interaction point group
//=============================================================================




//=============================================================================
// START: Test Overlap with a MultiBoundingVolume
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// END: Test Overlap with a MultiBoundingVolume
//=============================================================================




//=============================================================================
// START: Test Overlap with a BoundingVolume
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV ( 
                BVHNode<T> const * const nodeA, 
                Matrix4x4<T> const & transformA, 
                BoundingVolume<T> const * const pBV, 
                Matrix4x4<T> const & transformB )
{
    std::cout << "#4 BVHTree_BinarySphere<T>::TestOverlap_vs_BV( BVHNode<T>, Matrix4x4<T>, BoundingVolume<T>, Matrix4x4<T> )\n";

    //---------------------------------------------------------------
    switch ( pBV->GetType() ) {
        case Enum::BOUNDING_SPHERE:
            {
                Vector3<T>  sphereCenter;
                T           sphereRadius;
                ForBVSphereTransformations( pBV, transformB, sphereCenter, sphereRadius );

                // Collision Detection -- With Primitive-Primitive Test
                return TestOverlap_vs_BV_Sphere( nodeA, transformA, sphereCenter, sphereRadius );
            }
            break;
        case Enum::BOUNDING_CYLINDER:
            {
                Matrix4x4<T> nodeTransformation;
                ForBVCylinderTransformations( transformA, pBV, transformB, nodeTransformation );

                // Collision Detection -- With Primitive-Primitive Test
                return TestOverlap_vs_BV_Cylinder_AtOrigin( nodeA, nodeTransformation, pBV->GetRadius(), pBV->GetHeight()  );
            }
            break;
        case Enum::BOUNDING_BOX:
        case Enum::BOUNDING_ELLIPSOID:
        case Enum::BOUNDING_HALF_SPHERE:
        case Enum::BOUNDING_HALF_ELLIPSOID:
        case Enum::BOUNDING_CONE:
        case Enum::BOUNDING_CAPSULE:
        case Enum::BOUNDING_POLYTOPE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree_BinarySphere<T> 104)" << std::endl;
            break;
        default:
            std::cout << "Error: BVHTree_BinarySphere<T>::TestOverlap_vs_BV Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_TillLeafNodes
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_TillLeafNodes ( 
                BVHNode<T> const * const nodeA, 
                Matrix4x4<T> const & transformA, 
                BoundingVolume<T> const * const pBV, 
                Matrix4x4<T> const & transformB )
{
    // TEST PASSED
    //std::cout << "#4 BVHTree_BinarySphere<T>::TestOverlap_vs_BV_TillLeafNodes( BVHNode<T>, Matrix4x4<T>, BoundingVolume<T>, Matrix4x4<T> )\n";

    //---------------------------------------------------------------
    switch ( pBV->GetType() ) {
        case Enum::BOUNDING_SPHERE:
            {
                Vector3<T>  sphereCenter;
                T           sphereRadius;
                ForBVSphereTransformations( pBV, transformB, sphereCenter, sphereRadius );

                // Collision Detection -- W/O Primitive-Primitive Test
                return TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA, transformA, sphereCenter, sphereRadius );
            }
            break;
        case Enum::BOUNDING_CYLINDER:
            {
                Matrix4x4<T> nodeTransformation;
                ForBVCylinderTransformations( transformA, pBV, transformB, nodeTransformation );

                // Collision Detection -- W/O Primitive-Primitive Test
                return TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes( nodeA, nodeTransformation, pBV->GetRadius(), pBV->GetHeight() );
            }
            break;
        case Enum::BOUNDING_BOX:
        case Enum::BOUNDING_ELLIPSOID:
        case Enum::BOUNDING_HALF_SPHERE:
        case Enum::BOUNDING_HALF_ELLIPSOID:
        case Enum::BOUNDING_CONE:
        case Enum::BOUNDING_CAPSULE:
        case Enum::BOUNDING_POLYTOPE:
            std::cout << "NOT IMPLEMENTED YET! (ID: BVHTree_BinarySphere<T> 204)" << std::endl;
            break;
        default:
            std::cout << "Error: BVHTree_BinarySphere<T>::TestOverlap_vs_BV_TillLeafNodes Fn!" << std::endl;
            assert( false );
            break;
    }
    //---------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
// Transformations
//-----------------------------------------------------------------------------
template <typename T>
void BVHTree_BinarySphere<T>::ForBVSphereTransformations ( 
        BoundingVolume<T> const * const pBV,    
        Matrix4x4<T> const & bvTransformation,  
        Vector3<T> & sphereCenter,              
        T & sphereRadius                        
)
{
    // Transform the pBV due to its transformation, center, and radius
    Vector3<T> C( pBV->GetCenter() );
    Matrix4x4<T> translation( 1, 0, 0, C[0], 
                              0, 1, 0, C[1],
                              0, 0, 1, C[2],
                              0, 0, 0, 1 );
    Vector3<T> S( pBV->GetRadius(), pBV->GetRadius(), pBV->GetRadius() );
    Matrix4x4<T> scale( S[0], 0,    0,    0, 
                        0,    S[1], 0,    0,
                        0,    0,    S[2], 0,
                        0,    0,    0,    1 );
    Matrix4x4<T> trx = bvTransformation * translation * scale;

    // Sphere's center and radius after the transformation
    sphereCenter = (trx * Vector4<T>(0,0,0,1)).GetVector3();
    sphereRadius = ( (trx * Vector4<T>(1,0,0,1)) - sphereCenter ).Length();

    /*
    // DEBUG
    std::cout << "(trx * Vector4<T>(S[0],0,0,1)): " << (trx * Vector4<T>(S[0],0,0,1)) << "\n";

    std::cout << "transformB: " << transformB;
    std::cout << "transformB * translation: " << transformB * translation;
    std::cout << "transformB * translation * scale: " << transformB * translation * scale;
    std::cout << "pBV: " << *pBV << "\n";
    std::cout << "Sphere: center " <<  sphereCenter << "; radius " << sphereRadius << "\n";
    //*/
}
//-----------------------------------------------------------------------------
// ForBVCylinderTransformations #1
template <typename T>
void BVHTree_BinarySphere<T>::ForBVCylinderTransformations ( 
        BoundingVolume<T> const * const pBV,    
        Matrix4x4<T> & nodeTransform_OP         
)
{
    nodeTransform_OP = Matrix4x4<T>(    1, 0, 0, -pBV->GetCenter()[0], 
                                        0, 1, 0, -pBV->GetCenter()[1], 
                                        0, 0, 1, -pBV->GetCenter()[2], 
                                        0, 0, 0, 1
                                    );
}
//-----------------------------------------------------------------------------
// ForBVCylinderTransformations #2
template <typename T>
void BVHTree_BinarySphere<T>::ForBVCylinderTransformations ( 
        BoundingVolume<T> const * const pBV,    
        Matrix4x4<T> const & bvTransformation,  
        Matrix4x4<T> & nodeTransform_OP         
)
{
    nodeTransform_OP = bvTransformation * Matrix4x4<T>( 1, 0, 0, pBV->GetCenter()[0], 
                                                        0, 1, 0, pBV->GetCenter()[1], 
                                                        0, 0, 1, pBV->GetCenter()[2], 
                                                        0, 0, 0, 1
                                                    );
    nodeTransform_OP.Inversed();
}
//-----------------------------------------------------------------------------
// ForBVCylinderTransformations #3
template <typename T>
void BVHTree_BinarySphere<T>::ForBVCylinderTransformations ( 
        Matrix4x4<T> const & nodeTransform_IP,  
        BoundingVolume<T> const * const pBV,    
        Matrix4x4<T> & nodeTransform_OP         
)
{
    nodeTransform_OP = Matrix4x4<T>(    1, 0, 0, -pBV->GetCenter()[0], 
                                        0, 1, 0, -pBV->GetCenter()[1], 
                                        0, 0, 1, -pBV->GetCenter()[2], 
                                        0, 0, 0, 1
                                    )
                    * nodeTransform_IP;
}
//-----------------------------------------------------------------------------
// ForBVCylinderTransformations #4
template <typename T>
void BVHTree_BinarySphere<T>::ForBVCylinderTransformations ( 
        Matrix4x4<T> const & nodeTransform_IP,  
        BoundingVolume<T> const * const pBV,    
        Matrix4x4<T> const & bvTransformation,  
        Matrix4x4<T> & nodeTransform_OP         
)
{
    nodeTransform_OP = bvTransformation * Matrix4x4<T>( 1, 0, 0, pBV->GetCenter()[0], 
                                                        0, 1, 0, pBV->GetCenter()[1], 
                                                        0, 0, 1, pBV->GetCenter()[2], 
                                                        0, 0, 0, 1
                                                    );
    nodeTransform_OP = nodeTransform_OP.Inversed() * nodeTransform_IP;
}
//-----------------------------------------------------------------------------
// Transformations
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
// With Sphere BV
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Sphere #1
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere ( 
        BVHNode<T> const * const nodeA, 
        Vector3<T> const & sphereCenter, 
        T sphereRadius )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "#1 BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere( BVHNode<T>, Vector3<T>, T )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "\tCurrently call TestOverlap_vs_BV_Sphere_TillLeafNodes( ... ) instead\n";

    return TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA, sphereCenter, sphereRadius );

    //---------------------------------------------------------------
    // Do low level here!!!
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Sphere #2
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere ( 
        BVHNode<T> const * const nodeA, 
        Matrix4x4<T> const & transformA, 
        Vector3<T> const & sphereCenter, 
        T sphereRadius )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "#2 BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere( BVHNode<T>, Matrix4x4<T>, Vector3<T>, sphereCenter, sphereRadius )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "\tCurrently call TestOverlap_vs_BV_Sphere_TillLeafNodes( ... ) instead\n";

    return TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA, transformA, sphereCenter, sphereRadius );

    //---------------------------------------------------------------
    // Do low level here!!!
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// With Sphere BV
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
// With Sphere BV TillLeafNodes
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Sphere_TillLeafNodes #1
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere_TillLeafNodes ( 
        BVHNode<T> const * const nodeA, 
        Vector3<T> const & sphereCenter, 
        T sphereRadius )
{
    //std::cout << "#1 BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere_TillLeafNodes( BVHNode<T>, Vector3<T>, T )\n";

    //---------------------------------------------------------------
    // Test overlapping
    if ( nodeA->TestOverlapSphereWithBVSphere_NoPrimitiveTests( sphereCenter, sphereRadius ) > Math<T>::ZERO ) {
        return false;
    }
    //---------------------------------------------------------------
    // If node A is leaf
    if ( nodeA->IsLeaf() ) {
        m_svCollidedNode.push_back( const_cast< BVHNode<T> * >( nodeA ) );
        //std::cout << "\tTRUE -- nodeA: " << nodeA << "\n";
        #ifdef  TAPs_DEBUG_COLLISION_DETECTION
            const_cast< BVHNode<T> * >( nodeA )->flag = true;
        #endif//TAPs_DEBUG_COLLISION_DETECTION
        return true;
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        bool overlap1 = false, overlap2 = false;
        if ( nodeA->Child(0) ) {
            overlap1 = TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA->Child(0), sphereCenter, sphereRadius );
        }
        if ( nodeA->Child(1) ) {
            overlap2 = TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA->Child(1), sphereCenter, sphereRadius );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( nodeA )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Sphere_TillLeafNodes #2
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere_TillLeafNodes ( 
        BVHNode<T> const * const nodeA, 
        Matrix4x4<T> const & transformA, 
        Vector3<T> const & sphereCenter, 
        T sphereRadius )
{
    //std::cout << "#2 BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Sphere_TillLeafNodes( BVHNode<T>, Matrix4x4<T>, Vector3<T>, T )\n";
    //---------------------------------------------------------------
    // Test overlapping
    if ( nodeA->TestOverlapSphereWithBVSphere_NoPrimitiveTests( transformA, sphereCenter, sphereRadius ) > Math<T>::ZERO ) {
        return false;
    }
    //---------------------------------------------------------------
    // If node A is leaf
    else if ( nodeA->IsLeaf() ) {
        m_svCollidedNode.push_back( const_cast< BVHNode<T> * >( nodeA ) );
        //std::cout << "\tTRUE -- nodeA: " << nodeA << "\n";
        #ifdef  TAPs_DEBUG_COLLISION_DETECTION
            const_cast< BVHNode<T> * >( nodeA )->flag = true;
        #endif//TAPs_DEBUG_COLLISION_DETECTION
        return true;
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        bool overlap1 = false, overlap2 = false;
        if ( nodeA->Child(0) ) {
            overlap1 = TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA->Child(0), transformA, sphereCenter, sphereRadius );
        }
        if ( nodeA->Child(1) ) {
            overlap2 = TestOverlap_vs_BV_Sphere_TillLeafNodes( nodeA->Child(1), transformA, sphereCenter, sphereRadius );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( nodeA )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// With Sphere BV TillLeafNodes
//-----------------------------------------------------------------------------





//-----------------------------------------------------------------------------
// With Cylinder BV
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Cylinder
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Cylinder_AtOrigin ( 
        BVHNode<T> const * const nodeA, 
        Matrix4x4<T> const & transformA, 
        T cylinderRadius, 
        T cylinderHeight )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Cylinder_AtOrigin( BVHNode<T>, Matrix4x4<T>, cylinderRadius, cylinderHeight )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "\tCurrently call TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes( ... ) instead\n";

    return TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes( nodeA, transformA, cylinderRadius, cylinderHeight );

    //---------------------------------------------------------------
    // Do low level here!!!
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// With Cylinder BV
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
// With Cylinder BV TillLeafNodes
//-----------------------------------------------------------------------------
// TestOverlap_vs_BV_Cylinder_TillLeafNodes
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes ( 
        BVHNode<T> const * const nodeA, 
        Matrix4x4<T> const & transformA, 
        T cylinderRadius, 
        T cylinderHeight )
{
    //std::cout << "BVHTree_BinarySphere<T>::TestOverlap_vs_BV_Cylinder_TillLeafNodes( BVHNode<T>, Matrix4x4<T>, cylinderRadius, cylinderHeight )\n";

    //---------------------------------------------------------------
    // Test overlapping
    if ( nodeA->TestOverlapSphereWithBVCylinder_AtOrigin_NoPrimitiveTests( transformA, cylinderRadius, cylinderHeight ) == false ) {
        return false;
    }
    //---------------------------------------------------------------
    // If node A is leaf
    else if ( nodeA->IsLeaf() ) {
        #ifdef  TAPs_DEBUG_COLLISION_DETECTION
            const_cast< BVHNode<T> * >( nodeA )->flag = true;
        #endif//TAPs_DEBUG_COLLISION_DETECTION
        m_svCollidedNode.push_back( const_cast< BVHNode<T> * >( nodeA ) );
        //std::cout << "\tTRUE -- nodeA: " << nodeA << "\n";
        return true;
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        bool overlap1 = false, overlap2 = false;
        if ( nodeA->Child(0) ) {
            overlap1 = TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes( nodeA->Child(0), transformA, cylinderRadius, cylinderHeight );
        }
        if ( nodeA->Child(1) ) {
            overlap2 = TestOverlap_vs_BV_Cylinder_AtOrigin_TillLeafNodes( nodeA->Child(1), transformA, cylinderRadius, cylinderHeight );
        }
        if ( overlap1 || overlap2 ) {
            #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                const_cast< BVHNode<T> * >( nodeA )->flag = true;
            #endif//TAPs_DEBUG_COLLISION_DETECTION
            return true;
        }
        else {
            return false;
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// With Cylinder BV TillLeafNodes
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// END: Test Overlap with a BoundingVolume
//=============================================================================



//=============================================================================
// START: Test Overlap with Another BVHTree_BinarySphere
//-----------------------------------------------------------------------------
// TestOverlap_vs_BinarySphere
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BinarySphere ( 
                        BVHNode<T> const * const nodeA, 
                        Matrix4x4<T> const & transformA, 
                        BVHNode<T> const * const nodeB, 
                        Matrix4x4<T> const & transformB )
{
    #ifdef  TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "BVHTree_BinarySphere<T>::TestOverlap_vs_BinarySphere( BVHNode<T>, Matrix4x4<T>, BVHNode<T>, Matrix4x4<T> )\n";
    #endif//TAPs_DEBUG_BINARY_SPHERE_TREE_CDR
    std::cout << "\tCurrently call TestOverlap_vs_BinarySphere_TillLeafNodes( ... ) instead\n";

    bool bResult = TestOverlap_vs_BinarySphere_TillLeafNodes( nodeA, transformA, nodeB, transformB );
    if ( bResult ) {
        //-----------------------------------
        // Do Low-level Intersection Test HERE!
        //std::cout << "// Do Low-level Intersection Test HERE!\n";
        // E.g., Triangle-triangle intersection test
        //bResult = leaf node from nodeA->TestOverlapPrimitiveWithPrimitive( transformA, leaf node from nodeB, transformB );
    }
    return bResult;
}
//-----------------------------------------------------------------------------
// TestOverlap_vs_BinarySphere_TillLeafNodes
template <typename T>
bool BVHTree_BinarySphere<T>::TestOverlap_vs_BinarySphere_TillLeafNodes ( 
                        BVHNode<T> const * const nodeA, 
                        Matrix4x4<T> const & transformA, 
                        BVHNode<T> const * const nodeB, 
                        Matrix4x4<T> const & transformB )
{
    //---------------------------------------------------------------
    // Test overlapping
    if ( nodeA->TestOverlapSphereWithSphere_NoPrimitiveTests( transformA, nodeB, transformB ) > Math<T>::ZERO ) {
        return false;
    }

    //std::cout << "ah ha\n";
    //return true;
    //if ( false ) {}

    //---------------------------------------------------------------
    // If node A is leaf
    else if ( nodeA->IsLeaf() ) {
        //-------------------------------------------------
        // Both node A and node B are leaves
        if ( nodeB->IsLeaf() ) {
            //-----------------------------------
            // Do Low-level Intersection Test HERE!
            //std::cout << "// Do Low-level Intersection Test HERE!\n";
            //-----------------------------------
            //std::cout << "Distance (" << nodeA << "," << nodeB << "): " 
            //      << nodeA->TestOverlapSphereWithSphere_TillLeafNodes( nodeB ) << "\n";
            BVHNode<T> * nA = const_cast< BVHNode<T> * >( nodeA );
            BVHNode<T> * nB = const_cast< BVHNode<T> * >( nodeB );
            m_svCollidedNode.push_back( nA );
            m_svCollidedNodeThat.push_back( nB );
            //-----------------------------------
            return true;
        }
        //-------------------------------------------------
        // Node A is leaf, but node B is not
        else {
            bool overlap1 = false, overlap2 = false;
            if ( nodeB->Child(0) ) {
                overlap1 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA, transformA, nodeB->Child(0), transformB );
            }
            if ( nodeB->Child(1) ) {
                overlap2 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA, transformA, nodeB->Child(1), transformB );
            }
            if ( overlap1 || overlap2 ) return true;
            else                        return false;
        }
    }
    //---------------------------------------------------------------
    // If node A is not leaf
    else {
        //-------------------------------------------------
        // Node A is not leaf, but node B is
        if ( nodeB->IsLeaf() ) {
            bool overlap1 = false, overlap2 = false;
            if ( nodeA->Child(0) ) {
                overlap1 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(0), transformA, nodeB, transformB );
            }
            if ( nodeA->Child(1) ) {
                overlap2 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(1), transformA, nodeB, transformB );
            }
            if ( overlap1 || overlap2 ) return true;
            else                        return false;
        }
        //-------------------------------------------------
        // Both node A and node B are not leaves
        else {
            bool overlap1 = false, overlap2 = false, overlap3 = false, overlap4 = false;
            if ( nodeA->Child(0) && nodeB->Child(0) ) {
                overlap1 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(0), transformA, nodeB->Child(0), transformB );
            }
            if ( nodeA->Child(0) && nodeB->Child(1) ) {
                overlap2 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(0), transformA, nodeB->Child(1), transformB );
            }
            if ( nodeA->Child(1) && nodeB->Child(0) ) {
                overlap3 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(1), transformA, nodeB->Child(0), transformB );
            }
            if ( nodeA->Child(1) && nodeB->Child(1) ) {
                overlap4 = TestOverlap_vs_BinarySphere_TillLeafNodes( 
                                nodeA->Child(1), transformA, nodeB->Child(1), transformB );
            }
            if ( overlap1 || overlap2 || overlap3 || overlap4 )
                return true;
            else
                return false;
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// END: Test Overlap with Another BVHTree_BinarySphere
//=============================================================================


//=============================================================================
// BINARY SPHERE BOUNDING VOLUME HIERARCHY TREE
//-----------------------------------------------------------------------------
template <typename T>
BVHTree<T> * BVHTree_BinarySphere<T>::BSphereBuild ( 
        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 << "BSphereBuild (before): " << cdtree << std::endl;
    BVHNode<T> *node = CreateBVHNodeBSphere( NULL, heFaceList );
//  node->listHEFace  = heFaceList;         // DEBUG
    cdtree = new BVHTree<T>( transform, Enum::BVH_TREE_BINARY_SPHERE, 1, node );
    //FindOBBOfFaceList( heFaceList, node );
    Vector3<T> U, V, W;
    FindOBBOfFaceList( heFaceList, node->GetCenter(), U, V, W );
    if ( node ) BSphereBuildChildren( node, heFaceList, U );
    //std::cout << "BSphereBuild (after): " << cdtree << std::endl;

    SetNumOfNodes( CountNodes() );
    return cdtree;
}
//-----------------------------------------------------------------------------
// Create binary child spheres
template <typename T>
void BVHTree_BinarySphere<T>::BSphereBuildChildren ( 
    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;
    //---------------------------------------------------------------
    // CASE: one face
    // This is a leaf node.
    assert( size > 0 );         // ASSERT
    if ( 1 == size ) {
        return;
    }
    //---------------------------------------------------------------
    assert( pParentNode );          // ASSERT
    BVHNode<T> * leftChild  = NULL;
    BVHNode<T> * rightChild = NULL;
    //---------------------------------------------------------------
    // 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  = CreateBVHNodeBSphere( pParentNode, leftGroup );
//      leftChild->listHEFace  = leftGroup;         // DEBUG
        pParentNode->Child( 0, leftChild );
        //-------------------------------------------------
        // Create Right Leaf Node
        rightChild = CreateBVHNodeBSphere( pParentNode, rightGroup );
//      rightChild->listHEFace = rightGroup;        // DEBUG
        pParentNode->Child( 1, rightChild );
        return;
    }
    //---------------------------------------------------------------
    // CASE: more than two faces
    // Didive into two groups
    BSphereDivideToFaceGroups(  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  = CreateBVHNodeBSphere( pParentNode, leftGroup );
    FindOBBOfFaceList( leftGroup, leftChild->GetCenter(), U, V, W );
//  leftChild->listHEFace  = leftGroup;         // DEBUG
    pParentNode->Child( 0, leftChild );
    BSphereBuildChildren( leftChild, leftGroup, U );
    //---------------------------------------------------------------
    // Create Right Child Node
    rightChild = CreateBVHNodeBSphere( pParentNode, rightGroup );
    FindOBBOfFaceList( rightGroup, rightChild->GetCenter(), U, V, W );
//  rightChild->listHEFace = rightGroup;        // DEBUG
    pParentNode->Child( 1, rightChild );
    BSphereBuildChildren( rightChild, rightGroup, U );
}
//-----------------------------------------------------------------------------
// Find the group that this face belongs to
template <typename T>
int BVHTree_BinarySphere<T>::BSphereDetermineFaceGroup ( 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.GetY() >= 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_BinarySphere<T>::BSphereDivideToFaceGroups ( 
                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 ( BSphereDetermineFaceGroup( *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_BinarySphere<T>::CreateBVHNodeBSphere ( 
                        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] ) / T(2.0),
                        ( AABB[0][1] + AABB[1][1] ) / T(2.0),
                        ( AABB[0][2] + AABB[1][2] ) / T(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();
            squaredLength = (vertex - vCenter).Length();
            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;
    if ( 1 == static_cast<int>( heFaceList.size() ) ) {
        /*
        node = new BVHNode<T>( Enum::BVH_NODE_BINARY_SPHERE, 
                                -1, pParentNode, NULL );
        //*/
        //*
        BVHNodeLeafHalfEdgeAPrim<T> * node = new BVHNodeLeafHalfEdgeAPrim<T>( 
                Enum::BVH_NODE_LEAF_SPHERE_HALFEDGE_APRIM, 
                -1, pParentNode );
        node->m_primHEFace = heFaceList.front();
//      BVHNodeLeafHalfEdgePrims<T> * node = new BVHNodeLeafHalfEdgePrims<T>( 
//              Enum::BVH_NODE_LEAF_SPHERE_HALFEDGE_PRIMS, 
//              -1, pParentNode );
        //node->listHEFace = heFaceList;
        node->SetCenter( vCenter );
        node->SetRadius( radius );
        return node;
        //*/
    }
    else {
        BVHNode<T> * node = new BVHNode<T>( Enum::BVH_NODE_BINARY_SPHERE, 
                                            -1, pParentNode, NULL );
        node->SetCenter( vCenter );
        node->SetRadius( radius );
        return node;
    }
}
//*/
//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----