cmvr-es/include/utils/dynamics/link.h

//
// Created by xtkuang on 2025/7/8.
//

#ifndef LINK_H
#define LINK_H

#pragma once

#include <eigen3/Eigen/Core>
#include <memory>

#include "utils/dynamics/inertial.h"
#include "utils/math/liegroup.h"

namespace cmvr::dyn {
template<int DOF>
class Robot;
class Link;
class Joint;
class Collision;
struct CollisionResult;
class Geom;
class GeomCapsule;

enum class GeomType { kCapsule = 0 };

class Link : public std::enable_shared_from_this<Link> {
public:
    template<int DOF>
    friend class Robot;

    friend class Joint;

    static std::shared_ptr<Link> Make(std::string name, Inertial::MatrixType I = Inertial::I(1.));

    std::string GetName() const;

    std::weak_ptr<Joint> GetParentJoint();

    std::vector<std::shared_ptr<Joint> > GetChildJointList();

    const std::vector<std::shared_ptr<Joint> > &GetChildJointList() const;

    void AddCollision(const std::shared_ptr<Collision> &collision);

    std::vector<std::shared_ptr<Collision> > GetCollisions();

    const std::vector<std::shared_ptr<Collision> > &GetCollisions() const;

private:
    Link(std::string name, Inertial::MatrixType I);

private:
    std::string name_{};
    Inertial::MatrixType I_{};

    std::weak_ptr<Joint> parent_joint_;
    std::vector<std::shared_ptr<Joint> > child_joints_;

    std::vector<std::shared_ptr<Collision> > collisions_;
};

class Collision : public std::enable_shared_from_this<Collision> {
public:
    explicit Collision(std::string name);

    void SetOrigin(const math::SE3::MatrixType &T);

    math::SE3::MatrixType GetOrigin() const;

    void AddGeom(const std::shared_ptr<Geom> &geom);

    std::vector<std::shared_ptr<Geom> > GetGeoms();

    const std::vector<std::shared_ptr<Geom> > &GetGeoms() const;

private:
    std::string name_;
    math::SE3::MatrixType T_{math::SE3::Identity()};
    std::vector<std::shared_ptr<Geom> > geoms_;
};

class Geom : public std::enable_shared_from_this<Geom> {
public:
    Geom(unsigned int coltype = 0, unsigned int colaffinity = 0) : coltype_(coltype), colaffinity_(colaffinity) {
    }

    virtual ~Geom() = default;

    virtual GeomType GetType() const = 0;

    unsigned int GetColtype() const { return coltype_; }

    unsigned int GetColaffinity() const { return colaffinity_; }

    virtual std::optional<CollisionResult> ComputeMinimumDistance(const math::SE3::MatrixType &T,
                                                                  const Geom &other_geom,
                                                                  const math::SE3::MatrixType &other_T) const = 0;

    bool Filter(const Geom &other_geom) const {
        return (coltype_ & other_geom.colaffinity_) || (other_geom.coltype_ & colaffinity_);
    }

protected:
    unsigned int coltype_;
    unsigned int colaffinity_;
};

class GeomCapsule : public Geom {
public:
    GeomCapsule(double length, double radius, unsigned int coltype = 0, unsigned int colaffinity = 0);

    GeomCapsule(Eigen::Vector3d sp, Eigen::Vector3d ep, double radius, unsigned int coltype = 0,
                unsigned int colaffinity = 0);

    GeomType GetType() const override;

    std::optional<CollisionResult> ComputeMinimumDistance(const math::SE3::MatrixType &T, const Geom &other_geom,
                                                          const math::SE3::MatrixType &other_T) const override;

    Eigen::Vector3d GetStartPoint() const;

    Eigen::Vector3d GetEndPoint() const;

    double GetRadius() const;

private:
    Eigen::Vector3d sp_;
    Eigen::Vector3d ep_;
    double radius_;
};

struct CollisionResult {
    std::string link1;
    std::string link2;
    Eigen::Vector3d position1;
    Eigen::Vector3d position2;
    double distance;
};

}
#endif //LINK_H