private void DeclareInclude(IPathSegment patternSegment, bool isLastSegment)
{
var literalSegment = patternSegment as LiteralPathSegment;
if (literalSegment != null)
{
if (isLastSegment)
{
_declaredLiteralFileSegments.Add(literalSegment);
}
else
{
_declaredLiteralFolderSegments.Add(literalSegment);
_declaredLiteralFolderSegmentInString.Add(literalSegment.Value);
}
}
else if (patternSegment is ParentPathSegment)
{
_declaredParentPathSegment = true;
}
else if (patternSegment is WildcardPathSegment)
{
_declaredWildcardPathSegment = true;
}
}
public RelationBinaryOperationPathSegment(
IPathSegment left,
IPathSegment <RelationalOperationTypes> @operator,
IPathSegment right
) : base(left, @operator, right)
{
}
public override void PushDirectory(DirectoryInfoBase directory)
{
// copy the current frame
FrameData frame = Frame;
if (IsStackEmpty() || Frame.IsNotApplicable)
{
// when the stack is being initialized
// or no change is required.
}
else if (!TestMatchingSegment(directory.Name))
{
// nothing down this path is affected by this pattern
frame.IsNotApplicable = true;
}
else
{
// Determine this frame's contribution to the stem (if any)
IPathSegment segment = Pattern.Segments[Frame.SegmentIndex];
if (frame.InStem || segment.CanProduceStem)
{
frame.InStem = true;
frame.StemItems.Add(directory.Name);
}
// directory matches segment, advance position in pattern
frame.SegmentIndex = frame.SegmentIndex + 1;
}
PushDataFrame(frame);
}
public static bool CheckPathsEqual(IPath p1, IPath p2)
{
if (p1 == null)
{
return(false);
}
if (p2 == null)
{
return(false);
}
if (p1.Count != p2.Count)
{
return(false);
}
for (int i = 0; i < p1.Count; i++)
{
IPathSegment segment1 = p1[i];
IPathSegment segment2 = p2[i];
if (segment1.Start != segment2.Start || segment1.End != segment2.End)
{
return(false);
}
}
return(true);
}
IndexedPathSegmentTreeNode <string> CreatePathSegmentIndexedPathSegmentTreeNode(
XmlPathSegment pathSegment, IPathSegment parentPathSegment, IndexedPathSegmentTreeNode <string> parentNode)
{
var newIndexedValueTreeNode = new IndexedPathSegmentTreeNode <string>();
if (parentNode.EnumerationComplete)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
if (parentNode.CurrentValue is XDocument)
{
var document = parentNode.CurrentValue as XDocument;
newIndexedValueTreeNode.CurrentValue = document.Root;
}
else
{
XElementSegment(pathSegment, parentPathSegment, parentNode, newIndexedValueTreeNode);
}
}
return(newIndexedValueTreeNode);
}
protected virtual void BuildIndexedTree(IList <IPath> paths,
Dictionary <IPath, List <IPathSegment> > indexedPathSegments,
IndexedPathSegmentTreeNode <string> rootIndexedValueTreeNode)
{
foreach (IPath path in paths)
{
IndexedPathSegmentTreeNode <string> IndexedPathSegmentTreeNode = rootIndexedValueTreeNode;
int pathSegmentCount = 0;
while (pathSegmentCount < indexedPathSegments[path].Count)
{
IndexedPathSegmentTreeNode <string> tmpIndexedPathSegmentTreeNode;
IPathSegment pathSegment = indexedPathSegments[path][pathSegmentCount];
if (
!IndexedPathSegmentTreeNode.TryGetValue(pathSegment.ActualSegment,
out tmpIndexedPathSegmentTreeNode))
{
IndexedPathSegmentTreeNode <string> newIndexedPathSegmentTreeNode =
CreatePathSegmentIndexedPathSegmentTreeNode(pathSegment, IndexedPathSegmentTreeNode);
IndexedPathSegmentTreeNode.Add(pathSegment.ActualSegment, newIndexedPathSegmentTreeNode);
IndexedPathSegmentTreeNode = newIndexedPathSegmentTreeNode;
}
else
{
IndexedPathSegmentTreeNode = tmpIndexedPathSegmentTreeNode;
}
pathSegmentCount++;
}
}
}
/// <summary>
/// Returns twice the area under the quadrilateral formed between the segment and the y-axis.
/// + if points are ordered counter-clockwise.
/// </summary>
/// <param name="segment">The segment.</param>
/// <returns>System.Double.</returns>
protected double Area_i(IPathSegment segment)
{
CartesianCoordinate pt1 = segment.I;
CartesianCoordinate pt2 = segment.J;
return(pt1.X * pt2.Y - pt2.X * pt1.Y);
}
IEnumerable <string> SelectEnumberable(IList <IPathSegment> pathSegments, IPathSegment parentPathSegment, XElement element)
{
var returnData = new List <string>();
var currentElement = element;
if (pathSegments.Count > 0)
{
for (int i = 0; i < pathSegments.Count; i++)
{
var pathSegment = pathSegments[i] as XmlPathSegment;
XmlPathSegment previousPathSegment;
previousPathSegment = i > 0 ? pathSegments[i - 1] as XmlPathSegment : parentPathSegment as XmlPathSegment;
var lastSegment = i == pathSegments.Count - 1;
if (previousPathSegment != null && previousPathSegment.IsEnumarable)
{
return(SelectEnumerable(pathSegments, returnData, i, pathSegment, lastSegment, currentElement));
}
currentElement = SelectEnumerable(ref returnData, currentElement, pathSegment, lastSegment);
}
}
else
{
if (currentElement.Name == parentPathSegment.ActualSegment)
{
returnData.Add(currentElement.Value);
}
}
return(returnData);
}
/// <summary>
/// Parses the key properties based on the segment's target type, then creates a new segment for the key.
/// </summary>
/// <param name="segment">The segment to apply the key to.</param>
/// <param name="key">The key to apply.</param>
/// <returns>The newly created key segment.</returns>
private static IPathSegment CreateKeySegment(IPathSegment segment, KeyInstance key)
{
Debug.Assert(segment != null, "segment != null");
Debug.Assert(key != null && !key.IsEmpty, "key != null && !key.IsEmpty");
Debug.Assert(segment.SingleResult == false, "segment.SingleResult == false");
IEdmEntityType targetEntityType = null;
WebUtil.CheckSyntaxValid(segment.TargetEdmType != null && segment.TargetEdmType.IsEntityOrEntityCollectionType(out targetEntityType));
Debug.Assert(targetEntityType != null, "targetEntityType != null");
// Make sure the keys specified in the uri matches with the number of keys in the metadata
var keyProperties = targetEntityType.Key().ToList();
if (keyProperties.Count != key.ValueCount)
{
throw ExceptionUtil.CreateBadRequestError(ErrorStrings.BadRequest_KeyCountMismatch(targetEntityType.FullName()));
}
if (!key.AreValuesNamed && key.ValueCount > 1)
{
throw ExceptionUtil.CreateBadRequestError(ErrorStrings.RequestUriProcessor_KeysMustBeNamed);
}
WebUtil.CheckSyntaxValid(key.TryConvertValues(keyProperties));
return(new PathSegment(segment)
{
Key = key,
SingleResult = true,
});
}
public LogicBinaryOperationPathSegment(
IPathSegment left,
IPathSegment <LogicOperationTypes> @operator,
IPathSegment right
) : base(left, @operator, right)
{
}
protected override IndexedPathSegmentTreeNode <string> CreatePathSegmentIndexedPathSegmentTreeNode(
IPathSegment pathSegment, IndexedPathSegmentTreeNode <string> parentNode)
{
var newIndexedValueTreeNode = new IndexedPathSegmentTreeNode <string>();
if (parentNode.EnumerationComplete)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
if (pathSegment.IsEnumarable)
{
newIndexedValueTreeNode = IndexedEnumarablePathSegmentTreeNode(newIndexedValueTreeNode, pathSegment, parentNode);
}
else
{
newIndexedValueTreeNode.CurrentValue = GetScalarValueForPathSegement(pathSegment,
parentNode.CurrentValue);
if (newIndexedValueTreeNode.CurrentValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
}
}
return(newIndexedValueTreeNode);
}
/// <summary>
/// Determines if the point lies to the left of the segment extents max x-coordinate, not including the boundary coordinate.
/// </summary>
/// <param name="xPtN">The x-coordinate of pt n.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point lies to the left of the segment end, <c>false</c> otherwise.</returns>
public static bool PointIsLeftOfSegmentExtentsEndExclusive(
double xPtN,
IPathSegment segment)
{
PointExtents extents = segment.Extents;
return(xPtN < extents.MaxX);
}
/// <summary>
/// Determines if the point lies within the segment extents height, not including the boundary locations.
/// </summary>
/// <param name="yPtN">The y-coordinate of pt n.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point lies within the segment extents height, <c>false</c> otherwise.</returns>
public static bool PointIsWithinSegmentExtentsHeightExclusive(
double yPtN,
IPathSegment segment)
{
PointExtents extents = segment.Extents;
return(extents.MinY < yPtN && yPtN < extents.MaxY);
}
/// <summary>
/// Determines if the point lies below the segment extents end.
/// </summary>
/// <param name="yPtN">The y-coordinate of pt n.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point lies to below the segment bottom, <c>false</c> otherwise.</returns>
public static bool PointIsBelowSegmentExtentsExclusive(
double yPtN,
IPathSegment segment)
{
PointExtents extents = segment.Extents;
return(yPtN < extents.MaxY);
}
public BinaryPathSegment(
IPathSegment left,
IPathSegment right
)
{
Left = left;
Right = right;
}
/// <summary>
/// Determines if the point is to the left of the horizontally projected segment intersection.
/// </summary>
/// <param name="xPtN">The x-coordinate of pt n.</param>
/// <param name="xIntersection">The x-coordinate of the intersection of the projected line.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point is to the left of the horizontally projected segment intersection, <c>false</c> otherwise.</returns>
public static bool PointIsLeftOfSegmentIntersection(
double xPtN,
double xIntersection,
IPathSegment segment)
{
return(xPtN < xIntersection &&
PointIsWithinSegmentExtentsWidthInclusive(xIntersection, segment));
}
/// <summary>
/// Determines if the point is below the vertically projected segment intersection.
/// </summary>
/// <param name="yPtN">The y-coordinate of pt n.</param>
/// <param name="yIntersection">The y-coordinate of the intersection of the projected line.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point is below the vertically projected segment intersection, <c>false</c> otherwise.</returns>
public static bool PointIsBelowSegmentIntersection(
double yPtN,
double yIntersection,
IPathSegment segment)
{
return(yPtN < yIntersection &&
PointIsWithinSegmentExtentsHeightInclusive(yIntersection, segment));
}
public FunctionPathSegment(
IPathSegment name,
IPathSegment parameters
)
{
Name = name;
Parameters = parameters;
}
/// <summary>
/// Determines if the point lies within the segment extents width.
/// </summary>
/// <param name="xPtN">The x-coordinate of pt n.</param>
/// <param name="segment">The segment.</param>
/// <returns><c>true</c> if the point lies within the segment width, <c>false</c> otherwise.</returns>
public static bool PointIsWithinSegmentExtentsWidthExclusive(
double xPtN,
IPathSegment segment)
{
PointExtents extents = segment.Extents;
return(extents.MinX < xPtN && xPtN < extents.MaxX);
}
protected BinaryOperationPathSegment(
IPathSegment left,
IPathSegment <T> @operator,
IPathSegment right
) : base(left, right)
{
Operator = @operator;
}
private IEnumerable <object> SelectEnumberable(IList <IPathSegment> pathSegments, JToken data)
{
var returnData = new List <object>();
JToken currentData = data;
for (int i = 0; i < pathSegments.Count; i++)
{
IPathSegment pathSegment = pathSegments[i];
bool lastSegment = i == pathSegments.Count - 1;
if (pathSegment.IsEnumarable)
{
IEnumerable enumerableData = GetEnumerableValueForPathSegment(pathSegment, currentData);
if (enumerableData != null)
{
IEnumerator enumerator = enumerableData.GetEnumerator();
enumerator.Reset();
var testToken = enumerableData as JToken;
if (testToken.IsEnumerableOfPrimitives())
{
while (enumerator.MoveNext())
{
var currentToken = enumerator.Current as JToken;
if (currentData != null)
{
if (currentToken != null)
{
returnData.Add(currentToken.ToString());
}
}
}
}
else
{
while (enumerator.MoveNext())
{
returnData.AddRange(SelectEnumberable(pathSegments.Skip(i + 1).ToList(),
enumerator.Current as JToken));
}
}
}
return(returnData);
}
currentData = GetScalarValueForPathSegement(pathSegment, currentData);
if (currentData != null && lastSegment)
{
returnData.Add(currentData.ToString());
}
}
return(returnData);
}
/// <summary>
/// Returns a copy of the segment that joins the current segment with the prior segment.
/// </summary>
/// <param name="priorSegment">The prior segment.</param>
/// <returns>IPathSegment.</returns>
public override IPathSegment JoinWithPriorSegment(IPathSegment priorSegment)
{
if ((priorSegment.I == I || priorSegment.I == J) ||
(priorSegment.J == I || priorSegment.J == J))
{ // segments already joined
return(null);
}
return(new LineSegment(priorSegment.J, I));
}
private void AddPathSegmentToSkiaPath(SKPath skPath, IPathSegment pathSegment)
{
if (pathSegment is IBezierSegment bezierSegment)
{
skPath.CubicTo(
(float)bezierSegment.Point1.X, (float)bezierSegment.Point1.Y,
(float)bezierSegment.Point2.X, (float)bezierSegment.Point2.Y,
(float)bezierSegment.Point3.X, (float)bezierSegment.Point3.Y);
}
else if (pathSegment is IPolyBezierSegment polyBezierSegment)
{
List <Point> points = new List <Point>();
foreach (Point point in polyBezierSegment.Points)
{
points.Add(point);
}
if (points.Count % 3 != 0)
{
throw new InvalidOperationException($"IPolyBezerSegment contains {points.Count} points, which isn't a multiple of 3");
}
for (int i = 0; i < points.Count;)
{
var point1 = points[i + 0];
var point2 = points[i + 1];
var point3 = points[i + 2];
skPath.CubicTo(
(float)point1.X, (float)point1.Y,
(float)point2.X, (float)point2.Y,
(float)point3.X, (float)point3.Y);
i += 3;
}
}
else if (pathSegment is ILineSegment lineSegment)
{
skPath.LineTo((float)lineSegment.Point.X, (float)lineSegment.Point.Y);
}
else if (pathSegment is IQuadraticBezierSegment quadraticBezierSegment)
{
skPath.QuadTo(
(float)quadraticBezierSegment.Point1.X, (float)quadraticBezierSegment.Point1.Y,
(float)quadraticBezierSegment.Point2.X, (float)quadraticBezierSegment.Point2.Y);
}
else if (pathSegment is IPolyLineSegment polyLineSegment)
{
var skiaPoints = new List <SKPoint>();
AddSkiaPoints(polyLineSegment.Points, skiaPoints);
skPath.AddPoly(skiaPoints.ToArray());
}
else
{
throw new InvalidOperationException($"IPathSegment type {pathSegment.GetType()} not yet implemented");
}
}
/// <summary>
/// Returns a copy of the segment that joins the current segment with the following segment.
/// </summary>
/// <param name="followingSegment">The following segment.</param>
/// <returns>IPathSegment.</returns>
public override IPathSegment JoinWithFollowingSegment(IPathSegment followingSegment)
{
if ((followingSegment.I == I || followingSegment.I == J) ||
(followingSegment.J == I || followingSegment.J == J))
{ // segments already joined
return(null);
}
return(new LineSegment(J, followingSegment.I));
}
private void Remove(IPathSegment segment)
{
if (segment == null)
{
return;
}
segment.PropertyChanged -= ObserveShape;
if (segment is LineSegment)
{
var lineSegment = segment as LineSegment;
lineSegment.Point.PropertyChanged -= ObserveShape;
}
else if (segment is ArcSegment)
{
var arcSegment = segment as ArcSegment;
arcSegment.Point.PropertyChanged -= ObserveShape;
arcSegment.Size.PropertyChanged -= ObserveShape;
}
else if (segment is CubicBezierSegment)
{
var cubicBezierSegment = segment as CubicBezierSegment;
cubicBezierSegment.Point1.PropertyChanged -= ObserveShape;
cubicBezierSegment.Point2.PropertyChanged -= ObserveShape;
cubicBezierSegment.Point3.PropertyChanged -= ObserveShape;
}
else if (segment is QuadraticBezierSegment)
{
var quadraticBezierSegment = segment as QuadraticBezierSegment;
quadraticBezierSegment.Point1.PropertyChanged -= ObserveShape;
quadraticBezierSegment.Point2.PropertyChanged -= ObserveShape;
}
else if (segment is PolyLineSegment)
{
var polyLineSegment = segment as PolyLineSegment;
Remove(polyLineSegment.Points);
}
else if (segment is PolyCubicBezierSegment)
{
var polyCubicBezierSegment = segment as PolyCubicBezierSegment;
Remove(polyCubicBezierSegment.Points);
}
else if (segment is PolyQuadraticBezierSegment)
{
var polyQuadraticBezierSegment = segment as PolyQuadraticBezierSegment;
Remove(polyQuadraticBezierSegment.Points);
}
}
public void ToStringOnScalarSegment_Expected_ScalarFormat()
{
XmlPath path = new XmlPath();
IPathSegment segment = path.CreatePathSegment("Name");
const string expected = "Name";
string actual = segment.ToString();
Assert.AreEqual(expected, actual);
}
public void ToStringOnEnumerableSegment_WhereEnumerablesArentConsidered_Expected_ScalarFormat()
{
XmlPath path = new XmlPath();
IPathSegment segment = path.CreatePathSegment("Departments()");
const string expected = "Departments";
string actual = segment.ToString(false);
Assert.AreEqual(expected, actual);
}
public void ToStringOnScalarSegmentt_WhereEnumerablesAreConsidered__Expected_ScalarFormat()
{
XmlPath path = new XmlPath();
IPathSegment segment = path.CreatePathSegment("Name");
const string expected = "Name";
string actual = segment.ToString(true);
Assert.AreEqual(expected, actual);
}
public void CreateEnumerablePathSegmentFromSegmentText_Expected_EnumerableXmlPathSegment()
{
XmlPath path = new XmlPath();
IPathSegment segment = path.CreatePathSegment("Departments()");
bool expected = true;
bool actual = segment.IsEnumarable;
Assert.AreEqual(expected, actual);
}
public void CreateScalarPathSegmentFromSegmentText_Expected_ScalarXmlPathSegment()
{
XmlPath path = new XmlPath();
IPathSegment segment = path.CreatePathSegment("Name");
bool expected = false;
bool actual = segment.IsEnumarable;
Assert.AreEqual(expected, actual);
}
/// <summary>
/// Tries to handle the current segment as a key property value.
/// </summary>
/// <param name="segmentText">The segment text.</param>
/// <param name="previous">The previous segment.</param>
/// <param name="urlConvention">The current url convention for the server.</param>
/// <param name="keySegment">The key segment that was created if the segment could be interpreted as a key.</param>
/// <returns>Whether or not the segment was interpreted as a key.</returns>
internal static bool TryHandleSegmentAsKey(string segmentText, IPathSegment previous, UrlConvention urlConvention, out IPathSegment keySegment)
{
DebugUtils.CheckNoExternalCallers();
Debug.Assert(previous != null, "previous != null");
Debug.Assert(urlConvention != null, "urlConvention != null");
keySegment = null;
// If the current convention is not keys-as-segments, then this does not apply.
if (!urlConvention.GenerateKeyAsSegment)
{
return false;
}
// If the current identifier was preceeded by a '$' segment, then do not treat it as a key.
if (previous.IsEscapeMarker)
{
return false;
}
// Keys only apply to collections, so if the prior segment is already a singleton, do not treat this segment as a key.
if (previous.SingleResult)
{
return false;
}
// System segments (ie '$count') are never keys.
if (IsSystemSegment(segmentText))
{
return false;
}
// If the previous type is not an entity collection type
// TODO: collapse this and SingleResult.
IEdmEntityType targetEntityType;
if (previous.TargetEdmType == null || !previous.TargetEdmType.IsEntityOrEntityCollectionType(out targetEntityType))
{
return false;
}
// If the resource type has more than 1 key property, then do not treat the segment as a key.
var keyProperties = targetEntityType.Key().ToList();
if (keyProperties.Count > 1)
{
return false;
}
// At this point it must be treated as a key, so fail if it is malformed.
Debug.Assert(keyProperties.Count == 1, "keyProperties.Count == 1");
keySegment = CreateKeySegment(previous, KeyInstance.FromSegment(segmentText));
return true;
}
/// <summary>Tries to create a key segment for the given filter if it is non empty.</summary>
/// <param name="previous">Segment on which to compose.</param>
/// <param name="filter">Filter portion of segment, possibly null.</param>
/// <param name="keySegment">The key segment that was created if the key was non-empty.</param>
/// <returns>Whether the key was non-empty.</returns>
internal static bool TryCreateKeySegmentFromParentheses(IPathSegment previous, string filter, out IPathSegment keySegment)
{
DebugUtils.CheckNoExternalCallers();
Debug.Assert(filter != null, "filter != null");
Debug.Assert(previous != null, "segment!= null");
WebUtil.CheckSyntaxValid(!previous.SingleResult);
KeyInstance key;
WebUtil.CheckSyntaxValid(KeyInstance.TryParseKeysFromUri(filter, out key));
if (key.IsEmpty)
{
keySegment = null;
return false;
}
keySegment = CreateKeySegment(previous, key);
return true;
}
public bool Equals(IPathSegment other)
{
if (other is BezierPathSegment) {
BezierPathSegment bp = (BezierPathSegment)other;
// check that all the point match up
return bp.cb.P0 == cb.P0 && bp.cb.P1 == cb.P1 && bp.cb.P2 == cb.P2 && bp.cb.P3 == cb.P3;
}
else {
return false;
}
}
private bool DoSegmentsCollide(IPathSegment seg1, IPathSegment seg2, out double collideDist1, out double collideDist2)
{
bool doesIntersect = false;
Vector2 intersectPoint;
Vector2[] p = new Vector2[4];
collideDist1 = Double.MaxValue;
collideDist2 = Double.MaxValue;
LineSegment ls1 = new LineSegment(seg1.Start, seg1.End);
LineSegment ls2 = new LineSegment(seg2.Start, seg2.End);
doesIntersect = ls1.Intersect(ls2, out intersectPoint);
p[0] = ls1.ClosestPoint(seg2.Start);
p[1] = ls1.ClosestPoint(seg2.End);
p[2] = ls2.ClosestPoint(seg1.Start);
p[3] = ls2.ClosestPoint(seg1.End);
if (doesIntersect)
{
collideDist1 = seg1.Start.DistanceTo(intersectPoint);
collideDist2 = seg2.Start.DistanceTo(intersectPoint);
}
if ((p[0] - seg2.Start).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[0]))
{
collideDist1 = seg1.Start.DistanceTo(p[0]);
collideDist2 = 0;
doesIntersect = true;
}
}
if ((p[1] - seg2.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[1]))
{
collideDist1 = seg1.Start.DistanceTo(p[1]);
collideDist2 = seg2.Length;
doesIntersect = true;
}
}
if ((p[2] - seg1.Start).Length < collideThreshold)
{
collideDist1 = 0;
collideDist2 = seg2.Start.DistanceTo(p[2]);
}
if ((p[3] - seg1.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Length)
{
collideDist1 = seg1.Length;
collideDist2 = seg2.Start.DistanceTo(p[3]);
}
}
return doesIntersect;
}
protected virtual IndexedPathSegmentTreeNode<string> CreatePathSegmentIndexedPathSegmentTreeNode(
IPathSegment pathSegment, IndexedPathSegmentTreeNode<string> parentNode)
{
return null;
}
private void PlanPurePursuit()
{
if (pathCurrentlyTracked == null) return;
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0)
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
}
else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
{
//Console.WriteLine("2");
double lookaheadRef = 0;
PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
//the path lookahead point is at the end, and we cant do antyhing
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
goalPoint = lookaheadPointOnPath.pt;
}
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
//Console.WriteLine(errorX + " | " + errorY);
double errorDistance = verr.Length;
double currentYaw = currentPoint.yaw;
double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
//Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||");
//the reason for this is that our angles are defined from 0 to 360
//but the PID controller expects angle to be -180 to 180
//calculate the control outputs
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double commandedVelocity = (kPPosition * errorDistance);
double commandedHeading = kPHeading * errorYaw;
if (Math.Abs(commandedVelocity) > capVelocity) commandedVelocity = Math.Sign(commandedVelocity) * capVelocity;
if (Math.Abs(commandedHeading) > capHeadingDot) commandedHeading = Math.Sign(commandedHeading) * capHeadingDot;
//if (unsignedYawErrorNormalizeToOne > .1)
// find which input to use
RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)];
//commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));
//command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2)
command = new RobotTwoWheelCommand(0, 0);
else
{
if (currentInput.velocity < 0.3)
command = new RobotTwoWheelCommand(0.4, commandedHeading);
else
command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading);
}
//if (reachedGoal)
//{
// command.velocity = 0;
// command.turn = 0;
//}
//Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading);
//Console.WriteLine();
}
public void UpdatePath(IPath path)
{
if (path == null || path.Count == 0) return;
if (PathUtils.CheckPathsEqual(path, pathCurrentlyTracked)) { this.endpointsame = true; this.pathsame = true; return; } //
if (pathCurrentlyTracked != null && pathCurrentlyTracked.EndPoint.pt.X == path.EndPoint.pt.X && pathCurrentlyTracked.EndPoint.pt.Y == path.EndPoint.pt.Y)
{
this.endpointsame = true;
}
this.pathsame = false;
this.endpointsame = false;
pathCurrentlyTracked = path;
segmentCurrentlyTracked = path[0];
}
private IEnumerable GetEnumerableValueForPathSegment(IPathSegment pathSegment, IEnumerable<JToken> data)
{
var jObject = data as JObject;
IEnumerable returnVal = null;
if (jObject != null)
{
JProperty property = jObject.Property(pathSegment.ActualSegment);
if (property != null && property.IsEnumerable())
{
returnVal = property.Value as JArray;
}
}
return returnVal;
}
public virtual bool Equals(IPathSegment other)
{
LinePathSegment lps = other as LinePathSegment;
if (lps != null)
{
return lps.start == start && lps.end == end;
}
else
{
return false;
}
}
public override bool Equals(IPathSegment other)
{
return Equals(other);
}
public void UpdatePath(IPath path)
{
if (path == null || path.Count == 0) return;
if (PathUtils.CheckPathsEqual(path, pathCurrentlyTracked)) return;
lock (followerLock)
{
pathCurrentlyTracked = path;
segmentCurrentlyTracked = path[0];
}
}
private object GetScalarValueForPathSegement(IPathSegment pathSegment, object data)
{
PropertyInfo propertyInfo = data.GetType().GetProperty(pathSegment.ActualSegment);
object returnVal = null;
if (propertyInfo != null)
{
returnVal = propertyInfo.GetValue(data, null);
}
return returnVal;
}
/// <summary>
/// Checks if the path contains the specified segment.
/// </summary>
/// <param name="item">Segment to search for.</param>
/// <returns>True if the item is found in the path, false otherwise.</returns>
public bool Contains(IPathSegment item)
{
return segments.Contains(item);
}
/// <summary>
/// Add a path segment to the end of the path.
/// </summary>
/// <param name="item">New item to add</param>
/// <remarks>
/// There is no checking done to ensure that the EndPoint of the previous segment in the path matches the StartPoint of the new segment.
/// This checking must be done by the user.
/// </remarks>
public void Add(IPathSegment item)
{
segments.Add(item);
}
private IEnumerable<string> SelectEnumberable(IList<IPathSegment> pathSegments, IPathSegment parentPathSegment,
XElement element)
{
var returnData = new List<string>();
XElement currentElement = element;
if (pathSegments.Count > 0)
{
for (int i = 0; i < pathSegments.Count; i++)
{
var pathSegment = pathSegments[i] as XmlPathSegment;
XmlPathSegment previousPathSegment;
if (i > 0)
{
previousPathSegment = pathSegments[i - 1] as XmlPathSegment;
}
else
{
previousPathSegment = parentPathSegment as XmlPathSegment;
}
bool lastSegment = i == pathSegments.Count - 1;
if (previousPathSegment != null && previousPathSegment.IsEnumarable)
{
if (currentElement != null)
{
if (pathSegment != null)
{
List<XElement> childElements =
currentElement.Elements(pathSegment.ActualSegment).ToList();
if (childElements.Count > 0)
{
if (lastSegment)
{
foreach (XElement childElement in childElements)
{
if (pathSegment.IsAttribute)
{
XAttribute attribute = childElement.Attribute(pathSegment.ActualSegment);
if (attribute != null)
{
returnData.Add(attribute.Value);
}
else
{
throw new Exception(string.Format("Attribute {0} not found.",
pathSegment.ActualSegment));
}
}
else
{
returnData.Add(childElement.Value);
}
}
}
else
{
foreach (XElement childElement in childElements)
{
returnData.AddRange(SelectEnumberable(pathSegments.Skip(i + 1).ToList(),
pathSegment, childElement));
}
}
}
}
}
return returnData;
}
if (pathSegment != null && pathSegment.IsAttribute)
{
if (currentElement != null)
{
XAttribute attribute = currentElement.Attribute(pathSegment.ActualSegment);
if (attribute != null)
{
currentElement = null;
if (lastSegment)
{
returnData.Add(attribute.Value);
}
}
}
}
else
{
if (currentElement != null)
{
if (pathSegment != null)
{
currentElement = currentElement.Element(pathSegment.ActualSegment);
}
}
if (currentElement != null && lastSegment)
{
returnData.Add(currentElement.Value);
}
}
}
}
else if (currentElement.Name == parentPathSegment.ActualSegment)
{
returnData.Add(currentElement.Value);
}
return returnData;
}
public void UpdatePath(IPath path)
{
lock (followerLock)
{
if (PathUtils.CheckPathsEqual(path, pathCurrentlyTracked)) return;
pathCurrentlyTracked = path;
segmentCurrentlyTracked = path[0];
nextSegmentToTrack = path[1];
}
}
private IndexedPathSegmentTreeNode<string> CreatePathSegmentIndexedPathSegmentTreeNode(
XmlPathSegment pathSegment, IPathSegment parentPathSegment, IndexedPathSegmentTreeNode<string> parentNode)
{
var newIndexedValueTreeNode = new IndexedPathSegmentTreeNode<string>();
if (parentNode.EnumerationComplete)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
if (parentNode.CurrentValue is XDocument)
{
var document = parentNode.CurrentValue as XDocument;
newIndexedValueTreeNode.CurrentValue = document.Root;
}
else
{
var parentCurentElement = parentNode.CurrentValue as XElement;
if (parentPathSegment != null && parentPathSegment.IsEnumarable)
{
if (parentCurentElement != null)
{
List<XElement> childElements =
parentCurentElement.Elements(pathSegment.ActualSegment).ToList();
newIndexedValueTreeNode.EnumerableValue = childElements;
if (childElements.Count == 0)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
newIndexedValueTreeNode.Enumerator =
newIndexedValueTreeNode.EnumerableValue.GetEnumerator();
newIndexedValueTreeNode.Enumerator.Reset();
if (!newIndexedValueTreeNode.Enumerator.MoveNext())
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
newIndexedValueTreeNode.CurrentValue = newIndexedValueTreeNode.Enumerator.Current;
}
}
}
}
else
{
if (pathSegment.IsAttribute)
{
if (parentCurentElement != null)
{
newIndexedValueTreeNode.CurrentValue =
parentCurentElement.Attribute(pathSegment.ActualSegment);
}
if (newIndexedValueTreeNode.CurrentValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
}
else
{
if (parentCurentElement != null)
{
newIndexedValueTreeNode.CurrentValue =
parentCurentElement.Element(pathSegment.ActualSegment);
}
if (newIndexedValueTreeNode.CurrentValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
}
}
}
}
return newIndexedValueTreeNode;
}
protected override IndexedPathSegmentTreeNode<string> CreatePathSegmentIndexedPathSegmentTreeNode(
IPathSegment pathSegment, IndexedPathSegmentTreeNode<string> parentNode)
{
var newIndexedValueTreeNode = new IndexedPathSegmentTreeNode<string>();
if (parentNode.EnumerationComplete)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
if (pathSegment.IsEnumarable)
{
var data = parentNode.CurrentValue as JToken;
newIndexedValueTreeNode.EnumerableValue = GetEnumerableValueForPathSegment(pathSegment, data);
if (newIndexedValueTreeNode.EnumerableValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
bool isPrimitiveArray = false;
var jObject = data as JObject;
if (jObject != null)
{
JProperty property = jObject.Property(pathSegment.ActualSegment);
isPrimitiveArray = property.IsEnumerableOfPrimitives();
}
newIndexedValueTreeNode.Enumerator = newIndexedValueTreeNode.EnumerableValue.GetEnumerator();
newIndexedValueTreeNode.Enumerator.Reset();
if (isPrimitiveArray)
{
var valueBuilder = new StringBuilder();
while (newIndexedValueTreeNode.Enumerator.MoveNext())
{
valueBuilder.Append(newIndexedValueTreeNode.Enumerator.Current);
valueBuilder.Append(",");
}
newIndexedValueTreeNode.EnumerationComplete = true;
newIndexedValueTreeNode.CurrentValue = valueBuilder.ToString().TrimEnd(',');
}
else
{
if (!newIndexedValueTreeNode.Enumerator.MoveNext())
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
newIndexedValueTreeNode.CurrentValue = newIndexedValueTreeNode.Enumerator.Current;
}
}
}
}
else
{
newIndexedValueTreeNode.CurrentValue = GetScalarValueForPathSegement(pathSegment,
parentNode.CurrentValue as JToken);
if (newIndexedValueTreeNode.CurrentValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
}
}
return newIndexedValueTreeNode;
}
/// <summary>
/// Copies the segments of the path to an array, starting at a particular array index
/// </summary>
/// <param name="array">
/// The one-dimensional array that is the destination of the elements copied from the path. The array must have zero-based indexing.
/// </param>
/// <param name="arrayIndex">The zero-based index in array at which copying begins.</param>
public void CopyTo(IPathSegment[] array, int arrayIndex)
{
segments.CopyTo(array, arrayIndex);
}
private IEnumerable GetEnumerableValueForPathSegment(IPathSegment pathSegment, object data)
{
PropertyInfo propertyInfo = data.GetType().GetProperty(pathSegment.ActualSegment);
IEnumerable returnVal = null;
if (propertyInfo != null && propertyInfo.PropertyType.IsEnumerable())
{
returnVal = propertyInfo.GetValue(data, null) as IEnumerable;
}
return returnVal;
}
/// <summary>
/// Returns the index of the specified segment.
/// </summary>
/// <param name="item">Segment to look for.</param>
/// <returns>
/// Zero-based index of segment if found, -1 if not found.
/// </returns>
public int IndexOf(IPathSegment item)
{
return segments.IndexOf(item);
}
/// <summary>
/// Inserts the segment at <paramref name="index"/>.
/// </summary>
/// <param name="index">
/// Zero-based index to insert at. The new segment is placed before the segment currently at index.
/// If <paramref name="index"/> equals <see cref="Count"/>, new segment is placed at the end of the collection.
/// </param>
/// <param name="item">New segment to insert.</param>
/// <remarks>
/// There is no checking done to ensure that the <see cref="IPathSegment.EndPoint"/> of the previous segment in the path matches the <see cref="IPathSegment.StartPoint"/> of the new segment.
/// This checking must be done by the user.
/// </remarks>
public void Insert(int index, IPathSegment item)
{
segments.Insert(index, item);
}
/// <summary>
/// Parses the key properties based on the segment's target type, then creates a new segment for the key.
/// </summary>
/// <param name="segment">The segment to apply the key to.</param>
/// <param name="key">The key to apply.</param>
/// <returns>The newly created key segment.</returns>
private static IPathSegment CreateKeySegment(IPathSegment segment, KeyInstance key)
{
Debug.Assert(segment != null, "segment != null");
Debug.Assert(key != null && !key.IsEmpty, "key != null && !key.IsEmpty");
Debug.Assert(segment.SingleResult == false, "segment.SingleResult == false");
IEdmEntityType targetEntityType = null;
WebUtil.CheckSyntaxValid(segment.TargetEdmType != null && segment.TargetEdmType.IsEntityOrEntityCollectionType(out targetEntityType));
Debug.Assert(targetEntityType != null, "targetEntityType != null");
// Make sure the keys specified in the uri matches with the number of keys in the metadata
var keyProperties = targetEntityType.Key().ToList();
if (keyProperties.Count != key.ValueCount)
{
throw ExceptionUtil.CreateBadRequestError(ErrorStrings.BadRequest_KeyCountMismatch(targetEntityType.FullName()));
}
if (!key.AreValuesNamed && key.ValueCount > 1)
{
throw ExceptionUtil.CreateBadRequestError(ErrorStrings.RequestUriProcessor_KeysMustBeNamed);
}
WebUtil.CheckSyntaxValid(key.TryConvertValues(keyProperties));
return new PathSegment(segment)
{
Key = key,
SingleResult = true,
};
}
/// <summary>
/// Removes the first occurrence of a specific segment from the path
/// </summary>
/// <param name="item">The object to remove from the path</param>
/// <returns>
/// true if item was successfully removed from the path; otherwise, false.
/// This method also returns false if item is not found in the path
/// </returns>
/// <remarks>
/// Equality is determined using the Equals method of the path segment.
/// </remarks>
public bool Remove(IPathSegment item)
{
return segments.Remove(item);
}
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax)
{
lock (followerLock)
{
if (pathCurrentlyTracked == null)
{
Console.WriteLine("Null path tracked!");
return new RobotTwoWheelCommand(0, 0);
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0) //single waypoint case
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
startPoint = pathCurrentlyTracked.EndPoint.pt;
}
//else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
//{
// //Console.WriteLine("2");
// double lookaheadRef = 0;
// PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
// goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
//}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
if (segmentCurrentlyTracked != lookaheadPointOnPath.segment)
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
goalPoint = lookaheadPointOnPath.pt;
startPoint = currentPointOnPath.pt;
}
// Calculate errors
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
double tangentX = (goalPoint.X - startPoint.X);
double tangentY = (goalPoint.Y - startPoint.Y);
Vector2 tangent = new Vector2(tangentX, tangentY);
double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint);
double currentYaw = currentPoint.yaw;
double tempCurrentYaw = currentPoint.yaw;
errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw);
errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y)
- (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y;
//Console.Clear();
//Console.WriteLine("Current yaw is: " + currentYaw);
if (tangentEquation < currentPoint.y)
{
if (Math.PI/2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI)
{
//Console.WriteLine("Above line, pointing left");
errorDistance *= -1;
}
//else Console.WriteLine("Above line, pointing right");
}
else
{
if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI/2)
{
//Console.WriteLine("Below line, pointing right");
errorDistance *= -1;
}
//else Console.Write("Below line, pointing left");
}
//if we are really close to last waypoint, make the robot just stop
if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) &&
(PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam)
{
command = new RobotTwoWheelCommand(0, 0);
//Console.WriteLine("Acheived!");
}
else
{
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI);
double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage;
//double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance;
double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance;
if (Math.Abs(commandedVelocity) > transMax) commandedVelocity = Math.Sign(commandedVelocity) * transMax;
if (Math.Abs(commandedHeading) > turnMax)
{
commandedHeading = Math.Sign(commandedHeading) * turnMax;
commandedVelocity = 0.0;
}
/*if (unsignedYawErrorNormalizeToOne > .1)
commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/
command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
//Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance);
}
return command;
}
}
private JToken GetScalarValueForPathSegement(IPathSegment pathSegment, IEnumerable<JToken> data)
{
var jObject = data as JObject;
JToken returnVal = null;
if (jObject != null)
{
JProperty property = jObject.Property(pathSegment.ActualSegment);
if (property != null)
{
returnVal = property.Value;
}
}
return returnVal;
}
protected override IndexedPathSegmentTreeNode<string> CreatePathSegmentIndexedPathSegmentTreeNode(
IPathSegment pathSegment, IndexedPathSegmentTreeNode<string> parentNode)
{
var newIndexedValueTreeNode = new IndexedPathSegmentTreeNode<string>();
if (parentNode.EnumerationComplete)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
if (pathSegment.IsEnumarable)
{
newIndexedValueTreeNode.EnumerableValue = GetEnumerableValueForPathSegment(pathSegment,
parentNode.CurrentValue);
if (newIndexedValueTreeNode.EnumerableValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
newIndexedValueTreeNode.Enumerator = newIndexedValueTreeNode.EnumerableValue.GetEnumerator();
newIndexedValueTreeNode.Enumerator.Reset();
if (!newIndexedValueTreeNode.Enumerator.MoveNext())
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
else
{
newIndexedValueTreeNode.CurrentValue = newIndexedValueTreeNode.Enumerator.Current;
}
}
}
else
{
newIndexedValueTreeNode.CurrentValue = GetScalarValueForPathSegement(pathSegment,
parentNode.CurrentValue);
if (newIndexedValueTreeNode.CurrentValue == null)
{
newIndexedValueTreeNode.CurrentValue = string.Empty;
newIndexedValueTreeNode.EnumerationComplete = true;
}
}
}
return newIndexedValueTreeNode;
}
public bool Equals(IPathSegment other)
{
if (other is CirclePathSegment) {
CirclePathSegment cp = (CirclePathSegment)other;
return cp.radius == radius && cp.ccw == ccw && cp.startAngle == startAngle && cp.endAngle == endAngle && cp.center == center;
}
else {
return false;
}
}
