collision.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <Box2D/Box2D.h>

#include <se2ez/core/log.h>
#include <se2ez/core/geometry.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/collision.h>
#include <se2ez/core/cgal/polygon.h>
#include <se2ez/core/box2d/collision.h>

using namespace se2ez;

/** \brief Container class for Box2D's world representation.
 */
class se2ez::Box2DWorld
{
public:
    /** \brief Constructor.
     */
    Box2DWorld() : world(b2Vec2(0, 0))
    {
    }

    void setup()
    {
        world.SetAllowSleeping(false);
        world.SetAutoClearForces(true);
        world.SetContinuousPhysics(false);
        world.SetSubStepping(false);
    }

    b2World world;  ///< Box2D world.
};

/** \brief Container class for Box2D's contact representation.
 */
class se2ez::Box2DContact
{
public:
    /** \brief Constructor.
     */
    Box2DContact(b2Contact *contact) : contact(contact)
    {
    }

    /** \brief Get contact manifold this contact.
     *  \param[in] world The world contact manifold.
     *  \return The contact manifold.
     */
    b2Manifold *getManifolds(b2WorldManifold &world) const
    {
        contact->GetWorldManifold(&world);
        return contact->GetManifold();
    }

    b2Contact *contact;  ///< Box2D contact.
};

/** \brief Container class that converts se2ez::Geometry into a Box2D geometry.
 */
class se2ez::Box2DShape
{
public:
    /** \brief Constructor.
     *  \param[in] name Name of frame this geometry is associated with.
     *  \param[in] index Index within frames' geometry vector this geometry is associated with.
     *  \param[in] geometry Geometry to convert.
     *  \param[in] world Box2D world.
     *  \param[in] movable Does this frame move?
     */
    Box2DShape(const std::string &name, const unsigned int index, const GeometryPtr &geometry,
               Box2DWorldPtr world, bool movable)
      : movable(movable), name(name), index(index), geometry_(geometry)
    {
        // Create Box2D Body
        if (movable)
            bodyDef_.type = b2_dynamicBody;
        else
            bodyDef_.type = b2_staticBody;

        bodyDef_.userData = this;

        body_ = world->world.CreateBody(&bodyDef_);

        // Create Box2D fixture
        createShapes();
        shapeDef_.resize(shapes_.size());

        for (unsigned int i = 0; i < shapes_.size(); ++i)
        {
            shapeDef_[i].shape = shapes_[i];
            shapeDef_[i].density = 1;

            fixture_.emplace_back(body_->CreateFixture(&shapeDef_[i]));
        }
    }

    ~Box2DShape()
    {
        for (unsigned int i = 0; i < shapes_.size(); ++i)
            delete shapes_[i];

        for (unsigned int i = 0; i < points_.size(); ++i)
            delete[] points_[i].first;
    }

    /** \brief Sets the pose of this body.
     *  \param[in] pose Pose of this body.
     */
    void setPose(const Eigen::Isometry2d &pose)
    {
        const auto &x = pose.translation()[0];
        const auto &y = pose.translation()[1];
        body_->SetTransform(b2Vec2(x, y), tf::getRotation(pose));
    }

    /** \brief Get the current pose of this body in the Box2D world.
     *  \return The pose of the body.
     */
    Eigen::Isometry2d getPose() const
    {
        const auto &pos = body_->GetPosition();
        const auto &rot = body_->GetAngle();
        return tf::toIsometry(pos.x, pos.y, rot);
    }

    /** \brief Get the current pose of this body in the Box2D world.
     *  \return The pose of the body.
     */
    const b2Transform &getTransform() const
    {
        return body_->GetTransform();
    }

    /** \brief Get the list of fixtures attached to this body.
     *  \return The list of fixtures.
     */
    const std::vector<b2Fixture *> &getFixtures() const
    {
        return fixture_;
    }

