Skip to content

Instantly share code, notes, and snippets.

@justcoding121

justcoding121/gist:3943773

Last active October 12, 2015 00:28
Show Gist options
  • Save justcoding121/3943773 to your computer and use it in GitHub Desktop.
Save justcoding121/3943773 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gistfile1.txt
/**
*
*
* Tested in NetBeans IDE 6.5.1 with JDK 1.6.0_17
* Generates the configuration space along with the path and saves in png format.
* Modify the variables sample_size, distance if needed
* Please replace FNAME_input and FNAME_output variables as per preffered location of input and output files.
*/
import java.io.*;
import java.util.*;
import java.awt.geom.*;
import java.awt.*;
import java.awt.image.*;
public class prm {
final static String FNAME_input = "input.txt";
final static String FNAME_output = "output";//don't add txt extension
int sample_size=5000;
int neighbour_distance=10;
double divisions=10;
public Scanner in;
public PrintWriter out;
public double MAX_VALUE=-99999999;
public boolean[][] result_matrix=new boolean[361][361];
int no_of_obstacles;
int no_of_vertices;
Point2D.Double[] vertices;
String obstacle_index_list;
int[] size_of_obstacle;
Point2D.Double base= new Point2D.Double();
Point2D.Double end1= new Point2D.Double();
Point2D.Double end2= new Point2D.Double();
Line2D.Double arm1= new Line2D.Double();
Line2D.Double arm2= new Line2D.Double();
double arm1_length;
double arm2_length;
void open() throws IOException {
in = new Scanner( new File( FNAME_input) ) ;
out = new PrintWriter( new File( FNAME_output + ".txt" ) );
no_of_obstacles = in.nextInt();
no_of_vertices=in.nextInt();
vertices = new Point2D.Double[no_of_vertices];
obstacle_index_list=new String();
obstacle_index_list=" ";
size_of_obstacle= new int[no_of_obstacles];
for(int i=0;i<no_of_vertices;i++)
{
vertices[i]= new Point2D.Double(in.nextDouble(),in.nextDouble());
}
in.nextLine();
for(int j=0;j<no_of_obstacles;j++)
{
String temp=in.nextLine().trim();
size_of_obstacle[j]=temp.split(" ").length;
obstacle_index_list=obstacle_index_list+" "+temp;
}
obstacle_index_list=obstacle_index_list.trim();
base.setLocation(in.nextDouble(), in.nextDouble());
arm1_length=in.nextDouble();
arm2_length=in.nextDouble();
//end of reading data from input file
//Initializing result matrix to zero
for(int n=0;n<361;n++)
for(int o=0;o<361;o++)
result_matrix[n][o] = false;
}
//Starting to draw obstacle lines and computing intersections
public void configuration_space() throws IOException {
int p=0,q=0;
for(double theta1=0;theta1<=2*Math.PI;theta1=theta1+Math.PI/180)
{
q=0;
for(double theta2=0;theta2<=2*Math.PI;theta2=theta2+Math.PI/180)
{
boolean ans=false;
end1.setLocation(base.x+arm1_length*Math.cos(theta1), base.y+arm1_length*Math.sin(theta1));
end2.setLocation(end1.x+arm2_length*Math.cos(theta2), end1.y+arm2_length*Math.sin(theta2));
arm1.setLine(base, end1);
arm2.setLine(end1,end2);
String[] obstacle_index= obstacle_index_list.split(" ");
int offset=0;
for(int k=0;k<no_of_obstacles;k++)
{
for(int l=offset;l<size_of_obstacle[k]+offset;l++)
for(int m=l+1;m<size_of_obstacle[k]+offset;m++)
{
Line2D.Double current_line=new Line2D.Double(vertices[Integer.parseInt(obstacle_index[l])-1],vertices[Integer.parseInt(obstacle_index[m])-1]);
if(current_line.intersectsLine(arm1)||current_line.intersectsLine(arm2))
{
ans=ans|true;
}
}
offset=offset+size_of_obstacle[k];
}
if(ans)
result_matrix[p][q]=true;
q++;
}p++;
}
}
public boolean test_sample(double theta1,double theta2)
{
theta1=Math.toRadians(theta1);
theta2=Math.toRadians(theta2);
boolean ans=false;
end1.setLocation(base.x+arm1_length*Math.cos(theta1), base.y+arm1_length*Math.sin(theta1));
end2.setLocation(end1.x+arm2_length*Math.cos(theta2), end1.y+arm2_length*Math.sin(theta2));
arm1.setLine(base, end1);
arm2.setLine(end1,end2);
String[] obstacle_index= obstacle_index_list.split(" ");
int offset=0;
for(int k=0;k<no_of_obstacles;k++)
{
for(int l=offset;l<size_of_obstacle[k]+offset;l++)
for(int m=l+1;m<size_of_obstacle[k]+offset;m++)
{
Line2D.Double current_line=new Line2D.Double(vertices[Integer.parseInt(obstacle_index[l])-1],vertices[Integer.parseInt(obstacle_index[m])-1]);
if(current_line.intersectsLine(arm1)||current_line.intersectsLine(arm2))
{
ans=ans|true;
}
}
offset=offset+size_of_obstacle[k];
}
return ans;
}
public int[][] prm_execute()
{
Graph G=new Graph();
for(int i=0;i<sample_size+2;i++)
G.addVertex(Integer.toString(i));
Point2D.Double[] node_values = new Point2D.Double[sample_size+2];
for(int i=0;i<sample_size+2;i++)
node_values[i]=new Point2D.Double(MAX_VALUE,MAX_VALUE);
int low=0;
int high=360;
Random rn = new Random();
int n= high-low;
double max_divider=Math.pow(2,divisions);
int[][] display_matrix=new int[361][361];
for(int i=0;i<sample_size;i++)
{
int randx=rn.nextInt()%n;
int randy=rn.nextInt()%n;
if(randx<0)
randx=-randx;
if(randy<0)
randy=-randy;
if(!test_sample(randx,randy))
{
for(int j=0;j<sample_size;j++)
{
boolean flag1=false,flag2=false;
Double mid_x1,mid_y1,mid_x2,mid_y2;
if(node_values[j].x!=MAX_VALUE&&node_values[j].y!=MAX_VALUE&&node_values[j].distance(randx,randy)!=0)
if(Distance(node_values[j],randx,randy,360)<neighbour_distance)
{
if(!check(randx,randy,node_values[j].x,node_values[j].y))
{
for(int divider=2;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+node_values[j].x)/divider;
mid_y1=(randy*(divider-1)+node_values[j].y)/divider;
mid_x2=(randx+node_values[j].x*(divider-1))/divider;
mid_y2=(randy+node_values[j].y*(divider-1))/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
}
}
else{
mid_x1=(randx+node_values[j].x-360)/2;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy+node_values[j].y-360)/2;
if(mid_y1<0)mid_y1=mid_y1+360;
mid_x2=(randx+node_values[j].x-360)/2;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y-360)/2;
if(mid_y2<0)mid_y2=mid_y2+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x1,mid_y1))
{
mid_x1=(randx*(divider-1)+node_values[j].x-360*(divider-1))/divider;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy*(divider-1)+node_values[j].y-360*(divider-1))/divider;
if(mid_y1<0)mid_y1=mid_y1+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
else{
for(;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+mid_x1)/divider;
mid_y1=(randy*(divider-1)+mid_y1)/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
break;
}
}
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x2,mid_y2))
{
mid_x2=(randx+node_values[j].x*(divider-1)-360*(divider-1))/divider;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y*(divider-1)-360*(divider-1))/divider;
if(mid_y2<0)mid_y2=mid_y2+360;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
else
{
for(;divider<max_divider;divider=divider*2)
{
mid_x2=(randx+mid_x2*(divider-1))/divider;
mid_y2=(randy+mid_x2*(divider-1))/divider;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
break;
}
}
}
if (!(flag1|flag2))
if(node_values[j].distance(randx,randy)!=0)
{
G.addEdge(Integer.toString(i),Integer.toString(j));
}
}
}
node_values[i].x=randx;
node_values[i].y=randy;
}
}
double randx= in.nextDouble();
double randy= in.nextDouble();
int start_pos=sample_size; int goal_pos=sample_size+1;
boolean flag1,flag2;
for(int j=0;j<sample_size;j++)
if(node_values[j].distance(randx,randy)==0)
{
start_pos=j;
break;
}
for(int j=0;j<sample_size;j++)
{
flag1=false;flag2=false;
Double mid_x1,mid_y1,mid_x2,mid_y2;
if(node_values[j].x!=MAX_VALUE&&node_values[j].y!=MAX_VALUE&&node_values[j].distance(randx,randy)!=0)
if(Distance(node_values[j],randx,randy,360)<neighbour_distance)
{
if(!check(randx,randy,node_values[j].x,node_values[j].y))
{
for(int divider=2;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+node_values[j].x)/divider;
mid_y1=(randy*(divider-1)+node_values[j].y)/divider;
mid_x2=(randx+node_values[j].x*(divider-1))/divider;
mid_y2=(randy+node_values[j].y*(divider-1))/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
}
}
else{
mid_x1=(randx+node_values[j].x-360)/2;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy+node_values[j].y-360)/2;
if(mid_y1<0)mid_y1=mid_y1+360;
mid_x2=(randx+node_values[j].x-360)/2;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y-360)/2;
if(mid_y2<0)mid_y2=mid_y2+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x1,mid_y1))
{
mid_x1=(randx*(divider-1)+node_values[j].x-360*(divider-1))/divider;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy*(divider-1)+node_values[j].y-360*(divider-1))/divider;
if(mid_y1<0)mid_y1=mid_y1+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
else{
for(;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+mid_x1)/divider;
mid_y1=(randy*(divider-1)+mid_y1)/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
break;
}
}
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x2,mid_y2))
{
mid_x2=(randx+node_values[j].x*(divider-1)-360*(divider-1))/divider;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y*(divider-1)-360*(divider-1))/divider;
if(mid_y2<0)mid_y2=mid_y2+360;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
else
{
for(;divider<max_divider;divider=divider*2)
{
mid_x2=(randx+mid_x2*(divider-1))/divider;
mid_y2=(randy+mid_x2*(divider-1))/divider;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
break;
}
}
}
if (!(flag1|flag2))
if(node_values[j].distance(randx,randy)!=0)
{
G.addEdge(Integer.toString(start_pos),Integer.toString(j));
}
}
}
node_values[start_pos].x=randx;
node_values[start_pos].y=randy;
//in.nextLine();
randx= in.nextDouble();
randy= in.nextDouble();
for(int j=0;j<sample_size;j++)
if(node_values[j].distance(randx,randy)==0)
{
goal_pos=j;
break;
}
for(int j=0;j<sample_size;j++)
{
flag1=false;flag2=false;
Double mid_x1,mid_y1,mid_x2,mid_y2;
if(node_values[j].x!=MAX_VALUE&&node_values[j].y!=MAX_VALUE&&node_values[j].distance(randx,randy)!=0)
if(Distance(node_values[j],randx,randy,360)<neighbour_distance)
{
if(!check(randx,randy,node_values[j].x,node_values[j].y))
{
for(int divider=2;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+node_values[j].x)/divider;
mid_y1=(randy*(divider-1)+node_values[j].y)/divider;
mid_x2=(randx+node_values[j].x*(divider-1))/divider;
mid_y2=(randy+node_values[j].y*(divider-1))/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
}
}
else{
mid_x1=(randx+node_values[j].x-360)/2;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy+node_values[j].y-360)/2;
if(mid_y1<0)mid_y1=mid_y1+360;
mid_x2=(randx+node_values[j].x-360)/2;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y-360)/2;
if(mid_y2<0)mid_y2=mid_y2+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
flag2=flag2|test_sample(mid_x2,mid_y2);
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x1,mid_y1))
{
mid_x1=(randx*(divider-1)+node_values[j].x-360*(divider-1))/divider;
if(mid_x1<0)mid_x1=mid_x1+360;
mid_y1=(randy*(divider-1)+node_values[j].y-360*(divider-1))/divider;
if(mid_y1<0)mid_y1=mid_y1+360;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
else{
for(;divider<max_divider;divider=divider*2)
{
mid_x1=(randx*(divider-1)+mid_x1)/divider;
mid_y1=(randy*(divider-1)+mid_y1)/divider;
flag1=flag1|test_sample(mid_x1,mid_y1);
}
break;
}
}
for(int divider=4;divider<max_divider;divider=divider*2)
{
if(check(randx,randy,mid_x2,mid_y2))
{
mid_x2=(randx+node_values[j].x*(divider-1)-360*(divider-1))/divider;
if(mid_x2<0)mid_x2=mid_x2+360;
mid_y2=(randy+node_values[j].y*(divider-1)-360*(divider-1))/divider;
if(mid_y2<0)mid_y2=mid_y2+360;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
else
{
for(;divider<max_divider;divider=divider*2)
{
mid_x2=(randx+mid_x2*(divider-1))/divider;
mid_y2=(randy+mid_x2*(divider-1))/divider;
flag2=flag2|test_sample(mid_x2,mid_y2);
}
break;
}
}
}
if (!(flag1|flag2))
if(node_values[j].distance(randx,randy)!=0)
{
G.addEdge(Integer.toString(goal_pos),Integer.toString(j));
}
}
}
node_values[goal_pos].x=randx;
node_values[goal_pos].y=randy;
for(int i=0;i<361;i++)
for(int j=0;j<361;j++)
{
if(result_matrix[i][j]==true) display_matrix[i][j]=1;
else display_matrix[i][j]=0;
}
Iterable<String> path;
PathFinder object=new PathFinder(G,Integer.toString(start_pos));
if(object.isReachable(Integer.toString(goal_pos)))
{
path=object.pathTo(Integer.toString(goal_pos));
String[] path1=path.toString().split(" ");
for(int i=0;i<path1.length;i++)
{
display_matrix[(int)node_values[Integer.parseInt(path1[i])].x][(int)node_values[Integer.parseInt(path1[i])].y]=-1;
out.println((int)node_values[Integer.parseInt(path1[i])].x+" "+(int)node_values[Integer.parseInt(path1[i])].y);
}
}
else
{
System.out.println("No path found");
out.println("No path found");
}
display_matrix[(int)node_values[start_pos].x][(int)node_values[start_pos].y]=-1;
display_matrix[(int)node_values[goal_pos].x][(int)node_values[goal_pos].y]=-1;
return display_matrix;
}
public double Distance(Point2D.Double a, double x2,double y2, int size)
{
Double x = Math.abs(x2 - a.x);
Double y = Math.abs(y2- a.y);
Double minX = Math.min(x, (size - x));
minX *= minX;
Double minY = Math.min(y, (size - y));
minY *= minY;
return Math.sqrt(minX + minY);
}
public boolean check(Double x1,Double y1,Double x2,Double y2)
{
double x=Math.abs(x1-x2);
double y= Math.abs(y1-y2);
double a=360-x;
double b=360-y;
double minX=Math.min(x, 360-x);
double minY=Math.min(y, 360-y);
if(minX==a|minY==b)
return true;
return false;
}
public boolean check(int x1,int y1,Double x2,Double y2)
{
double x=Math.abs(x1-x2);
double y= Math.abs(y1-y2);
double a=360-x;
double b=360-y;
double minX=Math.min(x, 360-x);
double minY=Math.min(y, 360-y);
if(minX==a|minY==b)
return true;
return false;
}
//write the result
void write(int result[][]) throws IOException {
int width = 360;
int height = 360;
int[] pixels = new int [width*height];
for (int j = 0; j < height; j++) {
for (int i = 0; i < width; i++) {
if (result[i][j] == 1) {
pixels[j*width + i] = Color.RED.getRGB();
}
else if(result[i][j]==0) {
pixels[j*width + i] = Color.WHITE.getRGB();
}
else pixels[j*width + i] = Color.BLUE.getRGB();
}
}
BufferedImage pixelImage = new BufferedImage(width, height, BufferedImage.TYPE_INT_RGB);
pixelImage.setRGB(0, 0, width, height, pixels, 0, width);
try {
File outputfile = new File(FNAME_output+".png");
javax.imageio.ImageIO.write(pixelImage, "png", outputfile);
}catch(IOException e){}
}
public void close() throws IOException {
out.close();
}
public static void main( String[] args ) throws IOException {
new Thread() {
@Override
public void run() {
try {
prm solution = new prm();
solution.open();
solution.configuration_space();
solution.write(solution.prm_execute());
solution.close();
} catch ( Exception e ) {
throw new RuntimeException( e );
}
}
}.start();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment