Distance in 2D I'm going to start in 2d because its much easier that way. I
find that often when I'm trying to work out a problem, things are
much easier in 2d and more often than not, moving into 3d from there is
pretty easy. SI units
will be used throughout. I will define matrices in bold uppercase,
vectors in uppercase and scalars in lowercase.
The dot product will be shown as . and cross product as x. ||A|| means the magnitude of A. When I want to express
individual components of a vector, I will write A_x to mean the x
component of A. I will use * to denote multiplication, but also note
that sV might be used to show the scalar s multiplied by the vector
V. This is an imporant measure when writing any kind of physically based
simulation. In a timestepping simulation (where there are
discreet moments in time, i.e. every frame) it is important to know how
much two shapes have penetrated each other so a correction can be made.
Penetration is inevitible since objects are effectively "warping" from
location to location as they move, without passing through the points
inbetween. The alternative would be to do continuous collision
detection where the inbetween path is accounted for, but I'm going to
concentrate on a timestepping approach for the moment since it will be the
most familar to most games programmers. Pretty simple to start out with. Two circles are defined as
having a centre and a radius. They collide when the distance between the
two centres is less than the sum of their radii: ||A-B|| < (r1+r2) or in code: sqrt((A_x-B_x)² + (A_y-B_y)²) < (r1+r2) Of course you can speed this up by not having the compute
the magnitude ||A-B|| which involves a sqrt by squaring both sides of
the equation: ||A-B||² < (r1+r2)² or in code: (A_x-B_x)² + (A_y-B_y)² < (r1+r2)² So to calculate a measure of the penetration we can say: p = ||A-B|| - (r1+r2) You can see that this is a simple rearangement of
the collision test inequality ||A-B|| < (r1+r2) = ||A-B|| - (r1+r2) < 0 All I have done is to subtract (r1+r2) from both sides of
the inequality which is handily also the measure of penetration between
the two circles. So, when p < 0 The two spheres are penetrating by p metres. We also would
like the penetration vector so that we can correct the
penetration once we discover it. This is the vector that moves both
circles to the point where they just touch, correcting the penetration.
Importantly it is not only just a vector that does this, it is the
only vector which corrects the penetration by moving the minimum
amount. This is important because we only want to correct the error, not
introduce more by moving too much when we correct, or too little. N = (A-B) / ||A-B|| P = N*p Here we have calculated the normalised vector N between the two centres
and the penetration vector P by multiplying our unit direction by the
penetration distance. At this point you may have noticed that I haven't tried to
optimise this part of the calculation by using squared lengths etc. This
is generally ok because we can still do the initial intersection test
using squared maths and then once we have detected an intersection,
we can go for the heavier maths since it will happen far less often. You may also have noticed that during our calculations we
have calculated the collision normal N;
this is no accident and will help us out later when we come to the physics part.
Drag
spheres to move them in this applet Circles and
Capsules (point vs line) This is slightly trickier and relies on a certain
amount of understanding about projection and in particular the dot product operator. What we actually want to
calculate is the clostest distance between the centre of the circle and
the line which represents the axis of the capsule. We can do this by
projecting the centre of the circle onto the axis and then checking this
distance to see if its smaller than the sum of the radii of the two
shapes: Capsule defined by its centre point C, unit axis A,
length m and radius r1. Circle defined by its centre H and radius r2: Get capsule end points: P0 = C - A*(m/2) P1 = C + A*(m/2) Get vector from endpoint to circle: D = H - P0 Project vector onto axis (and clamp against 0 and
length): d = D . A (0 < d < m) Get point on axis: R = P0 + A*d Distance from point on axis to circle: b = ||H-R|| You should now recognise this as the circle vs circle test
above. The test for intersection is therefore: ||H-R|| < (r1+r2) We can find the measure of penetration in the same way as
before: p = ||H-R|| - (r1+r2) The circle and capsule penetrate by p metres and we can find
the contact normal: N = (H-R) / ||H-R||
Drag capsule end points and circle to move
them Capsule and capsule (line vs
line) When I first wrote this section I wrongly suggested that capsule vs
capsule was a mere extension of the capsule vs circle function above
(by projecting the end points of one capsule onto the axis of the other
[and visa versa] and taking the smallest of the distances between
projected points and end points). This doesn't work in the case where the
two capsules overlap; the distance from end points to the projections can
be greater than the sum of the radii of the two capsules. Also, doing the
test this way does not extend at all well into 3d. What we need to do is to consider the Minkowski difference of the two
line segments which make up the capsules axis. Since I am about to
introduce this subject in the next section (Polyhedra) I will assume
you have skipped ahead and familiarised yourself with this powerful
operator. Essentially, it comes down to finding the closest distance
from the origin to the surface of the difference, be that
positive or negative distance. Start by checking for overlap in the
line segments by testing the origin for containment within the difference
of the line segments. If the origin penetrates the difference, find the
closest point and therefore the contact normal. To resolve the
penetration, move either capsule in the direction of the contact normal by
the penetration distance + the sum of radii of the capsules. If there is a positive distance to any of the edge of the difference
you will need to find the minimum of clamped distances to each edge of the
difference from the origin. When I say clamped, I mean that the projection
of the origin onto each edge should be clamped at the boundaries of the
edge. If this distance is less than the sum of the radii, then there is
penetration and the penetration direction is the direction from the origin
to the point on the closest edge.
Drag capsules around, grab vertices to rotate In the applet the capsules are shown along with the Minkowski
difference of the axis which make up the capsules. The line from the
origin to the surface of the difference is the contact normal direction
and its length is the distance between capsules. I have made it change
colour when the length is greater than the sum of the radii of the
capsules and also when the line segments intersect. Now we get to the interesting stuff. How do you find the penetration
distance (or even the positive distance) between two convex polyhedra? Of
course, in 2d polyhedra are just polygons. So we can start with this and
move up into 3d once we have a basis to work from. Features In 2d, features of a
polygon are vertices and faces (edges). The minimum distance between two
polygons is the smallest of a set of distances between features of each
polygon. That is, the minimum distance can be expressed as the distance
between a pair of features. In 2d, with objects A and B its either the
distance between a vertex of A and a face of B or a face of A and a vertex
of B. You may have noticed that
actually I've already been using the concept of features in all my tests
so far, so it doesn't only apply to polyhedra. Minkowski difference Now that we know
that the minimum distance can be expressed as the smallest from a set of
distances between feature pairs from two objects, we could just go ahead
and compute each distance and pick the minimum as the actual distance
between polygons taking care, of course, to clamp our projections onto
faces against the 0 and 1 of the extents of the face (remember that a face
is an edge in 2d). This will work fine as
long as the two polygons don't intersect. The problem is that the closest
two features can be is 0, i.e. they intersect. But we actually need a
measure of penetration. We can achieve this by
introducing the Minkowski Difference
operation for convex polyhedra. The Minkowski difference of two
polyhedra is simply every vertex of one object subtracted from every
vertex of the other. We are only interested in the convex hull of the
resulting difference object. If you imagine one
polygon shrinking down to become a point whilest the other grows to
accommodate the features of the shrunken one, you begin to get an
intuition for what the Minkowski difference looks like. It has the
interesting property that when the two original polygons intersect, the
difference contains the origin. When they just touch, the origin is
just touching a feature of the difference. Moreover, the shortest distance
from the origin to a feature of the difference gives you the contact
normal and also something called the Minimum Translational Distance
which is the globally shortest distance you can move either shape
from penetration until they both just touch. The MTD is the measure of
penetration I was talking about above. So how do we go about computing the
MTD? Well, in 2d it's pretty easy and not too slow (only linear in the
number of faces of both polygons), but it gets more tricky in 3d because
you then have edges to worry about. The brute force method is
what I'm going to start out with which basically involves explicitly
computing the convex hull of the Minkowski difference of the two
polygons whilest keeping track of how close each face is to the
origin. Convex hull of the Minkowski
difference We only care about the
convex hull of the Minkowski difference because only this will give us the
correct MTD. We must be able to find a way to only create faces which are
part of the convex hull. Fortunately for us there is a nice way of doing
this, using the concept of supporting vertices. A supporting
vertex is simply a vertex which is "most in the direction of" a given
direction vector. Mathematically, this can be found as the vertex which
has the greatest dot product with a given direction
vector. For general polygons this
just amounts to a search through all vertices, but it can be sped up
either by using some kind of hierarchy (Dobkin
and Kirkpatrick developed a hierarchy giving a O(log
n) search time), or by exploiting temporal coherence (i.e. keeping
track of the last supporting vertex) and using a technique known as hill
climbing to only check neighboring vertices to see if a new support point
would be closer than the last. For cubes and quadrics
such as cones and cylinders constant cost supporting mapping functions
exist (see A
Fast and Robust GJK Implementation for Collision Detection of Convex
Objects by Gino Van Den Bergen). Since we are in 2d, we
will use the constant cost support mapping for a square: Half extents of the square in a vector E Rotation matrix
R Position of object P Supporting direction
D Transform direction into the space of the
square: D' =
R*D Compute local space
supporting direction: S_x = D'_x <0 ? -E_x :
E_x S_y = D'_y <0 ? -E_y :
E_y Transform into world
space and return S: S' = S*R
+ P Now for two polygons A
and B we can run through edges of A, finding the supporting vertex of B in
the direction of the edge "normal" of A and then using the Minkowski
difference we can create a face in the Minkowski difference
set: for (i=0; i<A.num_edges;
i++) { vector support_vertex =
B.get_supporting_vertex( -A.edges[i].perp() ); edge minkowski_edge(A.edges[i].start -
support_vertex
A.edges[i].end - support_vertex); } Doing the same for edges
of B and supporting vertices of A will give us the remaining faces in the
Minkowski difference: for (i=0; i<B.num_edges;
i++) { vector support_vertex =
A.get_supporting_vertex( -B.edges[i].perp() ); edge minkowski_edge(B.edges[i].start -
support_vertex B.edges[i].end
- support_vertex); } You may have noticed that we actually use the
inverted 2d normal in our search for supporting vertices; this is because
we always want do the difference operator on features which are as far
apart as possible in order to maintain our constraint that the resulting
features lie on the convex hull of the Minkowski
difference. So once this has been done we end up with a Minkowski difference object
which is composed of vertex-face difference pairs of both objects. I just
want to re-emphisise that point: every face in the difference
represents a pair of features. So if we can find the face closest
to the origin, we have found the pair of features which are closest in
each object.
Drag the boxes to move them around If we project the origin onto each face of the Minkowski
difference and keep track of the biggest negative distance (and the point
projected onto the face) we have the contact normal and the MTD. The
beauty of this is that because when the two polygons overlap, the
Minkowski difference contains the origin and because it is itself convex,
we only need to worry about distance to infinite plane (infinite edge in
2d) in our calculations - something which is not true if we need a
positive distance measure. Of course, if any of the distances to the infinite planes
are positive then we can exit straight away as we have found a separating
axis between the two objects, this greatly speeds up the calculation.
Remember, our MTD is only accurate for negative distances, but in general
you only care about the distance between two objects if they are colliding
in which case the distance is negative
anyway.