BAND *bd_copy(const BAND *A, BAND *B)
#endif
{
int lb,ub,i,j,n;
if ( !A )
error(E_NULL,"bd_copy");
if (A == B) return B;
n = A->mat->n;
if ( !B )
B = bd_get(A->lb,A->ub,n);
else if (B->lb != A->lb || B->ub != A->ub || B->mat->n != n )
B = bd_resize(B,A->lb,A->ub,n);
if (A->mat == B->mat) return B;
ub = B->ub = A->ub;
lb = B->lb = A->lb;
for ( i=0, j=n-lb; i <= lb; i++, j++ )
MEM_COPY(A->mat->me[i],B->mat->me[i],j*sizeof(Real));
for ( i=lb+1, j=1; i <= lb+ub; i++, j++ )
MEM_COPY(A->mat->me[i]+j,B->mat->me[i]+j,(n - j)*sizeof(Real));
return B;
}
BAND *bd_resize(BAND *A, int new_lb, int new_ub, int new_n)
#endif
{
int lb,ub,i,j,l,shift,umin;
Real **Av;
if (new_lb < 0 || new_ub < 0 || new_n <= 0)
error(E_NEG,"bd_resize");
if ( ! A )
return bd_get(new_lb,new_ub,new_n);
if ( A->lb+A->ub+1 > A->mat->m )
error(E_INTERN,"bd_resize");
if ( A->lb == new_lb && A->ub == new_ub && A->mat->n == new_n )
return A;
lb = A->lb;
ub = A->ub;
Av = A->mat->me;
umin = min(ub,new_ub);
/* ensure that unused triangles at edges are zero'd */
for ( i = 0; i < lb; i++ )
for ( j = A->mat->n - lb + i; j < A->mat->n; j++ )
Av[i][j] = 0.0;
for ( i = lb+1,l=1; l <= umin; i++,l++ )
for ( j = 0; j < l; j++ )
Av[i][j] = 0.0;
new_lb = A->lb = min(new_lb,new_n-1);
new_ub = A->ub = min(new_ub,new_n-1);
A->mat = m_resize(A->mat,new_lb+new_ub+1,new_n);
Av = A->mat->me;
/* if new_lb != lb then move the rows to get the main diag
in the new_lb row */
if (new_lb > lb) {
shift = new_lb-lb;
for (i=lb+umin, l=i+shift; i >= 0; i--,l--)
MEM_COPY(Av[i],Av[l],new_n*sizeof(Real));
for (l=shift-1; l >= 0; l--)
__zero__(Av[l],new_n);
}
else if (new_lb < lb) {
shift = lb - new_lb;
for (i=shift, l=0; i <= lb+umin; i++,l++)
MEM_COPY(Av[i],Av[l],new_n*sizeof(Real));
for (i=lb+umin+1; i <= new_lb+new_ub; i++)
__zero__(Av[i],new_n);
}
return A;
}
int test_traj_end(int why)
{
if((why & END_TRAJ) && trajectory_finished(&robot.traj))
return END_TRAJ;
if (why & END_NEAR) {
int16_t d_near = 100; /* mm */
/* XXX Change distance depending on speed. */
if (trajectory_in_window(&robot.traj, d_near, RAD(5.0)))
return END_NEAR;
}
if((why & END_OBSTACLE)) {
int i;
for(i=0;i<robot.beacon.nb_beacon;i++) {
if(robot.beacon.beacon[i].distance > 80) { /*cm*/
if(robot.distance_qr.previous_var > 0) {
/* Going forward. */
if(robot.beacon.beacon[i].direction > -30 && robot.beacon.beacon[i].direction < 30) {
trajectory_stop(&robot.traj);
while(robot.distance_qr.previous_var > 0);
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
return END_OBSTACLE;
}
} else if(robot.distance_qr.previous_var < 0) {
/* Going backward */
if(robot.beacon.beacon[i].direction < -30 || robot.beacon.beacon[i].direction > 30) {
trajectory_stop(&robot.traj);
while(robot.distance_qr.previous_var < 0);
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
return END_OBSTACLE;
}
}
}
}
}
if((why & END_BLOCKING) && bd_get(&robot.distance_bd)) {
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
return END_BLOCKING;
}
if((why & END_BLOCKING) && bd_get(&robot.angle_bd)) {
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
return END_BLOCKING;
}
if((why & END_TIMER) && strat_get_time() >= MATCH_TIME) {
trajectory_hardstop(&robot.traj);
return END_TIMER;
}
return 0;
}