    const bool movable;        ///< Is this a moving frame piece of geometry?
    const std::string name;    ///< Name of the frame this geometry is attached to.
    const unsigned int index;  ///< Index in frame this geometry is.

private:
    b2PolygonShape *createPolygon(const GeometryPtr &geometry)
    {
        auto *poly = new b2PolygonShape;

        unsigned int count = geometry->points.size();
        b2Vec2 *verts = new b2Vec2[count];

        // SE2EZ_DEBUG("Creating polygon with %1% vertices:", count);
        for (unsigned int i = 0; i < count; ++i)
        {
            // SE2EZ_DEBUG("  Vertex %1%: %2%, %3%", i, geometry->points[i](0), geometry->points[i](1));
            verts[i].Set(geometry->points[i](0), geometry->points[i](1));
        }

        poly->Set(verts, count);
        points_.emplace_back(verts, count);

        return poly;
    }

    /** \brief Convert the geometry of this body into a Box2D shape.
     */
    void createShapes()
    {
        switch (geometry_->type)
        {
            case Geometry::BOX:
            {
                auto *box = new b2PolygonShape;

                // A little magic here. Box2D internally pads polygons with a "skin" so that they appear to be
                // not in collision. Thus, as we want precision, we subtract that skin out so we get our exact
                // box.
                box->SetAsBox(geometry_->dimensions[0] - b2_polygonRadius,
                              geometry_->dimensions[1] - b2_polygonRadius);

                shapes_.emplace_back(box);
                break;
            }

            case Geometry::CIRCLE:
            {
                auto *circle = new b2CircleShape;
                circle->m_p.Set(0, 0);
                circle->m_radius = geometry_->dimensions[0];

                shapes_.emplace_back(circle);
                break;
            }

            case Geometry::CONVEX:
            {
                // Need to make sure that we stay under polygon limit.
                if (geometry_->points.size() > b2_maxPolygonVertices)
                {
                    geometries_ = geo::shatter(geometry_);
                    for (const auto &geo : geometries_)
                        shapes_.emplace_back(createPolygon(geo));
                }
                else
                    shapes_.emplace_back(createPolygon(geometry_));

                break;
            }

            case Geometry::SIMPLE:
            {
                // Need to break geometry into convex chunks
                auto partitions = geo::convexifyOptimal(geometry_);
                for (const auto &partition : partitions)
                {
                    if (partition->points.size() > b2_maxPolygonVertices)
                    {
                        auto triangles = geo::shatter(partition);
                        for (const auto &geo : triangles)
                        {
                            geometries_.emplace_back(geo);
                            shapes_.emplace_back(createPolygon(geo));
                        }
                    }
                    else
                    {
                        geometries_.emplace_back(partition);
                        shapes_.emplace_back(createPolygon(partition));
                    }
                }
                break;
            }
            default:
                throw std::runtime_error("Shape type not implemented!");
        };
    }

    const GeometryPtr &geometry_;          ///< Original geometry.
    std::vector<GeometryPtr> geometries_;  ///< Geometry (created from decompositions).

    b2BodyDef bodyDef_;                   ///< Box2D body definition to create body.
    std::vector<b2FixtureDef> shapeDef_;  ///< Box2d shape definitions to create fixtures.
    std::vector<b2Shape *> shapes_;       ///< Box2D geometries;

    b2Body *body_;                      ///< Box2D body.
    std::vector<b2Fixture *> fixture_;  ///< Box2D fixtures.

    std::vector<std::pair<b2Vec2 *, unsigned int>> points_;  ///< Polygon points.
};

///
/// Box2D Collision manager
///

Box2DCollisionManager::Box2DCollisionManager(const RobotPtr &robot) : robot_(robot)
{
    compile();
}

Box2DCollisionManager::~Box2DCollisionManager()
{
}

