Path Walking / Obstacle Avoidance

I have a map with integer grid points. The map is flat, so all points are on a 2D plane. There are objects scattered here and there on the map. Some objects are concave, and some are convex. I'm looking for a way to map a route between two points. I have come up with an algorithm which is fairly tight, but it hinges on the ability to do one thing:

For a line between any two points, find out whether it intersects any of the objects on the map.

There is one obvious way to handle this, which would be to walk the line from A to B checking each point to see whether it is part of one of the known objects. This seems painfully slow, but there may not be any superior alternative. The first thought I had was this:

1) Find the quadrant that the direction from A to B falls into. With 0 being North, then the quadrants are 315-45, 45-135, 135-225, and 225-315.

Assume that the angle theta falls between 0 and 45, the pseudo code would look something like this:

X as Integer

For each Y from Ya To Yb
X = Y\ (1/Tan(theta))
If check point (X,Y) Then
Exit, cause the line hit an object.
End If
Next

For angles 45-90, swap X and Y.
For angles 90-135 swap X and Y and negate Y.
etc.

What I'm looking for is a better way to do this. Of course, the pseudo code is not optimized, since 1/Tan(theta) is effectively a constant.

If there is also an optimal solution for the whole problem of finding a route from A to B, then I'd like to see it. It appears that route finding algorithms are based on roads. I don't have roads. This is about navigating around within a mostly empty space.

At some point you'll need a way to check if two segments intersect. Many ways for doing this exist, I'm sure, though I'll derive one using linear algebra since it's not likely that it's the one you would derive using slopes and maybe trig. You can choose whichever ends up being faster, of course. At the end I give a description of the algorithm that you should be able to implement without knowledge of linear algebra.


First segment = A to B. Assume A != B.
Second segment = C to D. Assume C != D.

Let v = (B-A), w = (D-C). Let L1 = |v| = distance from A to B. Let L2 = |w| = distance from C to D. The line AB can be parametrized with a parameter t1, where points on the line are of the form A+v*t1. Because of how I've defined v, points on the line *segment* AB must have 0 <= t1 <= 1. Similarly parametrize CD as C+w*t2, 0 <= t2 <= 1. Collide these segments, algebraically. For some t1, t2, say we have a point lying on both. Then...

A+v*t1 = C+w*t2
A.v + v.v*t1 = C.v + w.v*t2 [.v both sides]
L1^2*t1 = (C-A).v + w.v*t2 [v.v = |v|^2 = L1^2; dot product in real Euclidean space is bilinear]

A.w + v.w*t1 = C.w + w.w*t2 [.w both sides of first equation]
L2^2*t2 = (A-C).w + v.w*t1 [w.w = L2^2; bilinearity]
t2 = 1/L2^2 * [(A-C).w + v.w*t1]

L1^2*t1 = (C-A).v + w.v/L2^2 * [(A-C).w + v.w*t1]
L1^2*t1 = (C-A).v + w.v/L2^2 * (A-C).w + (w.v/L2)^2*t1
t1 = (C-A).v / L1^2 + (w.v)[(A-C).w] / (L1L2)^2 + (w.v / (L1L2))^2 * t1

Here we reach a bit of an impasse. If w.v = +/- L1L2, then the middle terms must all be zero, or we'll have no collision. Physically, w.v = +- L1L2 if and only if w || v (or w antiparallel to v), so it makes sense that we might not have a collision. Presume w.v != +/- L1L2...

t1 * (1 - (w.v / (L1L2))^2) = (C-A).v / L1^2 + (w.v)[(A-C).w] / (L1L2)^2
t1 = 1 / (1 - (w.v / (L1L2))^2) * [(C-A).v / L1^2 + (w.v)[(A-C).w] / (L1L2)^2]

There. The collision point has been found. I did this twice, once in text and once on paper to make sure I wouldn't make a mistake (since you'd need a linear algebra background to be able to verify it). This gives rise to the following collision detection algorithm.

Code:

---

