|
#include "stdafx.h" #include "DllFunc.h"
#include <stdio.h> #include <stdlib.h>
void asp(double result_s[], double A[], double sp[]) { result_s[0]=A[0]*sp[0]+A[3]*sp[1]+A[6]*sp[2]; result_s[1]=A[1]*sp[0]+A[4]*sp[1]+A[7]*sp[2]; result_s[2]=A[2]*sp[0]+A[5]*sp[1]+A[8]*sp[2]; } void matatb(double result[], double a[], double b[]) { result[0]=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]; result[1]=a[3]*b[0]+a[4]*b[1]+a[5]*b[2]; result[2]=a[6]*b[0]+a[7]*b[1]+a[8]*b[2];
result[3]=a[0]*b[3]+a[1]*b[4]+a[2]*b[5]; result[4]=a[3]*b[3]+a[4]*b[4]+a[5]*b[5]; result[5]=a[6]*b[3]+a[7]*b[4]+a[8]*b[5];
result[6]=a[0]*b[6]+a[1]*b[7]+a[2]*b[8]; result[7]=a[3]*b[6]+a[4]*b[7]+a[5]*b[8]; result[8]=a[6]*b[6]+a[7]*b[7]+a[8]*b[8]; }
double *InitTimePos; double *InitTimeOri; double *InitTimeVel; double *InitTimeAngVel; double *InitTimeAcc; double *InitTimeAngAcc;
NodalForceUsub_API void __cdecl nodal_force_ext (int id, double time, double upar[], int npar, int ifbody, int nodarr[], int nonde, int jflag, int iflag, double result[]) { using namespace rd_syscall; // Parameter Information // id : Nodal force seq. ID // time : Current simulation time of RecurDyn/Solver // upar : The list array of user parameters // npar : Number of arguments in the user parameter list array // ifbody : FFlex body seq. ID // nodarr : Node seq. ID array of input node set // nonde : Number of node of input node set // jflag : Jacobian flag // iflag : Initialization flag // result : [6*nonde]
if(npar<7) { errmes(1000,"Number of the input parameter must be 7",id,"NodalForceUSUB"); return; } int NodeID=(int)upar[0]; int ActionMarker=(int)upar[1]; int BaseMarker=(int)upar[2]; int TK=(int)upar[3]; int TC=(int)upar[4]; int RK=(int)upar[5]; int RC=(int)upar[6]; int ifTXActive,ifTYActive,ifTZActive; int ifRXActive,ifRYActive,ifRZActive; WCHAR NodeName[256]; char strNodeName[256];
double CurrentTimePos[12], CurrentTimeVel[6], CurrentTimeAcc[6], AtA[9], CheckA[9]; double dji[3],vji[3]; double Thetaij[3],wji[3],fp[6],dtmp[3]; int errflg,i,j; int iFinish;
if(iflag) { InitTimePos=new double[3*nonde]; InitTimeOri=new double[9*nonde]; InitTimeVel=new double[3*nonde]; InitTimeAngVel=new double[3*nonde]; InitTimeAcc=new double[3*nonde]; InitTimeAngAcc=new double[3*nonde]; for(i=0;i<nonde;i++) { get_fflex_nodestringname(ifbody,nodarr[i],(TCHAR*)NodeName,&errflg); wcstombs(strNodeName,NodeName,256); // Wide-byte character --> Multibyte character printmsg("====================",(int)strlen("==================")); printmsg(strNodeName,(int)strlen(strNodeName)); printmsg("====================",(int)strlen("==================")); } }
getfinishflag(&iFinish);
if(npar<13) // Default setting { ifTXActive = 1; // For trnanslation x direction, 0: Free, 1: Gen. a bushing force ifTYActive = 1; // For trnanslation y direction, 0: Free, 1: Gen. a bushing force ifTZActive = 1; // For trnanslation z direction, 0: Free, 1: Gen. a bushing force ifRXActive = 1; // For rotational x direction, 0: Free, 1: Gen. a bushing force ifRYActive = 1; // For rotational y direction, 0: Free, 1: Gen. a bushing force ifRZActive = 0; // For rotational z direction, 0: Free, 1: Gen. a bushing force } else { ifTXActive=(int)upar[7]; // For trnanslation x direction, 0: Free, 1: Gen. a bushing force ifTYActive=(int)upar[8]; // For trnanslation y direction, 0: Free, 1: Gen. a bushing force ifTZActive=(int)upar[9]; // For trnanslation z direction, 0: Free, 1: Gen. a bushing force ifRXActive=(int)upar[10]; // For rotational x direction, 0: Free, 1: Gen. a bushing force ifRYActive=(int)upar[11]; // For rotational y direction, 0: Free, 1: Gen. a bushing force ifRZActive=(int)upar[12]; // For rotational z direction, 0: Free, 1: Gen. a bushing force }
if(!iflag && !jflag && time == 0.0) { for(i=0;i<nonde;i++) { get_fflex_nodetpos(ifbody,nodarr[i],NULL,0,&InitTimePos[i*3],&errflg); get_fflex_noderpos(ifbody,nodarr[i],0,&InitTimeOri[i*9],&errflg); get_fflex_nodetvel(ifbody,nodarr[i],NULL,0,&InitTimeVel[i*3],&errflg); get_fflex_nodervel(ifbody,nodarr[i],NULL,0,&InitTimeAngVel[i*3],&errflg); get_fflex_nodetacc(ifbody,nodarr[i],NULL,0,&InitTimeAcc[i*3],&errflg); get_fflex_noderacc(ifbody,nodarr[i],NULL,0,&InitTimeAngAcc[i*3],&errflg); } }
for(i=0;i<nonde;i++) { get_fflex_nodepos(ifbody,nodarr[i],NULL,0,CurrentTimePos,&errflg); get_fflex_nodevel(ifbody,nodarr[i],NULL,0,CurrentTimeVel,&errflg); get_fflex_nodeacc(ifbody,nodarr[i],NULL,0,CurrentTimeAcc,&errflg);
for(j=0;j<3;j++) { dji[j]=CurrentTimePos[j]-InitTimePos[i*3+j]; vji[j]=CurrentTimeVel[j]-InitTimeVel[i*3+j]; wji[j]=CurrentTimeVel[j+3]-InitTimeAngVel[i*3+j]; } matatb(AtA,&CurrentTimePos[3],&InitTimeOri[i*9]); eulerangle(123,AtA,Thetaij,&errflg); orientationmatrix(123,Thetaij,CheckA,&errflg);
for(j=0;j<3;j++) { fp[j]=-dji[j]*TK-vji[j]*TC; dtmp[j]=Thetaij[j]*RK; }
asp(&fp[3],&CurrentTimePos[3],dtmp); // Force vector w.r.t global for(j=0;j<3;j++) { fp[j+3]=fp[j+3]-wji[j]*RC; }
if(ifTXActive==0) result[i*6+0]=0.0; else result[i*6+0]=fp[0]; if(ifTYActive==0) result[i*6+1]=0.0; else result[i*6+1]=fp[1]; if(ifTZActive==0) result[i*6+2]=0.0; else result[i*6+2]=fp[2]; if(ifRXActive==0) result[i*6+3]=0.0; else result[i*6+3]=fp[3]; if(ifRYActive==0) result[i*6+4]=0.0; else result[i*6+4]=fp[4]; if(ifRZActive==0) result[i*6+5]=0.0; else result[i*6+5]=fp[5]; }
if(iFinish==1) { if(InitTimePos) { delete [] InitTimePos; } if(InitTimeOri) { delete [] InitTimeOri; } if(InitTimeVel) { delete [] InitTimeVel; } if(InitTimeAngVel) { delete [] InitTimeAngVel; } if(InitTimeAcc) { delete [] InitTimeAcc; } if(InitTimeAngAcc) { delete [] InitTimeAngAcc; } } }
|