static void UpdateOnlyVisualBodyTransforms( RobotState* state, std::vector<const Joint*>* q) { assert(state->dirty_visuals_joint != NULL); q->push_back(state->dirty_visuals_joint); while (!q->empty()) { auto* joint = q->back(); q->pop_back(); auto* child_link = joint->child; auto& link_transform = state->link_transforms[GetLinkIndex(state->model, child_link)]; for (auto& visual : VisualBodies(child_link)) { auto& visual_transform = state->link_visual_transforms[ GetVisualBodyIndex(state->model, &visual)]; visual_transform = link_transform * visual.origin; } for (auto* child = child_link->children; child != NULL; child = child->sibling) { q->push_back(child); } } state->dirty_visuals_joint = NULL; }
int CheckHelpEnter(int top, int ylen) { if ((CurrentLink.nr == 0) || (CurrentLink.line < top) || (CurrentLink.line >= top+ylen)) return -1; else return GetLinkIndex(&CurrentLink); }
static void UpdateLinkTransforms(RobotState* state, std::vector<const Joint*>* q) { assert(state->dirty_links_joint != NULL); q->push_back(state->dirty_links_joint); while (!q->empty()) { auto* joint = q->back(); q->pop_back(); auto* child_link = joint->child; // update the joint transform auto& joint_transform = state->joint_transforms[GetJointIndex(state->model, joint)]; joint_transform = ComputeJointTransform(joint, GetJointPositions(state, joint)); // update the child link transform auto& link_transform = state->link_transforms[GetLinkIndex(state->model, child_link)]; if (joint->parent != NULL) { // parent_link * origin * joint transform auto& parent_transform = state->link_transforms[ GetLinkIndex(state->model, joint->parent)]; link_transform = parent_transform * joint->origin * joint_transform; } else { link_transform = joint->origin * joint_transform; } // recurse on children for (auto* child = child_link->children; child != NULL; child = child->sibling) { q->push_back(child); } } state->dirty_links_joint = NULL; }
int CheckHelpClick(int cx, int cy, int cxlen, int cylen, int msx, int msy, int top) { struct LinkPosition pos; if ((msx < cx) || (msy < cy) || (msx > cx + cxlen) || (msy > cy + cylen)) return -1; pos.line = top + msy - cy; pos.nr = SeekLinkNr(pos.line, msx - cx); if (pos.nr) { CurrentLink.nr = 0; return GetLinkIndex(&pos); } else return -1; }