void State::setRPY(double roll, double pitch, double yaw)
{
ivRoll = roll;
ivPitch = pitch;
ivYaw = yaw;
ivPose.getBasis().setRPY(ivRoll, ivPitch, ivYaw);
recomputeNormal();
}
State::State(const tf::Transform& t, Leg leg)
: ivLeg(leg)
, ivSwingHeight(0.0)
, ivSwayDuration(0.0)
, ivStepDuration(0.0)
, ivGroundContactSupport(1.0)
, body_vel(geometry_msgs::Vector3())
, sway_distance(0.0)
, swing_distance(0.0)
, cost(0.0)
, risk(0.0)
{
ivPose = t;
ivPose.getBasis().getRPY(ivRoll, ivPitch, ivYaw);
recomputeNormal();
}
State::State(double x, double y, double z, double roll, double pitch, double yaw, Leg leg)
: ivLeg(leg)
, ivRoll(roll)
, ivPitch(pitch)
, ivYaw(yaw)
, ivSwingHeight(0.0)
, ivSwayDuration(0.0)
, ivStepDuration(0.0)
, ivGroundContactSupport(1.0)
, body_vel(geometry_msgs::Vector3())
, sway_distance(0.0)
, swing_distance(0.0)
, cost(0.0)
, risk(0.0)
{
ivPose.setOrigin(tf::Vector3(x, y, z));
ivPose.getBasis().setRPY(ivRoll, ivPitch, ivYaw);
recomputeNormal();
}
void Triangle::read(TiXmlNode* node)
{
String materialName;
TiXMLHelper::GetAttribute(node, "materialName", &materialName);
material = resourceManager.getMaterial(materialName);
TiXmlNode* vertexInfosNode = node->FirstChild("vertexInfos");
assert(vertexInfosNode);
TiXmlNode* vertexInfoNode = vertexInfosNode->FirstChild("vertexInfo");
int vertexIndex = 0;
while (vertexInfoNode)
{
TiXMLHelper::GetAttribute(vertexInfoNode, "position", &coords[vertexIndex]);
vertexIndex++;
vertexInfoNode = vertexInfoNode->NextSibling();
}
assert(vertexIndex == 3);
recomputeNormal();
}
