Implementation of GJK/EPA

[bttner]

Recently, I walked myself through your implementation of GJK/EPA, and I would like to take the opportunity to discuss some aspects of it with you.

(1) MustIncludeC/D
I understand the idea of that boolean from [Theorem 4.7, 1]. What I find curious is that you apply that theorem only on the degenerated case. E.g., why not skip the edge AB in GetClosestPointOnTriangle when MustIncludeC is true? Did you find that the performance gain is neglectable?

Moreover, in GetClosestPointOnTriangle you are swapping the vertices A with C under some condition. But then edge BC can also be edge AB and MustIncludeC looses its meaning:

JoltPhysics/Jolt/Geometry/ClosestPoint.h

Lines 238 to 241 in a58e548

	// Edge BC
	Vec3 bc = c - inB;
	float bc_len_sq = bc.LengthSq();
	if (bc_len_sq > Square(FLT_EPSILON))

If the only scope of swapping A with C is a numerical more stable calculation of the normal n, what do you think of putting the following code at the beginning of GetClosestPointOnTriangle and removing swap_ac and its logic?

Vec3 ab = b - a;
Vec3 ac = c - a;
Vec3 bc = c - b;

Vec3 n;
if (bc.LengthSq() < ac.LengthSq())
{
    n = ac.Cross(bc);
}
else
{
    n = ab.Cross(ac);
}
float n_len_sq = n.LengthSq();
// In the following, remove swap_ac and its logic.

(2) OriginOutsideOfTetrahedronPlanes
Lets look at

JoltPhysics/Jolt/Geometry/ClosestPoint.h

Lines 367 to 372 in a58e548

	// For each plane get the side that is outside (determined by the 4th point)
	float signd0 = ad.Dot(ab_cross_ac); // D
	float signd1 = ab.Dot(ac_cross_ad); // B
	float signd2 = ac.Dot(ad_cross_ab); // C
	float signd3 = -ab.Dot(bd_cross_bc); // A
	Vec4 signd(signd0, signd1, signd2, signd3);

By the scalar triple product and some basic properties of the cross product, we have
signd0 = signd1 = signd2 = signd3.
Thus, only numerical issues can lead to the case that sign_bits is neither all positive or negative, and we can not deduce from sign_bits if the underlying tetrahedron is degenerated (the vertices A, B, C, and D are affinely dependent) or not. (Needless to say, the tetrahedron is degenerated if abs(signd0) <= eps for some eps >= 0; ideally, eps = 0). But maybe I am wrong on this. I would be grateful if you can lead me into the right direction on this matter.

(3) Unreachable code in CastRay and CastShape
Lets look at

JoltPhysics/Jolt/Geometry/GJKClosestPoint.h

Lines 612 to 625 in a58e548

	else if (set == 0xf)
	{
	#ifdef JPH_GJK_DEBUG
	Trace("Full simplex");
	#endif
	// If there are 4 points, x is inside the tetrahedron and we've found a hit
	// Double check if this is indeed the case
	if (v_len_sq <= tolerance_sq)
	break;
	// We failed to converge, restart
	needs_restart = true;
	}

If set == 0xf (0b1111), then GetClosestPointOnTetrahedron must have returned Vec3::sZero() and v_len_sq should be zero. Hence, it is not possible to reach the line saying needs_restart = true, or am I wrong?

(4) EPA
In the implementation of EPA you are using a tailored convex hull algorithm. I am curious if you have tried to use your fully fledged convex hull algorithm and if this can lead to better results? E.g., would you use the latter one in EPA when precision is required?

I am looking forward to your feedback. Thank you in advance.

[1] Collision Detection in Interactive 3D Environments by Gino Van den Bergen

[jrouwe]

E.g., why not skip the edge AB in GetClosestPointOnTriangle when MustIncludeC is true?

When A and C are swapped MustIncludeC loses its meaning, so you cannot at compile time decide which edges you can skip. The idea behind swapping was that if you don't swap A and C, you will compare 2 nearly parallel line segments, and the calculations d1 .. d4 and consequently the closest point becomes less accurate. So I don't think it is only the normal 'n' that gets affected by the swap. We could start skipping edges that include A in case A and C were swapped, but that leads to a lot of extra ifs and modern CPUs don't like ifs and rather run through a bunch of extra code.

But maybe I am wrong on this. I would be grateful if you can lead me into the right direction on this matter.

I'm not quite sure I understand your question. What do you think is wrong about the code?

Hence, it is not possible to reach the line saying needs_restart = true, or am I wrong?

I think you're right here. My bad. I've fixed this in #742 and that should speed things up a little bit. Thanks for that!

I am curious if you have tried to use your fully fledged convex hull algorithm and if this can lead to better results?

