bool InitRobotState(
RobotState* out,
const RobotModel* model,
bool with_velocities,
bool with_accelerations)
{
RobotState robot_state;
out->model = model;
int mult = 1;
if (with_velocities) {
++mult;
}
if (with_accelerations) {
++mult;
}
out->values.resize(mult * GetVariableCount(model));
for (int i = 0; i < GetVariableCount(model); ++i) {
out->values[i] = GetDefaultPosition(model, GetVariable(model, i));
}
double* data = out->values.data();
out->positions = data;
data += GetVariableCount(model);
if (with_velocities) {
out->velocities = data;
data += GetVariableCount(model);
}
if (with_accelerations) {
out->accelerations = data;
data += GetVariableCount(model);
}
out->transforms.resize(
GetLinkCount(model) +
GetJointCount(model) +
GetCollisionBodyCount(model) +
GetVisualBodyCount(model));
out->link_transforms = out->transforms.data();
out->joint_transforms = out->link_transforms + GetLinkCount(model);
out->link_collision_transforms = out->joint_transforms + GetJointCount(model);
out->link_visual_transforms = out->link_collision_transforms + GetCollisionBodyCount(model);
out->dirty_links_joint = model->root_joint;
out->dirty_collisions_joint = model->root_joint;
out->dirty_visuals_joint = model->root_joint;
return true;
}
bool ezSkeleton::IsCompatibleWith(const ezSkeleton& other) const
{
if (this == &other)
return true;
if (other.GetJointCount() != GetJointCount())
return false;
// TODO: This only checks the joint hierarchy, maybe it should check names or hierarchy based on names
for (ezUInt32 i = 0; i < m_Joints.GetCount(); ++i)
{
if (other.m_Joints[i].GetParentIndex() != m_Joints[i].GetParentIndex())
{
return false;
}
}
return true;
}
