diff --git a/src/decor.c b/src/decor.c index d962409..c5fde0f 100644 --- a/src/decor.c +++ b/src/decor.c @@ -36,7 +36,7 @@ bool decor_has_init(traj_info_t* traj_info) return DEC.count >= 0; } -void decor_positional_rootframe(traj_info_t* traj_info, double* pos) +void decor_positional_init(traj_info_t* traj_info, double* pos) { DEC.count = 0; DEC.pos[DEC.count * 3 + 0] = pos[0]; @@ -59,6 +59,29 @@ void decor_positional_addto(traj_info_t* traj_info, double* pos) DEC.count++; } +void decor_joint_init(traj_info_t* traj_info, double* pos) +{ + DEC.count = 0; + DEC.pos[DEC.count * 3 + 0] = pos[0]; + DEC.pos[DEC.count * 3 + 1] = pos[1]; + DEC.pos[DEC.count * 3 + 2] = pos[2]; + mju_copy(DEC.size + DEC.count * 3, DEC.size_default_joint, 3); + f_copy(DEC.rgba + DEC.count * 4, DEC.rgba_default_joint, 4); + DEC.rgba[DEC.count * 4 + 3] = 0.25; + DEC.count++; +} + +void decor_joint_addto(traj_info_t* traj_info, double* pos) +{ + DEC.pos[DEC.count * 3 + 0] = pos[0]; + DEC.pos[DEC.count * 3 + 1] = pos[1]; + DEC.pos[DEC.count * 3 + 2] = pos[2]; + mju_copy(DEC.size + DEC.count * 3, DEC.size_default_joint, 3); + f_copy(DEC.rgba + DEC.count * 4, DEC.rgba_default_joint, 4); + DEC.rgba[DEC.count * 4 + 3] = 0.25; + DEC.count++; +} + diff --git a/src/decor.h b/src/decor.h index cb99986..f3ae9d5 100644 --- a/src/decor.h +++ b/src/decor.h @@ -6,10 +6,15 @@ #include "mujoco.h" #include +#include "decor.h" + +void decor_single_sphere(traj_info_t* traj_info, mjvScene* scn, int num); void decor_showdecor(traj_info_t* traj_info, mjvScene* scn); void decor_reset(traj_info_t* traj_info); bool decor_has_init(traj_info_t* traj_info); -void decor_positional_rootframe(traj_info_t* traj_info, double* pos); +void decor_positional_init(traj_info_t* traj_info, double* pos); void decor_positional_addto(traj_info_t* traj_info, double* pos); +void decor_joint_init(traj_info_t* traj_info, double* pos); +void decor_joint_addto(traj_info_t* traj_info, double* pos); #endif diff --git a/src/node.c b/src/node.c index 5b8f1bf..df255dc 100644 --- a/src/node.c +++ b/src/node.c @@ -511,6 +511,7 @@ void node_position_jointmove(traj_info_t* traj_info, double rootframe_init; double filter; double temp_new_qpos_val; + bool update_decor; timeline_set_qposes_to_pose_frame( traj_info, @@ -518,6 +519,8 @@ void node_position_jointmove(traj_info_t* traj_info, rootframe); rootframe_init = traj_info->d->qpos[SEL.jointnum]; + update_decor = !decor_has_init(traj_info); + for (i = 0; i < NODECOUNT; i++) { frame = (traj_info->timeline->numposes / NODECOUNT) * i; @@ -552,6 +555,11 @@ void node_position_jointmove(traj_info_t* traj_info, traj_info->d->xquat + (4*body_id.id), body_id.id ); + if(update_decor && i == 0) + decor_joint_init(traj_info, node_qpos); + else if (update_decor) + decor_joint_addto(traj_info, node_qpos); + } } @@ -636,7 +644,7 @@ void node_scale_visually_positional( decor_update = 0; if(decor_update) - decor_positional_rootframe(traj_info, global_body_init_xpos_at_rootframe); + decor_positional_init(traj_info, global_body_init_xpos_at_rootframe); for (i = 0; i < NODECOUNT; i++) {