Initially, this EPA convex hull algorithm was the only convex hull algorithm I had. This started generating conflicts as the main purpose of the EPA convex hull builder is to be fast (which means for example that it is completely without memory allocations) while building ConvexHullShapes needed to be accurate and e.g. deal with 2D convex hulls as well. So, yes it could probably lead to better results if you use the fully fledged version but it would also cause the whole simulation to run really slowly.

[bttner]

Thank you so much for your reply. Your reasoning makes perfect sense and really helped improving my understanding of GJK/EPA.

I think that all my questions are answered. Once again, thank you for taking the time.

[jrouwe]

Now that the first change has been merged, do you still plan to remove swap_ac from the second part of the algorithm? (If not then I'll take a look)

[bttner]

Yes, I am working on it.

In GetClosestPointOnTetrahedron, you are calling GetClosestPointOnTriangle for triangle ABC, ACD, ADB, BDC, and compare the results with each other to find the closest point. This is a very common procedure. What I am curious about is if you have tried to use a similar approach as in GetClosestPointOnTriangle but decided to go for a comparison? What I mean is, have you tried this:

ProjectionResult project_origin(const Eigen::Vector3f &a,
                                const Eigen::Vector3f &b,
                                const Eigen::Vector3f &c,
                                const Eigen::Vector3f &d,
                                Eigen::Vector3f &closest, unsigned int &set,
                                bool must_include_d, bool degenerated_dropout,
                                float eps)
{
    Eigen::Vector3f ab = b - a;
    Eigen::Vector3f ac = c - a;
    Eigen::Vector3f ad = d - a;

    float ssv = s_scaled_signed_volume(ad, ac, ab);

    if (std::abs(ssv) < eps)
    {
        if (degenerated_dropout)
        {
            return ProjectionResult::Degenerated;
        }

        s_project_origin_degenerated(a, b, c, d, closest, set, must_include_d);

        return ProjectionResult::Degenerated;
    }

    // Vertex A
    float h1 = -a.dot(ab);
    float h2 = -a.dot(ac);
    float h3 = -a.dot(ad);
    if (h1 <= 0.f and h2 <= 0.f and h3 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0001;
        closest = a;

        return ProjectionResult::Valid;
    }

    // Vertex B
    float h4 = b.dot(ab);
    float h5 = b.dot(ac);
    float h6 = b.dot(ad);
    // h4 - h5 = -b . (c - b)
    float h4h5 = h4 - h5;
    // h4 - h6 = -b . (d - b)
    float h4h6 = h4 - h6;
    if (h4 <= 0.f and h4h5 <= 0.f and h4h6 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0010;
        closest = b;

        return ProjectionResult::Valid;
    }

    // Edge AB
    float h7 = h4 * h2 - h1 * h5;
    float h8 = h4 * h3 - h1 * h6;
    if (h4 > 0.f and h1 > 0.f and h7 <= 0.f and h8 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0011;

        float v = h1 / (h1 + h4);
        closest = a + v * ab;

        return ProjectionResult::Valid;
    }

    // Vertex C
    float h9 = c.dot(ac);
    float h10 = c.dot(ab);
    float h11 = c.dot(ad);
    // h9 - h10 = -c . (b - c)
    float h9h10 = h9 - h10;
    // h9 - h11 = -c . (d - c)
    float h9h11 = h9 - h11;
    if (h9 <= 0.f and h9h10 <= 0.f and h9h11 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0100;
        closest = c;

        return ProjectionResult::Valid;
    }

    // Edge AC
    float h12 = h9 * h1 - h2 * h10;
    float h13 = h9 * h3 - h2 * h11;
    if (h9 > 0.f and h2 > 0.f and h12 <= 0.f and h13 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0101;

        float v = h2 / (h2 + h9);
        closest = a + v * ac;

        return ProjectionResult::Valid;
    }

    // Edge BC
    // h10 - h11 = - c . (d - b)
    float h10h11 = h10 - h11;
    float h14 = h9h10 * h4 + h4h5 * h10;
    float h15 = h9h10 * h4h6 + h4h5 * h10h11;
    if (h9h10 > 0.f and h4h5 > 0.f and h14 <= 0.f and h15 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0110;

        float v = h4h5 / (h9h10 + h4h5);
        closest = b + v * (c - b);

        return ProjectionResult::Valid;
    }

    // Triangle ABC
    float h16 = h14 * h3 - h12 * h6 - h7 * h11;
    if (h14 > 0.f and h12 > 0.f and h7 > 0.f and h16 <= 0.f)
    {
        if (must_include_d)
        {
            return ProjectionResult::LastPointNotIncluded;
        }

        set = 0b0111;
        closest = (h14 * a + h12 * b + h7 * c) / (h14 + h12 + h7);

        return ProjectionResult::Valid;
    }

    // Vertex D
    float h17 = d.dot(ad);
    float h18 = d.dot(ab);
    float h19 = d.dot(ac);
    // h17 - h18 = -d . (b - d)
    float h17h18 = h17 - h18;
    // h17 - h19 = -d . (c - d)
    float h17h19 = h17 - h19;
    if (h17 <= 0.f and h17h18 <= 0.f and h17h19 <= 0.f)
    {
        set = 0b1000;
        closest = d;

        return ProjectionResult::Valid;
    }

    // Edge AD
    float h20 = h17 * h1 - h3 * h18;
    float h21 = h17 * h2 - h3 * h19;
    if (h17 > 0.f and h3 > 0.f and h20 <= 0.f and h21 <= 0.f)
    {
        set = 0b1001;

        float v = h3 / (h17 + h3);
        closest = a + v * ad;

        return ProjectionResult::Valid;
    }

    // Edge BD
    // h18 - h19 = d . (b - c)
    float h18h19 = h18 - h19;
    float h22 = h17h18 * h4 + h4h6 * h18;
    float h23 = h17h18 * h4h5 + h4h6 * h18h19;
    if (h17h18 > 0.f and h4h6 > 0.f and h22 <= 0.f and h23 <= 0.f)
    {
        set = 0b1010;

        float v = h4h6 / (h17h18 + h4h6);
        closest = b + v * (d - b);

        return ProjectionResult::Valid;
    }

    // Edge CD
    float h24 = h17h19 * h9 + h9h11 * h19;
    float h25 = h17h19 * h9h10 - h9h11 * h18h19;
    if (h17h19 > 0.f and h9h11 > 0.f and h24 <= 0.f and h25 <= 0.f)
    {
        set = 0b1100;

        float v = h9h11 / (h17h19 + h9h11);
        closest = c + v * (d - c);

        return ProjectionResult::Valid;
    }

    // Triangle ABD
    float h26 = h22 * h2 - h20 * h5 - h8 * h19;
    if (h22 > 0.f and h20 > 0.f and h8 > 0.f and h26 <= 0.f)
    {
        set = 0b1011;
        closest = (h22 * a + h20 * b + h8 * d) / (h22 + h20 + h8);

        return ProjectionResult::Valid;
    }

    // Triangle ACD
    float h27 = h24 * h1 - h21 * h10 - h13 * h18;
    if (h24 > 0.f and h21 > 0.f and h13 > 0.f and h27 <= 0.f)
    {
        set = 0b1101;
        closest = (h24 * a + h21 * c + h13 * d) / (h24 + h21 + h13);

        return ProjectionResult::Valid;
    }

    // Triangle BCD
    float h28 = h25 * h4 + h23 * h10 + h15 * h18;
    if (h25 > 0.f and h23 > 0.f and h15 > 0.f and h28 <= 0.f)
    {
        set = 0b1110;
        closest = (h25 * b + h23 * c + h15 * d) / (h25 + h23 + h15);

        return ProjectionResult::Valid;
    }

    set = 0b1111;
    // closest = (h28 * a + h27 * b + h26 * c + h16 * d) / (h16 + h26 + h27 +
    // h28);
    closest = Eigen::Vector3f::Zero();
    return ProjectionResult::Valid;
}

The code is from my own project to analyze various ways of implementing GJK.

[jrouwe]

No, I have not tried this, but I can imagine that it is a faster approach since it doesn't check as many duplicate edges/vertices.

B.t.w. A colleague of mine pointed out the following article:

https://ora.ox.ac.uk/objects/uuid:69c743d9-73de-4aff-8e6f-b4dd7c010907/download_file?safe_filename=GJK.PDF&file_format=application%2Fpdf&type_of_work=Journal+article

which I haven't tried out yet, but looks interesting.

[bttner]

Yes, I know this article. The author also published his code, see [2].

Some things to note:

• [2] does not reflect the paper anymore. However, you can find the code described in the paper in the Commit History.
• [2] currently applies (more or less) the procedure described in [Section 4.3.5, 1], that is, it simplifies the Johnson's distance algorithm by means of [Theorem 4.7, 1] (the last point added to the current simplex must be part of the next simplex). An example where you applied the Johnson's distance algorithm is GetClosestPointOnTriangle. Another example is the function project_origin in my last post. So, the overall idea communicated in [Section 4.3.5, 1] is to roll out the Johnson's distance algorithm for the cases needed (line, triangle, tetrahedron, ...) and then to simplify it by means of [Theorem 4.7, 1].
• (Well, to see that [2] is mathematically equivalent to the Johnson's distance algorithm is not that obvious. One needs to apply the vector triple product, basic rules of the cross and dot product, etc, to come to this conclusion. )

So, as I see it, the author of that paper backed down from his own approach and currently goes (more or less) with the one proposed in [1]. (If I am in the wrong with any of my observations, please let me know.) For this reason, I am not sure if the proposed method is actually an improvement. Nevertheless, if you decide to do a comparison, I would be very interested in it.

[1] Collision Detection in Interactive 3D Environments by Gino Van den Bergen
[2] https://github.com/MattiaMontanari/openGJK