#include "stdafx.h"

#include "DllFunc.h"

MatrixForce_API void __cdecl matrix_force

           (double time, double upar[], int npar, double disp[6], double velo[6],

           int jflag, int iflag, double result[6], double jdisp[36], double jvelo[36])

{

           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)

           //   disp   : Displacement vector between two force markers w.r.t. base marker. (Input)

           //   velo   : Velocity vector between two force markers w.r.t. base marker. (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 matrix force vector. (Output, Size : 6)

           //   jdisp  : Returned force Jacobian of displacement. (Output, Size : 36)

           //   jvelo  : Returned force Jacobian of velocity. (Output, Size : 36)

           // User Statement

           double tk, tc, rk, rc;

           int i;

           tk = upar[0];

           tc = upar[1];

           rk = upar[2];

           rc = upar[3];

           if ( jflag ) {

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

                                jdisp[i] = 0.0;

                                jvelo[i] = 0.0;

                     }

                     jdisp[0] = -tk;

                     jdisp[7] = -tk;

                     jdisp[14] = -tk;

                     jdisp[21] = -rk;

                     jdisp[28] = -rk;

                     jdisp[35] = -rk;

                     jvelo[0] = -tc;

                     jvelo[7] = -tc;

                     jvelo[14] = -tc;

                     jvelo[21] = -rc;

                     jvelo[28] = -rc;

                     jvelo[35] = -rc;

           }

           result[0] = - tk * disp[0] - tc * velo[0];

           result[1] = - tk * disp[1] - tc * velo[1];

           result[2] = - tk * disp[2] - tc * velo[2];

           result[3] = - rk * disp[3] - rc * velo[3];

           result[4] = - rk * disp[4] - rc * velo[4];

           result[5] = - rk * disp[5] - rc * velo[5];

}