int main(int argc, char *argv[]) { (void)argc; (void)argv; // Enable collision checking aa_rx_cl_init(); // Initialize scene graph struct aa_rx_sg *scenegraph = aa_rx_dl_sg__scenegraph(NULL); aa_rx_sg_init(scenegraph); aa_rx_sg_cl_init(scenegraph); // setup window struct aa_rx_win * win = baxter_demo_setup_window(scenegraph); struct aa_gl_globals *globals = aa_rx_win_gl_globals(win); aa_gl_globals_set_show_visual(globals, 0); aa_gl_globals_set_show_collision(globals, 1); struct display_cx cx = {0}; cx.win = win; cx.scenegraph = scenegraph; cx.i_q = aa_rx_sg_config_id(scenegraph, "left_s0"); cx.cl = aa_rx_cl_create( scenegraph ); { aa_rx_frame_id n = aa_rx_sg_frame_count(scenegraph); aa_rx_frame_id m = aa_rx_sg_config_count(scenegraph); double q[m]; AA_MEM_ZERO(q,m); double TF_rel[7*n]; double TF_abs[7*n]; aa_rx_sg_tf(scenegraph, m, q, n, TF_rel, 7, TF_abs, 7 ); struct aa_rx_cl_set *allowed = aa_rx_cl_set_create( scenegraph ); int col = aa_rx_cl_check( cx.cl, n, TF_abs, 7, allowed ); aa_rx_cl_allow_set( cx.cl, allowed ); aa_rx_cl_set_destroy( allowed ); } aa_rx_win_set_display( win, display, &cx ); aa_rx_win_display_loop(win); // Cleanup aa_rx_cl_destroy( cx.cl ); aa_rx_sg_destroy(scenegraph); aa_rx_win_destroy(win); SDL_Quit(); return 0; }
int display( struct aa_rx_win *win, void *cx_, struct aa_sdl_display_params *params ) { (void)win; struct display_cx *cx = (struct display_cx *)cx_; const struct timespec *now = aa_sdl_display_params_get_time_now(params); const struct timespec *last = aa_sdl_display_params_get_time_last(params); const struct aa_rx_sg *scenegraph = cx->scenegraph; size_t n = aa_rx_sg_frame_count(scenegraph); size_t m = aa_rx_sg_config_count(scenegraph); double q[m]; AA_MEM_ZERO(q,m); int first = aa_sdl_display_params_is_first(params); if( ! first ) { double dt = aa_tm_timespec2sec( aa_tm_sub(*now, *last) ); cx->q += dt * 45 * (M_PI/180); } q[ cx->i_q ] = cx->q; double TF_rel[7*n]; double TF_abs[7*n]; aa_rx_sg_tf(scenegraph, m, q, n, TF_rel, 7, TF_abs, 7 ); aa_rx_win_display_sg_tf( cx->win, params, scenegraph, n, TF_abs, 7 ); int col = aa_rx_cl_check( cx->cl, n, TF_abs, 7, NULL ); printf("in collision: %s\n", col ? "yes" : "no" ); return 0; }
int display( void *globals_, struct aa_sdl_display_params *param ) { int updated = aa_sdl_display_params_get_update(param); if( !updated ) return 0; const struct aa_gl_globals *globals = (const struct aa_gl_globals *) globals_; glClearColor(1.0f, 1.0f, 1.0f, 1.0f); check_error("glClearColor"); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); check_error("glClear"); size_t n = aa_rx_sg_frame_count(scenegraph); double TF_rel[7*n]; double TF_abs[7*n]; aa_rx_sg_tf(scenegraph, 0, NULL, 2, TF_rel, 7, TF_abs, 7 ); aa_rx_sg_render( scenegraph, globals, (size_t)n, TF_abs, 7 ); return updated; }