HydroSDL/hydro_source/analytics_dyn.cpp

Go to the documentation of this file.

#include "math.h" 
#include "mex.h" 
#include "string.h" 

#include "Datas.h" 
#include "dataFillings.h" 
#include "analytics_dyn.h" 

#ifndef PI_DEF 
#define pi 3.141592653589793 
#define PI 3.141592653589793 
#endif 

// File generated on 18-Apr-2006, Basile.Graf@epfl.ch 


 void acceleration_compute(datas *d) { 
 
double trigoV[6]; 
 
trigoV[0] = sin(d->psi);
trigoV[1] = sin(d->theta);
trigoV[2] = sin(d->phi);
trigoV[3] = cos(d->psi);
trigoV[4] = cos(d->theta);
trigoV[5] = cos(d->phi);
 
 
d->ddq[0] = 1/d->M*(trigoV[3]*trigoV[4]*d->Force_G_E1-1.*trigoV[0]*trigoV[4]*d->Force_G_E2+trigoV[1]*d->Force_G_E3);
d->ddq[1] = 1/d->M*(d->Force_G_E1*trigoV[0]*trigoV[5]+d->Force_G_E1*trigoV[3]*trigoV[1]*trigoV[2]+d->Force_G_E2*trigoV[3]*trigoV[5]-1.*d->Force_G_E2*trigoV[0]*trigoV[1]*trigoV[2]-1.*trigoV[4]*trigoV[2]*d->Force_G_E3);
d->ddq[2] = 1/d->M*(d->Force_G_E1*trigoV[0]*trigoV[2]-1.*d->Force_G_E1*trigoV[3]*trigoV[1]*trigoV[5]+d->Force_G_E2*trigoV[3]*trigoV[2]+d->Force_G_E2*trigoV[0]*trigoV[1]*trigoV[5]+trigoV[4]*trigoV[5]*d->Force_G_E3-1.*d->M*d->g);
d->ddq[3] = -1./trigoV[4]*(-1.*trigoV[3]*d->Iy*d->Torque_G_E1-1.*d->dpsi*pow(d->Ix,2)*d->dtheta+d->Ix*d->Torque_G_E2*trigoV[0]+d->dtheta*pow(d->Ix,2)*d->dphi*pow(trigoV[3],2)*trigoV[1]+d->dpsi*pow(d->Ix,2)*pow(trigoV[3],2)*d->dtheta-1.*d->Ix*pow(trigoV[3],2)*d->dtheta*d->Iz*d->dphi*trigoV[1]-1.*d->Ix*pow(trigoV[3],2)*d->dtheta*d->dpsi*d->Iz-1.*pow(d->Iy,2)*pow(trigoV[3],2)*d->dtheta*d->dphi*trigoV[1]+d->Iy*pow(trigoV[3],2)*d->dtheta*d->Iz*d->dphi*trigoV[1]+d->Iy*pow(trigoV[3],2)*d->dtheta*d->dpsi*d->Iz-1.*pow(d->Iy,2)*pow(trigoV[3],2)*d->dpsi*d->dtheta+d->Ix*d->dtheta*d->Iz*d->dphi*trigoV[1]+d->Ix*d->dtheta*d->dpsi*d->Iz+d->Ix*d->dpsi*d->Iy*d->dtheta+trigoV[0]*trigoV[4]*trigoV[3]*pow(d->Iy,2)*pow(d->dphi,2)*trigoV[1]+trigoV[0]*trigoV[4]*trigoV[3]*d->dpsi*pow(d->Iy,2)*d->dphi+trigoV[0]*trigoV[4]*trigoV[3]*d->Ix*d->Iz*pow(d->dphi,2)*trigoV[1]+trigoV[0]*trigoV[4]*trigoV[3]*d->Ix*d->Iz*d->dphi*d->dpsi-1.*trigoV[0]*trigoV[4]*trigoV[3]*d->Iy*d->Iz*pow(d->dphi,2)*trigoV[1]-1.*trigoV[0]*trigoV[4]*trigoV[3]*d->Iy*d->Iz*d->dphi*d->dpsi-1.*trigoV[1]*pow(d->Ix,2)*d->dphi*d->dtheta-1.*trigoV[1]*pow(d->Ix,2)*pow(d->dphi,2)*trigoV[3]*trigoV[4]*trigoV[0]-1.*d->Ix*d->dtheta*d->Iy*d->dphi*trigoV[1]-1.*d->dpsi*pow(d->Ix,2)*d->dphi*trigoV[3]*trigoV[4]*trigoV[0])/d->Iy/d->Ix;
d->ddq[4] = -1.*(trigoV[3]*trigoV[0]*trigoV[1]*pow(d->Ix,2)*d->dphi*d->dtheta-1.*d->Iz*d->Ix*pow(d->dphi,2)*pow(trigoV[3],2)*trigoV[4]*trigoV[1]-1.*d->Iz*d->dpsi*d->Ix*d->dphi*pow(trigoV[3],2)*trigoV[4]+d->dpsi*pow(d->Ix,2)*d->dphi*pow(trigoV[3],2)*trigoV[4]+trigoV[0]*trigoV[3]*d->Iy*d->dtheta*d->Iz*d->dphi*trigoV[1]+d->Iz*d->dpsi*d->Iy*d->dtheta*trigoV[3]*trigoV[0]+d->dpsi*pow(d->Ix,2)*d->dtheta*trigoV[3]*trigoV[0]-1.*pow(d->Iy,2)*pow(d->dphi,2)*trigoV[4]*pow(trigoV[3],2)*trigoV[1]-1.*d->Iy*d->Iz*pow(d->dphi,2)*trigoV[4]*trigoV[1]-1.*d->Iy*d->Iz*d->dphi*trigoV[4]*d->dpsi-1.*d->Ix*d->dpsi*d->Iy*d->dphi*trigoV[4]-1.*d->dpsi*pow(d->Iy,2)*d->dtheta*trigoV[3]*trigoV[0]-1.*trigoV[0]*trigoV[3]*d->dtheta*pow(d->Iy,2)*d->dphi*trigoV[1]-1.*d->Ix*d->Torque_G_E2*trigoV[3]+pow(d->Ix,2)*pow(d->dphi,2)*pow(trigoV[3],2)*trigoV[4]*trigoV[1]-1.*d->dpsi*pow(d->Iy,2)*d->dphi*pow(trigoV[3],2)*trigoV[4]+d->Iz*d->Iy*pow(d->dphi,2)*trigoV[4]*pow(trigoV[3],2)*trigoV[1]+d->Iz*d->dpsi*d->Iy*d->dphi*pow(trigoV[3],2)*trigoV[4]-1.*trigoV[0]*trigoV[3]*d->Ix*d->dtheta*d->Iz*d->dphi*trigoV[1]-1.*d->Iz*d->dpsi*d->Ix*d->dtheta*trigoV[3]*trigoV[0]+pow(d->Iy,2)*pow(d->dphi,2)*trigoV[4]*trigoV[1]+d->dpsi*pow(d->Iy,2)*d->dphi*trigoV[4]-1.*d->Iy*d->Torque_G_E1*trigoV[0])/d->Iy/d->Ix;
d->ddq[5] = 1/trigoV[4]*(-2.*pow(d->Iy,2)*pow(trigoV[4],2)*d->Ix*d->dphi*pow(trigoV[3],2)*d->dtheta-1.*trigoV[3]*trigoV[4]*trigoV[0]*trigoV[1]*pow(d->Iz,2)*d->Iy*d->dphi*d->dpsi-1.*d->Iz*pow(d->Ix,2)*d->dphi*d->dtheta+pow(d->Iz,2)*d->Ix*d->dphi*d->dtheta+trigoV[3]*trigoV[4]*trigoV[0]*trigoV[1]*d->Iz*d->dpsi*pow(d->Iy,2)*d->dphi-1.*trigoV[3]*trigoV[4]*trigoV[0]*pow(d->Iz,2)*d->Iy*pow(d->dphi,2)+trigoV[3]*pow(trigoV[4],3)*trigoV[0]*pow(d->Iz,2)*d->Iy*pow(d->dphi,2)-1.*trigoV[3]*trigoV[1]*d->Iz*d->Iy*d->Torque_G_E1-1.*d->Iz*pow(d->Ix,2)*pow(d->dphi,2)*trigoV[3]*trigoV[4]*trigoV[0]+d->Iz*pow(d->Ix,2)*d->dphi*pow(trigoV[3],2)*d->dtheta+d->Iz*pow(trigoV[4],3)*pow(d->Ix,2)*pow(d->dphi,2)*trigoV[3]*trigoV[0]-1.*pow(d->Iz,2)*d->Ix*pow(trigoV[3],2)*d->dphi*d->dtheta-1.*d->Iz*pow(d->Iy,2)*pow(trigoV[3],2)*d->dphi*d->dtheta+pow(d->Iz,2)*d->Iy*pow(trigoV[3],2)*d->dphi*d->dtheta-1.*pow(d->Iz,2)*pow(trigoV[4],2)*d->Ix*d->dphi*d->dtheta+d->Iz*pow(trigoV[4],2)*pow(d->Ix,2)*d->dphi*d->dtheta+trigoV[3]*trigoV[4]*trigoV[0]*pow(d->Iz,2)*d->Ix*pow(d->dphi,2)-1.*pow(d->Iz,2)*pow(trigoV[4],2)*d->Iy*pow(trigoV[3],2)*d->dphi*d->dtheta+d->Iz*trigoV[1]*d->Ix*d->Torque_G_E2*trigoV[0]+d->Iy*trigoV[4]*d->Ix*d->Torque_G_E3+pow(d->Iz,2)*pow(trigoV[4],2)*d->Ix*pow(trigoV[3],2)*d->dphi*d->dtheta+trigoV[3]*trigoV[4]*trigoV[0]*d->Iz*pow(d->Iy,2)*pow(d->dphi,2)-1.*trigoV[3]*pow(trigoV[4],3)*trigoV[0]*d->Iz*pow(d->Iy,2)*pow(d->dphi,2)-1.*trigoV[3]*pow(trigoV[4],3)*trigoV[0]*pow(d->Iz,2)*d->Ix*pow(d->dphi,2)-1.*d->Iz*trigoV[1]*d->dpsi*pow(d->Ix,2)*d->dtheta-1.*d->Iz*d->Ix*d->Iy*d->dphi*d->dtheta-1.*d->Iz*trigoV[1]*pow(d->Iy,2)*pow(trigoV[3],2)*d->dpsi*d->dtheta-1.*d->Iy*pow(trigoV[4],3)*pow(d->Ix,2)*pow(d->dphi,2)*trigoV[3]*trigoV[0]-1.*d->Iz*trigoV[1]*d->dpsi*pow(d->Ix,2)*d->dphi*trigoV[3]*trigoV[4]*trigoV[0]+d->Iz*trigoV[1]*d->dpsi*pow(d->Ix,2)*pow(trigoV[3],2)*d->dtheta-1.*pow(d->Iz,2)*trigoV[1]*d->Ix*pow(trigoV[3],2)*d->dtheta*d->dpsi+pow(d->Iz,2)*trigoV[1]*d->Ix*d->dtheta*d->dpsi+d->Iz*trigoV[1]*d->Ix*d->dpsi*d->Iy*d->dtheta+pow(d->Iz,2)*trigoV[1]*d->Iy*pow(trigoV[3],2)*d->dtheta*d->dpsi-1.*pow(d->Ix,2)*pow(trigoV[3],2)*pow(trigoV[4],2)*d->Iz*d->dphi*d->dtheta+trigoV[3]*trigoV[4]*trigoV[0]*trigoV[1]*pow(d->Iz,2)*d->Ix*d->dphi*d->dpsi+pow(d->Iy,2)*pow(trigoV[4],2)*pow(trigoV[3],2)*d->Iz*d->dphi*d->dtheta+d->Iy*trigoV[4]*pow(d->Ix,2)*pow(d->dtheta,2)*trigoV[3]*trigoV[0]+pow(d->Iy,2)*pow(trigoV[4],3)*d->Ix*pow(d->dphi,2)*trigoV[3]*trigoV[0]+pow(d->Iy,2)*pow(trigoV[4],2)*d->Ix*d->dphi*d->dtheta-1.*d->Iy*pow(trigoV[4],2)*pow(d->Ix,2)*d->dphi*d->dtheta-1.*pow(d->Iy,2)*trigoV[4]*d->Ix*pow(d->dtheta,2)*trigoV[3]*trigoV[0]+2.*pow(d->Ix,2)*pow(trigoV[3],2)*pow(trigoV[4],2)*d->Iy*d->dphi*d->dtheta)/d->Iz/d->Iy/d->Ix;
return; 
} 
 
 

---