private bool GetWedgeIntersection(int numpoints, double[] points, ref double[] newloc)
{
//double total_x = 0;
//double total_y = 0;
double x0, y0, x1, y1, x2, y2;
//double compConst = 0.01; // for comparing real numbers
double x01, y01;
//double x12, y12;
//double ax, ay, bx, by; //two intersections of two petals disks
double d01;//, d12
//double petalx0, petaly1, petaly0, petalr0, petalx1, petalr1;
//double p[5];
double[] petalx = new double[2 * numpoints];
double[] petaly = new double[2 * numpoints];
double[] petalr = new double[2 * numpoints];
double[] wedges = new double[2000];
double xmid, ymid, dist, x3, y3;
double x_1, y_1, x_2, y_2, x_3, y_3, x_4, y_4, tempx, tempy, x_5, y_5, x_6, y_6;
double ux, uy;
double[] p1 = new double[3], p2 = new double[3], p3 = new double[3], p4 = new double[3];
double[] initialConvexPoly = new double[500];
//double poly_points;
int numpolypoints = 0;
int howManyPoints = 0; // keeps the number of points used for representing the wedge
double line345 = 4.0, line789 = 4.0; // flag keeping which line to skip or construct
int numBadTriangle;
int i, j, k;
int s, flag, count, num;
int n, e;
double weight;
double petalcenterconstant, petalradiusconstant;
x0 = points[2 * numpoints - 4];
y0 = points[2 * numpoints - 3];
x1 = points[2 * numpoints - 2];
y1 = points[2 * numpoints - 1];
// minimum / maximum angle
double alpha, sinAlpha, cosAlpha, beta, sinBeta, cosBeta;
alpha = behavior.MinAngle * Math.PI / 180.0;
sinAlpha = Math.Sin(alpha);
cosAlpha = Math.Cos(alpha);
beta = behavior.MaxAngle * Math.PI / 180.0;
sinBeta = Math.Sin(beta);
cosBeta = Math.Cos(beta);
// initialize the constants
if (behavior.goodAngle == 1.0)
{
petalcenterconstant = 0;
petalradiusconstant = 0;
}
else
{
petalcenterconstant = 0.5 / Math.Tan(alpha);
petalradiusconstant = 0.5 / Math.Sin(alpha);
}
for (i = 0; i < numpoints * 2; i = i + 2)
{
// go to the next point
x2 = points[i];
y2 = points[i + 1];
// printf("POLYGON POINTS (p,q) #%d (%.12f, %.12f) (%.12f, %.12f)\n", i/2, x0, y0,x1, y1);
x01 = x1 - x0;
y01 = y1 - y0;
d01 = Math.Sqrt(x01 * x01 + y01 * y01);
// find the petal of each edge 01;
// printf("PETAL CONSTANT (%.12f, %.12f)\n",
// b.petalcenterconstant, b.petalradiusconstant );
// printf("PETAL DIFFS (%.6f, %.6f, %.4f)\n", x01, y01, d01);
//printf("i:%d numpoints:%d\n", i, numpoints);
petalx[i / 2] = x0 + 0.5 * x01 - petalcenterconstant * y01;
petaly[i / 2] = y0 + 0.5 * y01 + petalcenterconstant * x01;
petalr[i / 2] = petalradiusconstant * d01;
petalx[numpoints + i / 2] = petalx[i / 2];
petaly[numpoints + i / 2] = petaly[i / 2];
petalr[numpoints + i / 2] = petalr[i / 2];
//printf("PETAL POINTS #%d (%.12f, %.12f) R= %.12f\n", i/2, petalx[i/2],petaly[i/2], petalr[i/2]);
/// FIRST FIND THE HALF-PLANE POINTS FOR EACH PETAL
xmid = (x0 + x1) / 2.0; // mid point of pq
ymid = (y0 + y1) / 2.0;
// distance between xmid and petal center
dist = Math.Sqrt((petalx[i / 2] - xmid) * (petalx[i / 2] - xmid) + (petaly[i / 2] - ymid) * (petaly[i / 2] - ymid));
// find the unit vector goes from mid point to petal center
ux = (petalx[i / 2] - xmid) / dist;
uy = (petaly[i / 2] - ymid) / dist;
// find the third point other than p and q
x3 = petalx[i / 2] + ux * petalr[i / 2];
y3 = petaly[i / 2] + uy * petalr[i / 2];
/// FIND THE LINE POINTS BY THE ROTATION MATRIX
// cw rotation matrix [cosX sinX; -sinX cosX]
// cw rotation about (x,y) [ux*cosX + uy*sinX + x - x*cosX - y*sinX; -ux*sinX + uy*cosX + y + x*sinX - y*cosX]
// ccw rotation matrix [cosX -sinX; sinX cosX]
// ccw rotation about (x,y) [ux*cosX - uy*sinX + x - x*cosX + y*sinX; ux*sinX + uy*cosX + y - x*sinX - y*cosX]
/// LINE #1: (x1,y1) & (x_1,y_1)
// vector from p to q
ux = x1 - x0;
uy = y1 - y0;
// rotate the vector around p = (x0,y0) in ccw by alpha degrees
x_1 = x1 * cosAlpha - y1 * sinAlpha + x0 - x0 * cosAlpha + y0 * sinAlpha;
y_1 = x1 * sinAlpha + y1 * cosAlpha + y0 - x0 * sinAlpha - y0 * cosAlpha;
// add these to wedges list as lines in order
wedges[i * 20] = x0; wedges[i * 20 + 1] = y0;
wedges[i * 20 + 2] = x_1; wedges[i * 20 + 3] = y_1;
//printf("LINE #1 (%.12f, %.12f) (%.12f, %.12f)\n", x0,y0,x_1,y_1);
/// LINE #2: (x2,y2) & (x_2,y_2)
// vector from q to p
ux = x0 - x1;
uy = y0 - y1;
// rotate the vector around q = (x1,y1) in cw by alpha degrees
x_2 = x0 * cosAlpha + y0 * sinAlpha + x1 - x1 * cosAlpha - y1 * sinAlpha;
y_2 = -x0 * sinAlpha + y0 * cosAlpha + y1 + x1 * sinAlpha - y1 * cosAlpha;
// add these to wedges list as lines in order
wedges[i * 20 + 4] = x_2; wedges[i * 20 + 5] = y_2;
wedges[i * 20 + 6] = x1; wedges[i * 20 + 7] = y1;
//printf("LINE #2 (%.12f, %.12f) (%.12f, %.12f)\n", x_2,y_2,x1,y1);
// vector from (petalx, petaly) to (x3,y3)
ux = x3 - petalx[i / 2];
uy = y3 - petaly[i / 2];
tempx = x3; tempy = y3;
/// DETERMINE HOW MANY POINTS TO USE ACCORDING TO THE MINANGLE-MAXANGLE COMBINATION
// petal center angle
alpha = (2.0 * behavior.MaxAngle + behavior.MinAngle - 180.0);
if (alpha <= 0.0)
{// when only angle lines needed
// 4 point case
howManyPoints = 4;
//printf("4 point case\n");
line345 = 1.0;
line789 = 1.0;
}
else if (alpha <= 5.0)
{// when only angle lines plus two other lines are needed
// 6 point case
howManyPoints = 6;
//printf("6 point case\n");
line345 = 2.0;
line789 = 2.0;
}
else if (alpha <= 10.0)
{// when we need more lines
// 8 point case
howManyPoints = 8;
line345 = 3.0;
line789 = 3.0;
//printf("8 point case\n");
}
else
{// when we have a big wedge
// 10 point case
howManyPoints = 10;
//printf("10 point case\n");
line345 = 4.0;
line789 = 4.0;
}
alpha = alpha * Math.PI / 180.0;
/// LINE #3, #4, #5: (x3,y3) & (x_3,y_3)
for (j = 1; j < line345; j++)
{
if (line345 == 1)
continue;
// rotate the vector around (petalx,petaly) in cw by (alpha/3.0)*j degrees
x_3 = x3 * Math.Cos((alpha / (line345 - 1.0)) * j) + y3 * Math.Sin(((alpha / (line345 - 1.0)) * j)) + petalx[i / 2] - petalx[i / 2] * Math.Cos(((alpha / (line345 - 1.0)) * j)) - petaly[i / 2] * Math.Sin(((alpha / (line345 - 1.0)) * j));
y_3 = -x3 * Math.Sin(((alpha / (line345 - 1.0)) * j)) + y3 * Math.Cos(((alpha / (line345 - 1.0)) * j)) + petaly[i / 2] + petalx[i / 2] * Math.Sin(((alpha / (line345 - 1.0)) * j)) - petaly[i / 2] * Math.Cos(((alpha / (line345 - 1.0)) * j));
// add these to wedges list as lines in order
wedges[i * 20 + 8 + 4 * (j - 1)] = x_3; wedges[i * 20 + 9 + 4 * (j - 1)] = y_3;
wedges[i * 20 + 10 + 4 * (j - 1)] = tempx; wedges[i * 20 + 11 + 4 * (j - 1)] = tempy;
tempx = x_3; tempy = y_3;
}
/// LINE #6: (x2,y2) & (x_3,y_3)
// vector from q to p
ux = x0 - x1;
uy = y0 - y1;
// rotate the vector around q = (x1,y1) in cw by alpha degrees
x_5 = x0 * cosBeta + y0 * sinBeta + x1 - x1 * cosBeta - y1 * sinBeta;
y_5 = -x0 * sinBeta + y0 * cosBeta + y1 + x1 * sinBeta - y1 * cosBeta;
wedges[i * 20 + 20] = x1; wedges[i * 20 + 21] = y1;
wedges[i * 20 + 22] = x_5; wedges[i * 20 + 23] = y_5;
tempx = x3; tempy = y3;
/// LINE #7, #8, #9: (x3,y3) & (x_4,y_4)
for (j = 1; j < line789; j++)
{
if (line789 == 1)
continue;
// rotate the vector around (petalx,petaly) in ccw by (alpha/3.0)*j degrees
x_4 = x3 * Math.Cos((alpha / (line789 - 1.0)) * j) - y3 * Math.Sin((alpha / (line789 - 1.0)) * j) + petalx[i / 2] - petalx[i / 2] * Math.Cos((alpha / (line789 - 1.0)) * j) + petaly[i / 2] * Math.Sin((alpha / (line789 - 1.0)) * j);
y_4 = x3 * Math.Sin((alpha / (line789 - 1.0)) * j) + y3 * Math.Cos((alpha / (line789 - 1.0)) * j) + petaly[i / 2] - petalx[i / 2] * Math.Sin((alpha / (line789 - 1.0)) * j) - petaly[i / 2] * Math.Cos((alpha / (line789 - 1.0)) * j);
// add these to wedges list as lines in order
wedges[i * 20 + 24 + 4 * (j - 1)] = tempx; wedges[i * 20 + 25 + 4 * (j - 1)] = tempy;
wedges[i * 20 + 26 + 4 * (j - 1)] = x_4; wedges[i * 20 + 27 + 4 * (j - 1)] = y_4;
tempx = x_4; tempy = y_4;
}
/// LINE #10: (x1,y1) & (x_3,y_3)
// vector from p to q
ux = x1 - x0;
uy = y1 - y0;
// rotate the vector around p = (x0,y0) in ccw by alpha degrees
x_6 = x1 * cosBeta - y1 * sinBeta + x0 - x0 * cosBeta + y0 * sinBeta;
y_6 = x1 * sinBeta + y1 * cosBeta + y0 - x0 * sinBeta - y0 * cosBeta;
wedges[i * 20 + 36] = x_6; wedges[i * 20 + 37] = y_6;
wedges[i * 20 + 38] = x0; wedges[i * 20 + 39] = y0;
//printf("LINE #1 (%.12f, %.12f) (%.12f, %.12f)\n", x0,y0,x_1,y_1);
/// IF IT IS THE FIRST ONE, FIND THE CONVEX POLYGON
if (i == 0)
{
switch (howManyPoints)
{
case 4:
// line1 & line2 & line3 & line4
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_2, y_2, ref p1);
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_5, y_5, ref p2);
LineLineIntersection(x0, y0, x_6, y_6, x1, y1, x_5, y_5, ref p3);
LineLineIntersection(x0, y0, x_6, y_6, x1, y1, x_2, y_2, ref p4);
if ((p1[0] == 1.0) && (p2[0] == 1.0) && (p3[0] == 1.0) && (p4[0] == 1.0))
{
// #0
initialConvexPoly[0] = p1[1]; initialConvexPoly[1] = p1[2];
// #1
initialConvexPoly[2] = p2[1]; initialConvexPoly[3] = p2[2];
// #2
initialConvexPoly[4] = p3[1]; initialConvexPoly[5] = p3[2];
// #3
initialConvexPoly[6] = p4[1]; initialConvexPoly[7] = p4[2];
}
break;
case 6:
// line1 & line2 & line3
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_2, y_2, ref p1);
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_5, y_5, ref p2);
LineLineIntersection(x0, y0, x_6, y_6, x1, y1, x_2, y_2, ref p3);
if ((p1[0] == 1.0) && (p2[0] == 1.0) && (p3[0] == 1.0))
{
// #0
initialConvexPoly[0] = p1[1]; initialConvexPoly[1] = p1[2];
// #1
initialConvexPoly[2] = p2[1]; initialConvexPoly[3] = p2[2];
// #2
initialConvexPoly[4] = wedges[i * 20 + 8]; initialConvexPoly[5] = wedges[i * 20 + 9];
// #3
initialConvexPoly[6] = x3; initialConvexPoly[7] = y3;
// #4
initialConvexPoly[8] = wedges[i * 20 + 26]; initialConvexPoly[9] = wedges[i * 20 + 27];
// #5
initialConvexPoly[10] = p3[1]; initialConvexPoly[11] = p3[2];
}
break;
case 8:
// line1 & line2: p1
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_2, y_2, ref p1);
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_5, y_5, ref p2);
LineLineIntersection(x0, y0, x_6, y_6, x1, y1, x_2, y_2, ref p3);
if ((p1[0] == 1.0) && (p2[0] == 1.0) && (p3[0] == 1.0))
{
// #0
initialConvexPoly[0] = p1[1]; initialConvexPoly[1] = p1[2];
// #1
initialConvexPoly[2] = p2[1]; initialConvexPoly[3] = p2[2];
// #2
initialConvexPoly[4] = wedges[i * 20 + 12]; initialConvexPoly[5] = wedges[i * 20 + 13];
// #3
initialConvexPoly[6] = wedges[i * 20 + 8]; initialConvexPoly[7] = wedges[i * 20 + 9];
// #4
initialConvexPoly[8] = x3; initialConvexPoly[9] = y3;
// #5
initialConvexPoly[10] = wedges[i * 20 + 26]; initialConvexPoly[11] = wedges[i * 20 + 27];
// #6
initialConvexPoly[12] = wedges[i * 20 + 30]; initialConvexPoly[13] = wedges[i * 20 + 31];
// #7
initialConvexPoly[14] = p3[1]; initialConvexPoly[15] = p3[2];
}
break;
case 10:
// line1 & line2: p1
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_2, y_2, ref p1);
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_5, y_5, ref p2);
LineLineIntersection(x0, y0, x_6, y_6, x1, y1, x_2, y_2, ref p3);
//printf("p3 %f %f %f (%f %f) (%f %f) (%f %f) (%f %f)\n",p3[0],p3[1],p3[2], x0, y0, x_6, x_6, x1, y1, x_2, y_2);
if ((p1[0] == 1.0) && (p2[0] == 1.0) && (p3[0] == 1.0))
{
// #0
initialConvexPoly[0] = p1[1]; initialConvexPoly[1] = p1[2];
// #1
initialConvexPoly[2] = p2[1]; initialConvexPoly[3] = p2[2];
// #2
initialConvexPoly[4] = wedges[i * 20 + 16]; initialConvexPoly[5] = wedges[i * 20 + 17];
// #3
initialConvexPoly[6] = wedges[i * 20 + 12]; initialConvexPoly[7] = wedges[i * 20 + 13];
// #4
initialConvexPoly[8] = wedges[i * 20 + 8]; initialConvexPoly[9] = wedges[i * 20 + 9];
// #5
initialConvexPoly[10] = x3; initialConvexPoly[11] = y3;
// #6
initialConvexPoly[12] = wedges[i * 20 + 28]; initialConvexPoly[13] = wedges[i * 20 + 29];
// #7
initialConvexPoly[14] = wedges[i * 20 + 32]; initialConvexPoly[15] = wedges[i * 20 + 33];
// #8
initialConvexPoly[16] = wedges[i * 20 + 34]; initialConvexPoly[17] = wedges[i * 20 + 35];
// #9
initialConvexPoly[18] = p3[1]; initialConvexPoly[19] = p3[2];
}
break;
}
// printf("smallest edge (%f,%f) (%f,%f)\n", x0,y0, x1,y1);
// printf("real INITIAL POLY [%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;]\n", initialConvexPoly[0],initialConvexPoly[1],initialConvexPoly[2],initialConvexPoly[3],initialConvexPoly[4],initialConvexPoly[5],initialConvexPoly[6],initialConvexPoly[7],initialConvexPoly[8],initialConvexPoly[9],initialConvexPoly[10],initialConvexPoly[11],initialConvexPoly[12],initialConvexPoly[13],initialConvexPoly[14],initialConvexPoly[15],initialConvexPoly[16],initialConvexPoly[17],initialConvexPoly[18],initialConvexPoly[19]);
}
x0 = x1; y0 = y1;
x1 = x2; y1 = y2;
}
/// HALF PLANE INTERSECTION: START SPLITTING THE INITIAL POLYGON TO FIND FEASIBLE REGION
if (numpoints != 0)
{
// first intersect the opposite located ones
s = (numpoints - 1) / 2 + 1;
flag = 0;
count = 0;
i = 1;
num = howManyPoints;
for (j = 0; j < 40; j = j + 4)
{
// in order to skip non-existent lines
if (howManyPoints == 4 && (j == 8 || j == 12 || j == 16 || j == 24 || j == 28 || j == 32))
{
continue;
}
else if (howManyPoints == 6 && (j == 12 || j == 16 || j == 28 || j == 32))
{
continue;
}
else if (howManyPoints == 8 && (j == 16 || j == 32))
{
continue;
}
// printf("%d 1 INITIAL POLY [%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;]\n",num, initialConvexPoly[0],initialConvexPoly[1],initialConvexPoly[2],initialConvexPoly[3],initialConvexPoly[4],initialConvexPoly[5],initialConvexPoly[6],initialConvexPoly[7],initialConvexPoly[8],initialConvexPoly[9],initialConvexPoly[10],initialConvexPoly[11],initialConvexPoly[12],initialConvexPoly[13],initialConvexPoly[14],initialConvexPoly[15],initialConvexPoly[16],initialConvexPoly[17],initialConvexPoly[18],initialConvexPoly[19]);
// printf("line (%f, %f) (%f, %f)\n",wedges[40*s+j],wedges[40*s+1+j], wedges[40*s+2+j], wedges[40*s+3+j]);
numpolypoints = HalfPlaneIntersection(num, ref initialConvexPoly, wedges[40 * s + j], wedges[40 * s + 1 + j], wedges[40 * s + 2 + j], wedges[40 * s + 3 + j]);
if (numpolypoints == 0)
return false;
else
num = numpolypoints;
}
count++;
//printf("yes here\n");
while (count < numpoints - 1)
{
for (j = 0; j < 40; j = j + 4)
{
// in order to skip non-existent lines
if (howManyPoints == 4 && (j == 8 || j == 12 || j == 16 || j == 24 || j == 28 || j == 32))
{
continue;
}
else if (howManyPoints == 6 && (j == 12 || j == 16 || j == 28 || j == 32))
{
continue;
}
else if (howManyPoints == 8 && (j == 16 || j == 32))
{
continue;
}
////printf("%d 2 INITIAL POLY [%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;%.12f, %.12f;]\n",numpolypoints, initialConvexPoly[0],initialConvexPoly[1],initialConvexPoly[2],initialConvexPoly[3],initialConvexPoly[4],initialConvexPoly[5],initialConvexPoly[6],initialConvexPoly[7],initialConvexPoly[8],initialConvexPoly[9],initialConvexPoly[10],initialConvexPoly[11],initialConvexPoly[12],initialConvexPoly[13],initialConvexPoly[14],initialConvexPoly[15],initialConvexPoly[16],initialConvexPoly[17],initialConvexPoly[18],initialConvexPoly[19]);
//printf("line (%.20f, %.20f) (%.20f, %.20f)\n", wedges[40 * (i + s * flag) + j], wedges[40 * (i + s * flag) + 1 + j], wedges[40 * (i + s * flag) + 2 + j], wedges[40 * (i + s * flag) + 3 + j]);
numpolypoints = HalfPlaneIntersection(num, ref initialConvexPoly, wedges[40 * (i + s * flag) + j], wedges[40 * (i + s * flag) + 1 + j], wedges[40 * (i + s * flag) + 2 + j], wedges[40 * (i + s * flag) + 3 + j]);
if (numpolypoints == 0)
return false;
else
num = numpolypoints;
}
i = i + flag;
flag = (flag + 1) % 2;
count++;
}
/// IF THERE IS A FEASIBLE INTERSECTION POLYGON, FIND ITS CENTROID AS THE NEW LOCATION
FindPolyCentroid(numpolypoints, initialConvexPoly, ref newloc);
if (behavior.MaxAngle != 0.0)
{
numBadTriangle = 0;
for (j = 0; j < numpoints * 2 - 2; j = j + 2)
{
if (IsBadTriangleAngle(newloc[0], newloc[1], points[j], points[j + 1], points[j + 2], points[j + 3]))
{
numBadTriangle++;
}
}
if (IsBadTriangleAngle(newloc[0], newloc[1], points[0], points[1], points[numpoints * 2 - 2], points[numpoints * 2 - 1]))
{
numBadTriangle++;
}
if (numBadTriangle == 0)
{
return true;
}
n = (numpoints <= 2) ? 20 : 30;
// try points other than centroid
for (k = 0; k < 2 * numpoints; k = k + 2)
{
for (e = 1; e < n; e = e + 1)
{
newloc[0] = 0.0; newloc[1] = 0.0;
for (i = 0; i < 2 * numpoints; i = i + 2)
{
weight = 1.0 / numpoints;
if (i == k)
{
newloc[0] = newloc[0] + 0.1 * e * weight * points[i];
newloc[1] = newloc[1] + 0.1 * e * weight * points[i + 1];
}
else
{
weight = (1.0 - 0.1 * e * weight) / (double)(numpoints - 1.0);
newloc[0] = newloc[0] + weight * points[i];
newloc[1] = newloc[1] + weight * points[i + 1];
}
}
numBadTriangle = 0;
for (j = 0; j < numpoints * 2 - 2; j = j + 2)
{
if (IsBadTriangleAngle(newloc[0], newloc[1], points[j], points[j + 1], points[j + 2], points[j + 3]))
{
numBadTriangle++;
}
}
if (IsBadTriangleAngle(newloc[0], newloc[1], points[0], points[1], points[numpoints * 2 - 2], points[numpoints * 2 - 1]))
{
numBadTriangle++;
}
if (numBadTriangle == 0)
{
return true;
}
}
}
}
else
{
//printf("yes, we found a feasible region num: %d newloc (%.12f,%.12f)\n", numpolypoints, newloc[0], newloc[1]);
// for(i = 0; i < 2*numpolypoints; i = i+2){
// printf("point %d) (%.12f,%.12f)\n", i/2, initialConvexPoly[i], initialConvexPoly[i+1]);
// }
// printf("numpoints %d\n",numpoints);
return true;
}
}
return false;
}