/** Update the cache of blocked nodes. */ static void path_blocked_update (void) { uint8_t i, j; for (i = 0; i < PATH_GRID_NODES_NB; i++) { uint8_t valid = 1; /* First, gather information from tables. */ if (!path_nodes[i].usable || food_blocking (path_nodes[i].carry_corn)) valid = 0; else { vect_t pos; path_pos (i, &pos); /* Then, test for obstacles. */ for (j = 0; j < PATH_OBSTACLES_NB; j++) { if (path.obstacles[j].valid) { vect_t v = pos; vect_sub (&v, &path.obstacles[j].c); uint32_t dsq = vect_dot_product (&v, &v); uint32_t r = path.obstacles[j].r; if (dsq <= r * r) { valid = 0; break; } } } } /* Update cache. */ path.valid[i] = valid; } }
uint8_t radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, const vect_t *obs_pos, uint8_t obs_nb) { uint8_t i; /* Stop here if no obstacle. */ if (!obs_nb) return 0; vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); uint16_t d = vect_norm (&vd); /* If destination is realy near, stop here. */ if (d < RADAR_EPSILON_MM) return 0; /* If destination is near, use clearance to destination point instead of * stop length. */ vect_t t; if (d < RADAR_STOP_MM) t = *dest_pos; else { vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); t = *robot_pos; vect_translate (&t, &vd); } /* Now, look at obstacles. */ for (i = 0; i < obs_nb; i++) { /* Vector from robot to obstacle. */ vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); /* Ignore if in our back. */ int32_t dp = vect_dot_product (&vd, &vo); if (dp < 0) continue; /* Check distance. */ int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 + RADAR_OBSTACLE_RADIUS_MM) continue; /* Else, obstacle is blocking. */ return 1; } return 0; }
void test_basic_vector_op(void) { /******************************/ /* Test the vector operations */ /******************************/ /*create two vectors and show them*/ int vector_length = 5; double vector_a[5] = {2.5, -10.9, 15.8, 12.2, 7.9}; double vector_b[5] = {2.5, 0.2, 21.33, 70.1, -0.2}; double vector_c[5]; double result = 0.0; /*seed the random generator*/ srand(time(NULL)); printf("The two vectors a and b\n"); show_vector(vector_a, vector_length); show_vector(vector_b, vector_length); /*vector addition*/ printf("c = a + b\n"); /*copy a*/ memcpy(vector_c, vector_a, vector_length*sizeof(double)); /*compute*/ vect_add(vector_c, vector_b, vector_length); /*show expected and obtained results*/ printf("Expected: 5.0000 -10.7000 37.1300 82.3000 7.7000\n"); printf("Result: "); show_vector(vector_c, vector_length); /*vector subtraction*/ printf("c = a - b\n"); /*copy a*/ memcpy(vector_c, vector_a, vector_length*sizeof(double)); /*compute*/ vect_sub(vector_c, vector_b, vector_length); /*show expected and obtained results*/ printf("Expected: 0.0000 -11.1000 -5.5300 -57.9000 8.1000\n"); printf("Result: "); show_vector(vector_c, vector_length); /*vector scalar multiplication*/ printf("c = a * 5\n"); /*copy a*/ memcpy(vector_c, vector_a, vector_length*sizeof(double)); /*compute*/ vect_scalar_multiply(vector_c, 5, vector_length); /*show expected and obtained results*/ printf("Expected: 12.5000 -54.5000 79.0000 61.0000 39.5000\n"); printf("Result: "); show_vector(vector_c, vector_length); /*vector dot product*/ printf("c = a .* b\n"); /*compute*/ result = vect_dot_product(vector_a, vector_b, vector_length); /*show expected and obtained results*/ printf("Expected: 1194.72\n"); printf("Result: %2.2f\n",result); /*vector cross product*/ /*To be completed*/ /*vector norm*/ printf("c = |a|\n"); /*compute*/ result = vect_norm(vector_a, vector_length); /*show expected and obtained results*/ printf("Expected: 24.2064\n"); printf("Result: %2.4f\n",result); /*vector rand unit*/ printf("c is a random vector\n"); printf("|c| == 1\n"); /*compute*/ vect_rand_unit(vector_c, vector_length); /*show expected and obtained results*/ printf("The vector: "); show_vector(vector_c, vector_length); result = vect_norm(vector_c, vector_length); printf("Expected norm: 1.0000\n"); printf("Obtained norm: %2.4f\n",result); }
/** * void mtx_lanczos_procedure(double *A, double *Tm, int n, int m) * * @brief Function that implements the first step of the eigenproblem solution * based on lanzos algorithm. This function generates a matrix Tm that contains * a set (<=) of eigenvalues that approximate those of matrix A. Finding the eigenvalues * in Tm is easier and serves as an optimization method for problems in which only a few * eigenpairs are required. * @param A, the matrix on which the procedure is applied. Needs to be square and Hermitian. * @param (out)a, elements of the diagonal of the matrix Tm * @param (out)b, elements off diagonal of the matrix Tm * @param n, the dimensions of the square matrix A * @param m, the number of iterations for the lanczos procedure (and the dimensions of the array returned) */ void mtx_lanczos_procedure(double *A, double *a, double *b, int n, int m) { int i,j,array_index_i; double* v_i = (double*)calloc(n, sizeof(double)); double* a_times_v_i = (double*)calloc(n, sizeof(double)); double* b_times_v_i_minus_one = (double*)calloc(n, sizeof(double)); double* v_i_minus_one = (double*)calloc(n, sizeof(double)); double* w_i = (double*)calloc(n, sizeof(double)); double* v1 = (double*)calloc(n, sizeof(double)); double* temp; /*get a n-long random vector with norm equal to 1*/ //vect_rand_unit(v1,n); for(i=0;i<n;i++){ v1[i] = 1/sqrt(n); } /*first iteration of the procedure*/ i = 1; array_index_i = i-1; /*computes w_i*/ /*w[i] <= A*v[i]*/ mtx_mult(A, v1, w_i, n, n, 1); /*computes a_i*/ /*a[i] <= w[i]*v[i]*/ a[array_index_i] = vect_dot_product(w_i, v1,n); /*update w_i*/ /*w[i] <= w[i]-a[i]*v[i]-b[i]*v[i-1]*/ /*note: b[1] = 0*/ memcpy(a_times_v_i,v1,n*sizeof(double)); vect_scalar_multiply(a_times_v_i,a[array_index_i],n); vect_sub(w_i, a_times_v_i, n); /*computes next b_i*/ /*b[i+1] = ||w[i]||*/ b[array_index_i+1] = vect_norm(w_i,n); /*copy v1 to v_i*/ memcpy(v_i,v1,n*sizeof(double)); /*save v[i] to be used asv[i-1]*/ temp = v_i_minus_one; v_i_minus_one = v_i; /*computes next v_i = w_i/b(i+1)*/ /*v[i+1] = w[i]/b[i+1]*/ v_i = w_i; vect_scalar_multiply(v_i, 1/b[array_index_i+1], n); /*reuse memory former v_i_minus_one space for w_i*/ w_i = temp; /*rest of the iterations of the procedure*/ for(i=2;i<=m-1;i++){ array_index_i = i-1; /*computes w_i*/ /*w[i] <= A*v[i]*/ mtx_mult(A, v_i, w_i, n, n, 1); /*computes a_i*/ /*a[i] <= w[i]*v[i]*/ a[array_index_i] = vect_dot_product(w_i, v_i, n); /*update w_i*/ /*prepare a[i]*v[i]*/ memcpy(a_times_v_i,v_i,n*sizeof(double)); vect_scalar_multiply(a_times_v_i,a[array_index_i],n); /*prepare b[i]*v[i-1]*/ memcpy(b_times_v_i_minus_one,v_i_minus_one,n*sizeof(double)); vect_scalar_multiply(b_times_v_i_minus_one,b[array_index_i],n); /*w[i] <= w[i]-a[i]*v[i]-b[i]*v[i-1]*/ vect_sub(w_i, a_times_v_i,n); vect_sub(w_i, b_times_v_i_minus_one,n); /*computes next b_i*/ /*b[i+1] = ||w[i]||*/ b[array_index_i+1] = vect_norm(w_i,n); /*save current v_i*/ temp = v_i_minus_one; v_i_minus_one = v_i; /*computes next v_i = w_i/b(i+1)*/ /*v[i+1] = w[i]/b[i+1]*/ v_i = w_i; vect_scalar_multiply(v_i, 1/b[array_index_i+1],n); /*reuse memory former v_i_minus_one space for w_i*/ w_i = temp; } /*compute last term a[m]*/ array_index_i = m-1; mtx_mult(A, v_i, w_i, n, n, 1); a[array_index_i] = vect_dot_product(w_i,v_i,n); /*translate data representation in the b matrix to match the CLAPACK standard*/ for(i=0;i<(n-1);i++){ b[i] = b[i+1]; } b[n-1] = 0; free(v1); free(v_i); free(a_times_v_i); free(b_times_v_i_minus_one); free(v_i_minus_one); free(w_i); }