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); }