Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <mvsim/Wheel.h>
00011 #include <mrpt/opengl/CSetOfObjects.h>
00012 #include <mrpt/opengl/CCylinder.h>
00013 #include <mrpt/opengl/stock_objects.h>
00014 #include <rapidxml.hpp>
00015
00016 #include "xml_utils.h"
00017
00018 #include <mrpt/version.h>
00019 #if MRPT_VERSION<0x199
00020 #include <mrpt/utils/TColor.h>
00021 using mrpt::utils::TColor;
00022 using mrpt::utils::TColorf;
00023 using mrpt::utils::DEG2RAD;
00024 #else
00025 #include <mrpt/img/TColor.h>
00026 using mrpt::img::TColor;
00027 using mrpt::img::TColorf;
00028 using mrpt::DEG2RAD;
00029 #endif
00030
00031
00032 using namespace mvsim;
00033 using namespace std;
00034
00035 Wheel::Wheel()
00036 : x(.0),
00037 y(-.5),
00038 yaw(.0),
00039 diameter(.4),
00040 width(.2),
00041 mass(2.0),
00042 Iyy(1.0),
00043 color(0xff323232),
00044 phi(.0),
00045 w(.0)
00046 {
00047 recalcInertia();
00048 }
00049
00050 void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj)
00051 {
00052 obj.clear();
00053
00054 auto gl_wheel = mrpt::opengl::CCylinder::Create(
00055 0.5 * diameter, 0.5 * diameter, this->width, 15, 1);
00056 gl_wheel->setColor(TColorf(color));
00057 gl_wheel->setPose(
00058 mrpt::poses::CPose3D(
00059 0, 0.5 * width, 0, 0, 0, DEG2RAD(90)));
00060
00061 auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create();
00062 gl_wheel_frame->insert(gl_wheel);
00063 {
00064 mrpt::opengl::CSetOfObjects::Ptr gl_xyz =
00065 mrpt::opengl::stock_objects::CornerXYZSimple(0.9 * diameter, 2.0);
00066 gl_wheel_frame->insert(gl_xyz);
00067 }
00068
00069 obj.setPose(mrpt::math::TPose3D(x, y, 0.5 * diameter, yaw, 0.0, 0.0));
00070
00071 obj.insert(gl_wheel_frame);
00072 }
00073
00074 void Wheel::loadFromXML(const rapidxml::xml_node<char>* xml_node)
00075 {
00076 ASSERT_(xml_node);
00077
00078
00079
00080
00081 {
00082 const rapidxml::xml_attribute<char>* attr =
00083 xml_node->first_attribute("pos");
00084 if (attr && attr->value())
00085 {
00086 const std::string sAttr = attr->value();
00087 vec3 v = parseXYPHI(sAttr, true);
00088 this->x = v.vals[0];
00089 this->y = v.vals[1];
00090 this->yaw = v.vals[2];
00091 }
00092 }
00093
00094
00095 const double INERTIA_NOT_SET = -1.;
00096 this->Iyy = INERTIA_NOT_SET;
00097
00098 std::map<std::string, TParamEntry> attribs;
00099 attribs["mass"] = TParamEntry("%lf", &this->mass);
00100 attribs["width"] = TParamEntry("%lf", &this->width);
00101 attribs["diameter"] = TParamEntry("%lf", &this->diameter);
00102 attribs["color"] = TParamEntry("%color", &this->color);
00103 attribs["inertia"] = TParamEntry("%lf", &this->Iyy);
00104
00105 parse_xmlnode_attribs(*xml_node, attribs, "[Wheel]");
00106
00107
00108 if (Iyy == INERTIA_NOT_SET) this->recalcInertia();
00109 }
00110
00111
00112 void Wheel::recalcInertia()
00113 {
00114
00115 Iyy = mass * (0.25 * diameter * diameter) * 0.5;
00116 }