TAPsCUDA_DevFns_ModelElasticRod_Def.cu

Go to the documentation of this file.

/******************************************************************************
TAPsCUDA_DevFns_ModelElasticRod_Def.cu

CUDA Definition for ModelElasticRod (i.e., cpp file).

SUKITTI PUNAK   (09/03/2009)
UPDATE          (09/24/2009)
******************************************************************************/

#include "TAPsCUDA_DevFns_ModelElasticRod.cu"

BEGIN_NAMESPACE_TAPs__CUDA
//=============================================================================
// CUDA Device Functions
//-----------------------------------------------------------------------------

// An explicit Euler integration for calculating new velocity
__device__
float3 Device__EulerInt_NewVel_ModelElasticRod (
    float3 Force,       // force
    float3 Velocity,    // velocity
    float mass,         // mass
    float  h            // time step
)
{
    // F = mA; A = F/m; V = A*h; P = V*h
    float scale = h / mass;
    float3 vel_chg = Mul( Force, scale );
    return Add( Velocity, vel_chg );
}


// An explicit Euler integration for calculating new position
__device__
float3 Device__EulerInt_NewPos_ModelElasticRod (
    float3 Velocity,    // velocity
    float3 Position,    // position
    float  h            // time step
)
{
    return Add( Position, Mul( Velocity, h ) );
}


// A semi explicit Euler integration for calculating new orientation
__device__
float4 Device__EulerInt_NewOri_ModelElasticRod (
    float k_mdr,        // an constant value in proportion to material density and radius
    float l_rest,       // orientation's rest length
    float4 Orientation, // orientation
    float4 Torque,      // torque in 4-dimension
    float  h            // time step
)
{
    // Compute the components of the inverse of the inertia tersor matrix
    // Only the diagonal components are non-zero, since the rod's material is assumed to be isotropic.
    float c = l_rest * k_mdr;
    float Jinv_0 = 4.0f / c;
    float Jinv_1 = Jinv_0;
    float Jinv_2 = 2.0f / k_mdr;

    
    // Compute the angular velocity from the 4d torque
    //   1/2 * Q_j^T * torque_in_4Dim
    //   (Q_j^T * torque_in_4Dim) equals "the multiplication of conjugation_of_q_j with torque_in_4Dim"
    float4 torque_transformed = Mul( QuaternionMul( QuaternionConjugate( Orientation ), Torque ), 0.5f );
    // Angular velocity := 1/2 * Jinv * torque_transformed_into_3Dim
    float4 angularVelocity = make_float4(
        0.0f,   // deliberately set to zero
        0.5f * Jinv_0 * torque_transformed.y, 
        0.5f * Jinv_1 * torque_transformed.z, 
        0.5f * Jinv_2 * torque_transformed.w
    );

    // Compute the velocity of the orientation
    float4 q_velocity = Mul( QuaternionMul( Orientation, angularVelocity ), 0.5f );

    // Compute and return the next orientation
    return Add( Orientation, Mul( q_velocity, h ) );
}


// Calculate the stretch force of centerline
__device__
float3 Device__CalStretchForce_ModelElasticRod ( 
    float Ps,       // potential stretch constant
    float Lrest,    // rest length
    float Lcurr,    // current length
    float3 Fdir     // difference of two position -- i.e. unpolished force
)
{

    #ifdef  __DEVICE_EMULATION__
    {
        float mag = (Lcurr/Lrest - 1.0f) * Ps;
        float3 Fs = make_float3( mag*Fdir.x/Lcurr, mag*Fdir.y/Lcurr, mag*Fdir.z/Lcurr );
        printf( "Ps: %f\n", Ps );
        printf( "Lrest: %f\n", Lrest );
        printf( "Lcurr: %f\n", Lcurr );
        printf( "Lcurr/Lrest: %f\n", Lcurr/Lrest );
        printf( "Lcurr/Lrest - 1.0f: %f\n", Lcurr/Lrest-1.0f );
        printf( "mag: %f\n", mag );
        printf( "Fs: %f %f %f\n", Fs.x, Fs.y, Fs.z );
    }
    #endif

    float mag = (Lcurr/Lrest - 1.0f) * Ps;
    return make_float3( mag*Fdir.x/Lcurr, mag*Fdir.y/Lcurr, mag*Fdir.z/Lcurr );
}


