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