Find X,Y index into 2D array/matrix using an angle and number of cells
-
Hello, I am working on a robots projects and part of this project is a local map space (2D array/matrix). This map consists of 201 x 201 1cm squares, with the robot sitting in the middle. From a robots point of view this local map has a index range of -100x -100y to +100x +100y With the robot sitting at cell 0,0, obviously from a c/c++ point of view. The array/matrix is addressed from 0x to 200x and 0y to 200y. (0x,0y being array/matrix bottom left) I have successfully implement a robot rolling road and robot array/matrix rotations. However, for some reason my brain has gone into gimble lock. Trying to figure how to find the array/matrix x,y indices for a number of cells at a given angle from the middle of the map (robots position). The code below produces perfect result for 100 cells at 0,45,90,135,180,225,270,315 degrees but at other angle is fails, can someone help please. Many thanks in advance imk #define LOCAL_MAP_NUM_X_CELLS 201 #define LOCAL_MAP_NUM_Y_CELLS 201 #define DEG_TO_RAD 0.0174533 struct LocalMap { unsigned char Cell[ LOCAL_MAP_NUM_Y_CELLS ][ LOCAL_MAP_NUM_X_CELLS ]; }; typedef struct LocalMap LocalMap; int Cell_X; int Cell_Y; ComputeLocalMapCellIntexFromCentre( 315 , 100 , &Cell_X , &Cell_Y ); int ComputeLocalMapCellIntexFromCentre( double Angle , int NumberCells , int *pCell_X , int *pCell_Y ) { double AngleAdjustMent; double Sin = sin( Angle * DEG_TO_RAD); double Cos = cos( Angle * DEG_TO_RAD); double NumCells; AngleAdjustMent = ( fabs( sin( Angle * DEG_TO_RAD)) + fabs( cos( Angle * DEG_TO_RAD )) ); NumCells = (double)NumberCells; NumCells = round( (double)NumCells * AngleAdjustMent ); NumberCells = (int)NumCells; *pCell_X = (int)round( Sin * NumberCells ) + ( LOCAL_MAP_NUM_X_CELLS - 1 ) / 2; *pCell_Y = (int)round( Cos * NumberCells ) + ( LOCAL_MAP_NUM_Y_CELLS - 1 ) / 2; return( TRUE ); }
-
Hello, I am working on a robots projects and part of this project is a local map space (2D array/matrix). This map consists of 201 x 201 1cm squares, with the robot sitting in the middle. From a robots point of view this local map has a index range of -100x -100y to +100x +100y With the robot sitting at cell 0,0, obviously from a c/c++ point of view. The array/matrix is addressed from 0x to 200x and 0y to 200y. (0x,0y being array/matrix bottom left) I have successfully implement a robot rolling road and robot array/matrix rotations. However, for some reason my brain has gone into gimble lock. Trying to figure how to find the array/matrix x,y indices for a number of cells at a given angle from the middle of the map (robots position). The code below produces perfect result for 100 cells at 0,45,90,135,180,225,270,315 degrees but at other angle is fails, can someone help please. Many thanks in advance imk #define LOCAL_MAP_NUM_X_CELLS 201 #define LOCAL_MAP_NUM_Y_CELLS 201 #define DEG_TO_RAD 0.0174533 struct LocalMap { unsigned char Cell[ LOCAL_MAP_NUM_Y_CELLS ][ LOCAL_MAP_NUM_X_CELLS ]; }; typedef struct LocalMap LocalMap; int Cell_X; int Cell_Y; ComputeLocalMapCellIntexFromCentre( 315 , 100 , &Cell_X , &Cell_Y ); int ComputeLocalMapCellIntexFromCentre( double Angle , int NumberCells , int *pCell_X , int *pCell_Y ) { double AngleAdjustMent; double Sin = sin( Angle * DEG_TO_RAD); double Cos = cos( Angle * DEG_TO_RAD); double NumCells; AngleAdjustMent = ( fabs( sin( Angle * DEG_TO_RAD)) + fabs( cos( Angle * DEG_TO_RAD )) ); NumCells = (double)NumberCells; NumCells = round( (double)NumCells * AngleAdjustMent ); NumberCells = (int)NumCells; *pCell_X = (int)round( Sin * NumberCells ) + ( LOCAL_MAP_NUM_X_CELLS - 1 ) / 2; *pCell_Y = (int)round( Cos * NumberCells ) + ( LOCAL_MAP_NUM_Y_CELLS - 1 ) / 2; return( TRUE ); }
On the run now so I don't have time for a detailed answer but Bresenham's line algorithm[^] is your friend. Edit: Here is a detailed solution. First you have to determine the end point of your line. There might be smarter ways, but a direct way to do it is this:
/* Find endpoint of line with given angle*/
void find_endpoint (double angle, int* x, int* y)
{
assert (angle >= 0 && angle < 360);
if (angle >= 315 || angle <= 45) //end point on right side
{
*x = LOCAL_MAP_NUM_X_CELLS;
*y = (int)round((1 + sin (angle * DEG_TO_RAD)) * LOCAL_MAP_NUM_Y_CELLS / 2);
}
else if (angle <= 135) //endpoint on top side
{
*y = LOCAL_MAP_NUM_Y_CELLS;
*x = (int)round((1 + cos (angle * DEG_TO_RAD)) * LOCAL_MAP_NUM_X_CELLS / 2);
}
else if (angle < 225) //endpoint of left side
{
*x = 0;
*y = (int)round((1 + sin (angle * DEG_TO_RAD)) * LOCAL_MAP_NUM_Y_CELLS / 2);
}
else //endpoint of bottom side
{
*y = 0;
*x = (int)round((1 + cos (angle * DEG_TO_RAD)) * LOCAL_MAP_NUM_X_CELLS / 2);
}
}Once you have the starting point (100, 100) and the endpoint of your line, it is simply a question of applying the Bresensham algorithm to determine the points on the line:
// a point in matrix
struct pt {
int x;
int y;
};void bresenham (int x0, int y0, int x1, int y1, std::vector& cell)
{
int dx = abs (x1 - x0);
int sx = x0 < x1 ? 1 : -1;
int dy = -abs (y1 - y0);
int sy = y0 < y1 ? 1 : -1;
int error = dx + dy;while (1)
{
cell.push_back ({ x0, y0 });
if (x0 == x1 && y0 == y1)
break;
int e2 = 2 * error;
if (e2 >= dy)
{
if (x0 == x1)
break;
error = error + dy;
x0 = x0 + sx;
}
if (e2 <= dx)
{
if (y0 == y1)
break;
error = error + dx;
y0 = y0 + sy;
}
}
}Note that this is a direct transcription of the algorithm on the Wikipedia page mentioned above. The number of cells obviously varies so the simplest for me was to keep it in a vector. If you need to have strictly C solution, you will have to manage that yourself. The caller program looks like this:
int main (int argc, char **argv)
{
int x1, y1;
std::vector cells;//First quadrant, angle < 45
find_endpoint ( -
Hello, I am working on a robots projects and part of this project is a local map space (2D array/matrix). This map consists of 201 x 201 1cm squares, with the robot sitting in the middle. From a robots point of view this local map has a index range of -100x -100y to +100x +100y With the robot sitting at cell 0,0, obviously from a c/c++ point of view. The array/matrix is addressed from 0x to 200x and 0y to 200y. (0x,0y being array/matrix bottom left) I have successfully implement a robot rolling road and robot array/matrix rotations. However, for some reason my brain has gone into gimble lock. Trying to figure how to find the array/matrix x,y indices for a number of cells at a given angle from the middle of the map (robots position). The code below produces perfect result for 100 cells at 0,45,90,135,180,225,270,315 degrees but at other angle is fails, can someone help please. Many thanks in advance imk #define LOCAL_MAP_NUM_X_CELLS 201 #define LOCAL_MAP_NUM_Y_CELLS 201 #define DEG_TO_RAD 0.0174533 struct LocalMap { unsigned char Cell[ LOCAL_MAP_NUM_Y_CELLS ][ LOCAL_MAP_NUM_X_CELLS ]; }; typedef struct LocalMap LocalMap; int Cell_X; int Cell_Y; ComputeLocalMapCellIntexFromCentre( 315 , 100 , &Cell_X , &Cell_Y ); int ComputeLocalMapCellIntexFromCentre( double Angle , int NumberCells , int *pCell_X , int *pCell_Y ) { double AngleAdjustMent; double Sin = sin( Angle * DEG_TO_RAD); double Cos = cos( Angle * DEG_TO_RAD); double NumCells; AngleAdjustMent = ( fabs( sin( Angle * DEG_TO_RAD)) + fabs( cos( Angle * DEG_TO_RAD )) ); NumCells = (double)NumberCells; NumCells = round( (double)NumCells * AngleAdjustMent ); NumberCells = (int)NumCells; *pCell_X = (int)round( Sin * NumberCells ) + ( LOCAL_MAP_NUM_X_CELLS - 1 ) / 2; *pCell_Y = (int)round( Cos * NumberCells ) + ( LOCAL_MAP_NUM_Y_CELLS - 1 ) / 2; return( TRUE ); }
Normalize the angle to <= 90 (right angle triangle); calculate the opposite and adjacent sides; offset origin x by (int) adjacent, and origin y by (int) opposite.
"Before entering on an understanding, I have meditated for a long time, and have foreseen what might happen. It is not genius which reveals to me suddenly, secretly, what I have to say or to do in a circumstance unexpected by other people; it is reflection, it is meditation." - Napoleon I