// Calculate the bend torque of orientation
__device__
float4 Device__CalBendTorque_ModelElasticRod (
    float4 O1,      // orientation 1
    float4 O2,      // orientation 2
    float3 Pb       // potential bend constant -- xyz
)
{
    //return make_float4( 0.0f, 0.0f, 0.0f, 0.0f );
    
    float4 OA = Add( O2, O1 );  // O2 + O1
    float4 OS = Sub( O2, O1 );  // O2 - O1
    
    // B_k(q_{j+1}+q{j})
    float4 B1 = make_float4( -OA.y,  OA.x,  OA.w, -OA.z );
    float4 B2 = make_float4( -OA.z, -OA.w,  OA.x,  OA.y );
    float4 B3 = make_float4( -OA.w,  OA.z, -OA.y,  OA.x );
    
    // 2 * K_k ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) )
    float k1 = 2.0f * Pb.x * InnerProduct( B1, OS );
    float k2 = 2.0f * Pb.y * InnerProduct( B2, OS );
    float k3 = 2.0f * Pb.z * InnerProduct( B3, OS );
    
    // Differentiation of ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) ) by q_{j}
    float4 dB1 = make_float4(  -O2.y,  O2.x,  O2.w, -O2.z );
    float4 dB2 = make_float4(  -O2.z, -O2.w,  O2.x,  O2.y );
    float4 dB3 = make_float4(  -O2.w,  O2.z, -O2.y,  O2.x );
    
    return Add( Mul( dB1, k1 ), Add( Mul( dB2, k2 ), Mul( dB3, k3 ) ) );
}


// Calculate the constraint force applied to centerline
// to align centerline's tangent with orientation's 3rd direction
__device__
float3 Device__CalTheConstraintForce_ModelElasticRod (
    float3 Cen21,       // centerline2 - centerline1
    float l_Cen21,      // length of (centerline2 - centerline1)
    float inv_l_Cen21,  // the inverse length of (centerline2 - centerline1)
    float4 Ori,         // orientation
    float l_rest,       // rest length
    float Kc            // Kconstraint for aligning centerline's tangent with orientation's 3rd direction
)
{
    float l_Cen21_square = l_Cen21 * l_Cen21;
    
    // the third director of orientation
    float3 Dir3 = make_float3(
        2.0f*( Ori.y*Ori.w + Ori.x*Ori.z ),
        2.0f*( Ori.z*Ori.w - Ori.x*Ori.y ),
        Ori.x*Ori.x - Ori.y*Ori.y - Ori.z*Ori.z + Ori.w*Ori.w
    );
    
    // rest_len * Kappa * ( (r_{i+1}-r_{i})/||r_{i+1}-r_{i}|| - d3(q_{i}) )
    float3 Vc = Mul( Sub( Mul( Cen21, inv_l_Cen21 ), Dir3 ), l_rest * Kc );
    
    // Compute the constraint force for the centerline
    float3 Vcr = Mul( Vc, inv_l_Cen21 / l_Cen21_square );
    float xd = Cen21.x, yd = Cen21.y, zd = Cen21.z;
    float xd_yd = xd*yd;
    float xd_zd = xd*zd;
    float yd_zd = yd*zd;
    return make_float3(
        InnerProduct( Vcr, make_float3( l_Cen21_square - xd*xd, xd_yd, xd_zd ) ),
        InnerProduct( Vcr, make_float3( xd_yd, l_Cen21_square - yd*yd, yd_zd ) ),
        InnerProduct( Vcr, make_float3( xd_zd, yd_zd, l_Cen21_square - zd*zd ) )
    );
}


// Calculate the constraint torque applied to orientation
// to align centerline's tangent with orientation's 3rd direction
__device__
float4 Device__CalTheConstraintTorque_ModelElasticRod (
    float3 Cen21,       // centerline2 - centerline1
    float l_Cen21,      // length of (centerline2 - centerline1)
    float inv_l_Cen21,  // the inverse length of (centerline2 - centerline1)
    float4 Ori,         // orientation
    float l_rest,       // rest length
    float Kc            // Kconstraint for aligning centerline's tangent with orientation's 3rd direction
)
{
    // the third director of orientation
    float3 Dir3 = make_float3(
        2.0f*( Ori.y*Ori.w + Ori.x*Ori.z ),
        2.0f*( Ori.z*Ori.w - Ori.x*Ori.y ),
        Ori.x*Ori.x - Ori.y*Ori.y - Ori.z*Ori.z + Ori.w*Ori.w
    );
    
    // rest_len * Kappa * ( (r_{i+1}-r_{i})/||r_{i+1}-r_{i}|| - d3(q_{i}) )
    float3 Vc = Mul( Sub( Mul( Cen21, inv_l_Cen21 ), Dir3 ), l_rest * Kc );

    // Compute the constraint force (torque) for the orientation
    float3 Vcq = Mul( Vc, 2.0f );
    return make_float4(
        InnerProduct( Vcq, make_float3( Ori.z, -Ori.y,  Ori.x ) ),
        InnerProduct( Vcq, make_float3( Ori.w, -Ori.x, -Ori.y ) ),
        InnerProduct( Vcq, make_float3( Ori.x,  Ori.w, -Ori.z ) ),
        InnerProduct( Vcq, make_float3( Ori.y,  Ori.z,  Ori.w ) )
    );
}


