Block.h

/*+-------------------------------------------------------------------------+
  |                       Multiblock simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  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 double 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
    }
 
    double visual_scale() const { return visual_scale_; }
    void visual_scale(double v)
    {
        visual_scale_ = 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;  
 
    float linear_damping_ = 0.1;
 
    float angular_damping_ = 0.1;
 
    bool intangible_ = false;
 
    double visual_scale_ = std::numeric_limits<double>::quiet_NaN();
 
    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_}},
        {"visual_scale", {"%lf", &visual_scale_}},
        {"linear_damping", {"%f", &linear_damping_}},
        {"angular_damping", {"%f", &angular_damping_}}
        //
    };
 
    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 double 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()