//
import java.util.*; public class PathFinder { public boolean debug = false; // 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() { repel(0); repel(nSteps); // DO "DIVIDE AND CONQUER" INTERPOLATION interpolate(0,nSteps); // RESAMPLE ALL PATHS TO MINIMIZE MAXIMUM STEP LENGTH // (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]); } } } // 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 // RECURSIVE DIVIDE-AND-CONQUER INTERPOLATION private 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 private 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); } private 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; } } } // LINEAR INTERPOLATION UTILITY FUNCTION private double lerp(double t,double a,double b) { return a + t * (b - a); } private int nSteps; private double radius[], X[][], Y[][], _X[][], _Y[][]; }