// Calculate the total force of the node at vertexNo
__device__
float3 Device__CalForce_ModelElasticRod (
    int vertexNo,               // vertex number
    unsigned int numOfNodes,    // number of vertices
    float4 Pos0,                
    float4 Pos1,                
    float4 Pos2,                
    float4 Ori0,                
    float4 Ori1,                
    float radius,               // radius
    float length,               // rest length
    float mass,                 // point mass
    //float Kt,                 // kinetic translational constant
    //float3 Kr,                // kinetic rotational constant -- xyz
    //float Dt,                 // translational constant
    //float3 Dr,                // rotational dissipation constant -- xyz
    float Kconstraint_3rdDirAlignTangent,   // Kconstraint for aligning centerline's tangent with orientation's 3rd direction
    float Kvdamping,            // centerline's velocity damper
    float Ps,                   // potential stretch constant
    float3 Pb                   // potential bend constant -- xyz
)
{
    //float4 Pos1 = tex1Dfetch( CudaTexVertexList, vertexNo );      // position
    float3 P1 = XYZ( Pos1 );
    float3 total_force  = make_float3( 0.0f, 0.0f, 0.0f );
    
    if ( vertexNo > 0 ) {   // skip the first one
        // Cal the stretch force due to connection with the previous centerline
        //float4 Pos0 = tex1Dfetch( CudaTexVertexList, vertexNo-1 );    // position
        float3 P0 = XYZ( Pos0 );
        float3 P0_P1 = Sub( P0, P1 );
        
        #ifdef  __DEVICE_EMULATION__
        printf( "P1: %f %f %f\n", P1.x, P1.y, P1.z );
        printf( "P0: %f %f %f\n", P0.x, P0.y, P0.z );
        #endif
        
        float l_curr_01 = Length( P0_P1 );
        float3 force = Device__CalStretchForce_ModelElasticRod( 
            Ps,         
            length,     
            l_curr_01,  
            P0_P1       
        );
        total_force.x += force.x;
        total_force.y += force.y;
        total_force.z += force.z;
        
        #ifdef  __DEVICE_EMULATION__
        printf( "(#v>0) Fs[%i]: %f %f %f\n", vertexNo, force.x, force.y, force.z );
        #endif//__DEVICE_EMULATION__

        if ( vertexNo < numOfNodes - 2 ) {  // skip the last two
            // Cal the constraint force due to connection with the previous centerline and orientation
            //float4 Ori0 = tex1Dfetch( CudaTexOrientationList, vertexNo-1 );   // orientation
            float3 P10 = make_float3( -P0_P1.x, -P0_P1.y, -P0_P1.z );
            force = Device__CalTheConstraintForce_ModelElasticRod (
                P10,            
                l_curr_01,      
                1.0/l_curr_01,  
                Ori0,           
                length,         
                Kconstraint_3rdDirAlignTangent  
            );
            total_force.x -= force.x;
            total_force.y -= force.y;
            total_force.z -= force.z;

            #ifdef  __DEVICE_EMULATION__
            printf( "(#v>0) Fc[%i]: %f %f %f\n", vertexNo, force.x, force.y, force.z );
            #endif//__DEVICE_EMULATION__
        }
    }

    if ( vertexNo < numOfNodes - 1 ) {  // skip the last one
        // Cal the stretch force due to connection with the next centerline
        //float4 Pos2 = tex1Dfetch( CudaTexVertexList, vertexNo+1 );    // position
        float3 P2 = XYZ( Pos2 );
        float3 P2_P1 = Sub( P2, P1 );
        
        #ifdef  __DEVICE_EMULATION__
        printf( "P1: %f %f %f\n", P1.x, P1.y, P1.z );
        printf( "P2: %f %f %f\n", P2.x, P2.y, P2.z );
        #endif
        
        float l_curr_21 = Length( P2_P1 );
        float3 force = Device__CalStretchForce_ModelElasticRod( 
            Ps,         
            length,     
            l_curr_21,  
            P2_P1       
        );
        total_force.x += force.x;
        total_force.y += force.y;
        total_force.z += force.z;

        #ifdef  __DEVICE_EMULATION__
        printf( "(#v<#N-1) Fs[%i]: %f %f %f\n", vertexNo, force.x, force.y, force.z );
        #endif//__DEVICE_EMULATION__
        
        if ( vertexNo < numOfNodes - 2 ) {  // skip the last two
            // Cal the constraint force due to connection with the next centerline and orientation
            //float4 Ori1 = tex1Dfetch( CudaTexOrientationList, vertexNo ); // orientation
            force = Device__CalTheConstraintForce_ModelElasticRod (
                P2_P1,          
                l_curr_21,      
                1.0/l_curr_21,  
                Ori1,           
                length,         
                Kconstraint_3rdDirAlignTangent  
            );
            total_force.x += force.x;
            total_force.y += force.y;
            total_force.z += force.z;
            
            #ifdef  __DEVICE_EMULATION__
            printf( "(#v<#N-1) Fc[%i]: %f %f %f\n", vertexNo, force.x, force.y, force.z );
            #endif//__DEVICE_EMULATION__
        }
    }
    
    return total_force;
}


