I have written the following code, where I am trying to create a particle filter. The problem I am facing is that even after using intersects method and other conditions, the robot sometimes overlaps the obstacles. Moreover, it goes out of boundary through upper left corner after wobbling again and again with the corner.
I check for the collisions and then if it seems there is a collision I move the robot by amount x in the reverse direction.
I calculate amount x by the following method, where x means current position.
public void setXPosition_robot(int x)
{
double distance=0;
distance = unit_moved + randomDouble(0, forwardNoise);
robot_x= (int) (x + Math.sin(Math.toRadians(robot_orientation))*distance);
//System.out.println("Robot_X:"+robot_x);
}
I am using the following code to check the collisions:
private void adjustRobotOrientation(Graphics2D g)
{
int x=robot_x;
int y=robot_y;
if((x<0)&&(y<0))
{
robot_orientation=robot_orientation+randomDouble(160, 180);
}
if((x>620)||((y>395))||((x<1))||((y<1)))
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
}
private void collisionAvoidanceRobot(int x, int y, int r)
{
boolean collide1=false;
boolean collide2=false;
boolean collide3=false;
boolean collide4=false;
boolean collide5=false;
boolean collide6=false;
x+=unit_moved;
y+=unit_moved;
Shape collisionrobot=new Ellipse2D.Double(x,y,r,r);
collide1=collisionrobot.intersects(obst1);
if(collide1)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
collide2=collisionrobot.intersects(obst2);
if(collide2)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
collide3=collisionrobot.intersects(obst3);
if(collide3)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
collide4=collisionrobot.intersects(obst4);
if(collide4)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
collide5=collisionrobot.intersects(obst5);
if(collide5)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
collide6=collisionrobot.intersects(obst6);
if(collide6)
{
robot_orientation=robot_orientation+randomDouble(160, 220);
}
}
If you want to see, the complete code is in two files: 1. http://pastebin.com/ZYfSzptc (main class) 2. http://pastebin.com/hqTTyq5n