Axes::Axe DataMotion::determineAxe(Point * p1, Point * p2)
{
QVector<float> distance = calculDistance(p1, p2);
float max = qAbs(distance.at(0));// Ne pas remplacer 0 par Axes::X car cela a des effets de bords
for (int i=1; i<distance.size(); i++)
{
max = qMax(qAbs(max), qAbs(distance.at(i)));
}
qDebug() << distance;
Axes::Axe res = Axes::UNDEFINED;
if (max == qAbs(distance.at(Coordonnees::X)))
res = Axes::X;
if (max == qAbs(distance.at(Coordonnees::Y)))
res = Axes::Y;
if (max == qAbs(distance.at(Coordonnees::Z)))
res = Axes::Z;
//if (distance.at(Coordonnees::X) < 8) // Ne fonctionne pas correctement
// res = Axes::ALL;
return res;
}
// a faire lecture plusieur fois de la distance et moy var et exclusion des valeur extreme sur genre 10 val
// 20 ms max par boucle si rien autour => 200 ms par mesure de 10
void Scan::writeAngle(int angle) {
scanner->servo.write(angle);
double distance = calculDistance();
if ( scanner->taille < MAXSIZE ) {
scanner->taille += snprintf(scanner->data + (scanner->taille),MAXSIZE-(scanner->taille),"%.2f,",distance);
}
}
void set_coef(t_star *star, int x, int y)
{
int coef;
if (star->open_list[x][y] != -1)
{
coef = calculDistance(x, star->end_x, y, star->end_y) * 10;
coef += calcul_indice(star, x, y) + 1;
star->open_list[x][y] = coef;
}
}
float Position::calculDistance(Position P)
{
return calculDistance(P.getX(), P.getY());
}
void RT_secondOrder(Pnum* m,Pnum* velocity,unsigned nb_row, unsigned nb_col, unsigned* f, unsigned nb_points_front){
printf("Algorithm Rouy-Tourin : problem %d x %d with finite difference second order\n",nb_row,nb_col);
FILE *ft = gnudata_open("time_so");
FILE *fiter = gnudata_open("iter_so");
double time_start = give_time(), time_end = 0;
Pnum* m_0 = (Pnum*) malloc(nb_col*nb_row*sizeof(Pnum));
//Initialisation des noeuds
int i,j;
for (i = 0; i<nb_row; i++) {
for (j = 0; j<nb_col; j++) {
m_0[ind(nb_col,i,j)] = INF;
}
}
//Initialisation des noeuds sources
for (i = 0; i<2*nb_points_front; i+=2) {
m_0[ind(nb_col,f[i], f[i+1])] = 0.0;
}
int s = 0, fi = 0, nb_iter = 0, convergence = 1;
do{
convergence = 1;
copie(m_0, m, nb_row, nb_col);
for (i = 0; i<nb_row; i++) {
for (j = 0; j<nb_col; j++) {
if (isInObstacle(velocity, nb_col, i,j)){
m_0[ind(nb_col, i, j)] = INF;
continue;
}
Pnum x_m1 = (i-1 >= 0) ? m[ind(nb_col, i-1, j)] : INF;
Pnum x_m2 = (i-2 >= 0) ? m[ind(nb_col, i-2, j)] : INF;
Pnum x_p1 = (i+1 < nb_col) ? m[ind(nb_col, i+1, j)] : INF;
Pnum x_p2 = (i+2 < nb_col) ? m[ind(nb_col, i+2, j)] : INF;
Pnum y_m1 = (j-1 >= 0) ? m[ind(nb_col, i, j-1)] : INF;
Pnum y_m2 = (j-2 >= 0) ? m[ind(nb_col, i, j-2)] : INF;
Pnum y_p1 = (j+1 < nb_row) ? m[ind(nb_col, i, j+1)] : INF;
Pnum y_p2 = (j+2 < nb_row) ? m[ind(nb_col, i, j+2)] : INF;
Pnum sol, Tx, Ty, Tx_m , Tx_p, Ty_m, Ty_p, f;
f = velocity[ind(nb_col, i, j)];
if (x_p2 < INF && x_m2 < INF && y_m2 < INF && y_p2 < INF ){
s++;
Tx_m = fmaxf((4*x_m1-x_m2)/3.0, 0); Tx_p = fminf((4*x_p1-x_p2)/3.0, 0);
Ty_m = fmaxf((4*y_m1-y_m2)/3.0, 0); Ty_p = fminf((4*y_p1-y_p2)/3.0, 0);
sol = solveEquation_2(Tx_m,Tx_p, Ty_m, Ty_p, h, f);
}else{
fi++;
Tx = fminf(x_m1, x_p1);
Ty = fminf(y_m1, y_p1);
sol = solveEquation_1(Tx,Ty,h,f);
}
Pnum sol_min = fminf(m[ind(nb_col, i, j)],sol);
m_0[ind(nb_col, i, j)] = sol_min;
convergence = convergence && (fabsf( m[ind(nb_col,i, j)] - m_0[ind(nb_col, i, j)] ) <= epsilon);
}
}
nb_iter++;
} while (!convergence);
time_end = give_time();
dput_xy(ft, nb_row, time_end-time_start);
dput_xy(fiter, nb_row, nb_iter);
calculDistance(2,m, nb_col/2, nb_row/2, nb_row, nb_col);
free(m_0);
printf("%d itérations\n",nb_iter);
printf("TIME : %f sec\n\n",time_end-time_start);
}
void RT_firstOrder(Pnum* m,Pnum* velocity,unsigned nb_row, unsigned nb_col, unsigned* f, unsigned nb_points_front){
printf("Algorithm Rouy-Tourin : problem %d x %d with finite difference fisrt order\n",nb_row,nb_col);
FILE *ft = gnudata_open("time_fo");
FILE *fiter = gnudata_open("iter_fo");
double time_start = give_time(), time_end = 0;
Pnum* m_0 = (Pnum*) malloc(nb_col*nb_row*sizeof(Pnum));
//Initialisation des noeuds
int i,j,nb_iter = 0,convergence = 1;
for (i = 0; i<nb_row; i++) {
for (j = 0; j<nb_col; j++) {
m_0[ind(nb_col,i,j)] = INF;
}
}
//Initialisation des noeuds sources
for (i = 0; i<2*nb_points_front; i+=2) {
m_0[ind(nb_col,f[i], f[i+1])] = 0.0;
}
do{
convergence = 1;
copie(m_0, m, nb_row, nb_col);
for (i = 0; i<nb_row; i++) {
for (j = 0; j<nb_col; j++) {
if (isInObstacle(velocity, nb_col, i,j)){
m_0[ind(nb_col, i, j)] = INF;
continue;
}
Pnum x_m1 = (i-1 >= 0) ? m[ind(nb_col, i-1, j)] : INF;
Pnum x_p1 = (i+1 < nb_col) ? m[ind(nb_col, i+1, j)] : INF;
Pnum y_m1 = (j-1 >= 0) ? m[ind(nb_col, i, j-1)] : INF;
Pnum y_p1 = (j+1 < nb_row) ? m[ind(nb_col, i, j+1)] : INF;
Pnum Tx = fminf(x_m1, x_p1);
Pnum Ty = fminf(y_m1, y_p1);
Pnum f = velocity[ind(nb_col, i, j)];
Pnum sol = solveEquation_1(Tx,Ty,h,f);
Pnum sol_min = fminf(m[ind(nb_col, i, j)],sol);
m_0[ind(nb_col, i, j)] = sol_min;
convergence = convergence && (fabsf( m[ind(nb_col,i, j)] - m_0[ind(nb_col, i, j)] ) <= epsilon);
}
}
if (PRINT) {
printf("\n----\n");
print_matrice(m, nb_row, nb_col);
}
nb_iter++;
} while (!convergence);
time_end = give_time();
dput_xy(ft, nb_row, time_end-time_start);
dput_xy(fiter, nb_row, nb_iter);
calculDistance(1,m, nb_col/2, nb_row/2, nb_row, nb_col);
free(m_0);
printf("%d itérations\n",nb_iter);
printf("TIME : %f sec\n\n",time_end-time_start);
}
