TAPsColDetFns_NEW.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsColDetFns_NEW.cpp
SUKITTI PUNAK   (04/14/2010)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsColDetFns_NEW.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
//=============================================================================
//-----------------------------------------------------------------------------

// CDR for Elastic Rod with HalfEdgeModel<T>
//-----------------------------------------------------------------------------
#if ( defined TAPs_MODEL_ELASTIC_ROD_HPP && defined TAPs_HALF_EDGE_MODEL_HPP )
template <typename T>
bool CDR_ElasticRod_with_HalfEdgeModel ( 
    TAPs::ModelElasticRod<T> * const pERObj,        
    TAPs::OpenGL::HalfEdgeModel<T> * const  pObj,   
    TransformationSupport<T> * const pTransform     
)
{
    bool bResult = false;

    TransformationSupport<T> transform;
    if ( pTransform ) {
        transform.ReturnMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    transform.ReturnMatrixTransform() *= pObj->GetTransform().GetMatrixTransform();

    // Collision Detection
    TransformationSupport<T> oldTrx( pObj->GetTransform() );    // store the old transformation
    pObj->GetTransform() = transform;                           // set to the new transformation
    bResult = pERObj->CDUnit().GetBVHTree()->TestOverlapWithTillLeafNodes( pObj->GetBVHTree() );    // collision detection automatically uses the new transformation
    pObj->GetTransform() = oldTrx;                              // restore the old transformation
    if ( !bResult ) return bResult;

    // Collision Response
    bResult = false;
    T Kc = pERObj->RefToParameters().K_CDRwTriangle;

    // The list of collided BV leaf nodes
    std::vector< BVHNode<T> * > & ER_BVNodes  = pERObj->CDUnit().GetBVHTree()->GetListOfCollidedNodes();
    std::vector< BVHNode<T> * > & Obj_BVNodes = pERObj->CDUnit().GetBVHTree()->GetListOfCollidedNodesThat();
    std::vector< HEVertex<T> * const > vertexList;
    int noOfVertices;
    Matrix4x4<T> & TrxMat = transform.ReturnMatrixTransform();

    for ( int i = 0; i < static_cast<int>( ER_BVNodes.size() ); ++i ) {
        BVHNodeLeafElasticRodNode<T> * NodeER = dynamic_cast< BVHNodeLeafElasticRodNode<T> * >( ER_BVNodes[i] );
        HEFace<T> * Triangle = Obj_BVNodes[i]->GetAPrimitiveHalfEdgeFace();
        vertexList = Triangle->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
        Vector3<T> cylA( NodeER->GetPtrToPrimitive_1()->Centerline[pERObj->GetCurrentIndex()].GetPosition() );
        Vector3<T> cylB( NodeER->GetPtrToPrimitive_2()->Centerline[pERObj->GetCurrentIndex()].GetPosition() );
        Vector3<T> posA( (TrxMat * Vector4<T>(vertexList[0]->GetPosition())).GetVector3() );
        Vector3<T> posB( (TrxMat * Vector4<T>(vertexList[1]->GetPosition())).GetVector3() );
        Vector3<T> posC( (TrxMat * Vector4<T>(vertexList[2]->GetPosition())).GetVector3() );
        if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle(
                cylA, cylB,
                pERObj->RefToParameters().Radius + pERObj->RefToParameters().Offset_CD_Triangle,
                posA, posB, posC
        ) ) {
            Vector3<T> triCentroid( ( posA + posB + posC ) / 3.0 );
            Vector3<T> N( Triangle->GetNormal() );
            N.Normalized();
            Vector3<T> projPt1, projPt2;
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylA, posB, N, projPt1 );
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylB, posB, N, projPt2 );
            Vector3<T> V1( cylA - projPt1 );
            Vector3<T> V2( cylB - projPt2 );
            //Vector3<T> V1( cylA - triCentroid );
            //Vector3<T> V2( cylB - triCentroid );
            T value = V1 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_1()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_1()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_1()->Centerline[pERObj->GetCurrentIndex()].SetPosition( cylA - value * N ); // adjusted by position
                }
            }
            value = V2 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_2()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_2()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_2()->Centerline[pERObj->GetCurrentIndex()].SetPosition( cylB - value * N ); // adjusted by position
                }
            }
            bResult = true;
        }
    }
    return bResult;
}
#endif//#if ( defined TAPs_MODEL_ELASTIC_ROD_HPP && defined TAPs_HALF_EDGE_MODEL_HPP )
//-----------------------------------------------------------------------------

    
    
    
// CDR for Elastic Rod with Deformable Model Based On FEM
//-----------------------------------------------------------------------------
#if ( defined TAPs_MODEL_ELASTIC_ROD_HPP && defined TAPs_MODEL_DEF_BASED_ON_FEM_HPP )
template <typename T, typename DATA>
bool CDR_ElasticRod_with_ModelDefBasedOnFEM ( 
    TAPs::ModelElasticRod<T> * const pERObj,                        
    TAPs::ModelDefBasedOnFEM<T,DATA> * const    pDefModelFEMObj,    
    TransformationSupport<T> * const pTransform                     
)
{
    bool bResult;

    //ModelElasticRod<T>::SetOfElasticRodNodes::iterator it = pERObj->GetListOfNodes().begin();
    //while ( it != pERObj->GetListOfNodes().end() ) {
    //  it->IsCollide = false;
    //  ++it;
    //}

    TransformationSupport<T> transform;
    if ( pTransform ) {
        transform.RefToMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    transform.RefToMatrixTransform() *= pDefModelFEMObj->PtrToBVHTreeOfSurfaceMesh()->GetTransform().GetMatrixTransform();

    // Collision Detection
    TransformationSupport<T> oldTrx( pDefModelFEMObj->PtrToBVHTreeOfSurfaceMesh()->GetTransform() );    // store the old transformation
    pDefModelFEMObj->PtrToBVHTreeOfSurfaceMesh()->GetTransform() = transform;                           // set to the new transformation
    bResult = pERObj->CDUnit().GetBVHTree()->TestOverlapWithTillLeafNodes( pDefModelFEMObj->PtrToBVHTreeOfSurfaceMesh() );  // collision detection automatically uses the new transformation
    pDefModelFEMObj->PtrToBVHTreeOfSurfaceMesh()->GetTransform() = oldTrx;                              // restore the old transformation
    if ( !bResult ) return bResult;

    // Collision Response
    bResult = false;
    T Kc = pERObj->RefToParameters().K_CDRwTriangle;

    // The list of collided BV leaf nodes
    std::vector< BVHNode<T> * > & ER_BVNodes  = pERObj->CDUnit().GetBVHTree()->GetListOfCollidedNodes();
    std::vector< BVHNode<T> * > & Obj_BVNodes = pERObj->CDUnit().GetBVHTree()->GetListOfCollidedNodesThat();
    std::vector< HEVertex<T> * const > vertexList;
    int noOfVertices;
    Matrix4x4<T> & TrxMat = transform.RefToMatrixTransform();

    for ( int i = 0; i < static_cast<int>( ER_BVNodes.size() ); ++i ) {
        BVHNodeLeafElasticRodNode<T> * NodeER = dynamic_cast< BVHNodeLeafElasticRodNode<T> * >( ER_BVNodes[i] );
        HEFace<T> * Triangle = Obj_BVNodes[i]->GetAPrimitiveHalfEdgeFace();
        vertexList = Triangle->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
        Vector3<T> cylA( NodeER->GetPtrToPrimitive_1()->Centerline[pERObj->GetCurrentIndex()].GetPosition() );
        Vector3<T> cylB( NodeER->GetPtrToPrimitive_2()->Centerline[pERObj->GetCurrentIndex()].GetPosition() );
        Vector3<T> posA( (TrxMat * Vector4<T>(vertexList[0]->GetPosition())).GetVector3() );
        Vector3<T> posB( (TrxMat * Vector4<T>(vertexList[1]->GetPosition())).GetVector3() );
        Vector3<T> posC( (TrxMat * Vector4<T>(vertexList[2]->GetPosition())).GetVector3() );
        if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle(
                cylA, cylB,
                pERObj->RefToParameters().Radius + pERObj->RefToParameters().Offset_CD_Triangle,
                posA, posB, posC
        ) ) {
            Vector3<T> triCentroid( ( posA + posB + posC ) / 3.0 );
            Vector3<T> N( Triangle->GetNormal() );
            N.Normalized();
            Vector3<T> projPt1, projPt2;
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylA, posB, N, projPt1 );
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylB, posB, N, projPt2 );
            Vector3<T> V1( cylA - projPt1 );
            Vector3<T> V2( cylB - projPt2 );
            //Vector3<T> V1( cylA - triCentroid );
            //Vector3<T> V2( cylB - triCentroid );
            T value = V1 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_1()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_1()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_1()->Centerline[pERObj->GetCurrentIndex()].SetPosition( cylA - value * N ); // adjusted by position
                }
            }
            value = V2 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_2()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_2()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_2()->Centerline[pERObj->GetCurrentIndex()].SetPosition( cylB - value * N ); // adjusted by position
                }
            }
            bResult = true;

            //NodeER->GetPtrToPrimitive_1()->IsCollide = true;
            //NodeER->GetPtrToPrimitive_2()->IsCollide = true;
        }
    }
    return bResult;
}
#endif//#if ( defined TAPs_MODEL_ELASTIC_ROD_HPP && defined TAPs_MODEL_DEF_BASED_ON_FEM_HPP )
//-----------------------------------------------------------------------------




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