void BeforeLevelState::resume(RoidsApp * application) {
ApplicationState * s = getPreviousState();
changeState(application, this);
setPreviousState(s);
_screen->setMessage(getMessage());
_application->addScreen(_screen);
}
void QuitConfirmState::changeState(RoidsApp * application, ApplicationState * state) {
_application = application;
ApplicationState * s = _application->getState();
if(s != NULL) {
s->suspend();
setPreviousState(s);
}
_application->changeState(state);
}
/* Pas de diffusion */
void LogResSolver3D::diffuse (unsigned char b, float *const x, float *const x0, float a)
{
m_file = &m_fileDiff[b-1];
saveState(x, x0);
GS_solve(b,x,x0,a, 1/(1.0f + 6.0f * a), m_nbSteps);
m_file = &m_fileDiff[b+2];
setPreviousState(x, x0);
GCSSOR(x,x0,a, (1.0f + 6.0f * a), m_omegaDiff,m_nbSteps);
}
int main(int argc,char **argv) {
init();
ros::init(argc,argv, "Controller");
ros::NodeHandle nh;
ros::Subscriber sub_state = nh.subscribe("auv/state", 1000, &stateListenerCallBack);
ros::Subscriber sub_cmd_vel = nh.subscribe("sim/cmd_vel", 1000, &cmd_velCallBack);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("zeabus/cmd_vel",1000);
ros::Rate rate(100);
while(ros::ok) {
setPreviousState();
ros::spinOnce();
pub.publish(calculatePID());
//printf("Current velo : %lf %lf %lf (%lf %lf %lf)\n",vel[0],vel[1],vel[2],cmd_vel[0],cmd_vel[1],cmd_vel[2]);
printf(" x = %lf \n",position[3]);
printf("x=%lf y=%lf z=%lf\n",position[0],position[1],position[2]);
printf("roll=%lf pitch=%lf yaw=%lf\n",position[3],position[4],position[5]);
rate.sleep();
}
ros::shutdown();
}
void LogResSolver3D::project (float *const p, float *const div)
{
float h_x = 1.0 / m_nbVoxelsX, h_y = 1.0 / m_nbVoxelsY, h_z = 1.0 / m_nbVoxelsZ;
uint i, j, k;
for (i = 1; i <= m_nbVoxelsX; i++)
for (j = 1; j <= m_nbVoxelsY; j++)
for (k = 1; k <= m_nbVoxelsZ; k++){
div[IX (i, j, k)] =
-0.5f * (h_x * (m_u[IX (i + 1, j, k)] - m_u[IX (i - 1, j, k)]) +
h_y * (m_v[IX (i, j + 1, k)] - m_v[IX (i, j - 1, k)]) +
h_z * (m_w[IX (i, j, k + 1)] - m_w[IX (i, j, k - 1)]));
//p[IX (i, j, k)] = 0;
}
// set_bnd (0, div);
fill_n(p, m_nbVoxels, 0.0f);
// set_bnd (0, p);
m_file = &m_fileProj[m_index];
saveState(p, div);
GS_solve(0,p,div,1, 1/6.0f, m_nbSteps);
m_file = &m_fileProj[m_index+2];
setPreviousState(p, div);
GCSSOR(p,div,1, 6.0f, m_omegaProj,m_nbSteps);
for (i = 1; i <= m_nbVoxelsX; i++)
for (j = 1; j <= m_nbVoxelsY; j++)
for (k = 1; k <= m_nbVoxelsZ; k++){
m_u[IX (i, j, k)] -= 0.5f * (p[IX (i + 1, j, k)] - p[IX (i - 1, j, k)]) / h_x;
m_v[IX (i, j, k)] -= 0.5f * (p[IX (i, j + 1, k)] - p[IX (i, j - 1, k)]) / h_y;
m_w[IX (i, j, k)] -= 0.5f * (p[IX (i, j, k + 1)] - p[IX (i, j, k - 1)]) / h_z;
}
//set_bnd (1, u);
//set_bnd (2, v);
//set_bnd (3, w);
}
void optimizingFinished() { setPreviousState(); }
