void add_circle_to_buffer(vx_buffer_t* buf, double size, loc_t loc, const float* color) { vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, vxo_chain(vxo_mat_translate3(loc.x, loc.y, 0), vxo_mat_scale(size), vxo_circle(vxo_mesh_style(color))))); }
void project_measurements_through_homography(matd_t* H, vx_buffer_t* buf, zarray_t* pix_found, int size) { int npoints = NUM_CHART_BLOBS * 2; // line per chart blob float points[npoints*3]; float* real_world_coords; if(size == NUM_TARGETS) real_world_coords = target_coords; else if(size == NUM_CHART_BLOBS) real_world_coords = chart_coords; else assert(0); for(int i = 0; i < size; i++) { // run each real world point through homography and add to buf double tmp[3] = {real_world_coords[i*2], real_world_coords[i*2+1], 1}; matd_t* xy_matrix = matd_create_data(3,1,tmp); matd_t* pix_estimated = matd_op("(M)*M",H, xy_matrix); MATD_EL(pix_estimated,0,0) /= MATD_EL(pix_estimated,2, 0); MATD_EL(pix_estimated,1,0) /= MATD_EL(pix_estimated,2, 0); vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, vxo_chain(vxo_mat_translate3(MATD_EL(pix_estimated,0,0), MATD_EL(pix_estimated,1,0), 0), vxo_mat_scale(2.0), vxo_circle(vxo_mesh_style(vx_green))))); // create endpoints for lines loc_t pos; zarray_get(pix_found, i, &pos); // points[6*i + 0] = pos.x; points[6*i + 1] = pos.y; points[6*i + 2] = 0; points[6*i + 3] = MATD_EL(pix_estimated,0,0); points[6*i + 4] = MATD_EL(pix_estimated,1,0); points[6*i + 5] = 0; } // make lines vx_resc_t *verts = vx_resc_copyf(points, npoints*3); vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, vxo_lines(verts, npoints, GL_LINES, vxo_points_style(vx_blue, 2.0f)))); }
void * camera_loop(void * data) { state_t * state = data; sleep(2); // wait for 2 seconds before starting the animation matd_t * zaxis = matd_create(3,1); zaxis->data[2] = 1; vx_buffer_add_back(vx_world_get_buffer(state->world, "cam-circle"), vxo_chain(vxo_mat_scale(CAM_RADIUS), vxo_circle(vxo_lines_style(vx_green, 3)))); vx_buffer_swap(vx_world_get_buffer(state->world, "cam-circle")); int64_t start_mtime = vx_util_mtime(); // tell each layer to follow pthread_mutex_lock(&state->mutex); { zhash_iterator_t itr; zhash_iterator_init(state->layers, &itr); vx_display_t * key; vx_layer_t * vl; while(zhash_iterator_next(&itr, &key, &vl)) { if (1) { float eye3[] = {CAM_RADIUS,-CAM_RADIUS,45.0f}; float lookat3[] = {CAM_RADIUS,0,0.0f}; float up3[] = {0,1,0}; vx_layer_camera_lookat(vl, eye3, lookat3, up3, 0); } } } pthread_mutex_unlock(&state->mutex); while (state->running) { // 5 seconds revolutions double rad = ( (vx_util_mtime() - start_mtime) % 5000) * 2* M_PI / 5e3; // compute the current position and orientation of the "robot" matd_t * orientation = matd_angle_axis_to_quat(rad, zaxis); matd_t * pos = matd_create(3,1); pos->data[0] = cos(rad) * CAM_RADIUS; pos->data[1] = sin(rad) * CAM_RADIUS; // tell each layer to follow pthread_mutex_lock(&state->mutex); { zhash_iterator_t itr; zhash_iterator_init(state->layers, &itr); vx_display_t * key; vx_layer_t * vl; while(zhash_iterator_next(&itr, &key, &vl)) { vx_layer_camera_follow(vl, pos->data, orientation->data, 1); } } pthread_mutex_unlock(&state->mutex); vx_buffer_add_back(vx_world_get_buffer(state->world, "robot-proxy"), vxo_chain(vxo_mat_quat_pos(orientation->data, pos->data), vxo_box(vxo_lines_style(vx_purple, 3)))); vx_buffer_swap(vx_world_get_buffer(state->world, "robot-proxy")); matd_destroy(orientation); matd_destroy(pos); usleep(100000); } matd_destroy(zaxis); return NULL; }