Using distance3d for 2d collision detection and response

[elle-trudgett]

Hi, can distance3d work for 2D collision detection and response? I'm attempting to integrate this into a video game. To that end, I'm trying to figure out how far a character collider can move before it collides with some wall/etc.

But I'm trying it out for a very simple case and getting unexpected results. Here's a simple example showing 2 squares that overlap and what the minimum translation vector should be (or something similar) shown as an arrow.

I set up the squares like so:

square_1 = ConvexHullVertices(numpy.array([
    (0.0, 0.0, 0.0),
    (0.0, 1.0, 0.0),
    (1.0, 1.0, 0.0),
    (1.0, 0.0, 0.0),
]))

square_2 = ConvexHullVertices(numpy.array([
    (0.5, 0.5, 0.0),
    (0.5, 2.0, 0.0),
    (2.0, 2.0, 0.0),
    (2.0, 0.5, 0.0),
]))

Then I use GJK + EPA to compute the collision and minimum translation vector:

dist, p1, p2, simplex = gjk.gjk(square_1, square_2)
mtv, minkowski_faces, success = epa.epa(simplex, square_1, square_2)

The results that I get are:

dist = 0.0
p1 = array([0.8, 0.8, 0. ])
p2 = array([0.8, 0.8, 0. ])
simplex = array([[ 0.5,  0.5,  0. ],
       [-2. , -2. ,  0. ],
       [ 0. ,  0. ,  0. ],
       [ 0. ,  0. ,  0. ]])
mtv = array([0., 0., 0.])
minkowski_faces = array([[[ 0.5,  0.5,  0. ],
        [-2. , -2. ,  0. ],
        [ 0. ,  0. ,  0. ],
        [ 0. ,  0. ,  0. ]],

       [[ 0.5,  0.5,  0. ],
        [ 0. ,  0. ,  0. ],
        [ 0. ,  0. ,  0. ],
        [ 0. ,  0. ,  0. ]],

       [[ 0.5,  0.5,  0. ],
        [ 0. ,  0. ,  0. ],
        [-2. , -2. ,  0. ],
        [ 0. ,  0. ,  0. ]],

       [[-2. , -2. ,  0. ],
        [ 0. ,  0. ,  0. ],
        [ 0. ,  0. ,  0. ],
        [ 0. ,  0. ,  0. ]]])
success = True

My interpretation of this (correct me if I'm wrong) is that the polygons are intersecting (dist = 0) but there is no translation needed (mtv = (0, 0, 0)) which implies that they are touching?

I'm not sure how to interpret this result.

The other question I had is that the MTV is not guaranteed to be in the opposite direction of the character's motion (indeed, the GJK/EPA algorithms know nothing of this vector) so it should be as simple as either:

a) Projecting the MTA onto the movement vector, or b) Normalizing the movement vector and scaling it to the distance to the object

Would that work?

[elle-trudgett]

Thinking about it in 3 dimensions I could see that you'd be able to separate them with any movement in the z-axis, so that might explain why the MTV is 0, if I extrude by polygon sufficiently, e.g.:

    square_1 = ConvexHullVertices(numpy.array([
        (0.0, 0.0, 0.0),
        (0.0, 1.0, 0.0),
        (1.0, 1.0, 0.0),
        (1.0, 0.0, 0.0),
        (0.0, 0.0, 1000.0),
        (0.0, 1.0, 1000.0),
        (1.0, 1.0, 1000.0),
        (1.0, 0.0, 1000.0),
    ]))

    square_2 = ConvexHullVertices(numpy.array([
        (0.5, 0.5, 0.0),
        (0.5, 2.0, 0.0),
        (2.0, 2.0, 0.0),
        (2.0, 0.5, 0.0),
        (0.5, 0.5, 1000.0),
        (0.5, 2.0, 1000.0),
        (2.0, 2.0, 1000.0),
        (2.0, 0.5, 1000.0),
    ]))

I now get:

mtv = array([-0.13513514,  0.81081081,  0.        ])

which if I subtract from square_1 I get:

I would expect at least one of either the x or the y component of the MTV to be 0.5. Am I doing something wrong?

[elle-trudgett]

mpr works in this case:

square_1 = ConvexHullVertices(numpy.array([
    (0.0, 0.0, 0.0),
    (0.0, 1.0, 0.0),
    (1.0, 1.0, 0.0),
    (1.0, 0.0, 0.0),
]))

square_2 = ConvexHullVertices(numpy.array([
    (0.5, 0.5, 0.0),
    (0.5, 2.0, 0.0),
    (2.0, 2.0, 0.0),
    (2.0, 0.5, 0.0),
]))

intersection, depth, penetration_direction, contact_position = mpr.mpr_penetration(square_1, square_2)

print(f"{intersection = }, {depth = }, {penetration_direction = }, {contact_position = }")

gives me:

intersection = True, depth = 0.7071067811865476, penetration_direction = array([0.70710678, 0.70710678, 0.        ]), contact_position = array([0.75, 0.75, 0.  ])

But what is really strange is if I stretch out the first square:

square_1 = ConvexHullVertices(numpy.array([
    (0.0, 0.0, 0.0),
    (0.0, 1.0, 0.0),
    (3.0, 1.0, 0.0),
    (3.0, 0.0, 0.0),
]))

square_2 = ConvexHullVertices(numpy.array([
    (0.5, 0.5, 0.0),
    (0.5, 2.0, 0.0),
    (2.0, 2.0, 0.0),
    (2.0, 0.5, 0.0),
]))

intersection = False, depth = None, penetration_direction = None, contact_position = None

But doing the same extrusion trick

square_1 = ConvexHullVertices(numpy.array([
    (0.0, 0.0, 0.0),
    (0.0, 1.0, 0.0),
    (3.0, 1.0, 0.0),
    (3.0, 0.0, 0.0),
    (0.0, 0.0, 100.0),
    (0.0, 1.0, 100.0),
    (3.0, 1.0, 100.0),
    (3.0, 0.0, 100.0),
]))

square_2 = ConvexHullVertices(numpy.array([
    (0.5, 0.5, 0.0),
    (0.5, 2.0, 0.0),
    (2.0, 2.0, 0.0),
    (2.0, 0.5, 0.0),
    (0.5, 0.5, 100.0),
    (0.5, 2.0, 100.0),
    (2.0, 2.0, 100.0),
    (2.0, 0.5, 100.0),
]))

Then it does indeed give back a collision, although the contact_position is a little iffy for my case.

intersection = True, depth = 0.5, penetration_direction = array([0., 1., 0.]), contact_position = array([ 1.33333333,  0.8       , 50.        ])

[AlexanderFabisch]

Hi @elle-trudgett,

distance3d is designed exclusively for 3D! It was never tested for 2D. Having said that, I think the approach that you have should work. When I change the two squares to

square_1 = colliders.ConvexHullVertices(array([
    (0.0, 0.0, -1000.0),
    (0.0, 1.0, -1000.0),
    (1.0, 1.0, -1000.0),
    (1.0, 0.0, -1000.0),
    (0.0, 0.0, 1000.0),
    (0.0, 1.0, 1000.0),
    (1.0, 1.0, 1000.0),
    (1.0, 0.0, 1000.0),
]))

square_2 = colliders.ConvexHullVertices(array([
    (0.5, 0.5, -1000.0),
    (0.5, 2.0, -1000.0),
    (2.0, 2.0, -1000.0),
    (2.0, 0.5, -1000.0),
    (0.5, 0.5, 1000.0),
    (0.5, 2.0, 1000.0),
    (2.0, 2.0, 1000.0),
    (2.0, 0.5, 1000.0),
]))

I get the following result with GJK+EPA:

'mtv=array([0. , 0.5, 0. ]), success=True'