void Box2DCollisionManager::compile()
{
    std::unique_lock<std::mutex> lock(mutex_);

    shapes_.clear();

    world_ = std::make_shared<Box2DWorld>();
    acm_ = robot_->getACM();

    for (const auto &name : robot_->getFrameNames())
    {
        const auto &data = robot_->getFrameDataConst(name);
        const auto &frame = data->frame;
        const auto &geometry = frame->getGeometry();

        if (not geometry.empty())
        {
            auto en = shapes_.emplace(std::piecewise_construct,  //
                                      std::forward_as_tuple(name), std::forward_as_tuple());

            for (unsigned int i = 0; i < geometry.size(); ++i)
                en.first->second.emplace_back(
                    std::make_shared<Box2DShape>(name, i, geometry[i], world_, data->movable));
        }
    }

    world_->setup();
}

void Box2DCollisionManager::update(StatePtr &state)
{
    state->fk(robot_);

    for (const auto &e : shapes_)
    {
        const auto &poses = state->getGeometryPoses(robot_, e.first);
        for (unsigned int i = 0; i < poses.size(); ++i)
            e.second[i]->setPose(poses[i]);
    }

    world_->world.Step(1, 0, 0);
    world_->world.Step(1, 0, 0);
}

bool Box2DCollisionManager::checkContacts(const ContactCallback &callback)
{
    b2Contact *contact = world_->world.GetContactList();
    while (contact != nullptr)
    {
        auto fa = static_cast<Box2DShape *>(contact->GetFixtureA()->GetBody()->GetUserData());
        auto fb = static_cast<Box2DShape *>(contact->GetFixtureB()->GetBody()->GetUserData());

        if (contact->IsTouching() and acm_->collide(fa->name, fb->name))
        {
            Box2DContact container(contact);
            if (not callback(fa, fb, container))
                return false;
        }

        contact = contact->GetNext();
    }

    return true;
}

bool Box2DCollisionManager::checkPotentials(const PotentialCallback &callback)
{
    // Iterate through all combinations of shapes.
    for (auto ita = shapes_.begin(); ita != shapes_.end(); ++ita)
    {
        const auto &name_a = ita->first;
        const auto &shapes_a = ita->second;

        for (auto itb = ita; itb != shapes_.end(); ++itb)
        {
            const auto &name_b = itb->first;
            const auto &shapes_b = itb->second;

            // Ignore distances for frames that cannot collide.
            if (not acm_->collide(name_a, name_b))
                continue;

            for (const auto &sa : shapes_a)
            {
                for (const auto &sb : shapes_b)
                {
                    if (not sa->movable and not sb->movable)
                        continue;

                    if (not callback(sa.get(), sb.get()))
                        return false;
                }
            }
        }
    }

    return true;
}

bool Box2DCollisionManager::collide(StatePtr &state)
{
    std::unique_lock<std::mutex> lock(mutex_);
    update(state);

    return not checkContacts(
        [](const Box2DShape *, const Box2DShape *, const Box2DContact &) -> bool { return false; });
}

bool Box2DCollisionManager::collide(StatePtr &state, std::set<std::string> &frames)
{
    std::unique_lock<std::mutex> lock(mutex_);
    update(state);

    frames.clear();
    checkContacts([&frames](const Box2DShape *fa, const Box2DShape *fb, const Box2DContact &) -> bool {
        frames.emplace(fa->name);
        frames.emplace(fb->name);

        return true;
    });

    return frames.empty();
}

CollisionManager::SignedDistance Box2DCollisionManager::computeDistance(const Box2DShape *fa,
                                                                        const Box2DShape *fb) const
{
    SignedDistance sd;

    // Setup input.
    b2DistanceInput input;
    input.useRadii = true;

    // Set transforms (same across fixtures)
    input.transformA = fa->getTransform();
    input.transformB = fb->getTransform();

    // Iterate through all fixtures in each shape.
    for (const auto &faf : fa->getFixtures())
    {
        for (const auto &fbf : fb->getFixtures())
        {
            // Have to zero memory for cache.
            b2SimplexCache cache;
            bzero(&cache, sizeof(cache));

            // Index is ignored for non-chain shapes.
            input.proxyA.Set(faf->GetShape(), 0);
            input.proxyB.Set(fbf->GetShape(), 0);

            b2DistanceOutput output;
            b2Distance(&output, &cache, &input);

            if (output.distance < sd.distance)
            {
                sd.distance = output.distance;
                sd.point = {output.pointA.x, output.pointA.y};
                sd.normal = {output.pointB.x - output.pointA.x, output.pointB.y - output.pointA.y};
            }
        }
    }

    return sd;
}

