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);
}
