collision.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_BOX2D_COLLISION_
#define SE2EZ_CORE_BOX2D_COLLISION_

#include <mutex>

#include <se2ez/core/class_forward.h>
#include <se2ez/core/collision.h>

namespace se2ez
{
    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(Box2DWorld)
    SE2EZ_CLASS_FORWARD(Box2DShape)
    SE2EZ_CLASS_FORWARD(Box2DContact)
    SE2EZ_CLASS_FORWARD(Box2DListener)
    SE2EZ_CLASS_FORWARD(Box2DCollisionManager)
    /** \endcond */

    /** \class se2ez::Box2DWorldPtr
        \brief A shared pointer wrapper for se2ez::Box2DWorld. */

    /** \class se2ez::Box2DWorldConstPtr
        \brief A const shared pointer wrapper for se2ez::Box2DWorld. */

    /** \class se2ez::Box2DShapePtr
        \brief A shared pointer wrapper for se2ez::Box2DShape. */

    /** \class se2ez::Box2DShapeConstPtr
        \brief A const shared pointer wrapper for se2ez::Box2DShape. */

    /** \class se2ez::Box2DCollisionManagerPtr
        \brief A shared pointer wrapper for se2ez::Box2DCollisionManager. */

    /** \class se2ez::Box2DCollisionManagerConstPtr
        \brief A const shared pointer wrapper for se2ez::Box2DCollisionManager. */

    /** \brief A collisions manager that uses Box2D as the underlying collision checking engine.
     */
    class Box2DCollisionManager : public CollisionManager
    {
    public:
        /** \brief Constructor.
         *  \param[in] robot Robot to construct collision manager for.
         */
        Box2DCollisionManager(const RobotPtr &robot);

        /** \brief Destructor.
         */
        virtual ~Box2DCollisionManager();

        /** \brief Compiles internal data structures for collision checking. Needs to be called if the robot
         * is updated.
         */
        void compile() override;

        // non-copyable
        Box2DCollisionManager(const Box2DCollisionManager &) = delete;
        Box2DCollisionManager(Box2DCollisionManager &&) = delete;

        /** \brief Check if a state is in collision.
         *  \param[in] state State to check for collisions using an allowed collision map.
         *  \return Returns true if state is in collision, false otherwise.
         */
        bool collide(StatePtr &state) override;

        /** \brief Check if a state is in collision.
         *  \param[in] state State to check for collisions using an allowed collision map.
         *  \param[out] frames What frames are in collision.
         *  \return Returns true if state is in collision, false otherwise.
         */
        bool collide(StatePtr &state, std::set<std::string> &frames) override;

        /** \brief Computes the signed distance of a scene to collision.
         *  \param[in] state State to computed signed distance for.
         *  \return Returns signed distance.
         */
        SignedDistance distance(StatePtr &state) override;

        /** \brief Computes the signed distance of a scene to collision.
         *  \param[in] state State to computed signed distance for.
         *  \param[out] frames Distance of frame to collision.
         *  \return Returns signed distance.
         */
        SignedDistance distance(StatePtr &state, SignedDistanceMap &frames) override;

    private:
        /** \brief Updates internal world structure based upon robot state.
         *  \param[in] state State to update to.
         */
        void update(StatePtr &state);

        /** \brief Callback function for checking contacts. Takes in as argument the two fixtures that are a
         * part of the collision, as well as the contact information. Returns true if iteration should
         * continue, false if it should terminate early.
         */
        using ContactCallback = std::function<bool(const Box2DShape *, const Box2DShape *, const Box2DContact &)>;

        /** \brief Check all Box2D contacts in the world, and call a callback function for each.
         *  \param[in] callback Callback function to call for each collision.
         *  \return True if checked all without terminating, false if terminated early.
         */
        bool checkContacts(const ContactCallback &callback);

        /** \brief Callback function for checking all potential collisions. Takes in as argument the two
         * fixtures that are a part of the collision. Returns true if iteration should continue, false if it
         * should terminate early.
         */
        using PotentialCallback = std::function<bool(const Box2DShape *, const Box2DShape *)>;

        /** \brief Check all potential collisions in the world, and call a callback function for each.
         *  \param[in] callback Callback function to call for each pair.
         *  \return True if checked all without terminating, false if terminated early.
         */
        bool checkPotentials(const PotentialCallback &callback);

        /** \brief Compute distance from a contact.
         *  \param[in] contact Contact.
         *  \return distance between shapes in contact.
         */
        SignedDistance computeDistance(const Box2DContact &contact) const;

        /** \brief Compute distances between two Box2DShapes.
         *  \param[in] fa First shape.
         *  \param[in] fb Second shape.
         *  \return distance between shapes.
         */
        SignedDistance computeDistance(const Box2DShape *fa, const Box2DShape *fb) const;

        const RobotPtr robot_;          ///< Robot model used for collision checking.
        Box2DWorldPtr world_{nullptr};  ///< Box2D world model.
        std::map<std::string, std::vector<Box2DShapePtr>> shapes_;  ///< Map of frames to Box2D shapes.
        std::mutex mutex_;                                          ///< Mutex for collision checking.
    };
}  // namespace se2ez

#endif