Contact/collision normals and convex radius

[mihe]

You mentioned in godotengine/godot-proposals#7308 that:

The margins are only used to during the initial overlap test as a performance optimization, but not when calculating contact points/normals, these use the original shape.

This took me by surprise, as I was under the impression (and just took for granted) that since the convex radius of a box/cylinder/hull effectively changes the shape of it then any collision/contact normal should be affected as well.

Either I must be misunderstanding you or Jolt's API, or there is something strange going on here.

In usual fashion I've hijacked one of the tests in Samples (fork here), which this time happens to be ContactListenerTest, by removing its PostPhysicsUpdate and OnContactValidate and changing the remaining code to be the following:

void ContactListenerTest::Initialize()
{
    RefConst<Shape> box_shape = new BoxShape(Vec3(1.0f, 1.0f, 1.0f), 0.25f);

    Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, RVec3(0, 3.0f, -1.95f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
    body1.SetAllowSleeping(false);
    body1.SetLinearVelocity(Vec3(0, -1, 0));
    mBodyInterface->AddBody(body1.GetID(), EActivation::Activate);

    Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, RVec3(0.0, 0.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
    body2.SetAllowSleeping(false);
    mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);

    mPhysicsSystem->SetGravity(Vec3::sZero());
}

void ContactListenerTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
{
    const RVec3 point_on_1 = inManifold.GetWorldSpaceContactPointOn1(0);
    const RVec3 point_on_2 = inManifold.GetWorldSpaceContactPointOn2(0);

    Trace("");
    Trace("Point on 1: (%.2f, %.2f, %.2f)", point_on_1.GetX(), point_on_1.GetY(), point_on_1.GetZ());
    Trace("Point on 2: (%.2f, %.2f, %.2f)", point_on_2.GetX(), point_on_2.GetY(), point_on_2.GetZ());
    Trace("Normal: (%.2f, %.2f, %.2f)", inManifold.mWorldSpaceNormal.GetX(), inManifold.mWorldSpaceNormal.GetY(), inManifold.mWorldSpaceNormal.GetZ());
    Trace("");
}

With the convex radius of box_shape set to 0.25 like that, the normal as reported by ContactManifold::mWorldSpaceNormal for the initial impact ends up being (0.00, -0.88, 0.47) for me. If I instead set the convex radius of box_shape to be 0 then I get the expected normal of (0, 1, 0).

The contact points do however seem to be as expected in either case, so perhaps this is just a matter of me misunderstanding the purpose of mWorldSpaceNormal, and I should instead calculate the normal myself by doing (point_on_2 - point_on_1).Normalized()?

EDIT: Note that this also applies when using CollideShapeResult::mPenetrationAxis as the contact normal.

[jrouwe]

I added this code to the OnContactAdded function:

DebugRenderer::sInstance->DrawMarker(point_on_1, Color::sRed, 0.1f);
DebugRenderer::sInstance->DrawMarker(point_on_2, Color::sGreen, 0.1f);
DebugRenderer::sInstance->DrawArrow(point_on_2, point_on_2 + inManifold.mWorldSpaceNormal, Color::sYellow, 0.1f);

When running I pressed H to disable shape rendering followed by Shift+H to enable convex radius rendering:

This shows that the contact points are indeed outside of the convex radius and on the edges/faces of the actual shape. But you're right that the normal in this case indeed comes from GJK which will use the rounded edges. There are several code paths here so it is not easy to explain, first it does a GJK check which uses the rounded edges (note ExcludeConvexRadius):

JoltPhysics/Jolt/Physics/Collision/Shape/ConvexShape.cpp

Lines 82 to 92 in b1cd849

	{
	// Create support function
	SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius;
	const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1);
	const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2);
	// Transform shape 2 in the space of shape 1
	TransformedConvexObject<Support> transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
	// Perform GJK step
	status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);

If that finds a collision inside the convex radius we don't need to run EPA so you get the normal from GJK which uses rounded edges. For shapes that have vertices (including boxes) we actually fetch the vertices from the shape to calculate an accurate contact manifold:

JoltPhysics/Jolt/Physics/Collision/Shape/ConvexShape.cpp

Lines 145 to 151 in b1cd849

	if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
	{
	// Get supporting face of shape 1
	shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face);
	// Get supporting face of shape 2
	shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face);

This doesn't use rounded corners so that's why in this case you do get accurate contact positions. If however one of the shapes doesn't have vertices (e.g. a sphere) it will fall back to the GJK provided contact points which would be on the rounded edges.

If the contact is deep enough to be bigger than the convex radius we go to yet another code path here:

JoltPhysics/Jolt/Physics/Collision/Shape/ConvexShape.cpp

