//
import java.util.*; public class PathFinder { int nSteps; double radius[], X[][], Y[][], _X[][], _Y[][]; // CREATE A NEW PATH FINDER OBJECT public PathFinder(double radius[], double X[][], double Y[][]) { nSteps = X[0].length - 1; // NUMBER OF STEPS this.radius = radius; // RADIUS OF EACH ROBOT this.X = X; // X COORD OF EACH ROBOT FOR EACH STEP this.Y = Y; // Y COORD OF EACH ROBOT FOR EACH STEP _X = new double[radius.length][nSteps+1]; // TEMPORARY WORKSPACES _Y = new double[radius.length][nSteps+1]; // FOR PATH RESAMPLING } // FIND PATHS, GIVEN START AND END PATH POSITIONS public void findPaths() { interpolate(0,nSteps); // DO "DIVIDE AND CONQUER" INTERPOLATION resample(); // RESAMPLE ALL PATHS TO MINIMIZE MAXIMUM STEP LENGTH } public void modifyPaths() { interpolateMiddle(0,nSteps, 0,nSteps); resample(); } void resample() { // (1) MAKE A TABLE OF MAXIMUM DISTANCE TRAVELED BY EACH STEP double L[] = new double[nSteps+1]; for (int i = 0 ; i < radius.length ; i++) for (int j = 1 ; j <= nSteps ; j++) { double x = X[i][j] - X[i][j-1], y = Y[i][j] - Y[i][j-1]; L[j] = Math.max(L[j], Math.sqrt(x*x + y*y)); } // (2) SUM TO MAKE A PATH-INTEGRAL; RENORMALIZE SO THAT TOTAL IS nSteps double sum = 0; for (int j = 0 ; j <= nSteps ; j++) sum += L[j]; for (int j = 1 ; j <= nSteps ; j++) L[j] = L[j-1] + L[j] * nSteps / sum; // (3) USE DISTANCES IN PATH-INTEGRAL TO RESAMPLE ALL THE ORIGINAL PATHS for (int _j = 0 ; _j <= nSteps ; _j++) for (int i = 0 ; i < radius.length ; i++) { _X[i][_j] = X[i][_j]; _Y[i][_j] = Y[i][_j]; } for (int _j = 0 ; _j < nSteps ; _j++) for (int j = (int)L[_j]+1 ; j <= (int)L[_j+1] ; j++) { double t = (j - L[_j]) / (L[_j+1] - L[_j]); for (int i = 0 ; i < radius.length ; i++) { X[i][j] = lerp(t, _X[i][_j], _X[i][_j+1]); Y[i][j] = lerp(t, _Y[i][_j], _Y[i][_j+1]); } } } // RECURSIVE DIVIDE-AND-CONQUER INTERPOLATION, WEIGHTED TOWARD THE MIDDLE void interpolateMiddle(int a, int b, int lo, int hi) { if (b - a >= 2) { // WHILE THERE ARE STEPS TO INTERPOLATE int m = (a + b) / 2; // FIND INDEX OF MIDDLE OF PATH interpolateMiddle(a,b,lo,hi,m); // FIND POSITIONS AT MIDDLE OF PATH interpolateMiddle(a,m,lo,hi); // RECURSE FOR FIRST HALF OF PATH interpolateMiddle(m,b,lo,hi); // RECURSE FOR SECOND HALF OF PATH } } void interpolateMiddle(int j0, int j1, int lo,int hi, int dst) { double t = (double)(dst - j0) / (j1 - j0); double f = (double)(dst - lo) / (hi - lo); f = 1 - 2 * Math.abs(f - .5); f *= f; for (int i = 0 ; i < radius.length ; i++) { X[i][dst] = lerp(f, X[i][dst], lerp(t, X[i][j0], X[i][j1])); Y[i][dst] = lerp(f, Y[i][dst], lerp(t, Y[i][j0], Y[i][j1])); } repel(dst); } // RECURSIVE DIVIDE-AND-CONQUER INTERPOLATION void interpolate(int a, int b) { if (b - a >= 2) { // WHILE THERE ARE STEPS TO INTERPOLATE int m = (a + b) / 2; // FIND INDEX OF MIDDLE OF PATH interpolate(a,b,m); // FIND POSITIONS AT MIDDLE OF PATH interpolate(a,m); // RECURSE FOR FIRST HALF OF PATH interpolate(m,b); // RECURSE FOR SECOND HALF OF PATH } } // LINEARLY INTERPOLATE HALF-WAY POINT OF PATH, THEN REPEL void interpolate(int j0, int j1, int dst) { double t = (double)(dst - j0) / (j1 - j0); for (int i = 0 ; i < radius.length ; i++) { X[i][dst] = lerp(t, X[i][j0], X[i][j1]); Y[i][dst] = lerp(t, Y[i][j0], Y[i][j1]); } repel(dst); } // RELAX THE PATHS, BY FIRST BLURRING AND THEN CONSTRAINING THEM public void relax() { for (int j = 1 ; j < nSteps ; j++) { for (int i = 0 ; i < radius.length ; i++) { X[i][j] = (X[i][j-1] + X[i][j+1]) / 2; Y[i][j] = (Y[i][j-1] + Y[i][j+1]) / 2; } repel(j); } for (int j = nSteps-1 ; j > 0 ; j--) { for (int i = 0 ; i < radius.length ; i++) { X[i][j] = (X[i][j-1] + X[i][j+1]) / 2; Y[i][j] = (Y[i][j-1] + Y[i][j+1]) / 2; } repel(j); } } // READJUST ROBOT POSITIONS, SO THEY PUSH EACH OTHER AWAY public void repel(int j) { Random R = new Random(); for (int k = 0 ; k < 8 ; k++) { for (int i0 = 0 ; i0 < radius.length-1 ; i0++) { for (int i1 = i0+1 ; i1 < radius.length ; i1++) { double dx = X[i1][j] - X[i0][j]; double dy = Y[i1][j] - Y[i0][j]; double r = Math.sqrt(dx*dx + dy*dy); double dr = radius[i0] + radius[i1] - r; if (dr > 0) { double theta = (Math.abs(R.nextDouble()) % 1) - .5; double sin = Math.sin(theta); double cos = Math.cos(theta); double du = (cos * dx + sin * dy) * dr / r; double dv = (cos * dy - sin * dx) * dr / r; X[i0][j] -= .5 * du; Y[i0][j] -= .5 * dv; X[i1][j] += .5 * du; Y[i1][j] += .5 * dv; } } } for (int i = 0 ; i < radius.length ; i++) { double r = radius[i]; X[i][j] = Math.max(-1 + r, Math.min(1 - r, X[i][j])); Y[i][j] = Math.max(-1 + r, Math.min(1 - r, Y[i][j])); } } } // LINEAR INTERPOLATION UTILITY FUNCTION double lerp(double t,double a,double b) { return a + t * (b - a); } ///////////// HANDLING OBSTACLES IN THE SCENE // ADD POLYGONAL OBSTACLES public void addObstacle(double X[], double Y[], int n) { } // MOVE ROBOT DISK TO THE NEAREST POINT OUTSIDE A POLYGONAL OBSTACLE void avoidObstacle(double X[], double Y[], int n, double xyr[]) { } }