Rokojori
/
rj-action-library
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
rj-action-library/External/Triangle.NET/Triangle/NewLocation.cs

4115 lines
217 KiB
C#
Raw Permalink Blame History

// -----------------------------------------------------------------------
// <copyright file="NewLocation.cs">
// Original code by Hale Erten and Alper Üngör, http://www.cise.ufl.edu/~ungor/aCute/index.html
// Triangle.NET code by Christian Woltering, http://triangle.codeplex.com/
// </copyright>
// -----------------------------------------------------------------------
namespace TriangleNet
{
using System;
using TriangleNet.Topology;
using TriangleNet.Geometry;
using TriangleNet.Tools;
/// <summary>
/// Find new Steiner point locations.
/// </summary>
/// <remarks>
/// http://www.cise.ufl.edu/~ungor/aCute/index.html
/// </remarks>
class NewLocation
{
const double EPS = 1e-50;
IPredicates predicates;
Mesh mesh;
Behavior behavior;
// Work arrays for wegde intersection
double[] petalx = new double[20];
double[] petaly = new double[20];
double[] petalr = new double[20];
double[] wedges = new double[500];
double[] initialConvexPoly = new double[500];
// Work arrays for smoothing
double[] points_p = new double[500];
double[] points_q = new double[500];
double[] points_r = new double[500];
// Work arrays for convex polygon split
double[] poly1 = new double[100];
double[] poly2 = new double[100];
double[][] polys = new double[3][];
public NewLocation(Mesh mesh, IPredicates predicates)
{
this.mesh = mesh;
this.predicates = predicates;
this.behavior = mesh.behavior;
}
/// <summary>
/// Find a new location for a Steiner point.
/// </summary>
/// <param name="org"></param>
/// <param name="dest"></param>
/// <param name="apex"></param>
/// <param name="xi"></param>
/// <param name="eta"></param>
/// <param name="offcenter"></param>
/// <param name="badotri"></param>
/// <returns></returns>
public Point FindLocation(Vertex org, Vertex dest, Vertex apex,
ref double xi, ref double eta, bool offcenter, Otri badotri)
{
// Based on using -U switch, call the corresponding function
if (behavior.MaxAngle == 0.0)
{
// Disable the "no max angle" code. It may return weired vertex locations.
return FindNewLocationWithoutMaxAngle(org, dest, apex, ref xi, ref eta, true, badotri);
}
// With max angle
return FindNewLocation(org, dest, apex, ref xi, ref eta, true, badotri);
}
/// <summary>
/// Find a new location for a Steiner point.
/// </summary>
/// <param name="torg"></param>
/// <param name="tdest"></param>
/// <param name="tapex"></param>
/// <param name="circumcenter"></param>
/// <param name="xi"></param>
/// <param name="eta"></param>
/// <param name="offcenter"></param>
/// <param name="badotri"></param>
private Point FindNewLocationWithoutMaxAngle(Vertex torg, Vertex tdest, Vertex tapex,
ref double xi, ref double eta, bool offcenter, Otri badotri)
{
double offconstant = behavior.offconstant;
// for calculating the distances of the edges
double xdo, ydo, xao, yao, xda, yda;
double dodist, aodist, dadist;
// for exact calculation
double denominator;
double dx, dy, dxoff, dyoff;
////////////////////////////// HALE'S VARIABLES //////////////////////////////
// keeps the difference of coordinates edge
double xShortestEdge = 0, yShortestEdge = 0;
// keeps the square of edge lengths
double shortestEdgeDist = 0, middleEdgeDist = 0, longestEdgeDist = 0;
// keeps the vertices according to the angle incident to that vertex in a triangle
Point smallestAngleCorner, middleAngleCorner, largestAngleCorner;
// keeps the type of orientation if the triangle
int orientation = 0;
// keeps the coordinates of circumcenter of itself and neighbor triangle circumcenter
Point myCircumcenter, neighborCircumcenter;
// keeps if bad triangle is almost good or not
int almostGood = 0;
// keeps the cosine of the largest angle
double cosMaxAngle;
bool isObtuse; // 1: obtuse 0: nonobtuse
// keeps the radius of petal
double petalRadius;
// for calculating petal center
double xPetalCtr_1, yPetalCtr_1, xPetalCtr_2, yPetalCtr_2, xPetalCtr, yPetalCtr, xMidOfShortestEdge, yMidOfShortestEdge;
double dxcenter1, dycenter1, dxcenter2, dycenter2;
// for finding neighbor
Otri neighborotri = default(Otri);
double[] thirdPoint = new double[2];
//int neighborNotFound = -1;
bool neighborNotFound;
// for keeping the vertices of the neighbor triangle
Vertex neighborvertex_1;
Vertex neighborvertex_2;
Vertex neighborvertex_3;
// dummy variables
double xi_tmp = 0, eta_tmp = 0;
//vertex thirdVertex;
// for petal intersection
double vector_x, vector_y, xMidOfLongestEdge, yMidOfLongestEdge, inter_x, inter_y;
double[] p = new double[5], voronoiOrInter = new double[4];
bool isCorrect;
// for vector calculations in perturbation
double ax, ay, d;
double pertConst = 0.06; // perturbation constant
double lengthConst = 1; // used at comparing circumcenter's distance to proposed point's distance
double justAcute = 1; // used for making the program working for one direction only
// for smoothing
int relocated = 0;// used to differentiate between calling the deletevertex and just proposing a steiner point
double[] newloc = new double[2]; // new location suggested by smoothing
double origin_x = 0, origin_y = 0; // for keeping torg safe
Otri delotri; // keeping the original orientation for relocation process
// keeps the first and second direction suggested points
double dxFirstSuggestion, dyFirstSuggestion, dxSecondSuggestion, dySecondSuggestion;
// second direction variables
double xMidOfMiddleEdge, yMidOfMiddleEdge;
////////////////////////////// END OF HALE'S VARIABLES //////////////////////////////
Statistic.CircumcenterCount++;
// Compute the circumcenter of the triangle.
xdo = tdest.x - torg.x;
ydo = tdest.y - torg.y;
xao = tapex.x - torg.x;
yao = tapex.y - torg.y;
xda = tapex.x - tdest.x;
yda = tapex.y - tdest.y;
// keeps the square of the distances
dodist = xdo * xdo + ydo * ydo;
aodist = xao * xao + yao * yao;
dadist = (tdest.x - tapex.x) * (tdest.x - tapex.x) +
(tdest.y - tapex.y) * (tdest.y - tapex.y);
// checking if the user wanted exact arithmetic or not
if (Behavior.NoExact)
{
denominator = 0.5 / (xdo * yao - xao * ydo);
}
else
{
// Use the counterclockwise() routine to ensure a positive (and
// reasonably accurate) result, avoiding any possibility of
// division by zero.
denominator = 0.5 / predicates.CounterClockwise(tdest, tapex, torg);
// Don't count the above as an orientation test.
Statistic.CounterClockwiseCount--;
}
// calculate the circumcenter in terms of distance to origin point
dx = (yao * dodist - ydo * aodist) * denominator;
dy = (xdo * aodist - xao * dodist) * denominator;
// for debugging and for keeping circumcenter to use later
// coordinate value of the circumcenter
myCircumcenter = new Point(torg.x + dx, torg.y + dy);
delotri = badotri; // save for later
///////////////// FINDING THE ORIENTATION OF TRIANGLE //////////////////
// Find the (squared) length of the triangle's shortest edge. This
// serves as a conservative estimate of the insertion radius of the
// circumcenter's parent. The estimate is used to ensure that
// the algorithm terminates even if very small angles appear in
// the input PSLG.
// find the orientation of the triangle, basically shortest and longest edges
orientation = LongestShortestEdge(aodist, dadist, dodist);
//printf("org: (%f,%f), dest: (%f,%f), apex: (%f,%f)\n",torg[0],torg[1],tdest[0],tdest[1],tapex[0],tapex[1]);
/////////////////////////////////////////////////////////////////////////////////////////////
// 123: shortest: aodist // 213: shortest: dadist // 312: shortest: dodist //
// middle: dadist // middle: aodist // middle: aodist //
// longest: dodist // longest: dodist // longest: dadist //
// 132: shortest: aodist // 231: shortest: dadist // 321: shortest: dodist //
// middle: dodist // middle: dodist // middle: dadist //
// longest: dadist // longest: aodist // longest: aodist //
/////////////////////////////////////////////////////////////////////////////////////////////
switch (orientation)
{
case 123: // assign necessary information
/// smallest angle corner: dest
/// largest angle corner: apex
xShortestEdge = xao; yShortestEdge = yao;
shortestEdgeDist = aodist;
middleEdgeDist = dadist;
longestEdgeDist = dodist;
smallestAngleCorner = tdest;
middleAngleCorner = torg;
largestAngleCorner = tapex;
break;
case 132: // assign necessary information
/// smallest angle corner: dest
/// largest angle corner: org
xShortestEdge = xao; yShortestEdge = yao;
shortestEdgeDist = aodist;
middleEdgeDist = dodist;
longestEdgeDist = dadist;
smallestAngleCorner = tdest;
middleAngleCorner = tapex;
largestAngleCorner = torg;
break;
case 213: // assign necessary information
/// smallest angle corner: org
/// largest angle corner: apex
xShortestEdge = xda; yShortestEdge = yda;
shortestEdgeDist = dadist;
middleEdgeDist = aodist;
longestEdgeDist = dodist;
smallestAngleCorner = torg;
middleAngleCorner = tdest;
largestAngleCorner = tapex;
break;
case 231: // assign necessary information
/// smallest angle corner: org
/// largest angle corner: dest
xShortestEdge = xda; yShortestEdge = yda;
shortestEdgeDist = dadist;
middleEdgeDist = dodist;
longestEdgeDist = aodist;
smallestAngleCorner = torg;
middleAngleCorner = tapex;
largestAngleCorner = tdest;
break;
case 312: // assign necessary information
/// smallest angle corner: apex
/// largest angle corner: org
xShortestEdge = xdo; yShortestEdge = ydo;
shortestEdgeDist = dodist;
middleEdgeDist = aodist;
longestEdgeDist = dadist;
smallestAngleCorner = tapex;
middleAngleCorner = tdest;
largestAngleCorner = torg;
break;
case 321: // assign necessary information
default: // TODO: is this safe?
/// smallest angle corner: apex
/// largest angle corner: dest
xShortestEdge = xdo; yShortestEdge = ydo;
shortestEdgeDist = dodist;
middleEdgeDist = dadist;
longestEdgeDist = aodist;
smallestAngleCorner = tapex;
middleAngleCorner = torg;
largestAngleCorner = tdest;
break;
}// end of switch
// check for offcenter condition
if (offcenter && (offconstant > 0.0))
{
// origin has the smallest angle
if (orientation == 213 || orientation == 231)
{
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge - offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge + offconstant * xShortestEdge;
// If the off-center is closer to destination than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff <
(dx - xdo) * (dx - xdo) + (dy - ydo) * (dy - ydo))
{
dx = xdo + dxoff;
dy = ydo + dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
// destination has the smallest angle
}
else if (orientation == 123 || orientation == 132)
{
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge + offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge - offconstant * xShortestEdge;
// If the off-center is closer to the origin than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy)
{
dx = dxoff;
dy = dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
// apex has the smallest angle
}
else
{//orientation == 312 || orientation == 321
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge - offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge + offconstant * xShortestEdge;
// If the off-center is closer to the origin than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy)
{
dx = dxoff;
dy = dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
}
}
// if the bad triangle is almost good, apply our approach
if (almostGood == 1)
{
/// calculate cosine of largest angle ///
cosMaxAngle = (middleEdgeDist + shortestEdgeDist - longestEdgeDist) / (2 * Math.Sqrt(middleEdgeDist) * Math.Sqrt(shortestEdgeDist));
if (cosMaxAngle < 0.0)
{
// obtuse
isObtuse = true;
}
else if (Math.Abs(cosMaxAngle - 0.0) <= EPS)
{
// right triangle (largest angle is 90 degrees)
isObtuse = true;
}
else
{
// nonobtuse
isObtuse = false;
}
/// RELOCATION (LOCAL SMOOTHING) ///
/// check for possible relocation of one of triangle's points ///
relocated = DoSmoothing(delotri, torg, tdest, tapex, ref newloc);
/// if relocation is possible, delete that vertex and insert a vertex at the new location ///
if (relocated > 0)
{
Statistic.RelocationCount++;
dx = newloc[0] - torg.x;
dy = newloc[1] - torg.y;
origin_x = torg.x; // keep for later use
origin_y = torg.y;
switch (relocated)
{
case 1:
//printf("Relocate: (%f,%f)\n", torg[0],torg[1]);
mesh.DeleteVertex(ref delotri);
break;
case 2:
//printf("Relocate: (%f,%f)\n", tdest[0],tdest[1]);
delotri.Lnext();
mesh.DeleteVertex(ref delotri);
break;
case 3:
//printf("Relocate: (%f,%f)\n", tapex[0],tapex[1]);
delotri.Lprev();
mesh.DeleteVertex(ref delotri);
break;
}
}
else
{
// calculate radius of the petal according to angle constraint
// first find the visible region, PETAL
// find the center of the circle and radius
petalRadius = Math.Sqrt(shortestEdgeDist) / (2 * Math.Sin(behavior.MinAngle * Math.PI / 180.0));
/// compute two possible centers of the petal ///
// finding the center
// first find the middle point of smallest edge
xMidOfShortestEdge = (middleAngleCorner.x + largestAngleCorner.x) / 2.0;
yMidOfShortestEdge = (middleAngleCorner.y + largestAngleCorner.y) / 2.0;
// two possible centers
xPetalCtr_1 = xMidOfShortestEdge + Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (middleAngleCorner.y -
largestAngleCorner.y) / Math.Sqrt(shortestEdgeDist);
yPetalCtr_1 = yMidOfShortestEdge + Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (largestAngleCorner.x -
middleAngleCorner.x) / Math.Sqrt(shortestEdgeDist);
xPetalCtr_2 = xMidOfShortestEdge - Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (middleAngleCorner.y -
largestAngleCorner.y) / Math.Sqrt(shortestEdgeDist);
yPetalCtr_2 = yMidOfShortestEdge - Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (largestAngleCorner.x -
middleAngleCorner.x) / Math.Sqrt(shortestEdgeDist);
// find the correct circle since there will be two possible circles
// calculate the distance to smallest angle corner
dxcenter1 = (xPetalCtr_1 - smallestAngleCorner.x) * (xPetalCtr_1 - smallestAngleCorner.x);
dycenter1 = (yPetalCtr_1 - smallestAngleCorner.y) * (yPetalCtr_1 - smallestAngleCorner.y);
dxcenter2 = (xPetalCtr_2 - smallestAngleCorner.x) * (xPetalCtr_2 - smallestAngleCorner.x);
dycenter2 = (yPetalCtr_2 - smallestAngleCorner.y) * (yPetalCtr_2 - smallestAngleCorner.y);
// whichever is closer to smallest angle corner, it must be the center
if (dxcenter1 + dycenter1 <= dxcenter2 + dycenter2)
{
xPetalCtr = xPetalCtr_1; yPetalCtr = yPetalCtr_1;
}
else
{
xPetalCtr = xPetalCtr_2; yPetalCtr = yPetalCtr_2;
}
/// find the third point of the neighbor triangle ///
neighborNotFound = GetNeighborsVertex(badotri, middleAngleCorner.x, middleAngleCorner.y,
smallestAngleCorner.x, smallestAngleCorner.y, ref thirdPoint, ref neighborotri);
/// find the circumcenter of the neighbor triangle ///
dxFirstSuggestion = dx; // if we cannot find any appropriate suggestion, we use circumcenter
dyFirstSuggestion = dy;
// if there is a neighbor triangle
if (!neighborNotFound)
{
neighborvertex_1 = neighborotri.Org();
neighborvertex_2 = neighborotri.Dest();
neighborvertex_3 = neighborotri.Apex();
// now calculate neighbor's circumcenter which is the voronoi site
neighborCircumcenter = predicates.FindCircumcenter(neighborvertex_1, neighborvertex_2, neighborvertex_3,
ref xi_tmp, ref eta_tmp);
/// compute petal and Voronoi edge intersection ///
// in order to avoid degenerate cases, we need to do a vector based calculation for line
vector_x = (middleAngleCorner.y - smallestAngleCorner.y);//(-y, x)
vector_y = smallestAngleCorner.x - middleAngleCorner.x;
vector_x = myCircumcenter.x + vector_x;
vector_y = myCircumcenter.y + vector_y;
// by intersecting bisectors you will end up with the one you want to walk on
// then this line and circle should be intersected
CircleLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y,
xPetalCtr, yPetalCtr, petalRadius, ref p);
/// choose the correct intersection point ///
// calculate middle point of the longest edge(bisector)
xMidOfLongestEdge = (middleAngleCorner.x + smallestAngleCorner.x) / 2.0;
yMidOfLongestEdge = (middleAngleCorner.y + smallestAngleCorner.y) / 2.0;
// we need to find correct intersection point, since line intersects circle twice
isCorrect = ChooseCorrectPoint(xMidOfLongestEdge, yMidOfLongestEdge, p[3], p[4],
myCircumcenter.x, myCircumcenter.y, isObtuse);
// make sure which point is the correct one to be considered
if (isCorrect)
{
inter_x = p[3];
inter_y = p[4];
}
else
{
inter_x = p[1];
inter_y = p[2];
}
/// check if there is a Voronoi vertex between before intersection ///
// check if the voronoi vertex is between the intersection and circumcenter
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y,
neighborCircumcenter.x, neighborCircumcenter.y, ref voronoiOrInter);
/// determine the point to be suggested ///
if (p[0] > 0.0)
{ // there is at least one intersection point
// if it is between circumcenter and intersection
// if it returns 1.0 this means we have a voronoi vertex within feasible region
if (Math.Abs(voronoiOrInter[0] - 1.0) <= EPS)
{
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, neighborCircumcenter.x, neighborCircumcenter.y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{ // we are not creating a bad triangle
// neighbor's circumcenter is suggested
dxFirstSuggestion = voronoiOrInter[2] - torg.x;
dyFirstSuggestion = voronoiOrInter[3] - torg.y;
}
}
else
{ // there is no voronoi vertex between intersection point and circumcenter
if (IsBadTriangleAngle(largestAngleCorner.x, largestAngleCorner.y, middleAngleCorner.x, middleAngleCorner.y, inter_x, inter_y))
{
// if it is inside feasible region, then insert v2
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((inter_x - myCircumcenter.x) * (inter_x - myCircumcenter.x) +
(inter_y - myCircumcenter.y) * (inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - inter_x;
ay = myCircumcenter.y - inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
inter_x = inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
inter_y = inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{
// intersection point is suggested
dxFirstSuggestion = inter_x - torg.x;
dyFirstSuggestion = inter_y - torg.y;
}
}
else
{
// intersection point is suggested
dxFirstSuggestion = inter_x - torg.x;
dyFirstSuggestion = inter_y - torg.y;
}
}
/// if it is an acute triangle, check if it is a good enough location ///
// for acute triangle case, we need to check if it is ok to use either of them
if ((smallestAngleCorner.x - myCircumcenter.x) * (smallestAngleCorner.x - myCircumcenter.x) +
(smallestAngleCorner.y - myCircumcenter.y) * (smallestAngleCorner.y - myCircumcenter.y) >
lengthConst * ((smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y))))
{
// use circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}// else we stick to what we have found
}// intersection point
}// if it is on the boundary, meaning no neighbor triangle in this direction, try other direction
/// DO THE SAME THING FOR THE OTHER DIRECTION ///
/// find the third point of the neighbor triangle ///
neighborNotFound = GetNeighborsVertex(badotri, largestAngleCorner.x, largestAngleCorner.y,
smallestAngleCorner.x, smallestAngleCorner.y, ref thirdPoint, ref neighborotri);
/// find the circumcenter of the neighbor triangle ///
dxSecondSuggestion = dx; // if we cannot find any appropriate suggestion, we use circumcenter
dySecondSuggestion = dy;
// if there is a neighbor triangle
if (!neighborNotFound)
{
neighborvertex_1 = neighborotri.Org();
neighborvertex_2 = neighborotri.Dest();
neighborvertex_3 = neighborotri.Apex();
// now calculate neighbor's circumcenter which is the voronoi site
neighborCircumcenter = predicates.FindCircumcenter(neighborvertex_1, neighborvertex_2, neighborvertex_3,
ref xi_tmp, ref eta_tmp);
/// compute petal and Voronoi edge intersection ///
// in order to avoid degenerate cases, we need to do a vector based calculation for line
vector_x = (largestAngleCorner.y - smallestAngleCorner.y);//(-y, x)
vector_y = smallestAngleCorner.x - largestAngleCorner.x;
vector_x = myCircumcenter.x + vector_x;
vector_y = myCircumcenter.y + vector_y;
// by intersecting bisectors you will end up with the one you want to walk on
// then this line and circle should be intersected
CircleLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y,
xPetalCtr, yPetalCtr, petalRadius, ref p);
/// choose the correct intersection point ///
// calcuwedgeslate middle point of the longest edge(bisector)
xMidOfMiddleEdge = (largestAngleCorner.x + smallestAngleCorner.x) / 2.0;
yMidOfMiddleEdge = (largestAngleCorner.y + smallestAngleCorner.y) / 2.0;
// we need to find correct intersection point, since line intersects circle twice
// this direction is always ACUTE
isCorrect = ChooseCorrectPoint(xMidOfMiddleEdge, yMidOfMiddleEdge, p[3], p[4],
myCircumcenter.x, myCircumcenter.y, false/*(isObtuse+1)%2*/);
// make sure which point is the correct one to be considered
if (isCorrect)
{
inter_x = p[3];
inter_y = p[4];
}
else
{
inter_x = p[1];
inter_y = p[2];
}
/// check if there is a Voronoi vertex between before intersection ///
// check if the voronoi vertex is between the intersection and circumcenter
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y,
neighborCircumcenter.x, neighborCircumcenter.y, ref voronoiOrInter);
/// determine the point to be suggested ///
if (p[0] > 0.0)
{ // there is at least one intersection point
// if it is between circumcenter and intersection
// if it returns 1.0 this means we have a voronoi vertex within feasible region
if (Math.Abs(voronoiOrInter[0] - 1.0) <= EPS)
{
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, neighborCircumcenter.x, neighborCircumcenter.y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{ // we are not creating a bad triangle
// neighbor's circumcenter is suggested
dxSecondSuggestion = voronoiOrInter[2] - torg.x;
dySecondSuggestion = voronoiOrInter[3] - torg.y;
}
}
else
{ // there is no voronoi vertex between intersection point and circumcenter
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// if it is inside feasible region, then insert v2
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((inter_x - myCircumcenter.x) * (inter_x - myCircumcenter.x) +
(inter_y - myCircumcenter.y) * (inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - inter_x;
ay = myCircumcenter.y - inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
inter_x = inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
inter_y = inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{
// intersection point is suggested
dxSecondSuggestion = inter_x - torg.x;
dySecondSuggestion = inter_y - torg.y;
}
}
else
{
// intersection point is suggested
dxSecondSuggestion = inter_x - torg.x;
dySecondSuggestion = inter_y - torg.y;
}
}
/// if it is an acute triangle, check if it is a good enough location ///
// for acute triangle case, we need to check if it is ok to use either of them
if ((smallestAngleCorner.x - myCircumcenter.x) * (smallestAngleCorner.x - myCircumcenter.x) +
(smallestAngleCorner.y - myCircumcenter.y) * (smallestAngleCorner.y - myCircumcenter.y) >
lengthConst * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))))
{
// use circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}// else we stick on what we have found
}
}// if it is on the boundary, meaning no neighbor triangle in this direction, the other direction might be ok
if (isObtuse)
{
//obtuse: do nothing
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
else
{ // acute : consider other direction
if (justAcute * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))) >
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}// end if obtuse
}// end of relocation
}// end of almostGood
Point circumcenter = new Point();
if (relocated <= 0)
{
circumcenter.x = torg.x + dx;
circumcenter.y = torg.y + dy;
}
else
{
circumcenter.x = origin_x + dx;
circumcenter.y = origin_y + dy;
}
xi = (yao * dx - xao * dy) * (2.0 * denominator);
eta = (xdo * dy - ydo * dx) * (2.0 * denominator);
return circumcenter;
}
/// <summary>
/// Find a new location for a Steiner point.
/// </summary>
/// <param name="torg"></param>
/// <param name="tdest"></param>
/// <param name="tapex"></param>
/// <param name="circumcenter"></param>
/// <param name="xi"></param>
/// <param name="eta"></param>
/// <param name="offcenter"></param>
/// <param name="badotri"></param>
private Point FindNewLocation(Vertex torg, Vertex tdest, Vertex tapex,
ref double xi, ref double eta, bool offcenter, Otri badotri)
{
double offconstant = behavior.offconstant;
// for calculating the distances of the edges
double xdo, ydo, xao, yao, xda, yda;
double dodist, aodist, dadist;
// for exact calculation
double denominator;
double dx, dy, dxoff, dyoff;
////////////////////////////// HALE'S VARIABLES //////////////////////////////
// keeps the difference of coordinates edge
double xShortestEdge = 0, yShortestEdge = 0;
// keeps the square of edge lengths
double shortestEdgeDist = 0, middleEdgeDist = 0, longestEdgeDist = 0;
// keeps the vertices according to the angle incident to that vertex in a triangle
Point smallestAngleCorner, middleAngleCorner, largestAngleCorner;
// keeps the type of orientation if the triangle
int orientation = 0;
// keeps the coordinates of circumcenter of itself and neighbor triangle circumcenter
Point myCircumcenter, neighborCircumcenter;
// keeps if bad triangle is almost good or not
int almostGood = 0;
// keeps the cosine of the largest angle
double cosMaxAngle;
bool isObtuse; // 1: obtuse 0: nonobtuse
// keeps the radius of petal
double petalRadius;
// for calculating petal center
double xPetalCtr_1, yPetalCtr_1, xPetalCtr_2, yPetalCtr_2, xPetalCtr, yPetalCtr, xMidOfShortestEdge, yMidOfShortestEdge;
double dxcenter1, dycenter1, dxcenter2, dycenter2;
// for finding neighbor
Otri neighborotri = default(Otri);
double[] thirdPoint = new double[2];
//int neighborNotFound = -1;
// for keeping the vertices of the neighbor triangle
Vertex neighborvertex_1;
Vertex neighborvertex_2;
Vertex neighborvertex_3;
// dummy variables
double xi_tmp = 0, eta_tmp = 0;
//vertex thirdVertex;
// for petal intersection
double vector_x, vector_y, xMidOfLongestEdge, yMidOfLongestEdge, inter_x, inter_y;
double[] p = new double[5], voronoiOrInter = new double[4];
bool isCorrect;
// for vector calculations in perturbation
double ax, ay, d;
double pertConst = 0.06; // perturbation constant
double lengthConst = 1; // used at comparing circumcenter's distance to proposed point's distance
double justAcute = 1; // used for making the program working for one direction only
// for smoothing
int relocated = 0;// used to differentiate between calling the deletevertex and just proposing a steiner point
double[] newloc = new double[2]; // new location suggested by smoothing
double origin_x = 0, origin_y = 0; // for keeping torg safe
Otri delotri; // keeping the original orientation for relocation process
// keeps the first and second direction suggested points
double dxFirstSuggestion, dyFirstSuggestion, dxSecondSuggestion, dySecondSuggestion;
// second direction variables
double xMidOfMiddleEdge, yMidOfMiddleEdge;
double minangle; // in order to make sure that the circumcircle of the bad triangle is greater than petal
// for calculating the slab
double linepnt1_x, linepnt1_y, linepnt2_x, linepnt2_y; // two points of the line
double line_inter_x = 0, line_inter_y = 0;
double line_vector_x, line_vector_y;
double[] line_p = new double[3]; // used for getting the return values of functions related to line intersection
double[] line_result = new double[4];
// intersection of slab and the petal
double petal_slab_inter_x_first, petal_slab_inter_y_first, petal_slab_inter_x_second, petal_slab_inter_y_second, x_1, y_1, x_2, y_2;
double petal_bisector_x, petal_bisector_y, dist;
double alpha;
bool neighborNotFound_first;
bool neighborNotFound_second;
////////////////////////////// END OF HALE'S VARIABLES //////////////////////////////
Statistic.CircumcenterCount++;
// Compute the circumcenter of the triangle.
xdo = tdest.x - torg.x;
ydo = tdest.y - torg.y;
xao = tapex.x - torg.x;
yao = tapex.y - torg.y;
xda = tapex.x - tdest.x;
yda = tapex.y - tdest.y;
// keeps the square of the distances
dodist = xdo * xdo + ydo * ydo;
aodist = xao * xao + yao * yao;
dadist = (tdest.x - tapex.x) * (tdest.x - tapex.x) +
(tdest.y - tapex.y) * (tdest.y - tapex.y);
// checking if the user wanted exact arithmetic or not
if (Behavior.NoExact)
{
denominator = 0.5 / (xdo * yao - xao * ydo);
}
else
{
// Use the counterclockwise() routine to ensure a positive (and
// reasonably accurate) result, avoiding any possibility of
// division by zero.
denominator = 0.5 / predicates.CounterClockwise(tdest, tapex, torg);
// Don't count the above as an orientation test.
Statistic.CounterClockwiseCount--;
}
// calculate the circumcenter in terms of distance to origin point
dx = (yao * dodist - ydo * aodist) * denominator;
dy = (xdo * aodist - xao * dodist) * denominator;
// for debugging and for keeping circumcenter to use later
// coordinate value of the circumcenter
myCircumcenter = new Point(torg.x + dx, torg.y + dy);
delotri = badotri; // save for later
///////////////// FINDING THE ORIENTATION OF TRIANGLE //////////////////
// Find the (squared) length of the triangle's shortest edge. This
// serves as a conservative estimate of the insertion radius of the
// circumcenter's parent. The estimate is used to ensure that
// the algorithm terminates even if very small angles appear in
// the input PSLG.
// find the orientation of the triangle, basically shortest and longest edges
orientation = LongestShortestEdge(aodist, dadist, dodist);
//printf("org: (%f,%f), dest: (%f,%f), apex: (%f,%f)\n",torg[0],torg[1],tdest[0],tdest[1],tapex[0],tapex[1]);
/////////////////////////////////////////////////////////////////////////////////////////////
// 123: shortest: aodist // 213: shortest: dadist // 312: shortest: dodist //
// middle: dadist // middle: aodist // middle: aodist //
// longest: dodist // longest: dodist // longest: dadist //
// 132: shortest: aodist // 231: shortest: dadist // 321: shortest: dodist //
// middle: dodist // middle: dodist // middle: dadist //
// longest: dadist // longest: aodist // longest: aodist //
/////////////////////////////////////////////////////////////////////////////////////////////
switch (orientation)
{
case 123: // assign necessary information
/// smallest angle corner: dest
/// largest angle corner: apex
xShortestEdge = xao; yShortestEdge = yao;
shortestEdgeDist = aodist;
middleEdgeDist = dadist;
longestEdgeDist = dodist;
smallestAngleCorner = tdest;
middleAngleCorner = torg;
largestAngleCorner = tapex;
break;
case 132: // assign necessary information
/// smallest angle corner: dest
/// largest angle corner: org
xShortestEdge = xao; yShortestEdge = yao;
shortestEdgeDist = aodist;
middleEdgeDist = dodist;
longestEdgeDist = dadist;
smallestAngleCorner = tdest;
middleAngleCorner = tapex;
largestAngleCorner = torg;
break;
case 213: // assign necessary information
/// smallest angle corner: org
/// largest angle corner: apex
xShortestEdge = xda; yShortestEdge = yda;
shortestEdgeDist = dadist;
middleEdgeDist = aodist;
longestEdgeDist = dodist;
smallestAngleCorner = torg;
middleAngleCorner = tdest;
largestAngleCorner = tapex;
break;
case 231: // assign necessary information
/// smallest angle corner: org
/// largest angle corner: dest
xShortestEdge = xda; yShortestEdge = yda;
shortestEdgeDist = dadist;
middleEdgeDist = dodist;
longestEdgeDist = aodist;
smallestAngleCorner = torg;
middleAngleCorner = tapex;
largestAngleCorner = tdest;
break;
case 312: // assign necessary information
/// smallest angle corner: apex
/// largest angle corner: org
xShortestEdge = xdo; yShortestEdge = ydo;
shortestEdgeDist = dodist;
middleEdgeDist = aodist;
longestEdgeDist = dadist;
smallestAngleCorner = tapex;
middleAngleCorner = tdest;
largestAngleCorner = torg;
break;
case 321: // assign necessary information
default: // TODO: is this safe?
/// smallest angle corner: apex
/// largest angle corner: dest
xShortestEdge = xdo; yShortestEdge = ydo;
shortestEdgeDist = dodist;
middleEdgeDist = dadist;
longestEdgeDist = aodist;
smallestAngleCorner = tapex;
middleAngleCorner = torg;
largestAngleCorner = tdest;
break;
}// end of switch
// check for offcenter condition
if (offcenter && (offconstant > 0.0))
{
// origin has the smallest angle
if (orientation == 213 || orientation == 231)
{
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge - offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge + offconstant * xShortestEdge;
// If the off-center is closer to destination than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff <
(dx - xdo) * (dx - xdo) + (dy - ydo) * (dy - ydo))
{
dx = xdo + dxoff;
dy = ydo + dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
// destination has the smallest angle
}
else if (orientation == 123 || orientation == 132)
{
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge + offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge - offconstant * xShortestEdge;
// If the off-center is closer to the origin than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy)
{
dx = dxoff;
dy = dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
// apex has the smallest angle
}
else
{//orientation == 312 || orientation == 321
// Find the position of the off-center, as described by Alper Ungor.
dxoff = 0.5 * xShortestEdge - offconstant * yShortestEdge;
dyoff = 0.5 * yShortestEdge + offconstant * xShortestEdge;
// If the off-center is closer to the origin than the
// circumcenter, use the off-center instead.
/// doubleLY BAD CASE ///
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy)
{
dx = dxoff;
dy = dyoff;
}
/// ALMOST GOOD CASE ///
else
{
almostGood = 1;
}
}
}
// if the bad triangle is almost good, apply our approach
if (almostGood == 1)
{
/// calculate cosine of largest angle ///
cosMaxAngle = (middleEdgeDist + shortestEdgeDist - longestEdgeDist) / (2 * Math.Sqrt(middleEdgeDist) * Math.Sqrt(shortestEdgeDist));
if (cosMaxAngle < 0.0)
{
// obtuse
isObtuse = true;
}
else if (Math.Abs(cosMaxAngle - 0.0) <= EPS)
{
// right triangle (largest angle is 90 degrees)
isObtuse = true;
}
else
{
// nonobtuse
isObtuse = false;
}
/// RELOCATION (LOCAL SMOOTHING) ///
/// check for possible relocation of one of triangle's points ///
relocated = DoSmoothing(delotri, torg, tdest, tapex, ref newloc);
/// if relocation is possible, delete that vertex and insert a vertex at the new location ///
if (relocated > 0)
{
Statistic.RelocationCount++;
dx = newloc[0] - torg.x;
dy = newloc[1] - torg.y;
origin_x = torg.x; // keep for later use
origin_y = torg.y;
switch (relocated)
{
case 1:
//printf("Relocate: (%f,%f)\n", torg[0],torg[1]);
mesh.DeleteVertex(ref delotri);
break;
case 2:
//printf("Relocate: (%f,%f)\n", tdest[0],tdest[1]);
delotri.Lnext();
mesh.DeleteVertex(ref delotri);
break;
case 3:
//printf("Relocate: (%f,%f)\n", tapex[0],tapex[1]);
delotri.Lprev();
mesh.DeleteVertex(ref delotri);
break;
}
}
else
{
// calculate radius of the petal according to angle constraint
// first find the visible region, PETAL
// find the center of the circle and radius
// choose minimum angle as the maximum of quality angle and the minimum angle of the bad triangle
minangle = Math.Acos((middleEdgeDist + longestEdgeDist - shortestEdgeDist) / (2 * Math.Sqrt(middleEdgeDist) * Math.Sqrt(longestEdgeDist))) * 180.0 / Math.PI;
if (behavior.MinAngle > minangle)
{
minangle = behavior.MinAngle;
}
else
{
minangle = minangle + 0.5;
}
petalRadius = Math.Sqrt(shortestEdgeDist) / (2 * Math.Sin(minangle * Math.PI / 180.0));
/// compute two possible centers of the petal ///
// finding the center
// first find the middle point of smallest edge
xMidOfShortestEdge = (middleAngleCorner.x + largestAngleCorner.x) / 2.0;
yMidOfShortestEdge = (middleAngleCorner.y + largestAngleCorner.y) / 2.0;
// two possible centers
xPetalCtr_1 = xMidOfShortestEdge + Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (middleAngleCorner.y -
largestAngleCorner.y) / Math.Sqrt(shortestEdgeDist);
yPetalCtr_1 = yMidOfShortestEdge + Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (largestAngleCorner.x -
middleAngleCorner.x) / Math.Sqrt(shortestEdgeDist);
xPetalCtr_2 = xMidOfShortestEdge - Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (middleAngleCorner.y -
largestAngleCorner.y) / Math.Sqrt(shortestEdgeDist);
yPetalCtr_2 = yMidOfShortestEdge - Math.Sqrt(petalRadius * petalRadius - (shortestEdgeDist / 4)) * (largestAngleCorner.x -
middleAngleCorner.x) / Math.Sqrt(shortestEdgeDist);
// find the correct circle since there will be two possible circles
// calculate the distance to smallest angle corner
dxcenter1 = (xPetalCtr_1 - smallestAngleCorner.x) * (xPetalCtr_1 - smallestAngleCorner.x);
dycenter1 = (yPetalCtr_1 - smallestAngleCorner.y) * (yPetalCtr_1 - smallestAngleCorner.y);
dxcenter2 = (xPetalCtr_2 - smallestAngleCorner.x) * (xPetalCtr_2 - smallestAngleCorner.x);
dycenter2 = (yPetalCtr_2 - smallestAngleCorner.y) * (yPetalCtr_2 - smallestAngleCorner.y);
// whichever is closer to smallest angle corner, it must be the center
if (dxcenter1 + dycenter1 <= dxcenter2 + dycenter2)
{
xPetalCtr = xPetalCtr_1; yPetalCtr = yPetalCtr_1;
}
else
{
xPetalCtr = xPetalCtr_2; yPetalCtr = yPetalCtr_2;
}
/// find the third point of the neighbor triangle ///
neighborNotFound_first = GetNeighborsVertex(badotri, middleAngleCorner.x, middleAngleCorner.y,
smallestAngleCorner.x, smallestAngleCorner.y, ref thirdPoint, ref neighborotri);
/// find the circumcenter of the neighbor triangle ///
dxFirstSuggestion = dx; // if we cannot find any appropriate suggestion, we use circumcenter
dyFirstSuggestion = dy;
/// before checking the neighbor, find the petal and slab intersections ///
// calculate the intersection point of the petal and the slab lines
// first find the vector
// distance between xmid and petal center
dist = Math.Sqrt((xPetalCtr - xMidOfShortestEdge) * (xPetalCtr - xMidOfShortestEdge) + (yPetalCtr - yMidOfShortestEdge) * (yPetalCtr - yMidOfShortestEdge));
// find the unit vector goes from mid point to petal center
line_vector_x = (xPetalCtr - xMidOfShortestEdge) / dist;
line_vector_y = (yPetalCtr - yMidOfShortestEdge) / dist;
// find the third point other than p and q
petal_bisector_x = xPetalCtr + line_vector_x * petalRadius;
petal_bisector_y = yPetalCtr + line_vector_y * petalRadius;
alpha = (2.0 * behavior.MaxAngle + minangle - 180.0) * Math.PI / 180.0;
// rotate the vector cw around the petal center
x_1 = petal_bisector_x * Math.Cos(alpha) + petal_bisector_y * Math.Sin(alpha) + xPetalCtr - xPetalCtr * Math.Cos(alpha) - yPetalCtr * Math.Sin(alpha);
y_1 = -petal_bisector_x * Math.Sin(alpha) + petal_bisector_y * Math.Cos(alpha) + yPetalCtr + xPetalCtr * Math.Sin(alpha) - yPetalCtr * Math.Cos(alpha);
// rotate the vector ccw around the petal center
x_2 = petal_bisector_x * Math.Cos(alpha) - petal_bisector_y * Math.Sin(alpha) + xPetalCtr - xPetalCtr * Math.Cos(alpha) + yPetalCtr * Math.Sin(alpha);
y_2 = petal_bisector_x * Math.Sin(alpha) + petal_bisector_y * Math.Cos(alpha) + yPetalCtr - xPetalCtr * Math.Sin(alpha) - yPetalCtr * Math.Cos(alpha);
// we need to find correct intersection point, since there are two possibilities
// weather it is obtuse/acute the one closer to the minimum angle corner is the first direction
isCorrect = ChooseCorrectPoint(x_2, y_2, middleAngleCorner.x, middleAngleCorner.y, x_1, y_1, true);
// make sure which point is the correct one to be considered
if (isCorrect)
{
petal_slab_inter_x_first = x_1;
petal_slab_inter_y_first = y_1;
petal_slab_inter_x_second = x_2;
petal_slab_inter_y_second = y_2;
}
else
{
petal_slab_inter_x_first = x_2;
petal_slab_inter_y_first = y_2;
petal_slab_inter_x_second = x_1;
petal_slab_inter_y_second = y_1;
}
/// choose the correct intersection point ///
// calculate middle point of the longest edge(bisector)
xMidOfLongestEdge = (middleAngleCorner.x + smallestAngleCorner.x) / 2.0;
yMidOfLongestEdge = (middleAngleCorner.y + smallestAngleCorner.y) / 2.0;
// if there is a neighbor triangle
if (!neighborNotFound_first)
{
neighborvertex_1 = neighborotri.Org();
neighborvertex_2 = neighborotri.Dest();
neighborvertex_3 = neighborotri.Apex();
// now calculate neighbor's circumcenter which is the voronoi site
neighborCircumcenter = predicates.FindCircumcenter(neighborvertex_1, neighborvertex_2, neighborvertex_3,
ref xi_tmp, ref eta_tmp);
/// compute petal and Voronoi edge intersection ///
// in order to avoid degenerate cases, we need to do a vector based calculation for line
vector_x = (middleAngleCorner.y - smallestAngleCorner.y);//(-y, x)
vector_y = smallestAngleCorner.x - middleAngleCorner.x;
vector_x = myCircumcenter.x + vector_x;
vector_y = myCircumcenter.y + vector_y;
// by intersecting bisectors you will end up with the one you want to walk on
// then this line and circle should be intersected
CircleLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y,
xPetalCtr, yPetalCtr, petalRadius, ref p);
// we need to find correct intersection point, since line intersects circle twice
isCorrect = ChooseCorrectPoint(xMidOfLongestEdge, yMidOfLongestEdge, p[3], p[4],
myCircumcenter.x, myCircumcenter.y, isObtuse);
// make sure which point is the correct one to be considered
if (isCorrect)
{
inter_x = p[3];
inter_y = p[4];
}
else
{
inter_x = p[1];
inter_y = p[2];
}
//----------------------hale new first direction: for slab calculation---------------//
// calculate the intersection of angle lines and Voronoi
linepnt1_x = middleAngleCorner.x;
linepnt1_y = middleAngleCorner.y;
// vector from middleAngleCorner to largestAngleCorner
line_vector_x = largestAngleCorner.x - middleAngleCorner.x;
line_vector_y = largestAngleCorner.y - middleAngleCorner.y;
// rotate the vector around middleAngleCorner in cw by maxangle degrees
linepnt2_x = petal_slab_inter_x_first;
linepnt2_y = petal_slab_inter_y_first;
// now calculate the intersection of two lines
LineLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y, linepnt1_x, linepnt1_y, linepnt2_x, linepnt2_y, ref line_p);
// check if there is a suitable intersection
if (line_p[0] > 0.0)
{
line_inter_x = line_p[1];
line_inter_y = line_p[2];
}
else
{
// for debugging (to make sure)
//printf("1) No intersection between two lines!!!\n");
//printf("(%.14f,%.14f) (%.14f,%.14f) (%.14f,%.14f) (%.14f,%.14f)\n",myCircumcenter.x,myCircumcenter.y,vector_x,vector_y,linepnt1_x,linepnt1_y,linepnt2_x,linepnt2_y);
}
//---------------------------------------------------------------------//
/// check if there is a Voronoi vertex between before intersection ///
// check if the voronoi vertex is between the intersection and circumcenter
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y,
neighborCircumcenter.x, neighborCircumcenter.y, ref voronoiOrInter);
/// determine the point to be suggested ///
if (p[0] > 0.0)
{ // there is at least one intersection point
// if it is between circumcenter and intersection
// if it returns 1.0 this means we have a voronoi vertex within feasible region
if (Math.Abs(voronoiOrInter[0] - 1.0) <= EPS)
{
//-----------------hale new continues 1------------------//
// now check if the line intersection is between cc and voronoi
PointBetweenPoints(voronoiOrInter[2], voronoiOrInter[3], myCircumcenter.x, myCircumcenter.y, line_inter_x, line_inter_y, ref line_result);
if (Math.Abs(line_result[0] - 1.0) <= EPS && line_p[0] > 0.0)
{
// check if we can go further by picking the slab line and petal intersection
// calculate the distance to the smallest angle corner
// check if we create a bad triangle or not
if (((smallestAngleCorner.x - petal_slab_inter_x_first) * (smallestAngleCorner.x - petal_slab_inter_x_first) +
(smallestAngleCorner.y - petal_slab_inter_y_first) * (smallestAngleCorner.y - petal_slab_inter_y_first) >
lengthConst * ((smallestAngleCorner.x - line_inter_x) *
(smallestAngleCorner.x - line_inter_x) +
(smallestAngleCorner.y - line_inter_y) *
(smallestAngleCorner.y - line_inter_y)))
&& (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, petal_slab_inter_x_first, petal_slab_inter_y_first))
&& MinDistanceToNeighbor(petal_slab_inter_x_first, petal_slab_inter_y_first, ref neighborotri) > MinDistanceToNeighbor(line_inter_x, line_inter_y, ref neighborotri))
{
// check the neighbor's vertices also, which one if better
//slab and petal intersection is advised
dxFirstSuggestion = petal_slab_inter_x_first - torg.x;
dyFirstSuggestion = petal_slab_inter_y_first - torg.y;
}
else
{ // slab intersection point is further away
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((line_inter_x - myCircumcenter.x) * (line_inter_x - myCircumcenter.x) +
(line_inter_y - myCircumcenter.y) * (line_inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - line_inter_x;
ay = myCircumcenter.y - line_inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
line_inter_x = line_inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
line_inter_y = line_inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{
// intersection point is suggested
dxFirstSuggestion = line_inter_x - torg.x;
dyFirstSuggestion = line_inter_y - torg.y;
}
}
else
{// we are not creating a bad triangle
// slab intersection is advised
dxFirstSuggestion = line_result[2] - torg.x;
dyFirstSuggestion = line_result[3] - torg.y;
}
}
//------------------------------------------------------//
}
else
{
/// NOW APPLY A BREADTH-FIRST SEARCH ON THE VORONOI
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, neighborCircumcenter.x, neighborCircumcenter.y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{
// we are not creating a bad triangle
// neighbor's circumcenter is suggested
dxFirstSuggestion = voronoiOrInter[2] - torg.x;
dyFirstSuggestion = voronoiOrInter[3] - torg.y;
}
}
}
else
{ // there is no voronoi vertex between intersection point and circumcenter
//-----------------hale new continues 2-----------------//
// now check if the line intersection is between cc and intersection point
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y, line_inter_x, line_inter_y, ref line_result);
if (Math.Abs(line_result[0] - 1.0) <= EPS && line_p[0] > 0.0)
{
// check if we can go further by picking the slab line and petal intersection
// calculate the distance to the smallest angle corner
if (((smallestAngleCorner.x - petal_slab_inter_x_first) * (smallestAngleCorner.x - petal_slab_inter_x_first) +
(smallestAngleCorner.y - petal_slab_inter_y_first) * (smallestAngleCorner.y - petal_slab_inter_y_first) >
lengthConst * ((smallestAngleCorner.x - line_inter_x) *
(smallestAngleCorner.x - line_inter_x) +
(smallestAngleCorner.y - line_inter_y) *
(smallestAngleCorner.y - line_inter_y)))
&& (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, petal_slab_inter_x_first, petal_slab_inter_y_first))
&& MinDistanceToNeighbor(petal_slab_inter_x_first, petal_slab_inter_y_first, ref neighborotri) > MinDistanceToNeighbor(line_inter_x, line_inter_y, ref neighborotri))
{
//slab and petal intersection is advised
dxFirstSuggestion = petal_slab_inter_x_first - torg.x;
dyFirstSuggestion = petal_slab_inter_y_first - torg.y;
}
else
{ // slab intersection point is further away
if (IsBadTriangleAngle(largestAngleCorner.x, largestAngleCorner.y, middleAngleCorner.x, middleAngleCorner.y, line_inter_x, line_inter_y))
{
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((line_inter_x - myCircumcenter.x) * (line_inter_x - myCircumcenter.x) +
(line_inter_y - myCircumcenter.y) * (line_inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - line_inter_x;
ay = myCircumcenter.y - line_inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
line_inter_x = line_inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
line_inter_y = line_inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{
// intersection point is suggested
dxFirstSuggestion = line_inter_x - torg.x;
dyFirstSuggestion = line_inter_y - torg.y;
}
}
else
{// we are not creating a bad triangle
// slab intersection is advised
dxFirstSuggestion = line_result[2] - torg.x;
dyFirstSuggestion = line_result[3] - torg.y;
}
}
//------------------------------------------------------//
}
else
{
if (IsBadTriangleAngle(largestAngleCorner.x, largestAngleCorner.y, middleAngleCorner.x, middleAngleCorner.y, inter_x, inter_y))
{
//printf("testtriangle returned false! bad triangle\n");
// if it is inside feasible region, then insert v2
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((inter_x - myCircumcenter.x) * (inter_x - myCircumcenter.x) +
(inter_y - myCircumcenter.y) * (inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - inter_x;
ay = myCircumcenter.y - inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
inter_x = inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
inter_y = inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// go back to circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}
else
{
// intersection point is suggested
dxFirstSuggestion = inter_x - torg.x;
dyFirstSuggestion = inter_y - torg.y;
}
}
else
{
// intersection point is suggested
dxFirstSuggestion = inter_x - torg.x;
dyFirstSuggestion = inter_y - torg.y;
}
}
}
/// if it is an acute triangle, check if it is a good enough location ///
// for acute triangle case, we need to check if it is ok to use either of them
if ((smallestAngleCorner.x - myCircumcenter.x) * (smallestAngleCorner.x - myCircumcenter.x) +
(smallestAngleCorner.y - myCircumcenter.y) * (smallestAngleCorner.y - myCircumcenter.y) >
lengthConst * ((smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y))))
{
// use circumcenter
dxFirstSuggestion = dx;
dyFirstSuggestion = dy;
}// else we stick to what we have found
}// intersection point
}// if it is on the boundary, meaning no neighbor triangle in this direction, try other direction
/// DO THE SAME THING FOR THE OTHER DIRECTION ///
/// find the third point of the neighbor triangle ///
neighborNotFound_second = GetNeighborsVertex(badotri, largestAngleCorner.x, largestAngleCorner.y,
smallestAngleCorner.x, smallestAngleCorner.y, ref thirdPoint, ref neighborotri);
/// find the circumcenter of the neighbor triangle ///
dxSecondSuggestion = dx; // if we cannot find any appropriate suggestion, we use circumcenter
dySecondSuggestion = dy;
/// choose the correct intersection point ///
// calculate middle point of the longest edge(bisector)
xMidOfMiddleEdge = (largestAngleCorner.x + smallestAngleCorner.x) / 2.0;
yMidOfMiddleEdge = (largestAngleCorner.y + smallestAngleCorner.y) / 2.0;
// if there is a neighbor triangle
if (!neighborNotFound_second)
{
neighborvertex_1 = neighborotri.Org();
neighborvertex_2 = neighborotri.Dest();
neighborvertex_3 = neighborotri.Apex();
// now calculate neighbor's circumcenter which is the voronoi site
neighborCircumcenter = predicates.FindCircumcenter(neighborvertex_1, neighborvertex_2, neighborvertex_3,
ref xi_tmp, ref eta_tmp);
/// compute petal and Voronoi edge intersection ///
// in order to avoid degenerate cases, we need to do a vector based calculation for line
vector_x = (largestAngleCorner.y - smallestAngleCorner.y);//(-y, x)
vector_y = smallestAngleCorner.x - largestAngleCorner.x;
vector_x = myCircumcenter.x + vector_x;
vector_y = myCircumcenter.y + vector_y;
// by intersecting bisectors you will end up with the one you want to walk on
// then this line and circle should be intersected
CircleLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y,
xPetalCtr, yPetalCtr, petalRadius, ref p);
// we need to find correct intersection point, since line intersects circle twice
// this direction is always ACUTE
isCorrect = ChooseCorrectPoint(xMidOfMiddleEdge, yMidOfMiddleEdge, p[3], p[4],
myCircumcenter.x, myCircumcenter.y, false/*(isObtuse+1)%2*/);
// make sure which point is the correct one to be considered
if (isCorrect)
{
inter_x = p[3];
inter_y = p[4];
}
else
{
inter_x = p[1];
inter_y = p[2];
}
//----------------------hale new second direction:for slab calculation---------------//
// calculate the intersection of angle lines and Voronoi
linepnt1_x = largestAngleCorner.x;
linepnt1_y = largestAngleCorner.y;
// vector from largestAngleCorner to middleAngleCorner
line_vector_x = middleAngleCorner.x - largestAngleCorner.x;
line_vector_y = middleAngleCorner.y - largestAngleCorner.y;
// rotate the vector around largestAngleCorner in ccw by maxangle degrees
linepnt2_x = petal_slab_inter_x_second;
linepnt2_y = petal_slab_inter_y_second;
// now calculate the intersection of two lines
LineLineIntersection(myCircumcenter.x, myCircumcenter.y, vector_x, vector_y, linepnt1_x, linepnt1_y, linepnt2_x, linepnt2_y, ref line_p);
// check if there is a suitable intersection
if (line_p[0] > 0.0)
{
line_inter_x = line_p[1];
line_inter_y = line_p[2];
}
else
{
// for debugging (to make sure)
//printf("1) No intersection between two lines!!!\n");
//printf("(%.14f,%.14f) (%.14f,%.14f) (%.14f,%.14f) (%.14f,%.14f)\n",myCircumcenter.x,myCircumcenter.y,vector_x,vector_y,linepnt1_x,linepnt1_y,linepnt2_x,linepnt2_y);
}
//---------------------------------------------------------------------//
/// check if there is a Voronoi vertex between before intersection ///
// check if the voronoi vertex is between the intersection and circumcenter
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y,
neighborCircumcenter.x, neighborCircumcenter.y, ref voronoiOrInter);
/// determine the point to be suggested ///
if (p[0] > 0.0)
{ // there is at least one intersection point
// if it is between circumcenter and intersection
// if it returns 1.0 this means we have a voronoi vertex within feasible region
if (Math.Abs(voronoiOrInter[0] - 1.0) <= EPS)
{
//-----------------hale new continues 1------------------//
// now check if the line intersection is between cc and voronoi
PointBetweenPoints(voronoiOrInter[2], voronoiOrInter[3], myCircumcenter.x, myCircumcenter.y, line_inter_x, line_inter_y, ref line_result);
if (Math.Abs(line_result[0] - 1.0) <= EPS && line_p[0] > 0.0)
{
// check if we can go further by picking the slab line and petal intersection
// calculate the distance to the smallest angle corner
//
if (((smallestAngleCorner.x - petal_slab_inter_x_second) * (smallestAngleCorner.x - petal_slab_inter_x_second) +
(smallestAngleCorner.y - petal_slab_inter_y_second) * (smallestAngleCorner.y - petal_slab_inter_y_second) >
lengthConst * ((smallestAngleCorner.x - line_inter_x) *
(smallestAngleCorner.x - line_inter_x) +
(smallestAngleCorner.y - line_inter_y) *
(smallestAngleCorner.y - line_inter_y)))
&& (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, petal_slab_inter_x_second, petal_slab_inter_y_second))
&& MinDistanceToNeighbor(petal_slab_inter_x_second, petal_slab_inter_y_second, ref neighborotri) > MinDistanceToNeighbor(line_inter_x, line_inter_y, ref neighborotri))
{
// slab and petal intersection is advised
dxSecondSuggestion = petal_slab_inter_x_second - torg.x;
dySecondSuggestion = petal_slab_inter_y_second - torg.y;
}
else
{ // slab intersection point is further away
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((line_inter_x - myCircumcenter.x) * (line_inter_x - myCircumcenter.x) +
(line_inter_y - myCircumcenter.y) * (line_inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - line_inter_x;
ay = myCircumcenter.y - line_inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
line_inter_x = line_inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
line_inter_y = line_inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{
// intersection point is suggested
dxSecondSuggestion = line_inter_x - torg.x;
dySecondSuggestion = line_inter_y - torg.y;
}
}
else
{// we are not creating a bad triangle
// slab intersection is advised
dxSecondSuggestion = line_result[2] - torg.x;
dySecondSuggestion = line_result[3] - torg.y;
}
}
//------------------------------------------------------//
}
else
{
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, neighborCircumcenter.x, neighborCircumcenter.y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{ // we are not creating a bad triangle
// neighbor's circumcenter is suggested
dxSecondSuggestion = voronoiOrInter[2] - torg.x;
dySecondSuggestion = voronoiOrInter[3] - torg.y;
}
}
}
else
{ // there is no voronoi vertex between intersection point and circumcenter
//-----------------hale new continues 2-----------------//
// now check if the line intersection is between cc and intersection point
PointBetweenPoints(inter_x, inter_y, myCircumcenter.x, myCircumcenter.y, line_inter_x, line_inter_y, ref line_result);
if (Math.Abs(line_result[0] - 1.0) <= EPS && line_p[0] > 0.0)
{
// check if we can go further by picking the slab line and petal intersection
// calculate the distance to the smallest angle corner
if (((smallestAngleCorner.x - petal_slab_inter_x_second) * (smallestAngleCorner.x - petal_slab_inter_x_second) +
(smallestAngleCorner.y - petal_slab_inter_y_second) * (smallestAngleCorner.y - petal_slab_inter_y_second) >
lengthConst * ((smallestAngleCorner.x - line_inter_x) *
(smallestAngleCorner.x - line_inter_x) +
(smallestAngleCorner.y - line_inter_y) *
(smallestAngleCorner.y - line_inter_y)))
&& (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, petal_slab_inter_x_second, petal_slab_inter_y_second))
&& MinDistanceToNeighbor(petal_slab_inter_x_second, petal_slab_inter_y_second, ref neighborotri) > MinDistanceToNeighbor(line_inter_x, line_inter_y, ref neighborotri))
{
// slab and petal intersection is advised
dxSecondSuggestion = petal_slab_inter_x_second - torg.x;
dySecondSuggestion = petal_slab_inter_y_second - torg.y;
}
else
{ // slab intersection point is further away ;
if (IsBadTriangleAngle(largestAngleCorner.x, largestAngleCorner.y, middleAngleCorner.x, middleAngleCorner.y, line_inter_x, line_inter_y))
{
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((line_inter_x - myCircumcenter.x) * (line_inter_x - myCircumcenter.x) +
(line_inter_y - myCircumcenter.y) * (line_inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - line_inter_x;
ay = myCircumcenter.y - line_inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
line_inter_x = line_inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
line_inter_y = line_inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, line_inter_x, line_inter_y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{
// intersection point is suggested
dxSecondSuggestion = line_inter_x - torg.x;
dySecondSuggestion = line_inter_y - torg.y;
}
}
else
{
// we are not creating a bad triangle
// slab intersection is advised
dxSecondSuggestion = line_result[2] - torg.x;
dySecondSuggestion = line_result[3] - torg.y;
}
}
//------------------------------------------------------//
}
else
{
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// if it is inside feasible region, then insert v2
// apply perturbation
// find the distance between circumcenter and intersection point
d = Math.Sqrt((inter_x - myCircumcenter.x) * (inter_x - myCircumcenter.x) +
(inter_y - myCircumcenter.y) * (inter_y - myCircumcenter.y));
// then find the vector going from intersection point to circumcenter
ax = myCircumcenter.x - inter_x;
ay = myCircumcenter.y - inter_y;
ax = ax / d;
ay = ay / d;
// now calculate the new intersection point which is perturbated towards the circumcenter
inter_x = inter_x + ax * pertConst * Math.Sqrt(shortestEdgeDist);
inter_y = inter_y + ay * pertConst * Math.Sqrt(shortestEdgeDist);
if (IsBadTriangleAngle(middleAngleCorner.x, middleAngleCorner.y, largestAngleCorner.x, largestAngleCorner.y, inter_x, inter_y))
{
// go back to circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}
else
{
// intersection point is suggested
dxSecondSuggestion = inter_x - torg.x;
dySecondSuggestion = inter_y - torg.y;
}
}
else
{
// intersection point is suggested
dxSecondSuggestion = inter_x - torg.x;
dySecondSuggestion = inter_y - torg.y;
}
}
}
/// if it is an acute triangle, check if it is a good enough location ///
// for acute triangle case, we need to check if it is ok to use either of them
if ((smallestAngleCorner.x - myCircumcenter.x) * (smallestAngleCorner.x - myCircumcenter.x) +
(smallestAngleCorner.y - myCircumcenter.y) * (smallestAngleCorner.y - myCircumcenter.y) >
lengthConst * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))))
{
// use circumcenter
dxSecondSuggestion = dx;
dySecondSuggestion = dy;
}// else we stick on what we have found
}
}// if it is on the boundary, meaning no neighbor triangle in this direction, the other direction might be ok
if (isObtuse)
{
if (neighborNotFound_first && neighborNotFound_second)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (xMidOfMiddleEdge)) *
(smallestAngleCorner.x - (xMidOfMiddleEdge)) +
(smallestAngleCorner.y - (yMidOfMiddleEdge)) *
(smallestAngleCorner.y - (yMidOfMiddleEdge))) >
(smallestAngleCorner.x - (xMidOfLongestEdge)) *
(smallestAngleCorner.x - (xMidOfLongestEdge)) +
(smallestAngleCorner.y - (yMidOfLongestEdge)) *
(smallestAngleCorner.y - (yMidOfLongestEdge)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else if (neighborNotFound_first)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))) >
(smallestAngleCorner.x - (xMidOfLongestEdge)) *
(smallestAngleCorner.x - (xMidOfLongestEdge)) +
(smallestAngleCorner.y - (yMidOfLongestEdge)) *
(smallestAngleCorner.y - (yMidOfLongestEdge)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else if (neighborNotFound_second)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (xMidOfMiddleEdge)) *
(smallestAngleCorner.x - (xMidOfMiddleEdge)) +
(smallestAngleCorner.y - (yMidOfMiddleEdge)) *
(smallestAngleCorner.y - (yMidOfMiddleEdge))) >
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))) >
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
}
else
{ // acute : consider other direction
if (neighborNotFound_first && neighborNotFound_second)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (xMidOfMiddleEdge)) *
(smallestAngleCorner.x - (xMidOfMiddleEdge)) +
(smallestAngleCorner.y - (yMidOfMiddleEdge)) *
(smallestAngleCorner.y - (yMidOfMiddleEdge))) >
(smallestAngleCorner.x - (xMidOfLongestEdge)) *
(smallestAngleCorner.x - (xMidOfLongestEdge)) +
(smallestAngleCorner.y - (yMidOfLongestEdge)) *
(smallestAngleCorner.y - (yMidOfLongestEdge)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else if (neighborNotFound_first)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))) >
(smallestAngleCorner.x - (xMidOfLongestEdge)) *
(smallestAngleCorner.x - (xMidOfLongestEdge)) +
(smallestAngleCorner.y - (yMidOfLongestEdge)) *
(smallestAngleCorner.y - (yMidOfLongestEdge)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else if (neighborNotFound_second)
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (xMidOfMiddleEdge)) *
(smallestAngleCorner.x - (xMidOfMiddleEdge)) +
(smallestAngleCorner.y - (yMidOfMiddleEdge)) *
(smallestAngleCorner.y - (yMidOfMiddleEdge))) >
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
else
{
//obtuse: check if the other direction works
if (justAcute * ((smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxSecondSuggestion + torg.x)) +
(smallestAngleCorner.y - (dySecondSuggestion + torg.y)) *
(smallestAngleCorner.y - (dySecondSuggestion + torg.y))) >
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) *
(smallestAngleCorner.x - (dxFirstSuggestion + torg.x)) +
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)) *
(smallestAngleCorner.y - (dyFirstSuggestion + torg.y)))
{
dx = dxSecondSuggestion;
dy = dySecondSuggestion;
}
else
{
dx = dxFirstSuggestion;
dy = dyFirstSuggestion;
}
}
}// end if obtuse
}// end of relocation
}// end of almostGood
Point circumcenter = new Point();
if (relocated <= 0)
{
circumcenter.x = torg.x + dx;
circumcenter.y = torg.y + dy;
}
else
{
circumcenter.x = origin_x + dx;
circumcenter.y = origin_y + dy;
}
xi = (yao * dx - xao * dy) * (2.0 * denominator);
eta = (xdo * dy - ydo * dx) * (2.0 * denominator);
return circumcenter;
}
/// <summary>
/// Given square of edge lengths of a triangle,
// determine its orientation
/// </summary>
/// <param name="aodist"></param>
/// <param name="dadist"></param>
/// <param name="dodist"></param>
/// <returns>Returns a number indicating an orientation.</returns>
private int LongestShortestEdge(double aodist, double dadist, double dodist)
{
// 123: shortest: aodist // 213: shortest: dadist // 312: shortest: dodist
// middle: dadist // middle: aodist // middle: aodist
// longest: dodist // longest: dodist // longest: dadist
// 132: shortest: aodist // 231: shortest: dadist // 321: shortest: dodist
// middle: dodist // middle: dodist // middle: dadist
// longest: dadist // longest: aodist // longest: aodist
int max = 0, min = 0, mid = 0, minMidMax;
if (dodist < aodist && dodist < dadist)
{
min = 3; // apex is the smallest angle, dodist is the longest edge
if (aodist < dadist)
{
max = 2; // dadist is the longest edge
mid = 1; // aodist is the middle longest edge
}
else
{
max = 1; // aodist is the longest edge
mid = 2; // dadist is the middle longest edge
}
}
else if (aodist < dadist)
{
min = 1; // dest is the smallest angle, aodist is the biggest edge
if (dodist < dadist)
{
max = 2; // dadist is the longest edge
mid = 3; // dodist is the middle longest edge
}
else
{
max = 3; // dodist is the longest edge
mid = 2; // dadist is the middle longest edge
}
}
else
{
min = 2; // origin is the smallest angle, dadist is the biggest edge
if (aodist < dodist)
{
max = 3; // dodist is the longest edge
mid = 1; // aodist is the middle longest edge
}
else
{
max = 1; // aodist is the longest edge
mid = 3; // dodist is the middle longest edge
}
}
minMidMax = min * 100 + mid * 10 + max;
// HANDLE ISOSCELES TRIANGLE CASE
return minMidMax;
}
/// <summary>
/// Checks if smothing is possible for a given bad triangle.
/// </summary>
/// <param name="badotri"></param>
/// <param name="torg"></param>
/// <param name="tdest"></param>
/// <param name="tapex"></param>
/// <param name="newloc">The new location for the point, if somothing is possible.</param>
/// <returns>Returns 1, 2 or 3 if smoothing will work, 0 otherwise.</returns>
private int DoSmoothing(Otri badotri, Vertex torg, Vertex tdest, Vertex tapex,
ref double[] newloc)
{
int numpoints_p = 0;// keeps the number of points in a star of point p, q, r
int numpoints_q = 0;
int numpoints_r = 0;
//int i;
double[] possibilities = new double[6];//there can be more than one possibilities
int num_pos = 0; // number of possibilities
int flag1 = 0, flag2 = 0, flag3 = 0;
bool newLocFound = false;
//vertex v1, v2, v3; // for ccw test
//double p1[2], p2[2], p3[2];
//double temp[2];
//********************* TRY TO RELOCATE POINT "p" ***************
// get the surrounding points of p, so this gives us the triangles
numpoints_p = GetStarPoints(badotri, torg, tdest, tapex, 1, ref points_p);
// check if the points in counterclockwise order
// p1[0] = points_p[0]; p1[1] = points_p[1];
// p2[0] = points_p[2]; p2[1] = points_p[3];
// p3[0] = points_p[4]; p3[1] = points_p[5];
// v1 = (vertex)p1; v2 = (vertex)p2; v3 = (vertex)p3;
// if(counterclockwise(m,b,v1,v2,v3) < 0){
// // reverse the order to ccw
// for(i = 0; i < numpoints_p/2; i++){
// temp[0] = points_p[2*i];
// temp[1] = points_p[2*i+1];
// points_p[2*i] = points_p[2*(numpoints_p-1)-2*i];
// points_p[2*i+1] = points_p[2*(numpoints_p-1)+1-2*i];
// points_p[2*(numpoints_p-1)-2*i] = temp[0];
// points_p[2*(numpoints_p-1)+1-2*i] = temp[1];
// }
// }
// m.counterclockcount--;
// INTERSECTION OF PETALS
// first check whether the star angles are appropriate for relocation
if (torg.type == VertexType.FreeVertex && numpoints_p != 0 && ValidPolygonAngles(numpoints_p, points_p))
{
//newLocFound = getPetalIntersection(m, b, numpoints_p, points_p, newloc);
//newLocFound = getPetalIntersectionBruteForce(m, b,numpoints_p, points_p, newloc,torg[0],torg[1]);
if (behavior.MaxAngle == 0.0)
{
newLocFound = GetWedgeIntersectionWithoutMaxAngle(numpoints_p, points_p, ref newloc);
}
else
{
newLocFound = GetWedgeIntersection(numpoints_p, points_p, ref newloc);
}
//printf("call petal intersection for p\n");
// make sure the relocated point is a free vertex
if (newLocFound)
{
possibilities[0] = newloc[0];// something found
possibilities[1] = newloc[1];
num_pos++;// increase the number of possibilities
flag1 = 1;
}
}
//********************* TRY TO RELOCATE POINT "q" ***************
// get the surrounding points of q, so this gives us the triangles
numpoints_q = GetStarPoints(badotri, torg, tdest, tapex, 2, ref points_q);
// // check if the points in counterclockwise order
// v1[0] = points_q[0]; v1[1] = points_q[1];
// v2[0] = points_q[2]; v2[1] = points_q[3];
// v3[0] = points_q[4]; v3[1] = points_q[5];
// if(counterclockwise(m,b,v1,v2,v3) < 0){
// // reverse the order to ccw
// for(i = 0; i < numpoints_q/2; i++){
// temp[0] = points_q[2*i];
// temp[1] = points_q[2*i+1];
// points_q[2*i] = points_q[2*(numpoints_q-1)-2*i];
// points_q[2*i+1] = points_q[2*(numpoints_q-1)+1-2*i];
// points_q[2*(numpoints_q-1)-2*i] = temp[0];
// points_q[2*(numpoints_q-1)+1-2*i] = temp[1];
// }
// }
// m.counterclockcount--;
// INTERSECTION OF PETALS
// first check whether the star angles are appropriate for relocation
if (tdest.type == VertexType.FreeVertex && numpoints_q != 0 && ValidPolygonAngles(numpoints_q, points_q))
{
//newLocFound = getPetalIntersection(m, b,numpoints_q, points_q, newloc);
//newLocFound = getPetalIntersectionBruteForce(m, b,numpoints_q, points_q, newloc,tapex[0],tapex[1]);
if (behavior.MaxAngle == 0.0)
{
newLocFound = GetWedgeIntersectionWithoutMaxAngle(numpoints_q, points_q, ref newloc);
}
else
{
newLocFound = GetWedgeIntersection(numpoints_q, points_q, ref newloc);
}
//printf("call petal intersection for q\n");
// make sure the relocated point is a free vertex
if (newLocFound)
{
possibilities[2] = newloc[0];// something found
possibilities[3] = newloc[1];
num_pos++;// increase the number of possibilities
flag2 = 2;
}
}
//********************* TRY TO RELOCATE POINT "q" ***************
// get the surrounding points of r, so this gives us the triangles
numpoints_r = GetStarPoints(badotri, torg, tdest, tapex, 3, ref points_r);
// check if the points in counterclockwise order
// v1[0] = points_r[0]; v1[1] = points_r[1];
// v2[0] = points_r[2]; v2[1] = points_r[3];
// v3[0] = points_r[4]; v3[1] = points_r[5];
// if(counterclockwise(m,b,v1,v2,v3) < 0){
// // reverse the order to ccw
// for(i = 0; i < numpoints_r/2; i++){
// temp[0] = points_r[2*i];
// temp[1] = points_r[2*i+1];
// points_r[2*i] = points_r[2*(numpoints_r-1)-2*i];
// points_r[2*i+1] = points_r[2*(numpoints_r-1)+1-2*i];
// points_r[2*(numpoints_r-1)-2*i] = temp[0];
// points_r[2*(numpoints_r-1)+1-2*i] = temp[1];
// }
// }
// m.counterclockcount--;
// INTERSECTION OF PETALS
// first check whether the star angles are appropriate for relocation
if (tapex.type == VertexType.FreeVertex && numpoints_r != 0 && ValidPolygonAngles(numpoints_r, points_r))
{
//newLocFound = getPetalIntersection(m, b,numpoints_r, points_r, newloc);
//newLocFound = getPetalIntersectionBruteForce(m, b,numpoints_r, points_r, newloc,tdest[0],tdest[1]);
if (behavior.MaxAngle == 0.0)
{
newLocFound = GetWedgeIntersectionWithoutMaxAngle(numpoints_r, points_r, ref newloc);
}
else
{
newLocFound = GetWedgeIntersection(numpoints_r, points_r, ref newloc);
}
//printf("call petal intersection for r\n");
// make sure the relocated point is a free vertex
if (newLocFound)
{
possibilities[4] = newloc[0];// something found
possibilities[5] = newloc[1];
num_pos++;// increase the number of possibilities
flag3 = 3;
}
}
//printf("numpossibilities %d\n",num_pos);
//////////// AFTER FINISH CHECKING EVERY POSSIBILITY, CHOOSE ANY OF THE AVAILABLE ONE //////////////////////
if (num_pos > 0)
{
if (flag1 > 0)
{ // suggest to relocate origin
newloc[0] = possibilities[0];
newloc[1] = possibilities[1];
return flag1;
}
else
{
if (flag2 > 0)
{ // suggest to relocate apex
newloc[0] = possibilities[2];
newloc[1] = possibilities[3];
return flag2;
}
else
{// suggest to relocate destination
if (flag3 > 0)
{
newloc[0] = possibilities[4];
newloc[1] = possibilities[5];
return flag3;
}
}
}
}
return 0;// could not find any good relocation
}
/// <summary>
/// Finds the star of a given point.
/// </summary>
/// <param name="badotri"></param>
/// <param name="p"></param>
/// <param name="q"></param>
/// <param name="r"></param>
/// <param name="whichPoint"></param>
/// <param name="points">List of points on the star of the given point.</param>
/// <returns>Number of points on the star of the given point.</returns>
private int GetStarPoints(Otri badotri, Vertex p, Vertex q, Vertex r,
int whichPoint, ref double[] points)
{
Otri neighotri = default(Otri); // for return value of the function
Otri tempotri; // for temporary usage
double first_x = 0, first_y = 0; // keeps the first point to be considered
double second_x = 0, second_y = 0; // for determining the edge we will begin
double third_x = 0, third_y = 0; // termination
double[] returnPoint = new double[2]; // for keeping the returned point
int numvertices = 0; // for keeping number of surrounding vertices
// first determine which point to be used to find its neighbor triangles
switch (whichPoint)
{
case 1:
first_x = p.x; // point at the center
first_y = p.y;
second_x = r.x; // second vertex of first edge to consider
second_y = r.y;
third_x = q.x; // for terminating the search
third_y = q.y;
break;
case 2:
first_x = q.x; // point at the center
first_y = q.y;
second_x = p.x; // second vertex of first edge to consider
second_y = p.y;
third_x = r.x; // for terminating the search
third_y = r.y;
break;
case 3:
first_x = r.x; // point at the center
first_y = r.y;
second_x = q.x; // second vertex of first edge to consider
second_y = q.y;
third_x = p.x; // for terminating the search
third_y = p.y;
break;
}
tempotri = badotri;
// add first point as the end of first edge
points[numvertices] = second_x;
numvertices++;
points[numvertices] = second_y;
numvertices++;
// assign as dummy value
returnPoint[0] = second_x; returnPoint[1] = second_y;
// until we reach the third point of the beginning triangle
do
{
// find the neighbor's third point where it is incident to given edge
if (!GetNeighborsVertex(tempotri, first_x, first_y, second_x, second_y, ref returnPoint, ref neighotri))
{
// go to next triangle
tempotri = neighotri;
// now the second point is the neighbor's third vertex
second_x = returnPoint[0];
second_y = returnPoint[1];
// add a new point to the list of surrounding points
points[numvertices] = returnPoint[0];
numvertices++;
points[numvertices] = returnPoint[1];
numvertices++;
}
else
{
numvertices = 0;
break;
}
} while (!((Math.Abs(returnPoint[0] - third_x) <= EPS) &&
(Math.Abs(returnPoint[1] - third_y) <= EPS)));
return numvertices / 2;
}
/// <summary>
/// Gets a neighbours vertex.
/// </summary>
/// <param name="badotri"></param>
/// <param name="first_x"></param>
/// <param name="first_y"></param>
/// <param name="second_x"></param>
/// <param name="second_y"></param>
/// <param name="thirdpoint">Neighbor's third vertex incident to given edge.</param>
/// <param name="neighotri">Pointer for the neighbor triangle.</param>
/// <returns>Returns true if vertex was found.</returns>
private bool GetNeighborsVertex(Otri badotri,
double first_x, double first_y,
double second_x, double second_y,
ref double[] thirdpoint, ref Otri neighotri)
{
Otri neighbor = default(Otri); // keeps the neighbor triangles
bool notFound = false; // boolean variable if we can find that neighbor or not
// for keeping the vertices of the neighbor triangle
Vertex neighborvertex_1 = null;
Vertex neighborvertex_2 = null;
Vertex neighborvertex_3 = null;
// used for finding neighbor triangle
int firstVertexMatched = 0, secondVertexMatched = 0; // to find the correct neighbor
//triangle ptr; // Temporary variable used by sym()
//int i; // index variable
// find neighbors
// Check each of the triangle's three neighbors to find the correct one
for (badotri.orient = 0; badotri.orient < 3; badotri.orient++)
{
// Find the neighbor.
badotri.Sym(ref neighbor);
// check if it is the one we are looking for by checking the corners
// first check if the neighbor is nonexistent, since it can be on the border
if (neighbor.tri.id != Mesh.DUMMY)
{
// then check if two wanted corners are also in this triangle
// take the vertices of the candidate neighbor
neighborvertex_1 = neighbor.Org();
neighborvertex_2 = neighbor.Dest();
neighborvertex_3 = neighbor.Apex();
// check if it is really a triangle
if ((neighborvertex_1.x == neighborvertex_2.x && neighborvertex_1.y == neighborvertex_2.y)
|| (neighborvertex_2.x == neighborvertex_3.x && neighborvertex_2.y == neighborvertex_3.y)
|| (neighborvertex_1.x == neighborvertex_3.x && neighborvertex_1.y == neighborvertex_3.y))
{
//printf("Two vertices are the same!!!!!!!\n");
}
else
{
// begin searching for the correct neighbor triangle
firstVertexMatched = 0;
if ((Math.Abs(first_x - neighborvertex_1.x) < EPS) &&
(Math.Abs(first_y - neighborvertex_1.y) < EPS))
{
firstVertexMatched = 11; // neighbor's 1st vertex is matched to first vertex
}
else if ((Math.Abs(first_x - neighborvertex_2.x) < EPS) &&
(Math.Abs(first_y - neighborvertex_2.y) < EPS))
{
firstVertexMatched = 12; // neighbor's 2nd vertex is matched to first vertex
}
else if ((Math.Abs(first_x - neighborvertex_3.x) < EPS) &&
(Math.Abs(first_y - neighborvertex_3.y) < EPS))
{
firstVertexMatched = 13; // neighbor's 3rd vertex is matched to first vertex
}/*else{
// none of them matched
} // end of first vertex matching */
secondVertexMatched = 0;
if ((Math.Abs(second_x - neighborvertex_1.x) < EPS) &&
(Math.Abs(second_y - neighborvertex_1.y) < EPS))
{
secondVertexMatched = 21; // neighbor's 1st vertex is matched to second vertex
}
else if ((Math.Abs(second_x - neighborvertex_2.x) < EPS) &&
(Math.Abs(second_y - neighborvertex_2.y) < EPS))
{
secondVertexMatched = 22; // neighbor's 2nd vertex is matched to second vertex
}
else if ((Math.Abs(second_x - neighborvertex_3.x) < EPS) &&
(Math.Abs(second_y - neighborvertex_3.y) < EPS))
{
secondVertexMatched = 23; // neighbor's 3rd vertex is matched to second vertex
}/*else{
// none of them matched
} // end of second vertex matching*/
}
}// if neighbor exists or not
if (((firstVertexMatched == 11) && (secondVertexMatched == 22 || secondVertexMatched == 23))
|| ((firstVertexMatched == 12) && (secondVertexMatched == 21 || secondVertexMatched == 23))
|| ((firstVertexMatched == 13) && (secondVertexMatched == 21 || secondVertexMatched == 22)))
break;
}// end of for loop over all orientations
switch (firstVertexMatched)
{
case 0:
notFound = true;
break;
case 11:
if (secondVertexMatched == 22)
{
thirdpoint[0] = neighborvertex_3.x;
thirdpoint[1] = neighborvertex_3.y;
}
else if (secondVertexMatched == 23)
{
thirdpoint[0] = neighborvertex_2.x;
thirdpoint[1] = neighborvertex_2.y;
}
else { notFound = true; }
break;
case 12:
if (secondVertexMatched == 21)
{
thirdpoint[0] = neighborvertex_3.x;
thirdpoint[1] = neighborvertex_3.y;
}
else if (secondVertexMatched == 23)
{
thirdpoint[0] = neighborvertex_1.x;
thirdpoint[1] = neighborvertex_1.y;
}
else { notFound = true; }
break;
case 13:
if (secondVertexMatched == 21)
{
thirdpoint[0] = neighborvertex_2.x;
thirdpoint[1] = neighborvertex_2.y;
}
else if (secondVertexMatched == 22)
{
thirdpoint[0] = neighborvertex_1.x;
thirdpoint[1] = neighborvertex_1.y;
}
else { notFound = true; }
break;
default:
if (secondVertexMatched == 0) { notFound = true; }
break;
}
// pointer of the neighbor triangle
neighotri = neighbor;
return notFound;
}
/// <summary>
/// Find a new point location by wedge intersection.
/// </summary>
/// <param name="numpoints"></param>
/// <param name="points"></param>
/// <param name="newloc">A new location for the point according to surrounding points.</param>
/// <returns>Returns true if new location found</returns>
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];
// Resize work arrays
if (2 * numpoints > petalx.Length)
{
petalx = new double[2 * numpoints];
petaly = new double[2 * numpoints];
petalr = new double[2 * numpoints];
wedges = new double[2 * numpoints * 16 + 36];
}
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 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;
}
/// <summary>
/// Find a new point location by wedge intersection.
/// </summary>
/// <param name="numpoints"></param>
/// <param name="points"></param>
/// <param name="newloc">A new location for the point according to surrounding points.</param>
/// <returns>Returns true if new location found</returns>
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];
// Resize work arrays
if (2 * numpoints > petalx.Length)
{
petalx = new double[2 * numpoints];
petaly = new double[2 * numpoints];
petalr = new double[2 * numpoints];
wedges = new double[2 * numpoints * 20 + 40];
}
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];
double[] p2 = new double[3];
double[] p3 = new double[3];
double[] p4 = new double[3];
//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;
}
/// <summary>
/// Check polygon for min angle.
/// </summary>
/// <param name="numpoints"></param>
/// <param name="points"></param>
/// <returns>Returns true if the polygon has angles greater than 2*minangle.</returns>
private bool ValidPolygonAngles(int numpoints, double[] points)
{
int i;//,j
for (i = 0; i < numpoints; i++)
{
if (i == numpoints - 1)
{
if (IsBadPolygonAngle(points[i * 2], points[i * 2 + 1], points[0], points[1], points[2], points[3]))
{
return false; // one of the inner angles is less than required
}
}
else if (i == numpoints - 2)
{
if (IsBadPolygonAngle(points[i * 2], points[i * 2 + 1], points[(i + 1) * 2], points[(i + 1) * 2 + 1], points[0], points[1]))
{
return false; // one of the inner angles is less than required
}
}
else
{
if (IsBadPolygonAngle(points[i * 2], points[i * 2 + 1], points[(i + 1) * 2], points[(i + 1) * 2 + 1], points[(i + 2) * 2], points[(i + 2) * 2 + 1]))
{
return false; // one of the inner angles is less than required
}
}
}
return true; // all angles are valid
}
/// <summary>
/// Given three coordinates of a polygon, tests to see if it satisfies the minimum
/// angle condition for relocation.
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x3"></param>
/// <param name="y3"></param>
/// <returns>Returns true, if it is a BAD polygon corner, returns false if it is a GOOD
/// polygon corner</returns>
private bool IsBadPolygonAngle(double x1, double y1,
double x2, double y2, double x3, double y3)
{
// variables keeping the distance values for the edges
double dx12, dy12, dx23, dy23, dx31, dy31;
double dist12, dist23, dist31;
double cosAngle; // in order to check minimum angle condition
// calculate the side lengths
dx12 = x1 - x2;
dy12 = y1 - y2;
dx23 = x2 - x3;
dy23 = y2 - y3;
dx31 = x3 - x1;
dy31 = y3 - y1;
// calculate the squares of the side lentghs
dist12 = dx12 * dx12 + dy12 * dy12;
dist23 = dx23 * dx23 + dy23 * dy23;
dist31 = dx31 * dx31 + dy31 * dy31;
/// calculate cosine of largest angle ///
cosAngle = (dist12 + dist23 - dist31) / (2 * Math.Sqrt(dist12) * Math.Sqrt(dist23));
// Check whether the angle is smaller than permitted which is 2*minangle!!!
//printf("angle: %f 2*minangle = %f\n",acos(cosAngle)*180/PI, 2*acos(Math.Sqrt(b.goodangle))*180/PI);
if (Math.Acos(cosAngle) < 2 * Math.Acos(Math.Sqrt(behavior.goodAngle)))
{
return true;// it is a BAD triangle
}
return false;// it is a GOOD triangle
}
/// <summary>
/// Given four points representing two lines, returns the intersection point.
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x3"></param>
/// <param name="y3"></param>
/// <param name="x4"></param>
/// <param name="y4"></param>
/// <param name="p">The intersection point.</param>
/// <remarks>
// referenced to: http://local.wasp.uwa.edu.au/~pbourke/geometry/
/// </remarks>
private void LineLineIntersection(
double x1, double y1,
double x2, double y2,
double x3, double y3,
double x4, double y4, ref double[] p)
{
// x1,y1 P1 coordinates (point of line 1)
// x2,y2 P2 coordinates (point of line 1)
// x3,y3 P3 coordinates (point of line 2)
// x4,y4 P4 coordinates (point of line 2)
// p[1],p[2] intersection coordinates
//
// This function returns a pointer array which first index indicates
// weather they intersect on one point or not, followed by coordinate pairs.
double u_a, u_b, denom;
// calculate denominator first
denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
u_a = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
u_b = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
// if denominator and numerator equal to zero, lines are coincident
if (Math.Abs(denom - 0.0) < EPS && (Math.Abs(u_b - 0.0) < EPS && Math.Abs(u_a - 0.0) < EPS))
{
p[0] = 0.0;
}
// if denominator equals to zero, lines are parallel
else if (Math.Abs(denom - 0.0) < EPS)
{
p[0] = 0.0;
}
else
{
p[0] = 1.0;
u_a = u_a / denom;
u_b = u_b / denom;
p[1] = x1 + u_a * (x2 - x1); // not the intersection point
p[2] = y1 + u_a * (y2 - y1);
}
}
/// <summary>
/// Returns the convex polygon which is the intersection of the given convex
/// polygon with the halfplane on the left side (regarding the directional vector)
/// of the given line.
/// </summary>
/// <param name="numvertices"></param>
/// <param name="convexPoly"></param>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <returns></returns>
/// <remarks>
/// http://www.mathematik.uni-ulm.de/stochastik/lehre/ws03_04/rt/Geometry2D.ps
/// </remarks>
private int HalfPlaneIntersection(int numvertices, ref double[] convexPoly, double x1, double y1, double x2, double y2)
{
double dx, dy; // direction of the line
double z, min, max;
int i, j;
int numpolys;
double[] res = null;
int count = 0;
int intFound = 0;
dx = x2 - x1;
dy = y2 - y1;
numpolys = SplitConvexPolygon(numvertices, convexPoly, x1, y1, x2, y2, polys);
if (numpolys == 3)
{
count = numvertices;
}
else
{
for (i = 0; i < numpolys; i++)
{
min = double.MaxValue;
max = double.MinValue;
// compute the minimum and maximum of the
// third coordinate of the cross product
for (j = 1; j <= 2 * polys[i][0] - 1; j = j + 2)
{
z = dx * (polys[i][j + 1] - y1) - dy * (polys[i][j] - x1);
min = (z < min ? z : min);
max = (z > max ? z : max);
}
// ... and choose the (absolute) greater of both
z = (Math.Abs(min) > Math.Abs(max) ? min : max);
// and if it is positive, the polygon polys[i]
// is on the left side of line
if (z > 0.0)
{
res = polys[i];
intFound = 1;
break;
}
}
if (intFound == 1)
{
while (count < res[0])
{
convexPoly[2 * count] = res[2 * count + 1];
convexPoly[2 * count + 1] = res[2 * count + 2];
count++;
}
}
}
// update convexPoly
return count;
}
/// <summary>
/// Splits a convex polygons into one or two polygons through the intersection
/// with the given line (regarding the directional vector of the given line).
/// </summary>
/// <param name="numvertices"></param>
/// <param name="convexPoly"></param>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="polys"></param>
/// <returns></returns>
/// <remarks>
/// http://www.mathematik.uni-ulm.de/stochastik/lehre/ws03_04/rt/Geometry2D.ps
/// </remarks>
private int SplitConvexPolygon(int numvertices, double[] convexPoly, double x1, double y1, double x2, double y2, double[][] polys)
{
// state = 0: before the first intersection (with the line)
// state = 1: after the first intersection (with the line)
// state = 2: after the second intersection (with the line)
int state = 0;
double[] p = new double[3];
int poly1counter = 0;
int poly2counter = 0;
int numpolys;
int i;
double compConst = 0.000000000001;
// for debugging
int case1 = 0, case2 = 0, case3 = 0, case31 = 0, case32 = 0, case33 = 0, case311 = 0, case3111 = 0;
// intersect all edges of poly with line
for (i = 0; i < 2 * numvertices; i = i + 2)
{
int j = (i + 2 >= 2 * numvertices) ? 0 : i + 2;
LineLineSegmentIntersection(x1, y1, x2, y2, convexPoly[i], convexPoly[i + 1], convexPoly[j], convexPoly[j + 1], ref p);
// if this edge does not intersect with line
if (Math.Abs(p[0] - 0.0) <= compConst)
{
//System.out.println("null");
// add p[j] to the proper polygon
if (state == 1)
{
poly2counter++;
poly2[2 * poly2counter - 1] = convexPoly[j];
poly2[2 * poly2counter] = convexPoly[j + 1];
}
else
{
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
}
// debug
case1++;
}
// ... or if the intersection is the whole edge
else if (Math.Abs(p[0] - 2.0) <= compConst)
{
//System.out.println(o);
// then we can not reach state 1 and 2
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
// debug
case2++;
}
// ... or if the intersection is a point
else
{
// debug
case3++;
// if the point is the second vertex of the edge
if (Math.Abs(p[1] - convexPoly[j]) <= compConst && Math.Abs(p[2] - convexPoly[j + 1]) <= compConst)
{
// debug
case31++;
if (state == 1)
{
poly2counter++;
poly2[2 * poly2counter - 1] = convexPoly[j];
poly2[2 * poly2counter] = convexPoly[j + 1];
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
state++;
}
else if (state == 0)
{
// debug
case311++;
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
// test whether the polygon is splitted
// or the line only touches the polygon
if (i + 4 < 2 * numvertices)
{
int s1 = LinePointLocation(x1, y1, x2, y2, convexPoly[i], convexPoly[i + 1]);
int s2 = LinePointLocation(x1, y1, x2, y2, convexPoly[i + 4], convexPoly[i + 5]);
// the line only splits the polygon
// when the previous and next vertex lie
// on different sides of the line
if (s1 != s2 && s1 != 0 && s2 != 0)
{
// debug
case3111++;
poly2counter++;
poly2[2 * poly2counter - 1] = convexPoly[j];
poly2[2 * poly2counter] = convexPoly[j + 1];
state++;
}
}
}
}
// ... if the point is not the other vertex of the edge
else if (!(Math.Abs(p[1] - convexPoly[i]) <= compConst && Math.Abs(p[2] - convexPoly[i + 1]) <= compConst))
{
// debug
case32++;
poly1counter++;
poly1[2 * poly1counter - 1] = p[1];
poly1[2 * poly1counter] = p[2];
poly2counter++;
poly2[2 * poly2counter - 1] = p[1];
poly2[2 * poly2counter] = p[2];
if (state == 1)
{
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
}
else if (state == 0)
{
poly2counter++;
poly2[2 * poly2counter - 1] = convexPoly[j];
poly2[2 * poly2counter] = convexPoly[j + 1];
}
state++;
}
// ... else if the point is the second vertex of the edge
else
{
// debug
case33++;
if (state == 1)
{
poly2counter++;
poly2[2 * poly2counter - 1] = convexPoly[j];
poly2[2 * poly2counter] = convexPoly[j + 1];
}
else
{
poly1counter++;
poly1[2 * poly1counter - 1] = convexPoly[j];
poly1[2 * poly1counter] = convexPoly[j + 1];
}
}
}
}
// after splitting the state must be 0 or 2
// (depending whether the polygon was splitted or not)
if (state != 0 && state != 2)
{
// printf("there is something wrong state: %d\n", state);
// printf("polygon might not be convex!!\n");
// printf("case1: %d\ncase2: %d\ncase3: %d\ncase31: %d case311: %d case3111: %d\ncase32: %d\ncase33: %d\n", case1, case2, case3, case31, case311, case3111, case32, case33);
// printf("numvertices %d\n=============\n", numvertices);
// if there is something wrong with the intersection, just ignore this one
numpolys = 3;
}
else
{
// finally convert the vertex lists into convex polygons
numpolys = (state == 0) ? 1 : 2;
poly1[0] = poly1counter;
poly2[0] = poly2counter;
// convert the first convex polygon
polys[0] = poly1;
// convert the second convex polygon
if (state == 2)
{
polys[1] = poly2;
}
}
return numpolys;
}
/// <summary>
/// Determines on which side (relative to the direction) of the given line and the
/// point lies (regarding the directional vector) of the given line.
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns></returns>
/// <remarks>
/// http://www.mathematik.uni-ulm.de/stochastik/lehre/ws03_04/rt/Geometry2D.ps
/// </remarks>
private int LinePointLocation(double x1, double y1, double x2, double y2, double x, double y)
{
double z;
if (Math.Atan((y2 - y1) / (x2 - x1)) * 180.0 / Math.PI == 90.0)
{
if (Math.Abs(x1 - x) <= 0.00000000001)
return 0;
}
else
{
if (Math.Abs(y1 + (((y2 - y1) * (x - x1)) / (x2 - x1)) - y) <= EPS)
return 0;
}
// third component of the 3 dimensional product
z = (x2 - x1) * (y - y1) - (y2 - y1) * (x - x1);
if (Math.Abs(z - 0.0) <= 0.00000000001)
{
return 0;
}
else if (z > 0)
{
return 1;
}
else
{
return 2;
}
}
/// <summary>
/// Given four points representing one line and a line segment, returns the intersection point
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x3"></param>
/// <param name="y3"></param>
/// <param name="x4"></param>
/// <param name="y4"></param>
/// <param name="p"></param>
/// <remarks>
/// referenced to: http://local.wasp.uwa.edu.au/~pbourke/geometry/
/// </remarks>
private void LineLineSegmentIntersection(
double x1, double y1,
double x2, double y2,
double x3, double y3,
double x4, double y4, ref double[] p)
{
// x1,y1 P1 coordinates (point of line)
// x2,y2 P2 coordinates (point of line)
// x3,y3 P3 coordinates (point of line segment)
// x4,y4 P4 coordinates (point of line segment)
// p[1],p[2] intersection coordinates
//
// This function returns a pointer array which first index indicates
// weather they intersect on one point or not, followed by coordinate pairs.
double u_a, u_b, denom;
double compConst = 0.0000000000001;
// calculate denominator first
denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
u_a = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
u_b = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
//if(fabs(denom-0.0) < compConst && (fabs(u_b-0.0) < compConst && fabs(u_a-0.0) < compConst)){
//printf("denom %.20f u_b %.20f u_a %.20f\n",denom, u_b, u_a);
if (Math.Abs(denom - 0.0) < compConst)
{
if (Math.Abs(u_b - 0.0) < compConst && Math.Abs(u_a - 0.0) < compConst)
{
p[0] = 2.0; // if denominator and numerator equal to zero, lines are coincident
}
else
{
p[0] = 0.0;// if denominator equals to zero, lines are parallel
}
}
else
{
u_b = u_b / denom;
u_a = u_a / denom;
// printf("u_b %.20f\n", u_b);
if (u_b < -compConst || u_b > 1.0 + compConst)
{ // check if it is on the line segment
// printf("line (%.20f, %.20f) (%.20f, %.20f) line seg (%.20f, %.20f) (%.20f, %.20f) \n",x1, y1 ,x2, y2 ,x3, y3 , x4, y4);
p[0] = 0.0;
}
else
{
p[0] = 1.0;
p[1] = x1 + u_a * (x2 - x1); // intersection point
p[2] = y1 + u_a * (y2 - y1);
}
}
}
/// <summary>
/// Returns the centroid of a given polygon
/// </summary>
/// <param name="numpoints"></param>
/// <param name="points"></param>
/// <param name="centroid">Centroid of a given polygon </param>
private void FindPolyCentroid(int numpoints, double[] points, ref double[] centroid)
{
int i;
//double area = 0.0;//, temp
centroid[0] = 0.0; centroid[1] = 0.0;
for (i = 0; i < 2 * numpoints; i = i + 2)
{
centroid[0] = centroid[0] + points[i];
centroid[1] = centroid[1] + points[i + 1];
}
centroid[0] = centroid[0] / numpoints;
centroid[1] = centroid[1] / numpoints;
}
/// <summary>
/// Given two points representing a line and a radius together with a center point
/// representing a circle, returns the intersection points.
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x3"></param>
/// <param name="y3"></param>
/// <param name="r"></param>
/// <param name="p">Pointer to list of intersection points</param>
/// <remarks>
/// referenced to: http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
/// </remarks>
private void CircleLineIntersection(
double x1, double y1,
double x2, double y2,
double x3, double y3, double r, ref double[] p)
{
// x1,y1 P1 coordinates [point of line]
// x2,y2 P2 coordinates [point of line]
// x3,y3, r P3 coordinates(circle center) and radius [circle]
// p[1],p[2]; p[3],p[4] intersection coordinates
//
// This function returns a pointer array which first index indicates
// the number of intersection points, followed by coordinate pairs.
//double x , y ;
double a, b, c, mu, i;
a = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
b = 2 * ((x2 - x1) * (x1 - x3) + (y2 - y1) * (y1 - y3));
c = x3 * x3 + y3 * y3 + x1 * x1 + y1 * y1 - 2 * (x3 * x1 + y3 * y1) - r * r;
i = b * b - 4 * a * c;
if (i < 0.0)
{
// no intersection
p[0] = 0.0;
}
else if (Math.Abs(i - 0.0) < EPS)
{
// one intersection
p[0] = 1.0;
mu = -b / (2 * a);
p[1] = x1 + mu * (x2 - x1);
p[2] = y1 + mu * (y2 - y1);
}
else if (i > 0.0 && !(Math.Abs(a - 0.0) < EPS))
{
// two intersections
p[0] = 2.0;
// first intersection
mu = (-b + Math.Sqrt(i)) / (2 * a);
p[1] = x1 + mu * (x2 - x1);
p[2] = y1 + mu * (y2 - y1);
// second intersection
mu = (-b - Math.Sqrt(i)) / (2 * a);
p[3] = x1 + mu * (x2 - x1);
p[4] = y1 + mu * (y2 - y1);
}
else
{
p[0] = 0.0;
}
}
/// <summary>
/// Given three points, check if the point is the correct point that we are looking for.
/// </summary>
/// <param name="x1">P1 coordinates (bisector point of dual edge on triangle)</param>
/// <param name="y1">P1 coordinates (bisector point of dual edge on triangle)</param>
/// <param name="x2">P2 coordinates (intersection point)</param>
/// <param name="y2">P2 coordinates (intersection point)</param>
/// <param name="x3">P3 coordinates (circumcenter point)</param>
/// <param name="y3">P3 coordinates (circumcenter point)</param>
/// <param name="isObtuse"></param>
/// <returns>Returns true, if given point is the correct one otherwise return false.</returns>
private bool ChooseCorrectPoint(
double x1, double y1,
double x2, double y2,
double x3, double y3, bool isObtuse)
{
double d1, d2;
bool p;
// squared distance between circumcenter and intersection point
d1 = (x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3);
// squared distance between bisector point and intersection point
d2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
if (isObtuse)
{
// obtuse case
if (d2 >= d1)
{
p = true; // means we have found the right point
}
else
{
p = false; // means take the other point
}
}
else
{
// non-obtuse case
if (d2 < d1)
{
p = true; // means we have found the right point
}
else
{
p = false; // means take the other point
}
}
/// HANDLE RIGHT TRIANGLE CASE!!!!!!!!!!!!!!!!!!!!!!!!!!!!
return p;
}
/// <summary>
/// This function returns a pointer array which first index indicates the whether
/// the point is in between the other points, followed by coordinate pairs.
/// </summary>
/// <param name="x1">P1 coordinates [point of line] (point on Voronoi edge - intersection)</param>
/// <param name="y1">P1 coordinates [point of line] (point on Voronoi edge - intersection)</param>
/// <param name="x2">P2 coordinates [point of line] (circumcenter)</param>
/// <param name="y2">P2 coordinates [point of line] (circumcenter)</param>
/// <param name="x">P3 coordinates [point to be compared] (neighbor's circumcenter)</param>
/// <param name="y">P3 coordinates [point to be compared] (neighbor's circumcenter)</param>
/// <param name="p"></param>
private void PointBetweenPoints(double x1, double y1, double x2, double y2, double x, double y, ref double[] p)
{
// now check whether the point is close to circumcenter than intersection point
// BETWEEN THE POINTS
if ((x2 - x) * (x2 - x) + (y2 - y) * (y2 - y) < (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1))
{
p[0] = 1.0;
// calculate the squared distance to circumcenter
p[1] = (x - x2) * (x - x2) + (y - y2) * (y - y2);
p[2] = x;
p[3] = y;
}// *NOT* BETWEEN THE POINTS
else
{
p[0] = 0.0;
p[1] = 0.0;
p[2] = 0.0;
p[3] = 0.0;
}
}
/// <summary>
/// Given three coordinates of a triangle, tests a triangle to see if it satisfies
/// the minimum and/or maximum angle condition.
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <param name="x3"></param>
/// <param name="y3"></param>
/// <returns>Returns true, if it is a BAD triangle, returns false if it is a GOOD triangle.</returns>
private bool IsBadTriangleAngle(double x1, double y1, double x2, double y2, double x3, double y3)
{
// variables keeping the distance values for the edges
double dxod, dyod, dxda, dyda, dxao, dyao;
double dxod2, dyod2, dxda2, dyda2, dxao2, dyao2;
double apexlen, orglen, destlen;
double angle; // in order to check minimum angle condition
double maxangle; // in order to check minimum angle condition
// calculate the side lengths
dxod = x1 - x2;
dyod = y1 - y2;
dxda = x2 - x3;
dyda = y2 - y3;
dxao = x3 - x1;
dyao = y3 - y1;
// calculate the squares of the side lentghs
dxod2 = dxod * dxod;
dyod2 = dyod * dyod;
dxda2 = dxda * dxda;
dyda2 = dyda * dyda;
dxao2 = dxao * dxao;
dyao2 = dyao * dyao;
// Find the lengths of the triangle's three edges.
apexlen = dxod2 + dyod2;
orglen = dxda2 + dyda2;
destlen = dxao2 + dyao2;
// try to find the minimum edge and accordingly the pqr orientation
if ((apexlen < orglen) && (apexlen < destlen))
{
// Find the square of the cosine of the angle at the apex.
angle = dxda * dxao + dyda * dyao;
angle = angle * angle / (orglen * destlen);
}
else if (orglen < destlen)
{
// Find the square of the cosine of the angle at the origin.
angle = dxod * dxao + dyod * dyao;
angle = angle * angle / (apexlen * destlen);
}
else
{
// Find the square of the cosine of the angle at the destination.
angle = dxod * dxda + dyod * dyda;
angle = angle * angle / (apexlen * orglen);
}
// try to find the maximum edge and accordingly the pqr orientation
if ((apexlen > orglen) && (apexlen > destlen))
{
// Find the cosine of the angle at the apex.
maxangle = (orglen + destlen - apexlen) / (2 * Math.Sqrt(orglen * destlen));
}
else if (orglen > destlen)
{
// Find the cosine of the angle at the origin.
maxangle = (apexlen + destlen - orglen) / (2 * Math.Sqrt(apexlen * destlen));
}
else
{
// Find the cosine of the angle at the destination.
maxangle = (apexlen + orglen - destlen) / (2 * Math.Sqrt(apexlen * orglen));
}
// Check whether the angle is smaller than permitted.
if ((angle > behavior.goodAngle) || (behavior.MaxAngle != 0.00 && maxangle < behavior.maxGoodAngle))
{
return true;// it is a bad triangle
}
return false;// it is a good triangle
}
/// <summary>
/// Given the triangulation, and a vertex returns the minimum distance to the
/// vertices of the triangle where the given vertex located.
/// </summary>
/// <param name="newlocX"></param>
/// <param name="newlocY"></param>
/// <param name="searchtri"></param>
/// <returns></returns>
private double MinDistanceToNeighbor(double newlocX, double newlocY, ref Otri searchtri)
{
Otri horiz = default(Otri); // for search operation
LocateResult intersect = LocateResult.Outside;
Vertex v1, v2, v3, torg, tdest;
double d1, d2, d3, ahead;
//triangle ptr; // Temporary variable used by sym().
Point newvertex = new Point(newlocX, newlocY);
// printf("newvertex %f,%f\n", newvertex[0], newvertex[1]);
// Find the location of the vertex to be inserted. Check if a good
// starting triangle has already been provided by the caller.
// Find a boundary triangle.
//horiz.tri = m.dummytri;
//horiz.orient = 0;
//horiz.symself();
// Search for a triangle containing 'newvertex'.
// Start searching from the triangle provided by the caller.
// Where are we?
torg = searchtri.Org();
tdest = searchtri.Dest();
// Check the starting triangle's vertices.
if ((torg.x == newvertex.x) && (torg.y == newvertex.y))
{
intersect = LocateResult.OnVertex;
searchtri.Copy(ref horiz);
}
else if ((tdest.x == newvertex.x) && (tdest.y == newvertex.y))
{
searchtri.Lnext();
intersect = LocateResult.OnVertex;
searchtri.Copy(ref horiz);
}
else
{
// Orient 'searchtri' to fit the preconditions of calling preciselocate().
ahead = predicates.CounterClockwise(torg, tdest, newvertex);
if (ahead < 0.0)
{
// Turn around so that 'searchpoint' is to the left of the
// edge specified by 'searchtri'.
searchtri.Sym();
searchtri.Copy(ref horiz);
intersect = mesh.locator.PreciseLocate(newvertex, ref horiz, false);
}
else if (ahead == 0.0)
{
// Check if 'searchpoint' is between 'torg' and 'tdest'.
if (((torg.x < newvertex.x) == (newvertex.x < tdest.x)) &&
((torg.y < newvertex.y) == (newvertex.y < tdest.y)))
{
intersect = LocateResult.OnEdge;
searchtri.Copy(ref horiz);
}
}
else
{
searchtri.Copy(ref horiz);
intersect = mesh.locator.PreciseLocate(newvertex, ref horiz, false);
}
}
if (intersect == LocateResult.OnVertex || intersect == LocateResult.Outside)
{
// set distance to 0
//m.VertexDealloc(newvertex);
return 0.0;
}
else
{ // intersect == ONEDGE || intersect == INTRIANGLE
// find the triangle vertices
v1 = horiz.Org();
v2 = horiz.Dest();
v3 = horiz.Apex();
d1 = (v1.x - newvertex.x) * (v1.x - newvertex.x) + (v1.y - newvertex.y) * (v1.y - newvertex.y);
d2 = (v2.x - newvertex.x) * (v2.x - newvertex.x) + (v2.y - newvertex.y) * (v2.y - newvertex.y);
d3 = (v3.x - newvertex.x) * (v3.x - newvertex.x) + (v3.y - newvertex.y) * (v3.y - newvertex.y);
//m.VertexDealloc(newvertex);
// find minimum of the distance
if (d1 <= d2 && d1 <= d3)
{
return d1;
}
else if (d2 <= d3)
{
return d2;
}
else
{
return d3;
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink