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));
}
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 TrapezoidalMap()
{
Map = new HashSet <Trapezoid>();
_margin = 50.0f;
_bCross = null;
_cross = null;
}
private bool AngleSign()
{
Point a = (_head.Next - _head);
Point b = (_tail - _head);
return(FP.Atan2(a.Cross(b), a.Dot(b)) >= 0);
}
private FP GetCurvePosition(FP position)
{
//only for position in curve
CurveKey prev = keys[0];
CurveKey next;
for (int i = 1; i < keys.Count; i++)
{
next = Keys[i];
if (next.Position >= position)
{
if (prev.Continuity == CurveContinuity.Step)
{
if (position >= 1f)
{
return(next.Value);
}
return(prev.Value);
}
FP t = (position - prev.Position) / (next.Position - prev.Position); //to have t in [0,1]
FP ts = t * t;
FP tss = ts * t;
//After a lot of search on internet I have found all about spline function
// and bezier (phi'sss ancien) but finaly use hermite curve
//http://en.wikipedia.org/wiki/Cubic_Hermite_spline
//P(t) = (2*t^3 - 3t^2 + 1)*P0 + (t^3 - 2t^2 + t)m0 + (-2t^3 + 3t^2)P1 + (t^3-t^2)m1
//with P0.value = prev.value , m0 = prev.tangentOut, P1= next.value, m1 = next.TangentIn
return((2 * tss - 3 * ts + 1f) * prev.Value + (tss - 2 * ts + t) * prev.TangentOut + (3 * ts - 2 * tss) * next.Value +
(tss - ts) * next.TangentIn);
}
prev = next;
}
return(0f);
}
private FP Angle(Point p)
{
Point a = (p.Next - p);
Point b = (p.Prev - p);
return(FP.Atan2(a.Cross(b), a.Dot(b)));
}
// Partition a x-monotone mountain into triangles O(n)
// See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52
public void Process()
{
// Establish the proper sign
_positive = AngleSign();
// create monotone polygon - for dubug purposes
GenMonoPoly();
// Initialize internal angles at each nonbase vertex
// Link strictly convex vertices into a list, ignore reflex vertices
Point p = _head.Next;
while (p.Neq(_tail))
{
FP a = Angle(p);
// If the point is almost colinear with it's neighbor, remove it!
if (a >= PiSlop || a <= -PiSlop || a == 0.0f)
{
Remove(p);
}
else if (IsConvex(p))
{
_convexPoints.Add(p);
}
p = p.Next;
}
Triangulate();
}
public Point(FP x, FP y)
{
X = x;
Y = y;
Next = null;
Prev = null;
}
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);
}
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);
}
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 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);
}
/// <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;
}
/// 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);
}
/*
* 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);
}
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 );
}
/// <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 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;
}
}
}
}
public CurveKey(FP position, FP value)
: this(position, value, 0, 0, CurveContinuity.Smooth)
{
}
/// <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 AdvancingFrontNode(TriangulationPoint point)
{
Point = point;
Value = point.X;
}
public CurveKey(FP position, FP value, FP tangentIn, FP tangentOut)
: this(position, value, tangentIn, tangentOut, CurveContinuity.Smooth)
{
}
public PolygonPoint(FP x, FP y) : base(x, y)
{
}
private Point LineIntersect(Edge edge, FP x)
{
FP y = edge.Slope * x + edge.B;
return(new Point(x, y));
}
