EPA

Expanding Polytope Algorithm(EPA)算法是在运行GJK算法判定碰撞后，进一步计算得到穿透向量。

算法

闵可斯基差边上离原点最近的点所代表的支撑点对即为碰撞点对。通过扩充在GJK算法后得到的单纯形，使得其离原点最近的点为闵可斯基边上的点。

主流程

1. 搜索多面体离原点最近的面

2. 使用原点到该面的垂直方向作为支持函数的计算方向，得到新的支撑点，使用该点扩充多面体

3. 重复第一步，直到原点距离多面体最近的面的距离不再增加

扩充多面体

将面向新支撑点的面删除，剩下的轮廓的每一条边与新支撑点组成一个新的三角面。

用面的法向与面的中心到新支撑点的方向是否一致来判定该面是否删除，即：$N \cdot(point - center) > 0$

将所有删除的面的边加入到一个unordered_set中，加入时，查找set中是否已经有该边，若有，则该边是共享边，删除。最后set中就只剩下轮廓的边。

判定两条边是否同一条边时，要注意头尾点相反的也是同一条边。这里通过自定义的hash函数，对每条边都先将头尾中按x,y,z，从小到大排列。

数据结构

用EPAConvexHullBuilder类来管理多面体。用一个set来储存面，用面到原点的距离作为排序的依据。

std::set<Triangle,
decltype([](const Triangle &a, const Triangle &b)
{ return a.GetDistance() < b.GetDistance(); })> faces_;

程序

EPA.cpp

std::optional<Eigen::Vector3f> EPAAlgorithm(const Geometry &shape1, const Geometry &shape2, vector<Vector3f> &simplex)
{
    EPAConvexHullBuilder epa_builder{};
    epa_builder.buildConvexHull(simplex);
    float distance = epa_builder.GetDistance();
    do
    {
        distance = epa_builder.GetDistance();
        epa_builder.step(shape1, shape2);
    } while (distance < epa_builder.GetDistance());

    Vector3f penetration = epa_builder.GetLast();

    return penetration;
}

EPAConvexHullBuilder.hpp

/**
 * @brief EPAConvexHullBuilder is a class to build a convex hull from a set of points.
 */
class EPAConvexHullBuilder
{

    /**
     * @brief Edge is a class representing a edge specified to @class EPAConvexHullBuilder.
     */
    class Edge
    {
        Eigen::Vector3f head_{0.F, 0.F, 0.F}, tail_{0.F, 0.F, 0.F};

      public:
        bool operator==(const Edge &edge) const
        {
            return (this->head_ == edge.head_ && this->tail_ == edge.tail_) ||
                   (this->head_ == edge.tail_ && this->tail_ == edge.head_);
        }

        Edge(const Eigen::Vector3f &head, const Eigen::Vector3f &tail) : head_{head}, tail_{tail}
        {
        }

        const Eigen::Vector3f &GetHead() const
        {
            return head_;
        }

        const Eigen::Vector3f &GetTail() const
        {
            return tail_;
        }
    };

    /**
     * @brief Triangle is a class representing a triangle specified to @class EPAConvexHullBuilder.
     */
    class Triangle
    {
        Eigen::Vector3f a_{0.F, 0.F, 0.F}, b_{0.F, 0.F, 0.F}, c_{0.F, 0.F, 0.F};
        Eigen::Vector3f normal_{0.F, 0.F, 0.F};
        Eigen::Vector3f center_{0.F, 0.F, 0.F};

        float distance_; // distance to origin

      public:
        /**
         * @brief Triangle constructor.
         * @param a the first point.
         * @param b the second point.
         * @param c the third point.
         */
        Triangle(const Eigen::Vector3f &a, const Eigen::Vector3f &b, const Eigen::Vector3f &c);

        /**
         * @brief ~Triangle destructor.
         */
        ~Triangle() = default;

        /**
         * @brief getNormal gets the normal of the triangle.
         * @return the normal of the triangle.
         */
        Eigen::Vector3f getNormal() const
        {
            return normal_;
        }

        Eigen::Vector3f GetCenter() const
        {
            return center_;
        }

        /**
         * @brief getDistance gets the distance from the origin to the triangle.
         * @return the distance from the origin to the triangle.
         */
        float GetDistance() const
        {
            return distance_;
        }

        /**
         * @brief getA gets the first point of the triangle.
         * @return the first point of the triangle.
         */
        Eigen::Vector3f getA() const
        {
            return a_;
        };

        /**
         * @brief getB gets the second point of the triangle.
         * @return the second point of the triangle.
         */
        Eigen::Vector3f getB() const
        {
            return b_;
        };

        /**
         * @brief getC gets the third point of the triangle.
         * @return the third point of the triangle.
         */
        Eigen::Vector3f getC() const
        {
            return c_;
        };

        bool isFacing(const Eigen::Vector3f &point) const
        {
            return normal_.dot(point - center_) > 0;
        }
    };

    /*
    std::priority_queue<Triangle, decltype([](const Triangle &a, const Triangle &b) {
                            return a.GetDistance() < b.GetDistance();
                        })>
        faces_;
        */
    std::set<Triangle, decltype([](const Triangle &a, const Triangle &b) { return a.GetDistance() < b.GetDistance(); })>
        faces_;

  public:
    /**
     * @brief EPAConvexHullBuilder constructor.
     */
    EPAConvexHullBuilder() = default;

    /**
     * @brief ~EPAConvexHullBuilder destructor.
     */
    ~EPAConvexHullBuilder() = default;

