4.8.3.12. GET_FFLEX_NODEPOS
GET_FFLEX_NODEPOS subroutine returns a position vector and an orientation matrix for a node of FFlex body. This is an auxiliary subroutine for Nodal_Force and Nodal_Force_Ext.
Language type |
Subroutine |
FORTRAN |
call get_fflex_nodepos (ifbody,NodeSeq,MKID,nMK,POS,ErrFlg) |
C/C++ |
get_fflex_nodepos (ifbody,NodeSeq,MKID[2],2,POS,&errflg) |
Variable Name |
Size |
Description |
ifbody |
int |
Sequential id of FFlex body defined in RecurDyn/Solver. This is a related argument with the 5th argument of Nodal_Force_Ext subroutine. |
NodeSeq |
int |
Node sequential id defined in RecurDyn/Solver. This is a related argument with the 6th argument of Nodal_Force_Ext subroutine. |
MKID |
itn[2] |
An array of integer type. Each value should be zero or a marker id. 1st value is defined as base marker. 2nd value is defined as reference marker. |
nMK |
int |
An integer variable for considering base and reference marker. If nMK is 0, then the subroutine returns a global position vector and an orientation matrix of NodeSeq. If nMK is 1, then the subroutine calculates a position vector considering base marker. If nMK is 2, then the subroutine calculates a position vector considering base and reference marker. |
Pos |
double[12] |
An array of double precision type. The array size must be 12. First 3 values mean a position vector.
\(\mathbf{A}_{r}^{T}({{\mathbf{r}}_{FFlexNode}}-{{\mathbf{r}}_{b}})\)
Where,
\(\mathbf{A}_{r}^{{}}\) is a orientation matrix of the reference marker (=MKID[1]).
\({{\mathbf{r}}_{FFlexNode}}\) is a global position vector of NodeSeq.
\({{\mathbf{r}}_{b}}\) is a global position vector of the base marker (=MKID[0]).
The last 9 values mean an orientation matrix.
\(\mathbf{A}_{b}^{T}{{\mathbf{A}}_{FFlexNode}}\)
Where,
\(\mathbf{A}_{b}^{{}}\) is a orientation matrix of the base marker (=MKID[0]).
\({{\mathbf{A}}_{FFlexNode}}\) is an orientation matrix of NodeSeq
|
Errflg |
int |
Error flag.
If the result of this argument is -1
(means TRUE in Fortran logical
value), there is no error.
The others mean that there is an error.
|
C---- SUB. NODAL_FORCE_EXT
SUBROUTINE NODAL_FORCE_EXT
& (ID,TIME,UPAR,NPAR,IFBODY,NODARR,NONDE,JFLAG,IFLAG,RESULT)
C---- TO EXPORT * SUBROUTINE
!DEC$ ATTRIBUTES DLLEXPORT,C::NODAL_FORCE_EXT
C---- INCLUDE SYSTEM CALL
INCLUDE 'SYSCAL.F'
C---- DEFINE VARIABLES
C Parameter Information
C id: Nodal sequential identification
C time: Simulation time of RD/Solver
C upar: Parameters defined by user
C npar: Number of user parameters
C ifbody : FFlex body seq. ID
C nodarr: Node Seq. ID array of input node set
C nonde : Number of node of node set
C jflag: When RD/Solver evaluates a Jacobian, the flag is true.
C iflag: When RD/Solver initializes arrays, the flag is true.
C result: Returned values[6]
DOUBLE PRECISION TIME, UPAR(*)
INTEGER ID, NPAR,NONDE,NODARR(*), IFBODY
LOGICAL JFLAG, IFLAG
DOUBLE PRECISION RESULT[REFERENCE](6*nonde)
c local variable
integer NODEID, ActionMarker, BaseMarker
double precision TK, TC, RK, RC
integer ifTXActive, ifTYActive, ifTZActive
integer ifRXActive, ifRYActive, ifRZActive
integer iFinish, i, j, MKID(2), errflg, nmk
double precision CurrentTimePos(12)
double precision CURRENTTIMEVEL(6)
double precision CURRENTTIMEACC(6)
double precision dji(3), vji(3), wji(3), thetaij(3), ATA(9)
double precision CheckA(9), fp(6), dtmp(3)
double precision, STATIC :: InitTimePos(100*3)
double precision, STATIC :: InitTimeOri(100*9)
double precision, STATIC :: InitTimeVel(100*3)
double precision, STATIC :: InitTimeAngVel(100*3)
double precision, STATIC :: InitTimeAcc(100*3)
double precision, STATIC :: InitTimeAngAcc(100*3)
if(nonde .gt. 100) then
call errmes(1000,'Node set limit error',id,
& 'Nodal_Force_Ext')
endif
if(IFLAG .eq. .true.) then
endif
NODEID = upar(1)
ACTIONMARKER = upar(2)
BASEMARKER = upar(3)
TK = upar(4)
TC = upar(5)
RK = upar(6)
RC = upar(7)
call getfinishflag(iFinish)
if(npar.lt.13) then ! Default setting
! For translation x direction, 0: Free, 1: Gen. a bushing force
ifTXActive = 1
! For translation y direction, 0: Free, 1: Gen. a bushing force
ifTYActive = 1
! For translation z direction, 0: Free, 1: Gen. a bushing force
ifTZActive = 1
! For rotational x direction, 0: Free, 1: Gen. a bushing force
ifRXActive = 1
! For rotational y direction, 0: Free, 1: Gen. a bushing force
ifRYActive = 1
! For rotational z direction, 0: Free, 1: Gen. a bushing force
ifRZActive = 0
else
ifTXActive = upar(7)
ifTYActive = upar(8)
ifTZActive = upar(9)
ifRXActive = upar(10)
ifRYActive = upar(11)
ifRZActive = upar(12)
endif
MKID(1)=0 ! Base marker : Ground.Inertia
MKID(2)=0 ! Reference marker : Ground.Inertia
nmk = 0
if(iflag .eq. .false. .and.
& jflag .eq. .false. .and. time .eq. 0.0d0) then
do i=1, nonde
call GET_FFLEX_NODETPOS(ifbody,nodarr(i),MKID,
& nmk,InitTimePos((i-1)*3+1),errflg)
call GET_FFLEX_NODERPOS(ifbody,nodarr(i),nmk,
& InitTimeOri((i-1)*9+1),errflg)
call GET_FFLEX_NODETVEL(ifbody,nodarr(i),MKID,nmk,
& InitTimeVel((i-1)*3+1),errflg)
call GET_FFLEX_NODERVEL(ifbody,nodarr(i),MKID,nmk,
& InitTimeAngVel((i-1)*3+1),errflg)
call GET_FFLEX_NODETACC(ifbody,nodarr(i),MKID,nmk,
& InitTimeAcc((i-1)*3+1),errflg)
call GET_FFLEX_NODERACC(ifbody,nodarr(i),MKID,nmk,
& InitTimeAngAcc((i-1)*3+1),errflg)
enddo
endif
do i=1, nonde
call get_fflex_nodepos(ifbody,nodarr(i),MKID, nmk,
& CurrentTimePos,errflg)
call get_fflex_nodevel(ifbody,nodarr(i),MKID, nmk,
& CurrentTimeVel,errflg)
call get_fflex_nodeacc(ifbody,nodarr(i),MKID, nmk,
& CurrentTimeAcc,errflg)
do j=1, 3
dji(j)=CurrentTimePos(j)-InitTimePos((i-1)*3+j)
vji(j)=CurrentTimeVel(j)-InitTimeVel((i-1)*3+j)
wji(j)=CurrentTimeVel(j+3)-InitTimeAngVel((i-1)*3+j)
enddo
call matatb(AtA,CurrentTimePos(4),InitTimeOri((i-1)*9+1))
call eulerangle(123,AtA(1),Thetaij(1),errflg)
call orientationmatrix(123,Thetaij(1),CheckA(1),errflg)
do j=1, 3
fp(j)=-dji(j)*TK-vji(j)*TC;
dtmp(j)=Thetaij(j)*RK;
enddo
call asp(fp(4),CurrentTimePos(4),dtmp); ! Force vector w.r.t global
do j=1, 3
fp(j+3)=fp(j+3)-wji(j)*RC;
enddo
if(ifTXActive.eq.0) then
result((i-1)*6+1)=0.0;
else
result((i-1)*6+1)=fp(1);
endif
if(ifTYActive.eq.0) then
result((i-1)*6+2)=0.0;
else
result((i-1)*6+2)=fp(2);
endif
if(ifTZActive.eq.0) then
result((i-1)*6+3)=0.0;
else
result((i-1)*6+3)=fp(3);
endif
if(ifRXActive.eq.0) then
result((i-1)*6+4)=0.0;
else
result((i-1)*6+4)=fp(4);
endif
if(ifRYActive.eq.0) then
result((i-1)*6+5)=0.0;
else
result((i-1)*6+5)=fp(5);
endif
if(ifRZActive.eq.0) then
result((i-1)*6+6)=0.0;
else
result((i-1)*6+6)=fp(6);
endif
enddo
if(iFinish.eq.1) then
endif
RETURN
END
subroutine asp(result_s, A, sp)
double precision result_s(3),A(9),sp(3)
result_s(1)=A(1)*sp(1)+A(4)*sp(2)+A(7)*sp(3)
result_s(2)=A(2)*sp(1)+A(5)*sp(2)+A(8)*sp(3)
result_s(3)=A(3)*sp(1)+A(6)*sp(2)+A(9)*sp(3)
return
end
subroutine matatb(result_c, a, b)
double precision result_c(9),a(9),b(9)
result_c(1)=a(1)*b(1)+a(2)*b(2)+a(3)*b(3)
result_c(2)=a(4)*b(1)+a(5)*b(2)+a(6)*b(3)
result_c(3)=a(7)*b(1)+a(8)*b(2)+a(9)*b(3)
result_c(4)=a(1)*b(4)+a(2)*b(5)+a(3)*b(6)
result_c(5)=a(4)*b(4)+a(5)*b(5)+a(6)*b(6)
result_c(6)=a(7)*b(4)+a(8)*b(5)+a(9)*b(6)
result_c(7)=a(1)*b(7)+a(2)*b(8)+a(3)*b(9)
result_c(8)=a(4)*b(7)+a(5)*b(8)+a(6)*b(9)
result_c(9)=a(7)*b(7)+a(8)*b(8)+a(9)*b(9)
return
end
#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; }
}
}