TAPsColDetFns.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsColDetFns.cpp

ColDetFns (cpp file).

SUKITTI PUNAK   (07/09/2009)
UPDATE          (12/29/2010)
******************************************************************************/
#include "TAPsColDetFns.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__CD__Fn
//=============================================================================
//-----------------------------------------------------------------------------


#if ( defined TAPs_STRAND_MODEL_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_Strand_vs_MBV ( 
    TAPs::OpenGL::ModelStrand<T> *  pStrandObj,             
    TAPs::MultiBoundingVolume<T> const * const  pMBVObj,    
    TAPs::TransformationSupport<T> const * const pTransform 
)
{
    bool bResult = false;
    //---------------------------------------------------------------
    //bool isThePointInsideTheBV;   // the pt is inside or outside the bv
    Vector3<T> vDistance;           // displacement vector
    TransformationSupport<T> transform;
    //---------------------------------------------------------------
    if ( pTransform ) {
        transform.ReturnMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    //---------------------------------------------------------------
    transform.ReturnMatrixTransform() *= pMBVObj->GetTransform().GetMatrixTransform();
    TransformationSupport<T> savedTransform( transform );
    //---------------------------------------------------------------
    std::vector< BoundingVolume<T> * >::const_iterator pos = pMBVObj->GetBoundingVolumeList().begin();
    //---------------------------------------------------------------
    while ( pos != pMBVObj->GetBoundingVolumeList().end() ) {
        assert( *pos != NULL );
        vDistance.SetXYZ( 0, 0, 0 );
        transform = savedTransform;
        //-------------------------------------------------
        transform.ReturnMatrixTransform() *= (*pos)->GetTransform().GetMatrixTransform();
        //-------------------------------------------------
        // Transform each vertex of the strand to the bounding volume coordinate
        int iDeepestPoint       = -1;
        T   tDeepestDistance    = 9E60;
        Vector3<T>      vDirection;
        Vector3<T>      translation( transform.GetTranslation() );
        Matrix3x3<T>    inversedRotMatrix( transform.GetMatrixRotation().GetInverse() );

        switch ( (*pos)->GetType() ) {

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_CYLINDER:
            //---------------------------------------------
            #ifdef  TAPs_STRAND_LOCK_SEGMENTS
                for ( unsigned int s = 0; s < pStrandObj->GetNumSegments(); ++s ) {
                    // Skip locked or invisible segments
                    if ( pStrandObj->GetSegmentLockStatus(s) || !pStrandObj->GetSegmentVisibleStatus(s) ) {}
                    else {
                        for ( int i = pStrandObj->GetStartPtOfSegment(s); i <= pStrandObj->GetEndPtOfSegment(s); ++i )
            #else //TAPs_STRAND_LOCK_SEGMENTS
                {
                    {
                        for ( unsigned int i = 0; i <= pStrandObj->GetNumberOfLinks(); ++i )
            #endif//TAPs_STRAND_LOCK_SEGMENTS
                        {
                            // If the point is fixed, do not move it.
                            if ( pStrandObj->GetFixStatusOfPtNo(i) )    continue;

                            //------------------------------------------------------
                            // In the local coordinate of the bounding cylinder, 
                            // the cylinder axis is parallel to z-axis 
                            // where it is shifted by the cylinder center.
                            // The cylinder has radius, height and center.
                            // Its height is measured from -z and +z axis
                            // where its center is situated between height/2 in -z and +z axis.
                            Vector3<T> location = pStrandObj->GetPointPosition(i);
                            location -= translation;
                            location = inversedRotMatrix * location;
                            // Shift the location of point by the cylinder center
                            // i.e. shift the cylinder to the origin (0,0,0)
                            location -= (*pos)->GetCenter();
                            //------------------------------------------------------
                            // Now the cylinder is centered at the origin with its axis on the z-axis
                            // where its height is from -height/2 to +height/2.
                            // And the input point has been transformed into the cylinder coordinate.
                            // The test can be performed.
                            if ( TAPs::CGMath<T>::CD_CylinderAtOrigin_vs_Point( (*pos)->GetRadius(), (*pos)->GetHeight(), location, vDistance ) )
                            {
                                bResult = true;
                                //-------------------------
                                // Now rotate it back by the rotation matrix (from the transform matrix).
                                // REMARK: Translation (from the transform matrix) is NOT applied back, 
                                //         because vDistance represents a displacement vector from 
                                //         the input point to the cylinder surface.
                                //         I.e., the strand point is always in the world coordinates.
                                vDistance = transform.GetMatrixRotation() * vDistance;
                                //-------------------------
                                // This statement just ask the strand to move its collided points away
                                pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( i, vDistance );
                                T tDistance = vDistance.Length();
                                if ( tDeepestDistance > tDistance ) {
                                    tDeepestDistance = tDistance;
                                    iDeepestPoint = i;
                                    vDirection = vDistance;
                                }
                                //-------------------------
                            }
                        }// Back to Next Strand Point
                    }
                }
                break;
                // END: BOUNDING_CYLINDER

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_SPHERE:
            //---------------------------------------------
            #ifdef  TAPs_STRAND_LOCK_SEGMENTS
                for ( unsigned int s = 0; s < pStrandObj->GetNumSegments(); ++s ) {
                    // Skip locked or invisible segments
                    if ( pStrandObj->GetSegmentLockStatus(s) || !pStrandObj->GetSegmentVisibleStatus(s) ) {}
                    else {
                        for ( int i = pStrandObj->GetStartPtOfSegment(s); i <= pStrandObj->GetEndPtOfSegment(s); ++i )
            #else //TAPs_STRAND_LOCK_SEGMENTS
                {
                    {
                        for ( unsigned int i = 0; i <= pStrandObj->GetNumberOfLinks(); ++i )
            #endif//TAPs_STRAND_LOCK_SEGMENTS
                        {
                            // If the point is fixed, do not move it.
                            if ( pStrandObj->GetFixStatusOfPtNo(i) )    continue;

                            //------------------------------------------------------
                            // In the local coordinate of the bounding sphere centered at the origin 
                            Vector3<T> location = pStrandObj->GetPointPosition(i);
                            location -= translation;
                            location = inversedRotMatrix * location;
                            // Shift the location of point by the sphere center
                            // i.e. shift the sphere to the origin (0,0,0)
                            location -= (*pos)->GetCenter();
                            //------------------------------------------------------
                            // Now the sphere is centered at the origin
                            // And the input point has been transformed into the sphere coordinate.
                            // The test can be performed.
                            if ( TAPs::CGMath<T>::CD_SphereAtOrigin_vs_Point( (*pos)->GetRadius(), location, vDistance ) )
                            {
                                bResult = true;
                                //-------------------------
                                // Now rotate it back by the rotation matrix (from the transform matrix).
                                // REMARK: Translation (from the transform matrix) is NOT applied back, 
                                //         because vDistance represents a displacement vector from 
                                //         the input point to the sphere surface.
                                //         I.e., the strand point is always in the world coordinates.
                                vDistance = transform.GetMatrixRotation() * vDistance;
                                //-------------------------
                                // This statement just ask the strand to move its collided points away
                                pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( i, vDistance );
                                T tDistance = vDistance.Length();
                                if ( tDeepestDistance > tDistance ) {
                                    tDeepestDistance = tDistance;
                                    iDeepestPoint = i;
                                    vDirection = vDistance;
                                }
                                //-------------------------
                            }
                        }// Back to Next Strand Point
                    }
                }
                break;
                // END: BOUNDING_SPHERE

        }//END: switch ( (*pos)->GetType() ) {...}
        ++pos;  // Next Bounding Volume
    }
    return bResult;
}
// END: CDR_Strand_vs_MBV(...)


//-----------------------------------------------------------------------------
// START: namespace Private
namespace Private {

template <typename T>
bool CD ( 
    TAPs::OpenGL::ModelStrand<T> *  pStrandObj,                 
    TAPs::BVHTree<T> const * const  pObjBVH,                    
    TAPs::TransformationSupport<T> const * const pTransform,    
    std::vector< BVHNode< T > * > * listOfSutureCollidedNodes,  
    std::vector< BVHNode< T > * > * listOfObjectCollidedNodes   
)
{
    //--------------------------------------------------------------------
    BVHTree<T> * strandBVH = pStrandObj->GetBVHTree();
    bool result = strandBVH->TestOverlapWithTillLeafNodes( pObjBVH );
    if ( !result )  return result;
    //--------------------------------------------------------------------
    BVHNode<T> * cylinder;
    BVHNode<T> * triangle;
    //--------------------------------------------------------------------

    // HAVE TO CORPORATE pTransform for collision detection !!!

    int size = static_cast<int>( pStrandObj->GetBVHTree()->GetListOfCollidedNodes().size() );
    int linkNo;
    for ( int i = 0; i < size; ++i ) {
        cylinder = strandBVH->GetListOfCollidedNodes()[i];
        triangle = strandBVH->GetListOfCollidedNodesThat()[i];
        linkNo = cylinder->GetID();
        HEFace<T> * heFace = triangle->GetAPrimitiveHalfEdgeFace();
        std::vector< HEVertex<T> * const > vertices = heFace->GetPtrsToVertices();
        result = CGMath<T>::FindIntersectionCylinderTriangle(
                    pStrandObj->GetPointPosition( linkNo ), 
                    pStrandObj->GetPointPosition( linkNo+1 ), 
                    pStrandObj->GetRadius(), 
                    vertices[0]->GetPosition(), 
                    vertices[1]->GetPosition(), 
                    vertices[2]->GetPosition() );
        
        if ( result ) {
            listOfSutureCollidedNodes->push_back( strandBVH->GetListOfCollidedNodes()[i] );
            listOfObjectCollidedNodes->push_back( strandBVH->GetListOfCollidedNodesThat()[i] );
        }
    }
    //--------------------------------------------------------------------
    if ( static_cast<int>( listOfSutureCollidedNodes->size() ) > 0 )    return true;
    else                                                                return false;
}

//-----------------------------------------------------------------------------
template <typename T>
bool CR ( 
    TAPs::OpenGL::ModelStrand<T> *  pStrandObj,                 
    TAPs::BVHTree<T> const * const  pObjBVH,                    
    TAPs::TransformationSupport<T> const * const pTransform,    
    std::vector< BVHNode< T > * > * listOfSutureCollidedNodes,  
    std::vector< BVHNode< T > * > * listOfObjectCollidedNodes   
)
{
    // HAVE TO CORPORATE pTransform for collision detection !!!

    std::set< BVHNode<T> * > strandNodeSet;
    std::set< BVHNode<T> * > objectNodeSet;
    //--------------------------------------------------------------------
    Vector3<T> N, V1, V2;
    T value;
    //T offset = pStrandObj->GetRadius() * 2;
    //T offset = pStrandObj->GetRadius();
    T offset = 0;
    HEFace<T> * primB;
    std::vector< HEVertex<T> * const > vertexList;
    int noOfVertices;
    //--------------------------------------------------------------------
    // Deform Object away from Strand Object
    for ( int i = 0; i < static_cast<int>( listOfObjectCollidedNodes->size() ); ++i ) {

        assert( (*listOfObjectCollidedNodes)[i]->GetAPrimitiveHalfEdgeFace() != NULL );

        N = (*listOfObjectCollidedNodes)[i]->GetAPrimitiveHalfEdgeFace()->GetNormal();

        assert( N.Length() > 0.5 );

        // This is an ad hoc algorithm
        if ( true ) {
        //if ( separateDistance > Math<T>::ZERO ) {
            primB = (*listOfObjectCollidedNodes)[i]->GetAPrimitiveHalfEdgeFace();
            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );

            //-------------------------------------------------------
            // Set Positions of the stand link away from the triangle
            int linkNo = (*listOfSutureCollidedNodes)[i]->GetID();

            #ifdef  TAPs_ADVANCED_SIMULATION
            if ( 
                   pStrandObj->RetSimulationFlagsOfVertexNo( linkNo ).AreSimulationFlagsCleared() 
                && 
                   pStrandObj->RetSimulationFlagsOfVertexNo( linkNo+1 ).AreSimulationFlagsCleared() 
            ) 
            #endif//TAPs_ADVANCED_SIMULATION
            {
                // Limit the link length, to prevent the non-stop move of point(s).
                // Here the length is limited to not be over 8 * pStrandObj->GetLength().
                //Real length = ( pStrandObj->GetPointPosition( linkNo ) - pStrandObj->GetPointPosition( linkNo+1 ) ).Length();
                //if ( length > 4 * pStrandObj->GetLength() )   return;

                // Use triangle centroid to determine the depth of the intersection
                Vector3<T> triCentroid( ( vertexList[0]->GetPosition() + vertexList[1]->GetPosition() + vertexList[2]->GetPosition() ) / 3.0 );
                V1 = pStrandObj->GetPointPosition( linkNo   ) - triCentroid;
                V2 = pStrandObj->GetPointPosition( linkNo+1 ) - triCentroid;

                N.Normalized();
                value = V1 * N;
                if ( value < 0.0 ) {
                    pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( linkNo  , N * (offset - value) );
                }
                value = V2 * N;
                if ( value < 0.0 ) {
                    pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( linkNo+1, N * (offset - value) );
                }
                //-------------------------------------------------------
                // Set Positions of the stand link away from the triangle
                //N.Normalized();
                //N *= pStrandObj->GetRadius() * 2;
                //int linkNo = (*listOfSutureCollidedNodes)[i]->GetID();
                //pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( linkNo  , N );
                //pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( linkNo+1, N );

            }
        }
    }
    pStrandObj->UpdateBVHTree();

    return false;
}

} // END: namespace Private
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
//=============================================================================
#endif//#if ( defined TAPs_STRAND_MODEL_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )




#if ( defined TAPs_SUTURE_MODEL_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_Suture_vs_MBV ( 
    TAPs::OpenGL::ModelSuture<T> *  pSutureObj,             
    TAPs::MultiBoundingVolume<T> const * const  pMBVObj,    
    TAPs::TransformationSupport<T> const * const pTransform 
)
{
    bool bResult = false;
    //---------------------------------------------------------------
    //bool isThePointInsideTheBV;   // the pt is inside or outside the bv
    Vector3<T> vDistance;           // displacement vector
    TransformationSupport<T> transform;
    //---------------------------------------------------------------
    if ( pTransform ) {
        transform.ReturnMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    //---------------------------------------------------------------
    transform.ReturnMatrixTransform() *= pMBVObj->GetTransform().GetMatrixTransform();
    TransformationSupport<T> savedTransform( transform );
    //---------------------------------------------------------------
    std::vector< BoundingVolume<T> * >::const_iterator pos = pMBVObj->GetBoundingVolumeList().begin();
    //---------------------------------------------------------------
    while ( pos != pMBVObj->GetBoundingVolumeList().end() ) {
        assert( *pos != NULL );
        vDistance.SetXYZ( 0, 0, 0 );
        transform = savedTransform;
        //-------------------------------------------------
        transform.ReturnMatrixTransform() *= (*pos)->GetTransform().GetMatrixTransform();
        //-------------------------------------------------
        // Transform each vertex of the strand to the bounding volume coordinate
        int iDeepestPoint       = -1;
        //T tDeepestDistance    = Math<T>::MAX;
        //T tDeepestDistance    = std::numeric_limits<T>::max();
        T   tDeepestDistance    = 5E32;
        Vector3<T>      vDirection;
        Vector3<T>      translation( transform.GetTranslation() );
        Matrix3x3<T>    inversedRotMatrix( transform.GetMatrixRotation().GetInverse() );

        switch ( (*pos)->GetType() ) {

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_CYLINDER:
            //---------------------------------------------
            #ifdef  TAPs_STRAND_LOCK_SEGMENTS
                for ( unsigned int s = 0; s < pSutureObj->GetNumSegments(); ++s ) {
                    // Skip locked or invisible segments
                    if ( pSutureObj->GetSegmentLockStatus(s) || !pSutureObj->GetSegmentVisibleStatus(s) ) {}
                    else {
                        for ( int i = pSutureObj->GetStartPtOfSegment(s); i <= pSutureObj->GetEndPtOfSegment(s); ++i )
            #else //TAPs_STRAND_LOCK_SEGMENTS
                {
                    {
                        for ( unsigned int i = 0; i <= pSutureObj->GetNumberOfStrandLinksWithoutNeedle(); ++i )
            #endif//TAPs_STRAND_LOCK_SEGMENTS
                        {
                            // If the point is fixed, do not move it.
                            if ( pSutureObj->GetFixStatusOfPtNo(i) )    continue;

                            //------------------------------------------------------
                            // In the local coordinate of the bounding cylinder, 
                            // the cylinder axis is parallel to z-axis 
                            // where it is shifted by the cylinder center.
                            // The cylinder has radius, height and center.
                            // Its height is measured from -z and +z axis
                            // where its center is situated between height/2 in -z and +z axis.
                            Vector3<T> location = pSutureObj->GetPointPosition(i);
                            location -= translation;
                            location = inversedRotMatrix * location;
                            // Shift the location of point by the cylinder center
                            // i.e. shift the cylinder to the origin (0,0,0)
                            location -= (*pos)->GetCenter();
                            //------------------------------------------------------
                            // Now the cylinder is centered at the origin with its axis on the z-axis
                            // where its height is from -height/2 to +height/2.
                            // And the input point has been transformed into the cylinder coordinate.
                            // The test can be performed.
                            if ( TAPs::CGMath<T>::CD_CylinderAtOrigin_vs_Point( (*pos)->GetRadius(), (*pos)->GetHeight(), location, vDistance ) )
                            {
                                bResult = true;
                                //-------------------------
                                // Now rotate it back by the rotation matrix (from the transform matrix).
                                // REMARK: Translation (from the transform matrix) is NOT applied back, 
                                //         because vDistance represents a displacement vector from 
                                //         the input point to the cylinder surface.
                                //         I.e., the suture point is always in the world coordinates.
                                vDistance = transform.GetMatrixRotation() * vDistance;
                                //-------------------------
                                // This statement just ask the strand to move its collided points away
                                pSutureObj->ChangeOnlyPointPositionByAddedWithThisVector( i, vDistance );
                                T tDistance = vDistance.Length();
                                if ( tDeepestDistance > tDistance ) {
                                    tDeepestDistance = tDistance;
                                    iDeepestPoint = i;
                                    vDirection = vDistance;
                                }
                                //-------------------------
                            }
                        }// Back to Next Strand Point
                    }
                }
                break;
                // END: BOUNDING_CYLINDER

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_SPHERE:
            //---------------------------------------------
            #ifdef  TAPs_STRAND_LOCK_SEGMENTS
                for ( unsigned int s = 0; s < pSutureObj->GetNumSegments(); ++s ) {
                    // Skip locked or invisible segments
                    if ( pSutureObj->GetSegmentLockStatus(s) || !pSutureObj->GetSegmentVisibleStatus(s) ) {}
                    else {
                        for ( int i = pSutureObj->GetStartPtOfSegment(s); i <= pSutureObj->GetEndPtOfSegment(s); ++i )
            #else //TAPs_STRAND_LOCK_SEGMENTS
                {
                    {
                        for ( unsigned int i = 0; i <= pSutureObj->GetNumberOfStrandLinksWithoutNeedle(); ++i )
            #endif//TAPs_STRAND_LOCK_SEGMENTS
                        {
                            // If the point is fixed, do not move it.
                            if ( pSutureObj->GetFixStatusOfPtNo(i) )    continue;

                            //------------------------------------------------------
                            // In the local coordinate of the bounding sphere centered at the origin 
                            Vector3<T> location = pSutureObj->GetPointPosition(i);
                            location -= translation;
                            location = inversedRotMatrix * location;
                            // Shift the location of point by the sphere center
                            // i.e. shift the sphere to the origin (0,0,0)
                            location -= (*pos)->GetCenter();
                            //------------------------------------------------------
                            // Now the sphere is centered at the origin
                            // And the input point has been transformed into the sphere coordinate.
                            // The test can be performed.
                            if ( TAPs::CGMath<T>::CD_SphereAtOrigin_vs_Point( (*pos)->GetRadius(), location, vDistance ) )
                            {
                                bResult = true;
                                //-------------------------
                                // Now rotate it back by the rotation matrix (from the transform matrix).
                                // REMARK: Translation (from the transform matrix) is NOT applied back, 
                                //         because vDistance represents a displacement vector from 
                                //         the input point to the sphere surface.
                                //         I.e., the suture point is always in the world coordinates.
                                vDistance = transform.GetMatrixRotation() * vDistance;
                                //-------------------------
                                // This statement just ask the strand to move its collided points away
                                pSutureObj->ChangeOnlyPointPositionByAddedWithThisVector( i, vDistance );
                                T tDistance = vDistance.Length();
                                if ( tDeepestDistance > tDistance ) {
                                    tDeepestDistance = tDistance;
                                    iDeepestPoint = i;
                                    vDirection = vDistance;
                                }
                                //-------------------------
                            }
                        }// Back to Next Strand Point
                    }
                }
                break;
                // END: BOUNDING_SPHERE

        }//END: switch ( (*pos)->GetType() ) {...}
        ++pos;  // Next Bounding Volume
    }
    return bResult;
}
// END: CDR_Stuture_vs_MBV(...)
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
//=============================================================================
#endif//#if ( defined TAPs_SUTURE_MODEL_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )




#if ( defined TAPs_STRAND_MODEL_HPP && defined TAPs_DEFORM_MESH_HPP )
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_Strand_vs_DeformMesh ( 
    TAPs::OpenGL::ModelStrand<T> * pStrandObj,          
    TAPs::DeformMesh<T> const * const pDeformMeshObj    
)
{
    bool bResult = false;
    //---------------------------------------------------------------
    T distance, length;
    int iClosestRow, iClosestCol; 
    Vector3<T> strandPos, skinPos;
    int iNoRows;
    int iNoCols;
    pDeformMeshObj->GetNumberOfRowsAndColumns( iNoRows, iNoCols );
    for ( int i = 0; i < pStrandObj->GetNumberOfLinks(); ++i ) {
        iClosestRow = -1;
        iClosestCol = -1;
        distance = 0.1;
        strandPos = pStrandObj->GetPointPosition( i );
        for ( int r = 0; r < iNoRows; ++r ) {
            for ( int c = 0; c < iNoCols; ++c ) {
                skinPos = pDeformMeshObj->GetPositionOfPointNumber( r, c );
                length = (strandPos - skinPos).Length();
                if ( length < distance ) {
                    iClosestRow = r;
                    iClosestCol = c;
                    distance = length;
                }
            }
        }
        if ( iClosestRow >= 0 ) {
            bResult = true;
            pStrandObj->ChangeOnlyPointPositionByAddedWithThisVector( i, Vector3<T>( 0, 0.1, 0 ) );
        }
    }
    //---------------------------------------------------------------
    return bResult;
}
// END: CDR_Strand_vs_DeformMesh(...)
//-----------------------------------------------------------------------------
//=============================================================================
#endif//( defined TAPs_STRAND_MODEL_HPP && defined TAPs_DEFORM_MESH_HPP )




#if ( defined TAPs_SUTURE_MODEL_HPP && defined TAPs_DEFORM_MESH_HPP )
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_Suture_vs_DeformMesh ( 
    TAPs::OpenGL::ModelSuture<T> * pSutureObj,          
    TAPs::DeformMesh<T> const * const pDeformMeshObj    
)
{
    bool bResult = false;
    //---------------------------------------------------------------
    T distance, length;
    int iClosestRow, iClosestCol; 
    Vector3<T> suturePos, skinPos;
    int iNoRows;
    int iNoCols;
    pDeformMeshObj->GetNumberOfRowsAndColumns( iNoRows, iNoCols );
    for ( int i = 0; i < pSutureObj->GetNumberOfStrandLinksWithoutNeedle(); ++i ) {
        iClosestRow = -1;
        iClosestCol = -1;
        distance = 0.1;
        suturePos = pSutureObj->GetPointPosition( i );
        for ( int r = 0; r < iNoRows; ++r ) {
            for ( int c = 0; c < iNoCols; ++c ) {
                skinPos = pDeformMeshObj->GetPositionOfPointNumber( r, c );
                length = (suturePos - skinPos).Length();
                if ( length < distance ) {
                    iClosestRow = r;
                    iClosestCol = c;
                    distance = length;
                }
            }
        }
        if ( iClosestRow >= 0 ) {
            bResult = true;
            pSutureObj->ChangeOnlyPointPositionByAddedWithThisVector( i, Vector3<T>( 0, 0.1, 0 ) );
        }
    }
    //---------------------------------------------------------------
    return bResult;
}
// END: CDR_Stuture_vs_DeformMesh(...)
//-----------------------------------------------------------------------------
//=============================================================================
#endif//( defined TAPs_SUTURE_MODEL_HPP && defined TAPs_DEFORM_MESH_HPP )




#if ( defined TAPs_DEFORM_MESH_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_DeformMesh_vs_MBV ( 
    TAPs::DeformMesh<T> * pDeformMeshObj,                   
    TAPs::MultiBoundingVolume<T> const * const  pMBVObj,    
    TAPs::TransformationSupport<T> const * const pTransform 
)
{
    bool bResult = false;
    //---------------------------------------------------------------
    int iNoRows;
    int iNoCols;
    pDeformMeshObj->GetNumberOfRowsAndColumns( iNoRows, iNoCols );
    //---------------------------------------------------------------
    Vector3<T> vDistance;           // displacement vector
    TransformationSupport<T> transform;
    //---------------------------------------------------------------
    if ( pTransform ) {
        transform.ReturnMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    //---------------------------------------------------------------
    transform.ReturnMatrixTransform() *= pMBVObj->GetTransform().GetMatrixTransform();
    TransformationSupport<T> savedTransform( transform );
    //---------------------------------------------------------------
    std::vector< BoundingVolume<T> * >::const_iterator pos = pMBVObj->GetBoundingVolumeList().begin();
    //---------------------------------------------------------------
    while ( pos != pMBVObj->GetBoundingVolumeList().end() ) {
        assert( *pos != NULL );
        vDistance.SetXYZ( 0, 0, 0 );
        transform = savedTransform;
        //-------------------------------------------------
        transform.ReturnMatrixTransform() *= (*pos)->GetTransform().GetMatrixTransform();
        //-------------------------------------------------
        // For transforming each vertex of the deformable mesh to the bounding volume coordinate
        Vector3<T>      translation( transform.GetTranslation() );
        Matrix3x3<T>    inversedRotMatrix( transform.GetMatrixRotation().GetInverse() );

        switch ( (*pos)->GetType() ) {

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_CYLINDER:
            //---------------------------------------------
                // Transform each vertex of the flat skin to the bounding volume coordinate
                // Exclude the boundary
                for ( int r = 1; r < iNoRows-1; ++r ) {
                    for ( int c = 1; c < iNoCols-1; ++c ) {
                        //-------------------------------
                        // In the local coordinate of the bounding cylinder, 
                        // the cylinder axis is parallel to z-axis 
                        // where it is shifted by the cylinder center.
                        // The cylinder has radius, height and center.
                        // Its height is measured from -z and +z axis
                        // where its center is situated between height/2 in -z and +z axis.
                        Vector3<T> location = pDeformMeshObj->GetPositionOfPointNumber( r, c );
                        location -= translation;
                        location = inversedRotMatrix * location;
                        // Shift the location of point by the cylinder center
                        // i.e. shift the cylinder to the origin (0,0,0)
                        location -= (*pos)->GetCenter();
                        //-------------------------------
                        // Now the cylinder is centered at the origin with its axis on the z-axis
                        // where its height is from -height/2 to +height/2.
                        // And the input point has been transformed into the cylinder coordinate.
                        // The test can be performed.
                        if ( TAPs::CGMath<T>::CD_CylinderAtOrigin_vs_Point( (*pos)->GetRadius(), (*pos)->GetHeight(), location, vDistance ) )
                        {
                            bResult = true;
                            //-----------------------------
                            // Now rotate it back by the rotation matrix (from the transform matrix).
                            // REMARK: Translation (from the transform matrix) is NOT applied back, 
                            //         because vDistance represents a displacement vector from 
                            //         the input point to the cylinder surface.
                            //         I.e., the deformable mesh point is always in the world coordinates.
                            vDistance = transform.GetMatrixRotation() * vDistance;
                            //-----------------------------
                            // This statement just ask the vertex (row,col) of the flat skin to move its collided points away
                            //pDeformMeshObj->SetParticleFixStatus( r, c, true );   // DEBUG
                            //pDeformMeshObj->MovePositionOfPointNumber( r, c, vDistance );

                            // DEBUG (HACKING)
                            pDeformMeshObj->MovePositionOfPointNumber( r, c, Vector3<T>( 0, -1, 0 ) );
                            //-----------------------------
                        }
                    }// Back to the Next Column of the Flat Skin
                }// Back to the Next Row of the Flat Skin
                break;
                // END: BOUNDING_CYLINDER

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_SPHERE:
            //---------------------------------------------
                // Transform each vertex of the flat skin to the bounding volume coordinate
                // Exclude the boundary
                for ( int r = 1; r < iNoRows-1; ++r ) {
                    for ( int c = 1; c < iNoCols-1; ++c ) {
                        //-------------------------------
                        // In the local coordinate of the bounding cylinder, 
                        // the cylinder axis is parallel to z-axis 
                        // where it is shifted by the cylinder center.
                        // The cylinder has radius, height and center.
                        // Its height is measured from -z and +z axis
                        // where its center is situated between height/2 in -z and +z axis.
                        Vector3<T> location = pDeformMeshObj->GetPositionOfPointNumber( r, c );
                        location -= translation;
                        location = inversedRotMatrix * location;
                        // Shift the location of point by the cylinder center
                        // i.e. shift the cylinder to the origin (0,0,0)
                        location -= (*pos)->GetCenter();
                        //-------------------------------
                        // Now the cylinder is centered at the origin with its axis on the z-axis
                        // where its height is from -height/2 to +height/2.
                        // And the input point has been transformed into the cylinder coordinate.
                        // The test can be performed.
                        if ( TAPs::CGMath<T>::CD_SphereAtOrigin_vs_Point( (*pos)->GetRadius(), location, vDistance ) )
                        {
                            bResult = true;
                            //-----------------------------
                            // Now rotate it back by the rotation matrix (from the transform matrix).
                            // REMARK: Translation (from the transform matrix) is NOT applied back, 
                            //         because vDistance represents a displacement vector from 
                            //         the input point to the cylinder surface.
                            //         I.e., the deformable mesh point is always in the world coordinates.
                            vDistance = transform.GetMatrixRotation() * vDistance;
                            //-----------------------------
                            // This statement just ask the vertex (row,col) of the flat skin to move its collided points away
                            //pDeformMeshObj->SetParticleFixStatus( r, c, true );   // DEBUG
                            //pDeformMeshObj->MovePositionOfPointNumber( r, c, vDistance );

                            // DEBUG (HACKING)
                            pDeformMeshObj->MovePositionOfPointNumber( r, c, Vector3<T>( 0, -1, 0 ) );
                            //-----------------------------
                        }
                    }// Back to the Next Column of the Flat Skin
                }// Back to the Next Row of the Flat Skin
                break;
                // END: BOUNDING_SPHERE

        }//END: switch ( (*pos)->GetType() ) {...}
        //-------------------------------------------------
        ++pos;  // Next Bounding Volume
    }
    //---------------------------------------------------------------
    return bResult;
}
// END: CDR_DeformMesh_vs_MBV(...)
//-----------------------------------------------------------------------------
//=============================================================================
#endif//#if ( defined TAPs_DEFORM_MESH_HPP && defined TAPs_MULTI_BOUNDING_VOLUME_HPP )




#ifdef  TAPs_BOUNDING_VOLUME_HPP
//=============================================================================
// CDR_PolygonalMesh_vs_BV
//-----------------------------------------------------------------------------
//template <typename T>
//bool CDR_PolygonalMesh_vs_BV ( 
//  TAPs::OpenGL::PolygonalModel<T> * pPolygonalMeshObj,    //!< a PolygonalModel object
//  TAPs::BoundingVolume<T> const * const   pBVObj          //!< a bounding volume object
//)
//{
//  std::cout << "CDR_PolygonalMesh_vs_BV( PolygonalModel<T>, BoundingVolume<T> ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CDR_PolygonalMesh_vs_BV(...) for PolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
//template <typename T>
//bool CDR_PolygonalMesh_vs_BV ( 
//  TAPs::OpenGL::XPolygonalModel<T> * pPolygonalMeshObj,   //!< a PolygonalModel object
//  TAPs::BoundingVolume<T> const * const   pBVObj          //!< a bounding volume object
//)
//{
//  std::cout << "CDR_XPolygonalMesh_vs_BV( XPolygonalModel<T>, BoundingVolume<T> ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CDR_PolygonalMesh_vs_BV(...) for XPolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
template <typename T>
bool CDR_PolygonalMesh_vs_BV ( 
    TAPs::OpenGL::HalfEdgeModel<T> * pPolygonalMeshObj,     
    TAPs::BoundingVolume<T> const * const   pBVObj          
)
{
    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    //pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "CDR_HalfEdgeMesh_vs_BV( HalfEdgeModel<T>, BoundingVolume<T> )\n";
    #endif//TAPs_DEBUG_MODE


    //std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    //---------------------------------------------------------------
    if ( pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pBVObj ) )
    {
        //-------------------------------------------------
        T separateDistanceConstraint;
        Vector3<T> N, centerA;
        T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        std::vector< BVHNode<T> * > *   listOfCollidedNode_Def   = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedNodes();
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        //std::cout << "listOfCollidedNode_Def SIZE: " << listOfCollidedNode_Def->size() << "\n";

        // Collision Response
        //-------------------------------------------------
        // Deform the deformable polygonal mesh away from the rigid polygonal mesh
        switch ( pBVObj->GetType() ) {
            case TAPs::Enum::BOUNDING_SPHERE:
                for ( int i = 0; i < static_cast<int>( listOfCollidedNode_Def->size() ); ++i ) {

                    separateDistanceConstraint = 0.5 * ( (*listOfCollidedNode_Def)[i]->GetRadius() + pBVObj->GetRadius() );
                    //centerA = ( pRigidPolyMesh->GetTransform().GetMatrixTransform() * 
                    //          Vector4<T>( (*listOfCollidedNode_Rigid)[i]->GetCenter() ) ).GetVector3();
                    centerA = pBVObj->GetCenter();
                    N = centerA - (*listOfCollidedNode_Def)[i]->GetCenter();
                    separateDistance = separateDistanceConstraint - N.Length();
                    if ( true ) {
                    //if ( separateDistance > Math<T>::ZERO ) {
                        N.Normalized();
                        N *= separateDistance;
                        primB = (*listOfCollidedNode_Def)[i]->GetAPrimitiveHalfEdgeFace();
                        vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                        //-------------------------------
                        // For each vertex in a primitive
                        for ( int j = 0; j < noOfVertices; ++j ) {
                            //vertexList[j]->SetPosition( vertexList[j]->GetPosition() - N );
                            vertexList[j]->SetPosition( vertexList[j]->GetPosition() - Vector3<T>( 0.01, 0.01, 0.01 ) );
                            //vertexList[j]->
                        }
                    }

                    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
                    //(*listOfCollidedNode_Def)[i]->flag = true;
                    #endif//TAPs_DEBUG_COLLISION_DETECTION

                    // List of BVNodes to be updated
                    defPolyMesh_NodeSet.insert( (*listOfCollidedNode_Def)[i] );
                }
                break;

            default:
                std::cout << "CDR_PolygonalMesh_vs_BV of BV Type (" << pBVObj->GetType() << ") doesn't support!\n";
                break;
        }
        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        pPolygonalMeshObj->CalAndSetNormals();
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
        //-------------------------------------------------
        return true;
    }
    //---------------------------------------------------------------
    return false;
}
// END: CDR_PolygonalMesh_vs_BV(...) for HalfEdgeModel
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
template <typename T>
bool CDR_PolygonalMesh_vs_BV ( 
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pPolygonalMeshObj,   
    TAPs::BoundingVolume<T> const * const   pBVObj          
)
{
    ListOfContactPts.clear();
    ListOfForces.clear();

    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    std::cout << "CDR_PolygonalMesh_vs_BV( HalfEdgeModel<T>, BoundingVolume<T> )\n";
    //#endif//TAPs_DEBUG_MODE

    std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pBVObj );

    //---------------------------------------------------------------
    // Collision Response
    if ( bResult ) {

        // Reset the result for primitive-primitive test, 
        // because the previous result is only from bounding volume test.
        bResult = false;

        //-------------------------------------------------
        T separateDistanceConstraint;
        Vector3<T> N, centerA;
        T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        TransformationSupport<T> trxMBV;    // default constructor is an identity transformation

        //-------------------------------------------------
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        // Get the list of collided nodes and bounding volumes
        std::vector< BVsAndNodesList<T> > * bvList = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();
        //-------------------------------------------------
        // Traverse the list of bounding volumes
        std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin();
        //-------------------------------------------------
        //std::cout << "CDR_PolygonalMesh_vs_BV(...) for HETriMeshOneModelMultiParts --> \n";
        //std::cout << "\tCASE: the polygonal mesh object's transformation is on. -- NOT IMPLEMENTED YET!\n";

        // Mesh's inverse transformation
        Matrix4x4<T> trxInvMesh = pPolygonalMeshObj->GetTransform().GetMatrixTransform();
        trxInvMesh.Inversed();
        Vector3<T> vOrigin( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(0,0,0,1)).GetVector3() );

        while ( pos != bvList->end() ) {

            int count = 0;

            // Store the transformation of the bounding volume including the mesh's inverse transformation
            TransformationSupport<T> trx = (*pos).BV->GetTransform();
            (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trx.GetMatrixTransform() );

            //-----------------------------------------
            // Deform the deformable polygonal mesh away from the rigid polygonal mesh
            switch ( (*pos).BV->GetType() ) {

                case TAPs::Enum::BOUNDING_SPHERE:
                    {

                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVSphere
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            //Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 
                                    centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]     // out vectors
                                    //, &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                //ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/


                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                case TAPs::Enum::BOUNDING_CYLINDER:
                    {
                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0;
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();
                        Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVCylinder
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 
                                    topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord,    // BVCylinder properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]         // out vectors
                                    , &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                contactPt = ( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(contactPt)).GetVector3() );

                                ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/

                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                default:
                    std::cout << "CDR_PolygonalMesh_vs_BV of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n";
                    break;
            }

            // Restore the transformation of the bounding volume
            //(*pos).BV->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() );
            (*pos).BV->SetTransform( trx );
            // Next Bounding Volume
            ++pos;
        }
        //-------------------------------------------------

        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        //pPolygonalMeshObj->CalAndSetNormals();        // commented out to rely on AdvanceSimulation to update the normals
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
        // FOR HETriMeshOneModelMultiParts
        //-------------------------------------------------
        // Update for the model's internal simulation
        for ( int i = 0; i < static_cast<int>( pPolygonalMeshObj->GetListOfParts().size() ); ++i ) {
            pPolygonalMeshObj->GetPtrToPartNo(i)->UpdateStateToArray();
        }
    }
    //---------------------------------------------------------------
    if ( totalForce ) {
        totalForce->SetXYZ( 0, 0, 0 );
        Vector3<float> force( 0, 0, 0 );
        for ( int i = 0; i < ListOfForces.size(); ++i ) {
            force += ListOfForces[i];
        }
        force *= gain;
        (*totalForce).SetXYZ( force[0], force[1], force[2] );
    }

    return bResult;
}
// END: CDR_PolygonalMesh_vs_BV(...) for HETriMeshOneModelMultiParts
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
//=============================================================================
#endif//TAPs_BOUNDING_VOLUME_HPP




//=============================================================================
//-----------------------------------------------------------------------------
//template <typename T>
//bool CD_PolygonalMesh_vs_LineSegment ( 
//  TAPs::OpenGL::PolygonalModel<T> * pPolygonalMeshObj,    //!< a PolygonalModel object
//  TAPs::Vector3<T> const * const  ptA,    //!< the starting point of a line segment
//  TAPs::Vector3<T> const * const  ptB,    //!< the ending point of a line segment
//  std::vector< TAPs::Vector3<T> > * listOfIntersectedPoints,  //!< O/P: list of intersected points
//  std::vector< TAPs::Face<T>* > * listOfIntersectedFaces, //!< O/P: list of intersected faces
//  std::vector< TAPs::Vertex<T>* > * listOfClosestVertices //!< O/P: list of closest vertices
//)
//{
//  std::cout << __FILE__ << " " << __LINE__ << " ";
//  std::cout << "CD_PolygonalMesh_vs_LineSegment( PolygonalModel<T>, ... ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CD_PolygonalMesh_vs_LineSegment(...) for PolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
//template <typename T>
//bool CD_PolygonalMesh_vs_LineSegment ( 
//  TAPs::OpenGL::XPolygonalModel<T> * pPolygonalMeshObj,   //!< a PolygonalModel object
//  TAPs::Vector3<T> const * const  ptA,    //!< the starting point of a line segment
//  TAPs::Vector3<T> const * const  ptB,    //!< the ending point of a line segment
//  std::vector< TAPs::Vector3<T> > * listOfIntersectedPoints,  //!< O/P: list of intersected points
//  std::vector< TAPs::Face<T>* > * listOfIntersectedFaces,     //!< O/P: list of intersected faces
//  std::vector< TAPs::XVertex<T>* > * listOfClosestVertices    //!< O/P: list of closest vertices
//)
//{
//  std::cout << __FILE__ << " " << __LINE__ << " ";
//  std::cout << "CD_PolygonalMesh_vs_LineSegment( XPolygonalModel<T>, ... ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CD_PolygonalMesh_vs_LineSegment(...) for XPolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
template <typename T>
bool CD_PolygonalMesh_vs_LineSegment ( 
    TAPs::OpenGL::HalfEdgeModel<T> * pPolygonalMeshObj,     
    TAPs::Vector3<T> const * const  ptA,    
    TAPs::Vector3<T> const * const  ptB,    
    std::vector< TAPs::Vector3<T> > * listOfIntersectedPoints,  
    std::vector< TAPs::HEFace<T>* > * listOfIntersectedFaces,   
    std::vector< TAPs::HEVertex<T>* > * listOfClosestVertices   
)
{
    std::cout << __FILE__ << " " << __LINE__ << " ";
    std::cout << "CD_PolygonalMesh_vs_LineSegment( HalfEdgeModel<T>, ... ) -- NOT IMPLEMENTED YET!" << std::endl;

    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    std::cout << "CD_PolygonalMesh_vs_LineSegment( HalfEdgeModel<T>, ... )\n";
    //#endif//TAPs_DEBUG_MODE

    std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestIntersectionWithLineSegmentTillLeafNodes( ptA, ptB );
    //---------------------------------------------------------------
    // Collision Response
    //if ( bResult ) {
    //}
    //---------------------------------------------------------------
    return bResult;
}
// END: CD_PolygonalMesh_vs_LineSegment(...) for HalfEdgeModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
template <typename T>
bool CD_PolygonalMesh_vs_LineSegment ( 
    TAPs::OpenGL::HalfEdgeModel<T> * pPolygonalMeshObj, 
    TAPs::Vector3<T> const * const  ptA,    
    TAPs::Vector3<T> const * const  ptB,    
    std::vector< TAPs::Vector3<T> > * listOfIntersectedPoints,  
    std::vector< TAPs::HEFace<T>* > * listOfIntersectedFaces,   
    std::vector< TAPs::HEVertex<T>* > * listOfClosestVertices,  
    std::vector< T > * listOfRatios,                            
    std::vector< T > * listOfIntersectionAngles                 
)
{
    ListOfContactPts.clear();
    ListOfForces.clear();

    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestIntersectionWithLineSegmentTillLeafNodes( ptA, ptB );
    //---------------------------------------------------------------
    if ( bResult ) {

        // Include pPolygonalMeshObj's transformation
        TAPs::Matrix4x4<T> invTrx( pPolygonalMeshObj->GetTransform().RefToMatrixTransform().GetInverse() );
        TAPs::Vector3<T> A = invTrx * *ptA;
        TAPs::Vector3<T> B = invTrx * *ptB;

        std::vector< BVHNode<T> * > & nodeList = pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedNodes();
        T ratio, intersectionAngle;
        Vector3<T> projPt;
        HEFace<T> * triangle;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        for ( int i = 0; i < static_cast<int>( nodeList.size() ); ++i ) {
            // Check intersection of the line segment with the primitive triangle in the BV node
            triangle = nodeList[i]->GetAPrimitiveHalfEdgeFace();
            vertexList = triangle->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
            bool result = CGMath<T>::FindIntersectionLineSegmentTriangle( 
                                A, B,       // I/P: a line segment
                                vertexList[0]->GetPosition(),   // I/P: the triangle
                                vertexList[1]->GetPosition(), 
                                vertexList[2]->GetPosition(), 
                                ratio,              // O/P: the ratio
                                intersectionAngle,  // O/P: the intersection angle (in degrees)
                                projPt              // O/P: the projected point
                            );
            if ( result ) {
                #ifdef  TAPs_ADVANCED_SIMULATION
                    // Sort the elements by ratio -- from low to high
                    std::vector< T >::iterator itR = listOfRatios->begin();
                    std::vector< T >::iterator itA = listOfIntersectionAngles->begin();
                    std::vector< TAPs::Vector3<T> >::iterator itIP = listOfIntersectedPoints->begin();
                    std::vector< TAPs::HEFace<T>* >::iterator itIF = listOfIntersectedFaces->begin();
                    std::vector< TAPs::HEVertex<T>* >::iterator itCV = listOfClosestVertices->begin();
                    while ( itR != listOfRatios->end() ) {
                        if ( *itR > ratio ) {
                            if ( itR != listOfRatios->begin() ) {
                                --itR;
                                --itA;
                                --itIP;
                                --itIF;
                                --itCV;
                            }
                            break;
                        }
                        ++itR;
                        ++itA;
                        ++itIP;
                        ++itIF;
                        ++itCV;
                    }
                    listOfIntersectedPoints->insert( itIP, projPt );
                    listOfIntersectedFaces->insert( itIF, triangle );
                    listOfRatios->insert( itR, ratio );
                    listOfIntersectionAngles->insert( itA, intersectionAngle );

                    //listOfIntersectedPoints->push_back( projPt );
                    //listOfIntersectedFaces->push_back( triangle );
                    //listOfRatios->push_back( ratio );
                    
                    // Find the closest point
                    T distance0 = ( projPt - vertexList[0]->GetPosition() ).Length();
                    T distance1 = ( projPt - vertexList[1]->GetPosition() ).Length();
                    T distance2 = ( projPt - vertexList[2]->GetPosition() ).Length();
                    if ( distance0 < distance1 ) {
                        if ( distance0 < distance2 ) {
                            listOfClosestVertices->insert( itCV, vertexList[0] );
                            //listOfClosestVertices->push_back( vertexList[0] );
                            //vertexList[0]->SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::PUNCTURED );
                        }
                        else {
                            listOfClosestVertices->insert( itCV, vertexList[2] );
                            //listOfClosestVertices->push_back( vertexList[2] );
                            //vertexList[2]->SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::PUNCTURED );
                        }
                    }
                    else {
                        if ( distance1 < distance2 ) {
                            listOfClosestVertices->insert( itCV, vertexList[1] );
                            //listOfClosestVertices->push_back( vertexList[1] );
                            //vertexList[1]->SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::PUNCTURED );
                        }
                        else {
                            listOfClosestVertices->insert( itCV, vertexList[2] );
                            //listOfClosestVertices->push_back( vertexList[2] );
                            //vertexList[2]->SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::PUNCTURED );
                        }
                    }

                #endif//TAPs_ADVANCED_SIMULATION
                //std::cout << __FILE__ << " " << __LINE__ << " -- ";
                //std::cout << "Line Segment intersected Node " << nodeList[i] << "\n";
            }
        }
    }
    return bResult;
}
// END: CD_PolygonalMesh_vs_LineSegment(...) for HETriMeshOneModelMultiParts
//-----------------------------------------------------------------------------
//=============================================================================




#ifdef  TAPs_MULTI_BOUNDING_VOLUME_HPP
//=============================================================================
//-----------------------------------------------------------------------------
//template <typename T>
//bool CDR_PolygonalMesh_vs_MBV ( 
//  TAPs::OpenGL::PolygonalModel<T> * pPolygonalMeshObj,    //!< a PolygonalModel object
//  TAPs::MultiBoundingVolume<T> const * const pMBVObj      //!< a multi bounding volume object
//)
//{
//  std::cout << "CDR_PolygonalMesh_vs_MBV( PolygonalModel<T>, MultiBoundingVolume<T> ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CDR_PolygonalMesh_vs_MBV(...) for PolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
//template <typename T>
//bool CDR_PolygonalMesh_vs_MBV ( 
//  TAPs::OpenGL::XPolygonalModel<T> * pPolygonalMeshObj,   //!< a PolygonalModel object
//  TAPs::MultiBoundingVolume<T> const * const pMBVObj      //!< a multi bounding volume object
//)
//{
//  std::cout << "CDR_PolygonalMesh_vs_MBV( XPolygonalModel<T>, MultiBoundingVolume<T> ) -- NOT IMPLEMENTED YET!" << std::endl;
//  bool bResult = false;
//  //---------------------------------------------------------------
//  //---------------------------------------------------------------
//  return bResult;
//}
// END: CDR_PolygonalMesh_vs_MBV(...) for XPolygonalModel
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
template <typename T>
bool CDR_PolygonalMesh_vs_MBV ( 
    TAPs::OpenGL::HalfEdgeModel<T> * pPolygonalMeshObj,     
    TAPs::MultiBoundingVolume<T> const * const pMBVObj      
)
{
    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    std::cout << "CDR_PolygonalMesh_vs_MBV( HalfEdgeModel<T>, MultiBoundingVolume<T> )\n";
    //#endif//TAPs_DEBUG_MODE

    std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj );

    //---------------------------------------------------------------
    // Collision Response
    if ( bResult ) {

        // Reset the result for primitive-primitive test, 
        // because the previous result is only from bounding volume test.
        bResult = false;

        //-------------------------------------------------
        T separateDistanceConstraint;
        Vector3<T> N, centerA;
        T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        TransformationSupport<T> trxMBV;    // default constructor is an identity transformation
        trxMBV = pMBVObj->GetTransform();
        //-------------------------------------------------
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        // Get the list of collided nodes and bounding volumes
        std::vector< BVsAndNodesList<T> > * bvList = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();
        //-------------------------------------------------
        // Traverse the list of bounding volumes
        std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin();
        //-------------------------------------------------
        //std::cout << "CDR_PolygonalMesh_vs_MBV(...) for HalfEdgeModel --> \n";
        //std::cout << "\tCASE: the polygonal mesh object's transformation is on. -- NOT IMPLEMENTED YET!\n";

        // Mesh's inverse transformation
        Matrix4x4<T> trxInvMesh = pPolygonalMeshObj->GetTransform().GetMatrixTransform();
        trxInvMesh.Inversed();

        while ( pos != bvList->end() ) {

            int count = 0;

            // Store the transformation of the bounding volume including the mesh's inverse transformation
            TransformationSupport<T> trx = (*pos).BV->GetTransform();
            (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.GetMatrixTransform() * trx.GetMatrixTransform() );

            //-----------------------------------------
            // Deform the deformable polygonal mesh away from the rigid polygonal mesh
            switch ( (*pos).BV->GetType() ) {

                case TAPs::Enum::BOUNDING_SPHERE:
                    {

                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVSphere
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[j])).GetVector3() );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            //Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 
                                    centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]     // out vectors
                                    //, &contactPt
                            ) ) {
                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[0])).GetVector3()*CDScaleForSphereBVWithTriangle );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[1])).GetVector3()*CDScaleForSphereBVWithTriangle );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[2])).GetVector3()*CDScaleForSphereBVWithTriangle );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/


                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                case TAPs::Enum::BOUNDING_CYLINDER:
                    {
                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0;
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();
                        Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVCylinder
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[j])).GetVector3() );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 
                                    topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord,    // BVCylinder properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]         // out vectors
                            ) ) {
                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[0])).GetVector3()*CDScaleForCylinderBVWithTriangle );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[1])).GetVector3()*CDScaleForCylinderBVWithTriangle );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[2])).GetVector3()*CDScaleForCylinderBVWithTriangle );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/

                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                default:
                    std::cout << "CDR_PolygonalMesh_vs_MBV of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n";
                    break;
            }

            // Restore the transformation of the bounding volume
            (*pos).BV->SetTransform( trx );
            // Next Bounding Volume
            ++pos;
        }
        //-------------------------------------------------

        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        pPolygonalMeshObj->CalAndSetNormals();
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
    }
    //---------------------------------------------------------------
    return bResult;
}
// END: CDR_PolygonalMesh_vs_MBV(...) for HalfEdgeModel
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
template <typename T>
bool CDRigid_PolygonalMesh_vs_MBV ( 
    TAPs::OpenGL::HalfEdgeModel<T> * pPolygonalMeshObj,     
    TAPs::MultiBoundingVolume<T> const * const pMBVObj      
)
{
    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    std::cout << "CDR_PolygonalMesh_vs_MBV( HalfEdgeModel<T>, MultiBoundingVolume<T> )\n";
    //#endif//TAPs_DEBUG_MODE

    std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj );

    //---------------------------------------------------------------
    // Collision Response
    if ( bResult ) {

        // Reset the result for primitive-primitive test, 
        // because the previous result is only from bounding volume test.
        bResult = false;

        //-------------------------------------------------
        T separateDistanceConstraint;
        Vector3<T> N, centerA;
        T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        TransformationSupport<T> trxMBV;    // default constructor is an identity transformation
        trxMBV = pMBVObj->GetTransform();
        //-------------------------------------------------
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        // Get the list of collided nodes and bounding volumes
        std::vector< BVsAndNodesList<T> > * bvList = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();
        //-------------------------------------------------
        // Traverse the list of bounding volumes
        std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin();
        //-------------------------------------------------
        //std::cout << "CDR_PolygonalMesh_vs_MBV(...) for HalfEdgeModel --> \n";
        //std::cout << "\tCASE: the polygonal mesh object's transformation is on. -- NOT IMPLEMENTED YET!\n";

        // Mesh's inverse transformation
        Matrix4x4<T> trxInvMesh = pPolygonalMeshObj->GetTransform().GetMatrixTransform();
        trxInvMesh.Inversed();

        while ( pos != bvList->end() ) {

            int count = 0;

            // Store the transformation of the bounding volume including the mesh's inverse transformation
            TransformationSupport<T> trx = (*pos).BV->GetTransform();
            (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.GetMatrixTransform() * trx.GetMatrixTransform() );

            //-----------------------------------------
            // Deform the deformable polygonal mesh away from the rigid polygonal mesh
            switch ( (*pos).BV->GetType() ) {

                case TAPs::Enum::BOUNDING_SPHERE:
                    {

                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVSphere
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[j])).GetVector3() );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            //Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 
                                    centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]     // out vectors
                                    //, &contactPt
                            ) ) {
                                //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[0])).GetVector3()*CDScaleForSphereBVWithTriangle );
                                //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[1])).GetVector3()*CDScaleForSphereBVWithTriangle );
                                //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[2])).GetVector3()*CDScaleForSphereBVWithTriangle );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/


                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                case TAPs::Enum::BOUNDING_CYLINDER:
                    {
                        std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0;
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();
                        Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );
                        //---------------------------------------------

                        count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVCylinder
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[j])).GetVector3() );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 
                                    topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord,    // BVCylinder properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]         // out vectors
                            ) ) {
                                //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[0])).GetVector3()*CDScaleForCylinderBVWithTriangle );
                                //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[1])).GetVector3()*CDScaleForCylinderBVWithTriangle );
                                //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + (trxInvMesh * Vector4<T>(cdrVec[2])).GetVector3()*CDScaleForCylinderBVWithTriangle );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/

                            std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                default:
                    std::cout << "CDR_PolygonalMesh_vs_MBV of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n";
                    break;
            }

            // Restore the transformation of the bounding volume
            (*pos).BV->SetTransform( trx );
            // Next Bounding Volume
            ++pos;
        }
        //-------------------------------------------------

        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        //pPolygonalMeshObj->CalAndSetNormals();
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        //pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
    }
    //---------------------------------------------------------------
    return bResult;
}
// END: CDRigid_PolygonalMesh_vs_MBV(...) for HalfEdgeModel
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
template <typename T>
bool CDR_PolygonalMesh_vs_MBV ( 
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pPolygonalMeshObj,   
    TAPs::MultiBoundingVolume<T> const * const pMBVObj      
    , TAPs::Vector3<T> * totalForce     
    , T gain                            
)
{
    ListOfContactPts.clear();
    ListOfForces.clear();

    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    //std::cout << "CDR_PolygonalMesh_vs_MBV( HalfEdgeModel<T>, MultiBoundingVolume<T> )\n";
    //#endif//TAPs_DEBUG_MODE

    //std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj );

    //---------------------------------------------------------------
    // Collision Response
    if ( bResult ) {

        // Reset the result for primitive-primitive test, 
        // because the previous result is only from bounding volume test.
        bResult = false;

        //-------------------------------------------------
        //T separateDistanceConstraint;
        Vector3<T> N, centerA;
        //T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        TransformationSupport<T> trxMBV( pMBVObj->GetTransform() );
        //-------------------------------------------------
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        // Get the list of collided nodes and bounding volumes
        std::vector< BVsAndNodesList<T> > * bvList = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();
        //-------------------------------------------------
        // Traverse the list of bounding volumes
        std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin();
        //-------------------------------------------------
        //std::cout << "CDR_PolygonalMesh_vs_MBV(...) for HETriMeshOneModelMultiParts --> \n";
        //std::cout << "\tCASE: the polygonal mesh object's transformation is on. -- NOT IMPLEMENTED YET!\n";

        // Mesh's inverse transformation
        Matrix4x4<T> trxInvMesh = pPolygonalMeshObj->GetTransform().GetMatrixTransform();
        trxInvMesh.Inversed();
        Vector3<T> vOrigin( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(0,0,0,1)).GetVector3() );

        while ( pos != bvList->end() ) {

            //int count = 0;

            // Store the transformation of the bounding volume including the mesh's inverse transformation
            TransformationSupport<T> trx = (*pos).BV->GetTransform();
            (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.GetMatrixTransform() * trx.GetMatrixTransform() );

            //-----------------------------------------
            // Deform the deformable polygonal mesh away from the rigid polygonal mesh
            switch ( (*pos).BV->GetType() ) {

                case TAPs::Enum::BOUNDING_SPHERE:
                    {

                        //std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        //---------------------------------------------

                        //count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVSphere
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            //Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 
                                    centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]     // out vectors
                                    //, &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                //ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/


                            //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                case TAPs::Enum::BOUNDING_CYLINDER:
                    {
                        //std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0;
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();
                        Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );
                        //---------------------------------------------

                        //count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVCylinder
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 
                                    topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord,    // BVCylinder properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]         // out vectors
                                    , &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                contactPt = ( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(contactPt)).GetVector3() );

                                ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/

                            //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                default:
                    std::cout << "CDR_PolygonalMesh_vs_MBV of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n";
                    break;
            }

            // Restore the transformation of the bounding volume
            //(*pos).BV->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() );
            (*pos).BV->SetTransform( trx );
            // Next Bounding Volume
            ++pos;
        }
        //-------------------------------------------------

        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        //pPolygonalMeshObj->CalAndSetNormals();        // commented out to rely on AdvanceSimulation to update the normals
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
        // FOR HETriMeshOneModelMultiParts
        //-------------------------------------------------
        // Update for the model's internal simulation
        for ( int i = 0; i < static_cast<int>( pPolygonalMeshObj->GetListOfParts().size() ); ++i ) {
            pPolygonalMeshObj->GetPtrToPartNo(i)->UpdateStateToArray();
        }
    }
    //---------------------------------------------------------------
    if ( totalForce ) {
        totalForce->SetXYZ( 0, 0, 0 );
        Vector3<float> force( 0, 0, 0 );
        for ( int i = 0; i < static_cast<int>( ListOfForces.size() ); ++i ) {
            force += ListOfForces[i];
        }
        force *= gain;
        (*totalForce).SetXYZ( force[0], force[1], force[2] );
    }

    return bResult;
}
// END: CDR_PolygonalMesh_vs_MBV(...) for HETriMeshOneModelMultiParts
//-----------------------------------------------------------------------------





