For example, say I have a car and a car has a specific minimum turning radius and I want to drive that car from point a to point b, but the car isn't facing point b. How do I compute a path to point b? Being able to specify the orientation at the point b would also be good (say you want to drive to your driveway and then pull in to your garage -- it doesn't do much good if you got to your driveway by driving over your lawn and are facing sideways :)
A pointer to documentation (or even just a name) would be perfectly fine -- I'm having trouble finding anything at all.
In my attempts, they work in simple cases, but fail miserably in situations such as when point b is closer to a than the minimum turning radius.
For example, how would you determine a path similar to this (the bold path):
edit: In my actual problem, there are some simple pathing constraints, but I already have an A* algorithm in place that works, but it allows things to make instantaneous heading changes, so it looks silly seeing a car suddenly make a 90˚ turn on a dime when they get to a turn point.
Answer
I haven't worked through the full equations for this yet, but here's some visuals to help wrap our heads around the problem. It boils down to some geometry:
From any given starting point and orientation, we can draw two circles with our minimum turning radius - one on the left, one on the right. These describe the points on the tightest possible start to our path.
We can do the same for any desired end position and orientation. These circles describe the tightest possible end to our path.
Now the problem reduces to finding a path that joins one of the start circles to one of the end circles, kissing each one along its tangent.
(This is assuming we don't need to pathfind around obstacles in-between, which wasn't mentioned in the question. Stormwind's answer gets into how we can use navigation graph information for these types of problems. Once we have the sequence of nodes to pass through, we can apply the method below to each segment of the plan.)
If, for simplicity, we use straight lines, we get something like this:
This gives us the limiting case. Once you've found a path by this method, you can artificially inflate one or both start & end circles to get a less direct but smoother path, up until the point where the two circles kiss.
Computing these paths
Let's work out the cases for one turning direction - say we begin our path by turning right.
The center of our right turning circle is:
startRightCenter = carStart.position + carStart.right * minRadius
Let's call the angle of the straight section of our path (measured from the positive x-axis) pathAngle
If we draw a vector from rightCenter
to the point where we leave the turning circle (at which point we must be facing pathAngle), then that vector is...
startOffset = minRadius * (-cos(pathAngle), sin(pathAngle))
That means the point where we leave the circle must be...
departure = startRightCenter + startOffset
The point where we re-enter a turning circle depends on whether we're aiming to end with a left or a right turn:
// To end with a right turn:
reentry = endRightCenter + startOffset
// To end with a left turn: (crossover)
reentry = endLeftCenter - startOffset
Now, if we've done our job right, the line joining departure
to reentry
ought to be perpendicular to startOffset
:
dot(reentry - departure, startOffset) = 0
And solving this equation will give us the angle(s) at which this is true. (I use a plural here because technically there are two such angles, but one of them involves driving in reverse which is usually not what we want)
Let's substitute the right turn to right turn case as an example:
dot(endRightCenter + startOffset - startRightCenter - startOffset, startOffset) = 0
dot(endRightCenter - startRightCenter, startOffset) = 0
pathAngle = atan2(endRightCenter - startRightCenter)
The crossover case is more complicated - it's the one I haven't worked out all the math for yet. I'll post the answer without for now, in case it's useful to you while I work out the remaining details.
Edit: Destination inside minimum turning radius
It turns out, this method often works out-of-the box even when the destination is closer than our minimum turning distance. At least some part of one of the re-entry circles ends up outside the turn radius, letting us find a viable path as long as we don't mind it getting a bit pretzel-like...
If we don't like the path we get that way (or if one isn't feasible - I haven't checked every case exhaustively - maybe there are impossible ones), we can always drive straight forward or back until we get a suitable kissing contact between a start & end circle, as diagrammed above.
No comments:
Post a Comment