//
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[]) {
}
}