//-----------------------------------------------------------------------------
template <typename T>
bool CDRigid_PolygonalMesh_vs_MBV ( 
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pPolygonalMeshObj,   
    TAPs::MultiBoundingVolume<T> const * const pMBVObj      
    , TAPs::Vector3<T> * totalForce     
    , T gain                            
)
{
    ListOfContactPts.clear();
    ListOfForces.clear();

    #ifdef  TAPs_DEBUG_COLLISION_DETECTION
    pPolygonalMeshObj->GetBVHTree()->ClearFlags();
    #endif//TAPs_DEBUG_COLLISION_DETECTION

    //#ifdef    TAPs_DEBUG_MODE
    //std::cout << "CDR_PolygonalMesh_vs_MBV( HalfEdgeModel<T>, MultiBoundingVolume<T> )\n";
    //#endif//TAPs_DEBUG_MODE

    //std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n";

    bool bResult = false;

    // Collision Detection
    bResult = pPolygonalMeshObj->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj );

    //---------------------------------------------------------------
    // Collision Response
    if ( bResult ) {

        // Reset the result for primitive-primitive test, 
        // because the previous result is only from bounding volume test.
        bResult = false;

        //-------------------------------------------------
        //T separateDistanceConstraint;
        Vector3<T> N, centerA;
        //T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        TransformationSupport<T> trxMBV( pMBVObj->GetTransform() );
        //-------------------------------------------------
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;

        // Get the list of collided nodes and bounding volumes
        std::vector< BVsAndNodesList<T> > * bvList = &pPolygonalMeshObj->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();
        //-------------------------------------------------
        // Traverse the list of bounding volumes
        std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin();
        //-------------------------------------------------
        // Mesh's inverse transformation
        Matrix4x4<T> trxInvMesh = pPolygonalMeshObj->GetTransform().GetMatrixTransform();
        trxInvMesh.Inversed();
        Vector3<T> vOrigin( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(0,0,0,1)).GetVector3() );

        while ( pos != bvList->end() ) {

            //int count = 0;

            // Store the transformation of the bounding volume including the mesh's inverse transformation
            TransformationSupport<T> trx = (*pos).BV->GetTransform();
            (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.GetMatrixTransform() * trx.GetMatrixTransform() );

            //-----------------------------------------
            // Deform the deformable polygonal mesh away from the rigid polygonal mesh
            switch ( (*pos).BV->GetType() ) {

                case TAPs::Enum::BOUNDING_SPHERE:
                    {

                        //std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        //---------------------------------------------

                        //count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVSphere
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            //Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 
                                    centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]     // out vectors
                                    //, &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                //ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/


                            //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                case TAPs::Enum::BOUNDING_CYLINDER:
                    {
                        //std::cout << "BV: " << (*pos).BV->GetName() << "\n";

                        // For Test against triangle
                        //---------------------------------------------
                        // Transform the BV due to its transformation, center, and radius
                        T radiusOfBV = (*pos).BV->GetRadius();
                        Vector3<T> C( (*pos).BV->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( radiusOfBV, radiusOfBV, radiusOfBV );
                        Matrix4x4<T> scale( S[0], 0,    0,    0, 
                                            0,    S[1], 0,    0,
                                            0,    0,    S[2], 0,
                                            0,    0,    0,    1 );
                        // The BV's current transformation includes the mesh's inverse transformation (see the code above)
                        Matrix4x4<T> mat = (*pos).BV->GetTransform().GetMatrixTransform() * translation * scale;

                        // BV Sphere's center and radius after the whole transformation from 
                        // BV's transformation to mesh's inverse transformation.
                        // I.e., transforming the BV to mesh's coordinate
                        T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0;
                        Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3();
                        T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length();
                        Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );

                        // Inverse the mesh's transformation to the BV's pure transformation
                        mat = pPolygonalMeshObj->GetTransform().GetMatrixTransform() * mat;

                        // BV Sphere's center and radius after the BV's pure transformation
                        Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() );
                        radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length();
                        Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() );
                        Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() );
                        //---------------------------------------------

                        //count = 0;
                        for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) {

                            bool testResult = false;

                            // Assume triangle is in Sphere Node
                            // Test the triangle in sphere node with BVCylinder
                            primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace();
                            vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                            Vector3<T> cdrVec[3];

                            //*
                            // Test against each vertex in the triangle
                            //-----------------------------------
                            for ( int j = 0; j < noOfVertices; ++j ) {
                                if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) )
                                {
                                    //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] );
                                    Vector3<T> force( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin );

                                    //ListOfContactPts.push_back( Vector3<GLfloat>( (*pos).BV->GetCenter()[0], (*pos).BV->GetCenter()[1], (*pos).BV->GetCenter()[2] ) );
                                    ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) );
                                    ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                    testResult = true;
                                }
                            }
                            //-----------------------------------
                            //*/

                            //*
                            // Test against triangle
                            //-----------------------------------
                            Vector3<T> contactPt;
                            if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 
                                    topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord,    // BVCylinder properties
                                    vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices
                                    cdrVec[0], cdrVec[1], cdrVec[2]         // out vectors
                                    , &contactPt
                            ) ) {
                                Vector3<T> force_1( cdrVec[0] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_2( cdrVec[1] * CDScaleForSphereBVWithTriangle );
                                Vector3<T> force_3( cdrVec[2] * CDScaleForSphereBVWithTriangle );

                                //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 );
                                //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 );
                                //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 );

                                force_1 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin;
                                force_2 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin;
                                force_3 = (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin;

                                contactPt = ( (pPolygonalMeshObj->GetTransform().GetMatrixTransform() * Vector4<T>(contactPt)).GetVector3() );

                                ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) );
                                Vector3<T> force( ( force_1 + force_2 + force_3 )/3 );
                                ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) );

                                testResult = true;
                            }
                            //-----------------------------------
                            //*/

                            //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n";

                            // List of BVNodes to be updated
                            if ( testResult ) {
                                defPolyMesh_NodeSet.insert( (*pos).NodeList[i] );
                                bResult = true;
                            }
                        }
                    }
                    break;

                default:
                    std::cout << "CDRigid_PolygonalMesh_vs_MBV of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n";
                    break;
            }

            // Restore the transformation of the bounding volume
            //(*pos).BV->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() );
            (*pos).BV->SetTransform( trx );
            // Next Bounding Volume
            ++pos;
        }
        //-------------------------------------------------

        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        //pPolygonalMeshObj->CalAndSetNormals();        // commented out to rely on AdvanceSimulation to update the normals
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        //pPolygonalMeshObj->GetBVHTree()->Update( defPolyMesh_NodeSet );
        // FOR HETriMeshOneModelMultiParts
        //-------------------------------------------------
        // Update for the model's internal simulation
        //for ( int i = 0; i < static_cast<int>( pPolygonalMeshObj->GetListOfParts().size() ); ++i ) {
        //  pPolygonalMeshObj->GetPtrToPartNo(i)->UpdateStateToArray();
        //}
    }
    //---------------------------------------------------------------
    if ( totalForce ) {
        totalForce->SetXYZ( 0, 0, 0 );
        Vector3<float> force( 0, 0, 0 );
        for ( int i = 0; i < static_cast<int>( ListOfForces.size() ); ++i ) {
            force += ListOfForces[i];
        }
        force *= gain;
        (*totalForce).SetXYZ( force[0], force[1], force[2] );
    }

    return bResult;
}




