#include "stdafx.h"

#include "DllFunc.h"

#include "math.h"

ScrewForce_API void __cdecl screw_force

           (double time, double upar[], int npar, int jflag, int iflag, double result[6])

{

           using namespace rd_syscall;

           // Parameter Information

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

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

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

           //   jflag  : When RD/Solver evaluates a Jacobian, the flag is true. (Input)

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

           //   result : Returned screw force vector. (Output, Size : 6)

           // User Statement

           // SER DECLARED VARIABLES

           int mkid[2];

           double value1, value2, force, isign, l, mu, k, c;

           int errflg;

           l = 200.0;

           mu = 0.3;

           k = 1000.0;

           c = 10.0;

           // OMPUTE A NORMAL FORCE

           // FTER CASTING DOUBLE PRECISION TYPE ‘UPAR’INTO INTEGER TYPE ‘MKID’         

           // SSIGNS ‘UPAR[0], [2]’INTO 'MKID[0],[1]'.      

           mkid[0] = (int) upar[0];

           mkid[1] = (int) upar[2];

           // Y CALLING ‘SYSFNC’, USER CAN OBTAIN 'VALUE1' WHICH MEANS 'DY'.

           sysfnc("DY",mkid,2,&value1,&errflg);

           // IKEWISE, USER CAN OBTAIN 'VALUE2' WHICH MEANS 'VY'.        

           sysfnc("VY",mkid,2,&value2,&errflg);   

           // HEN THE WHEEL PENETRATES INTO THE GROUND, THE ‘RESULT(2)’WHICH MEANS ‘FY’HAS A VALUE.

           result[1] = -k*(value1-l) - c*value2;

           if ( result[1] <= 0.0 ) result[1] = 0.0;

           // OMPUTE A FRICTION FORCE

           sysfnc("VX",mkid,2,&value1,&errflg);

           sysfnc("WZ",mkid,2,&value2,&errflg);

           if ( value1+value2*l < -0.01 ) {

                     isign = -1.0;

           }

           else if ( value1+value2*l > 0.01 ) {

                     isign = 1.0;

           }

           else {

           isign = 0.0;

           }

           mkid[0] = (int) upar[0];      

           mkid[1] = (int) upar[1];

           sysfnc("FY",mkid,2,&force,&errflg);

           result[0] = -mu*fabs(force)*isign;

           // OMPUTE A TORQUE GENERATED BY THE FRICTION FORCE       

           sysfnc("FX",mkid,2,&force,&errflg);

           result[5] = l*fabs(force);

}