- Main Page
- Namespaces
- Data Structures
- Files

- Search for |

- File List
- Globals

# HydroSDL/hydro_source/analytics_dyn.cpp

Go to the documentation of this file.

    00001 #include "math.h"
    00002 #include "mex.h"
    00003 #include "string.h"
    00004
    00005 #include "Datas.h"
    00006 #include "dataFillings.h"
    00007 #include "analytics_dyn.h"
    00008
    00009 #ifndef PI_DEF
    00010 #define pi 3.141592653589793
    00011 #define PI 3.141592653589793
    00012 #endif
    00013
    00014 // File generated on 18-Apr-2006, Basile.Graf@epfl.ch
    00015
    00016
    00017  void acceleration_compute(datas *d) {
    00018
    00019 double trigoV[6];
    00020
    00021 trigoV[0] = sin(d->psi);
    00022 trigoV[1] = sin(d->theta);
    00023 trigoV[2] = sin(d->phi);
    00024 trigoV[3] = cos(d->psi);
    00025 trigoV[4] = cos(d->theta);
    00026 trigoV[5] = cos(d->phi);
    00027
    00028
    00029 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);
    00030 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);
    00031 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);
    00032 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;
    00033 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;
    00034 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;
    00035 return;
    00036 }
    00037
    00038