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