Lecture 7: Collision Detection
Based on Chapter 4, Bullo and Smith
Vaibhav Srivastava
Department of Electrical & Computer Engineering
Michigan State University
Feb 6, 2017
ECE 818: Robotics
http://www.egr.msu.edu/∼vaibhav/teaching/robotics.html
1 / 15
Collision Detection
• How to determine if the robot in a given configuration collides with an obstacle?
• Problem: Given two bodies B1 and B2 , determine if they collide
• Distance between two sets A and B us
dist(A, B) = inf inf dist(a, b).
a∈A b∈B
• To solve collision detection, we need to some some basic problems first
• Basic primitive #1: is a point in a convex polygon?
• Basic primitive #2: do two segments intersect?
• Basic primitive #3: do two convex polygons intersect?
2 / 15
Subsection 12: Basic primitive #1: is a point in a convex polygon?
Basic primitive #1: is a point in a convex polygon?
Problem 4.3 Given a convex polygon and a point, determine if the point is inside the polygon.
The polygon is defined by a counter-clockwise sequence of vertices, p , . . . , p in Figure 4.16.
1
Problem:
Given a convex polygon and a point, determine if the
point4 is inside the
For each side of the polygon, we define the interior normal as in Figure 4.17 for the side p1 p2 .
polygon.
p2
p2
q
p3
p1
p4
Figure 4.16: Testing if a point lies in a polygon
p1
Figure 4.17: Interior normal to a side of the polygon
•Note:
The apolygon
is defined
counter-clockwise
sequence
of vertices,
p1n, ...,
pn
convex polygon
canbybeaequivalently
represented
as either
(1) a set of
points
(the polygon having those points as vertices) or (2) a set of n half-planes (the polygon being
For each side
of half-planes).
the polygon,These
compute
interior normal
the• intersection
of the
two equivalent
representations are called Vertex
Description and Half-plane Description of a convex polygon. With some computational cost, it
is possible to convert one representation into the other.
Given a convex polygon with counter-clockwise vertices {p1 , . . . , pn } and a point q, the
3 / 15
The polygon is defined by a counter-clockwise sequence of vertices, p1 , . . . , p4 in Figure 4.16.
For each side of the polygon, we define the interior normal as in Figure 4.17 for the side p1 p2 .
Basic primitive #1: is a point in a convex polygon?
p2
p2
q
p3
p1
p4
Figure 4.16: Testing if a point lies in a polygon
p1
Figure 4.17: Interior normal to a side of the polygon
Note: a convex polygon can be equivalently represented as either (1) a set of n points
(the polygon having those points as vertices) or (2) a set of n half-planes (the polygon being
the intersection of the half-planes). These two equivalent representations are called Vertex
Description and Half-plane Description of a convex polygon. With some computational cost, it
is possible to convert one representation into the other.
Given a convex polygon with counter-clockwise vertices {p1 , . . . , pn } and a point q, the
following conditions are equivalent:
4 / 15
The polygon is defined by a counter-clockwise sequence of vertices, p1 , . . . , p4 in Figure 4.16.
For each side of the polygon, we define the interior normal as in Figure 4.17 for the side p1 p2 .
Basic primitive #1: is a point in a convex polygon?
p2
p2
q
p3
p1
p1
p4
Figure 4.16: Testing if a point lies in a polygon
Figure 4.17: Interior normal to a side of the polygon
Note: a convex polygon can be equivalently represented as either (1) a set of n points
The
following
statements
are equivalent:
(the polygon
having
those points
as vertices) or (2) a set of n half-planes (the polygon being
the intersection of the half-planes). These two equivalent representations are called Vertex
• the point q is in the polygon (possibly on the boundary)
Description and Half-plane Description of a convex polygon. With some computational cost, it
is possible
representation
the other.
• for alltoi convert
∈ {1, . one
. . , n}
the point qinto
belongs
to the half-plane with boundary line
Given a convex polygon with counter-clockwise vertices {p1 , . . . , pn } and a point q, the
passing through the vertices pi and pi+1 and containing the polygon, and
following conditions are equivalent:
• for all i ∈ {1, . . . , n}, the dot product between the interior normal to the side
pi pi+1 and the segment pi q is positive or zero
4 / 15
Basic primitive #2: do two segments intersect?
Chapter 4, slide 174
Robotic Planning and Kinematics, v0.91(e) (11 Apr 2016).
Subsection 13: Basic primitive #2: do two segments intersect?
Problem:
Given
two
if they
they intersect.
intersect.
Problem 4.4
Given
twosegments,
segments,determine
determine if
p3
p2
p1
p4
Figure 4.18: Testing for the intersection of two segments
Note: any two lines in the plane are in one of three exclusive configuration: (1) parallel
and coincident, (2) parallel and distinct, or (3) intersecting at a single point. In order for two
segments to intersect, the two corresponding lines may be coincident (and the two segments
need to intersect at least partly) or may intersect at a point (and the point must belong to the
two segments).
5 / 15
4.4. Collision detection methods
87
Basic primitive #2: do two segments intersect?
A segment is described by its two vertices as shown in Figure 4.19. Each point pa belonging to
For
point
a on p1 p2
the each
segment
p1 pp
2 can be written as
pa = p1 + sa (p2
p1 ),
for sa 2 [0, 1].
pb = p3 + sb (p4
p3 ),
for sb 2 [0, 1].
Similarly, we have
Assume p1 = (x1 , y1 ), p2 = (x2 , y2 ), p3 = (x3 , y3 ), and p4 = (x4 , y4 ). The equality pa = pb is
equivalent to two linear equations in the two unknowns sa , sb :
x1 + sa (x2
y1 + sa (y2
x1 ) = x3 + sb (x4
y1 ) = y3 + sb (y4
x3 ),
y3 ).
These two equations can be solved and, for example, one obtains
sa =
One can show that
(x4
(y4
x3 )(y1
y3 )(x2
y3 )
x1 )
(y4
(x4
y3 )(x1
x3 )(y2
x3 )
num
=:
.
y1 )
den
(4.2)
6 / 15
.4.3
These two equations can be solved and, for example, one obtains
Basic primitive
two
(x4 x3 )(y#2:
y3 )do (y
ysegments
x3 ) intersect?
num
1
4
3 )(x1
sa =
(y4
y3 )(x2
x1 )
(x4
x3 )(y2
y1 )
=:
den
.
(4.2)
One can show that
(i) if num = den = 0, then the two lines are coincident,
(ii) if num 6= 0 and den = 0, then the two lines are parallel and distinct, and
(iii) if num 6= 0 and den 6= 0, then the two lines are not parallel and therefore intersect at a single
point.
Now, if the two lines intersect at a point, one still needs to check that the intersection point
• Can
actually
belongs
segment. This
factbelongs
can be checked
by solving
for the coe�cients
verifytoifthe
intersection
point
to segment
by checking
sa , sb ∈ [0,sa1]and sb
and verifying that they belong to the interval [0, 1].
Basic primitive #3: do two convex polygons intersect?
Problem 4.5. Given two convex polygons, determine if they intersect.
There are �ve possible cases that one must consider, and each is illustrated in Figure 4.20.
7 / 15
Problem 4.5 Given two convex polygons, determine if they intersect.
ThereBasic
are fiveprimitive
possible cases#that
andpolygons
each is illustrated
below. It
3: one
domust
twoconsider,
convex
intersect?
is computationally easy to distinguish case (1) (no collision), from cases (2), (3) and (4). To
distinguish case (5) from case (1) is a bit more complex.
Problem: Given two convex polygons, determine if they intersect
P2 \ P 1 = ;
P2 \ P1 6= ;
P1 ⇢ P 2
P2 ⇢ P 1
P2 \ P1 6= ;
and no vertex is in common
8 / 15
Basic primitive # 3: do two convex polygons intersect?
P2 \ P1 6= ;
and no vertex is in common
Problem: Given
two convex polygons, determine if they intersect
Figure 4.20: The �ve possible cases for determining of two convex polygons intersect
The polygon-intersection algorithm
Input: two convex polygons P1 and P2
Output: collision or no collision
1: if (any vertex of P1 belongs to P2 ) OR (any vertex of P2 belongs to P1 ) :
2:
return collision
3: if any edge of P1 intersects any edge of P2 :
4:
return collision
5: return no collision
4.4.4
Extension to non-convex polygons
Collision detection can be extended to non-convex polygons. First, we need to extend primitive
#1 to test if a point lies in a non-convex polygon.
Problem 4.6. Given a non-convex polygon and a point, determine if the point is inside the polygon.
9 / 15
Extension to non-convex polygons
Problem: Given a non-convex polygon and a point, determine if the point is inside the
polygon.
• Recall Jordon Curve Theorem?
10 / 15
Extension to non-convex polygons
Problem: Given a non-convex polygon and a point, determine if the point is inside the
polygon.
• Recall Jordon Curve Theorem?
• The number of intersections between a line segment and a closed curve, where
Robotic Planning and Kinematics, v0.91(e) (11 Apr 2016).
Chapter 4, slide 179
the endpoints of the segment lie on the outside of the curve, is even.
s4
s3
s2
s5
s1
q
s6
s8
s7
Figure 4.19: The ray shooting algorithm. The point q lies inside the polygon, and thus the ray intersects the
boundary an odd number of times.
must lie outside the polygon. Figure 4.19 shows an example in which the ray intersects three
10 / 15
Ray-shooting Algorithm
4.4. Collision detection methods
89
Ray-shooting algorithm for point in non-convex polygon
Input: a point q and a non-convex polygon P with n sides s1 , . . . , sn .
Output: inside or outside
1: Choose an arbitrary direction, and de�ne a ray R extending from q in the chosen direction.
2: intersections = 0
3: for i from 1 to n :
4:
if segment si intersects the ray R :
5:
intersections = intersections + 1
6: if intersections is odd :
7:
return inside
8: return outside
s4
s3
s2
s5
s1
q
11 / 15
Does a ray and line segment intersect?
Problem: Given a line segment s and a ray R, determine if they intersect.
12 / 15
Does a ray and line segment intersect?
Robotic Planning
and Kinematics,
v0.91(e) (11sApr
2016).
Chapter 4, slide 180
Problem:
Given
a line segment
and
a ray R, determine if they intersect.
p4 = o + vd
p2
p1
p2
p1
d
v
o
p3 = o
Figure 4.20: Le� figure: Intersection of a ray and a segment. Right figure: The ray is converted to a segment with
length equal to the distance from o to p1 .
p4•=The
o + dv
as showndistance
on the right
4.20o and
maximum
on of
p1Figure
p2 from
is test it intersects with the line segment
defined by p1 and p2 using primitive #2. The segment and ray intersect if and only if the two
segments intersect.
d = max{ko − p1 k, ko − p2 k}
Third and finally we can test if two non-convex polygons intersect.
• define a line segment with end points p3 = o and p4 = o + dv
Problem 4.8 Given two non-convex polygons, determine if they intersect.
•For
Determine
if can
p1 pnow
intersect.
2 and
3 p4exact
this step, we
usepthe
same algorithm as in primitive #3. The two non-convex
polygons must be in one of the five cases shown in the figure above and thus we can apply the
12 / 15
Given two non-convex polygons, determine if they intersect.
Problem: Given two non-convex polygons, determine if they intersect.
• Check vertex intersection using ray-shooting
• Check edge intersections as before
13 / 15
Collision detection for general obstacles
Figure 4.15: Bounding boxes are used to over-approximate sets in collision detection problems. Le� figure: Axisaligned Bounding Box (AABB). Right figure: Oriented Bounding Box (OBB).
• approximate the complex shape by a simple enclosing shape
• if no collision occurs between the two simple enclosing shapes, then return a “no
collision result
• if a collision is detected between two simple enclosing shapes, then refine the
approximation
• to detect collisions between moving objects, discretize time and perform a
collision detection test for each time step
14 / 15
Runtime of the numerical computation of the free configuration space
• each of the sampling methods for generating N sample points, runs in O(N) time
• checking if a point belongs to convex polygon with n vertices has complexity O(n)
• checking if two lines intersect can be done in O(1)
• given two convex polygons, with n and m vertices respectively, our algorithm for
checking if they intersect has a runtime in O(nm)
• checking if two non-convex polygons intersect has the same runtime of O(nm)
15 / 15