private AdvancingFrontNode LocateNode(FP x)
{
AdvancingFrontNode node = FindSearchNode(x);
if (x < node.Value)
{
while ((node = node.Prev) != null)
{
if (x >= node.Value)
{
Search = node;
return(node);
}
}
}
else
{
while ((node = node.Next) != null)
{
if (x < node.Value)
{
Search = node.Prev;
return(node.Prev);
}
}
}
return(null);
}
public Point(FP x, FP y)
{
X = x;
Y = y;
Next = null;
Prev = null;
}
public TriangulationPoint Centroid()
{
FP cx = (Points[0].X + Points[1].X + Points[2].X) / 3f;
FP cy = (Points[0].Y + Points[1].Y + Points[2].Y) / 3f;
return(new TriangulationPoint(cx, cy));
}
public FP Area()
{
FP b = Points[0].X - Points[1].X;
FP h = Points[2].Y - Points[1].Y;
return(FP.Abs((b * h * 0.5f)));
}
public static Polygon RandomCircleSweep(FP scale, int vertexCount)
{
PolygonPoint point;
PolygonPoint[] points;
FP radius = scale / 4;
points = new PolygonPoint[vertexCount];
for (int i = 0; i < vertexCount; i++)
{
do
{
if (i % 250 == 0)
{
radius += scale / 2 * (0.5 - RNG.NextFP());
}
else if (i % 50 == 0)
{
radius += scale / 5 * (0.5 - RNG.NextFP());
}
else
{
radius += 25 * scale / vertexCount * (0.5 - RNG.NextFP());
}
radius = radius > scale / 2 ? scale / 2 : radius;
radius = radius < scale / 10 ? scale / 10 : radius;
} while (radius < scale / 10 || radius > scale / 2);
point = new PolygonPoint(radius * FP.Cos((PI_2 * i) / vertexCount),
radius * FP.Sin((PI_2 * i) / vertexCount));
points[i] = point;
}
return(new Polygon(points));
}
public CurveKey(FP position, FP value, FP tangentIn, FP tangentOut, CurveContinuity continuity)
{
this.position = position;
this.value = value;
this.tangentIn = tangentIn;
this.tangentOut = tangentOut;
this.continuity = continuity;
}
public FP Orient2D(Point pb, Point pc)
{
FP acx = X - pc.X;
FP bcx = pb.X - pc.X;
FP acy = Y - pc.Y;
FP bcy = pb.Y - pc.Y;
return(acx * bcy - acy * bcx);
}
private int GetNumberOfCycle(FP position)
{
FP cycle = (position - keys[0].Position) / (keys[keys.Count - 1].Position - keys[0].Position);
if (cycle < 0f)
{
cycle -= 1;
}
return((int)cycle);
}
public static List <TriangulationPoint> UniformDistribution(int n, FP scale)
{
List <TriangulationPoint> points = new List <TriangulationPoint>();
for (int i = 0; i < n; i++)
{
points.Add(new TriangulationPoint(scale * (0.5 - RNG.NextFP()), scale * (0.5 - RNG.NextFP())));
}
return(points);
}
public Triangulator(List <Point> polyLine, FP sheer)
{
_sheer = sheer;
Triangles = new List <List <Point> >();
Trapezoids = new List <Trapezoid>();
_xMonoPoly = new List <MonotoneMountain>();
_edgeList = InitEdges(polyLine);
_trapezoidalMap = new TrapezoidalMap();
_boundingBox = _trapezoidalMap.BoundingBox(_edgeList);
_queryGraph = new QueryGraph(Sink.Isink(_boundingBox));
Process();
}
/// <summary>
/// This implementation will use simple node traversal algorithm to find a point on the front
/// </summary>
public AdvancingFrontNode LocatePoint(TriangulationPoint point)
{
FP px = point.X;
AdvancingFrontNode node = FindSearchNode(px);
FP nx = node.Point.X;
if (px == nx)
{
if (point != node.Point)
{
// We might have two nodes with same x value for a short time
if (point == node.Prev.Point)
{
node = node.Prev;
}
else if (point == node.Next.Point)
{
node = node.Next;
}
else
{
throw new Exception("Failed to find Node for given afront point");
//node = null;
}
}
}
else if (px < nx)
{
while ((node = node.Prev) != null)
{
if (point == node.Point)
{
break;
}
}
}
else
{
while ((node = node.Next) != null)
{
if (point == node.Point)
{
break;
}
}
}
Search = node;
return(node);
}
/// Forumla to calculate signed area
/// Positive if CCW
/// Negative if CW
/// 0 if collinear
/// A[P1,P2,P3] = (x1*y2 - y1*x2) + (x2*y3 - y2*x3) + (x3*y1 - y3*x1)
/// = (x1-x3)*(y2-y3) - (y1-y3)*(x2-x3)
public static Orientation Orient2d(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc)
{
FP detleft = (pa.X - pc.X) * (pb.Y - pc.Y);
FP detright = (pa.Y - pc.Y) * (pb.X - pc.X);
FP val = detleft - detright;
if (val > -EPSILON && val < EPSILON)
{
return(Orientation.Collinear);
}
else if (val > 0)
{
return(Orientation.CCW);
}
return(Orientation.CW);
}
/// <summary>
/// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
/// Pass in 0 or FP.MaxValue to disable the limit.
/// maxAngularVelocity = 0 will disable the angular velocity limit.
/// </summary>
/// <param name="maxLinearVelocity">The max linear velocity.</param>
/// <param name="maxAngularVelocity">The max angular velocity.</param>
public VelocityLimitController(FP maxLinearVelocity, FP maxAngularVelocity)
: base(ControllerType.VelocityLimitController)
{
if (maxLinearVelocity == 0 || maxLinearVelocity == FP.MaxValue)
{
LimitLinearVelocity = false;
}
if (maxAngularVelocity == 0 || maxAngularVelocity == FP.MaxValue)
{
LimitAngularVelocity = false;
}
MaxLinearVelocity = maxLinearVelocity;
MaxAngularVelocity = maxAngularVelocity;
}
/*
* public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
* TriangulationPoint pd)
* {
* FP pdx = pd.X;
* FP pdy = pd.Y;
* FP adx = pa.X - pdx;
* FP ady = pa.Y - pdy;
* FP bdx = pb.X - pdx;
* FP bdy = pb.Y - pdy;
*
* FP adxbdy = adx*bdy;
* FP bdxady = bdx*ady;
* FP oabd = adxbdy - bdxady;
* // oabd = orient2d(pa,pb,pd);
* if (oabd <= 0)
* {
* return false;
* }
*
* FP cdx = pc.X - pdx;
* FP cdy = pc.Y - pdy;
*
* FP cdxady = cdx*ady;
* FP adxcdy = adx*cdy;
* FP ocad = cdxady - adxcdy;
* // ocad = orient2d(pc,pa,pd);
* if (ocad <= 0)
* {
* return false;
* }
* return true;
* }
*/
public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, TriangulationPoint pd)
{
FP oadb = (pa.X - pb.X) * (pd.Y - pb.Y) - (pd.X - pb.X) * (pa.Y - pb.Y);
if (oadb >= -EPSILON)
{
return(false);
}
FP oadc = (pa.X - pc.X) * (pd.Y - pc.Y) - (pd.X - pc.X) * (pa.Y - pc.Y);
if (oadc <= EPSILON)
{
return(false);
}
return(true);
}
public static List <TriangulationPoint> UniformGrid(int n, FP scale)
{
FP x = 0;
FP size = scale / n;
FP halfScale = 0.5 * scale;
List <TriangulationPoint> points = new List <TriangulationPoint>();
for (int i = 0; i < n + 1; i++)
{
x = halfScale - i * size;
for (int j = 0; j < n + 1; j++)
{
points.Add(new TriangulationPoint(x, halfScale - j * size));
}
}
return(points);
}
/// <summary>
/// Requirements:
/// 1. a,b and c form a triangle.
/// 2. a and d is know to be on opposite side of bc
/// <code>
/// a
/// +
/// / \
/// / \
/// b/ \c
/// +-------+
/// / B \
/// / \
/// </code>
/// Facts:
/// d has to be in area B to have a chance to be inside the circle formed by a,b and c
/// d is outside B if orient2d(a,b,d) or orient2d(c,a,d) is CW
/// This preknowledge gives us a way to optimize the incircle test
/// </summary>
/// <param name="pa">triangle point, opposite d</param>
/// <param name="pb">triangle point</param>
/// <param name="pc">triangle point</param>
/// <param name="pd">point opposite a</param>
/// <returns>true if d is inside circle, false if on circle edge</returns>
public static bool SmartIncircle(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
TriangulationPoint pd)
{
FP pdx = pd.X;
FP pdy = pd.Y;
FP adx = pa.X - pdx;
FP ady = pa.Y - pdy;
FP bdx = pb.X - pdx;
FP bdy = pb.Y - pdy;
FP adxbdy = adx * bdy;
FP bdxady = bdx * ady;
FP oabd = adxbdy - bdxady;
// oabd = orient2d(pa,pb,pd);
if (oabd <= 0)
{
return(false);
}
FP cdx = pc.X - pdx;
FP cdy = pc.Y - pdy;
FP cdxady = cdx * ady;
FP adxcdy = adx * cdy;
FP ocad = cdxady - adxcdy;
// ocad = orient2d(pc,pa,pd);
if (ocad <= 0)
{
return(false);
}
FP bdxcdy = bdx * cdy;
FP cdxbdy = cdx * bdy;
FP alift = adx * adx + ady * ady;
FP blift = bdx * bdx + bdy * bdy;
FP clift = cdx * cdx + cdy * cdy;
FP det = alift * (bdxcdy - cdxbdy) + blift * ocad + clift * oabd;
return(det > 0);
}
public override void PrepareTriangulation(Triangulatable t)
{
base.PrepareTriangulation(t);
FP xmax, xmin;
FP ymax, ymin;
xmax = xmin = Points[0].X;
ymax = ymin = Points[0].Y;
// Calculate bounds. Should be combined with the sorting
foreach (TriangulationPoint p in Points)
{
if (p.X > xmax)
{
xmax = p.X;
}
if (p.X < xmin)
{
xmin = p.X;
}
if (p.Y > ymax)
{
ymax = p.Y;
}
if (p.Y < ymin)
{
ymin = p.Y;
}
}
FP deltaX = ALPHA * (xmax - xmin);
FP deltaY = ALPHA * (ymax - ymin);
TriangulationPoint p1 = new TriangulationPoint(xmax + deltaX, ymin - deltaY);
TriangulationPoint p2 = new TriangulationPoint(xmin - deltaX, ymin - deltaY);
Head = p1;
Tail = p2;
// long time = System.nanoTime();
// Sort the points along y-axis
Points.Sort(_comparator);
// logger.info( "Triangulation setup [{}ms]", ( System.nanoTime() - time ) / 1e6 );
}
public Edge(Point p, Point q)
{
P = p;
Q = q;
if (q.X - p.X != 0)
{
Slope = (q.Y - p.Y) / (q.X - p.X);
}
else
{
Slope = 0;
}
B = p.Y - (p.X * Slope);
Above = null;
Below = null;
MPoints = new HashSet <Point>();
MPoints.Add(p);
MPoints.Add(q);
}
public override void Update(FP dt)
{
foreach (Body body in _bodies)
{
if (!IsActiveOn(body))
{
continue;
}
if (LimitLinearVelocity)
{
//Translation
// Check for large velocities.
FP translationX = dt * body._linearVelocity.x;
FP translationY = dt * body._linearVelocity.y;
FP result = translationX * translationX + translationY * translationY;
if (result > dt * _maxLinearSqared)
{
FP sq = FP.Sqrt(result);
FP ratio = _maxLinearVelocity / sq;
body._linearVelocity.x *= ratio;
body._linearVelocity.y *= ratio;
}
}
if (LimitAngularVelocity)
{
//Rotation
FP rotation = dt * body._angularVelocity;
if (rotation * rotation > _maxAngularSqared)
{
FP ratio = _maxAngularVelocity / FP.Abs(rotation);
body._angularVelocity *= ratio;
}
}
}
}
/// <summary>
/// ???
/// </summary>
/// <param name="node">middle node</param>
/// <returns>the angle between 3 front nodes</returns>
private static FP HoleAngle(AdvancingFrontNode node)
{
// XXX: do we really need a signed angle for holeAngle?
// could possible save some cycles here
/* Complex plane
* ab = cosA +i*sinA
* ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
* atan2(y,x) computes the principal value of the argument function
* applied to the complex number x+iy
* Where x = ax*bx + ay*by
* y = ax*by - ay*bx
*/
FP px = node.Point.X;
FP py = node.Point.Y;
FP ax = node.Next.Point.X - px;
FP ay = node.Next.Point.Y - py;
FP bx = node.Prev.Point.X - px;
FP by = node.Prev.Point.Y - py;
return(FP.Atan2(ax * by - ay * bx, ax * bx + ay * by));
}
private static FP Angle(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb)
{
/* Complex plane
* ab = cosA +i*sinA
* ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
* atan2(y,x) computes the principal value of the argument function
* applied to the complex number x+iy
* Where x = ax*bx + ay*by
* y = ax*by - ay*bx
*/
FP px = origin.X;
FP py = origin.Y;
FP ax = pa.X - px;
FP ay = pa.Y - py;
FP bx = pb.X - px;
FP by = pb.Y - py;
FP x = ax * by - ay * bx;
FP y = ax * bx + ay * by;
FP angle = FP.Atan2(x, y);
return(angle);
}
public static void ToSimUnits(FP x, FP y, out TSVector2 simUnits)
{
simUnits = TSVector2.zero;
simUnits.x = x * _simUnitsToDisplayUnitsRatio;
simUnits.y = y * _simUnitsToDisplayUnitsRatio;
}
public TriangulationPoint(FP x, FP y)
{
X = x;
Y = y;
}
private Point LineIntersect(Edge edge, FP x)
{
FP y = edge.Slope * x + edge.B;
return(new Point(x, y));
}
/// <summary>
/// MM: This seems to be used by LocateNode to guess a position in the implicit linked list of AdvancingFrontNodes near x
/// Removed an overload that depended on this being exact
/// </summary>
private AdvancingFrontNode FindSearchNode(FP x)
{
// TODO: implement BST index
return(Search);
}
public FP Evaluate(FP position)
{
CurveKey first = keys[0];
CurveKey last = keys[keys.Count - 1];
if (position < first.Position)
{
switch (PreLoop)
{
case CurveLoopType.Constant:
//constant
return(first.Value);
case CurveLoopType.Linear:
// linear y = a*x +b with a tangeant of last point
return(first.Value - first.TangentIn * (first.Position - position));
case CurveLoopType.Cycle:
//start -> end / start -> end
int cycle = GetNumberOfCycle(position);
FP virtualPos = position - (cycle * (last.Position - first.Position));
return(GetCurvePosition(virtualPos));
case CurveLoopType.CycleOffset:
//make the curve continue (with no step) so must up the curve each cycle of delta(value)
cycle = GetNumberOfCycle(position);
virtualPos = position - (cycle * (last.Position - first.Position));
return(GetCurvePosition(virtualPos) + cycle * (last.Value - first.Value));
case CurveLoopType.Oscillate:
//go back on curve from end and target start
// start-> end / end -> start
cycle = GetNumberOfCycle(position);
if (0 == cycle % 2f) //if pair
{
virtualPos = position - (cycle * (last.Position - first.Position));
}
else
{
virtualPos = last.Position - position + first.Position +
(cycle * (last.Position - first.Position));
}
return(GetCurvePosition(virtualPos));
}
}
else if (position > last.Position)
{
int cycle;
switch (PostLoop)
{
case CurveLoopType.Constant:
//constant
return(last.Value);
case CurveLoopType.Linear:
// linear y = a*x +b with a tangeant of last point
return(last.Value + first.TangentOut * (position - last.Position));
case CurveLoopType.Cycle:
//start -> end / start -> end
cycle = GetNumberOfCycle(position);
FP virtualPos = position - (cycle * (last.Position - first.Position));
return(GetCurvePosition(virtualPos));
case CurveLoopType.CycleOffset:
//make the curve continue (with no step) so must up the curve each cycle of delta(value)
cycle = GetNumberOfCycle(position);
virtualPos = position - (cycle * (last.Position - first.Position));
return(GetCurvePosition(virtualPos) + cycle * (last.Value - first.Value));
case CurveLoopType.Oscillate:
//go back on curve from end and target start
// start-> end / end -> start
cycle = GetNumberOfCycle(position);
virtualPos = position - (cycle * (last.Position - first.Position));
if (0 == cycle % 2f) //if pair
{
virtualPos = position - (cycle * (last.Position - first.Position));
}
else
{
virtualPos = last.Position - position + first.Position +
(cycle * (last.Position - first.Position));
}
return(GetCurvePosition(virtualPos));
}
}
//in curve
return(GetCurvePosition(position));
}
public AdvancingFrontNode(TriangulationPoint point)
{
Point = point;
Value = point.X;
}
public abstract void Update(FP dt);
public PolygonPoint(FP x, FP y) : base(x, y)
{
}
// TS - public static List<Vertices> ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid = true, FP tolerance = 0.001f)
public static List <Vertices> ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid, FP tolerance)
{
if (vertices.Count <= 3)
{
return new List <Vertices> {
vertices
}
}
;
List <Vertices> results;
switch (algorithm)
{
case TriangulationAlgorithm.Earclip:
if (Settings.SkipSanityChecks)
{
Debug.Assert(!vertices.IsCounterClockWise(), "The Earclip algorithm expects the polygon to be clockwise.");
}
else
{
if (vertices.IsCounterClockWise())
{
Vertices temp = new Vertices(vertices);
temp.Reverse();
results = EarclipDecomposer.ConvexPartition(temp, tolerance);
}
else
{
results = EarclipDecomposer.ConvexPartition(vertices, tolerance);
}
}
break;
case TriangulationAlgorithm.Bayazit:
if (Settings.SkipSanityChecks)
{
Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
}
else
{
if (!vertices.IsCounterClockWise())
{
Vertices temp = new Vertices(vertices);
temp.Reverse();
results = BayazitDecomposer.ConvexPartition(temp);
}
else
{
results = BayazitDecomposer.ConvexPartition(vertices);
}
}
break;
case TriangulationAlgorithm.Flipcode:
if (Settings.SkipSanityChecks)
{
Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
}
else
{
if (!vertices.IsCounterClockWise())
{
Vertices temp = new Vertices(vertices);
temp.Reverse();
results = FlipcodeDecomposer.ConvexPartition(temp);
}
else
{
results = FlipcodeDecomposer.ConvexPartition(vertices);
}
}
break;
case TriangulationAlgorithm.Seidel:
results = SeidelDecomposer.ConvexPartition(vertices, tolerance);
break;
case TriangulationAlgorithm.SeidelTrapezoids:
results = SeidelDecomposer.ConvexPartitionTrapezoid(vertices, tolerance);
break;
case TriangulationAlgorithm.Delauny:
results = CDTDecomposer.ConvexPartition(vertices);
break;
default:
throw new ArgumentOutOfRangeException("algorithm");
}
if (discardAndFixInvalid)
{
for (int i = results.Count - 1; i >= 0; i--)
{
Vertices polygon = results[i];
if (!ValidatePolygon(polygon))
{
results.RemoveAt(i);
}
}
}
return(results);
}
