void display_gl_jump_render(struct vm_s* vm, struct display_gl_s* display) { v4_t color_diffuse; v4_t color_ambient; mat4_t local; mesh_definition_t* def = display_gl_mesh_get_definiton(MESH_TYPE_VC); int32 vertices_count; v4_set(&color_diffuse, 1.f, 1.f, 1.0f, 1.0f); v4_set(&color_ambient, 0.0f, 0.0f, 0.0f, 0.0f); mat4_ident(&local); display_gl_generate_line_count(&vertices_count); if (display->jump_count > 0) { glDisable(GL_DEPTH_TEST); glBlendFunc(GL_SRC_ALPHA, GL_ONE); glBindBuffer(GL_ARRAY_BUFFER, display_gl_mesh_get_vb(display->jump_mesh)); glBufferSubData(GL_ARRAY_BUFFER, 0, display->jump_count * def->stride * vertices_count, display->jump_vertex_buffer); glBindBuffer(GL_ARRAY_BUFFER, 0); display_gl_mesh_render_start(display->mesh_renderer, MESH_TYPE_VC); display_gl_mesh_set_ambient(display->mesh_renderer, &color_ambient); display_gl_mesh_set_diffuse(display->mesh_renderer, &color_diffuse); display_gl_mesh_set_local(display->mesh_renderer, &local); display_gl_mesh_set_projection(display->mesh_renderer, &display->projection_view); display_gl_mesh_render_count(display->jump_mesh, display->jump_count * vertices_count); } }
void display_gl_text_render(display_gl_text_t* texts, mat4_t* projection_view) { int32 i; int32 vb_index = 0; v4_t color_diffuse; v4_t color_ambient; mat4_t local; v4_set(&color_diffuse, 1.f, 1.f, 1.0f, 1.0f); v4_set(&color_ambient, 0.0f, 0.0f, 0.0f, 0.0f); mat4_ident(&local); for (i = 0; i < texts->text_count; ++i) { t_text* text = &(texts->texts[i]); vb_index += stb_easy_font_print(text->position.x, text->position.y, text->text, (uint8*)&text->rgba, texts->text_mesh_vb + vb_index, texts->text_mesh_vb_size) * 64; } glDisable(GL_DEPTH_TEST); glBindBuffer(GL_ARRAY_BUFFER, display_gl_mesh_get_vb(texts->text_mesh)); glBufferSubData(GL_ARRAY_BUFFER, 0, vb_index, texts->text_mesh_vb); display_gl_mesh_render_start(texts->text_renderer, MESH_TYPE_VC); display_gl_mesh_set_ambient(texts->text_renderer, &color_ambient); display_gl_mesh_set_diffuse(texts->text_renderer, &color_diffuse); display_gl_mesh_set_projection(texts->text_renderer, projection_view); display_gl_mesh_set_local(texts->text_renderer, &local); display_gl_mesh_render_count(texts->text_mesh, (vb_index / 64) * 6); }
void display_render_io_process(struct s_vm* vm, t_display* display) { t_v4 color_io_process; t_v4 color_ambient; t_v3 light_direction; t_mat4 local; t_mat4 translate; t_mat4 rotation; t_quat quat; int i; v4_set(&color_io_process, 0.4f, 0.4f, 1.0f, 0.0f); v4_set(&color_ambient, 0.2f, 0.2f, 0.2f, 1.0f); v3_set(&light_direction, 0, 0, -1.0f); display_mesh_render_start(display->mesh_renderer, MESH_TYPE_VN); display_mesh_set_ambient(display->mesh_renderer, &color_ambient); display_mesh_set_light_direction(display->mesh_renderer, &light_direction); display_mesh_set_diffuse(display->mesh_renderer, &color_io_process); display_mesh_set_projection(display->mesh_renderer, &display->projection_view); mat4_ident(&local); for (i = 0; i < vm->process_count; ++i) { t_process* process = vm->processes[i]; float angle = (float)process->cycle_create + (float)display->frame_last_time ; int index = process->pc; float x = (float) (index % display->memory_stride); float y = (float) (index / display->memory_stride); x = x * DISPLAY_CELL_SIZE + DISPLAY_CELL_SIZE * 0.5f; y = y * DISPLAY_CELL_SIZE + DISPLAY_CELL_SIZE * 0.5f; mat4_ident(&translate); mat4_translate(&translate, x, y, DISPLAY_CELL_SIZE * 0.5f); quat_from_euler(&quat, angle, angle, angle); quat_to_mat4(&quat, &rotation); mat4_mul(&translate, &rotation, &local); display_mesh_set_local(display->mesh_renderer, &local); display_mesh_render(display->process_mesh); } }
int Reaching::SetActualRobPosition(joint_vec_t& angles){ if(AcceptablePosition(angles)){ v4_copy(angles,pos_angle); v4_set(0,0,0,0,v_angle); v_set(0,0,0,v_cart); Angle2Cart(pos_angle,pos_cart); CVector2ArmConfig(pos_angle,&position); v_clear(tar_angle); return 1; } else{ return 0; } }
void Reaching::Angle2Cart(pArmConfig_t config, cart_vec_t& out){ joint_vec_t& angle; v4_set(config->sfe,config->saa,config->shr,config->eb,angle); body->Angle2Cart(angle,out); }