/** 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);
}
