Block.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       Multiblock simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2024  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <box2d/b2_fixture.h>
#include <box2d/b2_friction_joint.h>
#include <box2d/b2_polygon_shape.h>
#include <box2d/b2_world.h>
#include <mrpt/img/TColor.h>
#include <mrpt/math/TPolygon2D.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/typemeta/TEnumType.h>
#include <mvsim/ClassFactory.h>
#include <mvsim/Sensors/SensorBase.h>
#include <mvsim/Simulable.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VisualObject.h>
#include <mvsim/basic_types.h>
 
#include <mutex>
 
namespace mvsim
{
enum class GeometryType : int32_t
{
        Invalid = -1,
        // ----------------
        Cylinder = 0,
        Sphere,
        Box,
        Ramp,
        SemiCylinderBump,
};
 
class Block : public VisualObject, public Simulable
{
   public:
        using Ptr = std::shared_ptr<Block>;
 
        static Ptr factory(World* parent, const rapidxml::xml_node<char>* xml_node);
        static Ptr factory(World* parent, const std::string& xml_text);
 
        static void register_block_class(const World& parent, const rapidxml::xml_node<char>* xml_node);
 
        // ------- Interface with "World" ------
        virtual void simul_pre_timestep(const TSimulContext& context) override;
        virtual void simul_post_timestep(const TSimulContext& context) override;
        virtual void apply_force(
                const mrpt::math::TVector2D& force,
                const mrpt::math::TPoint2D& applyPoint = mrpt::math::TPoint2D(0, 0)) override;
 
        virtual void create_multibody_system(b2World& world);
 
        virtual float getMaxBlockRadius() const { return maxRadius_; }
        virtual double getMass() const { return mass_; }
        b2Body* getBox2DBlockBody() { return b2dBody_; }
        mrpt::math::TPoint2D getBlockCenterOfMass() const
        {
                return block_com_;
        }  
 
        const mrpt::math::TPolygon2D& blockShape() const { return block_poly_; }
 
        void blockShape(const mrpt::math::TPolygon2D& p)
        {
                block_poly_ = p;
                updateMaxRadiusFromPoly();
                gl_block_.reset();      // regenerate 3D view
        }
 
        void setBlockIndex(size_t idx) { blockIndex_ = idx; }
        size_t getBlockIndex() const { return blockIndex_; }
 
        Block(World* parent);
 
        double ground_friction() const { return groundFriction_; }
        void ground_friction(double newValue) { groundFriction_ = newValue; }
 
        double mass() const { return mass_; }
        void mass(double newValue) { mass_ = newValue; }
 
        bool isStatic() const;
        void setIsStatic(bool b);
 
        const mrpt::img::TColor block_color() const { return block_color_; }
        void block_color(const mrpt::img::TColor& c)
        {
                block_color_ = c;
                gl_block_.reset();      // regenerate 3D view
        }
 
        double block_z_min() const { return block_z_min_; }
        double block_z_max() const { return block_z_max_; }
        void block_z_min(double v)
        {
                block_z_min_ = v;
                gl_block_.reset();      // regenerate 3D view
        }
        void block_z_max(double v)
        {
                block_z_max_ = v;
                gl_block_.reset();      // regenerate 3D view
        }
 
        bool default_block_z_min_max() const;
 
        VisualObject* meAsVisualObject() override { return this; }
 
        std::optional<float> getElevationAt(const mrpt::math::TPoint2D& worldXY) const override;
 
   protected:
        virtual void internalGuiUpdate(
                const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& viz,
                const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical, bool childrenOnly) override;
 
        size_t blockIndex_ = 0;
 
        std::vector<b2FrictionJoint*> friction_joints_;
 
        // Block info:
        double mass_ = 30.0;
        bool isStatic_ = false;
        mrpt::math::TPolygon2D block_poly_;
 
        double maxRadius_;
 
        double block_z_min_ = std::numeric_limits<double>::quiet_NaN(),
                   block_z_max_ = std::numeric_limits<double>::quiet_NaN();
 
        mrpt::img::TColor block_color_{0x00, 0x00, 0xff};
        mrpt::math::TPoint2D block_com_{.0, .0};  
 
        double lateral_friction_ = 0.5;  
        double groundFriction_ = 0.5;  
        double restitution_ = 0.01;      
 
        bool intangible_ = false;
 