This makes sense. It is one of the two shortest translation that separates the two squares. However, this case is particularly tricky and you might have discovered a bug here. It is tricky because the other solution is (0.5, 0, 0) and averaging the two solutions is not a good solution. So they are two local and global minima. I would have to take a look how EPA handles these cases.

My interpretation of this (correct me if I'm wrong) is that the polygons are intersecting (dist = 0) but there is no translation needed (mtv = (0, 0, 0)) which implies that they are touching?

Yes, EPA doesn't seem to handle flat shapes too well. In a way it is correct, since you have to move an infinitesimal amount along the z axis to separate the two shapes.

The other question I had is that the MTV is not guaranteed to be in the opposite direction of the character's motion (indeed, the GJK/EPA algorithms know nothing of this vector) so it should be as simple as either: a) Projecting the MTA onto the movement vector, or b) Normalizing the movement vector and scaling it to the distance to the object Would that work?

I think a more common approach for continuous collision detection is the following (I never tried it though): for a convex mesh of N vertices, you add an additional amount of N vertices that contain the original vertices plus the velocity of the mesh times some delta time.

$\forall n \in \{1, \ldots, N\}: v_n(t + \Delta t) = v_n(t) + \Delta t \cdot \dot{v}_n(t)$

$mesh = \{ v_n(t) | n \in \{1, \ldots, N\} \} \cup \{ v_n(t+\Delta t) | n \in \{1, \ldots, N\} \}$

You will get a convex hull around the current and next position of the object. This way you will most likely (not exactly) get an mtv in the opposite direction of the mesh' motion.

But what is really strange is if I stretch out the first square:

It is interesting though that MPR finds the "correct" penetration direction. I would have assumed that it will find something like (0, 0, 1), which easily separates the two flat squares in 3D. I would assume that there are multiple local minima in this case and the selection happens kind of arbitrarily. So I wouldn't rely on this. The trick of extruding the objects in the z dimension should definitely work though.

Then it does indeed give back a collision, although the contact_position is a little iffy for my case.

I think it is a perfect solution. It is exactly in the middle. What's the issue? If it is about the z coordinate, 50 is the middle between 0 and 100. MPR will return 0 if you replace 0 z coordinates by -100 in your square definition. But I guess you don't care about z anyway.

[elle-trudgett]

Thanks for your response! I think I understand that with the swept (convex hull) intersecting with the other polygon, the "contact point" will be inside the overlapping area. I was thinking of the contact point being where the polygons touch after being pushed out (i.e. no overlapping volume.)

In my case where I'm looking for the penetration vector in the opposite direction of the movement vector, it should be the case that the shortest MTV projected onto the movement vector should give me the resolution vector in that direction. Allow me to think aloud..

Let's say I'm moving $ABCD$ by $\overrightarrow{u}$ and checking for a collision with $EFGH$:

Sweeping the square gives us the convex hull poly1

As I understand it, $\overrightarrow{v}$ and $\overrightarrow{w}$ are both valid MTVs. If I were able to project $\overrightarrow{w}$ onto $\overrightarrow{u}$ I would get the MTV (along the movement axis) I'm looking for, which can then be added to $\overrightarrow{u}$ to get $\overrightarrow{u^{\prime}}$, which resolves the collision like so:

Anyway, I'm still playing around with the code so I'll see what I come up with

[AlexanderFabisch]

You need a projection onto u because the mtv might not be what you are looking for. Maybe you are looking for something more similar to the hydroelastic pressure field model that gives you a more smooth direction for a separating force: https://github.com/AlexanderFabisch/distance3d/blob/79ed43a897e0edd837f6c1750dd93c1cb58dfd38/examples/visualizations/vis_pressure_field.py#L24 (wrench is force and moment). I don't know if it is easy to use for 2D though.

I will reopen the issue since you might have discovered some edge case in EPA. I have to investigate it.

[AlexanderFabisch]

Just for my reference, this seems to be a bug.

Thinking about it in 3 dimensions I could see that you'd be able to separate them with any movement in the z-axis, so that might explain why the MTV is 0, if I extrude by polygon sufficiently, e.g.:

    square_1 = ConvexHullVertices(numpy.array([
        (0.0, 0.0, 0.0),
        (0.0, 1.0, 0.0),
        (1.0, 1.0, 0.0),
        (1.0, 0.0, 0.0),
        (0.0, 0.0, 1000.0),
        (0.0, 1.0, 1000.0),
        (1.0, 1.0, 1000.0),
        (1.0, 0.0, 1000.0),
    ]))

    square_2 = ConvexHullVertices(numpy.array([
        (0.5, 0.5, 0.0),
        (0.5, 2.0, 0.0),
        (2.0, 2.0, 0.0),
        (2.0, 0.5, 0.0),
        (0.5, 0.5, 1000.0),
        (0.5, 2.0, 1000.0),
        (2.0, 2.0, 1000.0),
        (2.0, 0.5, 1000.0),
    ]))

I now get:

mtv = array([-0.13513514,  0.81081081,  0.        ])

which if I subtract from square_1 I get:

I would expect at least one of either the x or the y component of the MTV to be 0.5. Am I doing something wrong?

[elle-trudgett]

You need a projection onto u because the mtv might not be what you are looking for.

It turns out that the projection is not quite what I'm looking for. So I have this kind of a situation.

polyA is moving by vector u to polyA'. There is a box in the way, polyB. Computing the intersection of polyA' with polyB' gives an MTV of (0, -0.25) - I believe - which corresponds to moving the tip of the triangle down to resolve the collision. That might be reasonable in this circumstance, but there are also cases where u is sufficiently large that the MTV would put polyA on the other side of polyB, which should not be possible (it means polyA has moved through polyB.)

What I'm trying to calculate, and maybe this isn't the best way of going about it, is the vector u', which goes from the colliding vertex to the intersection point and here polyA can be translated by without overlapping polyB. This is different from projecting the MTV it onto u, shown by the point labeled "Projection", guided by the pink line which is a line perpendicular to u.

note that in the case where polyA is larger than polyB it may be the other way around: a vertex of polyB colliding into an edge of polyA.

I feel like I don't have quite the right knack for geometry problems to see why I'm making this more complicated than it needs to be.

[elle-trudgett]

Hmm, I wonder if this simplifies with just triangles and no rotation.

[AlexanderFabisch]

polyA is moving by vector u to polyA'. There is a box in the way, polyB. Computing the intersection of polyA' with polyB' gives an MTV of (0, -0.25) - I believe - which corresponds to moving the tip of the triangle down to resolve the collision. That might be reasonable in this circumstance, but there are also cases where u is sufficiently large that the MTV would put polyA on the other side of polyB, which should not be possible (it means polyA has moved through polyB.)

The problem is that we have to discretize time in a simulation. The simplest solution for this problem is (similar to the solution that you posted below) to break $u'$ down into smaller steps. In physical simulations this is done by decreasing the delta time, hence, increasing the simulation frequency.

Another thing that you could take a look at, if you want to spend more time on this, is a version of GJK that computes the intersection of a ray and an convex polytope. There is one in the Jolt physics engine that I didn't translate to Python yet, but should be fairly easy to do. With this version of GJK you could intersect $u'$ and polyB to find the intersection point that you are looking for.

edit: Now that I think about it, I think you could turn each point of polyA plus the corresponding one of polyA' into a polygon and intersect it with polyB. The contact point would be the one that you are looking for.

[elle-trudgett]

I have been using the convex hull of polyA+polyA' to do intersections with polyB, but the contact point, again without knowledge of the movement vector, is not guaranteed to be correct, since it would be the same result as if the "swept polygon" was static.

[elle-trudgett]

I have had some success with taking the minimum distance from all points to all edges (from A to B and vice versa using -u) using line segment intersections (since the distance has to be parallel to u) but it seems very inefficient