    float GetDistance() const
    {
        return faces_.begin()->GetDistance();
    }

    Eigen::Vector3f GetLast() const
    {
        auto last = faces_.cbegin();
        Eigen::Vector3f outV;
        outV = (last->GetCenter().dot(last->getNormal()) / last->getNormal().squaredNorm()) * last->getNormal();
        return outV;
    }

    /**
     * @brief buildConvexHull builds a convex hull from a set of points.
     * @param points the set of points.
     * @return
     */
    void buildConvexHull(const std::vector<Eigen::Vector3f> &simplex);

    /**
     * @brief add a point to the convex hull.
     * @param point
     */
    void addPoint(const Eigen::Vector3f &point);

    void step(const Geometry &a, const Geometry &b);
};

EPAConvexHullBuilder.cpp

EPAConvexHullBuilder::Triangle::Triangle(const Eigen::Vector3f &a, const Eigen::Vector3f &b, const Eigen::Vector3f &c)
    : a_{a}, b_{b}, c_{c}
{
    center_ = (a + b + c) / 3.f;
    Eigen::Vector3f ab = b_ - a_;
    Eigen::Vector3f ac = c_ - a_;
    normal_ = ab.cross(ac);
    normal_.normalize();

    // assert(normal_.norm() != 0.F);

    Eigen::Vector3f origin{0.F, 0.F, 0.F};
    Eigen::Vector3f v = origin - a_;
    distance_ = v.dot(normal_);
    if (distance_ < 0)
    {
        distance_ = -distance_;
    }
    else
    {
        normal_ = -normal_;
    }
}

void EPAConvexHullBuilder::buildConvexHull(const std::vector<Eigen::Vector3f> &simplex)
{
    assert(simplex.size() == 4);

    Triangle t0(simplex[0], simplex[1], simplex[2]);
    Triangle t1(simplex[0], simplex[1], simplex[3]);
    Triangle t3(simplex[0], simplex[2], simplex[3]);
    Triangle t2(simplex[1], simplex[2], simplex[3]);

    faces_.insert(t0);
    faces_.insert(t1);
    faces_.insert(t2);
    faces_.insert(t3);
}

void EPAConvexHullBuilder::addPoint(const Eigen::Vector3f &point)
{
    auto hash_fun = [](const Edge &edge) -> size_t {
        Vector3f head = edge.GetHead();
        Vector3f tail = edge.GetTail();

        if (head.x() < tail.x())
        {
        }
        else if (head.x() > tail.x())
        {
            std::swap(head, tail);
        }
        else if (head.y() < tail.y())
        {
        }
        else if (head.y() > tail.y())
        {
            std::swap(head, tail);
        }
        else if (head.z() < head.z())
        {
        }
        else if (head.z() > tail.z())
        {
            std::swap(head, tail);
        }

        auto h1x = std::hash<float>{}(head.x());
        auto h2x = std::hash<float>{}(tail.x());

        auto h1y = std::hash<float>{}(head.y());
        auto h2y = std::hash<float>{}(tail.y());

        auto h1z = std::hash<float>{}(head.z());
        auto h2z = std::hash<float>{}(tail.z());
        return h1x ^ (h2x << 1) + h1y ^ (h2y << 2) + h1z ^ (h2z << 3);
    };

    std::unordered_set<Edge, decltype(hash_fun)> edges_keep;
    std::vector<Triangle> faces_delete{};
    for (const Triangle &face : faces_)
    {
        if (face.isFacing(point))
        {
            Vector3f a = face.getA();
            Vector3f b = face.getB();
            Vector3f c = face.getC();

            if (point == a || point == b || point == c)
            {
                return;
            }
            Edge edge0(a, b);
            Edge edge1(a, c);
            Edge edge2(b, c);
            if (edges_keep.find(edge0) != edges_keep.end())
            {
                edges_keep.erase(edge0);
            }
            else
            {
                edges_keep.insert(edge0);
            }

            if (edges_keep.find(edge1) != edges_keep.end())
            {
                edges_keep.erase(edge1);
            }
            else
            {
                edges_keep.insert(edge1);
            }

            if (edges_keep.find(edge2) != edges_keep.end())
            {
                edges_keep.erase(edge2);
            }
            else
            {
                edges_keep.insert(edge2);
            }

            // faces_.erase(face);
            faces_delete.push_back(face);
        }
    }

    for (const Triangle &face : faces_delete)
    {
        faces_.erase(face);
    }

    // new faces
    for (const Edge &edge : edges_keep)
    {
        Triangle new_face(edge.GetHead(), edge.GetTail(), point);

        if (new_face.getNormal().norm() != 0.F)
        {
            faces_.insert(new_face);
        }
    }
}

Vector3f MinkowskiDifferenceSupport(const Geometry &shape1, const Geometry &shape2, const Vector3f &dir)
{
    Vector3f a = shape1.GetSupportingPoint(dir);
    Vector3f b = shape2.GetSupportingPoint(-dir);
    return a - b;
}

void EPAConvexHullBuilder::step(const Geometry &a, const Geometry &b)
{
    const Triangle &triangle = *faces_.begin();
    Vector3f origin{0.F, 0.F, 0.F};
    Vector3f dir = PointToPlane(origin, triangle.getA(), triangle.getB(), triangle.getC());
    Vector3f point = MinkowskiDifferenceSupport(a, b, dir);
    addPoint(point);
}

测试

参考

1. github.com/jrouwe/Jolt…