Define P = (C-A).v / L1^2 + (w.v)[(A-C).w] / (L1L2)^2.
Define R = (A-C).w / L2^2. Define S = v.w / L2^2.
If v.w != +/- L1L2...
    Define Q = 1 - (w.v / (L1L2))^2. Calculate P / Q. This is t1.
    Test if 0 <= P/Q <= 1.
        If so, continue.
        If not, the collision does not occur on the segment.
    Calculate R + S*P/Q. This is t2.
    Test if 0 <= R + S*P/Q <= 1.
        If so, the collision occurs on the segment.
        If not, the collision does not occur on the segment.
If v.w = +/- L1L2...
    Test if P = 0.
        If so, continue.
        If not, no collision occurs, ever.
    Calculate R. This is t2 when t1 = 0.
    Test if 0 <= R <= 1.
        If so, the collision occurs on the segment.
        If not, continue.
    Calculate R+S. This is t2 when t1 = 1.
    Test if 0 <= R+S <= 1.
        If so, the collision occurs on the segment.
        If not, the segments are parallel but disjoint, so no collision occurs, ever.

---

Operations used:

Code:

---

Let V = (x1,y1), W = (x2,y2) be vectors [points]. Let c and d be numbers.

Vector addition/subtraction:
    V + W = (x1+x2, y1+y2)
    V - W = (x1-x2), y1-y2)
Vector multiplication/division by scalars:
    c*V = (c*x1, c*y1)
    V/c = (x1/c, y1/c)
Dot product:
    V.W = x1*x2 + y1*y2
Norm of a vector:
    |V| = length of V = Sqrt(x1^2 + y1^2)
Equality testing:
    Using floating point arithmetic, to test if c = d, test if (c-d) is very close to 0.
    This gets around rounding issues.

---

I assume the "objects on the map" are a series of connected line segments: (0,0), (0,1), (1,0), (0,0) for a triangle, for instance. If so, your algorithm will miss some collisions. Take A to B = (0,0), (1,2). Collide it with segment (1,-1), (0,1). The collision occurs at a non-integral point, though you only test collisions at grid points (X,Y).

