#include "stdafx.h"

#include "DllFunc.h"

#include "math.h"

TrackSoilForce_API void __cdecl track_soil_force

           (double time, int info[], double upar[], int npar, double dird[], double dirv[],

            double disp[], double lgori[], double ngpos[], double ngori[], double ngvel[],

            double length, double width, int iflag, double result[3])

{

           using namespace rd_syscall;

           // Parameter Information

      // time   : Simulation time of RD/Solver. (Input)

      // info   : ID of body & node & end_flag. (Input, 1 : TRUE)

      // upar   : parameters defined by user. (Input)

      // npar   : Number of user parameters. (Input)

      // dird   : Direction vector of ground patch. (Input)

      // dirv   : Velocity of a contact node relative to direction vector. (Input)

      // disp   : Sinkage & shear displacements. (Input)

      // lgori  : Global orientation of a track link. (Input)

      // ngpos  : Global position of a contact node. (Input)

      // ngori  : Gloabal orientation of a contact node. (Input)

      // ngvel  : Gloabal velocity of a contact node. (Input)

      // length : Length of meshed rectangle at a track link. (Input)

      // width  : Width of meshed rectangle at a track link. (Input)

      // iflag  : When RD/Solver initializes arrays, the flag is true. (Input, -1)

      // result : Returned track soil force vector. (Output, Size : 3)

           // User Statement

           int body_id, node_id, end_flag, i;

           double k_c, k_phi, n, cohesion, phi, m_k,area, pre_max, z, z_max, pre,

                     force, sgx, sgz,pre_sx, pre_sz,delta_sx, delta_sz, force_x, force_z,

                     u_x[3],u_y[3],u_z[3],vel_x,vel_y,vel_z,ku,ratio_zmax,ratio_n;

//--- ASSIGN INFORMATION

           body_id = info[0];

           node_id = info[1];

           end_flag = info[2];

//--- UNIT VECTOR OF DIRECTION

           for(i=0; i<3; i++){

                     u_x[i] = dird[i];

                     u_y[i] = dird[3+i];

                     u_z[i] = dird[6+i];

           }  

//--- VEOCITY OF A CONTACT NODE RELATIVE TO DIRECTION VECTOR

           vel_x = dirv[0];

           vel_y = dirv[1];

           vel_z = dirv[2];

//-------------------------------------------------------

//--- 1. PRESSURE - SINKAGE RELATIONSHIP

//-------------------------------------------------------

//--- SINKAGE & SHEAR DISPLACEMENTS

           z = disp[0];

           delta_sx = disp[1];

           delta_sz = disp[2];

           z_max = disp[3];

//--- DEFINE SOIL PARAMETERS

           k_c = 0.00047613;

           k_phi = 0.00076603;

           n = 1.1;

           cohesion = 0.00104;

           phi = 28.0*acos(-1.0)/180.0;

           m_k = 25.0;

//--- CALCULATE AREA RATIO RELATIVE TO A NORMAL VECTOR OF TRIANGULAR PATHCH

           ratio_n = fabs( ngori[3]*u_y[0] + ngori[4]*u_y[1]

                                                                                                + ngori[5]*u_y[2] );

           area = length*width*ratio_n;

//--- FOR REPETITIVE LOAD

           ratio_zmax = 0.01;

//--- CHECK IF SINKAGE IS IN THE LOADING OR UNLOADING/RELOADING CONDITION

//--- PLASTIC EFFECT

           if( (z>=0.0) && (z>=z_max) ) {

                     pre = ( k_c/width + k_phi ) *pow(z,n);

           }

//--- ELASTIC EFFECT

           else if ( (z>=0.0) && (z<z_max) && (z>=(z_max*(1.0-ratio_zmax))) ) {

                     pre_max = ( k_c/width + k_phi ) * pow(z_max,n);

                     ku = pre_max/(z_max*ratio_zmax);

                     pre = pre_max - ku*(z_max-z);

           }

           else {

                     pre = 0.0;

           }

           force = pre * area;

//--- GET GLOBAL FORCE

           for(i=0; i<3; i++){

                     result[i] = force * u_y[i];

           }

//-------------------------------------------------------

//--- 2. SHEAR STRESS - SHEAR DISPLACEMENT RELATIONSHIP

//-------------------------------------------------------

           if( vel_x>=0.0 ) {

                     sgx = -1.0;

           } 

           else {

                     sgx = 1.0;

           }

           if( vel_z>=0.0 ) {

                     sgz = -1.0;

           } 

           else {

                     sgz = 1.0;

           }

//----/ LONGITUDINAL FORCE /

           pre_sx = ( cohesion + pre*tan(phi) )

                                *( 1.0 - exp(-delta_sx/m_k) );

           area = length*width*ratio_n;

           force_x = pre_sx * area;

           for(i=0; i<3; i++){

                     result[i] = result[i] + fabs(force_x)*sgx*u_x[i];

           }

//----/ LATERAL FORCE /

           pre_sz = ( cohesion + pre*tan(phi) )

                                *( 1.0 - exp(-delta_sz/m_k) );

           area = length*width*ratio_n;

           force_z = pre_sz * area;

           for(i=0; i<3; i++){

                     result[i] = result[i] + fabs(force_z)*sgz*u_z[i];

           }

}