Lines 109 to 121 in b1cd849

	SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius;
	const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1);
	const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2);
	// Add separation distance
	AddConvexRadius<Support> shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance);
	// Transform shape 2 in the space of shape 1
	TransformedConvexObject<Support> transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
	// Perform EPA step
	if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
	return;

Which as you can see doesn't use convex radius at all (note IncludeConvexRadius) and should return both an accurate contact point and normal.

In my mind things worked a lot simpler than they actually do :)

[mihe]

Interesting! I've stared at that piece of code many times, but never really understood the different phases.

In either case, any weirdness as a result of the rounded edges seems to mostly go away when setting the convex radius to 0 for both shapes involved, so at least there's a simple escape hatch.

Do you have any idea why this discrepancy seems to exist between Bullet and Jolt, with regards to being able to have a convex radius of 0? Whenever this is discussed it's made to seem like a non-zero convex radius is just a fact of life when dealing with GJK/EPA, when Jolt clearly shows that's not the case. I've often wondered why that is.

[jrouwe]

I have no idea why it is not possible in Bullet.

[bttner]

In the following, I want to discuss the approach of Jolt and how it differs from other engines like PhysX or PyBullet.

Some points worth mentioning; let C be a convex set:

• GJK determines a point c in C that is closest to a point p.
• EPA calculates a point c on the boundary of C that is closest to a point p in C. Let us call c the penetration point of C.
• In both cases, we can assume without loss of generality that p is the null-vector.

In most implementations, however, one is required to provide two sets/geometries A and B. In this case, we have the following interpretations:

• GJK calculates the closest distance between these two sets.
• EPA calculates the minimum translational distance between these two sets (e.g., the vector d of minimal length that will separate A and B the "fastest" when A is translated in the direction of -d and B is translated in the direction of d).

Mathematically, the necessity to provide two sets does not hurt the general case, since one can take the null-vector for B. However, it makes the discussion of GJK and EPA more cumbersome. Thus, for the following discussion, imagine we input a single set C into GJK and EPA.

Why do we need a margin? The latter is well explained in [1, page 166 onwards]:

1. EPA is expensive. Reason: The problem that EPA tries to solve does not necessarily have a global minimum. Take the center point of a Disk D. Then every point on the boundary of D is a solution to the problem.
2. GJK is cheap (relative to EPA). Reason: The problem that GJK tries to solve does have a global minimum.
3. Because of (1) and the observation that most penetrations in physics simulations are shallow (recall that there should not actually be any penetrations in physics simulations if all objects are assumed to be solids), we would like to make more use of GJK. A hybrid approach was born...

Before we continue, let us define some more sets:

1. Let U be a user defined set. E.g., RefConst<BoxShape> U = new BoxShape(Vec3(1.f, 1.f, 1.f)).
2. Let B be a ball with radius m, where m stands for margin/convex radius.

In PhysX and PyBullet, the object that the physics engine works with is U + B. Here is a great video https://www.youtube.com/watch?v=BGAwRKPlpCw that visualizes that claim. The hybrid approach works like this:

1. We apply GJK on U and we let d be the minimal distance to U.
2. If d is positive and greater than m, then U + B does not contain the origin and, hence, no penetration is ocurring.
3. If d is positive and lower than m, then U + B contains the origin but not U. So, we have a shallow penetration and we use the result of GJK as an approximation for the penetration point/depth. (Note that we need to "project" the result of GJK on U + B, which is straight forward since B is a ball.)
4. If d is equal to zero, then U contains the origin and we apply EPA on U + B. The penetration depth is greater than m.

Jolt, however, works a bit different. Jolt first performs a convex decomposition, that is, it tries to find a set E for which we have U = E + B. In general, this is not possible. What is possible though is to find E such that E + B is a subset of U. Note that E is defined by
U->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer, scale). So, how does Jolt implement the hybrid approach in ConvexShape::sCollideConvexVsConvex?

1. GJK is applied on E. Now, let d be the minimal distance to E.
2. If d is positive and greater than m, then E + B does not contain the origin but U actually can. This case is illustrated by point P1 in the provided image. The red areas are the areas where Jolt misses to detect a penetration.
3. If d is positive and lower than m, then E + B contains the origin but not E. We use the result from the GJK step and project it on E + B but not U. This case is illustrated by point P2 and Q2 (the projected penetration point) in the provided image. Note that, in general, Q2 might be a boundary or interior point of U.
4. If d is equal to zero, then E contains the origin and we apply EPA on U. This case is illustrated by point P3 and Q3 in the provided image.

