I have a rectangle with a given x/y pos and theta in an Cartesian coordinate system.
Given the position, length and width of the robot I want to know which coordinates are covered by the robots body so I can set them as known to be free of obstacles.
I have a solution now – but it’s not elegant:
Not the complete code but the approach should be visible:
void setRobotPosAsFree() {
float[] LeftBottomCorner = new float[2];
float[] temp = new float[2];
for (float k = RobotWidth/2 ; k >= -RobotWidth/2 ; k=k-0.25) {
LeftBottomCorner = MapOffsetPosFloat(x_pos_MAP, y_pos_MAP, k, theta - radians(-90));
LeftBottomCorner = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], RobotLength/2, theta - radians(-180));
for (int x = 0; x <= RobotLength; x++) {
temp = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], x, theta);
map[int(temp[0])][int(temp[1])] = -127;
}
}
}
float[] MapOffsetPosFloat(float x, float y, float dist, float theta) {
float[] temp = new float[2];
float Xoff = dist * cos(theta); //x
float Yoff = dist * sin(theta); //y
temp[0] = x + Xoff;
temp[1] = y - Yoff;
return temp;
}