static carmen_vector_3D_t
get_velodyne_point_car_reference(double rot_angle, double vert_angle, double range, carmen_pose_3D_t velodyne_pose, carmen_pose_3D_t sensor_board_pose, rotation_matrix* velodyne_to_board_matrix, rotation_matrix* board_to_car_matrix)
{
double cos_rot_angle = cos(rot_angle);
double sin_rot_angle = sin(rot_angle);
double cos_vert_angle = cos(vert_angle);
double sin_vert_angle = sin(vert_angle);
double xy_distance = range * cos_vert_angle;
carmen_vector_3D_t velodyne_reference;
velodyne_reference.x = (xy_distance * cos_rot_angle);
velodyne_reference.y = (xy_distance * sin_rot_angle);
velodyne_reference.z = (range * sin_vert_angle);
carmen_vector_3D_t board_reference = multiply_matrix_vector(velodyne_to_board_matrix, velodyne_reference);
board_reference = add_vectors(board_reference, velodyne_pose.position);
carmen_vector_3D_t car_reference = multiply_matrix_vector(board_to_car_matrix, board_reference);
car_reference = add_vectors(car_reference, sensor_board_pose.position);
return car_reference;
}
/* Checks state safety. Returns true if safe, else false. */
bool is_safe() {
/* Work vector, length m. */
int work[RESOURCES];
/* Finish vector, length n. */
bool finish[CUSTOMERS];
/* Initialize work vector = available vector. */
copy_array(available, work);
/* Set all elements in finish vector to false. */
set_all_false(finish);
/* Find index i such that finish[i] == false && need[i] <= work. */
int i = find_i(work, finish);
/* If no such i exists, check if all finish elements are true. */
if (i == -1) { //such I does not exist
if (all_true(finish) == true) {
/* The system is in a safe state! */
return true;
}
}
else {
/* work = work + allocation */
add_vectors(work, allocation[i]);
finish[i] = true;
/* Go to step 2. */
i = find_i(work, finish);
}
}
static carmen_vector_3D_t
get_xsens_position_global_reference (carmen_pose_3D_t xsens_pose, carmen_pose_3D_t sensor_board_pose, carmen_pose_3D_t car_pose)
{
rotation_matrix* board_to_car_matrix = create_rotation_matrix (sensor_board_pose.orientation);
rotation_matrix* car_to_global_matrix = create_rotation_matrix (car_pose.orientation);
carmen_vector_3D_t car_reference = multiply_matrix_vector (board_to_car_matrix, xsens_pose.position);
car_reference = add_vectors (car_reference, sensor_board_pose.position);
carmen_vector_3D_t global_reference = multiply_matrix_vector (car_to_global_matrix, car_reference);
global_reference = add_vectors (global_reference, car_pose.position);
destroy_rotation_matrix (board_to_car_matrix);
destroy_rotation_matrix (car_to_global_matrix);
return global_reference;
}
//------------------------------------------------------------------------------------------------------------------------------
double error(level_type * level, int id_a, int id_b) {
double h3 = level->h * level->h * level->h;
add_vectors(level,VECTOR_TEMP,1.0,id_a,-1.0,id_b); // VECTOR_TEMP = id_a - id_b
double max = norm(level,VECTOR_TEMP);
return(max); // max norm of error function
double L2 = sqrt( dot(level,VECTOR_TEMP,VECTOR_TEMP)*h3);
return( L2); // normalized L2 error ?
}
carmen_vector_3D_t
carmen_change_sensor_reference(carmen_vector_3D_t position, carmen_vector_3D_t reference, rotation_matrix* transformation_matrix)
{
carmen_vector_3D_t new_reference = multiply_matrix_vector(transformation_matrix, reference);
carmen_vector_3D_t point = add_vectors(new_reference, position);
return point;
}
static expr *rexp3(int critical)
{
expr *e, *f;
int64_t v;
e = expr0(critical);
if (!e)
return NULL;
while (i == TOKEN_EQ || i == TOKEN_LT || i == TOKEN_GT ||
i == TOKEN_NE || i == TOKEN_LE || i == TOKEN_GE) {
int j = i;
i = scan(scpriv, tokval);
f = expr0(critical);
if (!f)
return NULL;
e = add_vectors(e, scalar_mult(f, -1L, false));
switch (j) {
case TOKEN_EQ:
case TOKEN_NE:
if (is_unknown(e))
v = -1; /* means unknown */
else if (!is_really_simple(e) || reloc_value(e) != 0)
v = (j == TOKEN_NE); /* unequal, so return true if NE */
else
v = (j == TOKEN_EQ); /* equal, so return true if EQ */
break;
default:
if (is_unknown(e))
v = -1; /* means unknown */
else if (!is_really_simple(e)) {
nasm_error(ERR_NONFATAL,
"`%s': operands differ by a non-scalar",
(j == TOKEN_LE ? "<=" : j == TOKEN_LT ? "<" : j ==
TOKEN_GE ? ">=" : ">"));
v = 0; /* must set it to _something_ */
} else {
int64_t vv = reloc_value(e);
if (vv == 0)
v = (j == TOKEN_LE || j == TOKEN_GE);
else if (vv > 0)
v = (j == TOKEN_GE || j == TOKEN_GT);
else /* vv < 0 */
v = (j == TOKEN_LE || j == TOKEN_LT);
}
break;
}
if (v == -1)
e = unknown_expr();
else
e = scalarvect(v);
}
return e;
}
void calculate_intensity(struct vector *c,
struct constants *k,
color ambient,
struct light *light,
struct vector *p,
struct vector *n,
struct vector *v) {
/* I = Ke + Ia Ka + Ip Kd (->L * ->N) + Ip Ks [(2->N(->L * ->N)- ->L) * ->V] */
double dotd, dots;
struct vector l, tmp;
l = *p;
subtract_vectors(&l, &light->l);
normalize(&l);
dotd = dot(&l, n);
if (dotd < 0) {
dotd = dots = 0;
goto add_terms;
}
tmp = *n;
scalar_mult(2 * dotd, &tmp);
subtract_vectors(&tmp, &l);
dots = dot(&tmp, v);
if (dots < 0)
dots = 0;
add_terms:
ctov(c, k->e);
ctov(&tmp, ambient);
tmp.x *= k->r[Ka];
tmp.y *= k->g[Ka];
tmp.z *= k->b[Ka];
add_vectors(c, &tmp);
ctov(&tmp, light->c);
tmp.x *= k->r[Kd] * dotd + k->r[Ks] * dots;
tmp.y *= k->g[Kd] * dotd + k->g[Kd] * dots;
tmp.z *= k->b[Kd] * dotd + k->b[Ks] * dots;
add_vectors(c, &tmp);
}
void
move_camera (carmen_vector_3D_t displacement)
{
rotation_matrix* r_matrix = create_rotation_matrix (camera_pose.orientation);
carmen_vector_3D_t displacement_world_coordinates = multiply_matrix_vector (r_matrix, displacement);
camera_pose.position = add_vectors (camera_pose.position, displacement_world_coordinates);
destroy_rotation_matrix (r_matrix);
}
static carmen_vector_3D_t
get_point_position_global_reference(carmen_vector_3D_t car_position, carmen_vector_3D_t car_reference, rotation_matrix* car_to_global_matrix)
{
//rotation_matrix* r_matrix = create_rotation_matrix(car_fused_pose.orientation);
carmen_vector_3D_t global_reference = multiply_matrix_vector(car_to_global_matrix, car_reference);
carmen_vector_3D_t point = add_vectors(global_reference, car_position);
//destroy_rotation_matrix(r_matrix);
return point;
}
static expr *expr4(int critical)
{
expr *e, *f;
e = expr5(critical);
if (!e)
return NULL;
while (i == '+' || i == '-') {
int j = i;
i = scan(scpriv, tokval);
f = expr5(critical);
if (!f)
return NULL;
switch (j) {
case '+':
e = add_vectors(e, f);
break;
case '-':
e = add_vectors(e, scalar_mult(f, -1L, false));
break;
}
}
return e;
}
//------------------------------------------------------------------------------------------------------------------------------
void BiCGStab(level_type * level, int x_id, int R_id, double a, double b, double desired_reduction_in_norm){
// Algorithm 7.7 in Iterative Methods for Sparse Linear Systems(Yousef Saad)
// uses "right" preconditioning... AD^{-1}(Dx) = b ... AD^{-1}y = b ... solve for y, then solve for x = D^{-1}y
int r0_id = VECTORS_RESERVED+0;
int r_id = VECTORS_RESERVED+1;
int p_id = VECTORS_RESERVED+2;
int s_id = VECTORS_RESERVED+3;
int Ap_id = VECTORS_RESERVED+4;
int As_id = VECTORS_RESERVED+5;
int jMax=200;
int j=0;
int BiCGStabFailed = 0;
int BiCGStabConverged = 0;
residual(level,r0_id,x_id,R_id,a,b); // r0[] = R_id[] - A(x_id)
scale_vector(level,r_id,1.0,r0_id); // r[] = r0[]
scale_vector(level,p_id,1.0,r0_id); // p[] = r0[]
double r_dot_r0 = dot(level,r_id,r0_id); // r_dot_r0 = dot(r,r0)
double norm_of_r0 = norm(level,r_id); // the norm of the initial residual...
if(r_dot_r0 == 0.0){BiCGStabConverged=1;} // entered BiCGStab with exact solution
if(norm_of_r0 == 0.0){BiCGStabConverged=1;} // entered BiCGStab with exact solution
while( (j<jMax) && (!BiCGStabFailed) && (!BiCGStabConverged) ){ // while(not done){
j++;level->Krylov_iterations++; //
#ifdef KRYLOV_DIAGONAL_PRECONDITION //
mul_vectors(level,VECTOR_TEMP,1.0,VECTOR_DINV,p_id); // temp[] = Dinv[]*p[]
apply_op(level,Ap_id,VECTOR_TEMP,a,b); // Ap = AD^{-1}(p)
#else //
apply_op(level,Ap_id,p_id,a,b); // Ap = A(p)
#endif //
double Ap_dot_r0 = dot(level,Ap_id,r0_id); // Ap_dot_r0 = dot(Ap,r0)
if(Ap_dot_r0 == 0.0){BiCGStabFailed=1;break;} // pivot breakdown ???
double alpha = r_dot_r0 / Ap_dot_r0; // alpha = r_dot_r0 / Ap_dot_r0
if(isinf(alpha)){BiCGStabFailed=2;break;} // pivot breakdown ???
add_vectors(level,x_id,1.0,x_id, alpha, p_id); // x_id[] = x_id[] + alpha*p[]
add_vectors(level,s_id,1.0,r_id,-alpha,Ap_id); // s[] = r[] - alpha*Ap[] (intermediate residual?)
double norm_of_s = norm(level,s_id); // FIX - redundant?? norm of intermediate residual
//if(level->my_rank==0)printf("norm(s)/norm(r0) = %e\n",norm_of_s/norm_of_r0);
if(norm_of_s == 0.0){BiCGStabConverged=1;break;} // FIX - redundant?? if As_dot_As==0, then As must be 0 which implies s==0
if(norm_of_s < desired_reduction_in_norm*norm_of_r0){BiCGStabConverged=1;break;}
#ifdef KRYLOV_DIAGONAL_PRECONDITION //
mul_vectors(level,VECTOR_TEMP,1.0,VECTOR_DINV,s_id); // temp[] = Dinv[]*s[]
apply_op(level,As_id,VECTOR_TEMP,a,b); // As = AD^{-1}(s)
#else //
apply_op(level,As_id,s_id,a,b); // As = A(s)
#endif //
double As_dot_As = dot(level,As_id,As_id); // As_dot_As = dot(As,As)
double As_dot_s = dot(level,As_id, s_id); // As_dot_s = dot(As, s)
if(As_dot_As == 0.0){BiCGStabConverged=1;break;} // converged ?
double omega = As_dot_s / As_dot_As; // omega = As_dot_s / As_dot_As
if(omega == 0.0){BiCGStabFailed=3;break;} // stabilization breakdown ???
if(isinf(omega)){BiCGStabFailed=4;break;} // stabilization breakdown ???
add_vectors(level,x_id,1.0,x_id, omega, s_id); // x_id[] = x_id[] + omega*s[]
add_vectors(level,r_id,1.0,s_id,-omega,As_id); // r[] = s[] - omega*As[] (recursively computed / updated residual)
double norm_of_r = norm(level,r_id); // norm of recursively computed residual (good enough??)
//if(level->my_rank==0)printf("norm(r)/norm(r0) = %e\n",norm_of_r/norm_of_r0);
if(norm_of_r == 0.0){BiCGStabConverged=1;break;} //
if(norm_of_r < desired_reduction_in_norm*norm_of_r0){BiCGStabConverged=1;break;}
#ifdef __DEBUG //
residual(level,VECTOR_TEMP,x_id,R_id,a,b); //
double norm_of_residual = norm(level,VECTOR_TEMP); //
if(level->my_rank==0)fprintf(stdout,"j=%8d, norm=%12.6e, norm_inital=%12.6e, reduction=%e\n",j,norm_of_residual,norm_of_r0,norm_of_residual/norm_of_r0); //
#endif //
double r_dot_r0_new = dot(level,r_id,r0_id); // r_dot_r0_new = dot(r,r0)
if(r_dot_r0_new == 0.0){BiCGStabFailed=5;break;} // Lanczos breakdown ???
double beta = (r_dot_r0_new/r_dot_r0) * (alpha/omega); // beta = (r_dot_r0_new/r_dot_r0) * (alpha/omega)
if(isinf(beta)){BiCGStabFailed=6;break;} // ???
add_vectors(level,VECTOR_TEMP,1.0,p_id,-omega, Ap_id); // VECTOR_TEMP = (p[]-omega*Ap[])
add_vectors(level, p_id,1.0,r_id, beta,VECTOR_TEMP); // p[] = r[] + beta*(p[]-omega*Ap[])
r_dot_r0 = r_dot_r0_new; // r_dot_r0 = r_dot_r0_new (save old r_dot_r0)
} // }
#ifdef KRYLOV_DIAGONAL_PRECONDITION //
mul_vectors(level,x_id,1.0,VECTOR_DINV,x_id); // x_id[] = Dinv[]*x_id[] // i.e. x = D^{-1}x'
#endif //
#ifdef __DEBUG
if(BiCGStabFailed)if(level->my_rank==0)fprintf(stderr,"BiCGStab Failed... error = %d\n",BiCGStabFailed);
#endif
}
void calc_normals(const char *course)
{
scalar_t *elevation;
scalar_t courseWidth, courseLength;
int nx, ny;
int x,y;
point_t p0, p1, p2;
vector_t n, nml, v1, v2;
char buff[BUFF_LEN];
sprintf( buff, "%s/courses/%s/normal.data", getparam_data_dir(), course );
get_course_dimensions( &courseWidth, &courseLength );
get_course_divisions( &nx, &ny );
if(nmls != (void*)-1 && nmls_fd != -1)
{
munmap(nmls, nmls_len);
close(nmls_fd);
}
#if 0
else {
free(nmls);
}
#endif
struct stat buf;
int exists = (stat(buff, &buf) == 0);
if(exists) {
nmls_fd = open(buff, O_RDONLY);
if ( nmls_fd == -1) {
handle_system_error( 1, "can't open file failed" );
}
TRDebugLog("mapping to memory normal.data\n");
nmls_len = sizeof(vector_t)*nx*ny;
nmls = mmap(NULL, nmls_len, PROT_READ, MAP_SHARED,nmls_fd, 0);
if ( nmls == (void *)-1 ) {
handle_system_error( 1, "read mmap failed" );
}
}
else {
nmls_len = sizeof(vector_t)*nx*ny;
#if TARGET_IPHONE_SIMULATOR
nmls_fd = open(buff, O_RDWR | O_CREAT | O_TRUNC, 0644);
if ( nmls_fd == -1) {
handle_system_error( 1, "can't open file failed" );
}
int result = lseek(nmls_fd, nmls_len-1, SEEK_SET);
if (result == -1) {
handle_system_error( 1, "can't write file failed" );
}
result = write(nmls_fd, "", 1);
if (result != 1) {
handle_system_error( 1, "can't write file failed" );
}
nmls = mmap(NULL, nmls_len, PROT_READ | PROT_WRITE, MAP_SHARED, nmls_fd, 0);
if ( nmls == (void *)-1 ) {
handle_system_error( 1, "write mmap failed" );
}
TRDebugLog("Writing to normal.data\n");
#else
# ifdef TR_DEBUG_MODE
abort(); // This shouldn't be reached on simulator. Crash to indicate.
# endif
nmls = malloc(nmls_len);
#endif
elevation = get_course_elev_data();
for ( y=0; y<ny; y++) {
for ( x=0; x<nx; x++) {
nml = make_vector( 0., 0., 0. );
p0 = make_point( XCD(x), ELEV(x,y), ZCD(y) );
/* The terrain is meshed as follows:
...
+-+-+-+-+ x<---+
|\|/|\|/| |
...+-+-+-+-+... V
|/|\|/|\| y
+-+-+-+-+
...
So there are two types of vertices: those surrounded by
four triangles (x+y is odd), and those surrounded by
eight (x+y is even).
*/
#define POINT(x,y) make_point( XCD(x), ELEV(x,y), ZCD(y) )
if ( (x + y) % 2 == 0 ) {
if ( x > 0 && y > 0 ) {
p1 = POINT(x, y-1);
p2 = POINT(x-1,y-1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
p1 = POINT(x-1,y-1);
p2 = POINT(x-1,y );
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x > 0 && y < ny-1 ) {
p1 = POINT(x-1,y );
p2 = POINT(x-1,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
p1 = POINT(x-1,y+1);
p2 = POINT(x ,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x < nx-1 && y > 0 ) {
p1 = POINT(x+1,y );
p2 = POINT(x+1,y-1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
p1 = POINT(x+1,y-1);
p2 = POINT(x ,y-1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x < nx-1 && y < ny-1 ) {
p1 = POINT(x+1,y );
p2 = POINT(x+1,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v1, v2 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
p1 = POINT(x+1,y+1);
p2 = POINT(x ,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v1, v2 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
} else {
/* x + y is odd */
if ( x > 0 && y > 0 ) {
p1 = POINT(x, y-1);
p2 = POINT(x-1,y );
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x > 0 && y < ny-1 ) {
p1 = POINT(x-1,y );
p2 = POINT(x ,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x < nx-1 && y > 0 ) {
p1 = POINT(x+1,y );
p2 = POINT(x ,y-1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v2, v1 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
if ( x < nx-1 && y < ny-1 ) {
p1 = POINT(x+1,y );
p2 = POINT(x ,y+1);
v1 = subtract_points( p1, p0 );
v2 = subtract_points( p2, p0 );
n = cross_product( v1, v2 );
check_assertion( n.y > 0, "course normal points down" );
normalize_vector( &n );
nml = add_vectors( nml, n );
}
}
normalize_vector( &nml );
NORMAL(x,y) = nml;
continue;
}
#undef POINT
}
#if TARGET_IPHONE_SIMULATOR
munmap(nmls, nmls_len);
close(nmls_fd);
nmls_fd = open(buff, O_RDONLY);
if (nmls_fd == -1) {
handle_system_error( 1, "can't remount normal.data" );
}
TRDebugLog("remounting to memory normal.data\n");
nmls_len = sizeof(vector_t)*nx*ny;
nmls = mmap(NULL, nmls_len, PROT_READ, MAP_SHARED, nmls_fd, 0);
if ( nmls == (void *)-1 ) {
handle_system_error( 1, "remount mmap failed" );
}
#endif
}
}
expr *evaluate(scanner sc, void *scprivate, struct tokenval *tv,
int *fwref, int critical, struct eval_hints *hints)
{
expr *e;
expr *f = NULL;
hint = hints;
if (hint)
hint->type = EAH_NOHINT;
if (critical & CRITICAL) {
critical &= ~CRITICAL;
bexpr = rexp0;
} else
bexpr = expr0;
scan = sc;
scpriv = scprivate;
tokval = tv;
opflags = fwref;
if (tokval->t_type == TOKEN_INVALID)
i = scan(scpriv, tokval);
else
i = tokval->t_type;
while (ntempexprs) /* initialize temporary storage */
nasm_free(tempexprs[--ntempexprs]);
e = bexpr(critical);
if (!e)
return NULL;
if (i == TOKEN_WRT) {
i = scan(scpriv, tokval); /* eat the WRT */
f = expr6(critical);
if (!f)
return NULL;
}
e = scalar_mult(e, 1L, false); /* strip far-absolute segment part */
if (f) {
expr *g;
if (is_just_unknown(f))
g = unknown_expr();
else {
int64_t value;
begintemp();
if (!is_reloc(f)) {
nasm_error(ERR_NONFATAL, "invalid right-hand operand to WRT");
return NULL;
}
value = reloc_seg(f);
if (value == NO_SEG)
value = reloc_value(f) | SEG_ABS;
else if (!(value & SEG_ABS) && !(value % 2) && critical) {
nasm_error(ERR_NONFATAL, "invalid right-hand operand to WRT");
return NULL;
}
addtotemp(EXPR_WRT, value);
g = finishtemp();
}
e = add_vectors(e, g);
}
return e;
}
uint8_t quest(vector v_B_c, vector v_sun_c, vector v_B_m, float sun_adc[], quaternion q_triad, uint8_t * p_w_ctrl)
{
uint8_t w_ctrl = *p_w_ctrl;
static uint16_t time_since_light;
static uint8_t light_prev = 1;
uint8_t light = 1, num_dark_sensors = 0, i, j;
vector v_sun_m, v_cross_m, v_cross_c, v_mc_cross, v_mc_add;
vector v_temp1, v_temp2;
vector v_triad;
float mu, nu, rho, k, triad;
for(i = 0; i < N_SS; i++)
{
if(sun_adc[i] < (0.5 * SS_GAIN))
num_dark_sensors++;
}
if(num_dark_sensors == N_SS)
light = 0;
if(light)
{
if(!w_ctrl)
{
time_since_light += FRAME_TIME;
if(time_since_light == 300)
w_ctrl = 0;
}
if(light_prev == 0)
{
w_ctrl = 0;
time_since_light = 0;
}
for(i = 0; i < (N_SS / 2); i++)
{
j = i * 2;
if(sun_adc[j] > sun_adc[j + 1])
v_sun_m[i] = (float)sun_adc[j];
else
v_sun_m[i] = -1 * (float)sun_adc[j + 1];
}
convert_unit_vector(v_sun_m);
/*v_B_m[0] = Current_state.mm.B_x;
v_B_m[1] = Current_state.mm.B_y;
v_B_m[2] = Current_state.mm.B_z;*/
vector_cross_product(v_B_m, v_sun_m, v_cross_m);
convert_unit_vector(v_cross_m);
vector_cross_product(v_B_c, v_sun_c, v_cross_c);
convert_unit_vector(v_cross_c);
mu = (1 + vector_dot_product(v_cross_m, v_cross_c)) * (MAG_WEIGHT * vector_dot_product(v_B_m, v_B_c) + (1 - MAG_WEIGHT) * vector_dot_product(v_sun_m, v_sun_c));
vector_cross_product(v_B_m, v_B_c, v_temp1);
vector_cross_product(v_sun_m, v_sun_c, v_temp2);
for(i = 0; i < 3; i++)
v_temp2[i] = v_temp1[i] * MAG_WEIGHT + (1 - MAG_WEIGHT) * v_temp2[i];
vector_cross_product(v_cross_m, v_cross_c, v_mc_cross);
mu += vector_dot_product(v_mc_cross, v_temp2);
add_vectors(v_cross_m, v_cross_c, v_mc_add);
nu = vector_dot_product(v_mc_add, v_temp2);
rho = sqrt(mu * mu + nu * nu);
if(mu > 0)
{
k = 1 / (2 * sqrt(rho * (rho + mu) * (1 + vector_dot_product(v_cross_m, v_cross_c))));
for(i = 0; i < 3; i++)
v_triad[i] = v_mc_cross[i] * (rho + mu) + v_mc_add[i] * nu;
triad = (rho + mu) * (1 + vector_dot_product(v_cross_m, v_cross_c));
}
else
{
k = 1 / (2 * sqrt(rho * (rho - mu) * (1 + vector_dot_product(v_cross_m, v_cross_c))));
for(i = 0; i < 3; i++)
v_triad[i] = v_mc_cross[i] * nu + v_mc_add[i] * (rho - mu);
triad = nu * (1 + vector_dot_product(v_cross_m, v_cross_c));
}
for(i = 0; i < 3; i++)
q_triad[i] = v_triad[i];
q_triad[3] = triad;
scalar_into_quaternion(q_triad, k);
}
else
{
for(i = 0; i < 3; i++)
q_triad[i] = 0;
q_triad[3] = 1;
}
light_prev = light;
return light;
}
void horizontal_convert(polygon poly,
struct screen *s,
enum shading_t type,
struct constants *k,
color ambient,
struct light *l,
struct vector norm[3],
struct vector *v) {
const double
y0 = poly[0]->y,
y1 = poly[1]->y,
y2 = poly[2]->y;
int ib, im, it;
int dy_bt, dy_bm, dy_mt;
double dx;
struct vector pb, pm, pt;
struct vector dp_dy_bt, dp_dy_bm, dp_dy_mt;
struct vector p0, p1;
edge e = {&p0, &p1};
struct vector p;
double dz_dx;
struct vector cb, cm, ct;
struct vector dc_dy_bt, dc_dy_bm, dc_dy_mt;
struct vector c0, c1;
struct vector c;
struct vector dc_dx;
struct vector dn_dy_bt, dn_dy_bm, dn_dy_mt;
struct vector n0, n1;
struct vector n;
struct vector dn_dx;
if (y0 <= y1 && y1 <= y2) {
ib = 0;
im = 1;
it = 2;
}
else if (y0 <= y2 && y2 <= y1) {
ib = 0;
im = 2;
it = 1;
}
else if (y1 <= y0 && y0 <= y2) {
ib = 1;
im = 0;
it = 2;
}
else if (y1 <= y2 && y2 <= y0) {
ib = 1;
im = 2;
it = 0;
}
else if (y2 <= y0 && y0 <= y1) {
ib = 2;
im = 0;
it = 1;
}
else {
ib = 2;
im = 1;
it = 0;
}
pb = *poly[ib];
pm = *poly[im];
pt = *poly[it];
pb.y = (int)pb.y;
pm.y = (int)pm.y;
pt.y = (int)pt.y;
dy_bt = pt.y - pb.y;
dy_bm = pm.y - pb.y;
dy_mt = pt.y - pm.y;
p0 = pb;
p1 = pb.y != pm.y? pb : pm;
dp_dy_bt = pt;
subtract_vectors(&dp_dy_bt, &pb);
scalar_div(dy_bt, &dp_dy_bt);
dp_dy_bm = pm;
subtract_vectors(&dp_dy_bm, &pb);
scalar_div(dy_bm, &dp_dy_bm);
dp_dy_mt = pt;
subtract_vectors(&dp_dy_mt, &pm);
scalar_div(dy_mt, &dp_dy_mt);
switch (type) {
case FLAT:
calculate_centroid(&p, poly);
calculate_intensity(&c, k, ambient, l, &p, norm, v);
for (p.y = pb.y; p.y <= pt.y; p.y++) {
p0.y = p1.y = p.y;
draw_line(e, s, vtoc(&c) );
add_vectors(&p0, &dp_dy_bt);
add_vectors(&p1, (p.y < pm.y? &dp_dy_bm : &dp_dy_mt) );
}
break;
case GOROUD:
calculate_intensity(&cb, k, ambient, l, poly[ib], norm + ib, v);
calculate_intensity(&cm, k, ambient, l, poly[im], norm + im, v);
calculate_intensity(&ct, k, ambient, l, poly[it], norm + it, v);
c0 = cb;
c1 = (pb.y != pm.y? cb : cm);
dc_dy_bt = ct;
subtract_vectors(&dc_dy_bt, &cb);
scalar_div(dy_bt, &dc_dy_bt);
dc_dy_bm = cm;
subtract_vectors(&dc_dy_bm, &cb);
scalar_div(dy_bm, &dc_dy_bm);
dc_dy_mt = ct;
subtract_vectors(&dc_dy_mt, &cm);
scalar_div(dy_mt, &dc_dy_mt);
for (p.y = pb.y; p.y <= pt.y; p.y++) {
dx = p1.x - p0.x;
dc_dx = c1;
subtract_vectors(&dc_dx, &c0);
scalar_div(dx, &dc_dx);
dz_dx = (p1.z - p0.z) / dx;
p.x = p0.x;
p.z = p0.z;
c = c0;
while (1) {
plot(s, vtoc(&c), &p);
if (p0.x < p1.x) {
p.z += dz_dx;
add_vectors(&c, &dc_dx);
p.x++;
if (p.x > p1.x)
break;
}
else {
p.z -= dz_dx;
subtract_vectors(&c, &dc_dx);
p.x--;
if (p.x < p1.x)
break;
}
}
add_vectors(&p0, &dp_dy_bt);
add_vectors(&c0, &dc_dy_bt);
if (p.y < pm.y) {
add_vectors(&p1, &dp_dy_bm);
add_vectors(&c1, &dc_dy_bm);
}
else {
add_vectors(&p1, &dp_dy_mt);
add_vectors(&c1, &dc_dy_mt);
}
}
break;
case PHONG:
n0 = norm[ib];
n1 = norm[pb.y != pm.y? ib : im];
dn_dy_bt = norm[it];
subtract_vectors(&dn_dy_bt, norm + ib);
scalar_div(dy_bt, &dn_dy_bt);
dn_dy_bm = norm[im];
subtract_vectors(&dn_dy_bm, norm + ib);
scalar_div(dy_bm, &dn_dy_bm);
dn_dy_mt = norm[it];
subtract_vectors(&dn_dy_mt, norm + im);
scalar_div(dy_mt, &dn_dy_mt);
for (p.y = pb.y; p.y <= pt.y; p.y++) {
dx = p1.x - p0.x;
dn_dx = n1;
subtract_vectors(&dn_dx, &n0);
scalar_div(dx, &dn_dx);
dz_dx = (p1.z - p0.z) / dx;
p = p0;
n = n0;
while (1) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
if (p0.x < p1.x) {
p.z += dz_dx;
add_vectors(&n, &dn_dx);
p.x++;
if (p.x > p1.x)
break;
}
else {
p.z -= dz_dx;
subtract_vectors(&n, &dn_dx);
p.x--;
if (p.x < p1.x)
break;
}
}
add_vectors(&p0, &dp_dy_bt);
add_vectors(&n0, &dn_dy_bt);
if (p.y < pm.y) {
add_vectors(&p1, &dp_dy_bm);
add_vectors(&n1, &dn_dy_bm);
}
else {
add_vectors(&p1, &dp_dy_mt);
add_vectors(&n1, &dn_dy_mt);
}
}
break;
}
}
void draw_shaded_edge(int x,
polygon poly,
struct screen *s,
enum shading_t type,
struct constants *k,
color ambient,
struct light *l,
struct vector norm[3],
struct vector *v) {
int i, j, tmp;
struct vector p, *p1, dp;
int d;
double derz; /* derz: derivative of z */
struct vector n, dern;
struct vector c;
switch (x) {
case 0:
i = 0;
j = 1;
break;
case 1:
i = 1;
j = 2;
break;
case 2:
i = 2;
j = 0;
break;
}
//swap points so we're always draing left to right
if (poly[i]->x > poly[j]->x) {
tmp = i;
i = j;
j = tmp;
}
p = *poly[i];
p1 = poly[j];
dp = *p1;
subtract_vectors(&dp, &p);
//positive slope: Octants 1, 2 (5 and 6)
if (dp.y > 0 ) {
//slope < 1: Octant 1 (5)
if (dp.x > dp.y) {
d = 2 * dp.y - dp.x;
derz = dp.z / dp.x;
n = norm[i];
dern = norm[j];
subtract_vectors(&dern, &n);
scalar_div(dp.x, &dern);
for (; p.x <= p1->x; p.x++) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
p.z += derz;
add_vectors(&n, &dern);
if ( d < 0 )
d += 2 * dp.y;
else {
p.y++;
d += 2 * (dp.y - dp.x);
}
}
}
//slope > 1: Octant 2 (6)
else {
d = dp.y - 2 * dp.x;
derz = dp.z / dp.y;
n = norm[i];
dern = norm[j];
subtract_vectors(&dern, &n);
scalar_div(dp.y, &dern);
for (; p.y <= p1->y; p.y++) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
p.z += derz;
add_vectors(&n, &dern);
if ( d > 0 )
d -= 2 * dp.x;
else {
p.x++;
d += 2 * (dp.y - dp.x);
}
}
}
}
//negative slope: Octants 7, 8 (3 and 4)
//slope > -1: Octant 8 (4)
else {
if ( dp.x > fabs(dp.y) ) {
d = 2 * dp.y + dp.x;
derz = dp.z / dp.x;
n = norm[i];
dern = norm[j];
subtract_vectors(&dern, &n);
scalar_div(dp.x, &dern);
for (; p.x <= p1->x; p.x++) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
p.z += derz;
add_vectors(&n, &dern);
if ( d > 0 )
d += 2 * dp.y;
else {
p.y--;
d += 2 * (dp.y + dp.x);
}
}
}
//slope < -1: Octant 7 (3)
else {
d = dp.y + 2 * dp.x;
derz = dp.z / dp.y;
n = norm[i];
dern = norm[j];
subtract_vectors(&dern, &n);
scalar_div(dp.y, &dern);
for (; p.y >= p1->y; p.y--) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
p.z -= derz;
subtract_vectors(&n, &dern);
if ( d < 0 )
d += 2 * dp.x;
else {
p.x++;
d += 2 * (dp.y + dp.x);
}
}
}
}
}
void
draw_ldmrs_objects(carmen_laser_ldmrs_object *ldmrs_objects_tracking, int num_ldmrs_objects, double min_velocity)
{
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < num_ldmrs_objects; i++)
{
if (ldmrs_objects_tracking[i].velocity < min_velocity)
continue;
carmen_pose_3D_t pos;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = ldmrs_objects_tracking[i].x;
pos.position.y = ldmrs_objects_tracking[i].y;
pos.position.z = 0.0;
pos.orientation.yaw = ldmrs_objects_tracking[i].orientation;
pos.orientation.roll = 0.0;
pos.orientation.pitch = 0.0;
W = ldmrs_objects_tracking[i].width;
L = ldmrs_objects_tracking[i].lenght;
H = 1.0;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(0.0,1.0,1.0);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
// char class_name[50];
//
// switch (ldmrs_objects_tracking[i].classId)
// {
// case 0:
// strcpy(class_name,"Unclassified");
// break;
// case 1:
// strcpy(class_name,"Small");
// break;
// case 2:
// strcpy(class_name,"Big");
// break;
// case 3:
// strcpy(class_name,"Pedestrian");
// break;
// case 4:
// strcpy(class_name,"Bike");
// break;
// case 5:
// strcpy(class_name,"Car");
// break;
// case 6:
// strcpy(class_name,"Truck");
// break;
// default:
// strcpy(class_name,"Unknown");
// break;
// }
/* has to drawn after stuff above, so that it appears on top */
//draw_number_associated(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].id,"");
draw_linear_velocity(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].velocity,
1.0);
destroy_rotation_matrix(rotate);
}
}
void vertical_convert(polygon poly,
struct screen *s,
enum shading_t type,
struct constants *k,
color ambient,
struct light *l,
struct vector norm[3],
struct vector *v) {
const double
x0 = poly[0]->x,
x1 = poly[1]->x,
x2 = poly[2]->x;
int il, im, ir;
int dx_lr, dx_lm, dx_mr;
double dy;
struct vector pl, pm, pr;
struct vector dp_dx_lr, dp_dx_lm, dp_dx_mr;
struct vector p0, p1;
edge e = {&p0, &p1};
struct vector p;
double dz_dy;
struct vector cl, cm, cr;
struct vector dc_dx_lr, dc_dx_lm, dc_dx_mr;
struct vector c0, c1;
struct vector c;
struct vector dc_dy;
struct vector dn_dx_lr, dn_dx_lm, dn_dx_mr;
struct vector n0, n1;
struct vector n;
struct vector dn_dy;
if (x0 <= x1 && x1 <= x2) {
il = 0;
im = 1;
ir = 2;
}
else if (x0 <= x2 && x2 <= x1) {
il = 0;
im = 2;
ir = 1;
}
else if (x1 <= x0 && x0 <= x2) {
il = 1;
im = 0;
ir = 2;
}
else if (x1 <= x2 && x2 <= x0) {
il = 1;
im = 2;
ir = 0;
}
else if (x2 <= x0 && x0 <= x1) {
il = 2;
im = 0;
ir = 1;
}
else {
il = 2;
im = 1;
ir = 0;
}
pl = *poly[il];
pm = *poly[im];
pr = *poly[ir];
pl.x = (int)pl.x;
pm.x = (int)pm.x;
pr.x = (int)pr.x;
dx_lr = pr.x - pl.x;
dx_lm = pm.x - pl.x;
dx_mr = pr.x - pm.x;
p0 = pl;
p1 = pl.x != pm.x? pl : pm;
dp_dx_lr = pr;
subtract_vectors(&dp_dx_lr, &pl);
scalar_div(dx_lr, &dp_dx_lr);
dp_dx_lm = pm;
subtract_vectors(&dp_dx_lm, &pl);
scalar_div(dx_lm, &dp_dx_lm);
dp_dx_mr = pr;
subtract_vectors(&dp_dx_mr, &pm);
scalar_div(dx_mr, &dp_dx_mr);
switch (type) {
case FLAT:
calculate_centroid(&p, poly);
calculate_intensity(&c, k, ambient, l, &p, norm, v);
for (p.x = pl.x; p.x <= pr.x; p.x++) {
p0.x = p1.x = p.x;
draw_line(e, s, vtoc(&c) );
add_vectors(&p0, &dp_dx_lr);
add_vectors(&p1, (p.x < pm.x? &dp_dx_lm : &dp_dx_mr) );
}
break;
case GOROUD:
calculate_intensity(&cl, k, ambient, l, poly[il], norm + il, v);
calculate_intensity(&cm, k, ambient, l, poly[im], norm + im, v);
calculate_intensity(&cr, k, ambient, l, poly[ir], norm + ir, v);
c0 = cl;
c1 = (pl.x != pm.x? cl : cm);
dc_dx_lr = cr;
subtract_vectors(&dc_dx_lr, &cl);
scalar_div(dx_lr, &dc_dx_lr);
dc_dx_lm = cm;
subtract_vectors(&dc_dx_lm, &cl);
scalar_div(dx_lm, &dc_dx_lm);
dc_dx_mr = cr;
subtract_vectors(&dc_dx_mr, &cm);
scalar_div(dx_mr, &dc_dx_mr);
for (p.x = pl.x; p.x <= pr.x; p.x++) {
dy = p1.y - p0.y;
dc_dy = c1;
subtract_vectors(&dc_dy, &c0);
scalar_div(dy, &dc_dy);
dz_dy = (p1.z - p0.z) / dy;
p.y = p0.y;
p.z = p0.z;
c = c0;
while (1) {
plot(s, vtoc(&c), &p);
if (p0.y < p1.y) {
p.z += dz_dy;
add_vectors(&c, &dc_dy);
p.y++;
if (p.y > p1.y)
break;
}
else {
p.z -= dz_dy;
subtract_vectors(&c, &dc_dy);
p.y--;
if (p.y < p1.y)
break;
}
}
add_vectors(&p0, &dp_dx_lr);
add_vectors(&c0, &dc_dx_lr);
if (p.x < pm.x) {
add_vectors(&p1, &dp_dx_lm);
add_vectors(&c1, &dc_dx_lm);
}
else {
add_vectors(&p1, &dp_dx_mr);
add_vectors(&c1, &dc_dx_mr);
}
}
break;
case PHONG:
n0 = norm[il];
n1 = norm[pl.x != pm.x? il : im];
dn_dx_lr = norm[ir];
subtract_vectors(&dn_dx_lr, norm + il);
scalar_div(dx_lr, &dn_dx_lr);
dn_dx_lm = norm[im];
subtract_vectors(&dn_dx_lm, norm + il);
scalar_div(dx_lm, &dn_dx_lm);
dn_dx_mr = norm[ir];
subtract_vectors(&dn_dx_mr, norm + im);
scalar_div(dx_mr, &dn_dx_mr);
for (p.x = pl.x; p.x <= pr.x; p.x++) {
dy = p1.y - p0.y;
dn_dy = n1;
subtract_vectors(&dn_dy, &n0);
scalar_div(dy, &dn_dy);
dz_dy = (p1.z - p0.z) / dy;
p = p0;
n = n0;
while (1) {
calculate_intensity(&c, k, ambient, l, &p, &n, v);
plot(s, vtoc(&c), &p);
if (p0.y < p1.y) {
p.z += dz_dy;
add_vectors(&n, &dn_dy);
p.y++;
if (p.y > p1.y)
break;
}
else {
p.z -= dz_dy;
subtract_vectors(&n, &dn_dy);
p.y--;
if (p.y < p1.y)
break;
}
}
add_vectors(&p0, &dp_dx_lr);
add_vectors(&n0, &dn_dx_lr);
if (p.x < pm.x) {
add_vectors(&p1, &dp_dx_lm);
add_vectors(&n1, &dn_dx_lm);
}
else {
add_vectors(&p1, &dp_dx_mr);
add_vectors(&n1, &dn_dx_mr);
}
}
break;
}
}
void
draw_tracking_moving_objects(moving_objects_tracking_t *moving_objects_tracking, int current_num_point_clouds,
carmen_vector_3D_t offset, int draw_particles_flag)
{
/*** MOVING OBJECTS MODULE ***/
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < current_num_point_clouds; i++)
{
if (moving_objects_tracking[i].geometric_model == -1)
continue;
carmen_pose_3D_t pos;
pos = moving_objects_tracking[i].moving_objects_pose;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = pos.position.x - offset.x;
pos.position.y = pos.position.y - offset.y;
pos.position.z = 0.0;
W = moving_objects_tracking[i].model_features.geometry.width;
L = moving_objects_tracking[i].model_features.geometry.length;
H = moving_objects_tracking[i].model_features.geometry.height;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(moving_objects_tracking[i].model_features.red,
moving_objects_tracking[i].model_features.green,
moving_objects_tracking[i].model_features.blue);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
/* has to drawn after stuff above, so that it appears on top */
draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
moving_objects_tracking[i].model_features.model_name);
draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].linear_velocity,
moving_objects_tracking[i].model_features.geometry.height);
destroy_rotation_matrix(rotate);
if (draw_particles_flag == 1) {
// //todo para visualizar as particulas apenas
// for(j = 0; j < 10; j++){
// carmen_pose_3D_t pos;
// pos = moving_objects_tracking[i].moving_objects_pose;
// carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
// carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
// double correction_wheel_height = 0.28;
// double W, L, H;
//
// pos.position.x = moving_objects_tracking[i].particulas[j].pose.x - offset.x;
// pos.position.y = moving_objects_tracking[i].particulas[j].pose.y - offset.y;
// pos.position.z = 0.0;
// pos.orientation.yaw = moving_objects_tracking[i].particulas[j].pose.theta;
//
// W = moving_objects_tracking[i].particulas[j].geometry.width;
// L = moving_objects_tracking[i].particulas[j].geometry.length;
// H = moving_objects_tracking[i].particulas[j].geometry.height;
//
// // W = moving_objects_tracking[i].width;
// // L = moving_objects_tracking[i].length;
// // H = moving_objects_tracking[i].height;
//
// rotate = compute_rotation_matrix(NULL, pos.orientation);
//
// glColor3d(moving_objects_tracking[i].model_features.red -0.5,
// moving_objects_tracking[i].model_features.green -0.5,
// moving_objects_tracking[i].model_features.blue -0.5);
//
// p1.x = - L/2.0;
// p1.y = - W/2.0;
// p1.z = 0.0 - correction_wheel_height;
//
// p2.x = L/2.0;
// p2.y = - W/2.0;
// p2.z = 0.0 - correction_wheel_height;
//
// p3.x = L/2.0;
// p3.y = W/2.0;
// p3.z = 0.0 - correction_wheel_height;
//
// p4.x = - L/2.0;
// p4.y = W/2.0;
// p4.z = 0.0 - correction_wheel_height;
//
// p5.x = - L/2.0;
// p5.y = - W/2.0;
// p5.z = H - correction_wheel_height;
//
// p6.x = L/2.0;
// p6.y = - W/2.0;
// p6.z = H - correction_wheel_height;
//
// p7.x = L/2.0;
// p7.y = W/2.0;
// p7.z = H - correction_wheel_height;
//
// p8.x = - L/2.0;
// p8.y = W/2.0;
// p8.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, p1);
// p1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p2);
// p2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p3);
// p3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p4);
// p4 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p5);
// p5 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p6);
// p6 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p7);
// p7 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p8);
// p8 = add_vectors(t, pos.position);
//
// glBegin (GL_LINES);
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p2.x, p2.y, p2.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p3.x, p3.y, p3.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p4.x, p4.y, p4.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p1.x, p1.y, p1.z);
// //////////////////////////////
//
// glVertex3d (p5.x, p5.y, p5.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p6.x, p6.y, p6.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p7.x, p7.y, p7.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glVertex3d (p8.x, p8.y, p8.z);
// glVertex3d (p5.x, p5.y, p5.z);
// //////////////////////////////
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p5.x, p5.y, p5.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glEnd ();
//
// /* Moving Object direction arrow */
// s1.x = 0.0;
// s1.y = 0.0;
// s1.z = H - correction_wheel_height;
//
// s2.x = L/2.0;
// s2.y = 0.0;
// s2.z = H - correction_wheel_height;
//
// s3.x = (L/2.0) - 0.3;
// s3.y = 0.2;
// s3.z = H - correction_wheel_height;
//
// s4.x = (L/2.0) - 0.3;
// s4.y = -0.2;
// s4.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, s1);
// s1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s2);
// s2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s3);
// s3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s4);
// s4 = add_vectors(t, pos.position);
//
// glBegin(GL_LINES);
// /* Part of arrow: | */
// glVertex3d(s1.x, s1.y, s1.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: / */
// glVertex3d(s3.x, s3.y, s3.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: \ */
// glVertex3d(s4.x, s4.y, s4.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// glEnd();
//
// //center of object
// glPushAttrib(GL_POINT_BIT);
// glPointSize(5);
// glBegin(GL_POINTS);
// glVertex3d(s1.x, s1.y, s1.z);
// glEnd();
// glPopAttrib();
//
// /* has to drawn after stuff above, so that it appears on top */
// // draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
// // moving_objects_tracking[i].model_features.model_name);
//// draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].particulas[j].velocity,
//// moving_objects_tracking[i].particulas[j].geometry.height);
//
// destroy_rotation_matrix(rotate);
// }
}
}
}