So, Jolt can not define an object that it consistently works with; it can be E + B but also U. So, why does Jolt do it like this? I belive the main reasons are performance and the observation that accuracy errors are small. GJK and EPA work with polytopes. Since there is no polyope P such that P = U + B, EPA can not find the actual solution (only an approximation) for U + B and may run into numerical problems along the way. Moreover, EPA will most likely perform more iterations on a "smooth" surface than a polytope that approximates that "smooth" surface. Here, U can be seen as an approximation of E + B.

Example

RefConst<BoxShape> box_shape1 = new BoxShape(Vec3(1.f, 1.f, 1.f));
RefConst<BoxShape> box_shape2 = new BoxShape(Vec3(1.f, 1.f, 1.f));

ConvexShape::SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius;
const ConvexShape::Support *shape1_excl_cvx_radius = box_shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, Vec3(1.f, 1.f, 1.f));
const ConvexShape::Support *shape2_excl_cvx_radius = box_shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, Vec3(1.f, 1.f, 1.f));

Mat44 transform1 = Mat44::sIdentity();
transform1.SetTranslation(Vec3(1.96, 1.96, 1.96));

Mat44 transform2 = Mat44::sIdentity();

Mat44 inverse_transform1 = transform1.InversedRotationTranslation();
Mat44 transform_2_to_1 = inverse_transform1 * transform2;
TransformedConvexObject<ConvexShape::Support> transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);

EPAPenetrationDepth pen_depth;
EPAPenetrationDepth::EStatus status __attribute__((unused));

float tolerance = 0.f;

Vec3 penetration_axis(1.f, 1.f, 1.f);
Vec3 point1, point2;

// Perform GJK step
status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius(), transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), tolerance, penetration_axis, point1, point2);

// Convert to world space
point1 = transform1 * point1;
point2 = transform1 * point2;
Vec3 penetration_axis_world __attribute__((unused)) = transform1.Multiply3x3(penetration_axis);

In the above snippet, we have status = EStatus::NotColliding, even though there is a penetration.

Well, I hope my reasoning is correct. I greatly appreciate any feedback.

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

[jrouwe]

Hello,

You're right that Jolt cannot find collisions in area P1 but I think your reasoning is not completely correct.

I think it is not finding 'U = E + B' but instead you should see it as: We have ball B with margin m and box U which has an inner box U_inner (which is U with 2 * m_u subtracted from all sides) and margin m_u (the user specified margin of U).

GJK will solve B + U_inner and instead of comparing d with m it will compare d with m + m_u.

The maximum error in P1 is (sqrt(2) - 1) * m_u instead of (sqrt(2) - 1) * m as suggested in the image above.

The reason why I did this was because:

1. I didn't want shapes to be inflated by the margin (otherwise it leaves things floating above the ground)
2. I wanted collision tests to reflect what the artist created, not a rounded off version of this.
3. I wanted consistent collision results between ray casts and shape tests (ray casts are against the original box and do not even look at margin).

You can of course solve 1 by shrinking the convex shape by its margin and then expanding it again by the same margin, but in my experience this doesn't give nice results for 'pointy' convex hulls.

So yes, the tradeoff is that it sometimes misses collisions. m_u is supposed to be small compared to the shape size, so I found the error acceptable.

[bttner]

Hello. Thank you for your reply.

True, I forgot about the maximum separation distance. So, let

• m_s = mMaxSeparationDistance,
• m_u = mConvexRadius of shape1 + mConvexRadius of shape 2,
• m = m_u + m_s,
• combined_radius_sq = m^2,
• B is a ball with radius m (B = B_s + B_u), B_s is ball with radius m_s, and B_u is a ball with radius m_u,
• E = U_inner.

Then, as you said, we have:

1. GJK operates on E + B and E + B can contain points that U does not contain. So, it is not necessarily the case that E + B is a subset of U.
2. If 0 < d <= m, then the solution of GJK is projected on E + B. However, ConvexShape::sCollideConvexVsConvex accounts for B_s and the maximum error of P1 is therefore (sqrt(2) - 1) * m_u.
3. If d is equal to zero, then EPA is applied on U + B_s. Again, ConvexShape::sCollideConvexVsConvex accounts for B_s...
4. Basically, we look at shape1 + B_s to get more contacts and then correct for B_s again. If "penetration_depth" is negative (only possible when m_s > 0), then "-penetration_depth" equals the minimum distance between the participating shapes and we know that both shapes are not penetrating each other. So, for example, we choose m_s > 0 to detect possible incoming penetrations in the vicinity of ioCollector.GetEarlyOutFraction().

If m_s = 0, then m = m_u and my first reasoning should apply however. In this regard, I think it is fair to say that one tries to find E such that U = E + B_u, since, if such E exists, your method does not produce any errors.

Great, I think I got it now (hopefully). Btw, I like your approach because of the reasons you mentioned. Overall, I think it is a very interesting balance between accuracy and performance.