private bool GetWedgeIntersectionWithoutMaxAngle(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, petaly0, petalr0, petalx1, petaly1, 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;
double ux, uy;
double alpha;
double[] p1 = new double[3];
double[] initialConvexPoly = new double[500];
//double poly_points;
int numpolypoints = 0;
//int numBadTriangle;
int i, j;
int s, flag, count, num;
double petalcenterconstant, petalradiusconstant;
x0 = points[2 * numpoints - 4];
y0 = points[2 * numpoints - 3];
x1 = points[2 * numpoints - 2];
y1 = points[2 * numpoints - 1];
// minimum angle
alpha = behavior.MinAngle * Math.PI / 180.0;
// 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)
{
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);
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 * Math.Cos(alpha) - y1 * Math.Sin(alpha) + x0 - x0 * Math.Cos(alpha) + y0 * Math.Sin(alpha);
y_1 = x1 * Math.Sin(alpha) + y1 * Math.Cos(alpha) + y0 - x0 * Math.Sin(alpha) - y0 * Math.Cos(alpha);
// add these to wedges list as lines in order
wedges[i * 16] = x0; wedges[i * 16 + 1] = y0;
wedges[i * 16 + 2] = x_1; wedges[i * 16 + 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 p to q
ux = x0 - x1;
uy = y0 - y1;
// rotate the vector around q = (x1,y1) in cw by alpha degrees
x_2 = x0 * Math.Cos(alpha) + y0 * Math.Sin(alpha) + x1 - x1 * Math.Cos(alpha) - y1 * Math.Sin(alpha);
y_2 = -x0 * Math.Sin(alpha) + y0 * Math.Cos(alpha) + y1 + x1 * Math.Sin(alpha) - y1 * Math.Cos(alpha);
// add these to wedges list as lines in order
wedges[i * 16 + 4] = x_2; wedges[i * 16 + 5] = y_2;
wedges[i * 16 + 6] = x1; wedges[i * 16 + 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;
/// LINE #3, #4, #5: (x3,y3) & (x_3,y_3)
for (j = 1; j < 4; j++)
{
// rotate the vector around (petalx,petaly) in cw by (60 - alpha)*j degrees
x_3 = x3 * Math.Cos((Math.PI / 3.0 - alpha) * j) + y3 * Math.Sin((Math.PI / 3.0 - alpha) * j) + petalx[i / 2] - petalx[i / 2] * Math.Cos((Math.PI / 3.0 - alpha) * j) - petaly[i / 2] * Math.Sin((Math.PI / 3.0 - alpha) * j);
y_3 = -x3 * Math.Sin((Math.PI / 3.0 - alpha) * j) + y3 * Math.Cos((Math.PI / 3.0 - alpha) * j) + petaly[i / 2] + petalx[i / 2] * Math.Sin((Math.PI / 3.0 - alpha) * j) - petaly[i / 2] * Math.Cos((Math.PI / 3.0 - alpha) * j);
// add these to wedges list as lines in order
wedges[i * 16 + 8 + 4 * (j - 1)] = x_3; wedges[i * 16 + 9 + 4 * (j - 1)] = y_3;
wedges[i * 16 + 10 + 4 * (j - 1)] = tempx; wedges[i * 16 + 11 + 4 * (j - 1)] = tempy;
tempx = x_3; tempy = y_3;
}
tempx = x3; tempy = y3;
/// LINE #6, #7, #8: (x3,y3) & (x_4,y_4)
for (j = 1; j < 4; j++)
{
// rotate the vector around (petalx,petaly) in ccw by (60 - alpha)*j degrees
x_4 = x3 * Math.Cos((Math.PI / 3.0 - alpha) * j) - y3 * Math.Sin((Math.PI / 3.0 - alpha) * j) + petalx[i / 2] - petalx[i / 2] * Math.Cos((Math.PI / 3.0 - alpha) * j) + petaly[i / 2] * Math.Sin((Math.PI / 3.0 - alpha) * j);
y_4 = x3 * Math.Sin((Math.PI / 3.0 - alpha) * j) + y3 * Math.Cos((Math.PI / 3.0 - alpha) * j) + petaly[i / 2] - petalx[i / 2] * Math.Sin((Math.PI / 3.0 - alpha) * j) - petaly[i / 2] * Math.Cos((Math.PI / 3.0 - alpha) * j);
// add these to wedges list as lines in order
wedges[i * 16 + 20 + 4 * (j - 1)] = tempx; wedges[i * 16 + 21 + 4 * (j - 1)] = tempy;
wedges[i * 16 + 22 + 4 * (j - 1)] = x_4; wedges[i * 16 + 23 + 4 * (j - 1)] = y_4;
tempx = x_4; tempy = y_4;
}
//printf("LINE #3 (%.12f, %.12f) (%.12f, %.12f)\n", x_3,y_3,x3,y3);
//printf("LINE #4 (%.12f, %.12f) (%.12f, %.12f)\n", x3,y3,x_4,y_4);
/// IF IT IS THE FIRST ONE, FIND THE CONVEX POLYGON
if (i == 0)
{
// line1 & line2: p1
LineLineIntersection(x0, y0, x_1, y_1, x1, y1, x_2, y_2, ref p1);
if ((p1[0] == 1.0))
{
// #0
initialConvexPoly[0] = p1[1]; initialConvexPoly[1] = p1[2];
// #1
initialConvexPoly[2] = wedges[i * 16 + 16]; initialConvexPoly[3] = wedges[i * 16 + 17];
// #2
initialConvexPoly[4] = wedges[i * 16 + 12]; initialConvexPoly[5] = wedges[i * 16 + 13];
// #3
initialConvexPoly[6] = wedges[i * 16 + 8]; initialConvexPoly[7] = wedges[i * 16 + 9];
// #4
initialConvexPoly[8] = x3; initialConvexPoly[9] = y3;
// #5
initialConvexPoly[10] = wedges[i * 16 + 22]; initialConvexPoly[11] = wedges[i * 16 + 23];
// #6
initialConvexPoly[12] = wedges[i * 16 + 26]; initialConvexPoly[13] = wedges[i * 16 + 27];
// #7
initialConvexPoly[14] = wedges[i * 16 + 30]; initialConvexPoly[15] = wedges[i * 16 + 31];
//printf("INITIAL POLY [%.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]);
}
}
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 = 8;
for (j = 0; j < 32; j = j + 4)
{
numpolypoints = HalfPlaneIntersection(num, ref initialConvexPoly, wedges[32 * s + j], wedges[32 * s + 1 + j], wedges[32 * s + 2 + j], wedges[32 * s + 3 + j]);
if (numpolypoints == 0)
return false;
else
num = numpolypoints;
}
count++;
while (count < numpoints - 1)
{
for (j = 0; j < 32; j = j + 4)
{
numpolypoints = HalfPlaneIntersection(num, ref initialConvexPoly, wedges[32 * (i + s * flag) + j], wedges[32 * (i + s * flag) + 1 + j], wedges[32 * (i + s * flag) + 2 + j], wedges[32 * (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.fixedArea)
{
// numBadTriangle = 0;
// for(j= 0; j < numpoints *2-2; j = j+2){
// if(testTriangleAngleArea(m,b,&newloc[0],&newloc[1], &points[j], &points[j+1], &points[j+2], &points[j+3] )){
// numBadTriangle++;
// }
// }
// if(testTriangleAngleArea(m,b, &newloc[0],&newloc[1], &points[0], &points[1], &points[numpoints*2-2], &points[numpoints*2-1] )){
// numBadTriangle++;
// }
//
// if (numBadTriangle == 0) {
//
// return 1;
// }
}
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;
}