Robotics Square Grid Intersection Point

问题

I'm trying to determine the point at which my robot will intersect with a wall given its location in a map and an angle its pointing at in radians. So to sum the problem up, given a square grid of any size [1-infinity], a object within that grid, and the angle at which that object is facing (radians), find the point of intersection with the border of the grid. For instance you have a 10 x 10 grid, your object is at position (5,5), and it is facing at an angle of pi/8 radians (Northeast direction). If this object were to move in a straight line, where would it intersect with the wall? Is there a generalized solution that would work for any position and any angle? So far what I'm doing is calculating a point outside the grid on the same trajectory and looking at all the points until I find a wall, but I feel like there is probably a more elegant solution. Thanks for the help!

回答1:

You can simply find the intersection point of two line segments.

First segment: The segment from the robot's position at the pointing angle. Make this segment larger than the diagonal of your grid to ensure that it would intersect the boundary if it was going to. Second Segment: The line segment that makes up the wall in question.

See http://www.faqs.org/faqs/graphics/algorithms-faq/ Section 1.03 for algorithm to intersect two line segments.

The comp.graphics.algorithms FAQ is a useful resource for common geometry problems in robotics.

回答2:

Pseudocode for ray with starting points inside rectangle:
Starting point (X0, Y0)
Ray angle Theta, c = Cos(Theta), s = Sin(Theta);
Rectangle coordinates: bottom left (X1,Y1), top right (X2,Y2)

if c >= 0 then //up
  XX = X2
else
  XX = X1

if s >= 0 then  //right
  YY = Y2
else
  YY = Y1

if c = 0 then //vertical ray
   return Intersection = (X0, YY)

if s = 0 then  //horizontal ray
   return Intersection = (XX, Y0)

tx = (XX - X0) / c   //parameter when vertical edge is met
ty = (YY - Y0) / s   //parameter when horizontal edge is met

if tx <= ty then  //vertical first
    return Intersection = (XX, Y0 + tx * s)
else            //horizontal first
    return  Intersection = (X0 + ty * c, YY)

来源：https://stackoverflow.com/questions/36758570/robotics-square-grid-intersection-point

标签

algorithm

math

geometry

trigonometry

robotics