        const TParameterDefinitions params_ = {
                {"mass", {"%lf", &mass_}},
                {"zmin", {"%lf", &block_z_min_}},
                {"zmax", {"%lf", &block_z_max_}},
                {"ground_friction", {"%lf", &groundFriction_}},
                {"lateral_friction", {"%lf", &lateral_friction_}},
                {"restitution", {"%lf", &restitution_}},
                {"color", {"%color", &block_color_}},
                {"intangible", {"%bool", &intangible_}},
                {"static", {"%bool", &isStatic_}}
                //
        };
 
        void updateMaxRadiusFromPoly();
 
        // Box2D elements:
        b2Fixture* fixture_block_;
 
   private:
        void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene);
 
        void internal_parseGeometry(const rapidxml::xml_node<char>& xml_geom_node);
 
        struct GeometryParams
        {
                GeometryParams() = default;
 
                std::string typeStr;  // cylinder, sphere, etc.
                GeometryType type = GeometryType::Invalid;
 
                float radius = 0;
                float length = 0, lx = 0, ly = 0, lz = 0;
                int vertex_count = 0;
 
                const TParameterDefinitions params = {
                        {"type", {"%s", &typeStr}},
                        {"radius", {"%f", &radius}},
                        {"length", {"%f", &length}},
                        {"lx", {"%f", &lx}},
                        {"ly", {"%f", &ly}},
                        {"lz", {"%f", &lz}},
                        {"vertex_count", {"%i", &vertex_count}},
                };
        };
 
        GeometryParams geomParams_;
 
        mrpt::opengl::CSetOfObjects::Ptr gl_block_;
        mrpt::opengl::CSetOfLines::Ptr gl_forces_;
        std::mutex force_segments_for_rendering_cs_;
        std::vector<mrpt::math::TSegment3D> force_segments_for_rendering_;
 
};      // end Block
 
class DummyInvisibleBlock : public VisualObject, public Simulable
{
   public:
        using Ptr = std::shared_ptr<DummyInvisibleBlock>;
 
        DummyInvisibleBlock(World* parent);
 
        static Ptr factory(World* parent, const rapidxml::xml_node<char>* xml_node);
        static Ptr factory(World* parent, const std::string& xml_text);
 
        // ------- Interface with "World" ------
        virtual void simul_pre_timestep(const TSimulContext& context) override
        {
                Simulable::simul_pre_timestep(context);
                for (auto& s : sensors_) s->simul_pre_timestep(context);
        }
        virtual void simul_post_timestep(const TSimulContext& context) override
        {
                Simulable::simul_post_timestep(context);
                for (auto& s : sensors_) s->simul_post_timestep(context);
        }
 
        virtual void apply_force(
                [[maybe_unused]] const mrpt::math::TVector2D& force,
                [[maybe_unused]] const mrpt::math::TPoint2D& applyPoint) override
        {
        }
 
        virtual void create_multibody_system(b2World&) {}
 
        virtual float getMaxBlockRadius() const { return 0; }
 
        virtual double getMass() const { return 0; }
 
        void add_sensor(const SensorBase::Ptr& sensor) { sensors_.push_back(sensor); }
 
   protected:
        void internalGuiUpdate(
                const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& viz,
                const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
                [[maybe_unused]] bool childrenOnly) override;
 
        void registerOnServer(mvsim::Client& c) override
        {
                // register myself, and my children objects:
                Simulable::registerOnServer(c);
                for (auto& sensor : sensors_) sensor->registerOnServer(c);
        }
 
   private:
        TListSensors sensors_;  
 
};      // end Block
 
}  // namespace mvsim
 
// TTypeEnum macros:
MRPT_ENUM_TYPE_BEGIN_NAMESPACE(mvsim, mvsim::GeometryType)
MRPT_FILL_ENUM_CUSTOM_NAME(GeometryType::Cylinder, "cylinder");
MRPT_FILL_ENUM_CUSTOM_NAME(GeometryType::Sphere, "sphere");
MRPT_FILL_ENUM_CUSTOM_NAME(GeometryType::Box, "box");
MRPT_FILL_ENUM_CUSTOM_NAME(GeometryType::Ramp, "ramp");
MRPT_FILL_ENUM_CUSTOM_NAME(GeometryType::SemiCylinderBump, "semi_cylinder_bump");
MRPT_ENUM_TYPE_END()