__device__
float4 Device__CalTorque_ModelElasticRod (
    int vertexNo,               
    unsigned int numOfNodes,    
    float4 Pos1,                
    float4 Pos2,                
    float4 Ori0,                
    float4 Ori1,                
    float4 Ori2,                
    float radius,               
    float length,               
    float mass,                 
    //float Kt,                 //!< kinetic translational constant
    //float3 Kr,                //!< kinetic rotational constant -- xyz
    //float Dt,                 //!< translational constant
    //float3 Dr,                //!< rotational dissipation constant -- xyz
    float Kconstraint_3rdDirAlignTangent,   
    float Kvdamping,            
    float Ps,                   
    float3 Pb                   
)
{
    //float4 Ori1 = tex1Dfetch( CudaTexOrientationList, vertexNo ); // orientation
    float4 total_torque = make_float4( 0.0f, 0.0f, 0.0f, 0.0f );

    //if ( vertexNo >= 0 )
    //  return total_torque;

    // Skip the last one
    if ( vertexNo < numOfNodes-1 ) 
    {
        {
            // Cal the constraint torque due to connection with the before and after centerlines
            // --- centerline1 ---- orientation1 --- centerline2 ---
            //float4 Pos1 = tex1Dfetch( CudaTexVertexList, vertexNo );  // position 
            float3 P1 = XYZ( Pos1 );
            //float4 Pos2 = tex1Dfetch( CudaTexVertexList, vertexNo+1 );    // position
            float3 P2 = XYZ( Pos2 );
            float3 P2_P1 = Sub( P2, P1 );
            float l_curr_21 = Length( P2_P1 );
            float4 torque = Device__CalTheConstraintTorque_ModelElasticRod (
                P2_P1,          
                l_curr_21,      
                1.0/l_curr_21,  
                Ori1,           
                length,         
                Kconstraint_3rdDirAlignTangent  
            );
            total_torque.x += torque.x;
            total_torque.y += torque.y;
            total_torque.z += torque.z;
            total_torque.w += torque.w;
            
            #ifdef  __DEVICE_EMULATION__
            printf( "(#v>0) Ctorque[%i]: %f %f %f %f\n", vertexNo, torque.x, torque.y, torque.z, torque.w );
            #endif//__DEVICE_EMULATION__
        }
        
        if ( vertexNo >= 0 )    // skip the first one
        {
            // Cal the bend torque due to connection with the previous orientation
            //float4 Ori0 = tex1Dfetch( CudaTexOrientationList, vertexNo-1 );   // orientation
            float4 torque = Device__CalBendTorque_ModelElasticRod(
                Ori1,   
                Ori0,   
                Pb      
            );
            total_torque.x += torque.x;
            total_torque.y += torque.y;
            total_torque.z += torque.z;
            total_torque.w += torque.w;
            
            #ifdef  __DEVICE_EMULATION__
            printf( "(#v>0) Btorque[%i]: %f %f %f %f\n", vertexNo, torque.x, torque.y, torque.z, torque.w );
            #endif//__DEVICE_EMULATION__
        }

        if ( vertexNo < numOfNodes - 2 )    // skip the last two
        {
            // Cal the bend torque due to connection with the next orientation
            //float4 Ori2 = tex1Dfetch( CudaTexOrientationList, vertexNo+1 );   // orientation
            float4 torque = Device__CalBendTorque_ModelElasticRod(
                Ori1,   
                Ori2,   
                Pb      
            );
            total_torque.z += torque.z;
            total_torque.x += torque.x;
            total_torque.w += torque.w;
            total_torque.y += torque.y;
            
            #ifdef  __DEVICE_EMULATION__
            printf( "(#v<#N-2) Btorque[%i]: %f %f %f %f\n", vertexNo, torque.x, torque.y, torque.z, torque.w );
            #endif//__DEVICE_EMULATION__
        }
    }
    return total_torque;
}
//-----------------------------------------------------------------------------
// CUDA Device Functions
//=============================================================================
END_NAMESPACE_TAPs__CUDA
//-----------------------------------------------------------------------------
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----