One obvious solution is to collision detect your segment A,B with every segment on the map. If there are many segments, though, this would get computationally expensive. An improvement would be to divide the map into grid boxes. For each box, keep track of which segments of your objects pass through that grid box. Then, when you want to see if a segment A,B hits any objects or not, check to see which grid boxes it passes through (using the algorithm you used to generate the grid box information in the first place). You can then use the algorithm in my previous post on those segments, pairwise, to see if they actually collide or if they just happen to pass through the same box. (You'd have a decent approximation of collision detection if you just checked if the lines passed through the same grid boxes--effectively, your objects would be "fuzzy" by about a box)

You could store the information about which grid boxes contain which line segments using a hash table. I'd imagine a 2 dimensional array would be inefficient, with many "null" values.

The major unresolved problem is figuring out which boxes a line segment passes through. If it'd be helpful I can try to find an algorithm, though it looks a bit tedious. Is this the response you were looking for?

I suspect that will help, but in the intervening time, I got thinking about the problem from a different perspective.

The actual problem deals with robot code. Therefore, while I can look at the world as integral units (because a resolution lower than 1cm is meaningless), I could also divide the world up into blocks the width of the robot, as anything smaller is a gap the bot couldn't fit through anyways. I initially considered blocking up the world like that, but rejected it because it would eventually become unweildly. If I could find something that didn't use loads of storage space, that would be better...but it may still be necessary for another step that I haven't really gotten thinking about much.

The bot will identify points on the plane (it can only move in two dimensions, effectively). Points that are sufficiently close will be grouped into objects. Each point can be connected to up to two other points (one on either side), which will make a mesh over time. However, I don't know the error in the measurements, so the exact location of an object may be a bit fuzzy. In any case, known points will be stored in one table, and an accumulated mesh of points will cause an object to be written into another table.

That's all background, though. The robot will learn by dreaming, which is something I will start a thread on once I have sufficient code written to figure out the dimensions and performance of the dreaming. I'm basically using a GA to train a novel neural net design (it is non-traditional, as there is no true hidden layer), but since I need many iterations to evolve the GA, the dreams will provide the training. What the dreams will mean for the software design is that I will need to create virtual worlds for the dreamscape, and objects in that virtual world can be known very well (without any error at all to the measurements). Some of the dreams will deal with getting to a certain place (don't we all have dreams like that?), which will entail route finding. What occurred to me just recently was that I can wrap each object in a virtual bounding box, which can be easily calculated by the max and min X and Y coordinates. As long as the target point does not lie within one of the bounding boxes, the route finding needs only to determine whether the line from the bot to the target intersects one of the bounding boxes, which can be done at such a coarse scale that the math should be much simpler. After all, I can look at the relative X,Y coordinates of the bot and the target and exclude all objects that don't even need to be considered.

Since the objects will be stored in a database, and I can dedicate part of the time of one processor to studying the object map in the database. It occurs to me that I can write a module that looks for bounding boxes that overlap, and can look at the objects within the bounding boxes to see whether there is sufficient space for the bot to pass between them, and if there is, write a line to a different table indicating the "channel" between the two objects. I think that should allow for a much simpler route finding routine. What you have given me might help with that.

However, bounding boxes are coarse, so it is entirely possible that the destination lies within a bounding box. In that case, I could cast line out from the target towards the bot. If those lines escape the box without ever intersecting the object causing the box, then that is a potential route for the bot to approach the target. A single line wouldn't mean anything in this case, because there has to be a gap large enough for the bot to fit through, so I couldn't just cast one line and be done with it. Still, one line would give me a place to start, and a couple lines parallel to the first line would tell me the rest. For one thing, it would give me a point on the box boundary for which the bot should head. This would be a new target, so the bot could seek that new target, and once there, proceed to the true target. What you have given me may be useful for testing those lines out from the target to the boundary. However, since an object isn't really a set of line segments necessarily, but rather a mesh of points that are rather fuzzy, it may make more sense to make small bounding boxes out of those points, and look for the intersection of the lines with one of those boxes. That would be so coarse that the simplistic method I suggested wouldn't fail.

However, the bot will be responding to a variety of needs, one of which will be exploring the unknown, and intersecting line segments will be a big item on that list.

This is kind of a sprawling project. I started with hardware, and dropped a bundle on some sensors that proved to be far less effective than the specs suggested. I'm now trying out a different kind of sensor, but I'm also being much less optimistic. That got me thinking about how to design the bot so that it could adjust for whatever sensors it happened to have...to some extent. The reality is not going to be particularly neat. I have written the code that should allow the bot to learn how to adjust for its track motors turning at different speeds (due to differences in the resistance of the motors), and it might notice track failures, track slippage, and things like that, but the world will probably still not conform to nice neat lines when it comes to objects. More likely, there will be a very fuzzy outline of objects. The more I think about it, the less likely it seems that I should even bother trying to turn objects into sets of polygons. We may see the world that way, but the bot will probably see the world as regions of higher probability of obstacles.

Well, I'm rambling now. Thanks for the line segment stuff. I'll use it when I get around to laying out the design for getting a route to fulfill the mapping desire in the robot.





Interestingly, I believe you have thoroughly answered the next question that I haven't even got around to asking yet.

Using regions of probability is an interesting idea. Mathematically, it could get complicated quickly. I'm imagining minimizing the probability of collision over the space of paths, which is a non-trivial application of the calculus of variations. If you have more specific questions I'd be happy to think on them.

On another note, it occurred to me that I was writing v.w = L1L2 while I abstractly meant v || w. In fact, I meant v.w = +/- L1L2; I've edited my post to reflect this minor change. It would only affect the outcome of the algorithm if the two line segments were antiparallel, but regardless it's a valid edge case.

I can simplify the probability issues through behavior. One of the dreams will be training the bot to explore an object. Exactly how that will work I will not know until I see it in action. One of the keys to the learning is that it has to be too complex to comprehend. For example, just to turn involves somewhere between 50 and 400 genes, depending on how you count them. One of the neat things about GAs is that if you can evaluate the results correctly (did the GA achieve the goal), then it WILL find an excellent (though not necessarily optimal) solution. Complexity just allows for a more nuanced response to complex inputs.