CollisionManager::SignedDistance Box2DCollisionManager::computeDistance(const Box2DContact &contact) const
{
    b2WorldManifold world;
    auto *manifold = contact.getManifolds(world);

    SignedDistance sd;
    sd.normal = {world.normal.x, world.normal.y};

    for (int i = 0; i < manifold->pointCount; ++i)
        if (world.separations[i] < sd.distance)
        {
            sd.distance = world.separations[i];
            sd.point = {world.points[i].x, world.points[i].y};
        }

    sd.normal *= sd.distance;

    return sd;
}

CollisionManager::SignedDistance Box2DCollisionManager::distance(StatePtr &state)
{
    std::unique_lock<std::mutex> lock(mutex_);
    update(state);

    SignedDistance d;
    const auto &checkSD = [&](const Box2DShape *fa, const Box2DShape *fb, SignedDistance &sd) {
        if (fa->movable)
        {
            sd.frame = fa->name;
            sd.collider = fb->name;
        }
        else if (fb->movable)
        {
            sd.frame = fb->name;
            sd.collider = fa->name;
            sd.flip();
        }

        if ((fa->movable or fb->movable) and sd.distance < d.distance)
            d = sd;
    };

    // First check for penetrating collisions.
    checkContacts([&](const Box2DShape *fa, const Box2DShape *fb, const Box2DContact &contact) -> bool {
        auto sd = computeDistance(contact);
        checkSD(fa, fb, sd);

        return true;
    });

    // If no touching contacts, check distances to all other shapes.
    // Otherwise, terminate early.
    if (d.distance <= 0)
        return d;

    // Iterate through all combinations of shapes.
    checkPotentials([&](const Box2DShape *fa, const Box2DShape *fb) -> bool {
        auto sd = computeDistance(fa, fb);
        checkSD(fa, fb, sd);

        return true;
    });

    return d;
}

CollisionManager::SignedDistance Box2DCollisionManager::distance(StatePtr &state, SignedDistanceMap &frames)
{
    std::unique_lock<std::mutex> lock(mutex_);
    update(state);

    frames.clear();
    SignedDistance d;

    // Update the signed distance map if signed distance is less, or create entry if it does not exist.
    const auto &updateFrame = [&frames](const SignedDistance &sd) {
        auto f = frames.find(sd.frame);
        if (f == frames.end())
            frames.emplace(sd.frame, sd);

        else if (sd.distance < f->second.distance)
            f->second = sd;
    };

    // Checks if signed distance information needs to be set for either shape (only movable)
    const auto &checkSD = [&](const Box2DShape *fa, const Box2DShape *fb, SignedDistance &sd) {
        sd.frame = fa->name;
        sd.collider = fb->name;
        updateFrame(sd);

        sd.frame = fb->name;
        sd.collider = fa->name;
        sd.flip();

        updateFrame(sd);

        if (sd.distance < d.distance)
            d = sd;
    };

    // First check for penetrating collisions.
    checkContacts([&](const Box2DShape *fa, const Box2DShape *fb, const Box2DContact &contact) -> bool {
        auto sd = computeDistance(contact);
        checkSD(fa, fb, sd);

        return true;
    });

    // Iterate through all combinations of shapes.
    checkPotentials([&](const Box2DShape *fa, const Box2DShape *fb) -> bool {
        auto sd = computeDistance(fa, fb);
        checkSD(fa, fb, sd);

        return true;
    });

    return d;
}