//-----------------------------------------------------------------------------
//=============================================================================
#endif//TAPs_MULTI_BOUNDING_VOLUME_HPP





//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
bool CDR_DeformPolygonalMesh_vs_RigidPolygonalMesh ( 
    TAPs::OpenGL::HalfEdgeModel<T> *             pDefPolyMesh,      
    TAPs::OpenGL::HalfEdgeModel<T> const * const pRigidPolyMesh     
)
{
    //---------------------------------------------------------------
    if ( pDefPolyMesh->GetBVHTree()->TestOverlapWithTillLeafNodes( pRigidPolyMesh->GetBVHTree() ) )
    {
        //-------------------------------------------------
        T separateDistanceConstraint;
        Vector3<T> N, centerA;
        T separateDistance;
        HEFace<T> * primB;
        std::vector< HEVertex<T> * const > vertexList;
        int noOfVertices;
        //-------------------------------------------------
        std::vector< BVHNode<T> * > *   listOfCollidedNode_Def   = &pDefPolyMesh->GetBVHTree()->GetListOfCollidedNodes();
        std::vector< BVHNode<T> * > *   listOfCollidedNode_Rigid = &pDefPolyMesh->GetBVHTree()->GetListOfCollidedNodesThat();
        std::set< BVHNode<T> * > defPolyMesh_NodeSet;
        //-------------------------------------------------
        // Deform the deformable polygonal mesh away from the rigid polygonal mesh
        for ( int i = 0; i < static_cast<int>( listOfCollidedNode_Def->size() ); ++i ) {

            separateDistanceConstraint = 0.5 * (   (*listOfCollidedNode_Def)[i]->GetRadius() 
                                                 + (*listOfCollidedNode_Rigid)[i]->GetRadius() );
            centerA = ( pRigidPolyMesh->GetTransform().GetMatrixTransform() * 
                        Vector4<T>( (*listOfCollidedNode_Rigid)[i]->GetCenter() ) ).GetVector3();
            N = centerA - (*listOfCollidedNode_Def)[i]->GetCenter();
            separateDistance = separateDistanceConstraint - N.Length();
            if ( separateDistance > Math<T>::ZERO ) {
                N.Normalized();
                N *= separateDistance;
                primB = (*listOfCollidedNode_Def)[i]->GetAPrimitiveHalfEdgeFace();
                vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
                //-------------------------------
                // For each vertex in a primitive
                for ( int j = 0; j < noOfVertices; ++j ) {
                    vertexList[j]->SetPosition( vertexList[j]->GetPosition() - N );
                }
            }
            // List of BVNodes to be updated
            defPolyMesh_NodeSet.insert( (*listOfCollidedNode_Def)[i] );
        }
        //-------------------------------------------------
        // Update Normals of Deformable Polygonal Mesh
        pDefPolyMesh->CalAndSetNormals();
        //-------------------------------------------------
        // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
        pDefPolyMesh->GetBVHTree()->Update( defPolyMesh_NodeSet );
        //-------------------------------------------------
        return true;
    }
    //---------------------------------------------------------------
    return false;
}
// END: CDR_DeformPolygonalMesh_vs_RigidPolygonalMesh(...)
//-----------------------------------------------------------------------------
//=============================================================================




