Wheel.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
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         // Parse attributes:
00078         // <l_wheel pos="0.0 -0.5 [OPTIONAL_ANG]" mass="2.0" width="0.10"
00079         // diameter="0.30" />
00080         // pos:
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         // Detect if inertia is manually set:
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         // If not manually overrided, calc automatically:
00108         if (Iyy == INERTIA_NOT_SET) this->recalcInertia();
00109 }
00110 
00111 // Recompute Iyy from mass, diameter and height.
00112 void Wheel::recalcInertia()
00113 {
00114         // Iyy = m*r^2 / 2
00115         Iyy = mass * (0.25 * diameter * diameter) * 0.5;
00116 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35