#include "common/PathPlan.h"
#include "common/Solvers.h"
#include "common/BezCoeffs.h"
#define PATHDEBUGGING 1
#define EPSILON1 1E-6
#define EPSILON2 1E-6
namespace PathPlan {
struct TnA : Segment {
double t;
TnA() : Segment(Coord(),Coord()),t(0) {}
};
static bool reallyRouteSpline(const SegmentV &barriers,
const Coord *inps, int inpn, TnA *tnas, Segment endSlopes,Line &out);
static void mkSpline(const Coord *inps, int inpn, TnA *tnas, Segment endSlopes,
Coord *sp0, Coord *sv0, Coord *sp1, Coord *sv1);
static bool splineFits(const SegmentV &barriers,Coord pa, Coord va,
Coord pb, Coord vb, bool forceflag, Line &out);
static bool splineIsInside(const SegmentV &barriers, Coord sps[4]);
bool Route(const SegmentV &barriers, const Line &input,Segment endSlopes,Line &out) {
#if PATHDEBUGGING >= 1
unsigned i,j;
for(i = 0; i < barriers.size() - 1; i++)
for(j = i + 1; j < barriers.size(); j++) {
if(barriers[i].a==barriers[j].a) continue;
if(barriers[i].a==barriers[j].b) continue;
if(barriers[i].b==barriers[j].a) continue;
if(barriers[i].b==barriers[j].b) continue;
if(segsIntersect(barriers[i].a,barriers[i].b,barriers[j].a,barriers[j].b)) {
report(r_error,"input region self-intersection: (%.3lf,%.3lf),(%.3lf,%.3lf) X (%.3lf,%.3lf),(%.3lf,%.3lf)\n",
barriers[i].a.x,
barriers[i].a.y,
barriers[i].b.x,
barriers[i].b.y,
barriers[j].a.x,
barriers[j].a.y,
barriers[j].b.x,
barriers[j].b.y);
return false;
}
}
#endif
endSlopes.a = endSlopes.a.Norm();
endSlopes.b = endSlopes.b.Norm();
out.degree = 3;
out.push_back(input.front());
TnA *tnas = new TnA[input.size()];
if(!reallyRouteSpline(barriers, &input[0],input.size(), tnas, endSlopes,out)) return false;
delete [] tnas;
#if DEBUGPATH
fprintf(stderr, "%%!PS-splinomatic\n");
fprintf(stderr, "%% edges\n");
fprintf(stderr,"0 0 1 setrgbcolor\n");
for(int ipi = 0; ipi < barriers.size(); ipi++)
fprintf(stderr, "newpath %f %f moveto %f %f lineto stroke\n",
edges[ipi].a.x,edges[ipi].a.y,
edges[ipi].b.x,edges[ipi].b.y);
fprintf(stderr, "%% input path\n");
fprintf(stderr,"1 0 0 setrgbcolor\n");
fprintf(stderr,"newpath");
for(ipi = 0; ipi < input.size(); ipi++) {
fprintf(stderr, " %f %f", inps[ipi].x, inps[ipi].y);
if(ipi == 0) fprintf(stderr," moveto");
else fprintf(stderr," lineto");
}
fprintf(stderr," stroke\n");
fprintf(stderr, "%% output spline\n");
fprintf(stderr,"1 .9 .8 setrgbcolor\n");
fprintf(stderr,"newpath");
for(int opi = 0; opi < opl; opi++) {
fprintf(stderr, " %f %f", ops[opi].x, ops[opi].y);
if(opi == 0) fprintf(stderr," moveto");
else if(opi % 3 == 0) fprintf(stderr," curveto");
}
fprintf(stderr," stroke\n");
#endif
return true;
}
static bool reallyRouteSpline(const SegmentV &barriers,
const Coord *inps, int inpn, TnA *tnas, Segment endSlopes,Line &out) {
tnas[0].t = 0;
int i;
for(i = 1; i < inpn; i++)
tnas[i].t = tnas[i - 1].t + dist(inps[i], inps[i - 1]);
for(i = 1; i < inpn; i++)
tnas[i].t /= tnas[inpn - 1].t;
for(i = 0; i < inpn; i++) {
tnas[i].a = endSlopes.a*B1(tnas[i].t);
tnas[i].b = endSlopes.b*B2(tnas[i].t);
}
Coord p1,v1,p2,v2;
mkSpline(inps, inpn, tnas, endSlopes, &p1, &v1, &p2, &v2);
if(splineFits(barriers, p1, v1, p2, v2,inpn == 2,out))
return true;
Coord cp1 = p1 + v1/3.0,
cp2 = p2 - v2/3.0;
double maxd=-1;
int maxi=-1;
for(i = 1; i < inpn - 1; i++) {
double t = tnas[i].t;
Coord p = p1*B0(t) + cp1*B1(t) + cp2*B2(t) + p2*B3(t);
double d = dist(p, inps[i]);
if(d > maxd)
maxd = d, maxi = i;
}
int spliti = maxi;
Coord splitv1 = (inps[spliti]-inps[spliti - 1]).Norm(),
splitv2 = (inps[spliti + 1]-inps[spliti]).Norm(),
splitv = (splitv1+splitv2).Norm();
if(!reallyRouteSpline(barriers,inps,spliti + 1,tnas,Segment(endSlopes.a,splitv),out) ||
!reallyRouteSpline(barriers,&inps[spliti],inpn - spliti,tnas,Segment(splitv,endSlopes.b),out))
return false;
return true;
}
static void mkSpline(const Coord *inps, int inpn, TnA *tnas, Segment endSlopes,
Coord *sp0, Coord *sv0, Coord *sp1, Coord *sv1) {
const Coord &front = *inps,&back = inps[inpn-1];
double c[2][2] = {{0,0},{0,0}},
x[2] = {0,0};
for(int i = 0; i < inpn; i++) {
c[0][0] += tnas[i].a*tnas[i].a;
c[0][1] += tnas[i].a*tnas[i].b;
c[1][0] = c[0][1];
c[1][1] += tnas[i].b*tnas[i].b;
Coord tmp = inps[i]-(front*B01(tnas[i].t)+
back*B23(tnas[i].t));
x[0] += tnas[i].a*tmp;
x[1] += tnas[i].b*tmp;
}
double det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1],
det0X = c[0][0] * x[1] - c[0][1] * x[0],
detX1 = x[0] * c[1][1] - x[1] * c[0][1];
double scale0 = 0.0,
scale3 = 0.0;
if(det01) {
scale0 = detX1 / det01;
scale3 = det0X / det01;
}
if(absol(det01) < EPSILON1 || scale0 <= 0.0 || scale3 <= 0.0) {
double d01 = dist(front, back) / 3.0;
scale0 = d01;
scale3 = d01;
}
*sp0 = front;
*sv0 = endSlopes.a*scale0;
*sp1 = back;
*sv1 = endSlopes.b*scale3;
}
static bool splineFits(const SegmentV &barriers,Coord pa, Coord va,
Coord pb, Coord vb, bool forceflag, Line &out) {
double a, b;
#if 0
double d = dist(pa,pb);
a = d, b = d;
#else
a = b = 4;
#endif
while(1) {
Coord sps[4] = {pa,
pa + va*a/3.0,
pb - vb*b/3.0,
pb};
if(splineIsInside(barriers, sps)) {
out.AddSeg(sps);
report(r_splineRoute,"success: %f %f\n", a, b);
return true;
}
if(a == 0 && b == 0) {
if(forceflag) {
out.AddSeg(sps);
report(r_splineRoute,"forced straight line: %f %f\n", a, b);
return true;
}
break;
}
if(a > .01)
a /= 2, b /= 2;
else
a = b = 0;
}
#if DEBUG >= 1
fprintf(stderr, "failure\n");
#endif
return false;
}
static bool splineIsInside(const SegmentV &barriers, Coord sps[4]) {
double roots[4];
for(unsigned ei = 0; ei < barriers.size(); ei++) {
int rootn = splineIntersectsLine(sps, barriers[ei], roots);
if(rootn == 4)
continue; for(int rooti = 0; rooti < rootn; rooti++) {
if(roots[rooti] < EPSILON2 || roots[rooti] > 1 - EPSILON2)
continue;
double t = roots[rooti],
td = B3(t),
tc = B2(t),
tb = B1(t),
ta = B0(t);
Coord ip(ta * sps[0].x + tb * sps[1].x +
tc * sps[2].x + td * sps[3].x,
ta * sps[0].y + tb * sps[1].y +
tc * sps[2].y + td * sps[3].y);
if(dSquared(ip, barriers[ei].a) < EPSILON1 ||
dSquared(ip, barriers[ei].b) < EPSILON1)
continue;
return false;
}
}
return true;
}
} inline void points2coeff(double v0, double v1, double v2, double v3,double coeff[4]) {
coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
coeff[1] = 3 * (v1 - v0);
coeff[0] = v0;
}
inline int addroot(double root, double roots[4], int rootn) {
if(root >= 0 && root <= 1)
roots[rootn++] = root;
return rootn;
}
int splineIntersectsLine(const Coord *sps, Segment seg,double *roots) {
double scoeff[4], xcoeff[2], ycoeff[2];
double xroots[3], yroots[3], tv, sv, rat;
int rootn, xrootn, yrootn, i, j;
xcoeff[0] = seg.a.x;
xcoeff[1] = seg.b.x - seg.a.x;
ycoeff[0] = seg.a.y;
ycoeff[1] = seg.b.y - seg.a.y;
rootn = 0;
if(fabs(xcoeff[1])<EPSILON1) {
if(fabs(ycoeff[1])<EPSILON1) {
points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
scoeff[0] -= xcoeff[0];
xrootn = solve3(scoeff, xroots);
points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
scoeff[0] -= ycoeff[0];
yrootn = solve3(scoeff, yroots);
if(xrootn == 4)
if(yrootn == 4)
return 4;
else
for(j = 0; j < yrootn; j++)
rootn = addroot(yroots[j], roots, rootn);
else if(yrootn == 4)
for(i = 0; i < xrootn; i++)
rootn = addroot(xroots[i], roots, rootn);
else
for(i = 0; i < xrootn; i++)
for(j = 0; j < yrootn; j++)
if(xroots[i] == yroots[j])
rootn = addroot(xroots[i], roots, rootn);
return rootn;
} else {
points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
scoeff[0] -= xcoeff[0];
xrootn = solve3(scoeff, xroots);
if(xrootn == 4)
return 4;
for(i = 0; i < xrootn; i++) {
tv = xroots[i];
if(tv >= 0 && tv <= 1) {
points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y,
scoeff);
sv = scoeff[0] + tv *(scoeff[1] + tv *
(scoeff[2] + tv * scoeff[3]));
sv = (sv - ycoeff[0]) / ycoeff[1];
if((0 <= sv) && (sv <= 1))
rootn = addroot(tv, roots, rootn);
}
}
return rootn;
}
} else {
rat = ycoeff[1] / xcoeff[1];
points2coeff(sps[0].y - rat * sps[0].x, sps[1].y - rat * sps[1].x,
sps[2].y - rat * sps[2].x, sps[3].y - rat * sps[3].x, scoeff);
scoeff[0] += rat * xcoeff[0] - ycoeff[0];
xrootn = solve3(scoeff, xroots);
if(xrootn == 4)
return 4;
for(i = 0; i < xrootn; i++) {
tv = xroots[i];
if(tv >= 0 && tv <= 1) {
points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
sv = scoeff[0] + tv * (scoeff[1] + tv *
(scoeff[2] + tv * scoeff[3]));
sv = (sv - xcoeff[0]) / xcoeff[1];
if((0 <= sv) && (sv <= 1))
rootn = addroot(tv, roots, rootn);
}
}
return rootn;
}
}