#include "stdafx.h"

#include "DllFunc.h"

#include "math.h"

void ats(double A[9],double s[3], double sp[3])

{

           sp[0]=A[0]*s[0]+A[1]*s[1]+A[2]*s[2];

           sp[1]=A[3]*s[0]+A[4]*s[1]+A[5]*s[2];

           sp[2]=A[6]*s[0]+A[7]*s[1]+A[8]*s[2];

}

ModalForce_API void __cdecl modal_force

           (int id, double time, double upar[], int npar, int ifbody, double pos[12],

           double vel[6], double acc[6], int nmode, int nnode, int nModalLoad, double *ModalLoads,

           int jflag, int iflag, double *result)

{

           using namespace rd_syscall;

           // Parameter Information

           //   id         : Modal force sequential identification. (Input)

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

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

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

           //   ifbody     : RFLEX body sequential ID. (Input)

           //   pos        : Position [1~3] and Orientation matrix [4~12] w.r.t Ground.InertiaMarker. (Input)

           //   vel        : Velocity vector w.r.t. Ground.InertiaMarker. (Input)

           //   acc        : Acceleration vector w.r.t Ground.InertiaMarker. (Input)

           //   nmode      : Number of selected mode. (Input)

           //   nnode      : Number of node. (Input)

           //   nModalLoad : Number of selected modal load cases. (Input)

           //   ModalLoads : Modal force vector. (Input, Size : (6 + nmode) x nModalLoad)

           //   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 modal froce vector. (Output, Size : 6 + nmode)

           // User Statement

           int i,j,ierr;

           double *Af;

           int mid[9];

           double MVel[3];

           double MPos[3];

           double MAcc[3];

           double VM;

           double Fdir[3];

           double Fscale[3];

           double dtmp[3];

           double Cd;

           double RHO;

           double Area[3];

           double darea;

           double UCF;

           //Initialization

           Af=&(pos[3]);

           RHO = upar[0];

           Cd = upar[1];

           Area[0] = upar[2]; // Assumption : Area of shell face doesn't change // for node61,63,65

           Area[1] = upar[3]; // Assumption : Area of shell face doesn't change // for node17,19,21

           Area[2] = upar[4]; // Assumption : Area of shell face doesn't change // for node105,107,109

           mid[0] = int(upar[5]); // Node61

           mid[1] = int(upar[6]); // Node63

           mid[2] = int(upar[7]); // Node65

           mid[3] = int(upar[8]); // Node17

           mid[4] = int(upar[9]); // Node19

           mid[5] = int(upar[10]); // Node21

           mid[6] = int(upar[11]); // Node105

           mid[7] = int(upar[12]); // Node107

           mid[8] = int(upar[13]); // Node109

           rd_ucf(&UCF);

           for(i=0;i<6+nmode;i++){

                     result[i] = 0.0;

           }

           // Test code for Air-resistance force in rflex

           // Cd : coefficient for a drag force

           // Air resistance force : 1/2*Cd*Rho(air)*v^2*Area

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

                     sysary("TVEL",&(mid[i]),1,MVel,3,&ierr);                       

                     VM=sqrt(MVel[0]*MVel[0]+MVel[1]*MVel[1]+MVel[2]*MVel[2]);

                     if (VM < 1.0e-17) {

                                Fdir[0] = 0.0;

                                Fdir[1] = 0.0;

                                Fdir[2] = 0.0;

                     }

                     else {

                                Fdir[0] = MVel[0]/VM*(-1.0);

                                Fdir[1] = MVel[1]/VM*(-1.0);

                                Fdir[2] = MVel[2]/VM*(-1.0);

                     }

                     if(i<3) darea = Area[0];

                     else if(i<6) darea = Area[1];

                     else darea = Area[2];

                     dtmp[0] = Fdir[0]*(Cd*RHO*VM*VM*darea/2)/UCF;

                     dtmp[1] = Fdir[1]*(Cd*RHO*VM*VM*darea/2)/UCF;

                     dtmp[2] = Fdir[2]*(Cd*RHO*VM*VM*darea/2)/UCF;

                     // Generalized Force (w.r.t RFlex Body Ref. Frame)

                     ats(Af,dtmp,Fscale);

                     for(j=0;j<6+nmode;j++){

                                result[j] += Fscale[0]*ModalLoads[j+(6+nmode)*(3*i)]+

                                                                Fscale[1]*ModalLoads[j+(6+nmode)*(3*i+1)]+

                                                                Fscale[2]*ModalLoads[j+(6+nmode)*(3*i+2)];

                     }

           }

}