//=============================================================================
//-----------------------------------------------------------------------------
//template <typename T>
//bool CDR_DeformPolygonalMesh_vs_DeformPolygonalMesh ( 
//  TAPs::OpenGL::HalfEdgeModel<T> * const pDefPolyMesh_1,  //!< the 1st deformable polygonal mesh model
//  TAPs::OpenGL::HalfEdgeModel<T> * const pDefPolyMesh_2,  //!< the 2nd deformable polygonal mesh model
//  T deformRatio   //!< deformation ratio (must be between [0-1]) where 0 means the model is 100% deformed while the other model is 0% deformed
//)
//{
//  //---------------------------------------------------------------
//  if ( pDefPolyMesh_1->GetBVHTree()->TestOverlapWithTillLeafNodes( pDefPolyMesh_2->GetBVHTree() ) )
//  {
//      //-------------------------------------------------
//      T separateDistanceConstraint;
//      Vector3<T> N, centerA;
//      T separateDistance;
//      HEFace<T> * primB;
//      std::vector< HEVertex<T> * const > vertexList;
//      int noOfVertices;
//      //-------------------------------------------------
//      std::vector< BVHNode<T> * > *   listOfCollidedNode_Def_1 = &pDefPolyMesh_1->GetBVHTree()->GetListOfCollidedNodes();
//      std::vector< BVHNode<T> * > *   listOfCollidedNode_Def_2 = &pDefPolyMesh_1->GetBVHTree()->GetListOfCollidedNodesThat();
//      std::set< BVHNode<T> * > defPolyMesh_NodeSet;
//      //-------------------------------------------------
//      //// Deform the deformable polygonal mesh away from the rigid polygonal mesh
//      //for ( int i = 0; i < static_cast<int>( listOfCollidedNode_Def->size() ); ++i ) {
//
//      //  separateDistanceConstraint = 0.5 * (   (*listOfCollidedNode_Def_1)[i]->GetRadius() 
//      //                                       + (*listOfCollidedNode_Def_2)[i]->GetRadius() );
//      //  centerA = ( pRigidPolyMesh->GetTransform().GetMatrixTransform() * 
//      //              Vector4<T>( (*listOfCollidedNode_Rigid)[i]->GetCenter() ) ).GetVector3();
//      //  N = centerA - (*listOfCollidedNode_Def)[i]->GetCenter();
//      //  separateDistance = separateDistanceConstraint - N.Length();
//      //  if ( separateDistance > Math<T>::ZERO ) {
//      //      N.Normalized();
//      //      N *= separateDistance;
//      //      primB = (*listOfCollidedNode_Def)[i]->GetAPrimitiveHalfEdgeFace();
//      //      vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
//      //      //-------------------------------
//      //      // For each vertex in a primitive
//      //      for ( int j = 0; j < noOfVertices; ++j ) {
//      //          vertexList[j]->SetPosition( vertexList[j]->GetPosition() - N );
//      //      }
//      //  }
//      //  // List of BVNodes to be updated
//      //  defPolyMesh_NodeSet.insert( (*listOfCollidedNode_Def)[i] );
//      //}
//      ////-------------------------------------------------
//      //// Update Normals of Deformable Polygonal Mesh
//      //pDefPolyMesh->CalAndSetNormals();
//      ////-------------------------------------------------
//      //// Update Bounding Volume Hierarchy of Deformable Polygonal Mesh
//      //pDefPolyMesh->GetBVHTree()->Update( defPolyMesh_NodeSet );
//      ////-------------------------------------------------
//      //return true;
//  }
//  //---------------------------------------------------------------
//  return false;
//}
// END: CDR_DeformPolygonalMesh_vs_DeformPolygonalMesh(...)
//-----------------------------------------------------------------------------
//=============================================================================




//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs__CD__Fn
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----