void
rotate_camera (carmen_orientation_3D_t rotation)
{
rotation_matrix* cam_matrix = create_rotation_matrix (camera_pose.orientation);
rotation_matrix* r_matrix = create_rotation_matrix (rotation);
rotation_matrix* new_cam_matrix = multiply_matrix_matrix (r_matrix, cam_matrix);
camera_pose.orientation = get_angles_from_rotation_matrix (new_cam_matrix);
destroy_rotation_matrix (cam_matrix);
destroy_rotation_matrix (r_matrix);
destroy_rotation_matrix (new_cam_matrix);
}
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;
}
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);
}
void add_velodyne_message(velodyne_360_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
switch_current_last(v_drawer);
int num_points = velodyne_message->number_of_32_laser_shots*32;
v_drawer->num_points_current = num_points;
v_drawer->points_current = realloc(v_drawer->points_current, num_points*sizeof(carmen_vector_3D_t));
v_drawer->angles_current = realloc(v_drawer->angles_current, num_points*sizeof(double));
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double rot_angle = carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
int j;
for(j = 0; j < 32; j++)
{
double vert_angle = carmen_degrees_to_radians(vertical_correction[j]);
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( -rot_angle, vert_angle,
velodyne_message->partial_scan[i].distance[j]/500.0,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose,
velodyne_to_board_matrix, board_to_car_matrix);
v_drawer->points_current[i*32 + j] = point_position;
v_drawer->angles_current[i*32 + j] = rot_angle;
}
}
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
}
void
rotate_coords_about_axis_dp_list(coord_3D **coords, int num_coords, coord_3D point,
vectormag_3D direction, double theta)
{
int i;
double *matrix;
matrix = create_rotation_matrix(point, direction, theta);
for(i = 0; i < num_coords; i++)
apply_rotation_matrix_to_coord_p(coords[i], matrix);
destroy_rotation_matrix(matrix);
}
void
velodyne_intensity_drawer_add_velodyne_message(velodyne_intensity_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message, carmen_pose_3D_t car_fused_pose, carmen_vector_3D_t car_fused_velocity, double car_fused_time)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
static point_cloud velodyne_points = {NULL, NULL, {0.0, 0.0, 0.0}, 0, 0.0};
static double last_timestamp = 0.0;
if (last_timestamp == 0.0)
{
last_timestamp = velodyne_message->timestamp;
return;
}
int num_points = velodyne_message->number_of_32_laser_shots*(32);
if(num_points > velodyne_points.num_points)
{
velodyne_points.points = realloc(velodyne_points.points, num_points*sizeof(carmen_vector_3D_t));
velodyne_points.point_color = realloc(velodyne_points.point_color, num_points*sizeof(carmen_vector_3D_t));
}
velodyne_points.num_points = num_points;
velodyne_points.car_position = car_fused_pose.position;
velodyne_points.timestamp = velodyne_message->timestamp;
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
rotation_matrix* r_matrix_car_to_global = create_rotation_matrix(car_fused_pose.orientation);
int k = 0;
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double shot_angle = -carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
if(check_angle_in_range(shot_angle, v_drawer->horizontal_angle, v_drawer->horizontal_range))
{
int j;
for(j = 0; j < 32; j++)
{
if( j == v_drawer->vertical_position )
{
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( shot_angle,
carmen_degrees_to_radians(vertical_correction[j]),
velodyne_message->partial_scan[i].distance[j]/500.0,
velodyne_to_board_matrix, board_to_car_matrix,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose);
double shot_time = last_timestamp + ((velodyne_message->timestamp - last_timestamp)*((double)i)/((double)velodyne_message->number_of_32_laser_shots));
carmen_vector_3D_t car_interpolated_position = carmen_get_interpolated_robot_position_at_time(car_fused_pose, car_fused_velocity, car_fused_time, shot_time, r_matrix_car_to_global);
carmen_vector_3D_t point_global_position = get_point_position_global_reference(car_interpolated_position, point_position, r_matrix_car_to_global);
velodyne_points.points[k] = point_global_position;
velodyne_points.point_color[k] = create_point_colors_intensity(velodyne_message->partial_scan[i].intensity[j]);
k++;
}
}
}
}
velodyne_points.num_points = k;
normalize_intensity(velodyne_points);
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
destroy_rotation_matrix(r_matrix_car_to_global);
add_point_cloud(v_drawer->pcloud_drawer, velodyne_points);
last_timestamp = velodyne_message->timestamp;
}
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);
// }
}
}
}
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);
}
}
